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