sdf_tracker_node.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <ros/ros.h>
00003 #include <ros/console.h>
00004 #include <sensor_msgs/Image.h>
00005 #include <cv_bridge/cv_bridge.h>
00006 
00007 // uncomment this part to enable an example for how to work with LIDAR data.
00008 //#define LIDAR
00009 
00010 #define EIGEN_NO_DEBUG
00011 
00012 #include <sdf_tracker/sdf_tracker.h>
00013 
00014 class SDFTrackerNode
00015 {
00016 public:
00017   SDFTrackerNode(SDF_Parameters &parameters);
00018   virtual ~SDFTrackerNode();
00019  void subscribeTopic(const std::string topic = std::string("default"));    
00020  void advertiseTopic(const std::string topic = std::string("default"));    
00021  void rgbCallback(const sensor_msgs::Image::ConstPtr& msg);
00022  void depthCallback(const sensor_msgs::Image::ConstPtr& msg);
00023 
00024 private:
00025   int skip_frames_;
00026   SDFTracker* myTracker_;
00027   SDF_Parameters myParameters_;
00028 
00029 
00030   std::vector<ros::Time> timestamps_;
00031   ros::NodeHandle nh_;
00032   ros::NodeHandle n_;
00033   ros::Subscriber depth_subscriber_;
00034   ros::Subscriber color_subscriber_;
00035   ros::Publisher depth_publisher_;
00036   ros::Publisher color_publisher_;
00037   
00038   ros::Timer heartbeat_depth_;
00039   std::string camera_name_;
00040   std::string loadVolume_;
00041 
00042   bool depth_registered_;
00043   bool use_texture_;
00044   bool makeTris_;
00045   bool makeVolume_;
00046   void publishDepthDenoisedImage(const ros::TimerEvent& event); 
00047 };
00048 
00049 
00050 SDFTrackerNode::SDFTrackerNode(SDF_Parameters &parameters)
00051 {
00052   myParameters_ = parameters;
00053   nh_ = ros::NodeHandle("~");
00054   n_ = ros::NodeHandle();
00055 
00056   //node specific parameters
00057   nh_.param("depth_registered", depth_registered_, false);
00058   nh_.param<std::string>("c_name", camera_name_,"camera");
00059   nh_.param<std::string>("LoadVolume", loadVolume_,"none");
00060   nh_.param("OutputTriangles",makeTris_, false);
00061   nh_.param("OutputVolume",makeVolume_, false);
00062   nh_.param("UseTexture",use_texture_, false);
00063 
00064   //parameters used for the SDF tracker
00065   nh_.getParam("ImageWidth", myParameters_.image_width);
00066   nh_.getParam("ImageHeight", myParameters_.image_height); 
00067   nh_.getParam("InteractiveMode", myParameters_.interactive_mode);
00068   nh_.getParam("MaxWeight",myParameters_.Wmax);
00069   nh_.getParam("CellSize",myParameters_.resolution);
00070   nh_.getParam("GridSizeX",myParameters_.XSize);
00071   nh_.getParam("GridSizeY",myParameters_.YSize);
00072   nh_.getParam("GridSizeZ",myParameters_.ZSize);
00073   nh_.getParam("PositiveTruncationDistance",myParameters_.Dmax);
00074   nh_.getParam("NegativeTruncationDistance",myParameters_.Dmin);
00075   nh_.getParam("RobustStatisticCoefficient", myParameters_.robust_statistic_coefficient);
00076   nh_.getParam("Regularization", myParameters_.regularization);
00077   nh_.getParam("MinPoseChangeToFuseData", myParameters_.min_pose_change);
00078   nh_.getParam("ConvergenceCondition", myParameters_.min_parameter_update);
00079   nh_.getParam("MaximumRaycastSteps", myParameters_.raycast_steps);
00080   nh_.getParam("FocalLengthX", myParameters_.fx);
00081   nh_.getParam("FocalLengthY", myParameters_.fy);
00082   nh_.getParam("CenterPointX", myParameters_.cx);
00083   nh_.getParam("CenterPointY", myParameters_.cy);
00084 
00085   myTracker_ = new SDFTracker(myParameters_);
00086 
00087   //if this is how you named your file, consider a career as a lottery-winner or password-cracker.
00088   if(loadVolume_.compare(std::string("none"))!=0) 
00089   {
00090     myTracker_->LoadSDF(loadVolume_); 
00091   }  
00092   
00093   skip_frames_ = 0;
00094 }
00095 
00096 SDFTrackerNode::~SDFTrackerNode()
00097 {
00098   if(myTracker_ != NULL) 
00099   {
00100     delete myTracker_;
00101     //parameters used for the SDF tracker Node 
00102     nh_.deleteParam("depth_registered");
00103     nh_.deleteParam("c_name");
00104     nh_.deleteParam("LoadVolume");
00105     nh_.deleteParam("OutputTriangles");
00106     nh_.deleteParam("OutputVolume");
00107     nh_.deleteParam("UseTexture");
00108     //parameters used for the SDF tracker
00109     nh_.deleteParam("ImageWidth");
00110     nh_.deleteParam("ImageHeight"); 
00111     nh_.deleteParam("InteractiveMode");
00112     nh_.deleteParam("MaxWeight");
00113     nh_.deleteParam("CellSize");
00114     nh_.deleteParam("GridSizeX");
00115     nh_.deleteParam("GridSizeY");
00116     nh_.deleteParam("GridSizeZ");
00117     nh_.deleteParam("PositiveTruncationDistance");
00118     nh_.deleteParam("NegativeTruncationDistance");
00119     nh_.deleteParam("RobustStatisticCoefficient");
00120     nh_.deleteParam("Regularization");
00121     nh_.deleteParam("MinPoseChangeToFuseData");
00122     nh_.deleteParam("ConvergenceCondition");
00123     nh_.deleteParam("MaximumRaycastSteps");
00124     nh_.deleteParam("FocalLengthX");
00125     nh_.deleteParam("FocalLengthY");
00126     nh_.deleteParam("CenterPointX");
00127     nh_.deleteParam("CenterPointY");
00128   }
00129 }
00130 
00131 
00132 void
00133 SDFTrackerNode::subscribeTopic(const std::string topic)
00134 {
00135   
00136   std::string subscribe_topic_depth = topic;
00137   std::string subscribe_topic_color = topic;
00138 
00139   if(depth_registered_)
00140   {
00141     if(topic.compare(std::string("default")) == 0)
00142     {
00143       subscribe_topic_depth = camera_name_+"/depth_registered/image";
00144       subscribe_topic_color = camera_name_+"/rgb/image_color";
00145     }
00146 
00147     depth_subscriber_ = n_.subscribe(subscribe_topic_depth, 1, &SDFTrackerNode::depthCallback, this);
00148     if(use_texture_) color_subscriber_ = n_.subscribe(subscribe_topic_color, 1, &SDFTrackerNode::rgbCallback, this);
00149   }
00150   else
00151   {
00152     if(topic.compare(std::string("default")) == 0)
00153     { 
00154       subscribe_topic_depth = camera_name_+"/depth/image";
00155       subscribe_topic_color = camera_name_+"/rgb/image_color";
00156     }
00157       depth_subscriber_ = n_.subscribe(subscribe_topic_depth, 1, &SDFTrackerNode::depthCallback, this);
00158       if(use_texture_) color_subscriber_ = n_.subscribe(subscribe_topic_color, 1, &SDFTrackerNode::rgbCallback, this);
00159   }
00160 }
00161 
00162 void
00163 SDFTrackerNode::advertiseTopic(const std::string topic)
00164 {
00165   std::string advertise_topic = topic;
00166 
00167   if(depth_registered_)
00168   {
00169     if(topic.compare(std::string("default")) == 0) advertise_topic = "/"+camera_name_+"/depth_registered/image_denoised";
00170     
00171     depth_publisher_ = n_.advertise<sensor_msgs::Image>(advertise_topic, 10); 
00172   }
00173   else
00174   {
00175     if(topic.compare(std::string("default")) == 0) advertise_topic = "/"+camera_name_+"/depth/image_denoised";
00176     depth_publisher_ = n_.advertise<sensor_msgs::Image>( advertise_topic ,10); 
00177   }
00178 
00179   heartbeat_depth_ = nh_.createTimer(ros::Duration(1.0), &SDFTrackerNode::publishDepthDenoisedImage, this);
00180 
00181 }
00182 void SDFTrackerNode::publishDepthDenoisedImage(const ros::TimerEvent& event) 
00183 {    
00184   if(depth_publisher_.getNumSubscribers()>0)
00185   {
00186     cv_bridge::CvImagePtr image_out (new cv_bridge::CvImage());
00187     image_out->header.stamp = ros::Time::now(); 
00188     image_out->encoding = "32FC1";      
00189     myTracker_->GetDenoisedImage(image_out->image);
00190     depth_publisher_.publish(image_out->toImageMsg());     
00191   } 
00192   return;
00193 };
00194 
00195 void SDFTrackerNode::rgbCallback(const sensor_msgs::Image::ConstPtr& msg)
00196 {
00197   cv_bridge::CvImageConstPtr bridge;
00198   try
00199   {
00200     bridge = cv_bridge::toCvCopy(msg, "8UC3");
00201   }
00202   catch (cv_bridge::Exception& e)
00203   {
00204     ROS_ERROR("Failed to transform rgb image.");
00205     return;
00206   
00207     /* do something colorful"*/
00208   }
00209 }
00210 
00211 void SDFTrackerNode::depthCallback(const sensor_msgs::Image::ConstPtr& msg)
00212 {
00213   cv_bridge::CvImageConstPtr bridge;
00214   try
00215   {
00216     bridge = cv_bridge::toCvCopy(msg, "32FC1");
00217   }
00218   catch (cv_bridge::Exception& e)
00219   {
00220     ROS_ERROR("Failed to transform depth image.");
00221     return;
00222   }
00223   
00224   if(skip_frames_ < 3){++skip_frames_; return;}
00225 
00226   if(!myTracker_->Quit())
00227   {
00228 
00229     #ifdef LIDAR
00230       
00231       //LIDAR-like alternative for updating with points (suggestion: set the MaxWeight parameter to a number around 300000 (648x480) )
00232       //MaximumRaycastSteps should also be set to a higher value than usual.
00233 
00234       std::vector<Eigen::Vector4d,Eigen::aligned_allocator<Eigen::Vector4d> > points;
00235       for (int u = 0; u < bridge->image.rows; ++u)
00236       {
00237         for (int v = 0; v < bridge->image.cols; ++v)
00238         {
00239           points.push_back( myTracker_->To3D(u,v,bridge->image.at<float>(u,v),myParameters_.fx,myParameters_.fy,myParameters_.cx,myParameters_.cy) );
00240         }
00241       }
00242 
00243       if(skip_frames_ == 3)
00244       {  
00245         myTracker_->UpdatePoints(points);
00246         myTracker_->FusePoints();
00247         ++skip_frames_;
00248       } 
00249       else myTracker_->UpdatePoints(points);
00250       
00251       Vector6d xi = myTracker_->EstimatePoseFromPoints();
00252       myTracker_->SetCurrentTransformation(myTracker_->Twist(xi).exp()*myTracker_->GetCurrentTransformation());    
00253       myTracker_->FusePoints();
00254       myTracker_->Render();
00255     #else
00256       myTracker_->FuseDepth(bridge->image);
00257       timestamps_.push_back(ros::Time::now());      
00258     #endif
00259 
00260   }
00261   else 
00262   {
00263     if(makeTris_)
00264     {
00265       myTracker_->MakeTriangles();
00266       myTracker_->SaveTriangles();
00267     }
00268   
00269     if(makeVolume_) {
00270       std::cout<<"saving volume\n";
00271       myTracker_->SaveSDF();
00272     }
00273   
00274     ros::shutdown();
00275   }
00276 }
00277 
00278 
00279 
00280 int main( int argc, char* argv[] )
00281 {
00282     ros::init(argc, argv, "sdf_tracker_node");
00283     
00284     SDF_Parameters param;
00285 
00286      //Pose Offset as a transformation matrix
00287     Eigen::Matrix4d initialTransformation = 
00288     Eigen::MatrixXd::Identity(4,4);
00289 
00290     //define translation offsets in x y z
00291     initialTransformation(0,3) = 0.0;  //x 
00292     initialTransformation(1,3) = 0.0;  //y
00293     initialTransformation(2,3) = -0.7; //z
00294 
00295     param.pose_offset = initialTransformation;
00296 
00297     SDFTrackerNode MyTrackerNode(param);
00298     MyTrackerNode.subscribeTopic();
00299     MyTrackerNode.advertiseTopic();
00300 
00301     ros::spin();
00302     
00303     return 0;
00304 }


sdf_tracker
Author(s): Daniel Canelhas
autogenerated on Wed Aug 26 2015 15:24:48