Point Cloud Library (PCL)  1.7.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 ///////////////////////////////////////////////////////////////////////////////////////////
42 template <typename PointSource, typename PointTarget, typename Scalar> void
45 {
46  setInputSource (cloud);
47 }
48 
49 ///////////////////////////////////////////////////////////////////////////////////////////
50 template <typename PointSource, typename PointTarget, typename Scalar> typename pcl::Registration<PointSource, PointTarget, Scalar>::PointCloudSourceConstPtr const
52 {
53  return (getInputSource ());
54 }
55 
56 ///////////////////////////////////////////////////////////////////////////////////////////
57 template <typename PointSource, typename PointTarget, typename Scalar> inline void
59 {
60  if (cloud->points.empty ())
61  {
62  PCL_ERROR ("[pcl::%s::setInputTarget] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
63  return;
64  }
65  target_ = cloud;
66  target_cloud_updated_ = true;
67 }
68 
69 ///////////////////////////////////////////////////////////////////////////////////////////
70 template <typename PointSource, typename PointTarget, typename Scalar> bool
72 {
73  if (!target_)
74  {
75  PCL_ERROR ("[pcl::registration::%s::compute] No input target dataset was given!\n", getClassName ().c_str ());
76  return (false);
77  }
78 
79  // Only update target kd-tree if a new target cloud was set
80  if (target_cloud_updated_ && !force_no_recompute_)
81  {
82  tree_->setInputCloud (target_);
83  target_cloud_updated_ = false;
84  }
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 
135  double fitness_score = 0.0;
136 
137  // Transform the input dataset using the final transformation
138  PointCloudSource input_transformed;
139  //transformPointCloud (*input_, input_transformed, final_transformation_);
140  input_transformed.resize (input_->size ());
141  for (size_t i = 0; i < input_->size (); ++i)
142  {
143  const PointSource &src = input_->points[i];
144  PointTarget &tgt = input_transformed.points[i];
145  tgt.x = static_cast<float> (final_transformation_ (0, 0) * src.x + final_transformation_ (0, 1) * src.y + final_transformation_ (0, 2) * src.z + final_transformation_ (0, 3));
146  tgt.y = static_cast<float> (final_transformation_ (1, 0) * src.x + final_transformation_ (1, 1) * src.y + final_transformation_ (1, 2) * src.z + final_transformation_ (1, 3));
147  tgt.z = static_cast<float> (final_transformation_ (2, 0) * src.x + final_transformation_ (2, 1) * src.y + final_transformation_ (2, 2) * src.z + final_transformation_ (2, 3));
148  }
149 
150  std::vector<int> nn_indices (1);
151  std::vector<float> nn_dists (1);
152 
153  // For each point in the source dataset
154  int nr = 0;
155  for (size_t i = 0; i < input_transformed.points.size (); ++i)
156  {
157  Eigen::Vector4f p1 = Eigen::Vector4f (input_transformed.points[i].x,
158  input_transformed.points[i].y,
159  input_transformed.points[i].z, 0);
160  // Find its nearest neighbor in the target
161  tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists);
162 
163  // Deal with occlusions (incomplete targets)
164  if (nn_dists[0] > max_range)
165  continue;
166 
167  Eigen::Vector4f p2 = Eigen::Vector4f (target_->points[nn_indices[0]].x,
168  target_->points[nn_indices[0]].y,
169  target_->points[nn_indices[0]].z, 0);
170  // Calculate the fitness score
171  fitness_score += fabs ((p1-p2).squaredNorm ());
172  nr++;
173  }
174 
175  if (nr > 0)
176  return (fitness_score / nr);
177  else
178  return (std::numeric_limits<double>::max ());
179 
180 }
181 
182 //////////////////////////////////////////////////////////////////////////////////////////////
183 template <typename PointSource, typename PointTarget, typename Scalar> inline void
185 {
186  align (output, Matrix4::Identity ());
187 }
188 
189 //////////////////////////////////////////////////////////////////////////////////////////////
190 template <typename PointSource, typename PointTarget, typename Scalar> inline void
192 {
193  if (!initCompute ())
194  return;
195 
196  // Resize the output dataset
197  if (output.points.size () != indices_->size ())
198  output.points.resize (indices_->size ());
199  // Copy the header
200  output.header = input_->header;
201  // Check if the output will be computed for all points or only a subset
202  if (indices_->size () != input_->points.size ())
203  {
204  output.width = static_cast<uint32_t> (indices_->size ());
205  output.height = 1;
206  }
207  else
208  {
209  output.width = static_cast<uint32_t> (input_->width);
210  output.height = input_->height;
211  }
212  output.is_dense = input_->is_dense;
213 
214  // Copy the point data to output
215  for (size_t i = 0; i < indices_->size (); ++i)
216  output.points[i] = input_->points[(*indices_)[i]];
217 
218  // Set the internal point representation of choice unless otherwise noted
219  if (point_representation_ && !force_no_recompute_)
220  tree_->setPointRepresentation (point_representation_);
221 
222  // Perform the actual transformation computation
223  converged_ = false;
224  final_transformation_ = transformation_ = previous_transformation_ = Matrix4::Identity ();
225 
226  // Right before we estimate the transformation, we set all the point.data[3] values to 1 to aid the rigid
227  // transformation
228  for (size_t i = 0; i < indices_->size (); ++i)
229  output.points[i].data[3] = 1.0;
230 
231  computeTransformation (output, guess);
232 
233  deinitCompute ();
234 }
235