41#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CYLINDER_H_
42#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CYLINDER_H_
44#include <unsupported/Eigen/NonLinearOptimization>
45#include <pcl/sample_consensus/sac_model_cylinder.h>
47#include <pcl/common/concatenate.h>
50template <
typename Po
intT,
typename Po
intNT>
bool
55 PCL_ERROR (
"[pcl::SampleConsensusModelCylinder::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n",
samples.
size (), sample_size_);
62template <
typename Po
intT,
typename Po
intNT>
bool
69 PCL_ERROR (
"[pcl::SampleConsensusModelCylinder::computeModelCoefficients] Invalid set of samples given (%lu)!\n",
samples.
size ());
75 PCL_ERROR (
"[pcl::SampleConsensusModelCylinder::computeModelCoefficients] No input dataset containing normals was given!\n");
79 if (std::abs ((*input_)[
samples[0]].x - (*input_)[
samples[1]].x) <= std::numeric_limits<float>::epsilon () &&
80 std::abs ((*input_)[
samples[0]].y - (*input_)[
samples[1]].y) <= std::numeric_limits<float>::epsilon () &&
81 std::abs ((*input_)[
samples[0]].z - (*input_)[
samples[1]].z) <= std::numeric_limits<float>::epsilon ())
89 Eigen::Vector4f n1 ((*normals_)[
samples[0]].normal[0], (*normals_)[
samples[0]].normal[1], (*normals_)[
samples[0]].normal[2], 0.0f);
90 Eigen::Vector4f n2 ((*normals_)[
samples[1]].normal[0], (*normals_)[
samples[1]].normal[1], (*normals_)[
samples[1]].normal[2], 0.0f);
91 Eigen::Vector4f w = n1 + p1 - p2;
93 float a = n1.dot (n1);
94 float b = n1.dot (n2);
95 float c = n2.dot (n2);
98 float denominator = a*
c - b*b;
101 if (denominator < 1
e-8)
104 tc = (b >
c ? d / b :
e /
c);
108 sc = (b*
e -
c*d) / denominator;
109 tc = (a*
e - b*d) / denominator;
113 Eigen::Vector4f
line_pt = p1 + n1 +
sc * n1;
132 PCL_DEBUG (
"[pcl::SampleConsensusModelCylinder::computeModelCoefficients] Model is (%g,%g,%g,%g,%g,%g,%g).\n",
139template <
typename Po
intT,
typename Po
intNT>
void
150 distances.resize (indices_->size ());
157 for (std::size_t i = 0; i < indices_->size (); ++i)
162 Eigen::Vector4f
pt ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z, 0.0f);
173 Eigen::Vector4f n ((*normals_)[(*indices_)[i]].normal[0], (*normals_)[(*indices_)[i]].normal[1], (*normals_)[(*indices_)[i]].normal[2], 0.0f);
182template <
typename Po
intT,
typename Po
intNT>
void
194 error_sqr_dists_.clear ();
195 inliers.reserve (indices_->size ());
196 error_sqr_dists_.reserve (indices_->size ());
203 for (std::size_t i = 0; i < indices_->size (); ++i)
207 Eigen::Vector4f
pt ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z, 0.0f);
219 Eigen::Vector4f n ((*normals_)[(*indices_)[i]].normal[0], (*normals_)[(*indices_)[i]].normal[1], (*normals_)[(*indices_)[i]].normal[2], 0.0f);
224 if (distance < threshold)
227 inliers.push_back ((*indices_)[i]);
228 error_sqr_dists_.push_back (distance);
234template <
typename Po
intT,
typename Po
intNT> std::size_t
242 std::size_t
nr_p = 0;
249 for (std::size_t i = 0; i < indices_->size (); ++i)
253 Eigen::Vector4f
pt ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z, 0.0f);
265 Eigen::Vector4f n ((*normals_)[(*indices_)[i]].normal[0], (*normals_)[(*indices_)[i]].normal[1], (*normals_)[(*indices_)[i]].normal[2], 0.0f);
276template <
typename Po
intT,
typename Po
intNT>
void
285 PCL_ERROR (
"[pcl::SampleConsensusModelCylinder::optimizeModelCoefficients] Given model is invalid!\n");
292 PCL_ERROR (
"[pcl::SampleConsensusModelCylinder:optimizeModelCoefficients] Not enough inliers found to optimize model coefficients (%lu)! Returning the same coefficients.\n",
inliers.
size ());
296 OptimizationFunctor functor (
this,
inliers);
297 Eigen::NumericalDiff<OptimizationFunctor >
num_diff (functor);
298 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>,
float>
lm (
num_diff);
302 PCL_DEBUG (
"[pcl::SampleConsensusModelCylinder::optimizeModelCoefficients] LM solver finished with exit code %i, having a residual norm of %g. \nInitial solution: %g %g %g %g %g %g %g \nFinal solution: %g %g %g %g %g %g %g\n",
314template <
typename Po
intT,
typename Po
intNT>
void
321 PCL_ERROR (
"[pcl::SampleConsensusModelCylinder::projectPoints] Given model is invalid!\n");
341 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
350 Eigen::Vector4f p ((*input_)[
inlier].x,
360 Eigen::Vector4f
dir = p -
pp;
374 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
390 Eigen::Vector4f
dir = p -
pp;
400template <
typename Po
intT,
typename Po
intNT>
bool
402 const std::set<index_t> &indices,
const Eigen::VectorXf &
model_coefficients,
const double threshold)
const
407 PCL_ERROR (
"[pcl::SampleConsensusModelCylinder::doSamplesVerifyModel] Given model is invalid!\n");
411 for (
const auto &index : indices)
416 Eigen::Vector4f
pt ((*input_)[index].x, (*input_)[index].y, (*input_)[index].z, 0.0f);
425template <
typename Po
intT,
typename Po
intNT>
double
435template <
typename Po
intT,
typename Po
intNT>
void
453template <
typename Po
intT,
typename Po
intNT>
bool
460 if (eps_angle_ > 0.0)
470 PCL_DEBUG (
"[pcl::SampleConsensusModelCylinder::isModelValid] Angle between cylinder direction and given axis is too large.\n");
475 if (radius_min_ != -std::numeric_limits<double>::max() &&
model_coefficients[6] < radius_min_)
477 PCL_DEBUG (
"[pcl::SampleConsensusModelCylinder::isModelValid] Radius is too small: should be larger than %g, but is %g.\n",
481 if (radius_max_ != std::numeric_limits<double>::max() &&
model_coefficients[6] > radius_max_)
483 PCL_DEBUG (
"[pcl::SampleConsensusModelCylinder::isModelValid] Radius is too big: should be smaller than %g, but is %g.\n",
491#define PCL_INSTANTIATE_SampleConsensusModelCylinder(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelCylinder<PointT, PointNT>;
Iterator class for point clouds with or without given indices.
std::size_t size() const
Size of the range the iterator is going through.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given cylinder model.
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 cylinder model.
bool isModelValid(const Eigen::VectorXf &model_coefficients) const override
Check whether a model is valid given the user constraints.
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the cylinder coefficients using the given inlier set and return them to the user.
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.
void projectPointToCylinder(const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients, Eigen::Vector4f &pt_proj) const
Project a point onto a cylinder given by its model coefficients (point_on_axis, axis_direction,...
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.
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
typename SampleConsensusModel< PointT >::PointCloud PointCloud
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 cylinder model coefficients.
double pointToLineDistance(const Eigen::Vector4f &pt, const Eigen::VectorXf &model_coefficients) const
Get the distance from a point to a line (represented by a point and a direction)
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid cylinder model, compute the model coefficients...
SampleConsensusModel represents the base model class.
Define standard C methods and C++ classes that are common to all methods.
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.
double sqrPointToLineDistance(const Eigen::Vector4f &pt, const Eigen::Vector4f &line_pt, const Eigen::Vector4f &line_dir)
Get the square distance from a point to a line (represented by a point and a direction)
Eigen::Map< Eigen::Vector4f, Eigen::Aligned > Vector4fMap
IndicesAllocator<> Indices
Type used for indices in PCL.
const Eigen::Map< const Eigen::Vector4f, Eigen::Aligned > Vector4fMapConst