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 #pragma once
42
43 namespace pcl {
44
45 template <typename PointSource, typename PointTarget, typename Scalar>
46 inline void
setInputSource(const PointCloudSourceConstPtr & cloud)47 Registration<PointSource, PointTarget, Scalar>::setInputSource(
48 const PointCloudSourceConstPtr& cloud)
49 {
50 if (cloud->points.empty()) {
51 PCL_ERROR("[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n",
52 getClassName().c_str());
53 return;
54 }
55 source_cloud_updated_ = true;
56 PCLBase<PointSource>::setInputCloud(cloud);
57 }
58
59 template <typename PointSource, typename PointTarget, typename Scalar>
60 inline void
setInputTarget(const PointCloudTargetConstPtr & cloud)61 Registration<PointSource, PointTarget, Scalar>::setInputTarget(
62 const PointCloudTargetConstPtr& cloud)
63 {
64 if (cloud->points.empty()) {
65 PCL_ERROR("[pcl::%s::setInputTarget] Invalid or empty point cloud dataset given!\n",
66 getClassName().c_str());
67 return;
68 }
69 target_ = cloud;
70 target_cloud_updated_ = true;
71 }
72
73 template <typename PointSource, typename PointTarget, typename Scalar>
74 bool
initCompute()75 Registration<PointSource, PointTarget, Scalar>::initCompute()
76 {
77 if (!target_) {
78 PCL_ERROR("[pcl::registration::%s::compute] No input target dataset was given!\n",
79 getClassName().c_str());
80 return (false);
81 }
82
83 // Only update target kd-tree if a new target cloud was set
84 if (target_cloud_updated_ && !force_no_recompute_) {
85 tree_->setInputCloud(target_);
86 target_cloud_updated_ = false;
87 }
88
89 // Update the correspondence estimation
90 if (correspondence_estimation_) {
91 correspondence_estimation_->setSearchMethodTarget(tree_, force_no_recompute_);
92 correspondence_estimation_->setSearchMethodSource(tree_reciprocal_,
93 force_no_recompute_reciprocal_);
94 }
95
96 // Note: we /cannot/ update the search method on all correspondence rejectors, because
97 // we know nothing about them. If they should be cached, they must be cached
98 // individually.
99
100 return (PCLBase<PointSource>::initCompute());
101 }
102
103 template <typename PointSource, typename PointTarget, typename Scalar>
104 bool
initComputeReciprocal()105 Registration<PointSource, PointTarget, Scalar>::initComputeReciprocal()
106 {
107 if (!input_) {
108 PCL_ERROR("[pcl::registration::%s::compute] No input source dataset was given!\n",
109 getClassName().c_str());
110 return (false);
111 }
112
113 if (source_cloud_updated_ && !force_no_recompute_reciprocal_) {
114 tree_reciprocal_->setInputCloud(input_);
115 source_cloud_updated_ = false;
116 }
117 return (true);
118 }
119
120 template <typename PointSource, typename PointTarget, typename Scalar>
121 inline double
getFitnessScore(const std::vector<float> & distances_a,const std::vector<float> & distances_b)122 Registration<PointSource, PointTarget, Scalar>::getFitnessScore(
123 const std::vector<float>& distances_a, const std::vector<float>& distances_b)
124 {
125 unsigned int nr_elem =
126 static_cast<unsigned int>(std::min(distances_a.size(), distances_b.size()));
127 Eigen::VectorXf map_a = Eigen::VectorXf::Map(&distances_a[0], nr_elem);
128 Eigen::VectorXf map_b = Eigen::VectorXf::Map(&distances_b[0], nr_elem);
129 return (static_cast<double>((map_a - map_b).sum()) / static_cast<double>(nr_elem));
130 }
131
132 template <typename PointSource, typename PointTarget, typename Scalar>
133 inline double
getFitnessScore(double max_range)134 Registration<PointSource, PointTarget, Scalar>::getFitnessScore(double max_range)
135 {
136 double fitness_score = 0.0;
137
138 // Transform the input dataset using the final transformation
139 PointCloudSource input_transformed;
140 transformPointCloud(*input_, input_transformed, final_transformation_);
141
142 pcl::Indices nn_indices(1);
143 std::vector<float> nn_dists(1);
144
145 // For each point in the source dataset
146 int nr = 0;
147 for (const auto& point : input_transformed) {
148 // Find its nearest neighbor in the target
149 tree_->nearestKSearch(point, 1, nn_indices, nn_dists);
150
151 // Deal with occlusions (incomplete targets)
152 if (nn_dists[0] <= max_range) {
153 // Add to the fitness score
154 fitness_score += nn_dists[0];
155 nr++;
156 }
157 }
158
159 if (nr > 0)
160 return (fitness_score / nr);
161 return (std::numeric_limits<double>::max());
162 }
163
164 template <typename PointSource, typename PointTarget, typename Scalar>
165 inline void
align(PointCloudSource & output)166 Registration<PointSource, PointTarget, Scalar>::align(PointCloudSource& output)
167 {
168 align(output, Matrix4::Identity());
169 }
170
171 template <typename PointSource, typename PointTarget, typename Scalar>
172 inline void
align(PointCloudSource & output,const Matrix4 & guess)173 Registration<PointSource, PointTarget, Scalar>::align(PointCloudSource& output,
174 const Matrix4& guess)
175 {
176 if (!initCompute())
177 return;
178
179 // Resize the output dataset
180 output.resize(indices_->size());
181 // Copy the header
182 output.header = input_->header;
183 // Check if the output will be computed for all points or only a subset
184 if (indices_->size() != input_->size()) {
185 output.width = indices_->size();
186 output.height = 1;
187 }
188 else {
189 output.width = static_cast<std::uint32_t>(input_->width);
190 output.height = input_->height;
191 }
192 output.is_dense = input_->is_dense;
193
194 // Copy the point data to output
195 for (std::size_t i = 0; i < indices_->size(); ++i)
196 output[i] = (*input_)[(*indices_)[i]];
197
198 // Set the internal point representation of choice unless otherwise noted
199 if (point_representation_ && !force_no_recompute_)
200 tree_->setPointRepresentation(point_representation_);
201
202 // Perform the actual transformation computation
203 converged_ = false;
204 final_transformation_ = transformation_ = previous_transformation_ =
205 Matrix4::Identity();
206
207 // Right before we estimate the transformation, we set all the point.data[3] values to
208 // 1 to aid the rigid transformation
209 for (std::size_t i = 0; i < indices_->size(); ++i)
210 output[i].data[3] = 1.0;
211
212 computeTransformation(output, guess);
213
214 deinitCompute();
215 }
216
217 } // namespace pcl
218