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
47 namespace pcl {
48
49 namespace registration {
50
51 template <typename PointSource, typename PointTarget, typename Scalar>
52 void
setInputTarget(const PointCloudTargetConstPtr & cloud)53 CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::setInputTarget(
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
71 template <typename PointSource, typename PointTarget, typename Scalar>
72 bool
initCompute()73 CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initCompute()
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
92 return (PCLBase<PointSource>::initCompute());
93 }
94
95 template <typename PointSource, typename PointTarget, typename Scalar>
96 bool
initComputeReciprocal()97 CorrespondenceEstimationBase<PointSource, PointTarget, Scalar>::initComputeReciprocal()
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
115 template <typename PointSource, typename PointTarget, typename Scalar>
116 void
determineCorrespondences(pcl::Correspondences & correspondences,double max_distance)117 CorrespondenceEstimation<PointSource, PointTarget, Scalar>::determineCorrespondences(
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);
129 pcl::Correspondence corr;
130 unsigned int nr_valid_correspondences = 0;
131
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!
135 if (isSamePointType<PointSource, PointTarget>()) {
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
171 template <typename PointSource, typename PointTarget, typename Scalar>
172 void
173 CorrespondenceEstimation<PointSource, PointTarget, Scalar>::
determineReciprocalCorrespondences(pcl::Correspondences & correspondences,double max_distance)174 determineReciprocalCorrespondences(pcl::Correspondences& correspondences,
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);
191 pcl::Correspondence corr;
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!
198 if (isSamePointType<PointSource, PointTarget>()) {
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_ */
260