Point Cloud Library (PCL)  1.14.1-dev
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 
43 namespace pcl {
44 
45 template <typename PointSource, typename PointTarget, typename Scalar>
46 inline 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 
59 template <typename PointSource, typename PointTarget, typename Scalar>
60 inline 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 
73 template <typename PointSource, typename PointTarget, typename Scalar>
74 bool
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 
103 template <typename PointSource, typename PointTarget, typename Scalar>
104 bool
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
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 
132 template <typename PointSource, typename PointTarget, typename Scalar>
133 inline 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 
166 template <typename PointSource, typename PointTarget, typename Scalar>
167 inline void
169 {
170  align(output, Matrix4::Identity());
171 }
172 
173 template <typename PointSource, typename PointTarget, typename Scalar>
174 inline 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).
Definition: point_cloud.h:403
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:462
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:398
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:392
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:400
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
Definition: registration.h:78
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
Definition: registration.h:82
Eigen::Matrix< Scalar, 4, 4 > Matrix4
Definition: registration.h:59
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.
Definition: transforms.hpp:221
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition: types.h:133
constexpr bool isXYZFinite(const PointT &) noexcept
Definition: point_tests.h:125