Point Cloud Library (PCL)  1.9.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
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 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_REGISTRATION_H_
42 #define PCL_SAMPLE_CONSENSUS_MODEL_REGISTRATION_H_
43 
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>
49 #include <map>
50 
51 namespace pcl
52 {
53  /** \brief SampleConsensusModelRegistration defines a model for Point-To-Point registration outlier rejection.
54  * \author Radu Bogdan Rusu
55  * \ingroup sample_consensus
56  */
57  template <typename PointT>
59  {
60  public:
66 
70 
71  typedef boost::shared_ptr<SampleConsensusModelRegistration> Ptr;
72 
73  /** \brief Constructor for base SampleConsensusModelRegistration.
74  * \param[in] cloud the input point cloud dataset
75  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
76  */
77  SampleConsensusModelRegistration (const PointCloudConstPtr &cloud,
78  bool random = false)
79  : SampleConsensusModel<PointT> (cloud, random)
80  , target_ ()
81  , indices_tgt_ ()
82  , correspondences_ ()
84  {
85  // Call our own setInputCloud
86  setInputCloud (cloud);
87  model_name_ = "SampleConsensusModelRegistration";
88  sample_size_ = 3;
89  model_size_ = 16;
90  }
91 
92  /** \brief Constructor for base SampleConsensusModelRegistration.
93  * \param[in] cloud the input point cloud dataset
94  * \param[in] indices a vector of point indices to be used from \a cloud
95  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
96  */
97  SampleConsensusModelRegistration (const PointCloudConstPtr &cloud,
98  const std::vector<int> &indices,
99  bool random = false)
100  : SampleConsensusModel<PointT> (cloud, indices, random)
101  , target_ ()
102  , indices_tgt_ ()
103  , correspondences_ ()
104  , sample_dist_thresh_ (0)
105  {
107  computeSampleDistanceThreshold (cloud, indices);
108  model_name_ = "SampleConsensusModelRegistration";
109  sample_size_ = 3;
110  model_size_ = 16;
111  }
112 
113  /** \brief Empty destructor */
115 
116  /** \brief Provide a pointer to the input dataset
117  * \param[in] cloud the const boost shared pointer to a PointCloud message
118  */
119  inline virtual void
120  setInputCloud (const PointCloudConstPtr &cloud)
121  {
125  }
126 
127  /** \brief Set the input point cloud target.
128  * \param[in] target the input point cloud target
129  */
130  inline void
131  setInputTarget (const PointCloudConstPtr &target)
132  {
133  target_ = target;
134  indices_tgt_.reset (new std::vector<int>);
135  // Cache the size and fill the target indices
136  int target_size = static_cast<int> (target->size ());
137  indices_tgt_->resize (target_size);
138 
139  for (int i = 0; i < target_size; ++i)
140  (*indices_tgt_)[i] = i;
142  }
143 
144  /** \brief Set the input point cloud target.
145  * \param[in] target the input point cloud target
146  * \param[in] indices_tgt a vector of point indices to be used from \a target
147  */
148  inline void
149  setInputTarget (const PointCloudConstPtr &target, const std::vector<int> &indices_tgt)
150  {
151  target_ = target;
152  indices_tgt_.reset (new std::vector<int> (indices_tgt));
154  }
155 
156  /** \brief Compute a 4x4 rigid transformation matrix from the samples given
157  * \param[in] samples the indices found as good candidates for creating a valid model
158  * \param[out] model_coefficients the resultant model coefficients
159  */
160  bool
161  computeModelCoefficients (const std::vector<int> &samples,
162  Eigen::VectorXf &model_coefficients) const;
163 
164  /** \brief Compute all distances from the transformed points to their correspondences
165  * \param[in] model_coefficients the 4x4 transformation matrix
166  * \param[out] distances the resultant estimated distances
167  */
168  void
169  getDistancesToModel (const Eigen::VectorXf &model_coefficients,
170  std::vector<double> &distances) const;
171 
172  /** \brief Select all the points which respect the given model coefficients as inliers.
173  * \param[in] model_coefficients the 4x4 transformation matrix
174  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
175  * \param[out] inliers the resultant model inliers
176  */
177  void
178  selectWithinDistance (const Eigen::VectorXf &model_coefficients,
179  const double threshold,
180  std::vector<int> &inliers);
181 
182  /** \brief Count all the points which respect the given model coefficients as inliers.
183  *
184  * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
185  * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
186  * \return the resultant number of inliers
187  */
188  virtual int
189  countWithinDistance (const Eigen::VectorXf &model_coefficients,
190  const double threshold) const;
191 
192  /** \brief Recompute the 4x4 transformation using the given inlier set
193  * \param[in] inliers the data inliers found as supporting the model
194  * \param[in] model_coefficients the initial guess for the optimization
195  * \param[out] optimized_coefficients the resultant recomputed transformation
196  */
197  void
198  optimizeModelCoefficients (const std::vector<int> &inliers,
199  const Eigen::VectorXf &model_coefficients,
200  Eigen::VectorXf &optimized_coefficients) const;
201 
202  void
203  projectPoints (const std::vector<int> &,
204  const Eigen::VectorXf &,
205  PointCloud &, bool = true) const
206  {
207  };
208 
209  bool
210  doSamplesVerifyModel (const std::set<int> &,
211  const Eigen::VectorXf &,
212  const double) const
213  {
214  return (false);
215  }
216 
217  /** \brief Return an unique id for this model (SACMODEL_REGISTRATION). */
218  inline pcl::SacModel
219  getModelType () const { return (SACMODEL_REGISTRATION); }
220 
221  protected:
224 
225  /** \brief Check if a sample of indices results in a good sample of points
226  * indices.
227  * \param[in] samples the resultant index samples
228  */
229  virtual bool
230  isSampleGood (const std::vector<int> &samples) const;
231 
232  /** \brief Computes an "optimal" sample distance threshold based on the
233  * principal directions of the input cloud.
234  * \param[in] cloud the const boost shared pointer to a PointCloud message
235  */
236  inline void
237  computeSampleDistanceThreshold (const PointCloudConstPtr &cloud)
238  {
239  // Compute the principal directions via PCA
240  Eigen::Vector4f xyz_centroid;
241  Eigen::Matrix3f covariance_matrix = Eigen::Matrix3f::Zero ();
242 
243  computeMeanAndCovarianceMatrix (*cloud, covariance_matrix, xyz_centroid);
244 
245  // Check if the covariance matrix is finite or not.
246  for (int i = 0; i < 3; ++i)
247  for (int j = 0; j < 3; ++j)
248  if (!pcl_isfinite (covariance_matrix.coeffRef (i, j)))
249  PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n");
250 
251  Eigen::Vector3f eigen_values;
252  pcl::eigen33 (covariance_matrix, eigen_values);
253 
254  // Compute the distance threshold for sample selection
255  sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0;
257  PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_);
258  }
259 
260  /** \brief Computes an "optimal" sample distance threshold based on the
261  * principal directions of the input cloud.
262  * \param[in] cloud the const boost shared pointer to a PointCloud message
263  * \param indices
264  */
265  inline void
266  computeSampleDistanceThreshold (const PointCloudConstPtr &cloud,
267  const std::vector<int> &indices)
268  {
269  // Compute the principal directions via PCA
270  Eigen::Vector4f xyz_centroid;
271  Eigen::Matrix3f covariance_matrix;
272  computeMeanAndCovarianceMatrix (*cloud, indices, covariance_matrix, xyz_centroid);
273 
274  // Check if the covariance matrix is finite or not.
275  for (int i = 0; i < 3; ++i)
276  for (int j = 0; j < 3; ++j)
277  if (!pcl_isfinite (covariance_matrix.coeffRef (i, j)))
278  PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeSampleDistanceThreshold] Covariance matrix has NaN values! Is the input cloud finite?\n");
279 
280  Eigen::Vector3f eigen_values;
281  pcl::eigen33 (covariance_matrix, eigen_values);
282 
283  // Compute the distance threshold for sample selection
284  sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0;
286  PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_);
287  }
288 
289  /** \brief Estimate a rigid transformation between a source and a target point cloud using an SVD closed-form
290  * solution of absolute orientation using unit quaternions
291  * \param[in] cloud_src the source point cloud dataset
292  * \param[in] indices_src the vector of indices describing the points of interest in cloud_src
293  * \param[in] cloud_tgt the target point cloud dataset
294  * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from
295  * indices_src
296  * \param[out] transform the resultant transformation matrix (as model coefficients)
297  *
298  * This method is an implementation of: Horn, B. “Closed-Form Solution of Absolute Orientation Using Unit Quaternions,” JOSA A, Vol. 4, No. 4, 1987
299  */
300  void
302  const std::vector<int> &indices_src,
303  const pcl::PointCloud<PointT> &cloud_tgt,
304  const std::vector<int> &indices_tgt,
305  Eigen::VectorXf &transform) const;
306 
307  /** \brief Compute mappings between original indices of the input_/target_ clouds. */
308  void
310  {
311  if (!indices_tgt_ || !indices_ || indices_->empty () || indices_->size () != indices_tgt_->size ())
312  return;
313  for (size_t i = 0; i < indices_->size (); ++i)
314  correspondences_[(*indices_)[i]] = (*indices_tgt_)[i];
315  }
316 
317  /** \brief A boost shared pointer to the target point cloud data array. */
318  PointCloudConstPtr target_;
319 
320  /** \brief A pointer to the vector of target point indices to use. */
321  boost::shared_ptr <std::vector<int> > indices_tgt_;
322 
323  /** \brief Given the index in the original point cloud, give the matching original index in the target cloud */
324  std::map<int, int> correspondences_;
325 
326  /** \brief Internal distance threshold used for the sample selection step. */
328  public:
329  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
330  };
331 }
332 
333 #include <pcl/sample_consensus/impl/sac_model_registration.hpp>
334 
335 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_REGISTRATION_H_
PointCloudConstPtr target_
A boost shared pointer to the target point cloud data array.
boost::shared_ptr< SampleConsensusModelRegistration > Ptr
std::map< int, int > correspondences_
Given the index in the original point cloud, give the matching original index in the target cloud...
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.
virtual ~SampleConsensusModelRegistration()
Empty destructor.
SampleConsensusModelRegistration defines a model for Point-To-Point registration outlier rejection...
void projectPoints(const std::vector< int > &, const Eigen::VectorXf &, PointCloud &, bool=true) const
Create a new point cloud with inliers projected onto the model.
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:489
This file defines compatibility wrappers for low level I/O functions.
Definition: convolution.h:45
pcl::SacModel getModelType() const
Return an unique id for this model (SACMODEL_REGISTRATION).
unsigned int model_size_
The number of coefficients in the model.
Definition: sac_model.h:575
void setInputTarget(const PointCloudConstPtr &target, const std::vector< int > &indices_tgt)
Set the input point cloud target.
bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients) const
Compute a 4x4 rigid transformation matrix from the samples given.
boost::shared_ptr< std::vector< int > > indices_
A pointer to the vector of point indices to use.
Definition: sac_model.h:540
SampleConsensusModelRegistration(const PointCloudConstPtr &cloud, const std::vector< int > &indices, bool random=false)
Constructor for base SampleConsensusModelRegistration.
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) const
Estimate a rigid transformation between a source and a target point cloud using an SVD closed-form so...
void optimizeModelCoefficients(const std::vector< int > &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
Recompute the 4x4 transformation using the given inlier set.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
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...
virtual int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const
Count all the points which respect the given model coefficients as inliers.
SampleConsensusModel represents the base model class.
Definition: sac_model.h:66
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: sac_model.h:304
std::string model_name_
The model name.
Definition: sac_model.h:534
pcl::PointCloud< PointT >::Ptr PointCloudPtr
Definition: sac_model.h:71
SampleConsensusModel< PointT >::PointCloud PointCloud
void computeOriginalIndexMapping()
Compute mappings between original indices of the input_/target_ clouds.
SacModel
Definition: model_types.h:46
bool doSamplesVerifyModel(const std::set< int > &, const Eigen::VectorXf &, const double) const
Verify whether a subset of indices verifies a given set of model coefficients.
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:251
pcl::PointCloud< PointT >::ConstPtr PointCloudConstPtr
Definition: sac_model.h:70
PointCloud represents the base class in PCL for storing collections of 3D points. ...
SampleConsensusModel< PointT >::PointCloudPtr PointCloudPtr
virtual bool isSampleGood(const std::vector< int > &samples) const
Check if a sample of indices results in a good sample of points indices.
void setInputTarget(const PointCloudConstPtr &target)
Set the input point cloud target.
void computeSampleDistanceThreshold(const PointCloudConstPtr &cloud)
Computes an "optimal" sample distance threshold based on the principal directions of the input cloud...
A point structure representing Euclidean xyz coordinates, and the RGB color.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const
Compute all distances from the transformed points to their correspondences.
SampleConsensusModel< PointT >::PointCloudConstPtr PointCloudConstPtr
boost::shared_ptr< std::vector< int > > indices_tgt_
A pointer to the vector of target point indices to use.
SampleConsensusModelRegistration(const PointCloudConstPtr &cloud, bool random=false)
Constructor for base SampleConsensusModelRegistration.
unsigned int sample_size_
The size of a sample from which the model is computed.
Definition: sac_model.h:572
double sample_dist_thresh_
Internal distance threshold used for the sample selection step.