Point Cloud Library (PCL) 1.12.0
Loading...
Searching...
No Matches
sac_model_registration.h
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 * $Id$
38 *
39 */
40
41#pragma once
42
43#include <pcl/memory.h>
44#include <pcl/pcl_macros.h>
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>
49#include <pcl/common/centroid.h>
50#include <map>
51#include <numeric> // for std::iota
52
53namespace pcl
54{
55 /** \brief SampleConsensusModelRegistration defines a model for Point-To-Point registration outlier rejection.
56 * \author Radu Bogdan Rusu
57 * \ingroup sample_consensus
58 */
59 template <typename PointT>
61 {
62 public:
68
72
75
76 /** \brief Constructor for base SampleConsensusModelRegistration.
77 * \param[in] cloud the input point cloud dataset
78 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
79 */
81 bool random = false)
83 , target_ ()
85 {
86 // Call our own setInputCloud
87 setInputCloud (cloud);
88 model_name_ = "SampleConsensusModelRegistration";
89 sample_size_ = 3;
90 model_size_ = 16;
91 }
92
93 /** \brief Constructor for base SampleConsensusModelRegistration.
94 * \param[in] cloud the input point cloud dataset
95 * \param[in] indices a vector of point indices to be used from \a cloud
96 * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
97 */
99 const Indices &indices,
100 bool random = false)
101 : SampleConsensusModel<PointT> (cloud, indices, random)
102 , target_ ()
104 {
106 computeSampleDistanceThreshold (cloud, indices);
107 model_name_ = "SampleConsensusModelRegistration";
108 sample_size_ = 3;
109 model_size_ = 16;
110 }
111
112 /** \brief Empty destructor */
114
115 /** \brief Provide a pointer to the input dataset
116 * \param[in] cloud the const boost shared pointer to a PointCloud message
117 */
118 inline void
125
126 /** \brief Set the input point cloud target.
127 * \param[in] target the input point cloud target
128 */
129 inline void
131 {
132 target_ = target;
133 // Cache the size and fill the target indices
134 const index_t target_size = static_cast<index_t> (target->size ());
135 indices_tgt_.reset (new Indices (target_size));
136 std::iota (indices_tgt_->begin (), indices_tgt_->end (), 0);
138 }
139
140 /** \brief Set the input point cloud target.
141 * \param[in] target the input point cloud target
142 * \param[in] indices_tgt a vector of point indices to be used from \a target
143 */
144 inline void
151
152 /** \brief Compute a 4x4 rigid transformation matrix from the samples given
153 * \param[in] samples the indices found as good candidates for creating a valid model
154 * \param[out] model_coefficients the resultant model coefficients
155 */
156 bool
158 Eigen::VectorXf &model_coefficients) const override;
159
160 /** \brief Compute all distances from the transformed points to their correspondences
161 * \param[in] model_coefficients the 4x4 transformation matrix
162 * \param[out] distances the resultant estimated distances
163 */
164 void
165 getDistancesToModel (const Eigen::VectorXf &model_coefficients,
166 std::vector<double> &distances) const override;
167
168 /** \brief Select all the points which respect the given model coefficients as inliers.
169 * \param[in] model_coefficients the 4x4 transformation matrix
170 * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
171 * \param[out] inliers the resultant model inliers
172 */
173 void
174 selectWithinDistance (const Eigen::VectorXf &model_coefficients,
175 const double threshold,
176 Indices &inliers) override;
177
178 /** \brief Count all the points which respect the given model coefficients as inliers.
179 *
180 * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
181 * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
182 * \return the resultant number of inliers
183 */
184 std::size_t
185 countWithinDistance (const Eigen::VectorXf &model_coefficients,
186 const double threshold) const override;
187
188 /** \brief Recompute the 4x4 transformation using the given inlier set
189 * \param[in] inliers the data inliers found as supporting the model
190 * \param[in] model_coefficients the initial guess for the optimization
191 * \param[out] optimized_coefficients the resultant recomputed transformation
192 */
193 void
195 const Eigen::VectorXf &model_coefficients,
196 Eigen::VectorXf &optimized_coefficients) const override;
197
198 void
200 const Eigen::VectorXf &,
201 PointCloud &, bool = true) const override
202 {
203 };
204
205 bool
206 doSamplesVerifyModel (const std::set<index_t> &,
207 const Eigen::VectorXf &,
208 const double) const override
209 {
210 return (false);
211 }
212
213 /** \brief Return a unique id for this model (SACMODEL_REGISTRATION). */
214 inline pcl::SacModel
215 getModelType () const override { return (SACMODEL_REGISTRATION); }
216
217 protected:
220
221 /** \brief Check if a sample of indices results in a good sample of points
222 * indices.
223 * \param[in] samples the resultant index samples
224 */
225 bool
226 isSampleGood (const Indices &samples) const override;
227
228 /** \brief Computes an "optimal" sample distance threshold based on the
229 * principal directions of the input cloud.
230 * \param[in] cloud the const boost shared pointer to a PointCloud message
231 */
232 inline void
234 {
235 // Compute the principal directions via PCA
236 Eigen::Vector4f xyz_centroid;
237 Eigen::Matrix3f covariance_matrix = Eigen::Matrix3f::Zero ();
238
240
241 // Check if the covariance matrix is finite or not.
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");
246
247 Eigen::Vector3f eigen_values;
248 pcl::eigen33 (covariance_matrix, eigen_values);
249
250 // Compute the distance threshold for sample selection
251 sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0;
253 PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_);
254 }
255
256 /** \brief Computes an "optimal" sample distance threshold based on the
257 * principal directions of the input cloud.
258 * \param[in] cloud the const boost shared pointer to a PointCloud message
259 * \param indices
260 */
261 inline void
263 const Indices &indices)
264 {
265 // Compute the principal directions via PCA
266 Eigen::Vector4f xyz_centroid;
267 Eigen::Matrix3f covariance_matrix;
268 computeMeanAndCovarianceMatrix (*cloud, indices, covariance_matrix, xyz_centroid);
269
270 // Check if the covariance matrix is finite or not.
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");
275
276 Eigen::Vector3f eigen_values;
277 pcl::eigen33 (covariance_matrix, eigen_values);
278
279 // Compute the distance threshold for sample selection
280 sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0;
282 PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_);
283 }
284
285 /** \brief Estimate a rigid transformation between a source and a target point cloud using an SVD closed-form
286 * solution of absolute orientation using unit quaternions
287 * \param[in] cloud_src the source point cloud dataset
288 * \param[in] indices_src the vector of indices describing the points of interest in cloud_src
289 * \param[in] cloud_tgt the target point cloud dataset
290 * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from
291 * indices_src
292 * \param[out] transform the resultant transformation matrix (as model coefficients)
293 *
294 * This method is an implementation of: Horn, B. “Closed-Form Solution of Absolute Orientation Using Unit Quaternions,” JOSA A, Vol. 4, No. 4, 1987
295 */
296 void
298 const Indices &indices_src,
300 const Indices &indices_tgt,
301 Eigen::VectorXf &transform) const;
302
303 /** \brief Compute mappings between original indices of the input_/target_ clouds. */
304 void
306 {
307 if (!indices_tgt_)
308 {
309 PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Cannot compute mapping: indices_tgt_ is null.\n");
310 return;
311 }
312 if (!indices_)
313 {
314 PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Cannot compute mapping: indices_ is null.\n");
315 return;
316 }
317 if (indices_->empty ())
318 {
319 PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Cannot compute mapping: indices_ is empty.\n");
320 return;
321 }
322 if (indices_->size () != indices_tgt_->size ())
323 {
324 PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Cannot compute mapping: indices_ and indices_tgt_ are not the same size (%zu vs %zu).\n",
325 indices_->size (), indices_tgt_->size ());
326 return;
327 }
328 for (std::size_t i = 0; i < indices_->size (); ++i)
329 correspondences_[(*indices_)[i]] = (*indices_tgt_)[i];
330 PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::computeOriginalIndexMapping] Successfully computed mapping.\n");
331 }
332
333 /** \brief A boost shared pointer to the target point cloud data array. */
335
336 /** \brief A pointer to the vector of target point indices to use. */
338
339 /** \brief Given the index in the original point cloud, give the matching original index in the target cloud */
340 std::map<index_t, index_t> correspondences_;
341
342 /** \brief Internal distance threshold used for the sample selection step. */
344 public:
346 };
347}
348
349#include <pcl/sample_consensus/impl/sac_model_registration.hpp>
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.
PointCloud represents the base class in PCL for storing collections of 3D points.
SampleConsensusModel represents the base model class.
Definition sac_model.h:70
unsigned int sample_size_
The size of a sample from which the model is computed.
Definition sac_model.h:588
typename PointCloud::ConstPtr PointCloudConstPtr
Definition sac_model.h:73
IndicesPtr indices_
A pointer to the vector of point indices to use.
Definition sac_model.h:556
PointCloudConstPtr input_
A boost shared pointer to the point cloud data array.
Definition sac_model.h:553
virtual bool isModelValid(const Eigen::VectorXf &model_coefficients) const
Check whether a model is valid given the user constraints.
Definition sac_model.h:527
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition sac_model.h:300
std::string model_name_
The model name.
Definition sac_model.h:550
unsigned int model_size_
The number of coefficients in the model.
Definition sac_model.h:591
typename PointCloud::Ptr PointCloudPtr
Definition sac_model.h:74
std::vector< double > error_sqr_dists_
A vector holding the distances to the computed model.
Definition sac_model.h:585
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.
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.
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.
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.
Definition memory.h:63
unsigned int computeMeanAndCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix, Eigen::Matrix< Scalar, 4, 1 > &centroid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
Definition centroid.hpp:485
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...
Definition eigen.hpp:296
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.
Definition types.h:112
shared_ptr< Indices > IndicesPtr
Definition pcl_base.h:58
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
@ SACMODEL_REGISTRATION
Definition model_types.h:60
Defines all the PCL and non-PCL macros used.
A point structure representing Euclidean xyz coordinates, and the RGB color.