tracking_sample.cpp
Go to the documentation of this file.
00001 #include <pcl/point_cloud.h>
00002 #include <pcl/point_types.h>
00003 #include <pcl/io/openni_grabber.h>
00004 #include <pcl/console/parse.h>
00005 #include <pcl/common/time.h>
00006 #include <pcl/common/centroid.h>
00007 
00008 #include <pcl/visualization/cloud_viewer.h>
00009 #include <pcl/visualization/pcl_visualizer.h>
00010 #include <pcl/io/pcd_io.h>
00011 
00012 #include <pcl/filters/passthrough.h>
00013 #include <pcl/filters/voxel_grid.h>
00014 #include <pcl/filters/approximate_voxel_grid.h>
00015 
00016 #include <pcl/sample_consensus/method_types.h>
00017 #include <pcl/sample_consensus/model_types.h>
00018 
00019 #include <pcl/search/pcl_search.h>
00020 #include <pcl/common/transforms.h>
00021 
00022 #include <boost/format.hpp>
00023 
00024 #include <pcl/tracking/tracking.h>
00025 #include <pcl/tracking/particle_filter.h>
00026 #include <pcl/tracking/kld_adaptive_particle_filter_omp.h>
00027 #include <pcl/tracking/particle_filter_omp.h>
00028 #include <pcl/tracking/coherence.h>
00029 #include <pcl/tracking/distance_coherence.h>
00030 #include <pcl/tracking/hsv_color_coherence.h>
00031 #include <pcl/tracking/approx_nearest_pair_point_cloud_coherence.h>
00032 #include <pcl/tracking/nearest_pair_point_cloud_coherence.h>
00033 
00034 using namespace pcl::tracking;
00035 
00036 typedef pcl::PointXYZRGBA RefPointType;
00037 typedef ParticleXYZRPY ParticleT;
00038 typedef pcl::PointCloud<pcl::PointXYZRGBA> Cloud;
00039 typedef Cloud::Ptr CloudPtr;
00040 typedef Cloud::ConstPtr CloudConstPtr;
00041 typedef ParticleFilterTracker<RefPointType, ParticleT> ParticleFilter;
00042 
00043 CloudPtr cloud_pass_;
00044 CloudPtr cloud_pass_downsampled_;
00045 CloudPtr target_cloud;
00046 
00047 boost::mutex mtx_;
00048 boost::shared_ptr<ParticleFilter> tracker_;
00049 bool new_cloud_;
00050 double downsampling_grid_size_;
00051 int counter;
00052 
00053 
00054 //Filter along a specified dimension
00055 void filterPassThrough (const CloudConstPtr &cloud, Cloud &result)
00056 {
00057   pcl::PassThrough<pcl::PointXYZRGBA> pass;
00058   pass.setFilterFieldName ("z");
00059   pass.setFilterLimits (0.0, 10.0);
00060   pass.setKeepOrganized (false);
00061   pass.setInputCloud (cloud);
00062   pass.filter (result);
00063 }
00064 
00065 
00066 void gridSampleApprox (const CloudConstPtr &cloud, Cloud &result, double leaf_size)
00067 {
00068   pcl::ApproximateVoxelGrid<pcl::PointXYZRGBA> grid;
00069   grid.setLeafSize (static_cast<float> (leaf_size), static_cast<float> (leaf_size), static_cast<float> (leaf_size));
00070   grid.setInputCloud (cloud);
00071   grid.filter (result);
00072 }
00073 
00074 
00075 //Draw the current particles
00076 bool
00077 drawParticles (pcl::visualization::PCLVisualizer& viz)
00078 {
00079   ParticleFilter::PointCloudStatePtr particles = tracker_->getParticles ();
00080   if (particles && new_cloud_)
00081     {
00082       //Set pointCloud with particle's points
00083       pcl::PointCloud<pcl::PointXYZ>::Ptr particle_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
00084       for (size_t i = 0; i < particles->points.size (); i++)
00085         {
00086           pcl::PointXYZ point;
00087           
00088           point.x = particles->points[i].x;
00089           point.y = particles->points[i].y;
00090           point.z = particles->points[i].z;
00091           particle_cloud->points.push_back (point);
00092         }
00093 
00094       //Draw red particles 
00095       {
00096         pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> red_color (particle_cloud, 250, 99, 71);
00097 
00098         if (!viz.updatePointCloud (particle_cloud, red_color, "particle cloud"))
00099           viz.addPointCloud (particle_cloud, red_color, "particle cloud");
00100       }
00101       return true;
00102     }
00103   else
00104     {
00105       return false;
00106     }
00107 }
00108 
00109 //Draw model reference point cloud
00110 void
00111 drawResult (pcl::visualization::PCLVisualizer& viz)
00112 {
00113   ParticleXYZRPY result = tracker_->getResult ();
00114   Eigen::Affine3f transformation = tracker_->toEigenMatrix (result);
00115 
00116   //move close to camera a little for better visualization
00117   transformation.translation () += Eigen::Vector3f (0.0f, 0.0f, -0.005f);
00118   CloudPtr result_cloud (new Cloud ());
00119   pcl::transformPointCloud<RefPointType> (*(tracker_->getReferenceCloud ()), *result_cloud, transformation);
00120 
00121   //Draw blue model reference point cloud
00122   {
00123     pcl::visualization::PointCloudColorHandlerCustom<RefPointType> blue_color (result_cloud, 0, 0, 255);
00124 
00125     if (!viz.updatePointCloud (result_cloud, blue_color, "resultcloud"))
00126       viz.addPointCloud (result_cloud, blue_color, "resultcloud");
00127   }
00128 }
00129 
00130 //visualization's callback function
00131 void
00132 viz_cb (pcl::visualization::PCLVisualizer& viz)
00133 {
00134   boost::mutex::scoped_lock lock (mtx_);
00135     
00136   if (!cloud_pass_)
00137     {
00138       boost::this_thread::sleep (boost::posix_time::seconds (1));
00139       return;
00140    }
00141 
00142   //Draw downsampled point cloud from sensor    
00143   if (new_cloud_ && cloud_pass_downsampled_)
00144     {
00145       CloudPtr cloud_pass;
00146       cloud_pass = cloud_pass_downsampled_;
00147     
00148       if (!viz.updatePointCloud (cloud_pass, "cloudpass"))
00149         {
00150           viz.addPointCloud (cloud_pass, "cloudpass");
00151           viz.resetCameraViewpoint ("cloudpass");
00152         }
00153       bool ret = drawParticles (viz);
00154       if (ret)
00155         drawResult (viz);
00156     }
00157   new_cloud_ = false;
00158 }
00159 
00160 //OpenNI Grabber's cloud Callback function
00161 void
00162 cloud_cb (const CloudConstPtr &cloud)
00163 {
00164   boost::mutex::scoped_lock lock (mtx_);
00165   cloud_pass_.reset (new Cloud);
00166   cloud_pass_downsampled_.reset (new Cloud);
00167   filterPassThrough (cloud, *cloud_pass_);
00168   gridSampleApprox (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_);
00169 
00170   if(counter < 10){
00171         counter++;
00172   }else{
00173         //Track the object
00174         tracker_->setInputCloud (cloud_pass_downsampled_);
00175         tracker_->compute ();
00176         new_cloud_ = true;
00177   }
00178 }
00179 
00180 int
00181 main (int argc, char** argv)
00182 {
00183   if (argc < 3)
00184     {
00185       PCL_WARN("Please set device_id pcd_filename(e.g. $ %s '#1' sample.pcd)\n", argv[0]);
00186       exit (1);
00187     }
00188 
00189   //read pcd file
00190   target_cloud.reset(new Cloud());
00191   if(pcl::io::loadPCDFile (argv[2], *target_cloud) == -1){
00192     std::cout << "pcd file not found" << std::endl;
00193     exit(-1);
00194   }
00195 
00196   std::string device_id = std::string (argv[1]);  
00197 
00198   counter = 0;
00199 
00200   //Set parameters
00201   new_cloud_  = false;
00202   downsampling_grid_size_ =  0.002;
00203 
00204   std::vector<double> default_step_covariance = std::vector<double> (6, 0.015 * 0.015);
00205   default_step_covariance[3] *= 40.0;
00206   default_step_covariance[4] *= 40.0;
00207   default_step_covariance[5] *= 40.0;
00208 
00209   std::vector<double> initial_noise_covariance = std::vector<double> (6, 0.00001);
00210   std::vector<double> default_initial_mean = std::vector<double> (6, 0.0);
00211 
00212   boost::shared_ptr<KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT> > tracker
00213     (new KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT> (8));
00214 
00215   ParticleT bin_size;
00216   bin_size.x = 0.1f;
00217   bin_size.y = 0.1f;
00218   bin_size.z = 0.1f;
00219   bin_size.roll = 0.1f;
00220   bin_size.pitch = 0.1f;
00221   bin_size.yaw = 0.1f;
00222 
00223 
00224   //Set all parameters for  KLDAdaptiveParticleFilterOMPTracker
00225   tracker->setMaximumParticleNum (1000);
00226   tracker->setDelta (0.99);
00227   tracker->setEpsilon (0.2);
00228   tracker->setBinSize (bin_size);
00229 
00230   //Set all parameters for  ParticleFilter
00231   tracker_ = tracker;
00232   tracker_->setTrans (Eigen::Affine3f::Identity ());
00233   tracker_->setStepNoiseCovariance (default_step_covariance);
00234   tracker_->setInitialNoiseCovariance (initial_noise_covariance);
00235   tracker_->setInitialNoiseMean (default_initial_mean);
00236   tracker_->setIterationNum (1);
00237   tracker_->setParticleNum (600);
00238   tracker_->setResampleLikelihoodThr(0.00);
00239   tracker_->setUseNormal (false);
00240 
00241 
00242   //Setup coherence object for tracking
00243   ApproxNearestPairPointCloudCoherence<RefPointType>::Ptr coherence = ApproxNearestPairPointCloudCoherence<RefPointType>::Ptr
00244     (new ApproxNearestPairPointCloudCoherence<RefPointType> ());
00245     
00246   boost::shared_ptr<DistanceCoherence<RefPointType> > distance_coherence
00247     = boost::shared_ptr<DistanceCoherence<RefPointType> > (new DistanceCoherence<RefPointType> ());
00248   coherence->addPointCoherence (distance_coherence);
00249 
00250   boost::shared_ptr<pcl::search::Octree<RefPointType> > search (new pcl::search::Octree<RefPointType> (0.01));
00251   coherence->setSearchMethod (search);
00252   coherence->setMaximumDistance (0.01);
00253 
00254   tracker_->setCloudCoherence (coherence);
00255 
00256   //prepare the model of tracker's target
00257   Eigen::Vector4f c;
00258   Eigen::Affine3f trans = Eigen::Affine3f::Identity ();
00259   CloudPtr transed_ref (new Cloud);
00260   CloudPtr transed_ref_downsampled (new Cloud);
00261 
00262   pcl::compute3DCentroid<RefPointType> (*target_cloud, c);
00263   trans.translation ().matrix () = Eigen::Vector3f (c[0], c[1], c[2]);
00264   pcl::transformPointCloud<RefPointType> (*target_cloud, *transed_ref, trans.inverse());
00265   gridSampleApprox (transed_ref, *transed_ref_downsampled, downsampling_grid_size_);
00266 
00267   //set reference model and trans
00268   tracker_->setReferenceCloud (transed_ref_downsampled);
00269   tracker_->setTrans (trans);
00270 
00271   //Setup OpenNIGrabber and viewer
00272   pcl::visualization::CloudViewer* viewer_ = new pcl::visualization::CloudViewer("PCL OpenNI Tracking Viewer");
00273   pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id);
00274   boost::function<void (const CloudConstPtr&)> f =
00275     boost::bind (&cloud_cb, _1);
00276   interface->registerCallback (f);
00277     
00278   viewer_->runOnVisualizationThread (boost::bind(&viz_cb, _1), "viz_cb");
00279 
00280   //Start viewer and object tracking
00281   interface->start();
00282   while (!viewer_->wasStopped ())
00283     boost::this_thread::sleep(boost::posix_time::seconds(1));
00284   interface->stop();
00285 }


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:36:44