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, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * $Id$ 00038 * 00039 */ 00040 00041 #ifndef PCL_GICP_H_ 00042 #define PCL_GICP_H_ 00043 00044 #include <pcl/registration/icp.h> 00045 #include <pcl/registration/bfgs.h> 00046 00047 namespace pcl 00048 { 00049 /** \brief GeneralizedIterativeClosestPoint is an ICP variant that implements the 00050 * generalized iterative closest point algorithm as described by Alex Segal et al. in 00051 * http://www.stanford.edu/~avsegal/resources/papers/Generalized_ICP.pdf 00052 * The approach is based on using anistropic cost functions to optimize the alignment 00053 * after closest point assignments have been made. 00054 * The original code uses GSL and ANN while in ours we use an eigen mapped BFGS and 00055 * FLANN. 00056 * \author Nizar Sallem 00057 * \ingroup registration 00058 */ 00059 template <typename PointSource, typename PointTarget> 00060 class GeneralizedIterativeClosestPoint : public IterativeClosestPoint<PointSource, PointTarget> 00061 { 00062 public: 00063 using IterativeClosestPoint<PointSource, PointTarget>::reg_name_; 00064 using IterativeClosestPoint<PointSource, PointTarget>::getClassName; 00065 using IterativeClosestPoint<PointSource, PointTarget>::indices_; 00066 using IterativeClosestPoint<PointSource, PointTarget>::target_; 00067 using IterativeClosestPoint<PointSource, PointTarget>::input_; 00068 using IterativeClosestPoint<PointSource, PointTarget>::tree_; 00069 using IterativeClosestPoint<PointSource, PointTarget>::tree_reciprocal_; 00070 using IterativeClosestPoint<PointSource, PointTarget>::nr_iterations_; 00071 using IterativeClosestPoint<PointSource, PointTarget>::max_iterations_; 00072 using IterativeClosestPoint<PointSource, PointTarget>::previous_transformation_; 00073 using IterativeClosestPoint<PointSource, PointTarget>::final_transformation_; 00074 using IterativeClosestPoint<PointSource, PointTarget>::transformation_; 00075 using IterativeClosestPoint<PointSource, PointTarget>::transformation_epsilon_; 00076 using IterativeClosestPoint<PointSource, PointTarget>::converged_; 00077 using IterativeClosestPoint<PointSource, PointTarget>::corr_dist_threshold_; 00078 using IterativeClosestPoint<PointSource, PointTarget>::inlier_threshold_; 00079 using IterativeClosestPoint<PointSource, PointTarget>::min_number_correspondences_; 00080 using IterativeClosestPoint<PointSource, PointTarget>::update_visualizer_; 00081 00082 typedef pcl::PointCloud<PointSource> PointCloudSource; 00083 typedef typename PointCloudSource::Ptr PointCloudSourcePtr; 00084 typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr; 00085 00086 typedef pcl::PointCloud<PointTarget> PointCloudTarget; 00087 typedef typename PointCloudTarget::Ptr PointCloudTargetPtr; 00088 typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr; 00089 00090 typedef PointIndices::Ptr PointIndicesPtr; 00091 typedef PointIndices::ConstPtr PointIndicesConstPtr; 00092 00093 typedef typename Registration<PointSource, PointTarget>::KdTree InputKdTree; 00094 typedef typename Registration<PointSource, PointTarget>::KdTreePtr InputKdTreePtr; 00095 00096 typedef boost::shared_ptr< GeneralizedIterativeClosestPoint<PointSource, PointTarget> > Ptr; 00097 typedef boost::shared_ptr< const GeneralizedIterativeClosestPoint<PointSource, PointTarget> > ConstPtr; 00098 00099 00100 typedef Eigen::Matrix<double, 6, 1> Vector6d; 00101 00102 /** \brief Empty constructor. */ 00103 GeneralizedIterativeClosestPoint () 00104 : k_correspondences_(20) 00105 , gicp_epsilon_(0.001) 00106 , rotation_epsilon_(2e-3) 00107 , input_covariances_(0) 00108 , target_covariances_(0) 00109 , mahalanobis_(0) 00110 , max_inner_iterations_(20) 00111 { 00112 min_number_correspondences_ = 4; 00113 reg_name_ = "GeneralizedIterativeClosestPoint"; 00114 max_iterations_ = 200; 00115 transformation_epsilon_ = 5e-4; 00116 corr_dist_threshold_ = 5.; 00117 rigid_transformation_estimation_ = 00118 boost::bind (&GeneralizedIterativeClosestPoint<PointSource, PointTarget>::estimateRigidTransformationBFGS, 00119 this, _1, _2, _3, _4, _5); 00120 } 00121 00122 /** \brief Provide a pointer to the input dataset 00123 * \param cloud the const boost shared pointer to a PointCloud message 00124 */ 00125 PCL_DEPRECATED (void setInputCloud (const PointCloudSourceConstPtr &cloud), "[pcl::registration::GeneralizedIterativeClosestPoint::setInputCloud] setInputCloud is deprecated. Please use setInputSource instead."); 00126 00127 /** \brief Provide a pointer to the input dataset 00128 * \param cloud the const boost shared pointer to a PointCloud message 00129 */ 00130 inline void 00131 setInputSource (const PointCloudSourceConstPtr &cloud) 00132 { 00133 00134 if (cloud->points.empty ()) 00135 { 00136 PCL_ERROR ("[pcl::%s::setInputSource] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ()); 00137 return; 00138 } 00139 PointCloudSource input = *cloud; 00140 // Set all the point.data[3] values to 1 to aid the rigid transformation 00141 for (size_t i = 0; i < input.size (); ++i) 00142 input[i].data[3] = 1.0; 00143 00144 pcl::IterativeClosestPoint<PointSource, PointTarget>::setInputSource (cloud); 00145 input_covariances_.reserve (input_->size ()); 00146 } 00147 00148 /** \brief Provide a pointer to the input target (e.g., the point cloud that we want to align the input source to) 00149 * \param[in] target the input point cloud target 00150 */ 00151 inline void 00152 setInputTarget (const PointCloudTargetConstPtr &target) 00153 { 00154 pcl::IterativeClosestPoint<PointSource, PointTarget>::setInputTarget(target); 00155 target_covariances_.reserve (target_->size ()); 00156 } 00157 00158 /** \brief Estimate a rigid rotation transformation between a source and a target point cloud using an iterative 00159 * non-linear Levenberg-Marquardt approach. 00160 * \param[in] cloud_src the source point cloud dataset 00161 * \param[in] indices_src the vector of indices describing the points of interest in \a cloud_src 00162 * \param[in] cloud_tgt the target point cloud dataset 00163 * \param[in] indices_tgt the vector of indices describing the correspondences of the interst points from \a indices_src 00164 * \param[out] transformation_matrix the resultant transformation matrix 00165 */ 00166 void 00167 estimateRigidTransformationBFGS (const PointCloudSource &cloud_src, 00168 const std::vector<int> &indices_src, 00169 const PointCloudTarget &cloud_tgt, 00170 const std::vector<int> &indices_tgt, 00171 Eigen::Matrix4f &transformation_matrix); 00172 00173 /** \brief \return Mahalanobis distance matrix for the given point index */ 00174 inline const Eigen::Matrix3d& mahalanobis(size_t index) const 00175 { 00176 assert(index < mahalanobis_.size()); 00177 return mahalanobis_[index]; 00178 } 00179 00180 /** \brief Computes rotation matrix derivative. 00181 * rotation matrix is obtainded from rotation angles x[3], x[4] and x[5] 00182 * \return d/d_rx, d/d_ry and d/d_rz respectively in g[3], g[4] and g[5] 00183 * param x array representing 3D transformation 00184 * param R rotation matrix 00185 * param g gradient vector 00186 */ 00187 void 00188 computeRDerivative(const Vector6d &x, const Eigen::Matrix3d &R, Vector6d &g) const; 00189 00190 /** \brief Set the rotation epsilon (maximum allowable difference between two 00191 * consecutive rotations) in order for an optimization to be considered as having 00192 * converged to the final solution. 00193 * \param epsilon the rotation epsilon 00194 */ 00195 inline void 00196 setRotationEpsilon (double epsilon) { rotation_epsilon_ = epsilon; } 00197 00198 /** \brief Get the rotation epsilon (maximum allowable difference between two 00199 * consecutive rotations) as set by the user. 00200 */ 00201 inline double 00202 getRotationEpsilon () { return (rotation_epsilon_); } 00203 00204 /** \brief Set the number of neighbors used when selecting a point neighbourhood 00205 * to compute covariances. 00206 * A higher value will bring more accurate covariance matrix but will make 00207 * covariances computation slower. 00208 * \param k the number of neighbors to use when computing covariances 00209 */ 00210 void 00211 setCorrespondenceRandomness (int k) { k_correspondences_ = k; } 00212 00213 /** \brief Get the number of neighbors used when computing covariances as set by 00214 * the user 00215 */ 00216 int 00217 getCorrespondenceRandomness () { return (k_correspondences_); } 00218 00219 /** set maximum number of iterations at the optimization step 00220 * \param[in] max maximum number of iterations for the optimizer 00221 */ 00222 void 00223 setMaximumOptimizerIterations (int max) { max_inner_iterations_ = max; } 00224 00225 ///\return maximum number of iterations at the optimization step 00226 int 00227 getMaximumOptimizerIterations () { return (max_inner_iterations_); } 00228 00229 protected: 00230 00231 /** \brief The number of neighbors used for covariances computation. 00232 * default: 20 00233 */ 00234 int k_correspondences_; 00235 00236 /** \brief The epsilon constant for gicp paper; this is NOT the convergence 00237 * tolerence 00238 * default: 0.001 00239 */ 00240 double gicp_epsilon_; 00241 00242 /** The epsilon constant for rotation error. (In GICP the transformation epsilon 00243 * is split in rotation part and translation part). 00244 * default: 2e-3 00245 */ 00246 double rotation_epsilon_; 00247 00248 /** \brief base transformation */ 00249 Eigen::Matrix4f base_transformation_; 00250 00251 /** \brief Temporary pointer to the source dataset. */ 00252 const PointCloudSource *tmp_src_; 00253 00254 /** \brief Temporary pointer to the target dataset. */ 00255 const PointCloudTarget *tmp_tgt_; 00256 00257 /** \brief Temporary pointer to the source dataset indices. */ 00258 const std::vector<int> *tmp_idx_src_; 00259 00260 /** \brief Temporary pointer to the target dataset indices. */ 00261 const std::vector<int> *tmp_idx_tgt_; 00262 00263 00264 /** \brief Input cloud points covariances. */ 00265 std::vector<Eigen::Matrix3d> input_covariances_; 00266 00267 /** \brief Target cloud points covariances. */ 00268 std::vector<Eigen::Matrix3d> target_covariances_; 00269 00270 /** \brief Mahalanobis matrices holder. */ 00271 std::vector<Eigen::Matrix3d> mahalanobis_; 00272 00273 /** \brief maximum number of optimizations */ 00274 int max_inner_iterations_; 00275 00276 /** \brief compute points covariances matrices according to the K nearest 00277 * neighbors. K is set via setCorrespondenceRandomness() methode. 00278 * \param cloud pointer to point cloud 00279 * \param tree KD tree performer for nearest neighbors search 00280 * \return cloud_covariance covariances matrices for each point in the cloud 00281 */ 00282 template<typename PointT> 00283 void computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud, 00284 const typename pcl::search::KdTree<PointT>::Ptr tree, 00285 std::vector<Eigen::Matrix3d>& cloud_covariances); 00286 00287 /** \return trace of mat1^t . mat2 00288 * \param mat1 matrix of dimension nxm 00289 * \param mat2 matrix of dimension nxp 00290 */ 00291 inline double 00292 matricesInnerProd(const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2) const 00293 { 00294 double r = 0.; 00295 size_t n = mat1.rows(); 00296 // tr(mat1^t.mat2) 00297 for(size_t i = 0; i < n; i++) 00298 for(size_t j = 0; j < n; j++) 00299 r += mat1 (j, i) * mat2 (i,j); 00300 return r; 00301 } 00302 00303 /** \brief Rigid transformation computation method with initial guess. 00304 * \param output the transformed input point cloud dataset using the rigid transformation found 00305 * \param guess the initial guess of the transformation to compute 00306 */ 00307 void 00308 computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess); 00309 00310 /** \brief Search for the closest nearest neighbor of a given point. 00311 * \param query the point to search a nearest neighbour for 00312 * \param index vector of size 1 to store the index of the nearest neighbour found 00313 * \param distance vector of size 1 to store the distance to nearest neighbour found 00314 */ 00315 inline bool 00316 searchForNeighbors (const PointSource &query, std::vector<int>& index, std::vector<float>& distance) 00317 { 00318 int k = tree_->nearestKSearch (query, 1, index, distance); 00319 if (k == 0) 00320 return (false); 00321 return (true); 00322 } 00323 00324 /// \brief compute transformation matrix from transformation matrix 00325 void applyState(Eigen::Matrix4f &t, const Vector6d& x) const; 00326 00327 /// \brief optimization functor structure 00328 struct OptimizationFunctorWithIndices : public BFGSDummyFunctor<double,6> 00329 { 00330 OptimizationFunctorWithIndices (const GeneralizedIterativeClosestPoint* gicp) 00331 : BFGSDummyFunctor<double,6> (), gicp_(gicp) {} 00332 double operator() (const Vector6d& x); 00333 void df(const Vector6d &x, Vector6d &df); 00334 void fdf(const Vector6d &x, double &f, Vector6d &df); 00335 00336 const GeneralizedIterativeClosestPoint *gicp_; 00337 }; 00338 00339 boost::function<void(const pcl::PointCloud<PointSource> &cloud_src, 00340 const std::vector<int> &src_indices, 00341 const pcl::PointCloud<PointTarget> &cloud_tgt, 00342 const std::vector<int> &tgt_indices, 00343 Eigen::Matrix4f &transformation_matrix)> rigid_transformation_estimation_; 00344 }; 00345 } 00346 00347 #include <pcl/registration/impl/gicp.hpp> 00348 00349 #endif //#ifndef PCL_GICP_H_