Loading...
Searching...
No Matches
sac_model_torus.hpp
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2009-2010, Willow Garage, Inc.
* Copyright (c) 2012-, Open Perception, Inc.
*
* 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.
*
* $Id$
*
*/
#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_TORUS_H_
#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_TORUS_H_
// clang-format off
#include <pcl/sample_consensus/sac_model_torus.h>
#include <pcl/common/concatenate.h>
// clang-format on
#include <unsupported/Eigen/NonLinearOptimization> // for LevenbergMarquardt
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT>
bool
pcl::SampleConsensusModelTorus<PointT, PointNT>::isSampleGood(
const Indices& samples) const
{
if (samples.size() != sample_size_) {
PCL_ERROR("[pcl::SampleConsensusTorus::isSampleGood] Wrong number of samples (is "
"%lu, should be %lu)!\n",
samples.size(),
sample_size_);
return (false);
}
Eigen::Vector3f n0 = Eigen::Vector3f((*normals_)[samples[0]].getNormalVector3fMap());
Eigen::Vector3f n1 = Eigen::Vector3f((*normals_)[samples[1]].getNormalVector3fMap());
Eigen::Vector3f n2 = Eigen::Vector3f((*normals_)[samples[2]].getNormalVector3fMap());
Eigen::Vector3f n3 = Eigen::Vector3f((*normals_)[samples[3]].getNormalVector3fMap());
// Required for numeric stability on computeModelCoefficients
if (std::abs((n0).cross(n1).squaredNorm()) <
Eigen::NumTraits<float>::dummy_precision() ||
std::abs((n0).cross(n2).squaredNorm()) <
Eigen::NumTraits<float>::dummy_precision() ||
std::abs((n0).cross(n3).squaredNorm()) <
Eigen::NumTraits<float>::dummy_precision() ||
std::abs((n1).cross(n2).squaredNorm()) <
Eigen::NumTraits<float>::dummy_precision() ||
std::abs((n1).cross(n3).squaredNorm()) <
Eigen::NumTraits<float>::dummy_precision()) {
PCL_ERROR("[pcl::SampleConsensusModelEllipse3D::isSampleGood] Sample points "
"normals too similar or collinear!\n");
return (false);
}
return (true);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
float
crossDot(const Eigen::Vector3f& v1, const Eigen::Vector3f& v2, const Eigen::Vector3f& v3)
{
return v1.cross(v2).dot(v3);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT>
bool
pcl::SampleConsensusModelTorus<PointT, PointNT>::computeModelCoefficients(
const Indices& samples, Eigen::VectorXf& model_coefficients) const
{
// Make sure that the samples are valid
if (!isSampleGood(samples)) {
PCL_ERROR("[pcl::SampleConsensusModelTorus::computeModelCoefficients] Invalid set "
"of samples given!\n");
return (false);
}
if (!normals_) {
PCL_ERROR("[pcl::SampleConsensusModelTorus::computeModelCoefficients] No input "
"dataset containing normals was given!\n");
return (false);
}
// Find axis using:
// @article{article,
// author = {Lukacs, G. and Marshall, David and Martin, R.},
// year = {2001},
// month = {09},
// pages = {},
// title = {Geometric Least-Squares Fitting of Spheres, Cylinders, Cones and Tori}
//}
const Eigen::Vector3f n0 = Eigen::Vector3f((*normals_)[samples[0]].getNormalVector3fMap());
const Eigen::Vector3f n1 = Eigen::Vector3f((*normals_)[samples[1]].getNormalVector3fMap());
const Eigen::Vector3f n2 = Eigen::Vector3f((*normals_)[samples[2]].getNormalVector3fMap());
const Eigen::Vector3f n3 = Eigen::Vector3f((*normals_)[samples[3]].getNormalVector3fMap());
const Eigen::Vector3f p0 = Eigen::Vector3f((*input_)[samples[0]].getVector3fMap());
const Eigen::Vector3f p1 = Eigen::Vector3f((*input_)[samples[1]].getVector3fMap());
const Eigen::Vector3f p2 = Eigen::Vector3f((*input_)[samples[2]].getVector3fMap());
const Eigen::Vector3f p3 = Eigen::Vector3f((*input_)[samples[3]].getVector3fMap());
const float a01 = crossDot(n0, n1, n2);
const float b01 = crossDot(n0, n1, n3);
const float a0 = crossDot(p2 - p1, n0, n2);
const float a1 = crossDot(p0 - p2, n1, n2);
const float b0 = crossDot(p3 - p1, n0, n3);
const float b1 = crossDot(p0 - p3, n1, n3);
const float a = crossDot(p0 - p2, p1 - p0, n2);
const float b = crossDot(p0 - p3, p1 - p0, n3);
// a10*t0*t1 + a0*t0 + a1*t1 + a = 0
// b10*t0*t1 + b0*t0 + b1*t1 + b = 0
//
// (a0 - b0*a10/b10)* t0 + (a1-b1*a10/b10) *t1 + a - b*a10/b10
// t0 = k * t1 + p
const float k = -(a1 - b1 * a01 / b01) / (a0 - b0 * a01 / b01);
const float p = -(a - b * a01 / b01) / (a0 - b0 * a01 / b01);
// Second deg eqn.
//
// b10*k*t1*t1 + b10*p*t1 | + b0*k *t1 + b0*p | + b1*t1 | + b = 0
//
// (b10*k) * t1*t1 + (b10*p + b0*k + b1) * t1 + (b0*p + b)
const float _a = (b01 * k);
const float _b = (b01 * p + b0 * k + b1);
const float _c = (b0 * p + b);
const float eps = Eigen::NumTraits<float>::dummy_precision();
// Check for imaginary solutions, or small denominators.
if ((_b * _b - 4 * _a * _c) < 0 || std::abs(a0 - b0 * a01) < eps ||
std::abs(b01) < eps || std::abs(_a) < eps) {
PCL_DEBUG("[pcl::SampleConsensusModelTorus::computeModelCoefficients] Can't "
"compute model coefficients with this method!\n");
return (false);
}
const float s0 = (-_b + std::sqrt(_b * _b - 4 * _a * _c)) / (2 * _a);
const float s1 = (-_b - std::sqrt(_b * _b - 4 * _a * _c)) / (2 * _a);
float r_maj_stddev_cycle1 = std::numeric_limits<float>::max();
for (float s : {s0, s1}) {
const float t1 = s;
const float t0 = k * t1 + p;
// Direction vector
Eigen::Vector3f d = ((p1 + n1 * t1) - (p0 + n0 * t0));
d.normalize();
// Flip direction, so that the first element of the direction vector is
// positive, for consistency.
if (d[0] < 0) {
d *= -1;
}
// Flip normals if required. Note |d| = 1
// d
// if (n0.dot(d) / n0.norm() < M_PI / 2 ) n0 = -n0;
// if (n1.dot(d) / n1.norm() < M_PI / 2 ) n1 = -n1;
// if (n2.dot(d) / n2.norm() < M_PI / 2 ) n2 = -n2;
// if (n3.dot(d) / n3.norm() < M_PI / 2 ) n3 = -n3;
// We fit the points to the plane of the torus.
// Ax + By + Cz + D = 0
// We know that all for each point plus its normal
// times the minor radius will give us a point
// in that plane
// Pplane_i = P_i + n_i * r
// we substitute A,x,B,y,C,z
// dx *( P_i_x + n_i_x * r ) + dy *( P_i_y + n_i_y * r ) +dz *( P_i_z + n_i_z * r )
// + D = 0 and finally (dx*P_i_x + dy*P_i_y + dz*P_i_z) + (dx*n_i_x + dy*n_i_y +
// dz*n_i_z ) * r + D = 0 We can set up a linear least squares system of two
// variables r and D
//
Eigen::MatrixXf A(4, 2);
A << d.dot(n0), 1, d.dot(n1), 1, d.dot(n2), 1, d.dot(n3), 1;
Eigen::Matrix<float, -1, -1> B(4, 1);
B << -d.dot(p0), -d.dot(p1), -d.dot(p2), -d.dot(p3);
Eigen::Matrix<float, -1, -1> sol;
sol = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B);
const float r_min = -sol(0);
const float D = sol(1);
// Axis line and plane intersect to find the centroid of the torus
// We take a random point on the line. We find P_rand + lambda * d belongs in the
// plane
const Eigen::Vector3f Pany = (p1 + n1 * t1);
const float lambda = (-d.dot(Pany) - D) / d.dot(d);
const Eigen::Vector3f centroid = Pany + d * lambda;
// Finally, the major radius. The least square solution will be
// the average in this case.
const float r_maj = std::sqrt(((p0 - r_min * n0 - centroid).squaredNorm() +
(p1 - r_min * n1 - centroid).squaredNorm() +
(p2 - r_min * n2 - centroid).squaredNorm() +
(p3 - r_min * n3 - centroid).squaredNorm()) /
4.f);
const float r_maj_stddev =
std::sqrt((std::pow(r_maj - (p0 - r_min * n0 - centroid).norm(), 2) +
std::pow(r_maj - (p1 - r_min * n1 - centroid).norm(), 2) +
std::pow(r_maj - (p2 - r_min * n2 - centroid).norm(), 2) +
std::pow(r_maj - (p3 - r_min * n3 - centroid).norm(), 2)) /
4.f);
// We select the minimum stddev cycle
if (r_maj_stddev < r_maj_stddev_cycle1) {
r_maj_stddev_cycle1 = r_maj_stddev;
}
else {
break;
}
model_coefficients.resize(model_size_);
model_coefficients[0] = r_maj;
model_coefficients[1] = r_min;
model_coefficients[2] = centroid[0];
model_coefficients[3] = centroid[1];
model_coefficients[4] = centroid[2];
model_coefficients[5] = d[0];
model_coefficients[6] = d[1];
model_coefficients[7] = d[2];
}
return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT>
void
pcl::SampleConsensusModelTorus<PointT, PointNT>::getDistancesToModel(
const Eigen::VectorXf& model_coefficients, std::vector<double>& distances) const
{
if (!isModelValid(model_coefficients)) {
distances.clear();
return;
}
distances.resize(indices_->size());
// Iterate through the 3d points and calculate the distances to the closest torus
// point
for (std::size_t i = 0; i < indices_->size(); ++i) {
const Eigen::Vector3f pt = (*input_)[(*indices_)[i]].getVector3fMap();
const Eigen::Vector3f pt_n = (*normals_)[(*indices_)[i]].getNormalVector3fMap();
Eigen::Vector3f torus_closest;
projectPointToTorus(pt, pt_n, model_coefficients, torus_closest);
distances[i] = (torus_closest - pt).norm();
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT>
void
pcl::SampleConsensusModelTorus<PointT, PointNT>::selectWithinDistance(
const Eigen::VectorXf& model_coefficients, const double threshold, Indices& inliers)
{
// Check if the model is valid given the user constraints
if (!isModelValid(model_coefficients)) {
inliers.clear();
return;
}
const float squared_threshold = threshold * threshold;
inliers.clear();
error_sqr_dists_.clear();
inliers.reserve(indices_->size());
error_sqr_dists_.reserve(indices_->size());
for (std::size_t i = 0; i < indices_->size(); ++i) {
const Eigen::Vector3f pt = (*input_)[(*indices_)[i]].getVector3fMap();
const Eigen::Vector3f pt_n = (*normals_)[(*indices_)[i]].getNormalVector3fMap();
Eigen::Vector3f torus_closest;
projectPointToTorus(pt, pt_n, model_coefficients, torus_closest);
const float distance = (torus_closest - pt).squaredNorm();
if (distance < squared_threshold) {
// Returns the indices of the points whose distances are smaller than the
// threshold
inliers.push_back((*indices_)[i]);
error_sqr_dists_.push_back(std::sqrt(distance));
}
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT>
std::size_t
pcl::SampleConsensusModelTorus<PointT, PointNT>::countWithinDistance(
const Eigen::VectorXf& model_coefficients, const double threshold) const
{
if (!isModelValid(model_coefficients))
return (0);
const float squared_threshold = threshold * threshold;
std::size_t nr_p = 0;
for (std::size_t i = 0; i < indices_->size(); ++i) {
const Eigen::Vector3f pt = (*input_)[(*indices_)[i]].getVector3fMap();
const Eigen::Vector3f pt_n = (*normals_)[(*indices_)[i]].getNormalVector3fMap();
Eigen::Vector3f torus_closest;
projectPointToTorus(pt, pt_n, model_coefficients, torus_closest);
const float distance = (torus_closest - pt).squaredNorm();
if (distance < squared_threshold) {
nr_p++;
}
}
return (nr_p);
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT>
void
pcl::SampleConsensusModelTorus<PointT, PointNT>::optimizeModelCoefficients(
const Indices& inliers,
const Eigen::VectorXf& model_coefficients,
Eigen::VectorXf& optimized_coefficients) const
{
optimized_coefficients = model_coefficients;
// Needs a set of valid model coefficients
if (!isModelValid(model_coefficients)) {
PCL_ERROR("[pcl::SampleConsensusModelTorus::optimizeModelCoefficients] Given model "
"is invalid!\n");
return;
}
// Need more than the minimum sample size to make a difference
if (inliers.size() <= sample_size_) {
PCL_ERROR("[pcl::SampleConsensusModelTorus::optimizeModelCoefficients] Not enough "
"inliers to refine/optimize the model's coefficients (%lu)! Returning "
"the same coefficients.\n",
inliers.size());
return;
}
OptimizationFunctor functor(this, inliers);
Eigen::NumericalDiff<OptimizationFunctor> num_diff(functor);
Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>, double> lm(
num_diff);
Eigen::VectorXd coeff = model_coefficients.cast<double>();
int info = lm.minimize(coeff);
if (!info) {
PCL_ERROR(
"[pcl::SampleConsensusModelTorus::optimizeModelCoefficients] Optimizer returned"
"with error (%i)! Returning ",
info);
return;
}
// Normalize direction vector
coeff.tail<3>().normalize();
optimized_coefficients = coeff.cast<float>();
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT>
void
pcl::SampleConsensusModelTorus<PointT, PointNT>::projectPointToTorus(
const Eigen::Vector3f& p_in,
const Eigen::Vector3f& p_n,
const Eigen::VectorXf& model_coefficients,
Eigen::Vector3f& pt_out) const
{
// Fetch optimization parameters
const float& R = model_coefficients[0];
const float& r = model_coefficients[1];
const float& x0 = model_coefficients[2];
const float& y0 = model_coefficients[3];
const float& z0 = model_coefficients[4];
const float& nx = model_coefficients[5];
const float& ny = model_coefficients[6];
const float& nz = model_coefficients[7];
// Normal of the plane where the torus circle lies
Eigen::Vector3f n{nx, ny, nz};
n.normalize();
Eigen::Vector3f pt0{x0, y0, z0};
// Ax + By + Cz + D = 0
const float D = -n.dot(pt0);
// Project to the torus circle plane folling the point normal
// we want to find lambda such that p + pn_n*lambda lies on the
// torus plane.
// A*(pt_x + lambda*pn_x) + B *(pt_y + lambda*pn_y) + ... + D = 0
// given that: n = [A,B,C]
// n.dot(P) + lambda*n.dot(pn) + D = 0
//
Eigen::Vector3f pt_proj;
// If the point lies in the torus plane, we just use it as projected
// C++20 -> [[likely]]
if (std::abs(n.dot(p_n)) > Eigen::NumTraits<float>::dummy_precision()) {
float lambda = (-D - n.dot(p_in)) / n.dot(p_n);
pt_proj = p_in + lambda * p_n;
}
else {
pt_proj = p_in;
}
// Closest point from the inner circle to the current point
const Eigen::Vector3f circle_closest = (pt_proj - pt0).normalized() * R + pt0;
// From the that closest point we move towards the goal point until we
// meet the surface of the torus
pt_out = (p_in - circle_closest).normalized() * r + circle_closest;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT>
void
pcl::SampleConsensusModelTorus<PointT, PointNT>::projectPoints(
const Indices& inliers,
const Eigen::VectorXf& model_coefficients,
PointCloud& projected_points,
bool copy_data_fields) const
{
// Needs a valid set of model coefficients
if (!isModelValid(model_coefficients)) {
PCL_ERROR(
"[pcl::SampleConsensusModelCylinder::projectPoints] Given model is invalid!\n");
return;
}
// Copy all the data fields from the input cloud to the projected one?
if (copy_data_fields) {
// Allocate enough space and copy the basics
projected_points.resize(input_->size());
projected_points.width = input_->width;
projected_points.height = input_->height;
using FieldList = typename pcl::traits::fieldList<PointT>::type;
// Iterate over each point
for (std::size_t i = 0; i < input_->size(); ++i)
// Iterate over each dimension
pcl::for_each_type<FieldList>(
NdConcatenateFunctor<PointT, PointT>((*input_)[i], projected_points[i]));
// Iterate through the 3d points and calculate the distances from them to the plane
for (const auto& inlier : inliers) {
Eigen::Vector3f q;
const Eigen::Vector3f pt_n = (*normals_)[inlier].getNormalVector3fMap();
projectPointToTorus(
(*input_)[inlier].getVector3fMap(), pt_n, model_coefficients, q);
projected_points[inlier].getVector3fMap() = q;
}
}
else {
// Allocate enough space and copy the basics
projected_points.resize(inliers.size());
projected_points.width = inliers.size();
projected_points.height = 1;
using FieldList = typename pcl::traits::fieldList<PointT>::type;
// Iterate over each point
for (std::size_t i = 0; i < inliers.size(); ++i) {
// Iterate over each dimension
pcl::for_each_type<FieldList>(NdConcatenateFunctor<PointT, PointT>(
(*input_)[inliers[i]], projected_points[i]));
}
for (const auto& inlier : inliers) {
Eigen::Vector3f q;
const Eigen::Vector3f pt_n = (*normals_)[inlier].getNormalVector3fMap();
projectPointToTorus(
(*input_)[inlier].getVector3fMap(), pt_n, model_coefficients, q);
projected_points[inlier].getVector3fMap() = q;
}
}
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT>
bool
pcl::SampleConsensusModelTorus<PointT, PointNT>::doSamplesVerifyModel(
const std::set<index_t>& indices,
const Eigen::VectorXf& model_coefficients,
const double threshold) const
{
for (const auto& index : indices) {
const Eigen::Vector3f pt_n = (*normals_)[index].getNormalVector3fMap();
Eigen::Vector3f torus_closest;
projectPointToTorus((*input_)[index].getVector3fMap(), pt_n, model_coefficients, torus_closest);
if (((*input_)[index].getVector3fMap() - torus_closest).squaredNorm() > threshold * threshold)
return (false);
}
return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT, typename PointNT>
bool
pcl::SampleConsensusModelTorus<PointT, PointNT>::isModelValid(
const Eigen::VectorXf& model_coefficients) const
{
if (!SampleConsensusModel<PointT>::isModelValid(model_coefficients))
return (false);
if (radius_min_ != std::numeric_limits<double>::lowest() &&
(model_coefficients[0] < radius_min_ || model_coefficients[1] < radius_min_)) {
PCL_DEBUG(
"[pcl::SampleConsensusModelTorus::isModelValid] Major radius OR minor radius "
"of torus is/are too small: should be larger than %g, but are {%g, %g}.\n",
radius_min_,
model_coefficients[0],
model_coefficients[1]);
return (false);
}
if (radius_max_ != std::numeric_limits<double>::max() &&
(model_coefficients[0] > radius_max_ || model_coefficients[1] > radius_max_)) {
PCL_DEBUG(
"[pcl::SampleConsensusModelTorus::isModelValid] Major radius OR minor radius "
"of torus is/are too big: should be smaller than %g, but are {%g, %g}.\n",
radius_max_,
model_coefficients[0],
model_coefficients[1]);
return (false);
}
return (true);
}
#define PCL_INSTANTIATE_SampleConsensusModelTorus(PointT, PointNT) \
template class PCL_EXPORTS pcl::SampleConsensusModelTorus<PointT, PointNT>;
#endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_TORUS_H_
pcl::SampleConsensusModelFromNormals::normals_
PointCloudNConstPtr normals_
A pointer to the input dataset that contains the point normals of the XYZ dataset.
Definition sac_model.h:671
pcl::SampleConsensusModel::radius_max_
double radius_max_
Definition sac_model.h:565
pcl::SampleConsensusModel::radius_min_
double radius_min_
The minimum and maximum radius limits for the model.
Definition sac_model.h:565
pcl::SampleConsensusModel::sample_size_
unsigned int sample_size_
The size of a sample from which the model is computed.
Definition sac_model.h:589
pcl::SampleConsensusModel::indices_
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition sac_model.h:557
pcl::SampleConsensusModel::input_
PointCloudConstPtr input_
A boost shared pointer to the point cloud data array.
Definition sac_model.h:554
pcl::SampleConsensusModel::isModelValid
virtual bool isModelValid(const Eigen::VectorXf &model_coefficients) const
Check whether a model is valid given the user constraints.
Definition sac_model.h:528
pcl::SampleConsensusModel::error_sqr_dists_
std::vector< double > error_sqr_dists_
A vector holding the distances to the computed model.
Definition sac_model.h:586
pcl::SampleConsensusModelTorus::projectPointToTorus
void projectPointToTorus(const Eigen::Vector3f &pt, const Eigen::Vector3f &pt_n, const Eigen::VectorXf &model_coefficients, Eigen::Vector3f &pt_proj) const
Project a point onto a torus given by its model coefficients (radii, torus_center_point,...
Definition sac_model_torus.hpp:409
pcl::SampleConsensusModelTorus::selectWithinDistance
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) override
Select all the points which respect the given model coefficients as inliers.
Definition sac_model_torus.hpp:298
pcl::SampleConsensusModelTorus::doSamplesVerifyModel
bool doSamplesVerifyModel(const std::set< index_t > &indices, const Eigen::VectorXf &model_coefficients, const double threshold) const override
Verify whether a subset of indices verifies the given torus model coefficients.
Definition sac_model_torus.hpp:531
pcl::SampleConsensusModelTorus::isModelValid
bool isModelValid(const Eigen::VectorXf &model_coefficients) const override
Check whether a model is valid given the user constraints.
Definition sac_model_torus.hpp:551
pcl::SampleConsensusModelTorus::computeModelCoefficients
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid torus model, compute the model coefficients fr...
Definition sac_model_torus.hpp:97
pcl::SampleConsensusModelTorus::optimizeModelCoefficients
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the torus coefficients using the given inlier set and return them to the user.
Definition sac_model_torus.hpp:361
pcl::SampleConsensusModelTorus::countWithinDistance
std::size_t countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const override
Count all the points which respect the given model coefficients as inliers.
Definition sac_model_torus.hpp:333
pcl::SampleConsensusModelTorus::isSampleGood
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
Definition sac_model_torus.hpp:53
pcl::SampleConsensusModelTorus::getDistancesToModel
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given torus model.
Definition sac_model_torus.hpp:271
pcl::SampleConsensusModelTorus::projectPoints
void projectPoints(const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true) const override
Create a new point cloud with inliers projected onto the torus model.
Definition sac_model_torus.hpp:468
pcl::B
@ B
Definition norms.h:54
pcl::distances
Definition distances.h:50
pcl::for_each_type
void for_each_type(F f)
Definition for_each_type.h:91
pcl::Indices
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
pcl::NdConcatenateFunctor
Helper functor structure for concatenate.
Definition concatenate.h:50