ndt_matcher_sequential_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_SEQUENTIAL_D2D_HH
00036 #define NDT_MATCHER_SEQUENTIAL_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 struct TransformParams {
00049     std::vector<Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>, 
00050                 Eigen::aligned_allocator<Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> > > fk, rg;
00051 };
00052 
00056 template <typename PointSource>
00057 class NDTMatcherSequentialD2D
00058 {
00059 public:
00060     NDTMatcherSequentialD2D(double _resolution)
00061     {
00062         current_resolution = _resolution;
00063         lfd1 = 1;
00064         lfd2 = 0.05;
00065         ITR_MAX = 500;
00066     }
00067     NDTMatcherSequentialD2D()
00068     {
00069         current_resolution = 0.5;
00070         lfd1 = 1;
00071         lfd2 = 0.05;
00072         ITR_MAX = 500;
00073     }
00074     NDTMatcherSequentialD2D(const NDTMatcherSequentialD2D& other)
00075     {
00076         current_resolution = other.current_resolution;
00077         lfd1 = 1;
00078         lfd2 = 0.05;
00079         ITR_MAX = 500;
00080     }
00081 
00082     bool add_cloud( pcl::PointCloud<PointSource>& cloud,
00083                     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& Tref);
00084     bool match_all(Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>& T_correction, bool useInitialGuess=false);
00085 
00086     //compute the score gradient & hessian of a point cloud + transformation to an NDT
00087     //input: moving, fixed, tr, computeHessian
00088     //output: score_gradient, Hessian, returns: score!
00089     virtual double derivativesNDT(
00090         const std::vector<NDTCell<PointSource>*> &source,
00091         const std::vector<NDTCell<PointSource>*> &target,
00092         Eigen::MatrixXd &score_gradient,
00093         Eigen::MatrixXd &Hessian,
00094         bool computeHessian
00095     );
00096     
00097     double current_resolution;
00098     int ITR_MAX;
00099 
00100     std::vector<Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>, 
00101                 Eigen::aligned_allocator<Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> > > transforms;
00102     std::vector<lslgeneric::NDTMap<PointSource>*, 
00103                 Eigen::aligned_allocator<lslgeneric::NDTMap<PointSource>* > > maps;
00104     std::vector<pcl::PointCloud<PointSource>, 
00105                 Eigen::aligned_allocator<pcl::PointCloud<PointSource>* > > pcs;
00106 protected:
00107 
00108     double lfd1,lfd2;
00109     int iteration_counter_internal;
00110 
00111 
00112     //iteratively update the score gradient and hessian
00113     virtual bool update_gradient_hessian_local(
00114         Eigen::MatrixXd &score_gradient,
00115         Eigen::MatrixXd &Hessian,
00116         const Eigen::Vector3d &m1,
00117         const Eigen::Matrix3d &C1,
00118         const double &likelihood,
00119         const Eigen::Matrix<double,3,6> &_Jest,
00120         const Eigen::Matrix<double,18,6> &_Hest,
00121         const Eigen::Matrix<double,3,18> &_Zest,
00122         const Eigen::Matrix<double,18,18> &_ZHest,
00123         bool computeHessian);
00124 
00125     //pre-computes the derivative matrices Jest, Hest, Zest, ZHest
00126     void computeDerivativesLocal(Eigen::Vector3d &m1, Eigen::Matrix3d C1,
00127                                  Eigen::Matrix<double,3,6> &_Jest,
00128                                  Eigen::Matrix<double,18,6> &_Hest,
00129                                  Eigen::Matrix<double,3,18> &_Zest,
00130                                  Eigen::Matrix<double,18,18> &_ZHest,
00131                                  bool computeHessian);
00132 
00133     //perform line search to find the best descent rate (Mohre&Thuente)
00134     //adapted from NOX
00135     double lineSearchMT(
00136         Eigen::Matrix<double,6,1> &increment,
00137         std::vector<NDTCell<PointSource>*> &source,
00138         std::vector<NDTCell<PointSource>*> &target);
00139 
00140     //auxiliary functions for MoreThuente line search
00141     struct MoreThuente
00142     {
00143         static double min(double a, double b);
00144         static double max(double a, double b);
00145         static double absmax(double a, double b, double c);
00146         static int cstep(double& stx, double& fx, double& dx,
00147                          double& sty, double& fy, double& dy,
00148                          double& stp, double& fp, double& dp,
00149                          bool& brackt, double stmin, double stmax);
00150     }; //end MoreThuente
00151 
00152     double normalizeAngle(double a);
00153 
00154 public:
00155     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00156 };
00157 
00158 } // end namespace
00159 
00160 #include <ndt_registration/impl/ndt_matcher_sequential_d2d.hpp>
00161 #endif


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