Point Cloud Library (PCL) 1.14.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
47namespace pcl {
48
49namespace registration {
50
51template <typename PointSource, typename PointTarget, typename Scalar>
52void
54 const PointCloudTargetConstPtr& cloud)
55{
56 if (cloud->points.empty()) {
57 PCL_ERROR("[pcl::registration::%s::setInputTarget] Invalid or empty point cloud "
58 "dataset given!\n",
59 getClassName().c_str());
60 return;
61 }
62 target_ = cloud;
63
64 // Set the internal point representation of choice
65 if (point_representation_)
66 tree_->setPointRepresentation(point_representation_);
67
68 target_cloud_updated_ = true;
69}
70
71template <typename PointSource, typename PointTarget, typename Scalar>
72bool
74{
75 if (!target_) {
76 PCL_ERROR("[pcl::registration::%s::compute] No input target dataset was given!\n",
77 getClassName().c_str());
78 return (false);
79 }
80
81 // Only update target kd-tree if a new target cloud was set
82 if (target_cloud_updated_ && !force_no_recompute_) {
83 // If the target indices have been given via setIndicesTarget
84 if (target_indices_)
85 tree_->setInputCloud(target_, target_indices_);
86 else
87 tree_->setInputCloud(target_);
88
89 target_cloud_updated_ = false;
90 }
91
93}
94
95template <typename PointSource, typename PointTarget, typename Scalar>
96bool
98{
99 // Only update source kd-tree if a new target cloud was set
100 if (source_cloud_updated_ && !force_no_recompute_reciprocal_) {
101 if (point_representation_reciprocal_)
102 tree_reciprocal_->setPointRepresentation(point_representation_reciprocal_);
103 // If the target indices have been given via setIndicesTarget
104 if (indices_)
105 tree_reciprocal_->setInputCloud(getInputSource(), getIndicesSource());
106 else
107 tree_reciprocal_->setInputCloud(getInputSource());
108
109 source_cloud_updated_ = false;
110 }
111
112 return (true);
113}
114
115namespace detail {
116
117template <
118 typename PointTarget,
119 typename PointSource,
120 typename Index,
121 typename std::enable_if_t<isSamePointType<PointSource, PointTarget>()>* = nullptr>
122const PointSource&
124{
125 return (*input)[idx];
126}
127
128template <
129 typename PointTarget,
130 typename PointSource,
131 typename Index,
132 typename std::enable_if_t<!isSamePointType<PointSource, PointTarget>()>* = nullptr>
133PointTarget
135{
136 // Copy the source data to a target PointTarget format so we can search in the tree
137 PointTarget pt;
138 copyPoint((*input)[idx], pt);
139 return pt;
140}
141
142} // namespace detail
143
144template <typename PointSource, typename PointTarget, typename Scalar>
145void
147 pcl::Correspondences& correspondences, double max_distance)
148{
149 if (!initCompute())
150 return;
151
152 correspondences.resize(indices_->size());
153
154 pcl::Indices index(1);
155 std::vector<float> distance(1);
157 unsigned int nr_valid_correspondences = 0;
158 double max_dist_sqr = max_distance * max_distance;
159
160 // Iterate over the input set of source indices
161 for (const auto& idx : (*indices_)) {
162 // Check if the template types are the same. If true, avoid a copy.
163 // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT
164 // macro!
165 const auto& pt = detail::pointCopyOrRef<PointTarget, PointSource>(input_, idx);
166 tree_->nearestKSearch(pt, 1, index, distance);
167 if (distance[0] > max_dist_sqr)
168 continue;
169
170 corr.index_query = idx;
171 corr.index_match = index[0];
172 corr.distance = distance[0];
173 correspondences[nr_valid_correspondences++] = corr;
174 }
175
176 correspondences.resize(nr_valid_correspondences);
177 deinitCompute();
178}
179
180template <typename PointSource, typename PointTarget, typename Scalar>
181void
184 double max_distance)
185{
186 if (!initCompute())
187 return;
188
189 // setup tree for reciprocal search
190 // Set the internal point representation of choice
191 if (!initComputeReciprocal())
192 return;
193 double max_dist_sqr = max_distance * max_distance;
194
195 correspondences.resize(indices_->size());
196 pcl::Indices index(1);
197 std::vector<float> distance(1);
198 pcl::Indices index_reciprocal(1);
199 std::vector<float> distance_reciprocal(1);
201 unsigned int nr_valid_correspondences = 0;
202 int target_idx = 0;
203
204 // Iterate over the input set of source indices
205 for (const auto& idx : (*indices_)) {
206 // Check if the template types are the same. If true, avoid a copy.
207 // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT
208 // macro!
209
210 const auto& pt_src = detail::pointCopyOrRef<PointTarget, PointSource>(input_, idx);
211
212 tree_->nearestKSearch(pt_src, 1, index, distance);
213 if (distance[0] > max_dist_sqr)
214 continue;
215
216 target_idx = index[0];
217 const auto& pt_tgt =
218 detail::pointCopyOrRef<PointSource, PointTarget>(target_, target_idx);
219
220 tree_reciprocal_->nearestKSearch(pt_tgt, 1, index_reciprocal, distance_reciprocal);
221 if (distance_reciprocal[0] > max_dist_sqr || idx != index_reciprocal[0])
222 continue;
223
224 corr.index_query = idx;
225 corr.index_match = index[0];
226 corr.distance = distance[0];
227 correspondences[nr_valid_correspondences++] = corr;
228 }
229 correspondences.resize(nr_valid_correspondences);
230 deinitCompute();
231}
232
233} // namespace registration
234} // namespace pcl
235
236//#define PCL_INSTANTIATE_CorrespondenceEstimation(T,U) template class PCL_EXPORTS
237// pcl::registration::CorrespondenceEstimation<T,U>;
238
239#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
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.