Loading...
Searching...
No Matches
particle_filter_omp.hpp
#ifndef PCL_TRACKING_IMPL_PARTICLE_OMP_FILTER_H_
#define PCL_TRACKING_IMPL_PARTICLE_OMP_FILTER_H_
#include <pcl/tracking/particle_filter_omp.h>
namespace pcl {
namespace tracking {
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename StateT>
void
ParticleFilterOMPTracker<PointInT, StateT>::setNumberOfThreads(unsigned int nr_threads)
{
if (nr_threads == 0)
#ifdef _OPENMP
threads_ = omp_get_num_procs();
#else
threads_ = 1;
#endif
else
threads_ = nr_threads;
}
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename StateT>
void
ParticleFilterOMPTracker<PointInT, StateT>::weight()
{
if (!use_normal_) {
// clang-format off
#pragma omp parallel for \
default(none) \
num_threads(threads_)
// clang-format on
for (int i = 0; i < particle_num_; i++)
this->computeTransformedPointCloudWithoutNormal((*particles_)[i],
*transed_reference_vector_[i]);
PointCloudInPtr coherence_input(new PointCloudIn);
this->cropInputPointCloud(input_, *coherence_input);
if (change_counter_ == 0) {
// test change detector
if (!use_change_detector_ || this->testChangeDetection(coherence_input)) {
changed_ = true;
change_counter_ = change_detector_interval_;
coherence_->setTargetCloud(coherence_input);
coherence_->initCompute();
// clang-format off
#pragma omp parallel for \
default(none) \
num_threads(threads_)
// clang-format on
for (int i = 0; i < particle_num_; i++) {
IndicesPtr indices; // dummy
coherence_->compute(
transed_reference_vector_[i], indices, (*particles_)[i].weight);
}
}
else
changed_ = false;
}
else {
--change_counter_;
coherence_->setTargetCloud(coherence_input);
coherence_->initCompute();
// clang-format off
#pragma omp parallel for \
default(none) \
num_threads(threads_)
// clang-format on
for (int i = 0; i < particle_num_; i++) {
IndicesPtr indices; // dummy
coherence_->compute(
transed_reference_vector_[i], indices, (*particles_)[i].weight);
}
}
}
else {
std::vector<IndicesPtr> indices_list(particle_num_);
for (int i = 0; i < particle_num_; i++) {
indices_list[i] = IndicesPtr(new pcl::Indices);
}
// clang-format off
#pragma omp parallel for \
default(none) \
shared(indices_list) \
num_threads(threads_)
// clang-format on
for (int i = 0; i < particle_num_; i++) {
this->computeTransformedPointCloudWithNormal(
(*particles_)[i], *indices_list[i], *transed_reference_vector_[i]);
}
PointCloudInPtr coherence_input(new PointCloudIn);
this->cropInputPointCloud(input_, *coherence_input);
coherence_->setTargetCloud(coherence_input);
coherence_->initCompute();
// clang-format off
#pragma omp parallel for \
default(none) \
shared(indices_list) \
num_threads(threads_)
// clang-format on
for (int i = 0; i < particle_num_; i++) {
coherence_->compute(
transed_reference_vector_[i], indices_list[i], (*particles_)[i].weight);
}
}
normalizeWeight();
}
} // namespace tracking
} // namespace pcl
#define PCL_INSTANTIATE_ParticleFilterOMPTracker(T, ST) \
template class PCL_EXPORTS pcl::tracking::ParticleFilterOMPTracker<T, ST>;
#endif
pcl::PCLBase< PointInT >::input_
PointCloudConstPtr input_
Definition pcl_base.h:147
pcl::tracking::ParticleFilterOMPTracker::PointCloudInPtr
typename PointCloudIn::Ptr PointCloudInPtr
Definition particle_filter_omp.h:41
pcl::tracking::ParticleFilterOMPTracker::weight
void weight() override
weighting phase of particle filter method.
Definition particle_filter_omp.hpp:26
pcl::tracking::ParticleFilterOMPTracker::PointCloudIn
typename Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
Definition particle_filter_omp.h:40
pcl::tracking::ParticleFilterOMPTracker::threads_
unsigned int threads_
The number of threads the scheduler should use.
Definition particle_filter_omp.h:77
pcl::tracking::ParticleFilterOMPTracker::setNumberOfThreads
void setNumberOfThreads(unsigned int nr_threads=0)
Initialize the scheduler and set the number of threads to use.
Definition particle_filter_omp.hpp:11
pcl::tracking::ParticleFilterTracker::coherence_
CloudCoherencePtr coherence_
A pointer to PointCloudCoherence.
Definition particle_filter.h:556
pcl::tracking::ParticleFilterTracker::testChangeDetection
bool testChangeDetection(const PointCloudInConstPtr &input)
Run change detection and return true if there is a change.
Definition particle_filter.hpp:218
pcl::tracking::ParticleFilterTracker::computeTransformedPointCloudWithNormal
void computeTransformedPointCloudWithNormal(const StateT &hypothesis, pcl::Indices &indices, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indic...
Definition particle_filter.hpp:298
pcl::tracking::ParticleFilterTracker::changed_
bool changed_
A flag to be true when change of pointclouds is detected.
Definition particle_filter.h:615
pcl::tracking::ParticleFilterTracker::transed_reference_vector_
std::vector< PointCloudInPtr > transed_reference_vector_
A list of the pointers to pointclouds.
Definition particle_filter.h:608
pcl::tracking::ParticleFilterTracker::computeTransformedPointCloudWithoutNormal
void computeTransformedPointCloudWithoutNormal(const StateT &hypothesis, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents and calculate indic...
Definition particle_filter.hpp:286
pcl::tracking::ParticleFilterTracker::change_counter_
unsigned int change_counter_
A counter to skip change detection.
Definition particle_filter.h:618
pcl::tracking::ParticleFilterTracker::use_normal_
bool use_normal_
A flag to use normal or not.
Definition particle_filter.h:589
pcl::tracking::ParticleFilterTracker::cropInputPointCloud
void cropInputPointCloud(const PointCloudInConstPtr &cloud, PointCloudIn &output)
Crop the pointcloud by the bounding box calculated from hypothesis and the reference pointcloud.
Definition particle_filter.hpp:162
pcl::tracking::ParticleFilterTracker::particles_
PointCloudStatePtr particles_
A pointer to the particles.
Definition particle_filter.h:553
pcl::tracking::ParticleFilterTracker::change_detector_interval_
unsigned int change_detector_interval_
The number of interval frame to run change detection.
Definition particle_filter.h:626
pcl::tracking::ParticleFilterTracker::particle_num_
int particle_num_
The number of the particles.
Definition particle_filter.h:541
pcl::tracking::ParticleFilterTracker::use_change_detector_
bool use_change_detector_
The flag which will be true if using change detection.
Definition particle_filter.h:632
pcl::tracking::ParticleFilterTracker::normalizeWeight
virtual void normalizeWeight()
Normalize the weights of all the particels.
Definition particle_filter.hpp:117
pcl::tracking
Definition approx_nearest_pair_point_cloud_coherence.h:7
pcl
Definition convolution.h:46
pcl::IndicesPtr
shared_ptr< Indices > IndicesPtr
Definition pcl_base.h:58
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133