From 2280b54f7f6cca5b515fcb307140e7cda401d9ef Mon Sep 17 00:00:00 2001 From: yuto inagaki Date: Wed, 15 May 2013 06:02:47 +0900 Subject: [PATCH] revise and shorten source code --- .../content/sources/tracking/CMakeLists.txt | 4 +- .../sources/tracking/segment_reference.cpp | 210 ------------------ .../sources/tracking/segment_reference.h | 110 --------- .../sources/tracking/tracking_sample.cpp | 166 ++++++++------ doc/tutorials/content/tracking.rst | 181 ++++++--------- 5 files changed, 162 insertions(+), 509 deletions(-) delete mode 100644 doc/tutorials/content/sources/tracking/segment_reference.cpp delete mode 100644 doc/tutorials/content/sources/tracking/segment_reference.h diff --git a/doc/tutorials/content/sources/tracking/CMakeLists.txt b/doc/tutorials/content/sources/tracking/CMakeLists.txt index 3eabd15c4bb..6edbeaedd37 100644 --- a/doc/tutorials/content/sources/tracking/CMakeLists.txt +++ b/doc/tutorials/content/sources/tracking/CMakeLists.txt @@ -8,5 +8,5 @@ include_directories(${PCL_INCLUDE_DIRS}) link_directories(${PCL_LIBRARY_DIRS}) add_definitions(${PCL_DEFINITIONS}) -add_executable (openni_tracking_sample src/openni_tracking_sample.cpp src/segment_reference.cpp) -target_link_libraries (openni_tracking_sample ${PCL_LIBRARIES}) +add_executable (tracking_sample tracking_sample.cpp) +target_link_libraries (tracking_sample ${PCL_LIBRARIES}) diff --git a/doc/tutorials/content/sources/tracking/segment_reference.cpp b/doc/tutorials/content/sources/tracking/segment_reference.cpp deleted file mode 100644 index 85302eb47e0..00000000000 --- a/doc/tutorials/content/sources/tracking/segment_reference.cpp +++ /dev/null @@ -1,210 +0,0 @@ -#include -#include -#include -#include - -#include -#include -#include - -#include -#include - -#include "segment_reference.h" - -using namespace pcl::tracking; - -pcl::visualization::CloudViewer* viewer_; -CloudPtr cloud_pass_; -CloudPtr cloud_pass_downsampled_; -CloudPtr plane_cloud_; -CloudPtr nonplane_cloud_; -CloudPtr cloud_hull_; -CloudPtr segmented_cloud_; -CloudPtr reference_; -std::vector hull_vertices_; - -std::string device_id_; -boost::mutex mtx_; -bool new_cloud_; -boost::shared_ptr tracker_; -int counter_; -double downsampling_grid_size_; - -void extractNonPlanePoints (const CloudConstPtr &cloud, - const CloudConstPtr &cloud_hull, - Cloud &result) -{ - pcl::ExtractPolygonalPrismData polygon_extract; - pcl::PointIndices::Ptr inliers_polygon (new pcl::PointIndices ()); - polygon_extract.setHeightLimits (0.01, 10.0); - polygon_extract.setInputPlanarHull (cloud_hull); - polygon_extract.setInputCloud (cloud); - polygon_extract.segment (*inliers_polygon); - { - pcl::ExtractIndices extract_positive; - extract_positive.setNegative (false); - extract_positive.setInputCloud (cloud); - extract_positive.setIndices (inliers_polygon); - extract_positive.filter (result); - } -} - -//Filter along a specified dimension -void filterPassThrough (const CloudConstPtr &cloud, Cloud &result) -{ - pcl::PassThrough pass; - pass.setFilterFieldName ("z"); - pass.setFilterLimits (0.0, 10.0); - pass.setKeepOrganized (false); - pass.setInputCloud (cloud); - pass.filter (result); -} - - -//Compute and get cluster_indies -void euclideanSegment (const CloudConstPtr &cloud, - std::vector &cluster_indices) -{ - pcl::EuclideanClusterExtraction ec; - KdTreePtr tree (new KdTree ()); - - ec.setClusterTolerance (0.05); // 2cm - ec.setMinClusterSize (50); - ec.setMaxClusterSize (25000); - ec.setSearchMethod (tree); - ec.setInputCloud (cloud); - ec.extract (cluster_indices); -} - -// -void gridSampleApprox (const CloudConstPtr &cloud, Cloud &result, double leaf_size) -{ - pcl::ApproximateVoxelGrid grid; - grid.setLeafSize (static_cast (leaf_size), static_cast (leaf_size), static_cast (leaf_size)); - grid.setInputCloud (cloud); - grid.filter (result); -} - -//plane segment -void planeSegmentation (const CloudConstPtr &cloud, - pcl::ModelCoefficients &coefficients, - pcl::PointIndices &inliers) -{ - pcl::SACSegmentation seg; - seg.setOptimizeCoefficients (true); - seg.setModelType (pcl::SACMODEL_PLANE); - seg.setMethodType (pcl::SAC_RANSAC); - seg.setMaxIterations (1000); - seg.setDistanceThreshold (0.03); - seg.setInputCloud (cloud); - seg.segment (inliers, coefficients); -} - - -void planeProjection (const CloudConstPtr &cloud, - Cloud &result, - const pcl::ModelCoefficients::ConstPtr &coefficients) -{ - pcl::ProjectInliers proj; - proj.setModelType (pcl::SACMODEL_PLANE); - proj.setInputCloud (cloud); - proj.setModelCoefficients (coefficients); - proj.filter (result); -} - - -void extractSegmentCluster (const CloudConstPtr &cloud, - const std::vector cluster_indices, - const int segment_index, - Cloud &result) -{ - pcl::PointIndices segmented_indices = cluster_indices[segment_index]; - for (size_t i = 0; i < segmented_indices.indices.size (); i++) - { - pcl::PointXYZRGBA point = cloud->points[segmented_indices.indices[i]]; - result.points.push_back (point); - } - result.width = uint32_t (result.points.size ()); - result.height = 1; - result.is_dense = true; -} - -//Create convexHull -void convexHull (const CloudConstPtr &cloud, - Cloud &, - std::vector &hull_vertices) -{ - pcl::ConvexHull chull; - chull.setInputCloud (cloud); - chull.reconstruct (*cloud_hull_, hull_vertices); -} - -//Decide model reference -void initialize_segemnted_reference(CloudPtr target_cloud, CloudPtr& transed_ref_downsampled, Eigen::Affine3f& trans){ - - std::vector cluster_indices; - euclideanSegment (target_cloud, cluster_indices); - if (cluster_indices.size () > 0) - { - // select the cluster to track - CloudPtr temp_cloud (new Cloud); - extractSegmentCluster (target_cloud, cluster_indices, 0, *temp_cloud); - Eigen::Vector4f c; - pcl::compute3DCentroid (*temp_cloud, c); - int segment_index = 0; - double segment_distance = c[0] * c[0] + c[1] * c[1]; - for (size_t i = 1; i < cluster_indices.size (); i++) - { - temp_cloud.reset (new Cloud); - extractSegmentCluster (target_cloud, cluster_indices, int (i), *temp_cloud); - pcl::compute3DCentroid (*temp_cloud, c); - double distance = c[0] * c[0] + c[1] * c[1]; - if (distance < segment_distance) - { - segment_index = int (i); - segment_distance = distance; - } - } - - segmented_cloud_.reset (new Cloud); - extractSegmentCluster (target_cloud, cluster_indices, segment_index, *segmented_cloud_); - RefCloudPtr transed_ref (new RefCloud); - pcl::compute3DCentroid (*segmented_cloud_, c); - trans.translation ().matrix () = Eigen::Vector3f (c[0], c[1], c[2]); - pcl::transformPointCloud (*segmented_cloud_, *transed_ref, trans.inverse()); - - gridSampleApprox (transed_ref, *transed_ref_downsampled, downsampling_grid_size_); - reference_ = transed_ref; - } - else - { - PCL_WARN ("euclidean segmentation failed\n"); - } -} - -//plane extracting -void extractPlanes(CloudPtr &target_cloud){ - pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ()); - pcl::PointIndices::Ptr inliers (new pcl::PointIndices ()); - planeSegmentation (cloud_pass_downsampled_, *coefficients, *inliers); - if (inliers->indices.size () > 3) - { - CloudPtr cloud_projected (new Cloud); - cloud_hull_.reset (new Cloud); - nonplane_cloud_.reset (new Cloud); - - planeProjection (cloud_pass_downsampled_, *cloud_projected, coefficients); - convexHull (cloud_projected, *cloud_hull_, hull_vertices_); - extractNonPlanePoints (cloud_pass_downsampled_, cloud_hull_, *nonplane_cloud_); - target_cloud = nonplane_cloud_; - } - else - { - PCL_WARN ("cannot segment plane\n"); - } -} - - - - diff --git a/doc/tutorials/content/sources/tracking/segment_reference.h b/doc/tutorials/content/sources/tracking/segment_reference.h deleted file mode 100644 index 4500b3c18d7..00000000000 --- a/doc/tutorials/content/sources/tracking/segment_reference.h +++ /dev/null @@ -1,110 +0,0 @@ -#ifndef __SEGMENT_REFERENCE_H__ -#define __SEGMENT_REFERENCE_H__ -#include -#include -#include -#include -#include -#include - -#include -#include - -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include - -#include - -#include -#include - -#include - - -using namespace pcl::tracking; - -typedef pcl::PointXYZRGBA RefPointType; -typedef ParticleXYZRPY ParticleT; -typedef pcl::PointCloud Cloud; -typedef pcl::PointCloud RefCloud; -typedef RefCloud::Ptr RefCloudPtr; -typedef RefCloud::ConstPtr RefCloudConstPtr; -typedef Cloud::Ptr CloudPtr; -typedef Cloud::ConstPtr CloudConstPtr; -typedef ParticleFilterTracker ParticleFilter; -typedef ParticleFilter::CoherencePtr CoherencePtr; -typedef pcl::search::KdTree KdTree; -typedef KdTree::Ptr KdTreePtr; - -extern pcl::visualization::CloudViewer* viewer_; -extern CloudPtr cloud_pass_; -extern CloudPtr cloud_pass_downsampled_; -extern CloudPtr plane_cloud_; -extern CloudPtr nonplane_cloud_; -extern CloudPtr cloud_hull_; -extern CloudPtr segmented_cloud_; -extern CloudPtr reference_; -extern std::vector hull_vertices_; - -extern std::string device_id_; -extern boost::mutex mtx_; -extern bool new_cloud_; -extern boost::shared_ptr tracker_; -extern int counter_; -extern double downsampling_grid_size_; - - -void extractNonPlanePoints (const CloudConstPtr &cloud, - const CloudConstPtr &cloud_hull, - Cloud &result); - -//Filter along a specified dimension -void filterPassThrough (const CloudConstPtr &cloud, Cloud &result); - -//Compute and get cluster_indies -void euclideanSegment (const CloudConstPtr &cloud, - std::vector &cluster_indices); - -// -void gridSampleApprox (const CloudConstPtr &cloud, Cloud &result, double leaf_size = 0.01); - -//plane segment -void planeSegmentation (const CloudConstPtr &cloud, - pcl::ModelCoefficients &coefficients, - pcl::PointIndices &inliers); - - -void planeProjection (const CloudConstPtr &cloud, - Cloud &result, - const pcl::ModelCoefficients::ConstPtr &coefficients); - -void extractSegmentCluster (const CloudConstPtr &cloud, - const std::vector cluster_indices, - const int segment_index, - Cloud &result); - -//Create convex hull -void convexHull (const CloudConstPtr &cloud, - Cloud &, - std::vector &hull_vertices); - -//Decide model reference -void initialize_segemnted_reference(CloudPtr target_cloud, CloudPtr& transed_ref_downsampled, Eigen::Affine3f& trans); - -//plane extracting -void extractPlanes(CloudPtr &target_cloud); - - -#endif // __SEGMENT_REFERENCE_H__ diff --git a/doc/tutorials/content/sources/tracking/tracking_sample.cpp b/doc/tutorials/content/sources/tracking/tracking_sample.cpp index d8d92659622..3c0ea052e75 100644 --- a/doc/tutorials/content/sources/tracking/tracking_sample.cpp +++ b/doc/tutorials/content/sources/tracking/tracking_sample.cpp @@ -1,19 +1,75 @@ +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#include + #include #include #include #include - #include #include #include - #include #include -#include "segment_reference.h" - using namespace pcl::tracking; +typedef pcl::PointXYZRGBA RefPointType; +typedef ParticleXYZRPY ParticleT; +typedef pcl::PointCloud Cloud; +typedef Cloud::Ptr CloudPtr; +typedef Cloud::ConstPtr CloudConstPtr; +typedef ParticleFilterTracker ParticleFilter; + +CloudPtr cloud_pass_; +CloudPtr cloud_pass_downsampled_; +CloudPtr target_cloud; + +boost::mutex mtx_; +boost::shared_ptr tracker_; +bool new_cloud_; +double downsampling_grid_size_; +int counter; + + +//Filter along a specified dimension +void filterPassThrough (const CloudConstPtr &cloud, Cloud &result) +{ + pcl::PassThrough pass; + pass.setFilterFieldName ("z"); + pass.setFilterLimits (0.0, 10.0); + pass.setKeepOrganized (false); + pass.setInputCloud (cloud); + pass.filter (result); +} + + +void gridSampleApprox (const CloudConstPtr &cloud, Cloud &result, double leaf_size) +{ + pcl::ApproximateVoxelGrid grid; + grid.setLeafSize (static_cast (leaf_size), static_cast (leaf_size), static_cast (leaf_size)); + grid.setInputCloud (cloud); + grid.filter (result); +} //Draw the current particles @@ -21,9 +77,8 @@ bool drawParticles (pcl::visualization::PCLVisualizer& viz) { ParticleFilter::PointCloudStatePtr particles = tracker_->getParticles (); - if (particles) + if (particles && new_cloud_) { - //Set pointCloud with particle's points pcl::PointCloud::Ptr particle_cloud (new pcl::PointCloud ()); for (size_t i = 0; i < particles->points.size (); i++) @@ -39,7 +94,6 @@ drawParticles (pcl::visualization::PCLVisualizer& viz) //Draw red particles { pcl::visualization::PointCloudColorHandlerCustom red_color (particle_cloud, 250, 99, 71); -// viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "particle cloud"); if (!viz.updatePointCloud (particle_cloud, red_color, "particle cloud")) viz.addPointCloud (particle_cloud, red_color, "particle cloud"); @@ -59,21 +113,19 @@ drawResult (pcl::visualization::PCLVisualizer& viz) ParticleXYZRPY result = tracker_->getResult (); Eigen::Affine3f transformation = tracker_->toEigenMatrix (result); - // move a little bit for better visualization + //move close to camera a little for better visualization transformation.translation () += Eigen::Vector3f (0.0f, 0.0f, -0.005f); - RefCloudPtr result_cloud (new RefCloud ()); + CloudPtr result_cloud (new Cloud ()); pcl::transformPointCloud (*(tracker_->getReferenceCloud ()), *result_cloud, transformation); //Draw blue model reference point cloud { pcl::visualization::PointCloudColorHandlerCustom blue_color (result_cloud, 0, 0, 255); -// viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "resultcloud"); if (!viz.updatePointCloud (result_cloud, blue_color, "resultcloud")) viz.addPointCloud (result_cloud, blue_color, "resultcloud"); } } - //visualization's callback function void @@ -98,19 +150,13 @@ viz_cb (pcl::visualization::PCLVisualizer& viz) viz.addPointCloud (cloud_pass, "cloudpass"); viz.resetCameraViewpoint ("cloudpass"); } - } - - //Draw point cloud related with tracking particles - if (new_cloud_ && reference_) - { bool ret = drawParticles (viz); if (ret) - drawResult (viz); + drawResult (viz); } new_cloud_ = false; } - //OpenNI Grabber's cloud Callback function void cloud_cb (const CloudConstPtr &cloud) @@ -119,69 +165,40 @@ cloud_cb (const CloudConstPtr &cloud) cloud_pass_.reset (new Cloud); cloud_pass_downsampled_.reset (new Cloud); filterPassThrough (cloud, *cloud_pass_); - - //Point Cloud given at first few frames has some noise - if (counter_ < 10) - { - gridSampleApprox (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_); - } - //Make model reference and set it to tracker_ - else if (counter_ == 10) - { - cloud_pass_downsampled_ = cloud_pass_; - CloudPtr target_cloud; - - PCL_INFO ("segmentation, please wait...\n"); - //remove Plane's point cloud - extractPlanes(target_cloud); - - if (target_cloud != NULL) - { - Eigen::Vector4f c; - Eigen::Affine3f trans = Eigen::Affine3f::Identity (); - CloudPtr transed_ref_downsampled (new Cloud); - - //Make model reference - initialize_segemnted_reference(target_cloud, transed_ref_downsampled, trans); - - tracker_->setReferenceCloud (transed_ref_downsampled); - tracker_->setTrans (trans); - } - PCL_INFO ("segmentation Complete!\n"); - PCL_INFO ("Start tracking\n"); - } - //Track the object - else - { - gridSampleApprox (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_); - tracker_->setInputCloud (cloud_pass_downsampled_); - tracker_->compute (); - } - - new_cloud_ = true; - counter_++; + gridSampleApprox (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_); + + if(counter < 10){ + counter++; + }else{ + //Track the object + tracker_->setInputCloud (cloud_pass_downsampled_); + tracker_->compute (); + new_cloud_ = true; + } } - - int main (int argc, char** argv) { - - if (argc < 2) + if (argc < 3) { - PCL_WARN("Please set device_id (e.g. $ %s '#1')\n", argv[0]); + PCL_WARN("Please set device_id pcd_filename(e.g. $ %s '#1' sample.pcd)\n", argv[0]); exit (1); } + //read pcd file + target_cloud.reset(new Cloud()); + if(pcl::io::loadPCDFile (argv[2], *target_cloud) == -1){ + std::cout << "pcd file not found" << std::endl; + exit(-1); + } + std::string device_id = std::string (argv[1]); - device_id_ = device_id; - viewer_ = new pcl::visualization::CloudViewer("PCL OpenNI Tracking Viewer"); + counter = 0; //Set parameters new_cloud_ = false; - counter_ = 0; downsampling_grid_size_ = 0.002; std::vector default_step_covariance = std::vector (6, 0.015 * 0.015); @@ -236,9 +253,23 @@ main (int argc, char** argv) tracker_->setCloudCoherence (coherence); + //prepare the model of tracker's target + Eigen::Vector4f c; + Eigen::Affine3f trans = Eigen::Affine3f::Identity (); + CloudPtr transed_ref (new Cloud); + CloudPtr transed_ref_downsampled (new Cloud); + pcl::compute3DCentroid (*target_cloud, c); + trans.translation ().matrix () = Eigen::Vector3f (c[0], c[1], c[2]); + pcl::transformPointCloud (*target_cloud, *transed_ref, trans.inverse()); + gridSampleApprox (transed_ref, *transed_ref_downsampled, downsampling_grid_size_); + + //set reference model and trans + tracker_->setReferenceCloud (transed_ref_downsampled); + tracker_->setTrans (trans); //Setup OpenNIGrabber and viewer + pcl::visualization::CloudViewer* viewer_ = new pcl::visualization::CloudViewer("PCL OpenNI Tracking Viewer"); pcl::Grabber* interface = new pcl::OpenNIGrabber (device_id); boost::function f = boost::bind (&cloud_cb, _1); @@ -251,5 +282,4 @@ main (int argc, char** argv) while (!viewer_->wasStopped ()) boost::this_thread::sleep(boost::posix_time::seconds(1)); interface->stop(); - } diff --git a/doc/tutorials/content/tracking.rst b/doc/tutorials/content/tracking.rst index 77d3a838b50..e503b6123f5 100644 --- a/doc/tutorials/content/tracking.rst +++ b/doc/tutorials/content/tracking.rst @@ -2,15 +2,27 @@ Tracking object in real time ---------------------------- +This tutorial explains 6D object tracking and show example code(tracking_sample.cpp) using pcl::tracking libraries. Implementing this example code, you can see the segment track the target object even if you move tracked object or your sensor device. In example, first, you should initialize tracker and you have to pass target object's point cloud to tracker so that tracker should know what to track. So, before this tutorial, you need to make segmented model with PCD file beforehand. Setting the model to tracker, it starts tracking the object. + +Following figure shows how looks like when trakcing works successfully. + +.. figure:: images/tracking/mergePicture.png + :height: 600 + :align: center + + fig1: The blue model tracks the cup successfully with red particles. -The pcl_tracking library contains data structures and mechanism for 3D tracking which uses Particle Filter Algorithm. This tracking will enable you to implement 6D-pose (position and rotation) tracking which is optimized to run in real time. In order to make reference model, the tracker at first separates point cloud of the target object from others. Then it starts tracking the object with Particles at each loop of function. +Details +------- +The pcl_tracking library contains data structures and mechanism for 3D tracking which uses Particle Filter Algorithm. This tracking will enable you to implement 6D-pose (position and rotation) tracking which is optimized to run in real time. -At each loop, tracking program proceeds along with following algorythm. + +At each loop, tracking program proceeds along with following algorythm.(see fig2) 1. (At t = t - 1) At first, using previous Pariticle's information about position and rotation, it will predict each position and rotation of them at the next frame. - 2. Next, we calculate weights of those particles with the likelihood formula below.(you can select whick likelihood function you use) + 2. Next, we calculate weights of those particles with the likelihood formula below.(you can select which likelihood function you use) 3. Finally, we use the evaluate function which compares real point cloud data from depth sensor with the predicted particles, and resample particles. @@ -23,145 +35,93 @@ At each loop, tracking program proceeds along with following algorythm. -.. image:: images/tracking/slideCapture.png +.. figure:: images/tracking/slideCapture.png :height: 400 + :align: center -Following figure shows how looks like when trakcing works successfully. + fig2: The process of tracking Particle Filter -.. image:: images/tracking/mergePicture.png - :height: 600 The code -------- -Create two files, paste following code with your editor and save each file as segment_reference.h, segment_reference.cpp and tracking_sample.cpp. +Create three files, paste following code with your editor and save it as tracking_sample.cpp. -segment_reference.h +tracking_sample.cpp -.. literalinclude:: sources/tracking/segment_reference.h +.. literalinclude:: sources/tracking/tracking_sample.cpp :language: cpp :linenos: -segment_reference.cpp +The explanation +--------------- -.. literalinclude:: sources/tracking/segment_reference.cpp +Now, let's break down the code piece by piece. + +.. literalinclude:: sources/tracking/tracking_sample.cpp :language: cpp - :linenos: + :lines: 224-239 -tracking_sample.cpp + +First, in main function, these lines set the parameters for tracking. .. literalinclude:: sources/tracking/tracking_sample.cpp :language: cpp - :linenos: + :lines: 243-254 -The explanation ---------------- +Here, we set likelihood function which tracker use when calculate weights. You can add more likelihood function as you like. By default, there are normals likelihood and color likelihood functions. When you want to add other likelihood function, all you have to do is initialize new Coherence Class and add the Coherence instance to coherence variable with addPointCoherence function. -Now, let's break down the code piece by piece. We focus on openni_tracking_sample.cpp which will teach you how to use tracking library. segment_reference.cpp is just for extracting tracking object from other point cloud and planes. +.. literalinclude:: sources/tracking/tracking_sample.cpp + :language: cpp + :lines: 256-269 -.. code-block:: cpp - :linenos: +In this part, we set the point cloud loaded from pcd file as reference model to tracker and also set model's transform values. - //Set all parameters for KLDAdaptiveParticleFilterOMPTracker - tracker->setMaximumParticleNum (500); - tracker->setDelta (0.99); - tracker->setEpsilon (0.2); - tracker->setBinSize (bin_size); - - //Set all parameters for ParticleFilter - tracker_ = tracker; - tracker_->setTrans (Eigen::Affine3f::Identity ()); - tracker_->setStepNoiseCovariance (default_step_covariance); - tracker_->setInitialNoiseCovariance (initial_noise_covariance); - tracker_->setInitialNoiseMean (default_initial_mean); - tracker_->setIterationNum (1); - tracker_->setParticleNum (400); - tracker_->setResampleLikelihoodThr(0.00); - tracker_->setUseNormal (false); +.. literalinclude:: sources/tracking/tracking_sample.cpp + :language: cpp + :lines: 170-177 -First, in main function, these lines set the parameters for tracking. -.. code-block:: cpp - :linenos: +Until the counter variable become equal to 10, we ignore the input point cloud, because the point cloud at first few frames often have noise. After counter variable reach to 10 frame, at each loop, we set downsampled input point cloud to tracker and the tracker will compute particles movement. - //Setup coherence object for tracking - ApproxNearestPairPointCloudCoherence::Ptr coherence = ApproxNearestPairPointCloudCoherence::Ptr(new ApproxNearestPairPointCloudCoherence ()); - - boost::shared_ptr > distance_coherence - = boost::shared_ptr > (new DistanceCoherence ()); - coherence->addPointCoherence (distance_coherence); +.. literalinclude:: sources/tracking/tracking_sample.cpp + :language: cpp + :lines: 79-79 - boost::shared_ptr > search (new pcl::search::Octree (0.01)); - coherence->setSearchMethod (search); - coherence->setMaximumDistance (0.01); - tracker_->setCloudCoherence (coherence); +In drawParticles function, you can get particles's positions by calling getParticles(). -Here, we set likelihood function which tracker use when calculate weights. You can add more likelihood function as you like. By default, there are normals likelihood and color likelihood functions. +.. literalinclude:: sources/tracking/tracking_sample.cpp + :language: cpp + :lines: 113-114 -.. code-block:: cpp - :linenos: - if (counter_ < 10) - { - gridSampleApprox (cloud_pass_, *cloud_pass_downsampled\_, downsampling_grid_size_); - } - //Make model reference and set it to tracker_ - else if (counter_ == 10) - { - cloud_pass_downsampled_ = cloud_pass_; - CloudPtr target_cloud; - - PCL_INFO ("segmentation, please wait...\n"); - //remove Plane's point cloud - extractPlanes(target_cloud); - - if (target_cloud != NULL) - { - Eigen::Vector4f c; - Eigen::Affine3f trans = Eigen::Affine3f::Identity (); - CloudPtr transed_ref_downsampled (new Cloud); - - //Make model reference - initialize_segemnted_reference(target_cloud, transed_ref_downsampled, trans); - - tracker_->setReferenceCloud (transed_ref_downsampled); - tracker_->setTrans (trans); - } - PCL_INFO ("segmentation Complete!\n"); - PCL_INFO ("Start tracking\n"); - } - //Track the object - else - { - gridSampleApprox (cloud_pass_, *cloud_pass_downsampled_, downsampling_grid_size_); - tracker_->setInputCloud (cloud_pass_downsampled_); - tracker_->compute (); - } - -Until the counter\_ variable become equal to 10, we ignore the input point cloud, because the point cloud at first few frames often have noise. At 10 frame, in order to make reference point cloud, we remove plane's point cloud from other, downsample and extract the segment of model. We give the model segment and segment's centroid to tracker\_. At each loop, we set input point cloud to tracker and the tracker will compute particles movement. - - ParticleFilter::PointCloudStatePtr particles = tracker\_->getParticles (); - -Finallly, in drawParticles function, you can get particles's positions by calling getParticles(). +In drawResult function, you can get model infomation about position and rotation. Compiling and running the program --------------------------------- -Create a CmakeLists.txt file and add the following lines into it. +Create a CMakeLists.txt file and add the following lines into it. .. literalinclude:: sources/tracking/CMakeLists.txt :language: cmake :linenos: -After you created the executable, you can then launch it. -Before you start tracking, we recommend that you should put the object you want to track in the center of screen on a plane where there is nothing without the object.(Please specify the device_id as second argument.): +If you finish saving CMakeLists.txt, let's prepare for running. + + 1. Put the target object on a plane where there is nothing. + 2. Put sensor device about 1 meter away from target. + 3. Don't move the target and the device until you launch tracking program. + 4. Output only target point cloud with your other code (See :ref:`planar_segmentation` tutorial) and save as tracking_target.pcd + +After you created model point cloud and the executable, you can then launch tracking_sample. Set device_id as second arguement and pcd file's name you made in above 4 as third. + + $ ./tracking_sample “#1” tracking_target.pcd - $ ./openni_tracking sample “#1” +After few seconds, tracking will start working and you can move tracking object around. As you can see in following pictures, the blue point cloud is reference model segmentation's cloud and the red one is particles' cloud. -After few seconds, you may see “segmentation Complete!” in terminal and you can move tracking object around. As you can see in following pictures, the blue point cloud is reference model segmentation's cloud and the red one is particles' cloud. .. image:: images/tracking/redone.png :height: 400 @@ -170,24 +130,7 @@ After few seconds, you may see “segmentation Complete!” in terminal and you :height: 400 - - - - - - - - - - - - - - - - - - - - +More Advanced +------------- +If you want to see more flexible and useful tracking code which starts tracking without preparing to make segemented model beforehand, you should refer a tracking code https://github.com/aginika/pcl/blob/master/apps/src/openni_tracking.cpp. It will show you better and more legible code. The above Figures are windows when you implement that code.