Point Cloud Library (PCL)  1.7.1
trimmed_icp.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, Willow Garage, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * $Id$
37  *
38  */
39 
40 /*
41  * trimmed_icp.h
42  *
43  * Created on: Mar 10, 2013
44  * Author: papazov
45  */
46 
47 #ifndef TRIMMED_ICP_H_
48 #define TRIMMED_ICP_H_
49 
50 #include <pcl/registration/transformation_estimation_svd.h>
51 #include <pcl/kdtree/kdtree_flann.h>
52 #include <pcl/correspondence.h>
53 #include <pcl/point_cloud.h>
54 #include <pcl/pcl_exports.h>
55 #include <limits>
56 
57 namespace pcl
58 {
59  namespace recognition
60  {
61  template<typename PointT, typename Scalar>
62  class PCL_EXPORTS TrimmedICP: public pcl::registration::TransformationEstimationSVD<PointT, PointT, Scalar>
63  {
64  public:
67 
68  typedef typename Eigen::Matrix<Scalar, 4, 4> Matrix4;
69 
70  public:
72  : new_to_old_energy_ratio_ (0.99f)
73  {}
74 
75  virtual ~TrimmedICP ()
76  {}
77 
78  /** \brief Call this method before calling align().
79  *
80  * \param[in] target is target point cloud. The method builds a kd-tree based on 'target' for performing fast closest point search.
81  * The source point cloud will be registered to 'target' (see align() method).
82  * */
83  inline void
84  init (const PointCloudConstPtr& target)
85  {
86  target_points_ = target;
87  kdtree_.setInputCloud (target);
88  }
89 
90  /** \brief The method performs trimmed ICP, i.e., it rigidly registers the source to the target (passed to the init() method).
91  *
92  * \param[in] source_points is the point cloud to be registered to the target.
93  * \param[in] num_source_points_to_use gives the number of closest source points taken into account for registration. By closest
94  * source points we mean the source points closest to the target. These points are computed anew at each iteration.
95  * \param[in,out] guess_and_result is the estimated rigid transform. IMPORTANT: this matrix is also taken as the initial guess
96  * for the alignment. If there is no guess, set the matrix to identity!
97  * */
98  inline void
99  align (const PointCloud& source_points, int num_source_points_to_use, Matrix4& guess_and_result) const
100  {
101  int num_trimmed_source_points = num_source_points_to_use, num_source_points = static_cast<int> (source_points.size ());
102 
103  if ( num_trimmed_source_points >= num_source_points )
104  {
105  printf ("WARNING in 'TrimmedICP::%s()': the user-defined number of source points of interest is greater or equal to "
106  "the total number of source points. Trimmed ICP will work correctly but won't be very efficient. Either set "
107  "the number of source points to use to a lower value or use standard ICP.\n", __func__);
108  num_trimmed_source_points = num_source_points;
109  }
110 
111  // These are vectors containing source to target correspondences
112  pcl::Correspondences full_src_to_tgt (num_source_points), trimmed_src_to_tgt (num_trimmed_source_points);
113 
114  // Some variables for the closest point search
115  pcl::PointXYZ transformed_source_point;
116  std::vector<int> target_index (1);
117  std::vector<float> sqr_dist_to_target (1);
118  float old_energy, energy = std::numeric_limits<float>::max ();
119 
120 // printf ("\nalign\n");
121 
122  do
123  {
124  // Update the correspondences
125  for ( int i = 0 ; i < num_source_points ; ++i )
126  {
127  // Transform the i-th source point based on the current transform matrix
128  aux::transform (guess_and_result, source_points.points[i], transformed_source_point);
129 
130  // Perform the closest point search
131  kdtree_.nearestKSearch (transformed_source_point, 1, target_index, sqr_dist_to_target);
132 
133  // Update the i-th correspondence
134  full_src_to_tgt[i].index_query = i;
135  full_src_to_tgt[i].index_match = target_index[0];
136  full_src_to_tgt[i].distance = sqr_dist_to_target[0];
137  }
138 
139  // Sort in ascending order according to the squared distance
140  std::sort (full_src_to_tgt.begin (), full_src_to_tgt.end (), TrimmedICP::compareCorrespondences);
141 
142  old_energy = energy;
143  energy = 0.0f;
144 
145  // Now, setup the trimmed correspondences used for the transform estimation
146  for ( int i = 0 ; i < num_trimmed_source_points ; ++i )
147  {
148  trimmed_src_to_tgt[i].index_query = full_src_to_tgt[i].index_query;
149  trimmed_src_to_tgt[i].index_match = full_src_to_tgt[i].index_match;
150  energy += full_src_to_tgt[i].distance;
151  }
152 
153  this->estimateRigidTransformation (source_points, *target_points_, trimmed_src_to_tgt, guess_and_result);
154 
155 // printf ("energy = %f, energy diff. = %f, ratio = %f\n", energy, old_energy - energy, energy/old_energy);
156  }
157  while ( energy/old_energy < new_to_old_energy_ratio_ ); // iterate if enough progress
158 
159 // printf ("\n");
160  }
161 
162  inline void
164  {
165  if ( ratio >= 1 )
166  new_to_old_energy_ratio_ = 0.99f;
167  else
168  new_to_old_energy_ratio_ = ratio;
169  }
170 
171  protected:
172  static inline bool
174  {
175  return static_cast<bool> (a.distance < b.distance);
176  }
177 
178  protected:
182  };
183  } // namespace recognition
184 } // namespace pcl
185 
186 
187 #endif /* TRIMMED_ICP_H_ */