Point Cloud Library (PCL)  1.11.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[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
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  std::vector<int> 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
167 {
168  align(output, Matrix4::Identity());
169 }
170 
171 template <typename PointSource, typename PointTarget, typename Scalar>
172 inline void
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
pcl
Definition: convolution.h:46
pcl::Registration::setInputSource
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)
Definition: registration.hpp:47
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:394
pcl::Registration::setInputTarget
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...
Definition: registration.hpp:61
pcl::Registration::align
void align(PointCloudSource &output)
Call the registration algorithm which estimates the transformation and returns the transformed source...
Definition: registration.hpp:166
pcl::Registration< PointSource, PointTarget >::PointCloudSourceConstPtr
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition: registration.h:78
pcl::PCLBase
PCL base class.
Definition: pcl_base.h:69
pcl::Registration::initComputeReciprocal
bool initComputeReciprocal()
Internal computation when reciprocal lookup is needed.
Definition: registration.hpp:105
pcl::PointCloud< PointSource >
pcl::Registration< PointSource, PointTarget >::Matrix4
Eigen::Matrix< float, 4, 4 > Matrix4
Definition: registration.h:59
pcl::PCLBase::setInputCloud
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:65
pcl::transformPointCloud
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
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:392
pcl::Registration::initCompute
bool initCompute()
Internal computation initialization.
Definition: registration.hpp:75
pcl::PointCloud::is_dense
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:397
pcl::PointCloud::resize
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:456
pcl::PointCloud::header
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:386
pcl::Registration::getFitnessScore
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)
Definition: registration.hpp:134
pcl::Registration< PointSource, PointTarget >::PointCloudTargetConstPtr
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Definition: registration.h:82