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