Point Cloud Library (PCL)  1.14.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  // Iterate over the input set of source indices
85  for (const auto& idx_i : (*indices_)) {
86  const auto& pt = detail::pointCopyOrRef<PointTarget, PointSource>(input_, idx_i);
87  tree_->nearestKSearch(pt, k_, nn_indices, nn_dists);
88 
89  // Among the K nearest neighbours find the one with minimum perpendicular distance
90  // to the normal
91  float min_dist = std::numeric_limits<float>::max();
92 
93  // Find the best correspondence
94  for (std::size_t j = 0; j < nn_indices.size(); j++) {
95  float cos_angle = (*source_normals_)[idx_i].normal_x *
96  (*target_normals_)[nn_indices[j]].normal_x +
97  (*source_normals_)[idx_i].normal_y *
98  (*target_normals_)[nn_indices[j]].normal_y +
99  (*source_normals_)[idx_i].normal_z *
100  (*target_normals_)[nn_indices[j]].normal_z;
101  float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
102 
103  if (dist < min_dist) {
104  min_dist = dist;
105  min_index = static_cast<int>(j);
106  }
107  }
108  if (min_dist > max_distance)
109  continue;
110 
111  corr.index_query = idx_i;
112  corr.index_match = nn_indices[min_index];
113  corr.distance = nn_dists[min_index]; // min_dist;
114  correspondences[nr_valid_correspondences++] = corr;
115  }
116  correspondences.resize(nr_valid_correspondences);
117  deinitCompute();
118 }
119 
120 template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar>
121 void
124  double max_distance)
125 {
126  if (!initCompute())
127  return;
128 
129  // Set the internal point representation of choice
130  if (!initComputeReciprocal())
131  return;
132 
133  correspondences.resize(indices_->size());
134 
135  pcl::Indices nn_indices(k_);
136  std::vector<float> nn_dists(k_);
137  pcl::Indices index_reciprocal(1);
138  std::vector<float> distance_reciprocal(1);
139 
140  int min_index = 0;
141 
142  pcl::Correspondence corr;
143  unsigned int nr_valid_correspondences = 0;
144  int target_idx = 0;
145 
146  // Iterate over the input set of source indices
147  for (const auto& idx_i : (*indices_)) {
148  // Check if the template types are the same. If true, avoid a copy.
149  // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT
150  // macro!
151  tree_->nearestKSearch(
152  detail::pointCopyOrRef<PointTarget, PointSource>(input_, idx_i),
153  k_,
154  nn_indices,
155  nn_dists);
156 
157  // Among the K nearest neighbours find the one with minimum perpendicular distance
158  // to the normal
159  float min_dist = std::numeric_limits<float>::max();
160 
161  // Find the best correspondence
162  for (std::size_t j = 0; j < nn_indices.size(); j++) {
163  float cos_angle = (*source_normals_)[idx_i].normal_x *
164  (*target_normals_)[nn_indices[j]].normal_x +
165  (*source_normals_)[idx_i].normal_y *
166  (*target_normals_)[nn_indices[j]].normal_y +
167  (*source_normals_)[idx_i].normal_z *
168  (*target_normals_)[nn_indices[j]].normal_z;
169  float dist = nn_dists[j] * (2.0f - cos_angle * cos_angle);
170 
171  if (dist < min_dist) {
172  min_dist = dist;
173  min_index = static_cast<int>(j);
174  }
175  }
176  if (min_dist > max_distance)
177  continue;
178 
179  // Check if the correspondence is reciprocal
180  target_idx = nn_indices[min_index];
181  tree_reciprocal_->nearestKSearch(
182  detail::pointCopyOrRef<PointSource, PointTarget>(target_, target_idx),
183  1,
184  index_reciprocal,
185  distance_reciprocal);
186 
187  if (idx_i != index_reciprocal[0])
188  continue;
189 
190  corr.index_query = idx_i;
191  corr.index_match = nn_indices[min_index];
192  corr.distance = nn_dists[min_index]; // min_dist;
193  correspondences[nr_valid_correspondences++] = corr;
194  }
195  correspondences.resize(nr_valid_correspondences);
196  deinitCompute();
197 }
198 
199 } // namespace registration
200 } // namespace pcl
201 
202 #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.
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.