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