Point Cloud Library (PCL)
1.11.1-dev
|
45 #include <pcl/pcl_base.h>
46 #include <pcl/sample_consensus/eigen.h>
47 #include <pcl/sample_consensus/sac_model.h>
48 #include <pcl/sample_consensus/model_types.h>
49 #include <pcl/common/eigen.h>
60 template <
typename Po
intT>
74 using Ptr = shared_ptr<SampleConsensusModelRegistration<PointT> >;
75 using ConstPtr = shared_ptr<const SampleConsensusModelRegistration<PointT>>;
135 const index_t target_size =
static_cast<index_t> (target->size ());
159 Eigen::VectorXf &model_coefficients)
const override;
167 std::vector<double> &distances)
const override;
176 const double threshold,
187 const double threshold)
const override;
196 const Eigen::VectorXf &model_coefficients,
197 Eigen::VectorXf &optimized_coefficients)
const override;
201 const Eigen::VectorXf &,
208 const Eigen::VectorXf &,
209 const double)
const override
237 Eigen::Vector4f xyz_centroid;
238 Eigen::Matrix3f covariance_matrix = Eigen::Matrix3f::Zero ();
243 for (
int i = 0; i < 3; ++i)
244 for (
int j = 0; j < 3; ++j)
245 if (!std::isfinite (covariance_matrix.coeffRef (i, j)))
246 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n");
248 Eigen::Vector3f eigen_values;
254 PCL_DEBUG (
"[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n",
sample_dist_thresh_);
267 Eigen::Vector4f xyz_centroid;
268 Eigen::Matrix3f covariance_matrix;
272 for (
int i = 0; i < 3; ++i)
273 for (
int j = 0; j < 3; ++j)
274 if (!std::isfinite (covariance_matrix.coeffRef (i, j)))
275 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n");
277 Eigen::Vector3f eigen_values;
283 PCL_DEBUG (
"[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n",
sample_dist_thresh_);
302 Eigen::VectorXf &transform)
const;
310 PCL_DEBUG (
"[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Cannot compute mapping: indices_tgt_ is null.\n");
315 PCL_DEBUG (
"[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Cannot compute mapping: indices_ is null.\n");
320 PCL_DEBUG (
"[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Cannot compute mapping: indices_ is empty.\n");
325 PCL_DEBUG (
"[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Cannot compute mapping: indices_ and indices_tgt_ are not the same size (%zu vs %zu).\n",
329 for (std::size_t i = 0; i <
indices_->size (); ++i)
331 PCL_DEBUG (
"[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Successfully computed mapping.\n");
350 #include <pcl/sample_consensus/impl/sac_model_registration.hpp>
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...
double sample_dist_thresh_
Internal distance threshold used for the sample selection step.
Defines all the PCL and non-PCL macros used.
shared_ptr< Indices > IndicesPtr
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 setInputCloud(const PointCloudConstPtr &cloud) override
Provide a pointer to the input dataset.
pcl::SacModel getModelType() const override
Return a unique id for this model (SACMODEL_REGISTRATION).
void computeSampleDistanceThreshold(const PointCloudConstPtr &cloud)
Computes an "optimal" sample distance threshold based on the principal directions of the input cloud.
unsigned int sample_size_
The size of a sample from which the model is computed.
unsigned int model_size_
The number of coefficients in the model.
PointCloud represents the base class in PCL for storing collections of 3D points.
detail::int_type_t< detail::index_type_size, detail::index_type_signed > index_t
Type used for an index in PCL.
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...
A point structure representing Euclidean xyz coordinates, and the RGB color.
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 >::PointCloudConstPtr PointCloudConstPtr
SampleConsensusModelRegistration(const PointCloudConstPtr &cloud, const Indices &indices, bool random=false)
Constructor for base SampleConsensusModelRegistration.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the transformed points to their correspondences.
void computeSampleDistanceThreshold(const PointCloudConstPtr &cloud, const Indices &indices)
Computes an "optimal" sample distance threshold based on the principal directions of the input cloud.
std::map< index_t, index_t > correspondences_
Given the index in the original point cloud, give the matching original index in the target cloud.
IndicesPtr indices_tgt_
A pointer to the vector of target point indices to use.
bool isSampleGood(const Indices &samples) const override
Check if a sample of indices results in a good sample of points indices.
IndicesPtr indices_
A pointer to the vector of point indices to use.
shared_ptr< const SampleConsensusModel< PointT > > ConstPtr
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
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.
#define PCL_MAKE_ALIGNED_OPERATOR_NEW
Macro to signal a class requires a custom allocator.
void computeOriginalIndexMapping()
Compute mappings between original indices of the input_/target_ clouds.
shared_ptr< SampleConsensusModel< PointT > > Ptr
std::string model_name_
The model name.
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.
IndicesAllocator<> Indices
Type used for indices in PCL.
typename PointCloud::ConstPtr PointCloudConstPtr
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.
bool computeModelCoefficients(const Indices &samples, Eigen::VectorXf &model_coefficients) const override
Compute a 4x4 rigid transformation matrix from the samples given.
void setInputTarget(const PointCloudConstPtr &target)
Set the input point cloud target.
PointCloudConstPtr target_
A boost shared pointer to the target point cloud data array.
typename PointCloud::Ptr PointCloudPtr
SampleConsensusModel represents the base model class.
SampleConsensusModelRegistration defines a model for Point-To-Point registration outlier rejection.
Defines functions, macros and traits for allocating and using memory.
void projectPoints(const Indices &, const Eigen::VectorXf &, PointCloud &, bool=true) const override
Create a new point cloud with inliers projected onto the model.
SampleConsensusModelRegistration(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelRegistration.
~SampleConsensusModelRegistration()
Empty destructor.
void setInputTarget(const PointCloudConstPtr &target, const Indices &indices_tgt)
Set the input point cloud target.