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.
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.(see fig2)
- (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.
- Next, we calculate weights of those particles with the likelihood formula below.(you can select which likelihood function you use)
- Finally, we use the evaluate function which compares real point cloud data from depth sensor with the predicted particles, and resample particles.
The code
Create three files, paste following code with your editor and save it as tracking_sample.cpp.
tracking_sample.cpp
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 | #include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/openni_grabber.h>
#include <pcl/console/parse.h>
#include <pcl/common/time.h>
#include <pcl/common/centroid.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/approximate_voxel_grid.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/search/pcl_search.h>
#include <pcl/common/transforms.h>
#include <boost/format.hpp>
#include <pcl/tracking/tracking.h>
#include <pcl/tracking/particle_filter.h>
#include <pcl/tracking/kld_adaptive_particle_filter_omp.h>
#include <pcl/tracking/particle_filter_omp.h>
#include <pcl/tracking/coherence.h>
#include <pcl/tracking/distance_coherence.h>
#include <pcl/tracking/hsv_color_coherence.h>
#include <pcl/tracking/approx_nearest_pair_point_cloud_coherence.h>
#include <pcl/tracking/nearest_pair_point_cloud_coherence.h>
using namespace pcl::tracking;
typedef pcl::PointXYZRGBA RefPointType;
typedef ParticleXYZRPY ParticleT;
typedef pcl::PointCloud<pcl::PointXYZRGBA> Cloud;
typedef Cloud::Ptr CloudPtr;
typedef Cloud::ConstPtr CloudConstPtr;
typedef ParticleFilterTracker<RefPointType, ParticleT> ParticleFilter;
CloudPtr cloud_pass_;
CloudPtr cloud_pass_downsampled_;
CloudPtr target_cloud;
boost::mutex mtx_;
boost::shared_ptr<ParticleFilter> tracker_;
bool new_cloud_;
double downsampling_grid_size_;
int counter;
//Filter along a specified dimension
void filterPassThrough (const CloudConstPtr &cloud, Cloud &result)
{
pcl::PassThrough<pcl::PointXYZRGBA> 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<pcl::PointXYZRGBA> grid;
grid.setLeafSize (static_cast<float> (leaf_size), static_cast<float> (leaf_size), static_cast<float> (leaf_size));
grid.setInputCloud (cloud);
grid.filter (result);
}
//Draw the current particles
bool
drawParticles (pcl::visualization::PCLVisualizer& viz)
{
ParticleFilter::PointCloudStatePtr particles = tracker_->getParticles ();
if (particles && new_cloud_)
{
//Set pointCloud with particle's points
pcl::PointCloud<pcl::PointXYZ>::Ptr particle_cloud (new pcl::PointCloud<pcl::PointXYZ> ());
for (size_t i = 0; i < particles->points.size (); i++)
{
pcl::PointXYZ point;
point.x = particles->points[i].x;
point.y = particles->points[i].y;
point.z = particles->points[i].z;
particle_cloud->points.push_back (point);
}
//Draw red particles
{
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> red_color (particle_cloud, 250, 99, 71);
if (!viz.updatePointCloud (particle_cloud, red_color, "particle cloud"))
viz.addPointCloud (particle_cloud, red_color, "particle cloud");
}
return true;
}
else
{
return false;
}
}
//Draw model reference point cloud
void
drawResult (pcl::visualization::PCLVisualizer& viz)
{
ParticleXYZRPY result = tracker_->getResult ();
Eigen::Affine3f transformation = tracker_->toEigenMatrix (result);
//move close to camera a little for better visualization
transformation.translation () += Eigen::Vector3f (0.0f, 0.0f, -0.005f);
CloudPtr result_cloud (new Cloud ());
pcl::transformPointCloud<RefPointType> (*(tracker_->getReferenceCloud ()), *result_cloud, transformation);
//Draw blue model reference point cloud
{
pcl::visualization::PointCloudColorHandlerCustom<RefPointType> blue_color (result_cloud, 0, 0, 255);
if (!viz.updatePointCloud (result_cloud, blue_color, "resultcloud"))
viz.addPointCloud (result_cloud, blue_color, "resultcloud");
}
}
//visualization's callback function
void
viz_cb (pcl::visualization::PCLVisualizer& viz)
{
boost::mutex::scoped_lock lock (mtx_);
if (!cloud_pass_)
{
boost::this_thread::sleep (boost::posix_time::seconds (1));
return;
}
//Draw downsampled point cloud from sensor
if (new_cloud_ && cloud_pass_downsampled_)
{
CloudPtr cloud_pass;
cloud_pass = cloud_pass_downsampled_;
if (!viz.updatePointCloud (cloud_pass, "cloudpass"))
{
viz.addPointCloud (cloud_pass, "cloudpass");
viz.resetCameraViewpoint ("cloudpass");
}
bool ret = drawParticles (viz);
if (ret)
drawResult (viz);
}
new_cloud_ = false;
}
//OpenNI Grabber's cloud Callback function
void
cloud_cb (const CloudConstPtr &cloud)
{
boost::mutex::scoped_lock lock (mtx_);
cloud_pass_.reset (new Cloud);
cloud_pass_downsampled_.reset (new Cloud);
filterPassThrough (cloud, *cloud_pass_);
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 < 3)
{
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]);
counter = 0;
//Set parameters
new_cloud_ = false;
downsampling_grid_size_ = 0.002;
std::vector<double> default_step_covariance = std::vector<double> (6, 0.015 * 0.015);
default_step_covariance[3] *= 40.0;
default_step_covariance[4] *= 40.0;
default_step_covariance[5] *= 40.0;
std::vector<double> initial_noise_covariance = std::vector<double> (6, 0.00001);
std::vector<double> default_initial_mean = std::vector<double> (6, 0.0);
boost::shared_ptr<KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT> > tracker
(new KLDAdaptiveParticleFilterOMPTracker<RefPointType, ParticleT> (8));
ParticleT bin_size;
bin_size.x = 0.1f;
bin_size.y = 0.1f;
bin_size.z = 0.1f;
bin_size.roll = 0.1f;
bin_size.pitch = 0.1f;
bin_size.yaw = 0.1f;
//Set all parameters for KLDAdaptiveParticleFilterOMPTracker
tracker->setMaximumParticleNum (1000);
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 (600);
tracker_->setResampleLikelihoodThr(0.00);
tracker_->setUseNormal (false);
//Setup coherence object for tracking
ApproxNearestPairPointCloudCoherence<RefPointType>::Ptr coherence = ApproxNearestPairPointCloudCoherence<RefPointType>::Ptr
(new ApproxNearestPairPointCloudCoherence<RefPointType> ());
boost::shared_ptr<DistanceCoherence<RefPointType> > distance_coherence
= boost::shared_ptr<DistanceCoherence<RefPointType> > (new DistanceCoherence<RefPointType> ());
coherence->addPointCoherence (distance_coherence);
boost::shared_ptr<pcl::search::Octree<RefPointType> > search (new pcl::search::Octree<RefPointType> (0.01));
coherence->setSearchMethod (search);
coherence->setMaximumDistance (0.01);
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<RefPointType> (*target_cloud, c);
trans.translation ().matrix () = Eigen::Vector3f (c[0], c[1], c[2]);
pcl::transformPointCloud<RefPointType> (*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<void (const CloudConstPtr&)> f =
boost::bind (&cloud_cb, _1);
interface->registerCallback (f);
viewer_->runOnVisualizationThread (boost::bind(&viz_cb, _1), "viz_cb");
//Start viewer and object tracking
interface->start();
while (!viewer_->wasStopped ())
boost::this_thread::sleep(boost::posix_time::seconds(1));
interface->stop();
}
|
The explanation
Now, let’s break down the code piece by piece.
//Set all parameters for KLDAdaptiveParticleFilterOMPTracker
tracker->setMaximumParticleNum (1000);
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 (600);
tracker_->setResampleLikelihoodThr(0.00);
tracker_->setUseNormal (false);
First, in main function, these lines set the parameters for tracking.
ApproxNearestPairPointCloudCoherence<RefPointType>::Ptr coherence = ApproxNearestPairPointCloudCoherence<RefPointType>::Ptr
(new ApproxNearestPairPointCloudCoherence<RefPointType> ());
boost::shared_ptr<DistanceCoherence<RefPointType> > distance_coherence
= boost::shared_ptr<DistanceCoherence<RefPointType> > (new DistanceCoherence<RefPointType> ());
coherence->addPointCoherence (distance_coherence);
boost::shared_ptr<pcl::search::Octree<RefPointType> > search (new pcl::search::Octree<RefPointType> (0.01));
coherence->setSearchMethod (search);
coherence->setMaximumDistance (0.01);
tracker_->setCloudCoherence (coherence);
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.
//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<RefPointType> (*target_cloud, c);
trans.translation ().matrix () = Eigen::Vector3f (c[0], c[1], c[2]);
pcl::transformPointCloud<RefPointType> (*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);
In this part, we set the point cloud loaded from pcd file as reference model to tracker and also set model’s transform values.
if(counter < 10){
counter++;
}else{
//Track the object
tracker_->setInputCloud (cloud_pass_downsampled_);
tracker_->compute ();
new_cloud_ = true;
}
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.
ParticleFilter::PointCloudStatePtr particles = tracker_->getParticles ();
In drawParticles function, you can get particles’s positions by calling getParticles().
ParticleXYZRPY result = tracker_->getResult ();
Eigen::Affine3f transformation = tracker_->toEigenMatrix (result);
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.
1 2 3 4 5 6 7 8 9 10 11 12 | cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
project(openni_tracking)
find_package(PCL 1.7 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable (tracking_sample tracking_sample.cpp)
target_link_libraries (tracking_sample ${PCL_LIBRARIES})
|
If you finish saving CMakeLists.txt, let’s prepare for running.
- Put the target object on a plane where there is nothing.
- Put sensor device about 1 meter away from target.
- Don’t move the target and the device until you launch tracking program.
- Output only target point cloud with your other code (See Plane model 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
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.


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.