00001 #ifndef PCL_TRACKING_IMPL_PARTICLE_FILTER_H_
00002 #define PCL_TRACKING_IMPL_PARTICLE_FILTER_H_
00003
00004 #include <boost/random.hpp>
00005
00006 #include <pcl/common/common.h>
00007 #include <pcl/common/eigen.h>
00008 #include <pcl/common/transforms.h>
00009
00010 template <typename PointInT, typename StateT> bool
00011 pcl::tracking::ParticleFilterTracker<PointInT, StateT>::initCompute ()
00012 {
00013 if (!Tracker<PointInT, StateT>::initCompute ())
00014 {
00015 PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
00016 return (false);
00017 }
00018
00019 if (transed_reference_vector_.empty ())
00020 {
00021
00022 transed_reference_vector_.resize (particle_num_);
00023 for (int i = 0; i < particle_num_; i++)
00024 {
00025 transed_reference_vector_[i] = PointCloudInPtr (new PointCloudIn ());
00026 }
00027 }
00028
00029 coherence_->setTargetCloud (input_);
00030
00031 if (!change_detector_)
00032 change_detector_ = boost::shared_ptr<pcl::octree::OctreePointCloudChangeDetector<PointInT> >(new pcl::octree::OctreePointCloudChangeDetector<PointInT> (change_detector_resolution_));
00033
00034 if (!particles_ || particles_->points.empty ())
00035 initParticles (true);
00036 return (true);
00037 }
00038
00039 template <typename PointInT, typename StateT> int
00040 pcl::tracking::ParticleFilterTracker<PointInT, StateT>::sampleWithReplacement
00041 (const std::vector<int>& a, const std::vector<double>& q)
00042 {
00043 using namespace boost;
00044 static mt19937 gen (static_cast<unsigned int>(time (0)));
00045 uniform_real<> dst (0.0, 1.0);
00046 variate_generator<mt19937&, uniform_real<> > rand (gen, dst);
00047 double rU = rand () * static_cast<double> (particles_->points.size ());
00048 int k = static_cast<int> (rU);
00049 rU -= k;
00050 if ( rU < q[k] )
00051 return k;
00052 else
00053 return a[k];
00054 }
00055
00056 template <typename PointInT, typename StateT> void
00057 pcl::tracking::ParticleFilterTracker<PointInT, StateT>::genAliasTable (std::vector<int> &a, std::vector<double> &q,
00058 const PointCloudStateConstPtr &particles)
00059 {
00060
00061 std::vector<int> HL (particles->points.size ());
00062 std::vector<int>::iterator H = HL.begin ();
00063 std::vector<int>::iterator L = HL.end () - 1;
00064 size_t num = particles->points.size ();
00065 for ( size_t i = 0; i < num; i++ )
00066 q[i] = particles->points[i].weight * static_cast<float> (num);
00067 for ( size_t i = 0; i < num; i++ )
00068 a[i] = static_cast<int> (i);
00069
00070 for ( size_t i = 0; i < num; i++ )
00071 if ( q[i] >= 1.0 )
00072 *H++ = static_cast<int> (i);
00073 else
00074 *L-- = static_cast<int> (i);
00075
00076 while ( H != HL.begin() && L != HL.end() - 1 )
00077 {
00078 int j = *(L + 1);
00079 int k = *(H - 1);
00080 a[j] = k;
00081 q[k] += q[j] - 1;
00082 L++;
00083 if ( q[k] < 1.0 )
00084 {
00085 *L-- = k;
00086 --H;
00087 }
00088 }
00089 }
00090
00091 template <typename PointInT, typename StateT> void
00092 pcl::tracking::ParticleFilterTracker<PointInT, StateT>::initParticles (bool reset)
00093 {
00094 particles_.reset (new PointCloudState ());
00095 std::vector<double> initial_noise_mean;
00096 if (reset)
00097 {
00098 representative_state_.zero ();
00099 StateT offset = StateT::toState (trans_);
00100 representative_state_ = offset;
00101 representative_state_.weight = 1.0f / static_cast<float> (particle_num_);
00102 }
00103
00104
00105 for ( int i = 0; i < particle_num_; i++ )
00106 {
00107 StateT p;
00108 p.zero ();
00109 p.sample (initial_noise_mean_, initial_noise_covariance_);
00110 p = p + representative_state_;
00111 p.weight = 1.0f / static_cast<float> (particle_num_);
00112 particles_->points.push_back (p);
00113 }
00114 }
00115
00116 template <typename PointInT, typename StateT> void
00117 pcl::tracking::ParticleFilterTracker<PointInT, StateT>::normalizeWeight ()
00118 {
00119
00120 double w_min = std::numeric_limits<double>::max ();
00121 double w_max = - std::numeric_limits<double>::max ();
00122 for ( size_t i = 0; i < particles_->points.size (); i++ )
00123 {
00124 double weight = particles_->points[i].weight;
00125 if (w_min > weight)
00126 w_min = weight;
00127 if (weight != 0.0 && w_max < weight)
00128 w_max = weight;
00129 }
00130
00131 fit_ratio_ = w_min;
00132 if (w_max != w_min)
00133 {
00134 for ( size_t i = 0; i < particles_->points.size (); i++ )
00135 {
00136 if (particles_->points[i].weight != 0.0)
00137 {
00138 particles_->points[i].weight = normalizeParticleWeight (particles_->points[i].weight, w_min, w_max);
00139 }
00140 }
00141 }
00142 else
00143 {
00144 for ( size_t i = 0; i < particles_->points.size (); i++ )
00145 particles_->points[i].weight = 1.0f / static_cast<float> (particles_->points.size ());
00146 }
00147
00148 double sum = 0.0;
00149 for ( size_t i = 0; i < particles_->points.size (); i++ )
00150 {
00151 sum += particles_->points[i].weight;
00152 }
00153
00154 if (sum != 0.0)
00155 {
00156 for ( size_t i = 0; i < particles_->points.size (); i++ )
00157 particles_->points[i].weight = particles_->points[i].weight / static_cast<float> (sum);
00158 }
00159 else
00160 {
00161 for ( size_t i = 0; i < particles_->points.size (); i++ )
00162 particles_->points[i].weight = 1.0f / static_cast<float> (particles_->points.size ());
00163 }
00164 }
00165
00167 template <typename PointInT, typename StateT> void
00168 pcl::tracking::ParticleFilterTracker<PointInT, StateT>::cropInputPointCloud (
00169 const PointCloudInConstPtr &, PointCloudIn &output)
00170 {
00171 double x_min, y_min, z_min, x_max, y_max, z_max;
00172 calcBoundingBox (x_min, x_max, y_min, y_max, z_min, z_max);
00173 pass_x_.setFilterLimits (float (x_min), float (x_max));
00174 pass_y_.setFilterLimits (float (y_min), float (y_max));
00175 pass_z_.setFilterLimits (float (z_min), float (z_max));
00176
00177
00178 PointCloudInPtr xcloud (new PointCloudIn);
00179 pass_x_.setInputCloud (input_);
00180 pass_x_.filter (*xcloud);
00181
00182 PointCloudInPtr ycloud (new PointCloudIn);
00183 pass_y_.setInputCloud (xcloud);
00184 pass_y_.filter (*ycloud);
00185
00186 pass_z_.setInputCloud (ycloud);
00187 pass_z_.filter (output);
00188 }
00189
00191 template <typename PointInT, typename StateT> void
00192 pcl::tracking::ParticleFilterTracker<PointInT, StateT>::calcBoundingBox (
00193 double &x_min, double &x_max, double &y_min, double &y_max, double &z_min, double &z_max)
00194 {
00195 x_min = y_min = z_min = std::numeric_limits<double>::max ();
00196 x_max = y_max = z_max = - std::numeric_limits<double>::max ();
00197
00198 for (size_t i = 0; i < transed_reference_vector_.size (); i++)
00199 {
00200 PointCloudInPtr target = transed_reference_vector_[i];
00201 PointInT Pmin, Pmax;
00202 pcl::getMinMax3D (*target, Pmin, Pmax);
00203 if (x_min > Pmin.x)
00204 x_min = Pmin.x;
00205 if (x_max < Pmax.x)
00206 x_max = Pmax.x;
00207 if (y_min > Pmin.y)
00208 y_min = Pmin.y;
00209 if (y_max < Pmax.y)
00210 y_max = Pmax.y;
00211 if (z_min > Pmin.z)
00212 z_min = Pmin.z;
00213 if (z_max < Pmax.z)
00214 z_max = Pmax.z;
00215 }
00216 }
00217
00218 template <typename PointInT, typename StateT> bool
00219 pcl::tracking::ParticleFilterTracker<PointInT, StateT>::testChangeDetection
00220 (const PointCloudInConstPtr &input)
00221 {
00222 change_detector_->setInputCloud (input);
00223 change_detector_->addPointsFromInputCloud ();
00224 std::vector<int> newPointIdxVector;
00225 change_detector_->getPointIndicesFromNewVoxels (newPointIdxVector, change_detector_filter_);
00226 change_detector_->switchBuffers ();
00227 return newPointIdxVector.size () > 0;
00228 }
00229
00230 template <typename PointInT, typename StateT> void
00231 pcl::tracking::ParticleFilterTracker<PointInT, StateT>::weight ()
00232 {
00233 if (!use_normal_)
00234 {
00235 for (size_t i = 0; i < particles_->points.size (); i++)
00236 {
00237 computeTransformedPointCloudWithoutNormal (particles_->points[i], *transed_reference_vector_[i]);
00238 }
00239
00240 PointCloudInPtr coherence_input (new PointCloudIn);
00241 cropInputPointCloud (input_, *coherence_input);
00242
00243 coherence_->setTargetCloud (coherence_input);
00244 coherence_->initCompute ();
00245 for (size_t i = 0; i < particles_->points.size (); i++)
00246 {
00247 IndicesPtr indices;
00248 coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight);
00249 }
00250 }
00251 else
00252 {
00253 for (size_t i = 0; i < particles_->points.size (); i++)
00254 {
00255 IndicesPtr indices (new std::vector<int>);
00256 computeTransformedPointCloudWithNormal (particles_->points[i], *indices, *transed_reference_vector_[i]);
00257 }
00258
00259 PointCloudInPtr coherence_input (new PointCloudIn);
00260 cropInputPointCloud (input_, *coherence_input);
00261
00262 coherence_->setTargetCloud (coherence_input);
00263 coherence_->initCompute ();
00264 for (size_t i = 0; i < particles_->points.size (); i++)
00265 {
00266 IndicesPtr indices (new std::vector<int>);
00267 coherence_->compute (transed_reference_vector_[i], indices, particles_->points[i].weight);
00268 }
00269 }
00270
00271 normalizeWeight ();
00272 }
00273
00274 template <typename PointInT, typename StateT> void
00275 pcl::tracking::ParticleFilterTracker<PointInT, StateT>::computeTransformedPointCloud
00276 (const StateT& hypothesis, std::vector<int>& indices, PointCloudIn &cloud)
00277 {
00278 if (use_normal_)
00279 computeTransformedPointCloudWithNormal (hypothesis, indices, cloud);
00280 else
00281 computeTransformedPointCloudWithoutNormal (hypothesis, cloud);
00282 }
00283
00284 template <typename PointInT, typename StateT> void
00285 pcl::tracking::ParticleFilterTracker<PointInT, StateT>::computeTransformedPointCloudWithoutNormal
00286 (const StateT& hypothesis, PointCloudIn &cloud)
00287 {
00288 const Eigen::Affine3f trans = toEigenMatrix (hypothesis);
00289
00290 pcl::transformPointCloud<PointInT> (*ref_, cloud, trans);
00291 }
00292
00294 template <typename PointInT, typename StateT> void
00295 pcl::tracking::ParticleFilterTracker<PointInT, StateT>::computeTransformedPointCloudWithNormal (
00296 #ifdef PCL_TRACKING_NORMAL_SUPPORTED
00297 const StateT& hypothesis, std::vector<int>& indices, PointCloudIn &cloud)
00298 #else
00299 const StateT&, std::vector<int>&, PointCloudIn &)
00300 #endif
00301 {
00302 #ifdef PCL_TRACKING_NORMAL_SUPPORTED
00303 const Eigen::Affine3f trans = toEigenMatrix (hypothesis);
00304
00305 pcl::transformPointCloudWithNormals<PointInT> (*ref_, cloud, trans);
00306 for ( size_t i = 0; i < cloud.points.size (); i++ )
00307 {
00308 PointInT input_point = cloud.points[i];
00309
00310 if (!pcl_isfinite(input_point.x) || !pcl_isfinite(input_point.y) || !pcl_isfinite(input_point.z))
00311 continue;
00312
00313 Eigen::Vector4f p = input_point.getVector4fMap ();
00314 Eigen::Vector4f n = input_point.getNormalVector4fMap ();
00315 double theta = pcl::getAngle3D (p, n);
00316 if ( theta > occlusion_angle_thr_ )
00317 indices.push_back (i);
00318 }
00319 #else
00320 PCL_WARN ("[pcl::%s::computeTransformedPointCloudWithoutNormal] use_normal_ == true is not supported in this Point Type.",
00321 getClassName ().c_str ());
00322 #endif
00323 }
00324
00325 template <typename PointInT, typename StateT> void
00326 pcl::tracking::ParticleFilterTracker<PointInT, StateT>::resample ()
00327 {
00328 resampleWithReplacement ();
00329 }
00330
00331 template <typename PointInT, typename StateT> void
00332 pcl::tracking::ParticleFilterTracker<PointInT, StateT>::resampleWithReplacement ()
00333 {
00334 std::vector<int> a (particles_->points.size ());
00335 std::vector<double> q (particles_->points.size ());
00336 genAliasTable (a, q, particles_);
00337
00338 const std::vector<double> zero_mean (StateT::stateDimension (), 0.0);
00339
00340 PointCloudStatePtr origparticles = particles_;
00341 particles_->points.clear ();
00342
00343 StateT p = representative_state_;
00344 particles_->points.push_back (p);
00345
00346
00347 int motion_num = static_cast<int> (particles_->points.size ()) * static_cast<int> (motion_ratio_);
00348 for ( int i = 1; i < motion_num; i++ )
00349 {
00350 int target_particle_index = sampleWithReplacement (a, q);
00351 StateT p = origparticles->points[target_particle_index];
00352
00353 p.sample (zero_mean, step_noise_covariance_);
00354 p = p + motion_;
00355 particles_->points.push_back (p);
00356 }
00357
00358
00359 for ( int i = motion_num; i < particle_num_; i++ )
00360 {
00361 int target_particle_index = sampleWithReplacement (a, q);
00362 StateT p = origparticles->points[target_particle_index];
00363
00364 p.sample (zero_mean, step_noise_covariance_);
00365 particles_->points.push_back (p);
00366 }
00367 }
00368
00369 template <typename PointInT, typename StateT> void
00370 pcl::tracking::ParticleFilterTracker<PointInT, StateT>::update ()
00371 {
00372
00373 StateT orig_representative = representative_state_;
00374 representative_state_.zero ();
00375 representative_state_.weight = 0.0;
00376 for ( size_t i = 0; i < particles_->points.size (); i++)
00377 {
00378 StateT p = particles_->points[i];
00379 representative_state_ = representative_state_ + p * p.weight;
00380 }
00381 representative_state_.weight = 1.0f / static_cast<float> (particles_->points.size ());
00382 motion_ = representative_state_ - orig_representative;
00383 }
00384
00385 template <typename PointInT, typename StateT> void
00386 pcl::tracking::ParticleFilterTracker<PointInT, StateT>::computeTracking ()
00387 {
00388
00389 for (int i = 0; i < iteration_num_; i++)
00390 {
00391 if (changed_)
00392 {
00393 resample ();
00394 }
00395
00396 weight ();
00397
00398 if (changed_)
00399 {
00400 update ();
00401 }
00402 }
00403
00404
00405
00406
00407
00408
00409 }
00410
00411 #define PCL_INSTANTIATE_ParticleFilterTracker(T,ST) template class PCL_EXPORTS pcl::tracking::ParticleFilterTracker<T,ST>;
00412
00413 #endif