Point Cloud Library (PCL)  1.3.1
gicp.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: gicp.h 3042 2011-11-01 04:44:47Z svn $
00035  *
00036  */
00037 
00038 #ifndef PCL_GICP_H_
00039 #define PCL_GICP_H_
00040 
00041 // PCL includes
00042 #include "pcl/registration/icp.h"
00043 
00044 #include <cminpack.h>
00045 
00046 namespace pcl
00047 {
00058   template <typename PointSource, typename PointTarget>
00059   class GeneralizedIterativeClosestPoint : public IterativeClosestPoint<PointSource, PointTarget>
00060   {
00061     using IterativeClosestPoint<PointSource, PointTarget>::reg_name_;
00062     using IterativeClosestPoint<PointSource, PointTarget>::getClassName;
00063     using IterativeClosestPoint<PointSource, PointTarget>::indices_;
00064     using IterativeClosestPoint<PointSource, PointTarget>::target_;
00065     using IterativeClosestPoint<PointSource, PointTarget>::input_;
00066     using IterativeClosestPoint<PointSource, PointTarget>::tree_;
00067     using IterativeClosestPoint<PointSource, PointTarget>::nr_iterations_;
00068     using IterativeClosestPoint<PointSource, PointTarget>::max_iterations_;
00069     using IterativeClosestPoint<PointSource, PointTarget>::previous_transformation_;
00070     using IterativeClosestPoint<PointSource, PointTarget>::final_transformation_;
00071     using IterativeClosestPoint<PointSource, PointTarget>::transformation_;
00072     using IterativeClosestPoint<PointSource, PointTarget>::transformation_epsilon_;
00073     using IterativeClosestPoint<PointSource, PointTarget>::converged_;
00074     using IterativeClosestPoint<PointSource, PointTarget>::corr_dist_threshold_;
00075     using IterativeClosestPoint<PointSource, PointTarget>::inlier_threshold_;
00076     using IterativeClosestPoint<PointSource, PointTarget>::min_number_correspondences_;
00077     using IterativeClosestPoint<PointSource, PointTarget>::rigid_transformation_estimation_;
00078     using IterativeClosestPoint<PointSource, PointTarget>::update_visualizer_;
00079 
00080     typedef pcl::PointCloud<PointSource> PointCloudSource;
00081     typedef typename PointCloudSource::Ptr PointCloudSourcePtr;
00082     typedef typename PointCloudSource::ConstPtr PointCloudSourceConstPtr;
00083 
00084     typedef pcl::PointCloud<PointTarget> PointCloudTarget;
00085     typedef typename PointCloudTarget::Ptr PointCloudTargetPtr;
00086     typedef typename PointCloudTarget::ConstPtr PointCloudTargetConstPtr;
00087 
00088     typedef PointIndices::Ptr PointIndicesPtr;
00089     typedef PointIndices::ConstPtr PointIndicesConstPtr;
00090 
00091     typedef typename pcl::KdTree<PointSource> InputKdTree;
00092     typedef typename pcl::KdTree<PointSource>::Ptr InputKdTreePtr;
00093 
00094     public:
00096       GeneralizedIterativeClosestPoint () :
00097       k_correspondences_(20), 
00098       gicp_epsilon_(0.0004), rotation_epsilon_(2e-3),
00099       input_covariances_(0), target_covariances_(0), mahalanobis_(0)
00100       {
00101         min_number_correspondences_ = 4;
00102         reg_name_ = "GeneralizedIterativeClosestPoint";
00103         max_iterations_ = 200;
00104         transformation_epsilon_ = 5e-4;
00105         corr_dist_threshold_ = 5. * 5.;
00106         rigid_transformation_estimation_ = 
00107           boost::bind (&GeneralizedIterativeClosestPoint<PointSource, PointTarget>::estimateRigidTransformationLM, 
00108               this, _1, _2, _3, _4, _5); 
00109         input_tree_.reset (new pcl::KdTreeFLANN<PointSource>);
00110       }
00111 
00115       inline void
00116       setInputCloud (const PointCloudSourceConstPtr &cloud)
00117       {
00118         if (cloud->points.empty ())
00119         {
00120           PCL_ERROR ("[pcl::%s::setInputInput] Invalid or empty point cloud dataset given!\n", getClassName ().c_str ());
00121           return;
00122         }
00123         PointCloudSource input = *cloud;
00124         // Set all the point.data[3] values to 1 to aid the rigid transformation
00125         for (size_t i = 0; i < input.size (); ++i)
00126           input[i].data[3] = 1.0;
00127         
00128         input_ = input.makeShared ();
00129         input_tree_->setInputCloud (input_);
00130         input_covariances_.reserve (input_->size ());
00131       }
00132 
00136       inline void 
00137       setInputTarget (const PointCloudTargetConstPtr &target)
00138       {
00139         pcl::Registration<PointSource, PointTarget>::setInputTarget(target);
00140         target_covariances_.reserve (target_->size ());
00141       }
00142 
00149       void 
00150       estimateRigidTransformationLM (const PointCloudSource &cloud_src, 
00151                                      const PointCloudTarget &cloud_tgt, 
00152                                      Eigen::Matrix4f &transformation_matrix);
00153 
00162       void 
00163       estimateRigidTransformationLM (const PointCloudSource &cloud_src, 
00164                                      const std::vector<int> &indices_src, 
00165                                      const PointCloudTarget &cloud_tgt, 
00166                                      const std::vector<int> &indices_tgt, 
00167                                      Eigen::Matrix4f &transformation_matrix);
00168       
00170       inline const Eigen::Matrix3d& mahalanobis(size_t index) const
00171       {
00172         assert(index < mahalanobis_.size());
00173         return mahalanobis_[index];
00174       }
00175 
00183       void
00184       computeRDerivative(const double x[], const Eigen::Matrix3d &R, double g[]);
00185 
00191       inline void 
00192       setRotationEpsilon (double epsilon) { rotation_epsilon_ = epsilon; }
00193 
00197       inline double 
00198       getRotationEpsilon () { return (rotation_epsilon_); }
00199 
00206       void
00207       setCorrespondenceRandomness (int k) { k_correspondences_ = k; }
00208 
00212       void
00213       getCorrespondenceRandomness () { return (k_correspondences_); }
00214 
00215     private:
00216 
00220       int k_correspondences_;
00225       double gicp_epsilon_;
00230       double rotation_epsilon_;
00232       Eigen::Matrix4f base_transformation_;
00243       static int 
00244       functionToOptimize (void *p, int m, int n, const double *x, double *fvec, double *fjac, int ldfjac, int iflag);
00245 
00256       static int 
00257       functionToOptimizeIndices (void *p, int m, int n, const double *x, double *fvec, double *fjac, int ldfjac, int iflag);
00258 
00260       boost::mutex tmp_mutex_;
00261 
00263       const PointCloudSource *tmp_src_;
00264 
00266       const PointCloudTarget  *tmp_tgt_;
00267 
00269       const std::vector<int> *tmp_idx_src_;
00270 
00272       const std::vector<int> *tmp_idx_tgt_;
00273 
00275       InputKdTreePtr input_tree_;
00276       
00278       std::vector<Eigen::Matrix3d> input_covariances_;
00279 
00281       std::vector<Eigen::Matrix3d> target_covariances_;
00282 
00284       std::vector<Eigen::Matrix3d> mahalanobis_;
00285       
00292       template<typename PointT>
00293       void computeCovariances(typename pcl::PointCloud<PointT>::ConstPtr cloud, 
00294                               const typename pcl::KdTree<PointT>::Ptr tree,
00295                               std::vector<Eigen::Matrix3d>& cloud_covariances);
00296 
00301       inline double 
00302       matricesInnerProd(const Eigen::MatrixXd& mat1, const Eigen::MatrixXd& mat2)
00303       {
00304         double r = 0.;
00305         size_t n = mat1.rows();
00306         // tr(mat1^t.mat2)
00307         for(size_t i = 0; i < n; i++)
00308           for(size_t j = 0; j < n; j++)
00309             r += mat1 (j, i) * mat2 (i,j);
00310         return r;
00311       }
00312 
00316       void 
00317       computeTransformation (PointCloudSource &output)
00318       {
00319         computeTransformation (output, Eigen::Matrix4f::Identity());
00320       }
00321 
00326       void 
00327       computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess);
00328 
00334       inline bool 
00335       searchForNeighbors (const PointSource &query, std::vector<int>& index, std::vector<float>& distance)
00336       {
00337         int k = tree_->nearestKSearch (query, 1, index, distance);
00338         if (k == 0)
00339           return (false);
00340         return (true);
00341       }
00342 
00346       inline void 
00347       heteregenious_product(const Eigen::Matrix4f& mat1, const Eigen::Matrix4f& mat2, Eigen::Matrix4d& result)
00348       {
00349         result.setZero();
00350         for(size_t i = 0; i < 4; i++)
00351           for(size_t j = 0; j < 4; j++)
00352             for(size_t k = 0; k < 4; k++)
00353               result(i,j)+= double(mat1(i,k)) * double(mat2(k,j));
00354       }
00355 
00356       void apply_state(Eigen::Matrix4f &t, const double x[]);
00357       void get_translation(const Eigen::Matrix4f &t, double &x, double &y, double &z);
00358       void get_rotation(const Eigen::Matrix4f &t, double &rx, double &ry, double &rz);
00359   };
00360 }
00361 
00362 #include "pcl/registration/impl/gicp.hpp"
00363 
00364 #endif  //#ifndef PCL_GICP_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines