Point Cloud Library (PCL)  1.11.0
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::setInputTarget] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
52  return;
53  }
54  target_ = cloud;
55  target_cloud_updated_ = true;
56 }
57 
58 
59 template <typename PointSource, typename PointTarget, typename Scalar> bool
61 {
62  if (!target_)
63  {
64  PCL_ERROR ("[pcl::registration::%s::compute] No input target dataset was given!\n", getClassName ().c_str ());
65  return (false);
66  }
67 
68  // Only update target kd-tree if a new target cloud was set
69  if (target_cloud_updated_ && !force_no_recompute_)
70  {
71  tree_->setInputCloud (target_);
72  target_cloud_updated_ = false;
73  }
74 
75  // Update the correspondence estimation
76  if (correspondence_estimation_)
77  {
78  correspondence_estimation_->setSearchMethodTarget (tree_, force_no_recompute_);
79  correspondence_estimation_->setSearchMethodSource (tree_reciprocal_, force_no_recompute_reciprocal_);
80  }
81 
82  // Note: we /cannot/ update the search method on all correspondence rejectors, because we know
83  // nothing about them. If they should be cached, they must be cached individually.
84 
86 }
87 
88 
89 template <typename PointSource, typename PointTarget, typename Scalar> bool
91 {
92  if (!input_)
93  {
94  PCL_ERROR ("[pcl::registration::%s::compute] No input source dataset was given!\n", getClassName ().c_str ());
95  return (false);
96  }
97 
98  if (source_cloud_updated_ && !force_no_recompute_reciprocal_)
99  {
100  tree_reciprocal_->setInputCloud (input_);
101  source_cloud_updated_ = false;
102  }
103  return (true);
104 }
105 
106 
107 template <typename PointSource, typename PointTarget, typename Scalar> inline double
109  const std::vector<float> &distances_a,
110  const std::vector<float> &distances_b)
111 {
112  unsigned int nr_elem = static_cast<unsigned int> (std::min (distances_a.size (), distances_b.size ()));
113  Eigen::VectorXf map_a = Eigen::VectorXf::Map (&distances_a[0], nr_elem);
114  Eigen::VectorXf map_b = Eigen::VectorXf::Map (&distances_b[0], nr_elem);
115  return (static_cast<double> ((map_a - map_b).sum ()) / static_cast<double> (nr_elem));
116 }
117 
118 
119 template <typename PointSource, typename PointTarget, typename Scalar> inline double
121 {
122  double fitness_score = 0.0;
123 
124  // Transform the input dataset using the final transformation
125  PointCloudSource input_transformed;
126  transformPointCloud (*input_, input_transformed, final_transformation_);
127 
128  std::vector<int> nn_indices (1);
129  std::vector<float> nn_dists (1);
130 
131  // For each point in the source dataset
132  int nr = 0;
133  for (std::size_t i = 0; i < input_transformed.points.size (); ++i)
134  {
135  // Find its nearest neighbor in the target
136  tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists);
137 
138  // Deal with occlusions (incomplete targets)
139  if (nn_dists[0] <= max_range)
140  {
141  // Add to the fitness score
142  fitness_score += nn_dists[0];
143  nr++;
144  }
145  }
146 
147  if (nr > 0)
148  return (fitness_score / nr);
149  return (std::numeric_limits<double>::max ());
150 
151 }
152 
153 
154 template <typename PointSource, typename PointTarget, typename Scalar> inline void
156 {
157  align (output, Matrix4::Identity ());
158 }
159 
160 
161 template <typename PointSource, typename PointTarget, typename Scalar> inline void
163 {
164  if (!initCompute ())
165  return;
166 
167  // Resize the output dataset
168  if (output.points.size () != indices_->size ())
169  output.points.resize (indices_->size ());
170  // Copy the header
171  output.header = input_->header;
172  // Check if the output will be computed for all points or only a subset
173  if (indices_->size () != input_->points.size ())
174  {
175  output.width = static_cast<std::uint32_t> (indices_->size ());
176  output.height = 1;
177  }
178  else
179  {
180  output.width = static_cast<std::uint32_t> (input_->width);
181  output.height = input_->height;
182  }
183  output.is_dense = input_->is_dense;
184 
185  // Copy the point data to output
186  for (std::size_t i = 0; i < indices_->size (); ++i)
187  output.points[i] = input_->points[(*indices_)[i]];
188 
189  // Set the internal point representation of choice unless otherwise noted
190  if (point_representation_ && !force_no_recompute_)
191  tree_->setPointRepresentation (point_representation_);
192 
193  // Perform the actual transformation computation
194  converged_ = false;
195  final_transformation_ = transformation_ = previous_transformation_ = Matrix4::Identity ();
196 
197  // Right before we estimate the transformation, we set all the point.data[3] values to 1 to aid the rigid
198  // transformation
199  for (std::size_t i = 0; i < indices_->size (); ++i)
200  output.points[i].data[3] = 1.0;
201 
202  computeTransformation (output, guess);
203 
204  deinitCompute ();
205 }
206 
207 } // namespace pcl
208 
bool initComputeReciprocal()
Internal computation when reciprocal lookup is needed.
void align(PointCloudSource &output)
Call the registration algorithm which estimates the transformation and returns the transformed source...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
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) ...
Eigen::Matrix< Scalar, 4, 4 > Matrix4
Definition: registration.h:64
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...
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Transform< Scalar, 3, Eigen::Affine > &transform, bool copy_all_fields)
Apply an affine transform defined by an Eigen Transform.
Definition: transforms.hpp:221
bool initCompute()
Internal computation initialization.
PCL base class.
Definition: pcl_base.h:69
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415
pcl::PCLHeader header
The point cloud header.
Definition: point_cloud.h:407
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:418
typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr
Definition: registration.h:87