41#ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_STICK_H_
42#define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_STICK_H_
44#include <pcl/sample_consensus/sac_model_stick.h>
46#include <pcl/common/concatenate.h>
47#include <pcl/common/eigen.h>
50template <
typename Po
intT>
bool
55 PCL_ERROR (
"[pcl::SampleConsensusModelStick::isSampleGood] Wrong number of samples (is %lu, should be %lu)!\n",
samples.
size (), sample_size_);
72template <
typename Po
intT>
bool
79 PCL_ERROR (
"[pcl::SampleConsensusModelStick::computeModelCoefficients] Invalid set of samples given (%lu)!\n",
samples.
size ());
99 PCL_DEBUG (
"[pcl::SampleConsensusModelStick::computeModelCoefficients] Model is (%g,%g,%g,%g,%g,%g).\n",
106template <
typename Po
intT>
void
113 PCL_ERROR (
"[pcl::SampleConsensusModelStick::getDistancesToModel] Given model is invalid!\n");
117 float sqr_threshold =
static_cast<float> (radius_max_ * radius_max_);
118 distances.resize (indices_->size ());
126 for (std::size_t i = 0; i < indices_->size (); ++i)
146template <
typename Po
intT>
void
153 PCL_ERROR (
"[pcl::SampleConsensusModelStick::selectWithinDistance] Given model is invalid!\n");
157 float sqr_threshold =
static_cast<float> (threshold * threshold);
160 error_sqr_dists_.clear ();
161 inliers.reserve (indices_->size ());
162 error_sqr_dists_.reserve (indices_->size ());
174 for (std::size_t i = 0; i < indices_->size (); ++i)
178 Eigen::Vector4f
dir = (*input_)[(*indices_)[i]].getVector4fMap () -
line_pt1;
189 inliers.push_back ((*indices_)[i]);
190 error_sqr_dists_.push_back (
static_cast<double> (
sqr_distance));
196template <
typename Po
intT> std::size_t
203 PCL_ERROR (
"[pcl::SampleConsensusModelStick::countWithinDistance] Given model is invalid!\n");
207 float sqr_threshold =
static_cast<float> (threshold * threshold);
221 for (std::size_t i = 0; i < indices_->size (); ++i)
225 Eigen::Vector4f
dir = (*input_)[(*indices_)[i]].getVector4fMap () -
line_pt1;
248template <
typename Po
intT>
void
262 PCL_ERROR (
"[pcl::SampleConsensusModelStick::optimizeModelCoefficients] Not enough inliers to refine/optimize the model's coefficients (%lu)! Returning the same coefficients.\n",
inliers.
size ());
270 Eigen::Vector4f centroid;
280 Eigen::Vector3f eigen_values;
289template <
typename Po
intT>
void
296 PCL_ERROR (
"[pcl::SampleConsensusModelStick::projectPoints] Given model is invalid!\n");
315 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
344 using FieldList =
typename pcl::traits::fieldList<PointT>::type;
369template <
typename Po
intT>
bool
371 const std::set<index_t> &indices,
const Eigen::VectorXf &
model_coefficients,
const double threshold)
const
376 PCL_ERROR (
"[pcl::SampleConsensusModelStick::doSamplesVerifyModel] Given model is invalid!\n");
386 float sqr_threshold =
static_cast<float> (threshold * threshold);
388 for (
const auto &index : indices)
401#define PCL_INSTANTIATE_SampleConsensusModelStick(T) template class PCL_EXPORTS pcl::SampleConsensusModelStick<T>;
Define methods for centroid estimation and covariance matrix calculus.
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 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 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 stick model coefficients.
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Check whether the given index samples can form a valid stick model, compute the model coefficients fr...
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all squared distances from the cloud data to a given stick model.
typename SampleConsensusModel< PointT >::PointCloud PointCloud
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 stick 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 optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the stick coefficients using the given inlier set and return them to the user.
void computeCorrespondingEigenVector(const Matrix &mat, const typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the corresponding eigenvector to the given eigenvalue of the symmetric positive semi defin...
unsigned int computeMeanAndCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
IndicesAllocator<> Indices
Type used for indices in PCL.