Loading...
Searching...
No Matches
ia_fpcs.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2014-, Open Perception, Inc.
* Copyright (C) 2008 Ben Gurion University of the Negev, Beer Sheva, Israel.
*
* All rights reserved
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
*/
#ifndef PCL_REGISTRATION_IMPL_IA_FPCS_H_
#define PCL_REGISTRATION_IMPL_IA_FPCS_H_
#include <pcl/common/distances.h>
#include <pcl/common/time.h>
#include <pcl/common/utils.h>
#include <pcl/filters/random_sample.h>
#include <pcl/registration/ia_fpcs.h>
#include <pcl/registration/transformation_estimation_3point.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <limits>
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT>
inline float
pcl::getMeanPointDensity(const typename pcl::PointCloud<PointT>::ConstPtr& cloud,
float max_dist,
int nr_threads)
{
const float max_dist_sqr = max_dist * max_dist;
const std::size_t s = cloud->size();
pcl::search::KdTree<PointT> tree;
tree.setInputCloud(cloud);
float mean_dist = 0.f;
int num = 0;
pcl::Indices ids(2);
std::vector<float> dists_sqr(2);
pcl::utils::ignore(nr_threads);
#pragma omp parallel for default(none) shared(tree, cloud) \
firstprivate(ids, dists_sqr) reduction(+ : mean_dist, num) \
firstprivate(s, max_dist_sqr) num_threads(nr_threads)
for (int i = 0; i < 1000; i++) {
tree.nearestKSearch((*cloud)[rand() % s], 2, ids, dists_sqr);
if (dists_sqr[1] < max_dist_sqr) {
mean_dist += std::sqrt(dists_sqr[1]);
num++;
}
}
return (mean_dist / num);
};
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT>
inline float
pcl::getMeanPointDensity(const typename pcl::PointCloud<PointT>::ConstPtr& cloud,
const pcl::Indices& indices,
float max_dist,
int nr_threads)
{
const float max_dist_sqr = max_dist * max_dist;
const std::size_t s = indices.size();
pcl::search::KdTree<PointT> tree;
tree.setInputCloud(cloud);
float mean_dist = 0.f;
int num = 0;
pcl::Indices ids(2);
std::vector<float> dists_sqr(2);
pcl::utils::ignore(nr_threads);
#if OPENMP_LEGACY_CONST_DATA_SHARING_RULE
#pragma omp parallel for default(none) shared(tree, cloud, indices) \
firstprivate(ids, dists_sqr) reduction(+ : mean_dist, num) num_threads(nr_threads)
#else
#pragma omp parallel for default(none) shared(tree, cloud, indices, s, max_dist_sqr) \
firstprivate(ids, dists_sqr) reduction(+ : mean_dist, num) num_threads(nr_threads)
#endif
for (int i = 0; i < 1000; i++) {
tree.nearestKSearch((*cloud)[indices[rand() % s]], 2, ids, dists_sqr);
if (dists_sqr[1] < max_dist_sqr) {
mean_dist += std::sqrt(dists_sqr[1]);
num++;
}
}
return (mean_dist / num);
};
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
FPCSInitialAlignment()
: source_normals_()
, target_normals_()
, score_threshold_(std::numeric_limits<float>::max())
, fitness_score_(std::numeric_limits<float>::max())
{
reg_name_ = "pcl::registration::FPCSInitialAlignment";
max_iterations_ = 0;
ransac_iterations_ = 1000;
transformation_estimation_.reset(
new pcl::registration::TransformationEstimation3Point<PointSource, PointTarget>);
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
void
pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
computeTransformation(PointCloudSource& output, const Eigen::Matrix4f& guess)
{
if (!initCompute())
return;
final_transformation_ = guess;
bool abort = false;
std::vector<MatchingCandidates> all_candidates(max_iterations_);
pcl::StopWatch timer;
#pragma omp parallel default(none) shared(abort, all_candidates, timer) \
num_threads(nr_threads_)
{
#ifdef _OPENMP
const unsigned int seed =
static_cast<unsigned int>(std::time(nullptr)) ^ omp_get_thread_num();
std::srand(seed);
PCL_DEBUG("[%s::computeTransformation] Using seed=%u\n", reg_name_.c_str(), seed);
#pragma omp for schedule(dynamic)
#endif
for (int i = 0; i < max_iterations_; i++) {
#pragma omp flush(abort)
MatchingCandidates candidates(1);
pcl::Indices base_indices(4);
all_candidates[i] = candidates;
if (!abort) {
float ratio[2];
// select four coplanar point base
if (selectBase(base_indices, ratio) == 0) {
// calculate candidate pair correspondences using diagonal lengths of base
pcl::Correspondences pairs_a, pairs_b;
if (bruteForceCorrespondences(base_indices[0], base_indices[1], pairs_a) ==
0 &&
bruteForceCorrespondences(base_indices[2], base_indices[3], pairs_b) ==
0) {
// determine candidate matches by combining pair correspondences based on
// segment distances
std::vector<pcl::Indices> matches;
if (determineBaseMatches(base_indices, matches, pairs_a, pairs_b, ratio) ==
0) {
// check and evaluate candidate matches and store them
handleMatches(base_indices, matches, candidates);
if (!candidates.empty())
all_candidates[i] = candidates;
}
}
}
// check terminate early (time or fitness_score threshold reached)
if (!candidates.empty() && candidates[0].fitness_score < score_threshold_) {
PCL_DEBUG("[%s::computeTransformation] Terminating because fitness score "
"(%g) is below threshold (%g).\n",
reg_name_.c_str(),
candidates[0].fitness_score,
score_threshold_);
abort = true;
}
else if (timer.getTimeSeconds() > max_runtime_) {
PCL_DEBUG("[%s::computeTransformation] Terminating because time exceeded.\n",
reg_name_.c_str());
abort = true;
}
#pragma omp flush(abort)
}
}
}
// determine best match over all tries
finalCompute(all_candidates);
// apply the final transformation
pcl::transformPointCloud(*input_, output, final_transformation_);
deinitCompute();
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
bool
pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
initCompute()
{
const unsigned int seed = std::time(nullptr);
std::srand(seed);
PCL_DEBUG("[%s::initCompute] Using seed=%u\n", reg_name_.c_str(), seed);
// basic pcl initialization
if (!pcl::PCLBase<PointSource>::initCompute())
return (false);
// check if source and target are given
if (!input_ || !target_) {
PCL_ERROR("[%s::initCompute] Source or target dataset not given!\n",
reg_name_.c_str());
return (false);
}
if (!target_indices_ || target_indices_->empty()) {
target_indices_.reset(new pcl::Indices(target_->size()));
int index = 0;
for (auto& target_index : *target_indices_)
target_index = index++;
target_cloud_updated_ = true;
}
// if a sample size for the point clouds is given; preferably no sampling of target
// cloud
if (nr_samples_ > 0 && static_cast<std::size_t>(nr_samples_) < indices_->size()) {
source_indices_ = pcl::IndicesPtr(new pcl::Indices);
pcl::RandomSample<PointSource> random_sampling;
random_sampling.setInputCloud(input_);
random_sampling.setIndices(indices_);
random_sampling.setSample(nr_samples_);
random_sampling.setSeed(seed);
random_sampling.filter(*source_indices_);
}
else
source_indices_ = indices_;
// check usage of normals
if (source_normals_ && target_normals_ && source_normals_->size() == input_->size() &&
target_normals_->size() == target_->size())
use_normals_ = true;
// set up tree structures
if (target_cloud_updated_) {
tree_->setInputCloud(target_, target_indices_);
target_cloud_updated_ = false;
}
// set predefined variables
constexpr int min_iterations = 4;
constexpr float diameter_fraction = 0.3f;
// get diameter of input cloud (distance between farthest points)
Eigen::Vector4f pt_min, pt_max;
pcl::getMinMax3D(*target_, *target_indices_, pt_min, pt_max);
diameter_ = (pt_max - pt_min).norm();
// derive the limits for the random base selection
float max_base_diameter = diameter_ * approx_overlap_ * 2.f;
max_base_diameter_sqr_ = max_base_diameter * max_base_diameter;
#ifdef _OPENMP
if (nr_threads_ < 1) {
nr_threads_ = omp_get_num_procs();
PCL_DEBUG("[%s::initCompute] Setting number of threads to %i.\n",
reg_name_.c_str(),
nr_threads_);
}
#endif // _OPENMP
// normalize the delta
if (normalize_delta_) {
float mean_dist = getMeanPointDensity<PointTarget>(
target_, *target_indices_, 0.05f * diameter_, nr_threads_);
delta_ *= mean_dist;
}
// heuristic determination of number of trials to have high probability of finding a
// good solution
if (max_iterations_ == 0) {
float first_est = std::log(small_error_) /
std::log(1.0 - std::pow(static_cast<double>(approx_overlap_),
static_cast<double>(min_iterations)));
max_iterations_ =
static_cast<int>(first_est / (diameter_fraction * approx_overlap_ * 2.f));
PCL_DEBUG("[%s::initCompute] Estimated max iterations as %i\n",
reg_name_.c_str(),
max_iterations_);
}
// set further parameter
if (score_threshold_ == std::numeric_limits<float>::max())
score_threshold_ = 1.f - approx_overlap_;
if (max_iterations_ < 4)
max_iterations_ = 4;
if (max_runtime_ < 1)
max_runtime_ = std::numeric_limits<int>::max();
// calculate internal parameters based on the the estimated point density
max_pair_diff_ = delta_ * 2.f;
max_edge_diff_ = delta_ * 4.f;
coincidation_limit_ = delta_ * 2.f; // EDITED: originally std::sqrt (delta_ * 2.f)
max_mse_ = powf(delta_ * 2.f, 2.f);
max_inlier_dist_sqr_ = powf(delta_ * 2.f, 2.f);
PCL_DEBUG("[%s::initCompute] delta_=%g, max_inlier_dist_sqr_=%g, "
"coincidation_limit_=%g, max_edge_diff_=%g, max_pair_diff_=%g\n",
reg_name_.c_str(),
delta_,
max_inlier_dist_sqr_,
coincidation_limit_,
max_edge_diff_,
max_pair_diff_);
// reset fitness_score
fitness_score_ = std::numeric_limits<float>::max();
return (true);
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
int
pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
selectBase(pcl::Indices& base_indices, float (&ratio)[2])
{
const float too_close_sqr = max_base_diameter_sqr_ * 0.01;
Eigen::VectorXf coefficients(4);
pcl::SampleConsensusModelPlane<PointTarget> plane(target_);
plane.setIndices(target_indices_);
Eigen::Vector4f centre_pt;
float nearest_to_plane = std::numeric_limits<float>::max();
// repeat base search until valid quadruple was found or ransac_iterations_ number of
// tries were unsuccessful
for (int i = 0; i < ransac_iterations_; i++) {
// random select an appropriate point triple
if (selectBaseTriangle(base_indices) < 0)
continue;
pcl::Indices base_triple(base_indices.begin(), base_indices.end() - 1);
plane.computeModelCoefficients(base_triple, coefficients);
pcl::compute3DCentroid(*target_, base_triple, centre_pt);
// loop over all points in source cloud to find most suitable fourth point
const PointTarget* pt1 = &((*target_)[base_indices[0]]);
const PointTarget* pt2 = &((*target_)[base_indices[1]]);
const PointTarget* pt3 = &((*target_)[base_indices[2]]);
for (const auto& target_index : *target_indices_) {
const PointTarget* pt4 = &((*target_)[target_index]);
float d1 = pcl::squaredEuclideanDistance(*pt4, *pt1);
float d2 = pcl::squaredEuclideanDistance(*pt4, *pt2);
float d3 = pcl::squaredEuclideanDistance(*pt4, *pt3);
float d4 = (pt4->getVector3fMap() - centre_pt.head<3>()).squaredNorm();
// check distance between points w.r.t minimum sampling distance; EDITED -> 4th
// point now also limited by max base line
if (d1 < too_close_sqr || d2 < too_close_sqr || d3 < too_close_sqr ||
d4 < too_close_sqr || d1 > max_base_diameter_sqr_ ||
d2 > max_base_diameter_sqr_ || d3 > max_base_diameter_sqr_)
continue;
// check distance to plane to get point closest to plane
float dist_to_plane = pcl::pointToPlaneDistance(*pt4, coefficients);
if (dist_to_plane < nearest_to_plane) {
base_indices[3] = target_index;
nearest_to_plane = dist_to_plane;
}
}
// check if at least one point fulfilled the conditions
if (nearest_to_plane != std::numeric_limits<float>::max()) {
// order points to build largest quadrangle and calculate intersection ratios of
// diagonals
setupBase(base_indices, ratio);
return (0);
}
}
// return unsuccessful if no quadruple was selected
return (-1);
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
int
pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
selectBaseTriangle(pcl::Indices& base_indices)
{
const auto nr_points = target_indices_->size();
float best_t = 0.f;
// choose random first point
base_indices[0] = (*target_indices_)[rand() % nr_points];
auto* index1 = base_indices.data();
// random search for 2 other points (as far away as overlap allows)
for (int i = 0; i < ransac_iterations_; i++) {
auto* index2 = &(*target_indices_)[rand() % nr_points];
auto* index3 = &(*target_indices_)[rand() % nr_points];
Eigen::Vector3f u =
(*target_)[*index2].getVector3fMap() - (*target_)[*index1].getVector3fMap();
Eigen::Vector3f v =
(*target_)[*index3].getVector3fMap() - (*target_)[*index1].getVector3fMap();
float t =
u.cross(v).squaredNorm(); // triangle area (0.5 * sqrt(t)) should be maximal
// check for most suitable point triple
if (t > best_t && u.squaredNorm() < max_base_diameter_sqr_ &&
v.squaredNorm() < max_base_diameter_sqr_) {
best_t = t;
base_indices[1] = *index2;
base_indices[2] = *index3;
}
}
// return if a triplet could be selected
return (best_t == 0.f ? -1 : 0);
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
void
pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
setupBase(pcl::Indices& base_indices, float (&ratio)[2])
{
float best_t = std::numeric_limits<float>::max();
const pcl::Indices copy(base_indices.begin(), base_indices.end());
pcl::Indices temp(base_indices.begin(), base_indices.end());
// loop over all combinations of base points
for (auto i = copy.begin(), i_e = copy.end(); i != i_e; ++i)
for (auto j = copy.begin(), j_e = copy.end(); j != j_e; ++j) {
if (i == j)
continue;
for (auto k = copy.begin(), k_e = copy.end(); k != k_e; ++k) {
if (k == j || k == i)
continue;
auto l = copy.begin();
while (l == i || l == j || l == k)
++l;
temp[0] = *i;
temp[1] = *j;
temp[2] = *k;
temp[3] = *l;
// calculate diagonal intersection ratios and check for suitable segment to
// segment distances
float ratio_temp[2];
float t = segmentToSegmentDist(temp, ratio_temp);
if (t < best_t) {
best_t = t;
ratio[0] = ratio_temp[0];
ratio[1] = ratio_temp[1];
base_indices = temp;
}
}
}
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
float
pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
segmentToSegmentDist(const pcl::Indices& base_indices, float (&ratio)[2])
{
// get point vectors
Eigen::Vector3f u = (*target_)[base_indices[1]].getVector3fMap() -
(*target_)[base_indices[0]].getVector3fMap();
Eigen::Vector3f v = (*target_)[base_indices[3]].getVector3fMap() -
(*target_)[base_indices[2]].getVector3fMap();
Eigen::Vector3f w = (*target_)[base_indices[0]].getVector3fMap() -
(*target_)[base_indices[2]].getVector3fMap();
// calculate segment distances
float a = u.dot(u);
float b = u.dot(v);
float c = v.dot(v);
float d = u.dot(w);
float e = v.dot(w);
float D = a * c - b * b;
float sN = 0.f, sD = D;
float tN = 0.f, tD = D;
// check segments
if (D < small_error_) {
sN = 0.f;
sD = 1.f;
tN = e;
tD = c;
}
else {
sN = (b * e - c * d);
tN = (a * e - b * d);
if (sN < 0.f) {
sN = 0.f;
tN = e;
tD = c;
}
else if (sN > sD) {
sN = sD;
tN = e + b;
tD = c;
}
}
if (tN < 0.f) {
tN = 0.f;
if (-d < 0.f)
sN = 0.f;
else if (-d > a)
sN = sD;
else {
sN = -d;
sD = a;
}
}
else if (tN > tD) {
tN = tD;
if ((-d + b) < 0.f)
sN = 0.f;
else if ((-d + b) > a)
sN = sD;
else {
sN = (-d + b);
sD = a;
}
}
// set intersection ratios
ratio[0] = (std::abs(sN) < small_error_) ? 0.f : sN / sD;
ratio[1] = (std::abs(tN) < small_error_) ? 0.f : tN / tD;
Eigen::Vector3f x = w + (ratio[0] * u) - (ratio[1] * v);
return (x.norm());
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
int
pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
bruteForceCorrespondences(int idx1, int idx2, pcl::Correspondences& pairs)
{
const float max_norm_diff = 0.5f * max_norm_diff_ * M_PI / 180.f;
// calculate reference segment distance and normal angle
float ref_dist = pcl::euclideanDistance((*target_)[idx1], (*target_)[idx2]);
float ref_norm_angle =
(use_normals_ ? ((*target_normals_)[idx1].getNormalVector3fMap() -
(*target_normals_)[idx2].getNormalVector3fMap())
.norm()
: 0.f);
// loop over all pairs of points in source point cloud
auto it_out = source_indices_->begin(), it_out_e = source_indices_->end() - 1;
auto it_in_e = source_indices_->end();
for (; it_out != it_out_e; it_out++) {
auto it_in = it_out + 1;
const PointSource* pt1 = &(*input_)[*it_out];
for (; it_in != it_in_e; it_in++) {
const PointSource* pt2 = &(*input_)[*it_in];
// check point distance compared to reference dist (from base)
float dist = pcl::euclideanDistance(*pt1, *pt2);
if (std::abs(dist - ref_dist) < max_pair_diff_) {
// add here normal evaluation if normals are given
if (use_normals_) {
const NormalT* pt1_n = &((*source_normals_)[*it_out]);
const NormalT* pt2_n = &((*source_normals_)[*it_in]);
float norm_angle_1 =
(pt1_n->getNormalVector3fMap() - pt2_n->getNormalVector3fMap()).norm();
float norm_angle_2 =
(pt1_n->getNormalVector3fMap() + pt2_n->getNormalVector3fMap()).norm();
float norm_diff = std::min<float>(std::abs(norm_angle_1 - ref_norm_angle),
std::abs(norm_angle_2 - ref_norm_angle));
if (norm_diff > max_norm_diff)
continue;
}
pairs.emplace_back(*it_in, *it_out, dist);
pairs.emplace_back(*it_out, *it_in, dist);
}
}
}
// return success if at least one correspondence was found
return (pairs.empty() ? -1 : 0);
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
int
pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
determineBaseMatches(const pcl::Indices& base_indices,
std::vector<pcl::Indices>& matches,
const pcl::Correspondences& pairs_a,
const pcl::Correspondences& pairs_b,
const float (&ratio)[2])
{
// calculate edge lengths of base
float dist_base[4];
dist_base[0] =
pcl::euclideanDistance((*target_)[base_indices[0]], (*target_)[base_indices[2]]);
dist_base[1] =
pcl::euclideanDistance((*target_)[base_indices[0]], (*target_)[base_indices[3]]);
dist_base[2] =
pcl::euclideanDistance((*target_)[base_indices[1]], (*target_)[base_indices[2]]);
dist_base[3] =
pcl::euclideanDistance((*target_)[base_indices[1]], (*target_)[base_indices[3]]);
// loop over first point pair correspondences and store intermediate points 'e' in new
// point cloud
PointCloudSourcePtr cloud_e(new PointCloudSource);
cloud_e->resize(pairs_a.size() * 2);
auto it_pt = cloud_e->begin();
for (const auto& pair : pairs_a) {
const PointSource* pt1 = &((*input_)[pair.index_match]);
const PointSource* pt2 = &((*input_)[pair.index_query]);
// calculate intermediate points using both ratios from base (r1,r2)
for (int i = 0; i < 2; i++, it_pt++) {
it_pt->x = pt1->x + ratio[i] * (pt2->x - pt1->x);
it_pt->y = pt1->y + ratio[i] * (pt2->y - pt1->y);
it_pt->z = pt1->z + ratio[i] * (pt2->z - pt1->z);
}
}
// initialize new kd tree of intermediate points from first point pair correspondences
KdTreeReciprocalPtr tree_e(new KdTreeReciprocal);
tree_e->setInputCloud(cloud_e);
pcl::Indices ids;
std::vector<float> dists_sqr;
// loop over second point pair correspondences
for (const auto& pair : pairs_b) {
const PointTarget* pt1 = &((*input_)[pair.index_match]);
const PointTarget* pt2 = &((*input_)[pair.index_query]);
// calculate intermediate points using both ratios from base (r1,r2)
for (const float& r : ratio) {
PointTarget pt_e;
pt_e.x = pt1->x + r * (pt2->x - pt1->x);
pt_e.y = pt1->y + r * (pt2->y - pt1->y);
pt_e.z = pt1->z + r * (pt2->z - pt1->z);
// search for corresponding intermediate points
tree_e->radiusSearch(pt_e, coincidation_limit_, ids, dists_sqr);
for (const auto& id : ids) {
pcl::Indices match_indices(4);
match_indices[0] =
pairs_a[static_cast<int>(std::floor((id / 2.f)))].index_match;
match_indices[1] =
pairs_a[static_cast<int>(std::floor((id / 2.f)))].index_query;
match_indices[2] = pair.index_match;
match_indices[3] = pair.index_query;
if (match_indices[0] == match_indices[2] ||
match_indices[0] == match_indices[3] ||
match_indices[1] == match_indices[2] ||
match_indices[1] == match_indices[3])
continue;
// EDITED: added coarse check of match based on edge length (due to rigid-body )
if (checkBaseMatch(match_indices, dist_base) < 0)
continue;
matches.push_back(match_indices);
}
}
}
// return unsuccessful if no match was found
if (matches.empty()) {
PCL_DEBUG("[%s::determineBaseMatches] no matches found\n", reg_name_.c_str());
return -1;
}
else {
PCL_DEBUG("[%s::determineBaseMatches] found %zu matches\n",
reg_name_.c_str(),
matches.size());
return 0;
}
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
int
pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
checkBaseMatch(const pcl::Indices& match_indices, const float (&dist_ref)[4])
{
float d0 =
pcl::euclideanDistance((*input_)[match_indices[0]], (*input_)[match_indices[2]]);
float d1 =
pcl::euclideanDistance((*input_)[match_indices[0]], (*input_)[match_indices[3]]);
float d2 =
pcl::euclideanDistance((*input_)[match_indices[1]], (*input_)[match_indices[2]]);
float d3 =
pcl::euclideanDistance((*input_)[match_indices[1]], (*input_)[match_indices[3]]);
// check edge distances of match w.r.t the base
return (std::abs(d0 - dist_ref[0]) < max_edge_diff_ &&
std::abs(d1 - dist_ref[1]) < max_edge_diff_ &&
std::abs(d2 - dist_ref[2]) < max_edge_diff_ &&
std::abs(d3 - dist_ref[3]) < max_edge_diff_)
? 0
: -1;
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
void
pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
handleMatches(const pcl::Indices& base_indices,
std::vector<pcl::Indices>& matches,
MatchingCandidates& candidates)
{
candidates.resize(1);
float fitness_score = std::numeric_limits<float>::max();
// loop over all Candidate matches
for (auto& match : matches) {
Eigen::Matrix4f transformation_temp;
pcl::Correspondences correspondences_temp;
correspondences_temp.emplace_back(match[0], base_indices[0], 0.0);
correspondences_temp.emplace_back(match[1], base_indices[1], 0.0);
correspondences_temp.emplace_back(match[2], base_indices[2], 0.0);
correspondences_temp.emplace_back(match[3], base_indices[3], 0.0);
// check match based on residuals of the corresponding points after
if (validateMatch(base_indices, match, correspondences_temp, transformation_temp) <
0)
continue;
// check resulting using a sub sample of the source point cloud and compare to
// previous matches
if (validateTransformation(transformation_temp, fitness_score) < 0)
continue;
// store best match as well as associated fitness_score and transformation
candidates[0].fitness_score = fitness_score;
candidates[0].transformation = transformation_temp;
correspondences_temp.erase(correspondences_temp.end() - 1);
candidates[0].correspondences = correspondences_temp;
}
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
void
pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
linkMatchWithBase(const pcl::Indices& base_indices,
pcl::Indices& match_indices,
pcl::Correspondences& correspondences)
{
// calculate centroid of base and target
Eigen::Vector4f centre_base{0, 0, 0, 0}, centre_match{0, 0, 0, 0};
pcl::compute3DCentroid(*target_, base_indices, centre_base);
pcl::compute3DCentroid(*input_, match_indices, centre_match);
PointTarget centre_pt_base;
centre_pt_base.x = centre_base[0];
centre_pt_base.y = centre_base[1];
centre_pt_base.z = centre_base[2];
PointSource centre_pt_match;
centre_pt_match.x = centre_match[0];
centre_pt_match.y = centre_match[1];
centre_pt_match.z = centre_match[2];
// find corresponding points according to their distance to the centroid
pcl::Indices copy = match_indices;
auto it_match_orig = match_indices.begin();
for (auto it_base = base_indices.cbegin(), it_base_e = base_indices.cend();
it_base != it_base_e;
it_base++, it_match_orig++) {
float dist_sqr_1 =
pcl::squaredEuclideanDistance((*target_)[*it_base], centre_pt_base);
float best_diff_sqr = std::numeric_limits<float>::max();
int best_index = -1;
for (const auto& match_index : copy) {
// calculate difference of distances to centre point
float dist_sqr_2 =
pcl::squaredEuclideanDistance((*input_)[match_index], centre_pt_match);
float diff_sqr = std::abs(dist_sqr_1 - dist_sqr_2);
if (diff_sqr < best_diff_sqr) {
best_diff_sqr = diff_sqr;
best_index = match_index;
}
}
// assign new correspondence and update indices of matched targets
correspondences.emplace_back(best_index, *it_base, best_diff_sqr);
*it_match_orig = best_index;
}
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
int
pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
validateMatch(const pcl::Indices& base_indices,
const pcl::Indices& match_indices,
const pcl::Correspondences& correspondences,
Eigen::Matrix4f& transformation)
{
// only use triplet of points to simplify process (possible due to planar case)
pcl::Correspondences correspondences_temp = correspondences;
correspondences_temp.erase(correspondences_temp.end() - 1);
// estimate transformation between correspondence set
transformation_estimation_->estimateRigidTransformation(
*input_, *target_, correspondences_temp, transformation);
// transform base points
PointCloudSource match_transformed;
pcl::transformPointCloud(*input_, match_indices, match_transformed, transformation);
// calculate residuals of transformation and check against maximum threshold
std::size_t nr_points = correspondences_temp.size();
float mse = 0.f;
for (std::size_t i = 0; i < nr_points; i++)
mse += pcl::squaredEuclideanDistance(match_transformed.points[i],
target_->points[base_indices[i]]);
mse /= nr_points;
return (mse < max_mse_ ? 0 : -1);
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
int
pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
validateTransformation(Eigen::Matrix4f& transformation, float& fitness_score)
{
// transform source point cloud
PointCloudSource source_transformed;
pcl::transformPointCloud(
*input_, *source_indices_, source_transformed, transformation);
std::size_t nr_points = source_transformed.size();
std::size_t terminate_value =
fitness_score > 1 ? 0
: static_cast<std::size_t>((1.f - fitness_score) * nr_points);
float inlier_score_temp = 0;
pcl::Indices ids(1);
std::vector<float> dists_sqr(1);
auto it = source_transformed.begin();
for (std::size_t i = 0; i < nr_points; it++, i++) {
// search for nearest point using kd tree search
tree_->nearestKSearch(*it, 1, ids, dists_sqr);
inlier_score_temp += (dists_sqr[0] < max_inlier_dist_sqr_ ? 1 : 0);
// early terminating
if (nr_points - i + inlier_score_temp < terminate_value)
break;
}
// check current costs and return unsuccessful if larger than previous ones
inlier_score_temp /= static_cast<float>(nr_points);
float fitness_score_temp = 1.f - inlier_score_temp;
if (fitness_score_temp > fitness_score)
return (-1);
fitness_score = fitness_score_temp;
return (0);
}
///////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
void
pcl::registration::FPCSInitialAlignment<PointSource, PointTarget, NormalT, Scalar>::
finalCompute(const std::vector<MatchingCandidates>& candidates)
{
// get best fitness_score over all tries
int nr_candidates = static_cast<int>(candidates.size());
int best_index = -1;
float best_score = std::numeric_limits<float>::max();
for (int i = 0; i < nr_candidates; i++) {
const float& fitness_score = candidates[i][0].fitness_score;
if (fitness_score < best_score) {
best_score = fitness_score;
best_index = i;
}
}
PCL_DEBUG(
"[%s::finalCompute] best score is %g (iteration %i), out of %zu iterations.\n",
reg_name_.c_str(),
best_score,
best_index,
candidates.size());
// check if a valid candidate was available
if (!(best_index < 0)) {
fitness_score_ = candidates[best_index][0].fitness_score;
final_transformation_ = candidates[best_index][0].transformation;
*correspondences_ = candidates[best_index][0].correspondences;
// here we define convergence if resulting fitness_score is below 1-threshold
converged_ = fitness_score_ < score_threshold_;
}
}
///////////////////////////////////////////////////////////////////////////////////////////
#endif // PCL_REGISTRATION_IMPL_IA_4PCS_H_
pcl::FilterIndices::filter
void filter(Indices &indices)
Calls the filtering method and returns the filtered point cloud indices.
Definition filter_indices.h:100
pcl::PCLBase< PointSource >::input_
PointCloudConstPtr input_
Definition pcl_base.h:147
pcl::PCLBase::setInputCloud
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition pcl_base.hpp:65
pcl::PCLBase< PointSource >::indices_
IndicesPtr indices_
Definition pcl_base.h:150
pcl::PCLBase::setIndices
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition pcl_base.hpp:72
pcl::PCLBase::initCompute
bool initCompute()
This method should get called before starting the actual computation.
Definition pcl_base.hpp:138
pcl::PCLBase< PointSource >::deinitCompute
bool deinitCompute()
Definition pcl_base.hpp:175
pcl::PointCloud::size
std::size_t size() const
Definition point_cloud.h:444
pcl::PointCloud::ConstPtr
shared_ptr< const PointCloud< PointT > > ConstPtr
Definition point_cloud.h:415
pcl::RandomSample
RandomSample applies a random sampling with uniform probability.
Definition random_sample.h:56
pcl::RandomSample::setSeed
void setSeed(unsigned int seed)
Set seed of random function.
Definition random_sample.h:106
pcl::RandomSample::setSample
void setSample(unsigned int sample)
Set number of indices to be sampled.
Definition random_sample.h:89
pcl::Registration< PointSource, PointTarget, float >::target_cloud_updated_
bool target_cloud_updated_
Definition registration.h:637
pcl::Registration< PointSource, PointTarget, float >::final_transformation_
Matrix4 final_transformation_
Definition registration.h:573
pcl::Registration< PointSource, PointTarget, float >::KdTreeReciprocalPtr
typename KdTreeReciprocal::Ptr KdTreeReciprocalPtr
Definition registration.h:74
pcl::Registration< PointSource, PointTarget, float >::reg_name_
std::string reg_name_
Definition registration.h:548
pcl::Registration< PointSource, PointTarget, float >::PointCloudSource
pcl::PointCloud< PointSource > PointCloudSource
Definition registration.h:76
pcl::Registration< PointSource, PointTarget, float >::PointCloudSourcePtr
typename PointCloudSource::Ptr PointCloudSourcePtr
Definition registration.h:77
pcl::Registration< PointSource, PointTarget, float >::tree_
KdTreePtr tree_
Definition registration.h:551
pcl::Registration< PointSource, PointTarget, float >::converged_
bool converged_
Definition registration.h:613
pcl::Registration< PointSource, PointTarget, float >::ransac_iterations_
int ransac_iterations_
Definition registration.h:566
pcl::Registration< PointSource, PointTarget, float >::correspondences_
CorrespondencesPtr correspondences_
Definition registration.h:621
pcl::Registration< PointSource, PointTarget, float >::transformation_estimation_
TransformationEstimationPtr transformation_estimation_
Definition registration.h:625
pcl::Registration< PointSource, PointTarget, float >::max_iterations_
int max_iterations_
Definition registration.h:563
pcl::Registration< PointSource, PointTarget, float >::KdTreeReciprocal
pcl::search::KdTree< PointSource > KdTreeReciprocal
Definition registration.h:73
pcl::Registration< PointSource, PointTarget, float >::target_
PointCloudTargetConstPtr target_
Definition registration.h:569
pcl::SampleConsensusModel::setIndices
void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition sac_model.h:324
pcl::SampleConsensusModelPlane
SampleConsensusModelPlane defines a model for 3D plane segmentation.
Definition sac_model_plane.h:143
pcl::SampleConsensusModelPlane::computeModelCoefficients
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid plane model, compute the model coefficients fr...
Definition sac_model_plane.hpp:81
pcl::StopWatch
Simple stopwatch.
Definition time.h:59
pcl::StopWatch::getTimeSeconds
double getTimeSeconds() const
Retrieve the time in seconds spent since the last call to reset().
Definition time.h:74
pcl::registration::FPCSInitialAlignment::finalCompute
virtual void finalCompute(const std::vector< MatchingCandidates > &candidates)
Final computation of best match out of vector of best matches.
Definition ia_fpcs.hpp:923
pcl::registration::FPCSInitialAlignment::setupBase
void setupBase(pcl::Indices &base_indices, float(&ratio)[2])
Setup the base (four coplanar points) by ordering the points and computing intersection ratios and se...
Definition ia_fpcs.hpp:456
pcl::registration::FPCSInitialAlignment::use_normals_
bool use_normals_
Use normals flag.
Definition ia_fpcs.h:519
pcl::registration::FPCSInitialAlignment< PointSource, PointTarget, pcl::Normal, float >::selectBaseTriangle
int selectBaseTriangle(pcl::Indices &base_indices)
Definition ia_fpcs.hpp:418
pcl::registration::FPCSInitialAlignment< PointSource, PointTarget, pcl::Normal, float >::max_edge_diff_
float max_edge_diff_
Definition ia_fpcs.h:538
pcl::registration::FPCSInitialAlignment::bruteForceCorrespondences
virtual int bruteForceCorrespondences(int idx1, int idx2, pcl::Correspondences &pairs)
Search for corresponding point pairs given the distance between two base points.
Definition ia_fpcs.hpp:584
pcl::registration::FPCSInitialAlignment::delta_
float delta_
Delta value of 4pcs algorithm (standard = 1.0).
Definition ia_fpcs.h:486
pcl::registration::FPCSInitialAlignment::selectBase
int selectBase(pcl::Indices &base_indices, float(&ratio)[2])
Select an approximately coplanar set of four points from the source cloud.
Definition ia_fpcs.hpp:352
pcl::registration::FPCSInitialAlignment::max_base_diameter_sqr_
float max_base_diameter_sqr_
Estimated squared metric overlap between source and target.
Definition ia_fpcs.h:516
pcl::registration::FPCSInitialAlignment< PointSource, PointTarget, pcl::Normal, float >::coincidation_limit_
float coincidation_limit_
Definition ia_fpcs.h:543
pcl::registration::FPCSInitialAlignment::validateTransformation
virtual int validateTransformation(Eigen::Matrix4f &transformation, float &fitness_score)
Validate the transformation by calculating the number of inliers after transforming the source cloud.
Definition ia_fpcs.hpp:881
pcl::registration::FPCSInitialAlignment::determineBaseMatches
virtual int determineBaseMatches(const pcl::Indices &base_indices, std::vector< pcl::Indices > &matches, const pcl::Correspondences &pairs_a, const pcl::Correspondences &pairs_b, const float(&ratio)[2])
Determine base matches by combining the point pair candidate and search for coinciding intersection p...
Definition ia_fpcs.hpp:638
pcl::registration::FPCSInitialAlignment::linkMatchWithBase
virtual void linkMatchWithBase(const pcl::Indices &base_indices, pcl::Indices &match_indices, pcl::Correspondences &correspondences)
Sets the correspondences between the base B and the match M by using the distance of each point to th...
Definition ia_fpcs.hpp:796
pcl::registration::FPCSInitialAlignment::handleMatches
virtual void handleMatches(const pcl::Indices &base_indices, std::vector< pcl::Indices > &matches, MatchingCandidates &candidates)
Method to handle current candidate matches.
Definition ia_fpcs.hpp:758
pcl::registration::FPCSInitialAlignment< PointSource, PointTarget, pcl::Normal, float >::max_inlier_dist_sqr_
float max_inlier_dist_sqr_
Definition ia_fpcs.h:553
pcl::registration::FPCSInitialAlignment::normalize_delta_
bool normalize_delta_
Normalize delta flag.
Definition ia_fpcs.h:522
pcl::registration::FPCSInitialAlignment::target_normals_
NormalsConstPtr target_normals_
Normals of target point cloud.
Definition ia_fpcs.h:469
pcl::registration::FPCSInitialAlignment::source_normals_
NormalsConstPtr source_normals_
Normals of source point cloud.
Definition ia_fpcs.h:466
pcl::registration::FPCSInitialAlignment::target_indices_
pcl::IndicesPtr target_indices_
A pointer to the vector of target point indices to use after sampling.
Definition ia_fpcs.h:528
pcl::registration::FPCSInitialAlignment< PointSource, PointTarget, pcl::Normal, float >::max_mse_
float max_mse_
Definition ia_fpcs.h:548
pcl::registration::FPCSInitialAlignment::checkBaseMatch
int checkBaseMatch(const pcl::Indices &match_indices, const float(&ds)[4])
Check if outer rectangle distance of matched points fit with the base rectangle.
Definition ia_fpcs.hpp:734
pcl::registration::FPCSInitialAlignment::nr_samples_
int nr_samples_
The number of points to uniformly sample the source point cloud.
Definition ia_fpcs.h:495
pcl::registration::FPCSInitialAlignment::approx_overlap_
float approx_overlap_
Estimated overlap between source and target (standard = 0.5).
Definition ia_fpcs.h:477
pcl::registration::FPCSInitialAlignment::source_indices_
pcl::IndicesPtr source_indices_
A pointer to the vector of source point indices to use after sampling.
Definition ia_fpcs.h:525
pcl::registration::FPCSInitialAlignment< PointSource, PointTarget, pcl::Normal, float >::segmentToSegmentDist
float segmentToSegmentDist(const pcl::Indices &base_indices, float(&ratio)[2])
Definition ia_fpcs.hpp:499
pcl::registration::FPCSInitialAlignment::nr_threads_
int nr_threads_
Number of threads for parallelization (standard = 1).
Definition ia_fpcs.h:474
pcl::registration::FPCSInitialAlignment::validateMatch
virtual int validateMatch(const pcl::Indices &base_indices, const pcl::Indices &match_indices, const pcl::Correspondences &correspondences, Eigen::Matrix4f &transformation)
Validate the matching by computing the transformation between the source and target based on the four...
Definition ia_fpcs.hpp:849
pcl::registration::FPCSInitialAlignment::max_norm_diff_
float max_norm_diff_
Maximum normal difference of corresponding point pairs in degrees (standard = 90).
Definition ia_fpcs.h:499
pcl::registration::FPCSInitialAlignment::fitness_score_
float fitness_score_
Resulting fitness score of the best match.
Definition ia_fpcs.h:506
pcl::registration::FPCSInitialAlignment::diameter_
float diameter_
Estimated diameter of the target point cloud.
Definition ia_fpcs.h:509
pcl::registration::FPCSInitialAlignment::FPCSInitialAlignment
FPCSInitialAlignment()
Constructor.
Definition ia_fpcs.hpp:125
pcl::registration::FPCSInitialAlignment< PointSource, PointTarget, pcl::Normal, float >::max_pair_diff_
float max_pair_diff_
Definition ia_fpcs.h:533
pcl::registration::FPCSInitialAlignment< PointSource, PointTarget, pcl::Normal, float >::small_error_
const float small_error_
Definition ia_fpcs.h:556
pcl::registration::FPCSInitialAlignment::max_runtime_
int max_runtime_
Maximum allowed computation time in seconds (standard = 0 => ~unlimited).
Definition ia_fpcs.h:503
pcl::registration::FPCSInitialAlignment::computeTransformation
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method.
Definition ia_fpcs.hpp:142
pcl::registration::FPCSInitialAlignment::score_threshold_
float score_threshold_
Score threshold to stop calculation with success.
Definition ia_fpcs.h:491
pcl::registration::FPCSInitialAlignment::initCompute
virtual bool initCompute()
Internal computation initialization.
Definition ia_fpcs.hpp:225
pcl::registration::TransformationEstimation3Point
TransformationEstimation3Points represents the class for transformation estimation based on:
Definition transformation_estimation_3point.h:59
pcl::search::KdTree
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
Definition kdtree.h:62
pcl::search::KdTree::nearestKSearch
int nearestKSearch(const PointT &point, int k, Indices &k_indices, std::vector< float > &k_sqr_distances) const override
Search for the k-nearest neighbors for the given query point.
Definition kdtree.hpp:100
pcl::search::KdTree::setInputCloud
bool setInputCloud(const PointCloudConstPtr &cloud, const IndicesConstPtr &indices=IndicesConstPtr()) override
Provide a pointer to the input dataset.
Definition kdtree.hpp:88
distances.h
Define standard C methods to do distance calculations.
time.h
Define methods for measuring time spent in code blocks.
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::transformPointCloud
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
Definition transforms.hpp:221
pcl::compute3DCentroid
unsigned int compute3DCentroid(ConstCloudIterator< PointT > &cloud_iterator, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the 3D (X-Y-Z) centroid of a set of points and return it as a 3D vector.
Definition centroid.hpp:57
pcl::pointToPlaneDistance
double pointToPlaneDistance(const Point &p, double a, double b, double c, double d)
Get the distance from a point to a plane (unsigned) defined by ax+by+cz+d=0.
Definition sac_model_plane.h:114
pcl::registration::MatchingCandidates
std::vector< MatchingCandidate, Eigen::aligned_allocator< MatchingCandidate > > MatchingCandidates
Definition matching_candidate.h:81
pcl::utils::ignore
void ignore(const T &...)
Utility function to eliminate unused variable warnings.
Definition utils.h:62
pcl::squaredEuclideanDistance
float squaredEuclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the squared euclidean distance between the two given points.
Definition distances.h:182
pcl::Correspondences
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
Definition correspondence.h:89
pcl::IndicesPtr
shared_ptr< Indices > IndicesPtr
Definition pcl_base.h:58
pcl::getMeanPointDensity
float getMeanPointDensity(const typename pcl::PointCloud< PointT >::ConstPtr &cloud, float max_dist, int nr_threads=1)
Compute the mean point density of a given point cloud.
Definition ia_fpcs.hpp:54
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
pcl::euclideanDistance
float euclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the euclidean distance between the two given points.
Definition distances.h:204
M_PI
#define M_PI
Definition pcl_macros.h:203