Point Cloud Library (PCL) 1.12.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_)
102 tree_reciprocal_->setPointRepresentation(point_representation_);
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
115template <typename PointSource, typename PointTarget, typename Scalar>
116void
118 pcl::Correspondences& correspondences, double max_distance)
119{
120 if (!initCompute())
121 return;
122
123 double max_dist_sqr = max_distance * max_distance;
124
125 correspondences.resize(indices_->size());
126
127 pcl::Indices index(1);
128 std::vector<float> distance(1);
130 unsigned int nr_valid_correspondences = 0;
132 // Check if the template types are the same. If true, avoid a copy.
133 // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT
134 // macro!
136 // Iterate over the input set of source indices
137 for (const auto& idx : (*indices_)) {
138 tree_->nearestKSearch((*input_)[idx], 1, index, distance);
139 if (distance[0] > max_dist_sqr)
140 continue;
141
142 corr.index_query = idx;
143 corr.index_match = index[0];
144 corr.distance = distance[0];
145 correspondences[nr_valid_correspondences++] = corr;
146 }
147 }
148 else {
149 PointTarget pt;
150
151 // Iterate over the input set of source indices
152 for (const auto& idx : (*indices_)) {
153 // Copy the source data to a target PointTarget format so we can search in the
154 // tree
155 copyPoint((*input_)[idx], pt);
156
157 tree_->nearestKSearch(pt, 1, index, distance);
158 if (distance[0] > max_dist_sqr)
159 continue;
160
161 corr.index_query = idx;
162 corr.index_match = index[0];
163 corr.distance = distance[0];
164 correspondences[nr_valid_correspondences++] = corr;
165 }
166 }
167 correspondences.resize(nr_valid_correspondences);
168 deinitCompute();
169}
170
171template <typename PointSource, typename PointTarget, typename Scalar>
172void
175 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 double max_dist_sqr = max_distance * max_distance;
185
186 correspondences.resize(indices_->size());
187 pcl::Indices index(1);
188 std::vector<float> distance(1);
189 pcl::Indices index_reciprocal(1);
190 std::vector<float> distance_reciprocal(1);
192 unsigned int nr_valid_correspondences = 0;
193 int target_idx = 0;
194
195 // Check if the template types are the same. If true, avoid a copy.
196 // Both point types MUST be registered using the POINT_CLOUD_REGISTER_POINT_STRUCT
197 // macro!
199 // Iterate over the input set of source indices
200 for (const auto& idx : (*indices_)) {
201 tree_->nearestKSearch((*input_)[idx], 1, index, distance);
202 if (distance[0] > max_dist_sqr)
203 continue;
204
205 target_idx = index[0];
206
207 tree_reciprocal_->nearestKSearch(
208 (*target_)[target_idx], 1, index_reciprocal, distance_reciprocal);
209 if (distance_reciprocal[0] > max_dist_sqr || idx != index_reciprocal[0])
210 continue;
211
212 corr.index_query = idx;
213 corr.index_match = index[0];
214 corr.distance = distance[0];
215 correspondences[nr_valid_correspondences++] = corr;
216 }
217 }
218 else {
219 PointTarget pt_src;
220 PointSource pt_tgt;
221
222 // Iterate over the input set of source indices
223 for (const auto& idx : (*indices_)) {
224 // Copy the source data to a target PointTarget format so we can search in the
225 // tree
226 copyPoint((*input_)[idx], pt_src);
227
228 tree_->nearestKSearch(pt_src, 1, index, distance);
229 if (distance[0] > max_dist_sqr)
230 continue;
231
232 target_idx = index[0];
233
234 // Copy the target data to a target PointSource format so we can search in the
235 // tree_reciprocal
236 copyPoint((*target_)[target_idx], pt_tgt);
237
238 tree_reciprocal_->nearestKSearch(
239 pt_tgt, 1, index_reciprocal, distance_reciprocal);
240 if (distance_reciprocal[0] > max_dist_sqr || idx != index_reciprocal[0])
241 continue;
242
243 corr.index_query = idx;
244 corr.index_match = index[0];
245 corr.distance = distance[0];
246 correspondences[nr_valid_correspondences++] = corr;
247 }
248 }
249 correspondences.resize(nr_valid_correspondences);
250 deinitCompute();
251}
252
253} // namespace registration
254} // namespace pcl
255
256//#define PCL_INSTANTIATE_CorrespondenceEstimation(T,U) template class PCL_EXPORTS
257// pcl::registration::CorrespondenceEstimation<T,U>;
258
259#endif /* PCL_REGISTRATION_IMPL_CORRESPONDENCE_ESTIMATION_H_ */
PCL base class.
Definition pcl_base.h:70
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.
std::vector< pcl::Correspondence, Eigen::aligned_allocator< pcl::Correspondence > > Correspondences
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
bool isSamePointType()
Check if two given point types are the same or not.
Definition io.h:323
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.