Point Cloud Library (PCL)  1.11.0
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 
46 namespace pcl
47 {
48 
49 namespace registration
50 {
51 
52 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> bool
54 {
55  if (!source_normals_ || !target_normals_)
56  {
57  PCL_WARN ("[pcl::registration::%s::initCompute] Datasets containing normals for source/target have not been given!\n", getClassName ().c_str ());
58  return (false);
59  }
60 
62 }
63 
64 ///////////////////////////////////////////////////////////////////////////////////////////
65 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
67  pcl::Correspondences &correspondences, double max_distance)
68 {
69  if (!initCompute ())
70  return;
71 
72  correspondences.resize (indices_->size ());
73 
74  std::vector<int> nn_indices (k_);
75  std::vector<float> nn_dists (k_);
76 
77  int min_index = 0;
78 
80  unsigned int nr_valid_correspondences = 0;
81 
82  // Check if the template types are the same. If true, avoid a copy.
83  // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro!
84  if (isSamePointType<PointSource, PointTarget> ())
85  {
86  PointTarget pt;
87  // Iterate over the input set of source indices
88  for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
89  {
90  tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
91 
92  // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
93  float min_dist = std::numeric_limits<float>::max ();
94 
95  // Find the best correspondence
96  for (std::size_t j = 0; j < nn_indices.size (); j++)
97  {
98  float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x +
99  source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
100  source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
101  float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
102 
103  if (dist < min_dist)
104  {
105  min_dist = dist;
106  min_index = static_cast<int> (j);
107  }
108  }
109  if (min_dist > max_distance)
110  continue;
111 
112  corr.index_query = *idx_i;
113  corr.index_match = nn_indices[min_index];
114  corr.distance = nn_dists[min_index];//min_dist;
115  correspondences[nr_valid_correspondences++] = corr;
116  }
117  }
118  else
119  {
120  PointTarget pt;
121 
122  // Iterate over the input set of source indices
123  for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
124  {
125  tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
126 
127  // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
128  float min_dist = std::numeric_limits<float>::max ();
129 
130  // Find the best correspondence
131  for (std::size_t j = 0; j < nn_indices.size (); j++)
132  {
133  PointSource pt_src;
134  // Copy the source data to a target PointTarget format so we can search in the tree
135  copyPoint (input_->points[*idx_i], pt_src);
136 
137  float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x +
138  source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
139  source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
140  float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
141 
142  if (dist < min_dist)
143  {
144  min_dist = dist;
145  min_index = static_cast<int> (j);
146  }
147  }
148  if (min_dist > max_distance)
149  continue;
150 
151  corr.index_query = *idx_i;
152  corr.index_match = nn_indices[min_index];
153  corr.distance = nn_dists[min_index];//min_dist;
154  correspondences[nr_valid_correspondences++] = corr;
155  }
156  }
157  correspondences.resize (nr_valid_correspondences);
158  deinitCompute ();
159 }
160 
161 
162 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
164  pcl::Correspondences &correspondences, double max_distance)
165 {
166  if (!initCompute ())
167  return;
168 
169  // Set the internal point representation of choice
170  if(!initComputeReciprocal())
171  return;
172 
173  correspondences.resize (indices_->size ());
174 
175  std::vector<int> nn_indices (k_);
176  std::vector<float> nn_dists (k_);
177  std::vector<int> index_reciprocal (1);
178  std::vector<float> distance_reciprocal (1);
179 
180  int min_index = 0;
181 
182  pcl::Correspondence corr;
183  unsigned int nr_valid_correspondences = 0;
184  int target_idx = 0;
185 
186  // Check if the template types are the same. If true, avoid a copy.
187  // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro!
188  if (isSamePointType<PointSource, PointTarget> ())
189  {
190  PointTarget pt;
191  // Iterate over the input set of source indices
192  for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
193  {
194  tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
195 
196  // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
197  float min_dist = std::numeric_limits<float>::max ();
198 
199  // Find the best correspondence
200  for (std::size_t j = 0; j < nn_indices.size (); j++)
201  {
202  float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x +
203  source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
204  source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
205  float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
206 
207  if (dist < min_dist)
208  {
209  min_dist = dist;
210  min_index = static_cast<int> (j);
211  }
212  }
213  if (min_dist > max_distance)
214  continue;
215 
216  // Check if the correspondence is reciprocal
217  target_idx = nn_indices[min_index];
218  tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);
219 
220  if (*idx_i != index_reciprocal[0])
221  continue;
222 
223  corr.index_query = *idx_i;
224  corr.index_match = nn_indices[min_index];
225  corr.distance = nn_dists[min_index];//min_dist;
226  correspondences[nr_valid_correspondences++] = corr;
227  }
228  }
229  else
230  {
231  PointTarget pt;
232 
233  // Iterate over the input set of source indices
234  for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
235  {
236  tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
237 
238  // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
239  float min_dist = std::numeric_limits<float>::max ();
240 
241  // Find the best correspondence
242  for (std::size_t j = 0; j < nn_indices.size (); j++)
243  {
244  PointSource pt_src;
245  // Copy the source data to a target PointTarget format so we can search in the tree
246  copyPoint (input_->points[*idx_i], pt_src);
247 
248  float cos_angle = source_normals_->points[*idx_i].normal_x * target_normals_->points[nn_indices[j]].normal_x +
249  source_normals_->points[*idx_i].normal_y * target_normals_->points[nn_indices[j]].normal_y +
250  source_normals_->points[*idx_i].normal_z * target_normals_->points[nn_indices[j]].normal_z ;
251  float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
252 
253  if (dist < min_dist)
254  {
255  min_dist = dist;
256  min_index = static_cast<int> (j);
257  }
258  }
259  if (min_dist > max_distance)
260  continue;
261 
262  // Check if the correspondence is reciprocal
263  target_idx = nn_indices[min_index];
264  tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);
265 
266  if (*idx_i != index_reciprocal[0])
267  continue;
268 
269  corr.index_query = *idx_i;
270  corr.index_match = nn_indices[min_index];
271  corr.distance = nn_dists[min_index];//min_dist;
272  correspondences[nr_valid_correspondences++] = corr;
273  }
274  }
275  correspondences.resize (nr_valid_correspondences);
276  deinitCompute ();
277 }
278 
279 } // namespace registration
280 } // namespace pcl
281 
282 #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
Correspondence represents a match between two entities (e.g., points, descriptors,...
int index_query
Index of the query (source) point.
int index_match
Index of the matching (target) point.