Point Cloud Library (PCL)  1.9.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
sac_model_registration.hpp
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_IMPL_SAC_MODEL_REGISTRATION_H_
42 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_H_
43 
44 #include <pcl/sample_consensus/sac_model_registration.h>
45 #include <pcl/common/point_operators.h>
46 #include <pcl/common/eigen.h>
47 #include <pcl/point_types.h>
48 
49 //////////////////////////////////////////////////////////////////////////
50 template <typename PointT> bool
51 pcl::SampleConsensusModelRegistration<PointT>::isSampleGood (const std::vector<int> &samples) const
52 {
53  using namespace pcl::common;
54  using namespace pcl::traits;
55 
56  PointT p10 = input_->points[samples[1]] - input_->points[samples[0]];
57  PointT p20 = input_->points[samples[2]] - input_->points[samples[0]];
58  PointT p21 = input_->points[samples[2]] - input_->points[samples[1]];
59 
60  return ((p10.x * p10.x + p10.y * p10.y + p10.z * p10.z) > sample_dist_thresh_ &&
61  (p20.x * p20.x + p20.y * p20.y + p20.z * p20.z) > sample_dist_thresh_ &&
62  (p21.x * p21.x + p21.y * p21.y + p21.z * p21.z) > sample_dist_thresh_);
63 }
64 
65 //////////////////////////////////////////////////////////////////////////
66 template <typename PointT> bool
67 pcl::SampleConsensusModelRegistration<PointT>::computeModelCoefficients (const std::vector<int> &samples, Eigen::VectorXf &model_coefficients) const
68 {
69  if (!target_)
70  {
71  PCL_ERROR ("[pcl::SampleConsensusModelRegistration::computeModelCoefficients] No target dataset given!\n");
72  return (false);
73  }
74  // Need 3 samples
75  if (samples.size () != 3)
76  return (false);
77 
78  std::vector<int> indices_tgt (3);
79  for (int i = 0; i < 3; ++i)
80  indices_tgt[i] = correspondences_.at (samples[i]);
81 
82  estimateRigidTransformationSVD (*input_, samples, *target_, indices_tgt, model_coefficients);
83  return (true);
84 }
85 
86 //////////////////////////////////////////////////////////////////////////
87 template <typename PointT> void
88 pcl::SampleConsensusModelRegistration<PointT>::getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) const
89 {
90  if (indices_->size () != indices_tgt_->size ())
91  {
92  PCL_ERROR ("[pcl::SampleConsensusModelRegistration::getDistancesToModel] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
93  distances.clear ();
94  return;
95  }
96  if (!target_)
97  {
98  PCL_ERROR ("[pcl::SampleConsensusModelRegistration::getDistanceToModel] No target dataset given!\n");
99  return;
100  }
101  // Check if the model is valid given the user constraints
102  if (!isModelValid (model_coefficients))
103  {
104  distances.clear ();
105  return;
106  }
107  distances.resize (indices_->size ());
108 
109  // Get the 4x4 transformation
110  Eigen::Matrix4f transform;
111  transform.row (0).matrix () = model_coefficients.segment<4>(0);
112  transform.row (1).matrix () = model_coefficients.segment<4>(4);
113  transform.row (2).matrix () = model_coefficients.segment<4>(8);
114  transform.row (3).matrix () = model_coefficients.segment<4>(12);
115 
116  for (size_t i = 0; i < indices_->size (); ++i)
117  {
118  Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
119  input_->points[(*indices_)[i]].y,
120  input_->points[(*indices_)[i]].z, 1);
121  Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x,
122  target_->points[(*indices_tgt_)[i]].y,
123  target_->points[(*indices_tgt_)[i]].z, 1);
124 
125  Eigen::Vector4f p_tr (transform * pt_src);
126  // Calculate the distance from the transformed point to its correspondence
127  // need to compute the real norm here to keep MSAC and friends general
128  distances[i] = (p_tr - pt_tgt).norm ();
129  }
130 }
131 
132 //////////////////////////////////////////////////////////////////////////
133 template <typename PointT> void
134 pcl::SampleConsensusModelRegistration<PointT>::selectWithinDistance (const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
135 {
136  if (indices_->size () != indices_tgt_->size ())
137  {
138  PCL_ERROR ("[pcl::SampleConsensusModelRegistration::selectWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
139  inliers.clear ();
140  return;
141  }
142  if (!target_)
143  {
144  PCL_ERROR ("[pcl::SampleConsensusModelRegistration::selectWithinDistance] No target dataset given!\n");
145  return;
146  }
147 
148  double thresh = threshold * threshold;
149 
150  // Check if the model is valid given the user constraints
151  if (!isModelValid (model_coefficients))
152  {
153  inliers.clear ();
154  return;
155  }
156 
157  int nr_p = 0;
158  inliers.resize (indices_->size ());
159  error_sqr_dists_.resize (indices_->size ());
160 
161  Eigen::Matrix4f transform;
162  transform.row (0).matrix () = model_coefficients.segment<4>(0);
163  transform.row (1).matrix () = model_coefficients.segment<4>(4);
164  transform.row (2).matrix () = model_coefficients.segment<4>(8);
165  transform.row (3).matrix () = model_coefficients.segment<4>(12);
166 
167  for (size_t i = 0; i < indices_->size (); ++i)
168  {
169  Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
170  input_->points[(*indices_)[i]].y,
171  input_->points[(*indices_)[i]].z, 1);
172  Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x,
173  target_->points[(*indices_tgt_)[i]].y,
174  target_->points[(*indices_tgt_)[i]].z, 1);
175 
176  Eigen::Vector4f p_tr (transform * pt_src);
177 
178  float distance = (p_tr - pt_tgt).squaredNorm ();
179  // Calculate the distance from the transformed point to its correspondence
180  if (distance < thresh)
181  {
182  inliers[nr_p] = (*indices_)[i];
183  error_sqr_dists_[nr_p] = static_cast<double> (distance);
184  ++nr_p;
185  }
186  }
187  inliers.resize (nr_p);
188  error_sqr_dists_.resize (nr_p);
189 }
190 
191 //////////////////////////////////////////////////////////////////////////
192 template <typename PointT> int
194  const Eigen::VectorXf &model_coefficients, const double threshold) const
195 {
196  if (indices_->size () != indices_tgt_->size ())
197  {
198  PCL_ERROR ("[pcl::SampleConsensusModelRegistration::countWithinDistance] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
199  return (0);
200  }
201  if (!target_)
202  {
203  PCL_ERROR ("[pcl::SampleConsensusModelRegistration::countWithinDistance] No target dataset given!\n");
204  return (0);
205  }
206 
207  double thresh = threshold * threshold;
208 
209  // Check if the model is valid given the user constraints
210  if (!isModelValid (model_coefficients))
211  return (0);
212 
213  Eigen::Matrix4f transform;
214  transform.row (0).matrix () = model_coefficients.segment<4>(0);
215  transform.row (1).matrix () = model_coefficients.segment<4>(4);
216  transform.row (2).matrix () = model_coefficients.segment<4>(8);
217  transform.row (3).matrix () = model_coefficients.segment<4>(12);
218 
219  int nr_p = 0;
220  for (size_t i = 0; i < indices_->size (); ++i)
221  {
222  Eigen::Vector4f pt_src (input_->points[(*indices_)[i]].x,
223  input_->points[(*indices_)[i]].y,
224  input_->points[(*indices_)[i]].z, 1);
225  Eigen::Vector4f pt_tgt (target_->points[(*indices_tgt_)[i]].x,
226  target_->points[(*indices_tgt_)[i]].y,
227  target_->points[(*indices_tgt_)[i]].z, 1);
228 
229  Eigen::Vector4f p_tr (transform * pt_src);
230  // Calculate the distance from the transformed point to its correspondence
231  if ((p_tr - pt_tgt).squaredNorm () < thresh)
232  nr_p++;
233  }
234  return (nr_p);
235 }
236 
237 //////////////////////////////////////////////////////////////////////////
238 template <typename PointT> void
239 pcl::SampleConsensusModelRegistration<PointT>::optimizeModelCoefficients (const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) const
240 {
241  if (indices_->size () != indices_tgt_->size ())
242  {
243  PCL_ERROR ("[pcl::SampleConsensusModelRegistration::optimizeModelCoefficients] Number of source indices (%lu) differs than number of target indices (%lu)!\n", indices_->size (), indices_tgt_->size ());
244  optimized_coefficients = model_coefficients;
245  return;
246  }
247 
248  // Check if the model is valid given the user constraints
249  if (!isModelValid (model_coefficients) || !target_)
250  {
251  optimized_coefficients = model_coefficients;
252  return;
253  }
254 
255  std::vector<int> indices_src (inliers.size ());
256  std::vector<int> indices_tgt (inliers.size ());
257  for (size_t i = 0; i < inliers.size (); ++i)
258  {
259  indices_src[i] = inliers[i];
260  indices_tgt[i] = correspondences_.at (indices_src[i]);
261  }
262 
263  estimateRigidTransformationSVD (*input_, indices_src, *target_, indices_tgt, optimized_coefficients);
264 }
265 
266 //////////////////////////////////////////////////////////////////////////
267 template <typename PointT> void
269  const pcl::PointCloud<PointT> &cloud_src,
270  const std::vector<int> &indices_src,
271  const pcl::PointCloud<PointT> &cloud_tgt,
272  const std::vector<int> &indices_tgt,
273  Eigen::VectorXf &transform) const
274 {
275  transform.resize (16);
276 
277  Eigen::Matrix<double, 3, Eigen::Dynamic> src (3, indices_src.size ());
278  Eigen::Matrix<double, 3, Eigen::Dynamic> tgt (3, indices_tgt.size ());
279 
280  for (size_t i = 0; i < indices_src.size (); ++i)
281  {
282  src (0, i) = cloud_src[indices_src[i]].x;
283  src (1, i) = cloud_src[indices_src[i]].y;
284  src (2, i) = cloud_src[indices_src[i]].z;
285 
286  tgt (0, i) = cloud_tgt[indices_tgt[i]].x;
287  tgt (1, i) = cloud_tgt[indices_tgt[i]].y;
288  tgt (2, i) = cloud_tgt[indices_tgt[i]].z;
289  }
290 
291  // Call Umeyama directly from Eigen
292  Eigen::Matrix4d transformation_matrix = pcl::umeyama (src, tgt, false);
293 
294  // Return the correct transformation
295  transform.segment<4> (0).matrix () = transformation_matrix.cast<float> ().row (0);
296  transform.segment<4> (4).matrix () = transformation_matrix.cast<float> ().row (1);
297  transform.segment<4> (8).matrix () = transformation_matrix.cast<float> ().row (2);
298  transform.segment<4> (12).matrix () = transformation_matrix.cast<float> ().row (3);
299 }
300 
301 #define PCL_INSTANTIATE_SampleConsensusModelRegistration(T) template class PCL_EXPORTS pcl::SampleConsensusModelRegistration<T>;
302 
303 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_REGISTRATION_H_
304 
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.
Eigen::internal::umeyama_transform_matrix_type< Derived, OtherDerived >::type umeyama(const Eigen::MatrixBase< Derived > &src, const Eigen::MatrixBase< OtherDerived > &dst, bool with_scaling=false)
Returns the transformation between two point sets.
Definition: eigen.hpp:739
bool computeModelCoefficients(const std::vector< int > &samples, Eigen::VectorXf &model_coefficients) const
Compute a 4x4 rigid transformation matrix from the samples given.
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 int countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const
Count all the points which respect the given model coefficients as inliers.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const
Compute all distances from the transformed points to their correspondences.