Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2012, Willow Garage, Inc. 00006 * 00007 * All rights reserved. 00008 * 00009 * Redistribution and use in source and binary forms, with or without 00010 * modification, are permitted provided that the following conditions 00011 * are met: 00012 * 00013 * * Redistributions of source code must retain the above copyright 00014 * notice, this list of conditions and the following disclaimer. 00015 * * Redistributions in binary form must reproduce the above 00016 * copyright notice, this list of conditions and the following 00017 * disclaimer in the documentation and/or other materials provided 00018 * with the distribution. 00019 * * Neither the name of Willow Garage, Inc. nor the names of its 00020 * contributors may be used to endorse or promote products derived 00021 * from this software without specific prior written permission. 00022 * 00023 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 * POSSIBILITY OF SUCH DAMAGE. 00035 * 00036 * $Id$ 00037 * 00038 */ 00039 00040 /* 00041 * trimmed_icp.h 00042 * 00043 * Created on: Mar 10, 2013 00044 * Author: papazov 00045 */ 00046 00047 #ifndef TRIMMED_ICP_H_ 00048 #define TRIMMED_ICP_H_ 00049 00050 #include <pcl/registration/transformation_estimation_svd.h> 00051 #include <pcl/kdtree/kdtree_flann.h> 00052 #include <pcl/correspondence.h> 00053 #include <pcl/point_cloud.h> 00054 #include <pcl/pcl_exports.h> 00055 #include <limits> 00056 00057 namespace pcl 00058 { 00059 namespace recognition 00060 { 00061 template<typename PointT, typename Scalar> 00062 class PCL_EXPORTS TrimmedICP: public pcl::registration::TransformationEstimationSVD<PointT, PointT, Scalar> 00063 { 00064 public: 00065 typedef pcl::PointCloud<PointT> PointCloud; 00066 typedef typename PointCloud::ConstPtr PointCloudConstPtr; 00067 00068 typedef typename Eigen::Matrix<Scalar, 4, 4> Matrix4; 00069 00070 public: 00071 TrimmedICP () 00072 : new_to_old_energy_ratio_ (0.99f) 00073 {} 00074 00075 virtual ~TrimmedICP () 00076 {} 00077 00078 /** \brief Call this method before calling align(). 00079 * 00080 * \param[in] target is target point cloud. The method builds a kd-tree based on 'target' for performing fast closest point search. 00081 * The source point cloud will be registered to 'target' (see align() method). 00082 * */ 00083 inline void 00084 init (const PointCloudConstPtr& target) 00085 { 00086 target_points_ = target; 00087 kdtree_.setInputCloud (target); 00088 } 00089 00090 /** \brief The method performs trimmed ICP, i.e., it rigidly registers the source to the target (passed to the init() method). 00091 * 00092 * \param[in] source_points is the point cloud to be registered to the target. 00093 * \param[in] num_source_points_to_use gives the number of closest source points taken into account for registration. By closest 00094 * source points we mean the source points closest to the target. These points are computed anew at each iteration. 00095 * \param[in,out] guess_and_result is the estimated rigid transform. IMPORTANT: this matrix is also taken as the initial guess 00096 * for the alignment. If there is no guess, set the matrix to identity! 00097 * */ 00098 inline void 00099 align (const PointCloud& source_points, int num_source_points_to_use, Matrix4& guess_and_result) const 00100 { 00101 int num_trimmed_source_points = num_source_points_to_use, num_source_points = static_cast<int> (source_points.size ()); 00102 00103 if ( num_trimmed_source_points >= num_source_points ) 00104 { 00105 printf ("WARNING in 'TrimmedICP::%s()': the user-defined number of source points of interest is greater or equal to " 00106 "the total number of source points. Trimmed ICP will work correctly but won't be very efficient. Either set " 00107 "the number of source points to use to a lower value or use standard ICP.\n", __func__); 00108 num_trimmed_source_points = num_source_points; 00109 } 00110 00111 // These are vectors containing source to target correspondences 00112 pcl::Correspondences full_src_to_tgt (num_source_points), trimmed_src_to_tgt (num_trimmed_source_points); 00113 00114 // Some variables for the closest point search 00115 pcl::PointXYZ transformed_source_point; 00116 std::vector<int> target_index (1); 00117 std::vector<float> sqr_dist_to_target (1); 00118 float old_energy, energy = std::numeric_limits<float>::max (); 00119 00120 // printf ("\nalign\n"); 00121 00122 do 00123 { 00124 // Update the correspondences 00125 for ( int i = 0 ; i < num_source_points ; ++i ) 00126 { 00127 // Transform the i-th source point based on the current transform matrix 00128 aux::transform (guess_and_result, source_points.points[i], transformed_source_point); 00129 00130 // Perform the closest point search 00131 kdtree_.nearestKSearch (transformed_source_point, 1, target_index, sqr_dist_to_target); 00132 00133 // Update the i-th correspondence 00134 full_src_to_tgt[i].index_query = i; 00135 full_src_to_tgt[i].index_match = target_index[0]; 00136 full_src_to_tgt[i].distance = sqr_dist_to_target[0]; 00137 } 00138 00139 // Sort in ascending order according to the squared distance 00140 std::sort (full_src_to_tgt.begin (), full_src_to_tgt.end (), TrimmedICP::compareCorrespondences); 00141 00142 old_energy = energy; 00143 energy = 0.0f; 00144 00145 // Now, setup the trimmed correspondences used for the transform estimation 00146 for ( int i = 0 ; i < num_trimmed_source_points ; ++i ) 00147 { 00148 trimmed_src_to_tgt[i].index_query = full_src_to_tgt[i].index_query; 00149 trimmed_src_to_tgt[i].index_match = full_src_to_tgt[i].index_match; 00150 energy += full_src_to_tgt[i].distance; 00151 } 00152 00153 this->estimateRigidTransformation (source_points, *target_points_, trimmed_src_to_tgt, guess_and_result); 00154 00155 // printf ("energy = %f, energy diff. = %f, ratio = %f\n", energy, old_energy - energy, energy/old_energy); 00156 } 00157 while ( energy/old_energy < new_to_old_energy_ratio_ ); // iterate if enough progress 00158 00159 // printf ("\n"); 00160 } 00161 00162 inline void 00163 setNewToOldEnergyRatio (float ratio) 00164 { 00165 if ( ratio >= 1 ) 00166 new_to_old_energy_ratio_ = 0.99f; 00167 else 00168 new_to_old_energy_ratio_ = ratio; 00169 } 00170 00171 protected: 00172 static inline bool 00173 compareCorrespondences (const pcl::Correspondence& a, const pcl::Correspondence& b) 00174 { 00175 return static_cast<bool> (a.distance < b.distance); 00176 } 00177 00178 protected: 00179 PointCloudConstPtr target_points_; 00180 pcl::KdTreeFLANN<PointT> kdtree_; 00181 float new_to_old_energy_ratio_; 00182 }; 00183 } // namespace recognition 00184 } // namespace pcl 00185 00186 00187 #endif /* TRIMMED_ICP_H_ */