Point Cloud Library (PCL)  1.12.1-dev
correspondence_estimation_backprojection.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2012-, Open Perception, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  *
38  */
39 
40 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_
41 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_
42 
43 #include <pcl/common/copy_point.h>
44 
45 namespace pcl {
46 
47 namespace registration {
48 
49 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
50 bool
53 {
54  if (!source_normals_ || !target_normals_) {
55  PCL_WARN("[pcl::registration::%s::initCompute] Datasets containing normals for "
56  "source/target have not been given!\n",
57  getClassName().c_str());
58  return (false);
59  }
60 
61  return (
63 }
64 
65 ///////////////////////////////////////////////////////////////////////////////////////////
66 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
67 void
69  determineCorrespondences(pcl::Correspondences& correspondences, double max_distance)
70 {
71  if (!initCompute())
72  return;
73 
74  correspondences.resize(indices_->size());
75 
76  pcl::Indices nn_indices(k_);
77  std::vector<float> nn_dists(k_);
78 
79  int min_index = 0;
80 
82  unsigned int nr_valid_correspondences = 0;
83 
84  // Check if the template types are the same. If true, avoid a copy.
85  // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT
86  // macro!
87  if (isSamePointType<PointSource, PointTarget>()) {
88  PointTarget pt;
89  // Iterate over the input set of source indices
90  for (const auto& idx_i : (*indices_)) {
91  tree_->nearestKSearch((*input_)[idx_i], k_, nn_indices, nn_dists);
92 
93  // Among the K nearest neighbours find the one with minimum perpendicular distance
94  // to the normal
95  float min_dist = std::numeric_limits<float>::max();
96 
97  // Find the best correspondence
98  for (std::size_t j = 0; j < nn_indices.size(); j++) {
99  float cos_angle = (*source_normals_)[idx_i].normal_x *
100  (*target_normals_)[nn_indices[j]].normal_x +
101  (*source_normals_)[idx_i].normal_y *
102  (*target_normals_)[nn_indices[j]].normal_y +
103  (*source_normals_)[idx_i].normal_z *
104  (*target_normals_)[nn_indices[j]].normal_z;
105  float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
106 
107  if (dist < min_dist) {
108  min_dist = dist;
109  min_index = static_cast<int>(j);
110  }
111  }
112  if (min_dist > max_distance)
113  continue;
114 
115  corr.index_query = idx_i;
116  corr.index_match = nn_indices[min_index];
117  corr.distance = nn_dists[min_index]; // min_dist;
118  correspondences[nr_valid_correspondences++] = corr;
119  }
120  }
121  else {
122  PointTarget pt;
123 
124  // Iterate over the input set of source indices
125  for (const auto& idx_i : (*indices_)) {
126  tree_->nearestKSearch((*input_)[idx_i], k_, nn_indices, nn_dists);
127 
128  // Among the K nearest neighbours find the one with minimum perpendicular distance
129  // to the normal
130  float min_dist = std::numeric_limits<float>::max();
131 
132  // Find the best correspondence
133  for (std::size_t j = 0; j < nn_indices.size(); j++) {
134  PointSource pt_src;
135  // Copy the source data to a target PointTarget format so we can search in the
136  // tree
137  copyPoint((*input_)[idx_i], pt_src);
138 
139  float cos_angle = (*source_normals_)[idx_i].normal_x *
140  (*target_normals_)[nn_indices[j]].normal_x +
141  (*source_normals_)[idx_i].normal_y *
142  (*target_normals_)[nn_indices[j]].normal_y +
143  (*source_normals_)[idx_i].normal_z *
144  (*target_normals_)[nn_indices[j]].normal_z;
145  float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
146 
147  if (dist < min_dist) {
148  min_dist = dist;
149  min_index = static_cast<int>(j);
150  }
151  }
152  if (min_dist > max_distance)
153  continue;
154 
155  corr.index_query = idx_i;
156  corr.index_match = nn_indices[min_index];
157  corr.distance = nn_dists[min_index]; // min_dist;
158  correspondences[nr_valid_correspondences++] = corr;
159  }
160  }
161  correspondences.resize(nr_valid_correspondences);
162  deinitCompute();
163 }
164 
165 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
166 void
169  double max_distance)
170 {
171  if (!initCompute())
172  return;
173 
174  // Set the internal point representation of choice
175  if (!initComputeReciprocal())
176  return;
177 
178  correspondences.resize(indices_->size());
179 
180  pcl::Indices nn_indices(k_);
181  std::vector<float> nn_dists(k_);
182  pcl::Indices index_reciprocal(1);
183  std::vector<float> distance_reciprocal(1);
184 
185  int min_index = 0;
186 
187  pcl::Correspondence corr;
188  unsigned int nr_valid_correspondences = 0;
189  int target_idx = 0;
190 
191  // Check if the template types are the same. If true, avoid a copy.
192  // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT
193  // macro!
194  if (isSamePointType<PointSource, PointTarget>()) {
195  PointTarget pt;
196  // Iterate over the input set of source indices
197  for (const auto& idx_i : (*indices_)) {
198  tree_->nearestKSearch((*input_)[idx_i], k_, nn_indices, nn_dists);
199 
200  // Among the K nearest neighbours find the one with minimum perpendicular distance
201  // to the normal
202  float min_dist = std::numeric_limits<float>::max();
203 
204  // Find the best correspondence
205  for (std::size_t j = 0; j < nn_indices.size(); j++) {
206  float cos_angle = (*source_normals_)[idx_i].normal_x *
207  (*target_normals_)[nn_indices[j]].normal_x +
208  (*source_normals_)[idx_i].normal_y *
209  (*target_normals_)[nn_indices[j]].normal_y +
210  (*source_normals_)[idx_i].normal_z *
211  (*target_normals_)[nn_indices[j]].normal_z;
212  float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
213 
214  if (dist < min_dist) {
215  min_dist = dist;
216  min_index = static_cast<int>(j);
217  }
218  }
219  if (min_dist > max_distance)
220  continue;
221 
222  // Check if the correspondence is reciprocal
223  target_idx = nn_indices[min_index];
224  tree_reciprocal_->nearestKSearch(
225  (*target_)[target_idx], 1, index_reciprocal, distance_reciprocal);
226 
227  if (idx_i != index_reciprocal[0])
228  continue;
229 
230  corr.index_query = idx_i;
231  corr.index_match = nn_indices[min_index];
232  corr.distance = nn_dists[min_index]; // min_dist;
233  correspondences[nr_valid_correspondences++] = corr;
234  }
235  }
236  else {
237  PointTarget pt;
238 
239  // Iterate over the input set of source indices
240  for (const auto& idx_i : (*indices_)) {
241  tree_->nearestKSearch((*input_)[idx_i], k_, nn_indices, nn_dists);
242 
243  // Among the K nearest neighbours find the one with minimum perpendicular distance
244  // to the normal
245  float min_dist = std::numeric_limits<float>::max();
246 
247  // Find the best correspondence
248  for (std::size_t j = 0; j < nn_indices.size(); j++) {
249  PointSource pt_src;
250  // Copy the source data to a target PointTarget format so we can search in the
251  // tree
252  copyPoint((*input_)[idx_i], pt_src);
253 
254  float cos_angle = (*source_normals_)[idx_i].normal_x *
255  (*target_normals_)[nn_indices[j]].normal_x +
256  (*source_normals_)[idx_i].normal_y *
257  (*target_normals_)[nn_indices[j]].normal_y +
258  (*source_normals_)[idx_i].normal_z *
259  (*target_normals_)[nn_indices[j]].normal_z;
260  float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
261 
262  if (dist < min_dist) {
263  min_dist = dist;
264  min_index = static_cast<int>(j);
265  }
266  }
267  if (min_dist > max_distance)
268  continue;
269 
270  // Check if the correspondence is reciprocal
271  target_idx = nn_indices[min_index];
272  tree_reciprocal_->nearestKSearch(
273  (*target_)[target_idx], 1, index_reciprocal, distance_reciprocal);
274 
275  if (idx_i != index_reciprocal[0])
276  continue;
277 
278  corr.index_query = idx_i;
279  corr.index_match = nn_indices[min_index];
280  corr.distance = nn_dists[min_index]; // min_dist;
281  correspondences[nr_valid_correspondences++] = corr;
282  }
283  }
284  correspondences.resize(nr_valid_correspondences);
285  deinitCompute();
286 }
287 
288 } // namespace registration
289 } // namespace pcl
290 
291 #endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_BACK_PROJECTION_HPP_
virtual void determineReciprocalCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max())
Determine the reciprocal correspondences between input and target cloud.
void determineCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max())
Determine the correspondences between input and target cloud.
Abstract CorrespondenceEstimationBase class.
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
Definition: copy_point.hpp:137
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
Correspondence represents a match between two entities (e.g., points, descriptors,...
index_t index_query
Index of the query (source) point.
index_t index_match
Index of the matching (target) point.