41 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_REGISTRATION_H_
42 #define PCL_SAMPLE_CONSENSUS_MODEL_REGISTRATION_H_
44 #include <pcl/sample_consensus/eigen.h>
45 #include <pcl/sample_consensus/sac_model.h>
46 #include <pcl/sample_consensus/model_types.h>
47 #include <pcl/common/eigen.h>
48 #include <pcl/common/centroid.h>
57 template <
typename Po
intT>
69 typedef boost::shared_ptr<SampleConsensusModelRegistration>
Ptr;
93 const std::vector<int> &indices,
128 int target_size =
static_cast<int> (target->size ());
131 for (
int i = 0; i < target_size; ++i)
144 indices_tgt_.reset (
new std::vector<int> (indices_tgt));
154 Eigen::VectorXf &model_coefficients);
162 std::vector<double> &distances);
171 const double threshold,
172 std::vector<int> &inliers);
182 const double threshold);
191 const Eigen::VectorXf &model_coefficients,
192 Eigen::VectorXf &optimized_coefficients);
196 const Eigen::VectorXf &,
203 const Eigen::VectorXf &,
221 if (model_coefficients.size () != 16)
242 Eigen::Vector4f xyz_centroid;
243 Eigen::Matrix3f covariance_matrix = Eigen::Matrix3f::Zero ();
248 for (
int i = 0; i < 3; ++i)
249 for (
int j = 0; j < 3; ++j)
250 if (!pcl_isfinite (covariance_matrix.coeffRef (i, j)))
251 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n");
253 Eigen::Vector3f eigen_values;
259 PCL_DEBUG (
"[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n",
sample_dist_thresh_);
268 const std::vector<int> &indices)
271 Eigen::Vector4f xyz_centroid;
272 Eigen::Matrix3f covariance_matrix;
276 for (
int i = 0; i < 3; ++i)
277 for (
int j = 0; j < 3; ++j)
278 if (!pcl_isfinite (covariance_matrix.coeffRef (i, j)))
279 PCL_ERROR (
"[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n");
281 Eigen::Vector3f eigen_values;
287 PCL_DEBUG (
"[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n",
sample_dist_thresh_);
303 const std::vector<int> &indices_src,
305 const std::vector<int> &indices_tgt,
306 Eigen::VectorXf &transform);
314 for (
size_t i = 0; i <
indices_->size (); ++i)
330 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
334 #include <pcl/sample_consensus/impl/sac_model_registration.hpp>
336 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_REGISTRATION_H_
pcl::PointCloud< PointT >::Ptr PointCloudPtr
SampleConsensusModelRegistration defines a model for Point-To-Point registration outlier rejection...
bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients)
Compute a 4x4 rigid transformation matrix from the samples given.
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.
void setInputTarget(const PointCloudConstPtr &target, const std::vector< int > &indices_tgt)
Set the input point cloud target.
bool doSamplesVerifyModel(const std::set< int > &, const Eigen::VectorXf &, const double)
Verify whether a subset of indices verifies a given set of model coefficients.
SampleConsensusModel< PointT >::PointCloudPtr PointCloudPtr
SampleConsensusModelRegistration(const PointCloudConstPtr &cloud, const std::vector< int > &indices, bool random=false)
Constructor for base SampleConsensusModelRegistration.
std::map< int, int > correspondences_
Given the index in the original point cloud, give the matching original index in the target cloud...
SampleConsensusModel< PointT >::PointCloudConstPtr PointCloudConstPtr
void setInputTarget(const PointCloudConstPtr &target)
Set the input point cloud target.
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold)
Count all the points which respect the given model coefficients as inliers.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
void projectPoints(const std::vector< int > &, const Eigen::VectorXf &, PointCloud &, bool=true)
Create a new point cloud with inliers projected onto the model.
void estimateRigidTransformationSVD(const pcl::PointCloud< PointT > &cloud_src, const std::vector< int > &indices_src, const pcl::PointCloud< PointT > &cloud_tgt, const std::vector< int > &indices_tgt, Eigen::VectorXf &transform)
Estimate a rigid transformation between a source and a target point cloud using an SVD closed-form so...
virtual ~SampleConsensusModelRegistration()
Empty destructor.
void optimizeModelCoefficients(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients)
Recompute the 4x4 transformation using the given inlier set.
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, std::vector< int > &inliers)
Select all the points which respect the given model coefficients as inliers.
SampleConsensusModel< PointT >::PointCloud PointCloud
SampleConsensusModel represents the base model class.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances)
Compute all distances from the transformed points to their correspondences.
boost::shared_ptr< std::vector< int > > indices_tgt_
A pointer to the vector of target point indices to use.
boost::shared_ptr< std::vector< int > > indices_
A pointer to the vector of point indices to use.
virtual bool isSampleGood(const std::vector< int > &samples) const
Check if a sample of indices results in a good sample of points indices.
A point structure representing Euclidean xyz coordinates, and the RGB color.
boost::shared_ptr< SampleConsensusModelRegistration > Ptr
void computeOriginalIndexMapping()
Compute mappings between original indices of the input_/target_ clouds.
void computeSampleDistanceThreshold(const PointCloudConstPtr &cloud, const std::vector< int > &indices)
Computes an "optimal" sample distance threshold based on the principal directions of the input cloud...
PointCloudConstPtr target_
A boost shared pointer to the target point cloud data array.
pcl::PointCloud< PointT >::ConstPtr PointCloudConstPtr
void computeSampleDistanceThreshold(const PointCloudConstPtr &cloud)
Computes an "optimal" sample distance threshold based on the principal directions of the input cloud...
bool isModelValid(const Eigen::VectorXf &model_coefficients)
Check whether a model is valid given the user constraints.
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...
pcl::SacModel getModelType() const
Return an unique id for this model (SACMODEL_REGISTRATION).
SampleConsensusModelRegistration(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelRegistration.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.