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