Point Cloud Library (PCL)  1.7.1
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:
64 
68 
69  typedef boost::shared_ptr<SampleConsensusModelRegistration> Ptr;
70 
71  /** \brief Constructor for base SampleConsensusModelRegistration.
72  * \param[in] cloud the input point cloud dataset
73  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
74  */
76  bool random = false)
77  : SampleConsensusModel<PointT> (cloud, random)
78  , target_ ()
79  , indices_tgt_ ()
80  , correspondences_ ()
82  {
83  // Call our own setInputCloud
84  setInputCloud (cloud);
85  }
86 
87  /** \brief Constructor for base SampleConsensusModelRegistration.
88  * \param[in] cloud the input point cloud dataset
89  * \param[in] indices a vector of point indices to be used from \a cloud
90  * \param[in] random if true set the random seed to the current time, else set to 12345 (default: false)
91  */
93  const std::vector<int> &indices,
94  bool random = false)
95  : SampleConsensusModel<PointT> (cloud, indices, random)
96  , target_ ()
97  , indices_tgt_ ()
98  , correspondences_ ()
100  {
102  computeSampleDistanceThreshold (cloud, indices);
103  }
104 
105  /** \brief Empty destructor */
107 
108  /** \brief Provide a pointer to the input dataset
109  * \param[in] cloud the const boost shared pointer to a PointCloud message
110  */
111  inline virtual void
113  {
117  }
118 
119  /** \brief Set the input point cloud target.
120  * \param[in] target the input point cloud target
121  */
122  inline void
124  {
125  target_ = target;
126  indices_tgt_.reset (new std::vector<int>);
127  // Cache the size and fill the target indices
128  int target_size = static_cast<int> (target->size ());
129  indices_tgt_->resize (target_size);
130 
131  for (int i = 0; i < target_size; ++i)
132  (*indices_tgt_)[i] = i;
134  }
135 
136  /** \brief Set the input point cloud target.
137  * \param[in] target the input point cloud target
138  * \param[in] indices_tgt a vector of point indices to be used from \a target
139  */
140  inline void
141  setInputTarget (const PointCloudConstPtr &target, const std::vector<int> &indices_tgt)
142  {
143  target_ = target;
144  indices_tgt_.reset (new std::vector<int> (indices_tgt));
146  }
147 
148  /** \brief Compute a 4x4 rigid transformation matrix from the samples given
149  * \param[in] samples the indices found as good candidates for creating a valid model
150  * \param[out] model_coefficients the resultant model coefficients
151  */
152  bool
153  computeModelCoefficients (const std::vector<int> &samples,
154  Eigen::VectorXf &model_coefficients);
155 
156  /** \brief Compute all distances from the transformed points to their correspondences
157  * \param[in] model_coefficients the 4x4 transformation matrix
158  * \param[out] distances the resultant estimated distances
159  */
160  void
161  getDistancesToModel (const Eigen::VectorXf &model_coefficients,
162  std::vector<double> &distances);
163 
164  /** \brief Select all the points which respect the given model coefficients as inliers.
165  * \param[in] model_coefficients the 4x4 transformation matrix
166  * \param[in] threshold a maximum admissible distance threshold for determining the inliers from the outliers
167  * \param[out] inliers the resultant model inliers
168  */
169  void
170  selectWithinDistance (const Eigen::VectorXf &model_coefficients,
171  const double threshold,
172  std::vector<int> &inliers);
173 
174  /** \brief Count all the points which respect the given model coefficients as inliers.
175  *
176  * \param[in] model_coefficients the coefficients of a model that we need to compute distances to
177  * \param[in] threshold maximum admissible distance threshold for determining the inliers from the outliers
178  * \return the resultant number of inliers
179  */
180  virtual int
181  countWithinDistance (const Eigen::VectorXf &model_coefficients,
182  const double threshold);
183 
184  /** \brief Recompute the 4x4 transformation using the given inlier set
185  * \param[in] inliers the data inliers found as supporting the model
186  * \param[in] model_coefficients the initial guess for the optimization
187  * \param[out] optimized_coefficients the resultant recomputed transformation
188  */
189  void
190  optimizeModelCoefficients (const std::vector<int> &inliers,
191  const Eigen::VectorXf &model_coefficients,
192  Eigen::VectorXf &optimized_coefficients);
193 
194  void
195  projectPoints (const std::vector<int> &,
196  const Eigen::VectorXf &,
197  PointCloud &, bool = true)
198  {
199  };
200 
201  bool
202  doSamplesVerifyModel (const std::set<int> &,
203  const Eigen::VectorXf &,
204  const double)
205  {
206  return (false);
207  }
208 
209  /** \brief Return an unique id for this model (SACMODEL_REGISTRATION). */
210  inline pcl::SacModel
211  getModelType () const { return (SACMODEL_REGISTRATION); }
212 
213  protected:
214  /** \brief Check whether a model is valid given the user constraints.
215  * \param[in] model_coefficients the set of model coefficients
216  */
217  inline bool
218  isModelValid (const Eigen::VectorXf &model_coefficients)
219  {
220  // Needs a valid model coefficients
221  if (model_coefficients.size () != 16)
222  return (false);
223 
224  return (true);
225  }
226 
227  /** \brief Check if a sample of indices results in a good sample of points
228  * indices.
229  * \param[in] samples the resultant index samples
230  */
231  virtual bool
232  isSampleGood (const std::vector<int> &samples) const;
233 
234  /** \brief Computes an "optimal" sample distance threshold based on the
235  * principal directions of the input cloud.
236  * \param[in] cloud the const boost shared pointer to a PointCloud message
237  */
238  inline void
240  {
241  // Compute the principal directions via PCA
242  Eigen::Vector4f xyz_centroid;
243  Eigen::Matrix3f covariance_matrix = Eigen::Matrix3f::Zero ();
244 
245  computeMeanAndCovarianceMatrix (*cloud, covariance_matrix, xyz_centroid);
246 
247  // Check if the covariance matrix is finite or not.
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");
252 
253  Eigen::Vector3f eigen_values;
254  pcl::eigen33 (covariance_matrix, eigen_values);
255 
256  // Compute the distance threshold for sample selection
257  sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0;
259  PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_);
260  }
261 
262  /** \brief Computes an "optimal" sample distance threshold based on the
263  * principal directions of the input cloud.
264  * \param[in] cloud the const boost shared pointer to a PointCloud message
265  */
266  inline void
268  const std::vector<int> &indices)
269  {
270  // Compute the principal directions via PCA
271  Eigen::Vector4f xyz_centroid;
272  Eigen::Matrix3f covariance_matrix;
273  computeMeanAndCovarianceMatrix (*cloud, indices, covariance_matrix, xyz_centroid);
274 
275  // Check if the covariance matrix is finite or not.
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");
280 
281  Eigen::Vector3f eigen_values;
282  pcl::eigen33 (covariance_matrix, eigen_values);
283 
284  // Compute the distance threshold for sample selection
285  sample_dist_thresh_ = eigen_values.array ().sqrt ().sum () / 3.0;
287  PCL_DEBUG ("[pcl::SampleConsensusModelRegistration::setInputCloud] Estimated a sample selection distance threshold of: %f\n", sample_dist_thresh_);
288  }
289 
290  /** \brief Estimate a rigid transformation between a source and a target point cloud using an SVD closed-form
291  * solution of absolute orientation using unit quaternions
292  * \param[in] cloud_src the source point cloud dataset
293  * \param[in] indices_src the vector of indices describing the points of interest in cloud_src
294  * \param[in] cloud_tgt the target point cloud dataset
295  * \param[in] indices_tgt the vector of indices describing the correspondences of the interest points from
296  * indices_src
297  * \param[out] transform the resultant transformation matrix (as model coefficients)
298  *
299  * This method is an implementation of: Horn, B. “Closed-Form Solution of Absolute Orientation Using Unit Quaternions,” JOSA A, Vol. 4, No. 4, 1987
300  */
301  void
303  const std::vector<int> &indices_src,
304  const pcl::PointCloud<PointT> &cloud_tgt,
305  const std::vector<int> &indices_tgt,
306  Eigen::VectorXf &transform);
307 
308  /** \brief Compute mappings between original indices of the input_/target_ clouds. */
309  void
311  {
312  if (!indices_tgt_ || !indices_ || indices_->empty () || indices_->size () != indices_tgt_->size ())
313  return;
314  for (size_t i = 0; i < indices_->size (); ++i)
315  correspondences_[(*indices_)[i]] = (*indices_tgt_)[i];
316  }
317 
318  /** \brief A boost shared pointer to the target point cloud data array. */
320 
321  /** \brief A pointer to the vector of target point indices to use. */
322  boost::shared_ptr <std::vector<int> > indices_tgt_;
323 
324  /** \brief Given the index in the original point cloud, give the matching original index in the target cloud */
325  std::map<int, int> correspondences_;
326 
327  /** \brief Internal distance threshold used for the sample selection step. */
329  public:
330  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
331  };
332 }
333 
334 #include <pcl/sample_consensus/impl/sac_model_registration.hpp>
335 
336 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_REGISTRATION_H_