45#include <pcl/pcl_base.h>
46#include <pcl/sample_consensus/sac_model.h>
47#include <pcl/sample_consensus/model_types.h>
48#include <pcl/common/eigen.h>
59 template <
typename Po
intT>
73 using Ptr = shared_ptr<SampleConsensusModelRegistration<PointT> >;
74 using ConstPtr = shared_ptr<const SampleConsensusModelRegistration<PointT>>;
134 const index_t target_size =
static_cast<index_t> (target->size ());
158 Eigen::VectorXf &model_coefficients)
const override;
166 std::vector<double> &distances)
const override;
175 const double threshold,
186 const double threshold)
const override;
195 const Eigen::VectorXf &model_coefficients,
196 Eigen::VectorXf &optimized_coefficients)
const override;
200 const Eigen::VectorXf &,
207 const Eigen::VectorXf &,
208 const double)
const override
236 Eigen::Vector4f xyz_centroid;
237 Eigen::Matrix3f covariance_matrix = Eigen::Matrix3f::Zero ();
242 for (
int i = 0; i < 3; ++i)
243 for (
int j = 0; j < 3; ++j)
244 if (!std::isfinite (covariance_matrix.coeffRef (i, j)))
245 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n");
247 Eigen::Vector3f eigen_values;
253 PCL_DEBUG (
"[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n",
sample_dist_thresh_);
266 Eigen::Vector4f xyz_centroid;
267 Eigen::Matrix3f covariance_matrix;
271 for (
int i = 0; i < 3; ++i)
272 for (
int j = 0; j < 3; ++j)
273 if (!std::isfinite (covariance_matrix.coeffRef (i, j)))
274 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n");
276 Eigen::Vector3f eigen_values;
282 PCL_DEBUG (
"[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n",
sample_dist_thresh_);
301 Eigen::VectorXf &transform)
const;
309 PCL_DEBUG (
"[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Cannot compute mapping: indices_tgt_ is null.\n");
314 PCL_DEBUG (
"[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Cannot compute mapping: indices_ is null.\n");
319 PCL_DEBUG (
"[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Cannot compute mapping: indices_ is empty.\n");
324 PCL_DEBUG (
"[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Cannot compute mapping: indices_ and indices_tgt_ are not the same size (%zu vs %zu).\n",
328 for (std::size_t i = 0; i <
indices_->size (); ++i)
330 PCL_DEBUG (
"[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Successfully computed mapping.\n");
349#include <pcl/sample_consensus/impl/sac_model_registration.hpp>
Define methods for centroid estimation and covariance matrix calculus.
PointCloud represents the base class in PCL for storing collections of 3D points.
SampleConsensusModel represents the base model class.
unsigned int sample_size_
The size of a sample from which the model is computed.
typename PointCloud::ConstPtr PointCloudConstPtr
IndicesPtr indices_
A pointer to the vector of point indices to use.
PointCloudConstPtr input_
A boost shared pointer to the point cloud data array.
virtual bool isModelValid(const Eigen::VectorXf &model_coefficients) const
Check whether a model is valid given the user constraints.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
std::string model_name_
The model name.
unsigned int model_size_
The number of coefficients in the model.
typename PointCloud::Ptr PointCloudPtr
std::vector< double > error_sqr_dists_
A vector holding the distances to the computed model.
SampleConsensusModelRegistration defines a model for Point-To-Point registration outlier rejection.
std::map< index_t, index_t > correspondences_
Given the index in the original point cloud, give the matching original index in the target cloud.
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.
SampleConsensusModelRegistration(const PointCloudConstPtr &cloud, const Indices &indices, bool random=false)
Constructor for base SampleConsensusModelRegistration.
IndicesPtr indices_tgt_
A pointer to the vector of target point indices to use.
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Compute a 4x4 rigid transformation matrix from the samples given.
typename SampleConsensusModel< PointT >::PointCloudConstPtr PointCloudConstPtr
PointCloudConstPtr target_
A boost shared pointer to the target point cloud data array.
pcl::SacModel getModelType() const override
Return a unique id for this model (SACMODEL_REGISTRATION).
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the transformed points to their correspondences.
void projectPoints(const Indices &, const Eigen::VectorXf &, PointCloud &, bool=true) const override
Create a new point cloud with inliers projected onto the model.
void setInputTarget(const PointCloudConstPtr &target, const Indices &indices_tgt)
Set the input point cloud target.
void setInputTarget(const PointCloudConstPtr &target)
Set the input point cloud target.
SampleConsensusModelRegistration(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelRegistration.
bool doSamplesVerifyModel(const std::set< index_t > &, const Eigen::VectorXf &, const double) const override
Verify whether a subset of indices verifies a given set of model coefficients.
typename SampleConsensusModel< PointT >::PointCloudPtr PointCloudPtr
void estimateRigidTransformationSVD(const pcl::PointCloud< PointT > &cloud_src, const Indices &indices_src, const pcl::PointCloud< PointT > &cloud_tgt, const Indices &indices_tgt, Eigen::VectorXf &transform) const
Estimate a rigid transformation between a source and a target point cloud using an SVD closed-form so...
void computeSampleDistanceThreshold(const PointCloudConstPtr &cloud, const Indices &indices)
Computes an "optimal" sample distance threshold based on the principal directions of the input cloud.
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
void optimizeModelCoefficients(const Indices &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const override
Recompute the 4x4 transformation using the given inlier set.
shared_ptr< SampleConsensusModelRegistration< PointT > > Ptr
double sample_dist_thresh_
Internal distance threshold used for the sample selection step.
void computeSampleDistanceThreshold(const PointCloudConstPtr &cloud)
Computes an "optimal" sample distance threshold based on the principal directions of the input cloud.
shared_ptr< const SampleConsensusModelRegistration< PointT > > ConstPtr
void setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
typename SampleConsensusModel< PointT >::PointCloud PointCloud
void computeOriginalIndexMapping()
Compute mappings between original indices of the input_/target_ clouds.
~SampleConsensusModelRegistration()
Empty destructor.
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.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
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...
Defines functions, macros and traits for allocating and using memory.
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
shared_ptr< Indices > IndicesPtr
IndicesAllocator<> Indices
Type used for indices in PCL.
Defines all the PCL and non-PCL macros used.
A point structure representing Euclidean xyz coordinates, and the RGB color.