39#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_3D_HPP_
40#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_CIRCLE_3D_HPP_
44#include <unsupported/Eigen/NonLinearOptimization>
45#include <pcl/sample_consensus/sac_model_circle3d.h>
46#include <pcl/common/concatenate.h>
49template <
typename Po
intT>
bool
55 PCL_ERROR (
"[pcl::SampleConsensusModelCircle3D::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n",
samples.
size (), sample_size_);
67 return (p1.dot (p2) < 0.000001);
71template <
typename Po
intT>
bool
77 PCL_ERROR (
"[pcl::SampleConsensusModelCircle3D::computeModelCoefficients] Invalid set of samples given (%lu)!\n",
samples.
size ());
117 PCL_DEBUG (
"[pcl::SampleConsensusModelCircle3D::computeModelCoefficients] Model is (%g,%g,%g,%g,%g,%g,%g).\n",
124template <
typename Po
intT>
void
133 distances.resize (indices_->size ());
136 for (std::size_t i = 0; i < indices_->size (); ++i)
146 Eigen::Vector3d
P ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
171template <
typename Po
intT>
void
183 inliers.reserve (indices_->size ());
187 for (std::size_t i = 0; i < indices_->size (); ++i)
191 Eigen::Vector3d
P ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
213 inliers.push_back ((*indices_)[i]);
219template <
typename Po
intT> std::size_t
226 std::size_t
nr_p = 0;
230 for (std::size_t i = 0; i < indices_->size (); ++i)
234 Eigen::Vector3d
P ((*input_)[(*indices_)[i]].x, (*input_)[(*indices_)[i]].y, (*input_)[(*indices_)[i]].z);
261template <
typename Po
intT>
void
272 PCL_ERROR (
"[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Given model is invalid!\n");
279 PCL_ERROR (
"[pcl::SampleConsensusModelCircle3D::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n",
inliers.
size ());
283 OptimizationFunctor functor (
this,
inliers);
284 Eigen::NumericalDiff<OptimizationFunctor>
num_diff (functor);
285 Eigen::LevenbergMarquardt<Eigen::NumericalDiff<OptimizationFunctor>,
double>
lm (
num_diff);
286 Eigen::VectorXd
coeff;
288 for (Eigen::Index i = 0; i <
coeff.
size (); ++i)
292 PCL_DEBUG (
"[pcl::SampleConsensusModelCircle3D::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",
293 info,
lm.fvec.norm (),
model_coefficients[0],
model_coefficients[1],
model_coefficients[2],
model_coefficients[3],
model_coefficients[4],
model_coefficients[5],
model_coefficients[6],
optimized_coefficients[0],
optimized_coefficients[1],
optimized_coefficients[2],
optimized_coefficients[3],
optimized_coefficients[4],
optimized_coefficients[5],
optimized_coefficients[6]);
297template <
typename Po
intT>
void
305 PCL_ERROR (
"[pcl::SampleConsensusModelCircle3D::projectPoints] Given model is invalid!\n");
320 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
362 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
399template <
typename Po
intT>
bool
401 const std::set<index_t> &indices,
403 const double threshold)
const
408 PCL_ERROR (
"[pcl::SampleConsensusModelCircle3D::doSamplesVerifyModel] Given model is invalid!\n");
413 for (
const auto &index : indices)
420 Eigen::Vector3d
P ((*input_)[index].x, (*input_)[index].y, (*input_)[index].z);
445template <
typename Po
intT>
bool
453 PCL_DEBUG (
"[pcl::SampleConsensusModelCircle3D::isModelValid] Radius of circle is too small: should be larger than %g, but is %g.\n",
459 PCL_DEBUG (
"[pcl::SampleConsensusModelCircle3D::isModelValid] Radius of circle is too big: should be smaller than %g, but is %g.\n",
467#define PCL_INSTANTIATE_SampleConsensusModelCircle3D(T) template class PCL_EXPORTS pcl::SampleConsensusModelCircle3D<T>;
Iterator class for point clouds with or without given indices.
ConstCloudIterator(const PointCloud< PointT > &cloud)
std::size_t size() const
Size of the range the iterator is going through.
bool isModelValid(const Eigen::VectorXf &model_coefficients) const override
Check whether a model is valid given the user constraints.
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 3d circle model coefficients.
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
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 3d circle model.
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 getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given 3D circle model.
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the 3d circle coefficients using the given inlier set and return them to the user.
typename SampleConsensusModel< PointT >::PointCloud PointCloud
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) override
Compute all distances from the cloud data to a given 3D circle model.
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid 2D circle model, compute the model coefficient...
SampleConsensusModel represents the base model class.
IndicesAllocator<> Indices
Type used for indices in PCL.