Loading...
Searching...
No Matches
particle_filter.hpp
#ifndef PCL_TRACKING_IMPL_PARTICLE_FILTER_H_
#define PCL_TRACKING_IMPL_PARTICLE_FILTER_H_
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/tracking/particle_filter.h>
#include <random>
namespace pcl {
namespace tracking {
template <typename PointInT, typename StateT>
bool
ParticleFilterTracker<PointInT, StateT>::initCompute()
{
if (!Tracker<PointInT, StateT>::initCompute()) {
PCL_ERROR("[pcl::%s::initCompute] Init failed.\n", getClassName().c_str());
return (false);
}
if (transed_reference_vector_.empty()) {
// only one time allocation
transed_reference_vector_.resize(particle_num_);
for (int i = 0; i < particle_num_; i++) {
transed_reference_vector_[i] = PointCloudInPtr(new PointCloudIn());
}
}
coherence_->setTargetCloud(input_);
if (!change_detector_)
change_detector_.reset(new pcl::octree::OctreePointCloudChangeDetector<PointInT>(
change_detector_resolution_));
if (!particles_ || particles_->points.empty())
initParticles(true);
return (true);
}
template <typename PointInT, typename StateT>
int
ParticleFilterTracker<PointInT, StateT>::sampleWithReplacement(
const std::vector<int>& a, const std::vector<double>& q)
{
static std::mt19937 rng{std::random_device{}()};
std::uniform_real_distribution<> rd(0.0, 1.0);
double rU = rd(rng) * static_cast<double>(particles_->size());
int k = static_cast<int>(rU);
rU -= k; /* rU - [rU] */
if (rU < q[k])
return k;
return a[k];
}
template <typename PointInT, typename StateT>
void
ParticleFilterTracker<PointInT, StateT>::genAliasTable(
std::vector<int>& a,
std::vector<double>& q,
const PointCloudStateConstPtr& particles)
{
/* generate an alias table, a and q */
std::vector<int> HL(particles->size());
auto H = HL.begin();
auto L = HL.end() - 1;
const auto num = particles->size();
for (std::size_t i = 0; i < num; i++)
q[i] = (*particles)[i].weight * static_cast<float>(num);
for (std::size_t i = 0; i < num; i++)
a[i] = static_cast<int>(i);
// setup H and L
for (std::size_t i = 0; i < num; i++)
if (q[i] >= 1.0)
*H++ = static_cast<int>(i);
else
*L-- = static_cast<int>(i);
while (H != HL.begin() && L != HL.end() - 1) {
int j = *(L + 1);
int k = *(H - 1);
a[j] = k;
q[k] += q[j] - 1;
++L;
if (q[k] < 1.0) {
*L-- = k;
--H;
}
}
}
template <typename PointInT, typename StateT>
void
ParticleFilterTracker<PointInT, StateT>::initParticles(bool reset)
{
particles_.reset(new PointCloudState());
if (reset) {
representative_state_.zero();
StateT offset = StateT::toState(trans_);
representative_state_ = offset;
representative_state_.weight = 1.0f / static_cast<float>(particle_num_);
}
// sampling...
for (int i = 0; i < particle_num_; i++) {
StateT p;
p.zero();
p.sample(initial_noise_mean_, initial_noise_covariance_);
p = p + representative_state_;
p.weight = 1.0f / static_cast<float>(particle_num_);
particles_->points.push_back(p); // update
}
}
template <typename PointInT, typename StateT>
void
ParticleFilterTracker<PointInT, StateT>::normalizeWeight()
{
// apply exponential function
double w_min = std::numeric_limits<double>::max();
double w_max = -std::numeric_limits<double>::max();
for (const auto& point : *particles_) {
double weight = point.weight;
if (w_min > weight)
w_min = weight;
if (weight != 0.0 && w_max < weight)
w_max = weight;
}
fit_ratio_ = w_min;
if (w_max != w_min) {
for (auto& point : *particles_) {
if (point.weight != 0.0) {
point.weight =
static_cast<float>(normalizeParticleWeight(point.weight, w_min, w_max));
}
}
}
else {
for (auto& point : *particles_)
point.weight = 1.0f / static_cast<float>(particles_->size());
}
double sum = 0.0;
for (const auto& point : *particles_) {
sum += point.weight;
}
if (sum != 0.0) {
for (auto& point : *particles_)
point.weight /= static_cast<float>(sum);
}
else {
for (auto& point : *particles_)
point.weight = 1.0f / static_cast<float>(particles_->size());
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename StateT>
void
ParticleFilterTracker<PointInT, StateT>::cropInputPointCloud(
const PointCloudInConstPtr&, PointCloudIn& output)
{
double x_min, y_min, z_min, x_max, y_max, z_max;
calcBoundingBox(x_min, x_max, y_min, y_max, z_min, z_max);
pass_x_.setFilterLimits(static_cast<float>(x_min), static_cast<float>(x_max));
pass_y_.setFilterLimits(static_cast<float>(y_min), static_cast<float>(y_max));
pass_z_.setFilterLimits(static_cast<float>(z_min), static_cast<float>(z_max));
// x
PointCloudInPtr xcloud(new PointCloudIn);
pass_x_.setInputCloud(input_);
pass_x_.filter(*xcloud);
// y
PointCloudInPtr ycloud(new PointCloudIn);
pass_y_.setInputCloud(xcloud);
pass_y_.filter(*ycloud);
// z
pass_z_.setInputCloud(ycloud);
pass_z_.filter(output);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename StateT>
void
ParticleFilterTracker<PointInT, StateT>::calcBoundingBox(double& x_min,
double& x_max,
double& y_min,
double& y_max,
double& z_min,
double& z_max)
{
x_min = y_min = z_min = std::numeric_limits<double>::max();
x_max = y_max = z_max = -std::numeric_limits<double>::max();
for (std::size_t i = 0; i < transed_reference_vector_.size(); i++) {
PointCloudInPtr target = transed_reference_vector_[i];
PointInT Pmin, Pmax;
pcl::getMinMax3D(*target, Pmin, Pmax);
if (x_min > Pmin.x)
x_min = Pmin.x;
if (x_max < Pmax.x)
x_max = Pmax.x;
if (y_min > Pmin.y)
y_min = Pmin.y;
if (y_max < Pmax.y)
y_max = Pmax.y;
if (z_min > Pmin.z)
z_min = Pmin.z;
if (z_max < Pmax.z)
z_max = Pmax.z;
}
}
template <typename PointInT, typename StateT>
bool
ParticleFilterTracker<PointInT, StateT>::testChangeDetection(
const PointCloudInConstPtr& input)
{
change_detector_->setInputCloud(input);
change_detector_->addPointsFromInputCloud();
pcl::Indices newPointIdxVector;
change_detector_->getPointIndicesFromNewVoxels(newPointIdxVector,
change_detector_filter_);
change_detector_->switchBuffers();
return !newPointIdxVector.empty();
}
template <typename PointInT, typename StateT>
void
ParticleFilterTracker<PointInT, StateT>::weight()
{
if (!use_normal_) {
for (std::size_t i = 0; i < particles_->size(); i++) {
computeTransformedPointCloudWithoutNormal((*particles_)[i],
*transed_reference_vector_[i]);
}
PointCloudInPtr coherence_input(new PointCloudIn);
cropInputPointCloud(input_, *coherence_input);
coherence_->setTargetCloud(coherence_input);
coherence_->initCompute();
for (std::size_t i = 0; i < particles_->size(); i++) {
IndicesPtr indices;
coherence_->compute(
transed_reference_vector_[i], indices, (*particles_)[i].weight);
}
}
else {
for (std::size_t i = 0; i < particles_->size(); i++) {
IndicesPtr indices(new pcl::Indices);
computeTransformedPointCloudWithNormal(
(*particles_)[i], *indices, *transed_reference_vector_[i]);
}
PointCloudInPtr coherence_input(new PointCloudIn);
cropInputPointCloud(input_, *coherence_input);
coherence_->setTargetCloud(coherence_input);
coherence_->initCompute();
for (std::size_t i = 0; i < particles_->size(); i++) {
IndicesPtr indices(new pcl::Indices);
coherence_->compute(
transed_reference_vector_[i], indices, (*particles_)[i].weight);
}
}
normalizeWeight();
}
template <typename PointInT, typename StateT>
void
ParticleFilterTracker<PointInT, StateT>::computeTransformedPointCloud(
const StateT& hypothesis, pcl::Indices& indices, PointCloudIn& cloud)
{
if (use_normal_)
computeTransformedPointCloudWithNormal(hypothesis, indices, cloud);
else
computeTransformedPointCloudWithoutNormal(hypothesis, cloud);
}
template <typename PointInT, typename StateT>
void
ParticleFilterTracker<PointInT, StateT>::computeTransformedPointCloudWithoutNormal(
const StateT& hypothesis, PointCloudIn& cloud)
{
const Eigen::Affine3f trans = toEigenMatrix(hypothesis);
// destructively assigns to cloud
pcl::transformPointCloud<PointInT>(*ref_, cloud, trans);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename StateT>
template <typename PointT, pcl::traits::HasNormal<PointT>>
void
ParticleFilterTracker<PointInT, StateT>::computeTransformedPointCloudWithNormal(
const StateT& hypothesis, pcl::Indices& indices, PointCloudIn& cloud)
{
const Eigen::Affine3f trans = toEigenMatrix(hypothesis);
// destructively assigns to cloud
pcl::transformPointCloudWithNormals<PointInT>(*ref_, cloud, trans);
for (std::size_t i = 0; i < cloud.size(); i++) {
PointInT input_point = cloud[i];
if (!std::isfinite(input_point.x) || !std::isfinite(input_point.y) ||
!std::isfinite(input_point.z))
continue;
// take occlusion into account
Eigen::Vector4f p = input_point.getVector4fMap();
Eigen::Vector4f n = input_point.getNormalVector4fMap();
double theta = pcl::getAngle3D(p, n);
if (theta > occlusion_angle_thr_)
indices.push_back(i);
}
}
template <typename PointInT, typename StateT>
void
ParticleFilterTracker<PointInT, StateT>::resample()
{
resampleWithReplacement();
}
template <typename PointInT, typename StateT>
void
ParticleFilterTracker<PointInT, StateT>::resampleWithReplacement()
{
std::vector<int> a(particles_->size());
std::vector<double> q(particles_->size());
genAliasTable(a, q, particles_);
const std::vector<double> zero_mean(StateT::stateDimension(), 0.0);
// memoize the original list of particles
PointCloudStatePtr origparticles = particles_;
particles_->points.clear();
// the first particle, it is a just copy of the maximum result
StateT p = representative_state_;
particles_->points.push_back(p);
// with motion
int motion_num =
static_cast<int>(particles_->size()) * static_cast<int>(motion_ratio_);
for (int i = 1; i < motion_num; i++) {
int target_particle_index = sampleWithReplacement(a, q);
StateT p = (*origparticles)[target_particle_index];
// add noise using gaussian
p.sample(zero_mean, step_noise_covariance_);
p = p + motion_;
particles_->points.push_back(p);
}
// no motion
for (int i = motion_num; i < particle_num_; i++) {
int target_particle_index = sampleWithReplacement(a, q);
StateT p = (*origparticles)[target_particle_index];
// add noise using gaussian
p.sample(zero_mean, step_noise_covariance_);
particles_->points.push_back(p);
}
}
template <typename PointInT, typename StateT>
void
ParticleFilterTracker<PointInT, StateT>::update()
{
StateT orig_representative = representative_state_;
representative_state_.zero();
representative_state_.weight = 0.0;
representative_state_ =
StateT::weightedAverage(particles_->begin(), particles_->end());
representative_state_.weight = 1.0f / static_cast<float>(particles_->size());
motion_ = representative_state_ - orig_representative;
}
template <typename PointInT, typename StateT>
void
ParticleFilterTracker<PointInT, StateT>::computeTracking()
{
for (int i = 0; i < iteration_num_; i++) {
if (changed_) {
resample();
}
weight(); // likelihood is called in it
if (changed_) {
update();
}
}
// if ( getResult ().weight < resample_likelihood_thr_ )
// {
// PCL_WARN ("too small likelihood, re-initializing...\n");
// initParticles (false);
// }
}
} // namespace tracking
} // namespace pcl
#define PCL_INSTANTIATE_ParticleFilterTracker(T, ST) \
template class PCL_EXPORTS pcl::tracking::ParticleFilterTracker<T, ST>;
#endif
pcl::octree::OctreePointCloudChangeDetector
Octree pointcloud change detector class
Definition octree_pointcloud_changedetector.h:69
pcl::tracking::ParticleFilterTracker::genAliasTable
void genAliasTable(std::vector< int > &a, std::vector< double > &q, const PointCloudStateConstPtr &particles)
Generate the tables for walker's alias method.
Definition particle_filter.hpp:58
pcl::tracking::ParticleFilterTracker::calcBoundingBox
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.
Definition particle_filter.hpp:187
pcl::tracking::ParticleFilterTracker::initParticles
void initParticles(bool reset)
Initialize the particles.
Definition particle_filter.hpp:94
pcl::tracking::ParticleFilterTracker::sampleWithReplacement
int sampleWithReplacement(const std::vector< int > &a, const std::vector< double > &q)
Implementation of "sample with replacement" using Walker's alias method.
Definition particle_filter.hpp:42
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::PointCloudInConstPtr
typename PointCloudIn::ConstPtr PointCloudInConstPtr
Definition particle_filter.h:37
pcl::tracking::ParticleFilterTracker::computeTransformedPointCloud
void computeTransformedPointCloud(const StateT &hypothesis, pcl::Indices &indices, PointCloudIn &cloud)
Compute a reference pointcloud transformed to the pose that hypothesis represents.
Definition particle_filter.hpp:275
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::resampleWithReplacement
void resampleWithReplacement()
Resampling the particle with replacement.
Definition particle_filter.hpp:328
pcl::tracking::ParticleFilterTracker::PointCloudStatePtr
typename PointCloudState::Ptr PointCloudStatePtr
Definition particle_filter.h:40
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::resample
virtual void resample()
Resampling phase of particle filter method.
Definition particle_filter.hpp:321
pcl::tracking::ParticleFilterTracker::PointCloudStateConstPtr
typename PointCloudState::ConstPtr PointCloudStateConstPtr
Definition particle_filter.h:41
pcl::tracking::ParticleFilterTracker::computeTracking
void computeTracking() override
Track the pointcloud using particle filter method.
Definition particle_filter.hpp:379
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::update
virtual void update()
Calculate the weighted mean of the particles and set it as the result.
Definition particle_filter.hpp:366
pcl::tracking::ParticleFilterTracker::weight
virtual void weight()
Weighting phase of particle filter method.
Definition particle_filter.hpp:232
pcl::tracking::ParticleFilterTracker::initCompute
bool initCompute() override
This method should get called before starting the actual computation.
Definition particle_filter.hpp:14
pcl::tracking::ParticleFilterTracker::PointCloudInPtr
typename PointCloudIn::Ptr PointCloudInPtr
Definition particle_filter.h:36
pcl::tracking::ParticleFilterTracker::PointCloudState
typename Tracker< PointInT, StateT >::PointCloudState PointCloudState
Definition particle_filter.h:39
pcl::tracking::ParticleFilterTracker::PointCloudIn
typename Tracker< PointInT, StateT >::PointCloudIn PointCloudIn
Definition particle_filter.h:35
pcl::tracking::ParticleFilterTracker::normalizeWeight
virtual void normalizeWeight()
Normalize the weights of all the particels.
Definition particle_filter.hpp:117
pcl::tracking::Tracker
Tracker represents the base tracker class.
Definition tracker.h:55
common.h
Define standard C methods and C++ classes that are common to all methods.
pcl::getMinMax3D
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.
Definition common.hpp:295
pcl::getAngle3D
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.
Definition common.hpp:47
pcl
Definition convolution.h:46
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
pcl::IndicesPtr
shared_ptr< Indices > IndicesPtr
Definition pcl_base.h:58