ndt_matcher_d2d_2d.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_2D_HH
00036 #define NDT_MATCHER_D2D_2D_HH
00037 
00038 #include "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_2D
00054 {
00055 public:
00062     NDTMatcherD2D_2D(bool _isIrregularGrid,
00063                   bool useDefaultGridResolutions, std::vector<double> _resolutions)
00064     {
00065         this->init(_isIrregularGrid,useDefaultGridResolutions,_resolutions);
00066     }
00067     NDTMatcherD2D_2D()
00068     {
00069         this->init(false,true,std::vector<double>());
00070     }
00071     NDTMatcherD2D_2D(const NDTMatcherD2D_2D& 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 
00110     //compute the score gradient & hessian of a point cloud + transformation to an NDT (assuming a 2D transformation)
00111     // input: moving, fixed, tr, computeHessian
00112     //output: score_gradient, Hessian, returns: score!
00113     virtual double derivativesNDT_2d(
00114         const std::vector<NDTCell<PointSource>*> &source,
00115         const NDTMap<PointTarget> &target,
00116         Eigen::MatrixXd &score_gradient,
00117         Eigen::MatrixXd &Hessian,
00118         bool computeHessian
00119     );
00120     double finalscore;
00121     double current_resolution;
00122     
00124     int ITR_MAX;
00126     bool step_control;
00128     double DELTA_SCORE;
00129 protected:
00130 
00131     Eigen::Matrix<double,3,3> Jest;
00132     Eigen::Matrix<double,9,3> Hest;
00133     Eigen::Matrix<double,3,9> Zest;
00134     Eigen::Matrix<double,9,9> ZHest;
00135     //vars for gradient
00136     Eigen::Matrix<double,3,1> xtBJ, xtBZBx, Q;
00137     //vars for hessian
00138     Eigen::Matrix<double,3,3> JtBJ, xtBZBJ, xtBH, xtBZBZBx, xtBZhBx;
00139     Eigen::Matrix<double,1,3> TMP1, xtB;
00140 
00141     int NUMBER_OF_ACTIVE_CELLS;
00142     int NUMBER_OF_POINTS;
00143     double lfd1,lfd2;
00144     int iteration_counter_internal;
00145     bool isIrregularGrid;
00146     std::vector<double> resolutions;
00147 
00148     double normalizeAngle(double a);
00149     //initializes stuff;
00150     void init(bool _isIrregularGrid,
00151               bool useDefaultGridResolutions, std::vector<double> _resolutions);
00152 
00153     //iteratively update the score gradient and hessian
00154     virtual bool update_gradient_hessian_2d(
00155         Eigen::MatrixXd &score_gradient,
00156         Eigen::MatrixXd &Hessian,
00157         const Eigen::Vector3d &m1,
00158         const Eigen::Matrix3d &C1,
00159         const double &likelihood,
00160         bool computeHessian);
00161 
00162     //pre-computes the derivative matrices Jest, Hest, Zest, ZHest
00163     void computeDerivatives_2d(Eigen::Vector3d &m1, Eigen::Matrix3d C1, bool computeHessian=true);
00164 
00165     //iteratively update the score gradient and hessian (2d version)
00166     virtual bool update_gradient_hessian_local_2d(
00167         Eigen::MatrixXd &score_gradient,
00168         Eigen::MatrixXd &Hessian,
00169         const Eigen::Vector3d &m1,
00170         const Eigen::Matrix3d &C1,
00171         const double &likelihood,
00172         const Eigen::Matrix<double,3,3> &_Jest,
00173         const Eigen::Matrix<double,9,3> &_Hest,
00174         const Eigen::Matrix<double,3,9> &_Zest,
00175         const Eigen::Matrix<double,9,9> &_ZHest,
00176         bool computeHessian);
00177 
00178     //pre-computes the derivative matrices Jest, Hest, Zest, ZHest
00179     void computeDerivativesLocal_2d(Eigen::Vector3d &m1, Eigen::Matrix3d C1,
00180                                  Eigen::Matrix<double,3,3> &_Jest,
00181                                  Eigen::Matrix<double,9,3> &_Hest,
00182                                  Eigen::Matrix<double,3,9> &_Zest,
00183                                  Eigen::Matrix<double,9,9> &_ZHest,
00184                                  bool computeHessian);
00185 
00186     //perform line search to find the best descent rate (Mohre&Thuente)
00187     //adapted from NOX
00188     double lineSearch2D(
00189         Eigen::Matrix<double,3,1> &increment,
00190         std::vector<NDTCell<PointSource>*> &source,
00191         NDTMap<PointTarget> &target) ;
00192 
00193     //auxiliary functions for MoreThuente line search
00194     struct MoreThuente
00195     {
00196         static double min(double a, double b);
00197         static double max(double a, double b);
00198         static double absmax(double a, double b, double c);
00199         static int cstep(double& stx, double& fx, double& dx,
00200                          double& sty, double& fy, double& dy,
00201                          double& stp, double& fp, double& dp,
00202                          bool& brackt, double stmin, double stmax);
00203     }; //end MoreThuente
00204     //perform a subsampling depending on user choice
00205 
00206 public:
00207     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00208 };
00209 
00210 } // end namespace
00211 
00212 #include <impl/ndt_matcher_d2d_2d.hpp>
00213 #endif


ndt_registration
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Mon Jan 6 2014 11:32:03