Point Cloud Library (PCL)
1.7.0
|
00001 #ifndef PCL_TRACKING_IMPL_PARTICLE_FILTER_H_ 00002 #define PCL_TRACKING_IMPL_PARTICLE_FILTER_H_ 00003 00004 #include <pcl/common/common.h> 00005 #include <pcl/common/eigen.h> 00006 #include <pcl/common/transforms.h> 00007 #include <pcl/tracking/boost.h> 00008 #include <pcl/tracking/particle_filter.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 // only one time allocation 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; /* rU - [rU] */ 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 /* generate an alias table, a and q */ 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 // setup H and L 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 // sampling... 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); // update 00113 } 00114 } 00115 00116 template <typename PointInT, typename StateT> void 00117 pcl::tracking::ParticleFilterTracker<PointInT, StateT>::normalizeWeight () 00118 { 00119 // apply exponential function 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 = static_cast<float> (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 00166 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 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 // x 00178 PointCloudInPtr xcloud (new PointCloudIn); 00179 pass_x_.setInputCloud (input_); 00180 pass_x_.filter (*xcloud); 00181 // y 00182 PointCloudInPtr ycloud (new PointCloudIn); 00183 pass_y_.setInputCloud (xcloud); 00184 pass_y_.filter (*ycloud); 00185 // z 00186 pass_z_.setInputCloud (ycloud); 00187 pass_z_.filter (output); 00188 } 00189 00190 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 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 // destructively assigns to cloud 00290 pcl::transformPointCloud<PointInT> (*ref_, cloud, trans); 00291 } 00292 00293 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 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 // destructively assigns to cloud 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 // take occlusion into account 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 // memoize the original list of particles 00340 PointCloudStatePtr origparticles = particles_; 00341 particles_->points.clear (); 00342 // the first particle, it is a just copy of the maximum result 00343 StateT p = representative_state_; 00344 particles_->points.push_back (p); 00345 00346 // with motion 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 // add noise using gaussian 00353 p.sample (zero_mean, step_noise_covariance_); 00354 p = p + motion_; 00355 particles_->points.push_back (p); 00356 } 00357 00358 // no motion 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 // add noise using gaussian 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 (); // likelihood is called in it 00397 00398 if (changed_) 00399 { 00400 update (); 00401 } 00402 } 00403 00404 // if ( getResult ().weight < resample_likelihood_thr_ ) 00405 // { 00406 // PCL_WARN ("too small likelihood, re-initializing...\n"); 00407 // initParticles (false); 00408 // } 00409 } 00410 00411 #define PCL_INSTANTIATE_ParticleFilterTracker(T,ST) template class PCL_EXPORTS pcl::tracking::ParticleFilterTracker<T,ST>; 00412 00413 #endif