39#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CONE_H_
40#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CONE_H_
42#include <unsupported/Eigen/NonLinearOptimization>
43#include <pcl/sample_consensus/sac_model_cone.h>
45#include <pcl/common/concatenate.h>
48template <
typename Po
intT,
typename Po
intNT>
bool
53 PCL_ERROR (
"[pcl::SampleConsensusModelCone::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n",
samples.
size (), sample_size_);
60template <
typename Po
intT,
typename Po
intNT>
bool
67 PCL_ERROR (
"[pcl::SampleConsensusModelCone::computeModelCoefficients] Invalid set of samples given (%lu)!\n",
samples.
size ());
73 PCL_ERROR (
"[pcl::SampleConsensusModelCone::computeModelCoefficients] No input dataset containing normals was given!\n");
81 Eigen::Vector4f n1 ((*normals_)[
samples[0]].normal[0], (*normals_)[
samples[0]].normal[1], (*normals_)[
samples[0]].normal[2], 0.0f);
82 Eigen::Vector4f n2 ((*normals_)[
samples[1]].normal[0], (*normals_)[
samples[1]].normal[1], (*normals_)[
samples[1]].normal[2], 0.0f);
83 Eigen::Vector4f
n3 ((*normals_)[
samples[2]].normal[0], (*normals_)[
samples[2]].normal[1], (*normals_)[
samples[2]].normal[2], 0.0f);
86 Eigen::Vector4f
ortho12 = n1.cross3(n2);
90 float denominator = n1.dot(
ortho23);
92 float d1 = p1.dot (n1);
93 float d2 = p2.dot (n2);
94 float d3 = p3.dot (
n3);
99 Eigen::Vector4f
ap1 = p1 -
apex;
100 Eigen::Vector4f
ap2 = p2 -
apex;
101 Eigen::Vector4f
ap3 = p3 -
apex;
138 PCL_DEBUG (
"[pcl::SampleConsensusModelCone::computeModelCoefficients] Model is (%g,%g,%g,%g,%g,%g,%g).\n",
145template <
typename Po
intT,
typename Po
intNT>
void
156 distances.resize (indices_->size ());
165 for (std::size_t i = 0; i < indices_->size (); ++i)
167 Eigen::Vector4f
pt ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z, 0.0f);
190 Eigen::Vector4f n ((*normals_)[(*indices_)[i]].normal[0], (*normals_)[(*indices_)[i]].normal[1], (*normals_)[(*indices_)[i]].normal[2], 0.0f);
199template <
typename Po
intT,
typename Po
intNT>
void
211 error_sqr_dists_.clear ();
212 inliers.reserve (indices_->size ());
213 error_sqr_dists_.reserve (indices_->size ());
222 for (std::size_t i = 0; i < indices_->size (); ++i)
224 Eigen::Vector4f
pt ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z, 0.0f);
249 Eigen::Vector4f n ((*normals_)[(*indices_)[i]].normal[0], (*normals_)[(*indices_)[i]].normal[1], (*normals_)[(*indices_)[i]].normal[2], 0.0f);
255 if (distance < threshold)
258 inliers.push_back ((*indices_)[i]);
259 error_sqr_dists_.push_back (distance);
265template <
typename Po
intT,
typename Po
intNT> std::size_t
274 std::size_t
nr_p = 0;
283 for (std::size_t i = 0; i < indices_->size (); ++i)
285 Eigen::Vector4f
pt ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z, 0.0f);
310 Eigen::Vector4f n ((*normals_)[(*indices_)[i]].normal[0], (*normals_)[(*indices_)[i]].normal[1], (*normals_)[(*indices_)[i]].normal[2], 0.0f);
321template <
typename Po
intT,
typename Po
intNT>
void
330 PCL_ERROR (
"[pcl::SampleConsensusModelCone::optimizeModelCoefficients] Given model is invalid!\n");
337 PCL_ERROR (
"[pcl::SampleConsensusModelCone:optimizeModelCoefficients] Not enough inliers found to optimize model coefficients (%lu)! Returning the same coefficients.\n",
inliers.
size ());
341 OptimizationFunctor functor (
this,
inliers);
342 Eigen::NumericalDiff<OptimizationFunctor >
num_diff (functor);
343 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>,
float>
lm (
num_diff);
347 PCL_DEBUG (
"[pcl::SampleConsensusModelCone::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",
359template <
typename Po
intT,
typename Po
intNT>
void
366 PCL_ERROR (
"[pcl::SampleConsensusModelCone::projectPoints] Given model is invalid!\n");
388 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
397 Eigen::Vector4f
pt ((*input_)[
inlier].x,
411 Eigen::Vector4f height =
apex -
pp;
425 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
445 Eigen::Vector4f height =
apex -
pp;
455template <
typename Po
intT,
typename Po
intNT>
bool
457 const std::set<index_t> &indices,
const Eigen::VectorXf &
model_coefficients,
const double threshold)
const
462 PCL_ERROR (
"[pcl::SampleConsensusModelCone::doSamplesVerifyModel] Given model is invalid!\n");
474 for (
const auto &index : indices)
476 Eigen::Vector4f
pt ((*input_)[index].x, (*input_)[index].y, (*input_)[index].z, 0.0f);
498template <
typename Po
intT,
typename Po
intNT>
double
508template <
typename Po
intT,
typename Po
intNT>
bool
515 if (eps_angle_ > 0.0)
525 PCL_DEBUG (
"[pcl::SampleConsensusModelCone::isModelValid] Angle between cone direction and given axis is too large.\n");
532 PCL_DEBUG (
"[pcl::SampleConsensusModelCone::isModelValid] The opening angle is too small: should be larger than %g, but is %g.\n",
538 PCL_DEBUG (
"[pcl::SampleConsensusModelCone::isModelValid] The opening angle is too big: should be smaller than %g, but is %g.\n",
546#define PCL_INSTANTIATE_SampleConsensusModelCone(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelCone<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 optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the cone coefficients using the given inlier set and return them to the user.
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 cone model.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given cone model.
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.
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid cone model, compute the model coefficients fro...
bool isModelValid(const Eigen::VectorXf &model_coefficients) const override
Check whether a model is valid given the user constraints.
double pointToAxisDistance(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 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 cone model coefficients.
typename SampleConsensusModel< PointT >::PointCloud PointCloud
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.
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