openni_tracking.cpp
Go to the documentation of this file.
00001 #include <pcl/tracking/tracking.h>
00002 #include <pcl/tracking/particle_filter.h>
00003 #include <pcl/tracking/kld_adaptive_particle_filter_omp.h>
00004 #include <pcl/tracking/particle_filter_omp.h>
00005 
00006 #include <pcl/tracking/coherence.h>
00007 #include <pcl/tracking/distance_coherence.h>
00008 #include <pcl/tracking/hsv_color_coherence.h>
00009 #include <pcl/tracking/normal_coherence.h>
00010 
00011 #include <pcl/tracking/approx_nearest_pair_point_cloud_coherence.h>
00012 #include <pcl/tracking/nearest_pair_point_cloud_coherence.h>
00013 
00014 #include <pcl/point_cloud.h>
00015 #include <pcl/point_types.h>
00016 #include <pcl/io/openni_grabber.h>
00017 #include <pcl/console/parse.h>
00018 #include <pcl/common/time.h>
00019 #include <pcl/common/centroid.h>
00020 
00021 #include <pcl/visualization/cloud_viewer.h>
00022 #include <pcl/visualization/pcl_visualizer.h>
00023 
00024 #include <pcl/io/pcd_io.h>
00025 
00026 #include <pcl/filters/passthrough.h>
00027 #include <pcl/filters/project_inliers.h>
00028 #include <pcl/filters/voxel_grid.h>
00029 #include <pcl/filters/approximate_voxel_grid.h>
00030 #include <pcl/filters/extract_indices.h>
00031 
00032 #include <pcl/features/normal_3d.h>
00033 #include <pcl/features/normal_3d_omp.h>
00034 #include <pcl/features/integral_image_normal.h>
00035 
00036 #include <pcl/sample_consensus/method_types.h>
00037 #include <pcl/sample_consensus/model_types.h>
00038 
00039 #include <pcl/segmentation/sac_segmentation.h>
00040 #include <pcl/segmentation/extract_polygonal_prism_data.h>
00041 #include <pcl/segmentation/extract_clusters.h>
00042 
00043 #include <pcl/surface/convex_hull.h>
00044 
00045 #include <pcl/search/pcl_search.h>
00046 #include <pcl/common/transforms.h>
00047 
00048 #include <boost/format.hpp>
00049 
00050 #define FPS_CALC_BEGIN                          \
00051     static double duration = 0;                 \
00052     double start_time = pcl::getTime ();        \
00053 
00054 #define FPS_CALC_END(_WHAT_)                    \
00055   {                                             \
00056     double end_time = pcl::getTime ();          \
00057     static unsigned count = 0;                  \
00058     if (++count == 10)                          \
00059     {                                           \
00060       std::cout << "Average framerate("<< _WHAT_ << "): " << double(count)/double(duration) << " Hz" <<  std::endl; \
00061       count = 0;                                                        \
00062       duration = 0.0;                                                   \
00063     }                                           \
00064     else                                        \
00065     {                                           \
00066       duration += end_time - start_time;        \
00067     }                                           \
00068   }
00069 
00070 using namespace pcl::tracking;
00071 
00072 template <typename PointType>
00073 class OpenNISegmentTracking
00074 {
00075 public:
00076   //typedef pcl::PointXYZRGBANormal RefPointType;
00077   typedef pcl::PointXYZRGBA RefPointType;
00078   //typedef pcl::PointXYZ RefPointType;
00079   typedef ParticleXYZRPY ParticleT;
00080   
00081   typedef pcl::PointCloud<PointType> Cloud;
00082   typedef pcl::PointCloud<RefPointType> RefCloud;
00083   typedef typename RefCloud::Ptr RefCloudPtr;
00084   typedef typename RefCloud::ConstPtr RefCloudConstPtr;
00085   typedef typename Cloud::Ptr CloudPtr;
00086   typedef typename Cloud::ConstPtr CloudConstPtr;
00087   //typedef KLDAdaptiveParticleFilterTracker<RefPointType, ParticleT> ParticleFilter;
00088   //typedef KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT> ParticleFilter;
00089   //typedef ParticleFilterOMPTracker<RefPointType, ParticleT> ParticleFilter;
00090   typedef ParticleFilterTracker<RefPointType, ParticleT> ParticleFilter;
00091   typedef typename ParticleFilter::CoherencePtr CoherencePtr;
00092   typedef typename pcl::search::KdTree<PointType> KdTree;
00093   typedef typename KdTree::Ptr KdTreePtr;
00094   OpenNISegmentTracking (const std::string& device_id, int thread_nr, double downsampling_grid_size,
00095                          bool use_convex_hull,
00096                          bool visualize_non_downsample, bool visualize_particles,
00097                          bool use_fixed)
00098   : viewer_ ("PCL OpenNI Tracking Viewer")
00099   , device_id_ (device_id)
00100   , new_cloud_ (false)
00101   , ne_ (thread_nr)
00102   , counter_ (0)
00103   , use_convex_hull_ (use_convex_hull)
00104   , visualize_non_downsample_ (visualize_non_downsample)
00105   , visualize_particles_ (visualize_particles)
00106   , downsampling_grid_size_ (downsampling_grid_size)
00107   {
00108     KdTreePtr tree (new KdTree (false));
00109     ne_.setSearchMethod (tree);
00110     ne_.setRadiusSearch (0.03);
00111     
00112     std::vector<double> default_step_covariance = std::vector<double> (6, 0.015 * 0.015);
00113     default_step_covariance[3] *= 40.0;
00114     default_step_covariance[4] *= 40.0;
00115     default_step_covariance[5] *= 40.0;
00116     
00117     std::vector<double> initial_noise_covariance = std::vector<double> (6, 0.00001);
00118     std::vector<double> default_initial_mean = std::vector<double> (6, 0.0);
00119     if (use_fixed)
00120     {
00121       boost::shared_ptr<ParticleFilterOMPTracker<RefPointType, ParticleT> > tracker
00122         (new ParticleFilterOMPTracker<RefPointType, ParticleT> (thread_nr));
00123       tracker_ = tracker;
00124     }
00125     else
00126     {
00127       boost::shared_ptr<KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT> > tracker
00128         (new KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT> (thread_nr));
00129       tracker->setMaximumParticleNum (500);
00130       tracker->setDelta (0.99);
00131       tracker->setEpsilon (0.2);
00132       ParticleT bin_size;
00133       bin_size.x = 0.1f;
00134       bin_size.y = 0.1f;
00135       bin_size.z = 0.1f;
00136       bin_size.roll = 0.1f;
00137       bin_size.pitch = 0.1f;
00138       bin_size.yaw = 0.1f;
00139       tracker->setBinSize (bin_size);
00140       tracker_ = tracker;
00141     }
00142     
00143     tracker_->setTrans (Eigen::Affine3f::Identity ());
00144     tracker_->setStepNoiseCovariance (default_step_covariance);
00145     tracker_->setInitialNoiseCovariance (initial_noise_covariance);
00146     tracker_->setInitialNoiseMean (default_initial_mean);
00147     tracker_->setIterationNum (1);
00148     
00149     tracker_->setParticleNum (400);
00150     tracker_->setResampleLikelihoodThr(0.00);
00151     tracker_->setUseNormal (false);
00152     // setup coherences
00153     ApproxNearestPairPointCloudCoherence<RefPointType>::Ptr coherence = ApproxNearestPairPointCloudCoherence<RefPointType>::Ptr
00154       (new ApproxNearestPairPointCloudCoherence<RefPointType> ());
00155     // NearestPairPointCloudCoherence<RefPointType>::Ptr coherence = NearestPairPointCloudCoherence<RefPointType>::Ptr
00156     //   (new NearestPairPointCloudCoherence<RefPointType> ());
00157     
00158     boost::shared_ptr<DistanceCoherence<RefPointType> > distance_coherence
00159       = boost::shared_ptr<DistanceCoherence<RefPointType> > (new DistanceCoherence<RefPointType> ());
00160     coherence->addPointCoherence (distance_coherence);
00161     
00162     boost::shared_ptr<HSVColorCoherence<RefPointType> > color_coherence
00163       = boost::shared_ptr<HSVColorCoherence<RefPointType> > (new HSVColorCoherence<RefPointType> ());
00164     color_coherence->setWeight (0.1);
00165     coherence->addPointCoherence (color_coherence);
00166     
00167     //boost::shared_ptr<pcl::search::KdTree<RefPointType> > search (new pcl::search::KdTree<RefPointType> (false));
00168     boost::shared_ptr<pcl::search::Octree<RefPointType> > search (new pcl::search::Octree<RefPointType> (0.01));
00169     //boost::shared_ptr<pcl::search::OrganizedNeighbor<RefPointType> > search (new pcl::search::OrganizedNeighbor<RefPointType>);
00170     coherence->setSearchMethod (search);
00171     coherence->setMaximumDistance (0.01);
00172     tracker_->setCloudCoherence (coherence);
00173   }
00174 
00175   bool
00176   drawParticles (pcl::visualization::PCLVisualizer& viz)
00177   {
00178     ParticleFilter::PointCloudStatePtr particles = tracker_->getParticles ();
00179     if (particles)
00180     {
00181       if (visualize_particles_)
00182       {
00183         pcl::PointCloud<pcl::PointXYZ>::Ptr particle_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
00184         for (size_t i = 0; i < particles->points.size (); i++)
00185         {
00186           pcl::PointXYZ point;
00187           
00188           point.x = particles->points[i].x;
00189           point.y = particles->points[i].y;
00190           point.z = particles->points[i].z;
00191           particle_cloud->points.push_back (point);
00192         }
00193         
00194         {
00195           pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> blue_color (particle_cloud, 250, 99, 71);
00196           if (!viz.updatePointCloud (particle_cloud, blue_color, "particle cloud"))
00197             viz.addPointCloud (particle_cloud, blue_color, "particle cloud");
00198         }
00199       }
00200       return true;
00201     }
00202     else
00203     {
00204       PCL_WARN ("no particles\n");
00205       return false;
00206     }
00207   }
00208   
00209   void
00210   drawResult (pcl::visualization::PCLVisualizer& viz)
00211   {
00212     ParticleXYZRPY result = tracker_->getResult ();
00213     Eigen::Affine3f transformation = tracker_->toEigenMatrix (result);
00214     // move a little bit for better visualization
00215     transformation.translation () += Eigen::Vector3f (0.0f, 0.0f, -0.005f);
00216     RefCloudPtr result_cloud (new RefCloud ());
00217 
00218     if (!visualize_non_downsample_)
00219       pcl::transformPointCloud<RefPointType> (*(tracker_->getReferenceCloud ()), *result_cloud, transformation);
00220     else
00221       pcl::transformPointCloud<RefPointType> (*reference_, *result_cloud, transformation);
00222 
00223     {
00224       pcl::visualization::PointCloudColorHandlerCustom<RefPointType> red_color (result_cloud, 0, 0, 255);
00225       if (!viz.updatePointCloud (result_cloud, red_color, "resultcloud"))
00226         viz.addPointCloud (result_cloud, red_color, "resultcloud");
00227     }
00228     
00229   }
00230   
00231   void
00232   viz_cb (pcl::visualization::PCLVisualizer& viz)
00233   {
00234     boost::mutex::scoped_lock lock (mtx_);
00235     
00236     if (!cloud_pass_)
00237     {
00238       boost::this_thread::sleep (boost::posix_time::seconds (1));
00239       return;
00240     }
00241     
00242     if (new_cloud_ && cloud_pass_downsampled_)
00243     {
00244       CloudPtr cloud_pass;
00245       if (!visualize_non_downsample_)
00246         cloud_pass = cloud_pass_downsampled_;
00247       else
00248         cloud_pass = cloud_pass_;
00249       
00250       if (!viz.updatePointCloud (cloud_pass, "cloudpass"))
00251         {
00252           viz.addPointCloud (cloud_pass, "cloudpass");
00253           viz.resetCameraViewpoint ("cloudpass");
00254         }
00255     }
00256 
00257     if (new_cloud_ && reference_)
00258     {
00259       bool ret = drawParticles (viz);
00260       if (ret)
00261       {
00262         drawResult (viz);
00263         
00264         // draw some texts
00265         viz.removeShape ("N");
00266         viz.addText ((boost::format ("number of Reference PointClouds: %d") % tracker_->getReferenceCloud ()->points.size ()).str (),
00267                      10, 20, 20, 1.0, 1.0, 1.0, "N");
00268         
00269         viz.removeShape ("M");
00270         viz.addText ((boost::format ("number of Measured PointClouds:  %d") % cloud_pass_downsampled_->points.size ()).str (),
00271                      10, 40, 20, 1.0, 1.0, 1.0, "M");
00272         
00273         viz.removeShape ("tracking");
00274         viz.addText ((boost::format ("tracking:        %f fps") % (1.0 / tracking_time_)).str (),
00275                      10, 60, 20, 1.0, 1.0, 1.0, "tracking");
00276         
00277         viz.removeShape ("downsampling");
00278         viz.addText ((boost::format ("downsampling:    %f fps") % (1.0 / downsampling_time_)).str (),
00279                      10, 80, 20, 1.0, 1.0, 1.0, "downsampling");
00280         
00281         viz.removeShape ("computation");
00282         viz.addText ((boost::format ("computation:     %f fps") % (1.0 / computation_time_)).str (),
00283                      10, 100, 20, 1.0, 1.0, 1.0, "computation");
00284 
00285         viz.removeShape ("particles");
00286         viz.addText ((boost::format ("particles:     %d") % tracker_->getParticles ()->points.size ()).str (),
00287                      10, 120, 20, 1.0, 1.0, 1.0, "particles");
00288         
00289       }
00290     }
00291     new_cloud_ = false;
00292   }
00293 
00294   void filterPassThrough (const CloudConstPtr &cloud, Cloud &result)
00295   {
00296     FPS_CALC_BEGIN;
00297     pcl::PassThrough<PointType> pass;
00298     pass.setFilterFieldName ("z");
00299     pass.setFilterLimits (0.0, 10.0);
00300     //pass.setFilterLimits (0.0, 1.5);
00301     //pass.setFilterLimits (0.0, 0.6);
00302     pass.setKeepOrganized (false);
00303     pass.setInputCloud (cloud);
00304     pass.filter (result);
00305     FPS_CALC_END("filterPassThrough");
00306   }
00307 
00308   void euclideanSegment (const CloudConstPtr &cloud,
00309                          std::vector<pcl::PointIndices> &cluster_indices)
00310   {
00311     FPS_CALC_BEGIN;
00312     pcl::EuclideanClusterExtraction<PointType> ec;
00313     KdTreePtr tree (new KdTree ());
00314     
00315     ec.setClusterTolerance (0.05); // 2cm
00316     ec.setMinClusterSize (50);
00317     ec.setMaxClusterSize (25000);
00318     //ec.setMaxClusterSize (400);
00319     ec.setSearchMethod (tree);
00320     ec.setInputCloud (cloud);
00321     ec.extract (cluster_indices);
00322     FPS_CALC_END("euclideanSegmentation");
00323   }
00324   
00325   void gridSample (const CloudConstPtr &cloud, Cloud &result, double leaf_size = 0.01)
00326   {
00327     FPS_CALC_BEGIN;
00328     double start = pcl::getTime ();
00329     pcl::VoxelGrid<PointType> grid;
00330     //pcl::ApproximateVoxelGrid<PointType> grid;
00331     grid.setLeafSize (float (leaf_size), float (leaf_size), float (leaf_size));
00332     grid.setInputCloud (cloud);
00333     grid.filter (result);
00334     //result = *cloud;
00335     double end = pcl::getTime ();
00336     downsampling_time_ = end - start;
00337     FPS_CALC_END("gridSample");
00338   }
00339   
00340   void gridSampleApprox (const CloudConstPtr &cloud, Cloud &result, double leaf_size = 0.01)
00341   {
00342     FPS_CALC_BEGIN;
00343     double start = pcl::getTime ();
00344     //pcl::VoxelGrid<PointType> grid;
00345     pcl::ApproximateVoxelGrid<PointType> grid;
00346     grid.setLeafSize (static_cast<float> (leaf_size), static_cast<float> (leaf_size), static_cast<float> (leaf_size));
00347     grid.setInputCloud (cloud);
00348     grid.filter (result);
00349     //result = *cloud;
00350     double end = pcl::getTime ();
00351     downsampling_time_ = end - start;
00352     FPS_CALC_END("gridSample");
00353   }
00354   
00355   void planeSegmentation (const CloudConstPtr &cloud,
00356                           pcl::ModelCoefficients &coefficients,
00357                           pcl::PointIndices &inliers)
00358   {
00359     FPS_CALC_BEGIN;
00360     pcl::SACSegmentation<PointType> seg;
00361     seg.setOptimizeCoefficients (true);
00362     seg.setModelType (pcl::SACMODEL_PLANE);
00363     seg.setMethodType (pcl::SAC_RANSAC);
00364     seg.setMaxIterations (1000);
00365     seg.setDistanceThreshold (0.03);
00366     seg.setInputCloud (cloud);
00367     seg.segment (inliers, coefficients);
00368     FPS_CALC_END("planeSegmentation");
00369   }
00370 
00371   void planeProjection (const CloudConstPtr &cloud,
00372                         Cloud &result,
00373                         const pcl::ModelCoefficients::ConstPtr &coefficients)
00374   {
00375     FPS_CALC_BEGIN;
00376     pcl::ProjectInliers<PointType> proj;
00377     proj.setModelType (pcl::SACMODEL_PLANE);
00378     proj.setInputCloud (cloud);
00379     proj.setModelCoefficients (coefficients);
00380     proj.filter (result);
00381     FPS_CALC_END("planeProjection");
00382   }
00383 
00384   void convexHull (const CloudConstPtr &cloud,
00385                    Cloud &,
00386                    std::vector<pcl::Vertices> &hull_vertices)
00387   {
00388     FPS_CALC_BEGIN;
00389     pcl::ConvexHull<PointType> chull;
00390     chull.setInputCloud (cloud);
00391     chull.reconstruct (*cloud_hull_, hull_vertices);
00392     FPS_CALC_END("convexHull");
00393   }
00394 
00395   void normalEstimation (const CloudConstPtr &cloud,
00396                          pcl::PointCloud<pcl::Normal> &result)
00397   {
00398     FPS_CALC_BEGIN;
00399     ne_.setInputCloud (cloud);
00400     ne_.compute (result);
00401     FPS_CALC_END("normalEstimation");
00402   }
00403   
00404   void tracking (const RefCloudConstPtr &cloud)
00405   {
00406     double start = pcl::getTime ();
00407     FPS_CALC_BEGIN;
00408     tracker_->setInputCloud (cloud);
00409     tracker_->compute ();
00410     double end = pcl::getTime ();
00411     FPS_CALC_END("tracking");
00412     tracking_time_ = end - start;
00413   }
00414 
00415   void addNormalToCloud (const CloudConstPtr &cloud,
00416                          const pcl::PointCloud<pcl::Normal>::ConstPtr &normals,
00417                          RefCloud &result)
00418   {
00419     result.width = cloud->width;
00420     result.height = cloud->height;
00421     result.is_dense = cloud->is_dense;
00422     for (size_t i = 0; i < cloud->points.size (); i++)
00423     {
00424       RefPointType point;
00425       point.x = cloud->points[i].x;
00426       point.y = cloud->points[i].y;
00427       point.z = cloud->points[i].z;
00428       point.rgba = cloud->points[i].rgba;
00429       // point.normal[0] = normals->points[i].normal[0];
00430       // point.normal[1] = normals->points[i].normal[1];
00431       // point.normal[2] = normals->points[i].normal[2];
00432       result.points.push_back (point);
00433     }
00434   }
00435 
00436   void extractNonPlanePoints (const CloudConstPtr &cloud,
00437                               const CloudConstPtr &cloud_hull,
00438                               Cloud &result)
00439   {
00440     pcl::ExtractPolygonalPrismData<PointType> polygon_extract;
00441     pcl::PointIndices::Ptr inliers_polygon (new pcl::PointIndices ());
00442     polygon_extract.setHeightLimits (0.01, 10.0);
00443     polygon_extract.setInputPlanarHull (cloud_hull);
00444     polygon_extract.setInputCloud (cloud);
00445     polygon_extract.segment (*inliers_polygon);
00446     {
00447       pcl::ExtractIndices<PointType> extract_positive;
00448       extract_positive.setNegative (false);
00449       extract_positive.setInputCloud (cloud);
00450       extract_positive.setIndices (inliers_polygon);
00451       extract_positive.filter (result);
00452     }
00453   }
00454 
00455   void removeZeroPoints (const CloudConstPtr &cloud,
00456                          Cloud &result)
00457   {
00458     for (size_t i = 0; i < cloud->points.size (); i++)
00459     {
00460       PointType point = cloud->points[i];
00461       if (!(fabs(point.x) < 0.01 &&
00462             fabs(point.y) < 0.01 &&
00463             fabs(point.z) < 0.01) &&
00464           !pcl_isnan(point.x) &&
00465           !pcl_isnan(point.y) &&
00466           !pcl_isnan(point.z))
00467         result.points.push_back(point);
00468     }
00469 
00470     result.width = static_cast<uint32_t> (result.points.size ());
00471     result.height = 1;
00472     result.is_dense = true;
00473   }
00474   
00475   void extractSegmentCluster (const CloudConstPtr &cloud,
00476                               const std::vector<pcl::PointIndices> cluster_indices,
00477                               const int segment_index,
00478                               Cloud &result)
00479   {
00480     pcl::PointIndices segmented_indices = cluster_indices[segment_index];
00481     for (size_t i = 0; i < segmented_indices.indices.size (); i++)
00482     {
00483       PointType point = cloud->points[segmented_indices.indices[i]];
00484       result.points.push_back (point);
00485     }
00486     result.width = uint32_t (result.points.size ());
00487     result.height = 1;
00488     result.is_dense = true;
00489   }
00490   
00491   void
00492   cloud_cb (const CloudConstPtr &cloud)
00493   {
00494     boost::mutex::scoped_lock lock (mtx_);
00495     double start = pcl::getTime ();
00496     FPS_CALC_BEGIN;
00497     cloud_pass_.reset (new Cloud);
00498     cloud_pass_downsampled_.reset (new Cloud);
00499     pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
00500     pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
00501     filterPassThrough (cloud, *cloud_pass_);
00502     if (counter_ < 10)
00503     {
00504       gridSample (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_);
00505     }
00506     else if (counter_ == 10)
00507     {
00508       //gridSample (cloud_pass_, *cloud_pass_downsampled_, 0.01);
00509       cloud_pass_downsampled_ = cloud_pass_;
00510       CloudPtr target_cloud;
00511       if (use_convex_hull_)
00512       {
00513         planeSegmentation (cloud_pass_downsampled_, *coefficients, *inliers);
00514         if (inliers->indices.size () > 3)
00515         {
00516           CloudPtr cloud_projected (new Cloud);
00517           cloud_hull_.reset (new Cloud);
00518           nonplane_cloud_.reset (new Cloud);
00519           
00520           planeProjection (cloud_pass_downsampled_, *cloud_projected, coefficients);
00521           convexHull (cloud_projected, *cloud_hull_, hull_vertices_);
00522           
00523           extractNonPlanePoints (cloud_pass_downsampled_, cloud_hull_, *nonplane_cloud_);
00524           target_cloud = nonplane_cloud_;
00525         }
00526         else
00527         {
00528           PCL_WARN ("cannot segment plane\n");
00529         }
00530       }
00531       else
00532       {
00533         PCL_WARN ("without plane segmentation\n");
00534         target_cloud = cloud_pass_downsampled_;
00535       }
00536       
00537       if (target_cloud != NULL)
00538       {
00539         PCL_INFO ("segmentation, please wait...\n");
00540         std::vector<pcl::PointIndices> cluster_indices;
00541         euclideanSegment (target_cloud, cluster_indices);
00542         if (cluster_indices.size () > 0)
00543         {
00544           // select the cluster to track
00545           CloudPtr temp_cloud (new Cloud);
00546           extractSegmentCluster (target_cloud, cluster_indices, 0, *temp_cloud);
00547           Eigen::Vector4f c;
00548           pcl::compute3DCentroid<RefPointType> (*temp_cloud, c);
00549           int segment_index = 0;
00550           double segment_distance = c[0] * c[0] + c[1] * c[1];
00551           for (size_t i = 1; i < cluster_indices.size (); i++)
00552           {
00553             temp_cloud.reset (new Cloud);
00554             extractSegmentCluster (target_cloud, cluster_indices, int (i), *temp_cloud);
00555             pcl::compute3DCentroid<RefPointType> (*temp_cloud, c);
00556             double distance = c[0] * c[0] + c[1] * c[1];
00557             if (distance < segment_distance)
00558             {
00559               segment_index = int (i);
00560               segment_distance = distance;
00561             }
00562           }
00563           
00564           segmented_cloud_.reset (new Cloud);
00565           extractSegmentCluster (target_cloud, cluster_indices, segment_index, *segmented_cloud_);
00566           //pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
00567           //normalEstimation (segmented_cloud_, *normals);
00568           RefCloudPtr ref_cloud (new RefCloud);
00569           //addNormalToCloud (segmented_cloud_, normals, *ref_cloud);
00570           ref_cloud = segmented_cloud_;
00571           RefCloudPtr nonzero_ref (new RefCloud);
00572           removeZeroPoints (ref_cloud, *nonzero_ref);
00573           
00574           PCL_INFO ("calculating cog\n");
00575           
00576           RefCloudPtr transed_ref (new RefCloud);
00577           pcl::compute3DCentroid<RefPointType> (*nonzero_ref, c);
00578           Eigen::Affine3f trans = Eigen::Affine3f::Identity ();
00579           trans.translation () = Eigen::Vector3f (c[0], c[1], c[2]);
00580           //pcl::transformPointCloudWithNormals<RefPointType> (*ref_cloud, *transed_ref, trans.inverse());
00581           pcl::transformPointCloud<RefPointType> (*nonzero_ref, *transed_ref, trans.inverse());
00582           CloudPtr transed_ref_downsampled (new Cloud);
00583           gridSample (transed_ref, *transed_ref_downsampled, downsampling_grid_size_);
00584           tracker_->setReferenceCloud (transed_ref_downsampled);
00585           tracker_->setTrans (trans);
00586           reference_ = transed_ref;
00587           tracker_->setMinIndices (int (ref_cloud->points.size ()) / 2);
00588         }
00589         else
00590         {
00591           PCL_WARN ("euclidean segmentation failed\n");
00592         }
00593       }
00594     }
00595     else
00596     {
00597       //normals_.reset (new pcl::PointCloud<pcl::Normal>);
00598       //normalEstimation (cloud_pass_downsampled_, *normals_);
00599       //RefCloudPtr tracking_cloud (new RefCloud ());
00600       //addNormalToCloud (cloud_pass_downsampled_, normals_, *tracking_cloud);
00601       //tracking_cloud = cloud_pass_downsampled_;
00602       
00603       //*cloud_pass_downsampled_ = *cloud_pass_;
00604       //cloud_pass_downsampled_ = cloud_pass_;
00605       gridSampleApprox (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_);
00606       tracking (cloud_pass_downsampled_);
00607     }
00608     
00609     new_cloud_ = true;
00610     double end = pcl::getTime ();
00611     computation_time_ = end - start;
00612     FPS_CALC_END("computation");
00613     counter_++;
00614   }
00615       
00616   void
00617   run ()
00618   {
00619     pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id_);
00620     boost::function<void (const CloudConstPtr&)> f =
00621       boost::bind (&OpenNISegmentTracking::cloud_cb, this, _1);
00622     interface->registerCallback (f);
00623     
00624     viewer_.runOnVisualizationThread (boost::bind(&OpenNISegmentTracking::viz_cb, this, _1), "viz_cb");
00625     
00626     interface->start ();
00627       
00628     while (!viewer_.wasStopped ())
00629       boost::this_thread::sleep(boost::posix_time::seconds(1));
00630     interface->stop ();
00631   }
00632   
00633   pcl::visualization::CloudViewer viewer_;
00634   pcl::PointCloud<pcl::Normal>::Ptr normals_;
00635   CloudPtr cloud_pass_;
00636   CloudPtr cloud_pass_downsampled_;
00637   CloudPtr plane_cloud_;
00638   CloudPtr nonplane_cloud_;
00639   CloudPtr cloud_hull_;
00640   CloudPtr segmented_cloud_;
00641   CloudPtr reference_;
00642   std::vector<pcl::Vertices> hull_vertices_;
00643   
00644   std::string device_id_;
00645   boost::mutex mtx_;
00646   bool new_cloud_;
00647   pcl::NormalEstimationOMP<PointType, pcl::Normal> ne_; // to store threadpool
00648   boost::shared_ptr<ParticleFilter> tracker_;
00649   int counter_;
00650   bool use_convex_hull_;
00651   bool visualize_non_downsample_;
00652   bool visualize_particles_;
00653   double tracking_time_;
00654   double computation_time_;
00655   double downsampling_time_;
00656   double downsampling_grid_size_;
00657   };
00658 
00659 void
00660 usage (char** argv)
00661 {
00662   std::cout << "usage: " << argv[0] << " <device_id> [-C] [-g]\n\n";
00663   std::cout << "  -C:  initialize the pointcloud to track without plane segmentation"
00664             << std::endl;
00665   std::cout << "  -D: visualizing with non-downsampled pointclouds."
00666             << std::endl;
00667   std::cout << "  -P: not visualizing particle cloud."
00668             << std::endl;
00669   std::cout << "  -fixed: use the fixed number of the particles."
00670             << std::endl;
00671   std::cout << "  -d <value>: specify the grid size of downsampling (defaults to 0.01)."
00672             << std::endl;
00673 }
00674 
00675 int
00676 main (int argc, char** argv)
00677 {
00678   bool use_convex_hull = true;
00679   bool visualize_non_downsample = false;
00680   bool visualize_particles = true;
00681   bool use_fixed = false;
00682 
00683   double downsampling_grid_size = 0.01;
00684   
00685   if (pcl::console::find_argument (argc, argv, "-C") > 0)
00686     use_convex_hull = false;
00687   if (pcl::console::find_argument (argc, argv, "-D") > 0)
00688     visualize_non_downsample = true;
00689   if (pcl::console::find_argument (argc, argv, "-P") > 0)
00690     visualize_particles = false;
00691   if (pcl::console::find_argument (argc, argv, "-fixed") > 0)
00692     use_fixed = true;
00693   pcl::console::parse_argument (argc, argv, "-d", downsampling_grid_size);
00694   if (argc < 2)
00695   {
00696     usage (argv);
00697     exit (1);
00698   }
00699   
00700   std::string device_id = std::string (argv[1]);
00701 
00702   if (device_id == "--help" || device_id == "-h")
00703   {
00704     usage (argv);
00705     exit (1);
00706   }
00707   
00708   // open kinect
00709   OpenNISegmentTracking<pcl::PointXYZRGBA> v (device_id, 8, downsampling_grid_size,
00710                                               use_convex_hull,
00711                                               visualize_non_downsample, visualize_particles,
00712                                               use_fixed);
00713   v.run ();
00714 }


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:16:06