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