Point Cloud Library (PCL)  1.10.0-dev
ia_ransac.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, 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 IA_RANSAC_HPP_
42 #define IA_RANSAC_HPP_
43 
44 #include <pcl/common/distances.h>
45 
46 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
47 template <typename PointSource, typename PointTarget, typename FeatureT> void
49 {
50  if (features == nullptr || features->empty ())
51  {
52  PCL_ERROR ("[pcl::%s::setSourceFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
53  return;
54  }
55  input_features_ = features;
56 }
57 
58 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
59 template <typename PointSource, typename PointTarget, typename FeatureT> void
61 {
62  if (features == nullptr || features->empty ())
63  {
64  PCL_ERROR ("[pcl::%s::setTargetFeatures] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
65  return;
66  }
67  target_features_ = features;
68  feature_tree_->setInputCloud (target_features_);
69 }
70 
71 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
72 template <typename PointSource, typename PointTarget, typename FeatureT> void
74  const PointCloudSource &cloud, int nr_samples, float min_sample_distance,
75  std::vector<int> &sample_indices)
76 {
77  if (nr_samples > static_cast<int> (cloud.points.size ()))
78  {
79  PCL_ERROR ("[pcl::%s::selectSamples] ", getClassName ().c_str ());
80  PCL_ERROR ("The number of samples (%d) must not be greater than the number of points (%lu)!\n",
81  nr_samples, cloud.points.size ());
82  return;
83  }
84 
85  // Iteratively draw random samples until nr_samples is reached
86  int iterations_without_a_sample = 0;
87  int max_iterations_without_a_sample = static_cast<int> (3 * cloud.points.size ());
88  sample_indices.clear ();
89  while (static_cast<int> (sample_indices.size ()) < nr_samples)
90  {
91  // Choose a sample at random
92  int sample_index = getRandomIndex (static_cast<int> (cloud.points.size ()));
93 
94  // Check to see if the sample is 1) unique and 2) far away from the other samples
95  bool valid_sample = true;
96  for (const int &sample_idx : sample_indices)
97  {
98  float distance_between_samples = euclideanDistance (cloud.points[sample_index], cloud.points[sample_idx]);
99 
100  if (sample_index == sample_idx || distance_between_samples < min_sample_distance)
101  {
102  valid_sample = false;
103  break;
104  }
105  }
106 
107  // If the sample is valid, add it to the output
108  if (valid_sample)
109  {
110  sample_indices.push_back (sample_index);
111  iterations_without_a_sample = 0;
112  }
113  else
114  ++iterations_without_a_sample;
115 
116  // If no valid samples can be found, relax the inter-sample distance requirements
117  if (iterations_without_a_sample >= max_iterations_without_a_sample)
118  {
119  PCL_WARN ("[pcl::%s::selectSamples] ", getClassName ().c_str ());
120  PCL_WARN ("No valid sample found after %d iterations. Relaxing min_sample_distance_ to %f\n",
121  iterations_without_a_sample, 0.5*min_sample_distance);
122 
123  min_sample_distance_ *= 0.5f;
124  min_sample_distance = min_sample_distance_;
125  iterations_without_a_sample = 0;
126  }
127  }
128 }
129 
130 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
131 template <typename PointSource, typename PointTarget, typename FeatureT> void
133  const FeatureCloud &input_features, const std::vector<int> &sample_indices,
134  std::vector<int> &corresponding_indices)
135 {
136  std::vector<int> nn_indices (k_correspondences_);
137  std::vector<float> nn_distances (k_correspondences_);
138 
139  corresponding_indices.resize (sample_indices.size ());
140  for (std::size_t i = 0; i < sample_indices.size (); ++i)
141  {
142  // Find the k features nearest to input_features.points[sample_indices[i]]
143  feature_tree_->nearestKSearch (input_features, sample_indices[i], k_correspondences_, nn_indices, nn_distances);
144 
145  // Select one at random and add it to corresponding_indices
146  int random_correspondence = getRandomIndex (k_correspondences_);
147  corresponding_indices[i] = nn_indices[random_correspondence];
148  }
149 }
150 
151 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
152 template <typename PointSource, typename PointTarget, typename FeatureT> float
154  const PointCloudSource &cloud, float)
155 {
156  std::vector<int> nn_index (1);
157  std::vector<float> nn_distance (1);
158 
159  const ErrorFunctor & compute_error = *error_functor_;
160  float error = 0;
161 
162  for (int i = 0; i < static_cast<int> (cloud.points.size ()); ++i)
163  {
164  // Find the distance between cloud.points[i] and its nearest neighbor in the target point cloud
165  tree_->nearestKSearch (cloud, i, 1, nn_index, nn_distance);
166 
167  // Compute the error
168  error += compute_error (nn_distance[0]);
169  }
170  return (error);
171 }
172 
173 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
174 template <typename PointSource, typename PointTarget, typename FeatureT> void
176 {
177  // Some sanity checks first
178  if (!input_features_)
179  {
180  PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
181  PCL_ERROR ("No source features were given! Call setSourceFeatures before aligning.\n");
182  return;
183  }
184  if (!target_features_)
185  {
186  PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
187  PCL_ERROR ("No target features were given! Call setTargetFeatures before aligning.\n");
188  return;
189  }
190 
191  if (input_->size () != input_features_->size ())
192  {
193  PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
194  PCL_ERROR ("The source points and source feature points need to be in a one-to-one relationship! Current input cloud sizes: %ld vs %ld.\n",
195  input_->size (), input_features_->size ());
196  return;
197  }
198 
199  if (target_->size () != target_features_->size ())
200  {
201  PCL_ERROR ("[pcl::%s::computeTransformation] ", getClassName ().c_str ());
202  PCL_ERROR ("The target points and target feature points need to be in a one-to-one relationship! Current input cloud sizes: %ld vs %ld.\n",
203  target_->size (), target_features_->size ());
204  return;
205  }
206 
207  if (!error_functor_)
208  error_functor_.reset (new TruncatedError (static_cast<float> (corr_dist_threshold_)));
209 
210 
211  std::vector<int> sample_indices (nr_samples_);
212  std::vector<int> corresponding_indices (nr_samples_);
213  PointCloudSource input_transformed;
214  float error, lowest_error (0);
215 
216  final_transformation_ = guess;
217  int i_iter = 0;
218  converged_ = false;
219  if (!guess.isApprox (Eigen::Matrix4f::Identity (), 0.01f))
220  {
221  // If guess is not the Identity matrix we check it.
222  transformPointCloud (*input_, input_transformed, final_transformation_);
223  lowest_error = computeErrorMetric (input_transformed, static_cast<float> (corr_dist_threshold_));
224  i_iter = 1;
225  }
226 
227  for (; i_iter < max_iterations_; ++i_iter)
228  {
229  // Draw nr_samples_ random samples
230  selectSamples (*input_, nr_samples_, min_sample_distance_, sample_indices);
231 
232  // Find corresponding features in the target cloud
233  findSimilarFeatures (*input_features_, sample_indices, corresponding_indices);
234 
235  // Estimate the transform from the samples to their corresponding points
236  transformation_estimation_->estimateRigidTransformation (*input_, sample_indices, *target_, corresponding_indices, transformation_);
237 
238  // Transform the data and compute the error
239  transformPointCloud (*input_, input_transformed, transformation_);
240  error = computeErrorMetric (input_transformed, static_cast<float> (corr_dist_threshold_));
241 
242  // If the new error is lower, update the final transformation
243  if (i_iter == 0 || error < lowest_error)
244  {
245  lowest_error = error;
246  final_transformation_ = transformation_;
247  converged_=true;
248  }
249  }
250 
251  // Apply the final transformation
252  transformPointCloud (*input_, output, final_transformation_);
253 }
254 
255 #endif //#ifndef IA_RANSAC_HPP_
256 
Define standard C methods to do distance calculations.
void setTargetFeatures(const FeatureCloudConstPtr &features)
Provide a shared pointer to the target point cloud&#39;s feature descriptors.
Definition: ia_ransac.hpp:60
float euclideanDistance(const PointType1 &p1, const PointType2 &p2)
Calculate the euclidean distance between the two given points.
Definition: distances.h:200
void setSourceFeatures(const FeatureCloudConstPtr &features)
Provide a shared pointer to the source point cloud&#39;s feature descriptors.
Definition: ia_ransac.hpp:48
float computeErrorMetric(const PointCloudSource &cloud, float threshold)
An error metric for that computes the quality of the alignment between the given cloud and the target...
Definition: ia_ransac.hpp:153
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields=true)
Apply an affine transform defined by an Eigen Transform.
Definition: transforms.hpp:215
void selectSamples(const PointCloudSource &cloud, int nr_samples, float min_sample_distance, std::vector< int > &sample_indices)
Select nr_samples sample points from cloud while making sure that their pairwise distances are greate...
Definition: ia_ransac.hpp:73
typename Registration< PointSource, PointTarget >::PointCloudSource PointCloudSource
Definition: ia_ransac.h:72
void findSimilarFeatures(const FeatureCloud &input_features, const std::vector< int > &sample_indices, std::vector< int > &corresponding_indices)
For each of the sample points, find a list of points in the target cloud whose features are similar t...
Definition: ia_ransac.hpp:132
void computeTransformation(PointCloudSource &output, const Eigen::Matrix4f &guess) override
Rigid transformation computation method.
Definition: ia_ransac.hpp:175
typename FeatureCloud::ConstPtr FeatureCloudConstPtr
Definition: ia_ransac.h:83