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 
46 template <typename PointSource, typename PointTarget, typename Scalar> inline void
48 {
49  if (cloud->points.empty ())
50  {
51  PCL_ERROR ("[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
52  return;
53  }
54  source_cloud_updated_ = true;
56 }
57 
58 template <typename PointSource, typename PointTarget, typename Scalar> inline void
60 {
61  if (cloud->points.empty ())
62  {
63  PCL_ERROR ("[pcl::%s::setInputTarget] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
64  return;
65  }
66  target_ = cloud;
67  target_cloud_updated_ = true;
68 }
69 
70 
71 template <typename PointSource, typename PointTarget, typename Scalar> bool
73 {
74  if (!target_)
75  {
76  PCL_ERROR ("[pcl::registration::%s::compute] No input target dataset was given!\n", getClassName ().c_str ());
77  return (false);
78  }
79 
80  // Only update target kd-tree if a new target cloud was set
81  if (target_cloud_updated_ && !force_no_recompute_)
82  {
83  tree_->setInputCloud (target_);
84  target_cloud_updated_ = false;
85  }
86 
87  // Update the correspondence estimation
88  if (correspondence_estimation_)
89  {
90  correspondence_estimation_->setSearchMethodTarget (tree_, force_no_recompute_);
91  correspondence_estimation_->setSearchMethodSource (tree_reciprocal_, force_no_recompute_reciprocal_);
92  }
93 
94  // Note: we /cannot/ update the search method on all correspondence rejectors, because we know
95  // nothing about them. If they should be cached, they must be cached individually.
96 
98 }
99 
100 
101 template <typename PointSource, typename PointTarget, typename Scalar> bool
103 {
104  if (!input_)
105  {
106  PCL_ERROR ("[pcl::registration::%s::compute] No input source dataset was given!\n", getClassName ().c_str ());
107  return (false);
108  }
109 
110  if (source_cloud_updated_ && !force_no_recompute_reciprocal_)
111  {
112  tree_reciprocal_->setInputCloud (input_);
113  source_cloud_updated_ = false;
114  }
115  return (true);
116 }
117 
118 
119 template <typename PointSource, typename PointTarget, typename Scalar> inline double
121  const std::vector<float> &distances_a,
122  const std::vector<float> &distances_b)
123 {
124  unsigned int nr_elem = static_cast<unsigned int> (std::min (distances_a.size (), distances_b.size ()));
125  Eigen::VectorXf map_a = Eigen::VectorXf::Map (&distances_a[0], nr_elem);
126  Eigen::VectorXf map_b = Eigen::VectorXf::Map (&distances_b[0], nr_elem);
127  return (static_cast<double> ((map_a - map_b).sum ()) / static_cast<double> (nr_elem));
128 }
129 
130 
131 template <typename PointSource, typename PointTarget, typename Scalar> inline double
133 {
134  double fitness_score = 0.0;
135 
136  // Transform the input dataset using the final transformation
137  PointCloudSource input_transformed;
138  transformPointCloud (*input_, input_transformed, final_transformation_);
139 
140  std::vector<int> nn_indices (1);
141  std::vector<float> nn_dists (1);
142 
143  // For each point in the source dataset
144  int nr = 0;
145  for (const auto& point: input_transformed)
146  {
147  // Find its nearest neighbor in the target
148  tree_->nearestKSearch (point, 1, nn_indices, nn_dists);
149 
150  // Deal with occlusions (incomplete targets)
151  if (nn_dists[0] <= max_range)
152  {
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 
165 
166 template <typename PointSource, typename PointTarget, typename Scalar> inline void
168 {
169  align (output, Matrix4::Identity ());
170 }
171 
172 
173 template <typename PointSource, typename PointTarget, typename Scalar> inline void
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  {
186  output.width = indices_->size ();
187  output.height = 1;
188  }
189  else
190  {
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_ = Matrix4::Identity ();
207 
208  // Right before we estimate the transformation, we set all the point.data[3] values to 1 to aid the rigid
209  // transformation
210  for (std::size_t i = 0; i < indices_->size (); ++i)
211  output[i].data[3] = 1.0;
212 
213  computeTransformation (output, guess);
214 
215  deinitCompute ();
216 }
217 
218 } // namespace pcl
219 
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:393
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:59
pcl::Registration::align
void align(PointCloudSource &output)
Call the registration algorithm which estimates the transformation and returns the transformed source...
Definition: registration.hpp:167
pcl::Registration< PointSource, PointTarget >::PointCloudSourceConstPtr
typename PointCloudSource::ConstPtr PointCloudSourceConstPtr
Definition: registration.h:82
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:102
pcl::PointCloud< PointSource >
pcl::Registration< PointSource, PointTarget >::Matrix4
Eigen::Matrix< float, 4, 4 > Matrix4
Definition: registration.h:63
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:391
pcl::Registration::initCompute
bool initCompute()
Internal computation initialization.
Definition: registration.hpp:72
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:396
pcl::PointCloud::resize
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:455
pcl::PointCloud::header
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:385
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:132
pcl::Registration< PointSource, PointTarget >::PointCloudTargetConstPtr
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Definition: registration.h:86