ndt_matcher_d2d.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_D2D_HH
00036 #define NDT_MATCHER_D2D_HH
00037 
00038 #include "ndt_map/ndt_map.h"
00039 #include "pcl/point_cloud.h"
00040 #include "Eigen/Core"
00041 
00042 #include <stdlib.h>
00043 #include <stdio.h>
00044 #include <math.h>
00045 
00046 namespace lslgeneric
00047 {
00048 
00052 template <typename PointSource, typename PointTarget>
00053 class NDTMatcherD2D
00054 {
00055 public:
00062     NDTMatcherD2D(bool _isIrregularGrid,
00063                   bool useDefaultGridResolutions, std::vector<double> _resolutions)
00064     {
00065         this->init(_isIrregularGrid,useDefaultGridResolutions,_resolutions);
00066     }
00067     NDTMatcherD2D()
00068     {
00069         this->init(false,true,std::vector<double>());
00070     }
00071     NDTMatcherD2D(const NDTMatcherD2D& other)
00072     {
00073         this->init(other.isIrregularGrid,false,other.resolutions);
00074     }
00075 
00089     bool match( pcl::PointCloud<PointTarget>& target,
00090                 pcl::PointCloud<PointSource>& source,
00091                 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T,
00092                 bool useInitialGuess = false);
00093 
00105     bool match( NDTMap<PointTarget>& target,
00106                 NDTMap<PointSource>& source,
00107                 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T,
00108                 bool useInitialGuess = false);
00109 
00115     bool covariance( pcl::PointCloud<PointTarget>& target,
00116                      pcl::PointCloud<PointSource>& source,
00117                      Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T,
00118                      Eigen::MatrixXd &cov
00119                    );
00120 
00125     bool covariance( NDTMap<PointTarget>& target,
00126                      NDTMap<PointSource>& source,
00127                      Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T,
00128                      Eigen::MatrixXd &cov
00129                    );
00130 
00131     //compute the score of a point cloud to an NDT //UNUSED
00132     virtual double scoreNDT(std::vector<NDTCell<PointSource>*> &source, NDTMap<PointTarget> &target);
00133 
00134     virtual double scoreNDT_OM(NDTMap<PointSource> &source, NDTMap<PointTarget> &target);
00135 
00136 
00137     virtual double scoreNDTPositive(std::vector<NDTCell<PointSource>*> &sourceNDT, NDTMap<PointTarget> &targetNDT,
00138                                     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T);
00139 
00140     //compute the score gradient & hessian of a point cloud + transformation to an NDT
00141     // input: moving, fixed, tr, computeHessian
00142     //output: score_gradient, Hessian, returns: score!
00143     virtual double derivativesNDT(
00144         const std::vector<NDTCell<PointSource>*> &source,
00145         const NDTMap<PointTarget> &target,
00146         Eigen::MatrixXd &score_gradient,
00147         Eigen::MatrixXd &Hessian,
00148         bool computeHessian
00149     );
00150     /*
00151         void generateScoreDebug(const char* out, pcl::PointCloud<pcl::PointXYZ>& fixed,
00152                             pcl::PointCloud<pcl::PointXYZ>& moving);
00153     */
00154     double finalscore;
00155     double current_resolution;
00157     int ITR_MAX;
00159     bool step_control;
00161     double DELTA_SCORE;
00162 
00163 protected:
00164 
00165     Eigen::Matrix<double,3,6> Jest;
00166     Eigen::Matrix<double,18,6> Hest;
00167     Eigen::Matrix<double,3,18> Zest;
00168     Eigen::Matrix<double,18,18> ZHest;
00169 
00170     Eigen::Matrix<double,3,3> dRdx, dRdy, dRdz;
00171     Eigen::Matrix<double,3,3> dRdxdx, dRdxdy, dRdxdz, dRdydy, dRdydz, dRdzdz;
00172     //lf = likelihood function d1 and d2 from the paper
00173     double lfd1,lfd2;
00174     //bool useSimpleDerivatives;
00175     int iteration_counter_internal;
00176 
00177     bool isIrregularGrid;
00178     std::vector<double> resolutions;
00179 
00180     //initializes stuff;
00181     void init(bool _isIrregularGrid,
00182               bool useDefaultGridResolutions, std::vector<double> _resolutions);
00183 
00184     //iteratively update the score gradient and hessian
00185     virtual bool update_gradient_hessian(
00186         Eigen::MatrixXd &score_gradient,
00187         Eigen::MatrixXd &Hessian,
00188         const Eigen::Vector3d &m1,
00189         const Eigen::Matrix3d &C1,
00190         const double &likelihood,
00191         bool computeHessian);
00192 
00193     //pre-computes the derivative matrices Jest, Hest, Zest, ZHest
00194     void computeDerivatives(Eigen::Vector3d &m1, Eigen::Matrix3d C1, bool computeHessian=true);
00195 
00196     //iteratively update the score gradient and hessian
00197     virtual bool update_gradient_hessian_local(
00198         Eigen::MatrixXd &score_gradient,
00199         Eigen::MatrixXd &Hessian,
00200         const Eigen::Vector3d &m1,
00201         const Eigen::Matrix3d &C1,
00202         const double &likelihood,
00203         const Eigen::Matrix<double,3,6> &_Jest,
00204         const Eigen::Matrix<double,18,6> &_Hest,
00205         const Eigen::Matrix<double,3,18> &_Zest,
00206         const Eigen::Matrix<double,18,18> &_ZHest,
00207         bool computeHessian);
00208 
00209     //pre-computes the derivative matrices Jest, Hest, Zest, ZHest
00210     void computeDerivativesLocal(Eigen::Vector3d &m1, Eigen::Matrix3d C1,
00211                                  Eigen::Matrix<double,3,6> &_Jest,
00212                                  Eigen::Matrix<double,18,6> &_Hest,
00213                                  Eigen::Matrix<double,3,18> &_Zest,
00214                                  Eigen::Matrix<double,18,18> &_ZHest,
00215                                  bool computeHessian);
00216     
00217     //perform line search to find the best descent rate (Mohre&Thuente)
00218     //adapted from NOX
00219     double lineSearchMT(
00220         Eigen::Matrix<double,6,1> &increment,
00221         std::vector<NDTCell<PointSource>*> &source,
00222         NDTMap<PointTarget> &target) ;
00223 
00224     //auxiliary functions for MoreThuente line search
00225     struct MoreThuente
00226     {
00227         static double min(double a, double b);
00228         static double max(double a, double b);
00229         static double absmax(double a, double b, double c);
00230         static int cstep(double& stx, double& fx, double& dx,
00231                          double& sty, double& fy, double& dy,
00232                          double& stp, double& fp, double& dp,
00233                          bool& brackt, double stmin, double stmax);
00234     }; //end MoreThuente
00235 
00236     //perform a subsampling depending on user choice
00237     int NUMBER_OF_POINTS;
00238     /*
00239     void gradient_numeric(
00240         std::vector<NDTCell*> &moving,
00241         NDTMap &fixed,
00242         Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &transform,
00243         //Eigen::Vector3d &eulerAngles,
00244         Eigen::Matrix<double,6,1> &score_gradient
00245         );
00246     */
00247 
00248 protected:
00249     //storage for pre-computed angular derivatives
00250     //Eigen::Vector3d jest13, jest23, jest04, jest14, jest24, jest05, jest15, jest25;
00251     //Eigen::Vector3d a2,a3, b2,b3, c2,c3, d1,d2,d3, e1,e2,e3, f1,f2,f3;
00252     int NUMBER_OF_ACTIVE_CELLS;
00253     double normalizeAngle(double a);
00254 
00255     //vars for gradient
00256     Eigen::Matrix<double,6,1> xtBJ, xtBZBx, Q;
00257     //vars for hessian
00258     Eigen::Matrix<double,6,6> JtBJ, xtBZBJ, xtBH, xtBZBZBx, xtBZhBx;
00259     Eigen::Matrix<double,1,3> TMP1, xtB;
00260 
00261 public:
00262     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00263 };
00264 
00265 } // end namespace
00266 
00267 #include <ndt_registration/impl/ndt_matcher_d2d.hpp>
00268 #endif


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