Point Cloud Library (PCL)  1.7.2
correspondence_estimation_normal_shooting.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 #ifndef PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_
41 #define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_
42 
43 #include <pcl/common/copy_point.h>
44 
45 ///////////////////////////////////////////////////////////////////////////////////////////
46 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> bool
48 {
49  if (!source_normals_)
50  {
51  PCL_WARN ("[pcl::registration::%s::initCompute] Datasets containing normals for source have not been given!\n", getClassName ().c_str ());
52  return (false);
53  }
54 
56 }
57 
58 ///////////////////////////////////////////////////////////////////////////////////////////
59 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
61  pcl::Correspondences &correspondences, double max_distance)
62 {
63  if (!initCompute ())
64  return;
65 
66  correspondences.resize (indices_->size ());
67 
68  std::vector<int> nn_indices (k_);
69  std::vector<float> nn_dists (k_);
70 
71  double min_dist = std::numeric_limits<double>::max ();
72  int min_index = 0;
73 
75  unsigned int nr_valid_correspondences = 0;
76 
77  // Check if the template types are the same. If true, avoid a copy.
78  // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro!
79  if (isSamePointType<PointSource, PointTarget> ())
80  {
81  PointTarget pt;
82  // Iterate over the input set of source indices
83  for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
84  {
85  tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
86 
87  // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
88  min_dist = std::numeric_limits<double>::max ();
89 
90  // Find the best correspondence
91  for (size_t j = 0; j < nn_indices.size (); j++)
92  {
93  // computing the distance between a point and a line in 3d.
94  // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
95  pt.x = target_->points[nn_indices[j]].x - input_->points[*idx_i].x;
96  pt.y = target_->points[nn_indices[j]].y - input_->points[*idx_i].y;
97  pt.z = target_->points[nn_indices[j]].z - input_->points[*idx_i].z;
98 
99  const NormalT &normal = source_normals_->points[*idx_i];
100  Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z);
101  Eigen::Vector3d V (pt.x, pt.y, pt.z);
102  Eigen::Vector3d C = N.cross (V);
103 
104  // Check if we have a better correspondence
105  double dist = C.dot (C);
106  if (dist < min_dist)
107  {
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  {
123  PointTarget pt;
124 
125  // Iterate over the input set of source indices
126  for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
127  {
128  tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
129 
130  // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
131  min_dist = std::numeric_limits<double>::max ();
132 
133  // Find the best correspondence
134  for (size_t j = 0; j < nn_indices.size (); j++)
135  {
136  PointSource pt_src;
137  // Copy the source data to a target PointTarget format so we can search in the tree
138  copyPoint (input_->points[*idx_i], pt_src);
139 
140  // computing the distance between a point and a line in 3d.
141  // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
142  pt.x = target_->points[nn_indices[j]].x - pt_src.x;
143  pt.y = target_->points[nn_indices[j]].y - pt_src.y;
144  pt.z = target_->points[nn_indices[j]].z - pt_src.z;
145 
146  const NormalT &normal = source_normals_->points[*idx_i];
147  Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z);
148  Eigen::Vector3d V (pt.x, pt.y, pt.z);
149  Eigen::Vector3d C = N.cross (V);
150 
151  // Check if we have a better correspondence
152  double dist = C.dot (C);
153  if (dist < min_dist)
154  {
155  min_dist = dist;
156  min_index = static_cast<int> (j);
157  }
158  }
159  if (min_dist > max_distance)
160  continue;
161 
162  corr.index_query = *idx_i;
163  corr.index_match = nn_indices[min_index];
164  corr.distance = nn_dists[min_index];//min_dist;
165  correspondences[nr_valid_correspondences++] = corr;
166  }
167  }
168  correspondences.resize (nr_valid_correspondences);
169  deinitCompute ();
170 }
171 
172 ///////////////////////////////////////////////////////////////////////////////////////////
173 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> void
175  pcl::Correspondences &correspondences, double max_distance)
176 {
177  if (!initCompute ())
178  return;
179 
180  // setup tree for reciprocal search
181  // Set the internal point representation of choice
182  if (!initComputeReciprocal ())
183  return;
184 
185  correspondences.resize (indices_->size ());
186 
187  std::vector<int> nn_indices (k_);
188  std::vector<float> nn_dists (k_);
189  std::vector<int> index_reciprocal (1);
190  std::vector<float> distance_reciprocal (1);
191 
192  double min_dist = std::numeric_limits<double>::max ();
193  int min_index = 0;
194 
195  pcl::Correspondence corr;
196  unsigned int nr_valid_correspondences = 0;
197  int target_idx = 0;
198 
199  // Check if the template types are the same. If true, avoid a copy.
200  // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT macro!
201  if (isSamePointType<PointSource, PointTarget> ())
202  {
203  PointTarget pt;
204  // Iterate over the input set of source indices
205  for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
206  {
207  tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
208 
209  // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
210  min_dist = std::numeric_limits<double>::max ();
211 
212  // Find the best correspondence
213  for (size_t j = 0; j < nn_indices.size (); j++)
214  {
215  // computing the distance between a point and a line in 3d.
216  // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
217  pt.x = target_->points[nn_indices[j]].x - input_->points[*idx_i].x;
218  pt.y = target_->points[nn_indices[j]].y - input_->points[*idx_i].y;
219  pt.z = target_->points[nn_indices[j]].z - input_->points[*idx_i].z;
220 
221  const NormalT &normal = source_normals_->points[*idx_i];
222  Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z);
223  Eigen::Vector3d V (pt.x, pt.y, pt.z);
224  Eigen::Vector3d C = N.cross (V);
225 
226  // Check if we have a better correspondence
227  double dist = C.dot (C);
228  if (dist < min_dist)
229  {
230  min_dist = dist;
231  min_index = static_cast<int> (j);
232  }
233  }
234  if (min_dist > max_distance)
235  continue;
236 
237  // Check if the correspondence is reciprocal
238  target_idx = nn_indices[min_index];
239  tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);
240 
241  if (*idx_i != index_reciprocal[0])
242  continue;
243 
244  // Correspondence IS reciprocal, save it and continue
245  corr.index_query = *idx_i;
246  corr.index_match = nn_indices[min_index];
247  corr.distance = nn_dists[min_index];//min_dist;
248  correspondences[nr_valid_correspondences++] = corr;
249  }
250  }
251  else
252  {
253  PointTarget pt;
254 
255  // Iterate over the input set of source indices
256  for (std::vector<int>::const_iterator idx_i = indices_->begin (); idx_i != indices_->end (); ++idx_i)
257  {
258  tree_->nearestKSearch (input_->points[*idx_i], k_, nn_indices, nn_dists);
259 
260  // Among the K nearest neighbours find the one with minimum perpendicular distance to the normal
261  min_dist = std::numeric_limits<double>::max ();
262 
263  // Find the best correspondence
264  for (size_t j = 0; j < nn_indices.size (); j++)
265  {
266  PointSource pt_src;
267  // Copy the source data to a target PointTarget format so we can search in the tree
268  copyPoint (input_->points[*idx_i], pt_src);
269 
270  // computing the distance between a point and a line in 3d.
271  // Reference - http://mathworld.wolfram.com/Point-LineDistance3-Dimensional.html
272  pt.x = target_->points[nn_indices[j]].x - pt_src.x;
273  pt.y = target_->points[nn_indices[j]].y - pt_src.y;
274  pt.z = target_->points[nn_indices[j]].z - pt_src.z;
275 
276  const NormalT &normal = source_normals_->points[*idx_i];
277  Eigen::Vector3d N (normal.normal_x, normal.normal_y, normal.normal_z);
278  Eigen::Vector3d V (pt.x, pt.y, pt.z);
279  Eigen::Vector3d C = N.cross (V);
280 
281  // Check if we have a better correspondence
282  double dist = C.dot (C);
283  if (dist < min_dist)
284  {
285  min_dist = dist;
286  min_index = static_cast<int> (j);
287  }
288  }
289  if (min_dist > max_distance)
290  continue;
291 
292  // Check if the correspondence is reciprocal
293  target_idx = nn_indices[min_index];
294  tree_reciprocal_->nearestKSearch (target_->points[target_idx], 1, index_reciprocal, distance_reciprocal);
295 
296  if (*idx_i != index_reciprocal[0])
297  continue;
298 
299  // Correspondence IS reciprocal, save it and continue
300  corr.index_query = *idx_i;
301  corr.index_match = nn_indices[min_index];
302  corr.distance = nn_dists[min_index];//min_dist;
303  correspondences[nr_valid_correspondences++] = corr;
304  }
305  }
306  correspondences.resize (nr_valid_correspondences);
307  deinitCompute ();
308 }
309 
310 #endif // PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_NORMAL_SHOOTING_H_
A point structure representing normal coordinates and the surface curvature estimate.
int index_match
Index of the matching (target) point.
virtual void determineReciprocalCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max())
Determine the reciprocal correspondences between input and target cloud.
Correspondence represents a match between two entities (e.g., points, descriptors, etc).
int index_query
Index of the query (source) point.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
void determineCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max())
Determine the correspondences between input and target cloud.
void copyPoint(const PointInT &point_in, PointOutT &point_out)
Copy the fields of a source point into a target point.
Definition: copy_point.hpp:138
Abstract CorrespondenceEstimationBase class.