00001 /*************************************************************** 00002 * ROS port of ROBULAR implementation 00003 * 00004 * Original header: 00005 * * 00006 * This source code is part of ROBULAR. 00007 * 00008 * ROBULAR Copyright (c) 2009 Joerg Mueller, Boris Lau, Kai 00009 * Wurm. Autonomous Intelligent Systems, Department of Computer 00010 * Science, University of Freiburg, Germany. 00011 **************************************************************/ 00012 00013 #ifndef ROS_POSE_TRACKER 00014 #define ROS_POSE_TRACKER 00015 00016 #include <map> 00017 #include <vector> 00018 #include <string> 00019 #include <tf/tf.h> 00020 #include <visualization_msgs/MarkerArray.h> 00021 #include <Eigen/Core> 00022 #include <Eigen/Geometry> 00023 #include <tf/transform_broadcaster.h> 00024 00025 class PoseTracker{ 00026 public: 00027 PoseTracker(); 00028 00029 void setDebug(bool d); 00030 void init(const std::string &propFile, double scale, tf::TransformBroadcaster* bc, double _sqrErrThreshold, bool _useRansac, unsigned int _numRansacSamples); 00031 00032 void process(const visualization_msgs::MarkerArray::ConstPtr &msg); 00033 00034 protected: 00035 struct MarkerEval { 00036 Eigen::Vector3d model; 00037 Eigen::Vector3d real; 00038 bool use; 00039 double sqrErr; 00040 }; 00041 00042 bool calculatePose(const visualization_msgs::MarkerArray::ConstPtr &m); 00043 double getConsensus(std::vector<MarkerEval> &points, Eigen::Affine3d &p, int &numInliers); 00044 bool initialized; 00045 std::vector<MarkerEval> markerEval; 00046 bool useRansac; 00047 int numRansacSamples; 00048 double sqrErrThreshold; 00049 bool debug; 00050 00051 // object information 00052 std::string bodyName; 00053 std::vector<Eigen::Vector3d> bodyMarkerPositions; 00054 std::vector<std::string> bodyMarkerNames; 00055 00056 tf::TransformBroadcaster* broadcaster; 00057 00058 void parsePropFile(const std::string &logfile, double scale); 00059 }; 00060 00061 #endif 00062
