1 #ifndef PCL_TRACKING_IMPL_PARTICLE_FILTER_H_
2 #define PCL_TRACKING_IMPL_PARTICLE_FILTER_H_
5 #include <pcl/common/transforms.h>
6 #include <pcl/tracking/particle_filter.h>
12 template <
typename Po
intInT,
typename StateT>
17 PCL_ERROR(
"[pcl::%s::initCompute] Init failed.\n", getClassName().c_str());
21 if (transed_reference_vector_.empty()) {
23 transed_reference_vector_.resize(particle_num_);
24 for (
int i = 0; i < particle_num_; i++) {
29 coherence_->setTargetCloud(input_);
31 if (!change_detector_)
33 change_detector_resolution_));
35 if (!particles_ || particles_->points.empty())
40 template <
typename Po
intInT,
typename StateT>
43 const std::vector<int>& a,
const std::vector<double>& q)
45 static std::mt19937 rng{std::random_device{}()};
46 std::uniform_real_distribution<> rd(0.0, 1.0);
48 double rU = rd(rng) *
static_cast<double>(particles_->size());
49 int k =
static_cast<int>(rU);
56 template <
typename Po
intInT,
typename StateT>
60 std::vector<double>& q,
64 std::vector<int> HL(particles->size());
65 std::vector<int>::iterator H = HL.begin();
66 std::vector<int>::iterator L = HL.end() - 1;
67 const auto num = particles->size();
68 for (std::size_t i = 0; i < num; i++)
69 q[i] = (*particles)[i].weight *
static_cast<float>(num);
70 for (std::size_t i = 0; i < num; i++)
71 a[i] =
static_cast<int>(i);
73 for (std::size_t i = 0; i < num; i++)
75 *H++ =
static_cast<int>(i);
77 *L-- =
static_cast<int>(i);
79 while (H != HL.begin() && L != HL.end() - 1) {
92 template <
typename Po
intInT,
typename StateT>
98 representative_state_.zero();
99 StateT offset = StateT::toState(trans_);
100 representative_state_ = offset;
101 representative_state_.weight = 1.0f /
static_cast<float>(particle_num_);
105 for (
int i = 0; i < particle_num_; i++) {
108 p.sample(initial_noise_mean_, initial_noise_covariance_);
109 p = p + representative_state_;
110 p.weight = 1.0f /
static_cast<float>(particle_num_);
111 particles_->points.push_back(p);
115 template <
typename Po
intInT,
typename StateT>
120 double w_min = std::numeric_limits<double>::max();
121 double w_max = -std::numeric_limits<double>::max();
122 for (
const auto& point : *particles_) {
123 double weight = point.weight;
126 if (weight != 0.0 && w_max < weight)
131 if (w_max != w_min) {
132 for (
auto& point : *particles_) {
133 if (point.weight != 0.0) {
135 static_cast<float>(normalizeParticleWeight(point.weight, w_min, w_max));
140 for (
auto& point : *particles_)
141 point.weight = 1.0f /
static_cast<float>(particles_->size());
145 for (
const auto& point : *particles_) {
150 for (
auto& point : *particles_)
151 point.weight /=
static_cast<float>(sum);
154 for (
auto& point : *particles_)
155 point.weight = 1.0f /
static_cast<float>(particles_->size());
160 template <
typename Po
intInT,
typename StateT>
165 double x_min, y_min, z_min, x_max, y_max, z_max;
166 calcBoundingBox(x_min, x_max, y_min, y_max, z_min, z_max);
167 pass_x_.setFilterLimits(
float(x_min),
float(x_max));
168 pass_y_.setFilterLimits(
float(y_min),
float(y_max));
169 pass_z_.setFilterLimits(
float(z_min),
float(z_max));
173 pass_x_.setInputCloud(input_);
174 pass_x_.filter(*xcloud);
177 pass_y_.setInputCloud(xcloud);
178 pass_y_.filter(*ycloud);
180 pass_z_.setInputCloud(ycloud);
181 pass_z_.filter(output);
185 template <
typename Po
intInT,
typename StateT>
194 x_min = y_min = z_min = std::numeric_limits<double>::max();
195 x_max = y_max = z_max = -std::numeric_limits<double>::max();
197 for (std::size_t i = 0; i < transed_reference_vector_.size(); i++) {
216 template <
typename Po
intInT,
typename StateT>
221 change_detector_->setInputCloud(input);
222 change_detector_->addPointsFromInputCloud();
223 std::vector<int> newPointIdxVector;
224 change_detector_->getPointIndicesFromNewVoxels(newPointIdxVector,
225 change_detector_filter_);
226 change_detector_->switchBuffers();
227 return !newPointIdxVector.empty();
230 template <
typename Po
intInT,
typename StateT>
235 for (std::size_t i = 0; i < particles_->size(); i++) {
236 computeTransformedPointCloudWithoutNormal((*particles_)[i],
237 *transed_reference_vector_[i]);
241 cropInputPointCloud(input_, *coherence_input);
243 coherence_->setTargetCloud(coherence_input);
244 coherence_->initCompute();
245 for (std::size_t i = 0; i < particles_->size(); i++) {
248 transed_reference_vector_[i], indices, (*particles_)[i].weight);
252 for (std::size_t i = 0; i < particles_->size(); i++) {
254 computeTransformedPointCloudWithNormal(
255 (*particles_)[i], *indices, *transed_reference_vector_[i]);
259 cropInputPointCloud(input_, *coherence_input);
261 coherence_->setTargetCloud(coherence_input);
262 coherence_->initCompute();
263 for (std::size_t i = 0; i < particles_->size(); i++) {
266 transed_reference_vector_[i], indices, (*particles_)[i].weight);
273 template <
typename Po
intInT,
typename StateT>
276 const StateT& hypothesis, std::vector<int>& indices,
PointCloudIn& cloud)
279 computeTransformedPointCloudWithNormal(hypothesis, indices, cloud);
281 computeTransformedPointCloudWithoutNormal(hypothesis, cloud);
284 template <
typename Po
intInT,
typename StateT>
289 const Eigen::Affine3f trans = toEigenMatrix(hypothesis);
291 pcl::transformPointCloud<PointInT>(*ref_, cloud, trans);
295 template <
typename Po
intInT,
typename StateT>
298 #ifdef PCL_TRACKING_NORMAL_SUPPORTED
299 const StateT& hypothesis, std::vector<int>& indices,
PointCloudIn& cloud)
304 #ifdef PCL_TRACKING_NORMAL_SUPPORTED
305 const Eigen::Affine3f trans = toEigenMatrix(hypothesis);
307 pcl::transformPointCloudWithNormals<PointInT>(*ref_, cloud, trans);
308 for (std::size_t i = 0; i < cloud.size(); i++) {
309 PointInT input_point = cloud[i];
311 if (!std::isfinite(input_point.x) || !std::isfinite(input_point.y) ||
312 !std::isfinite(input_point.z))
315 Eigen::Vector4f p = input_point.getVector4fMap();
316 Eigen::Vector4f n = input_point.getNormalVector4fMap();
318 if (theta > occlusion_angle_thr_)
319 indices.push_back(i);
322 PCL_WARN(
"[pcl::%s::computeTransformedPointCloudWithoutNormal] "
323 "use_normal_ == true is not supported in this Point Type.",
324 getClassName().c_str());
328 template <
typename Po
intInT,
typename StateT>
332 resampleWithReplacement();
335 template <
typename Po
intInT,
typename StateT>
339 std::vector<int> a(particles_->size());
340 std::vector<double> q(particles_->size());
341 genAliasTable(a, q, particles_);
343 const std::vector<double> zero_mean(StateT::stateDimension(), 0.0);
346 particles_->points.clear();
348 StateT p = representative_state_;
349 particles_->points.push_back(p);
353 static_cast<int>(particles_->size()) *
static_cast<int>(motion_ratio_);
354 for (
int i = 1; i < motion_num; i++) {
355 int target_particle_index = sampleWithReplacement(a, q);
356 StateT p = (*origparticles)[target_particle_index];
358 p.sample(zero_mean, step_noise_covariance_);
360 particles_->points.push_back(p);
364 for (
int i = motion_num; i < particle_num_; i++) {
365 int target_particle_index = sampleWithReplacement(a, q);
366 StateT p = (*origparticles)[target_particle_index];
368 p.sample(zero_mean, step_noise_covariance_);
369 particles_->points.push_back(p);
373 template <
typename Po
intInT,
typename StateT>
378 StateT orig_representative = representative_state_;
379 representative_state_.zero();
380 representative_state_.
weight = 0.0;
381 for (
const auto& p : *particles_) {
382 representative_state_ = representative_state_ + p * p.weight;
384 representative_state_.weight = 1.0f /
static_cast<float>(particles_->size());
385 motion_ = representative_state_ - orig_representative;
388 template <
typename Po
intInT,
typename StateT>
393 for (
int i = 0; i < iteration_num_; i++) {
414 #define PCL_INSTANTIATE_ParticleFilterTracker(T, ST) \
415 template class PCL_EXPORTS pcl::tracking::ParticleFilterTracker<T, ST>;
void genAliasTable(std::vector< int > &a, std::vector< double > &q, const PointCloudStateConstPtr &particles)
Generate the tables for walker's alias method.
void calcBoundingBox(double &x_min, double &x_max, double &y_min, double &y_max, double &z_min, double &z_max)
Compute the parameters for the bounding box of hypothesis pointclouds.
void initParticles(bool reset)
Initialize the particles.
int sampleWithReplacement(const std::vector< int > &a, const std::vector< double > &q)
Implementation of "sample with replacement" using Walker's alias method.
void computeTransformedPointCloud(const StateT &hypothesis, std::vector< int > &indices, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents.
bool testChangeDetection(const PointCloudInConstPtr &input)
Run change detection and return true if there is a change.
typename PointCloudIn::ConstPtr PointCloudInConstPtr
void resampleWithReplacement()
Resampling the particle with replacement.
typename PointCloudState::Ptr PointCloudStatePtr
void computeTransformedPointCloudWithNormal(const StateT &hypothesis, std::vector< int > &indices, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indic...
void computeTransformedPointCloudWithoutNormal(const StateT &hypothesis, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indic...
virtual void resample()
Resampling phase of particle filter method.
typename PointCloudState::ConstPtr PointCloudStateConstPtr
void computeTracking() override
Track the pointcloud using particle filter method.
void cropInputPointCloud(const PointCloudInConstPtr &cloud, PointCloudIn &output)
Crop the pointcloud by the bounding box calculated from hypothesis and the reference pointcloud.
virtual void update()
Calculate the weighted mean of the particles and set it as the result.
virtual void weight()
Weighting phase of particle filter method.
bool initCompute() override
This method should get called before starting the actua computation.
typename PointCloudIn::Ptr PointCloudInPtr
typename Tracker< PointInT, StateT >::PointCloudState PointCloudState
typename Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
virtual void normalizeWeight()
Normalize the weights of all the particels.
Tracker represents the base tracker class.
Define standard C methods and C++ classes that are common to all methods.
void getMinMax3D(const pcl::PointCloud< PointT > &cloud, PointT &min_pt, PointT &max_pt)
Get the minimum and maximum values on each of the 3 (x-y-z) dimensions in a given pointcloud.
double getAngle3D(const Eigen::Vector4f &v1, const Eigen::Vector4f &v2, const bool in_degree=false)
Compute the smallest angle between two 3D vectors in radians (default) or degree.
shared_ptr< Indices > IndicesPtr