Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
registration.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#pragma once
42
43namespace pcl {
44
45template <typename PointSource, typename PointTarget, typename Scalar>
46inline void
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;
57}
58
59template <typename PointSource, typename PointTarget, typename Scalar>
60inline void
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
73template <typename PointSource, typename PointTarget, typename Scalar>
74bool
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
101}
102
103template <typename PointSource, typename PointTarget, typename Scalar>
104bool
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
120template <typename PointSource, typename PointTarget, typename Scalar>
121inline double
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.data(), nr_elem);
128 Eigen::VectorXf map_b = Eigen::VectorXf::Map(distances_b.data(), nr_elem);
129 return (static_cast<double>((map_a - map_b).sum()) / static_cast<double>(nr_elem));
130}
131
132template <typename PointSource, typename PointTarget, typename Scalar>
133inline double
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 if (!input_->is_dense && !pcl::isXYZFinite(point))
149 continue;
150 // Find its nearest neighbor in the target
151 tree_->nearestKSearch(point, 1, nn_indices, nn_dists);
152
153 // Deal with occlusions (incomplete targets)
154 if (nn_dists[0] <= max_range) {
155 // Add to the fitness score
156 fitness_score += nn_dists[0];
157 nr++;
158 }
159 }
160
161 if (nr > 0)
162 return (fitness_score / nr);
163 return (std::numeric_limits<double>::max());
164}
165
166template <typename PointSource, typename PointTarget, typename Scalar>
167inline void
169{
170 align(output, Matrix4::Identity());
171}
172
173template <typename PointSource, typename PointTarget, typename Scalar>
174inline void
176 const Matrix4& guess)
177{
178 if (!initCompute())
179 return;
180
181 // Resize the output dataset
182 output.resize(indices_->size());
183 // Copy the header
184 output.header = input_->header;
185 // Check if the output will be computed for all points or only a subset
186 if (indices_->size() != input_->size()) {
187 output.width = indices_->size();
188 output.height = 1;
189 }
190 else {
191 output.width = static_cast<std::uint32_t>(input_->width);
192 output.height = input_->height;
193 }
194 output.is_dense = input_->is_dense;
195
196 // Copy the point data to output
197 for (std::size_t i = 0; i < indices_->size(); ++i)
198 output[i] = (*input_)[(*indices_)[i]];
199
200 // Set the internal point representation of choice unless otherwise noted
201 if (point_representation_ && !force_no_recompute_)
202 tree_->setPointRepresentation(point_representation_);
203
204 // Perform the actual transformation computation
205 converged_ = false;
206 final_transformation_ = transformation_ = previous_transformation_ =
207 Matrix4::Identity();
208
209 // Right before we estimate the transformation, we set all the point.data[3] values to
210 // 1 to aid the rigid transformation
211 for (std::size_t i = 0; i < indices_->size(); ++i)
212 output[i].data[3] = 1.0;
213
214 computeTransformation(output, guess);
215
216 deinitCompute();
217}
218
219} // namespace pcl
PCL base class.
Definition pcl_base.h:70
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition pcl_base.hpp:65
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
void resize(std::size_t count)
Resizes the container to contain count elements.
std::uint32_t width
The point cloud width (if organized as an image-structure).
pcl::PCLHeader header
The point cloud header.
std::uint32_t height
The point cloud height (if organized as an image-structure).
bool initCompute()
Internal computation initialization.
virtual void setInputSource(const PointCloudSourceConstPtr &cloud)
Provide a pointer to the input source (e.g., the point cloud that we want to align to the target)
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
virtual 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...
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Eigen::Matrix< Scalar, 4, 4 > Matrix4
void align(PointCloudSource &output)
Call the registration algorithm which estimates the transformation and returns the transformed source...
double getFitnessScore(double max_range=std::numeric_limits< double >::max())
Obtain the Euclidean fitness score (e.g., mean of squared distances from the source to the target)
bool initComputeReciprocal()
Internal computation when reciprocal lookup is needed.
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
constexpr bool isXYZFinite(const PointT &) noexcept