ndt_matcher_p2d.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, AASS Research Center, Orebro University.
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 AASS Research Center 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  */
00035 #ifndef NDT_MATCHER_HH
00036 #define NDT_MATCHER_HH
00037 
00038 #include "ndt_map/ndt_map.h"
00039 #include "pcl/point_cloud.h"
00040 #include "Eigen/Core"
00041 
00042 namespace lslgeneric
00043 {
00044 
00048 template <typename PointSource, typename PointTarget>
00049 class NDTMatcherP2D
00050 {
00051 public:
00052     NDTMatcherP2D(std::vector<double> _resolutions)
00053     {
00054         this->init(false,_resolutions);
00055     }
00056     NDTMatcherP2D()
00057     {
00058         this->init(true,std::vector<double>());
00059     }
00060     NDTMatcherP2D(const NDTMatcherP2D<PointSource,PointTarget>& other)
00061     {
00062         this->init(false,other.resolutions);
00063     }
00077     bool match( pcl::PointCloud<PointTarget>& target,
00078                 pcl::PointCloud<PointSource>& source,
00079                 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T );
00080 
00092     bool match( NDTMap<PointTarget>& target,
00093                 pcl::PointCloud<PointSource>& source,
00094                 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T );
00095 
00101     bool covariance( pcl::PointCloud<PointTarget>& target,
00102                      pcl::PointCloud<PointSource>& source,
00103                      Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T,
00104                      Eigen::Matrix<double,6,6> &cov
00105                    );
00106 
00107     //compute the score of a point cloud to an NDT
00108     double scorePointCloud(pcl::PointCloud<PointSource> &source,
00109                            NDTMap<PointTarget> &target);
00110 
00111     //compute the score gradient & hessian of a point cloud + transformation to an NDT
00112     // input: moving, fixed, tr, computeHessian
00113     //output: score_gradient, Hessian
00114     void derivativesPointCloud(pcl::PointCloud<PointSource> &source,
00115                                NDTMap<PointTarget> &target,
00116                                Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &transform,
00117                                Eigen::Matrix<double,6,1> &score_gradient,
00118                                Eigen::Matrix<double,6,6> &Hessian,
00119                                bool computeHessian);
00120 
00121     void generateScoreDebug(const char* out, pcl::PointCloud<PointTarget>& target,
00122                             pcl::PointCloud<PointSource>& source);
00123 
00124     double finalscore;
00125 private:
00126 
00127     Eigen::Matrix<double,3,6> Jest;
00128     Eigen::Matrix<double,18,6> Hest;
00129     //lf = likelihood function d1 and d2 from the paper
00130     double lfd1,lfd2;
00131     bool useSimpleDerivatives;
00132     double current_resolution;
00133     bool isIrregularGrid;
00134 
00135     //pre-computes the multipliers of the derivatives for all points
00136     void precomputeAngleDerivatives(Eigen::Vector3d &eulerAngles);
00137 
00138     //iteratively update the score gradient
00139     bool update_score_gradient(Eigen::Matrix<double,6,1> &score_gradient,
00140                                Eigen::Vector3d &transformed,
00141                                Eigen::Matrix3d &Cinv);
00142     //iteratively update the hessian matrix
00143     void update_hessian(Eigen::Matrix<double,6,6> &Hessian,
00144                         Eigen::Vector3d &transformed,
00145                         Eigen::Matrix3d &Cinv);
00146 
00147     //pre-computes the derivative matrices Jest and Hest
00148     void computeDerivatives(PointSource &pt);
00149 
00150     //perform line search to find the best descent rate (naive case)
00151     double lineSearch(double score_here,
00152                       Eigen::Matrix<double,6,1> &score_gradient,
00153                       Eigen::Matrix<double,6,1> &increment,
00154                       pcl::PointCloud<PointSource> &source,
00155                       NDTMap<PointTarget> &target) ;
00156 
00157     //perform line search to find the best descent rate (Mohre&Thuente)
00158     //adapted from NOX
00159     double lineSearchMT( Eigen::Matrix<double,6,1> &score_gradient_init,
00160                          Eigen::Matrix<double,6,1> &increment,
00161                          pcl::PointCloud<PointSource> &source,
00162                          Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &globalT,
00163                          NDTMap<PointTarget> &target) ;
00164 
00165     //compute finited difference derivative, for debug
00166     /*
00167         void fdd(pcl::PointCloud<PointSource> &source,
00168              NDTMap<PointTarget> &target,
00169              Eigen::Matrix<double,6,1> &score_gradient);
00170     */
00171 
00172     //auxiliary functions for MoreThuente line search
00173     struct MoreThuente
00174     {
00175         static double min(double a, double b);
00176         static double max(double a, double b);
00177         static double absmax(double a, double b, double c);
00178         static int cstep(double& stx, double& fx, double& dx,
00179                          double& sty, double& fy, double& dy,
00180                          double& stp, double& fp, double& dp,
00181                          bool& brackt, double stmin, double stmax);
00182     }; //end MoreThuente
00183 
00184     //perform a subsampling depending on user choice
00185     pcl::PointCloud<PointSource> subsample(pcl::PointCloud<PointSource>& original);
00186     int NUMBER_OF_POINTS;
00187     int NUMBER_OF_ACTIVE_CELLS;
00188 
00189 private:
00190     //storage for pre-computed angular derivatives
00191     Eigen::Vector3d jest13, jest23, jest04, jest14, jest24, jest05, jest15, jest25;
00192     Eigen::Vector3d a2,a3, b2,b3, c2,c3, d1,d2,d3, e1,e2,e3, f1,f2,f3;
00193 
00194     std::vector<double> resolutions;
00195     //initializes stuff;
00196     void init(bool useDefaultGridResolutions, std::vector<double> _resolutions);
00197     double normalizeAngle(double a);
00198 public:
00199     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00200 };
00201 
00202 } // end namespace
00203 
00204 #include <ndt_registration/impl/ndt_matcher_p2d.hpp>
00205 
00206 #endif


ndt_registration
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Mon Oct 6 2014 03:19:30