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