00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #ifndef NDT_MATCHER_D2D_HH
00036 #define NDT_MATCHER_D2D_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
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
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
00141
00142
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
00152
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
00173 double lfd1,lfd2;
00174
00175 int iteration_counter_internal;
00176
00177 bool isIrregularGrid;
00178 std::vector<double> resolutions;
00179
00180
00181 void init(bool _isIrregularGrid,
00182 bool useDefaultGridResolutions, std::vector<double> _resolutions);
00183
00184
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
00194 void computeDerivatives(Eigen::Vector3d &m1, Eigen::Matrix3d C1, bool computeHessian=true);
00195
00196
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
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
00218
00219 double lineSearchMT(
00220 Eigen::Matrix<double,6,1> &increment,
00221 std::vector<NDTCell<PointSource>*> &source,
00222 NDTMap<PointTarget> &target) ;
00223
00224
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 };
00235
00236
00237 int NUMBER_OF_POINTS;
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248 protected:
00249
00250
00251
00252 int NUMBER_OF_ACTIVE_CELLS;
00253 double normalizeAngle(double a);
00254
00255
00256 Eigen::Matrix<double,6,1> xtBJ, xtBZBx, Q;
00257
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 }
00266
00267 #include <impl/ndt_matcher_d2d.hpp>
00268 #endif