Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
correspondence_estimation.hpp
1/*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2010-2011, 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_H_
42#define PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_
43
44#include <pcl/common/copy_point.h>
45#include <pcl/common/io.h>
46#include <pcl/common/point_tests.h> // for isXYZFinite
47
48namespace pcl {
49
50namespace registration {
51
52template <typename PointSource, typename PointTarget, typename Scalar>
53void
55 const PointCloudTargetConstPtr& cloud)
56{
57 if (cloud->points.empty()) {
58 PCL_ERROR("[pcl::registration::%s::setInputTarget] Invalid or empty point cloud "
59 "dataset given!\n",
60 getClassName().c_str());
61 return;
62 }
63 target_ = cloud;
64
65 // Set the internal point representation of choice
66 if (point_representation_)
67 tree_->setPointRepresentation(point_representation_);
68
69 target_cloud_updated_ = true;
70}
71
72template <typename PointSource, typename PointTarget, typename Scalar>
73bool
75{
76 if (!target_) {
77 PCL_ERROR("[pcl::registration::%s::compute] No input target dataset was given!\n",
78 getClassName().c_str());
79 return (false);
80 }
81
82 // Only update target kd-tree if a new target cloud was set
83 if (target_cloud_updated_ && !force_no_recompute_) {
84 // If the target indices have been given via setIndicesTarget
85 if (target_indices_)
86 tree_->setInputCloud(target_, target_indices_);
87 else
88 tree_->setInputCloud(target_);
89
90 target_cloud_updated_ = false;
91 }
92
94}
95
96template <typename PointSource, typename PointTarget, typename Scalar>
97bool
99{
100 // Only update source kd-tree if a new target cloud was set
101 if (source_cloud_updated_ && !force_no_recompute_reciprocal_) {
102 if (point_representation_reciprocal_)
103 tree_reciprocal_->setPointRepresentation(point_representation_reciprocal_);
104 // If the target indices have been given via setIndicesTarget
105 if (indices_)
106 tree_reciprocal_->setInputCloud(getInputSource(), getIndicesSource());
107 else
108 tree_reciprocal_->setInputCloud(getInputSource());
109
110 source_cloud_updated_ = false;
111 }
112
113 return (true);
114}
115
116namespace detail {
117
118template <
119 typename PointTarget,
120 typename PointSource,
121 typename Index,
122 typename std::enable_if_t<isSamePointType<PointSource, PointTarget>()>* = nullptr>
123const PointSource&
125{
126 return (*input)[idx];
127}
128
129template <
130 typename PointTarget,
131 typename PointSource,
132 typename Index,
133 typename std::enable_if_t<!isSamePointType<PointSource, PointTarget>()>* = nullptr>
134PointTarget
136{
137 // Copy the source data to a target PointTarget format so we can search in the tree
138 PointTarget pt;
139 copyPoint((*input)[idx], pt);
140 return pt;
141}
142
143} // namespace detail
144
145template <typename PointSource, typename PointTarget, typename Scalar>
146void
148 pcl::Correspondences& correspondences, double max_distance)
149{
150 if (!initCompute())
151 return;
152
153 correspondences.resize(indices_->size());
154
155 pcl::Indices index(1);
156 std::vector<float> distance(1);
157 std::vector<pcl::Correspondences> per_thread_correspondences(num_threads_);
158 for (auto& corrs : per_thread_correspondences) {
159 corrs.reserve(2 * indices_->size() / num_threads_);
160 }
161 double max_dist_sqr = max_distance * max_distance;
162
163#pragma omp parallel for default(none) \
164 shared(max_dist_sqr, per_thread_correspondences) firstprivate(index, distance) \
165 num_threads(num_threads_)
166 // Iterate over the input set of source indices
167 for (int i = 0; i < static_cast<int>(indices_->size()); i++) {
168 const auto& idx = (*indices_)[i];
169 // Check if the template types are the same. If true, avoid a copy.
170 // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT
171 // macro!
172 const auto& pt = detail::pointCopyOrRef<PointTarget, PointSource>(input_, idx);
173 if (!input_->is_dense && !pcl::isXYZFinite(pt))
174 continue;
175 tree_->nearestKSearch(pt, 1, index, distance);
176 if (distance[0] > max_dist_sqr)
177 continue;
178
180 corr.index_query = idx;
181 corr.index_match = index[0];
182 corr.distance = distance[0];
183
184#ifdef _OPENMP
185 const int thread_num = omp_get_thread_num();
186#else
187 const int thread_num = 0;
188#endif
189
190 per_thread_correspondences[thread_num].emplace_back(corr);
191 }
192
193 if (num_threads_ == 1) {
194 correspondences = std::move(per_thread_correspondences.front());
195 }
196 else {
197 const unsigned int nr_correspondences = std::accumulate(
198 per_thread_correspondences.begin(),
199 per_thread_correspondences.end(),
200 static_cast<unsigned int>(0),
201 [](const auto sum, const auto& corr) { return sum + corr.size(); });
202 correspondences.resize(nr_correspondences);
203
204 // Merge per-thread correspondences while keeping them ordered
205 auto insert_loc = correspondences.begin();
206 for (const auto& corrs : per_thread_correspondences) {
207 const auto new_insert_loc = std::move(corrs.begin(), corrs.end(), insert_loc);
208 std::inplace_merge(correspondences.begin(),
209 insert_loc,
210 insert_loc + corrs.size(),
211 [](const auto& lhs, const auto& rhs) {
212 return lhs.index_query < rhs.index_query;
213 });
214 insert_loc = new_insert_loc;
215 }
216 }
217 deinitCompute();
218}
219
220template <typename PointSource, typename PointTarget, typename Scalar>
221void
224 double max_distance)
225{
226 if (!initCompute())
227 return;
228
229 // setup tree for reciprocal search
230 // Set the internal point representation of choice
231 if (!initComputeReciprocal())
232 return;
233 double max_dist_sqr = max_distance * max_distance;
234
235 correspondences.resize(indices_->size());
236 pcl::Indices index(1);
237 std::vector<float> distance(1);
238 pcl::Indices index_reciprocal(1);
239 std::vector<float> distance_reciprocal(1);
240 std::vector<pcl::Correspondences> per_thread_correspondences(num_threads_);
241 for (auto& corrs : per_thread_correspondences) {
242 corrs.reserve(2 * indices_->size() / num_threads_);
243 }
244
245#pragma omp parallel for default(none) \
246 shared(max_dist_sqr, per_thread_correspondences) \
247 firstprivate(index, distance, index_reciprocal, distance_reciprocal) \
248 num_threads(num_threads_)
249 // Iterate over the input set of source indices
250 for (int i = 0; i < static_cast<int>(indices_->size()); i++) {
251 const auto& idx = (*indices_)[i];
252 // Check if the template types are the same. If true, avoid a copy.
253 // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT
254 // macro!
255
256 const auto& pt_src = detail::pointCopyOrRef<PointTarget, PointSource>(input_, idx);
257 if (!input_->is_dense && !pcl::isXYZFinite(pt_src))
258 continue;
259 tree_->nearestKSearch(pt_src, 1, index, distance);
260 if (distance[0] > max_dist_sqr)
261 continue;
262
263 const auto target_idx = index[0];
264 const auto& pt_tgt =
265 detail::pointCopyOrRef<PointSource, PointTarget>(target_, target_idx);
266
267 tree_reciprocal_->nearestKSearch(pt_tgt, 1, index_reciprocal, distance_reciprocal);
268 if (distance_reciprocal[0] > max_dist_sqr || idx != index_reciprocal[0])
269 continue;
270
272 corr.index_query = idx;
273 corr.index_match = index[0];
274 corr.distance = distance[0];
275
276#ifdef _OPENMP
277 const int thread_num = omp_get_thread_num();
278#else
279 const int thread_num = 0;
280#endif
281
282 per_thread_correspondences[thread_num].emplace_back(corr);
283 }
284
285 if (num_threads_ == 1) {
286 correspondences = std::move(per_thread_correspondences.front());
287 }
288 else {
289 const unsigned int nr_correspondences = std::accumulate(
290 per_thread_correspondences.begin(),
291 per_thread_correspondences.end(),
292 static_cast<unsigned int>(0),
293 [](const auto sum, const auto& corr) { return sum + corr.size(); });
294 correspondences.resize(nr_correspondences);
295
296 // Merge per-thread correspondences while keeping them ordered
297 auto insert_loc = correspondences.begin();
298 for (const auto& corrs : per_thread_correspondences) {
299 const auto new_insert_loc = std::move(corrs.begin(), corrs.end(), insert_loc);
300 std::inplace_merge(correspondences.begin(),
301 insert_loc,
302 insert_loc + corrs.size(),
303 [](const auto& lhs, const auto& rhs) {
304 return lhs.index_query < rhs.index_query;
305 });
306 insert_loc = new_insert_loc;
307 }
308 }
309
310 deinitCompute();
311}
312
313} // namespace registration
314} // namespace pcl
315
316//#define PCL_INSTANTIATE_CorrespondenceEstimation(T,U) template class PCL_EXPORTS
317// pcl::registration::CorrespondenceEstimation<T,U>;
318
319#endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_ */
PCL base class.
Definition pcl_base.h:70
shared_ptr< const PointCloud< PointT > > ConstPtr
bool initCompute()
Internal computation initialization.
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
void setInputTarget(const PointCloudTargetConstPtr &cloud)
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
bool initComputeReciprocal()
Internal computation initialization for reciprocal correspondences.
void determineCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max()) override
Determine the correspondences between input and target cloud.
void determineReciprocalCorrespondences(pcl::Correspondences &correspondences, double max_distance=std::numeric_limits< double >::max()) override
Determine the reciprocal 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.
const PointSource & pointCopyOrRef(typename pcl::PointCloud< PointSource >::ConstPtr &input, const Index &idx)
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
constexpr bool isXYZFinite(const PointT &) noexcept
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.