#include <ndt_matcher_d2d.h>
Classes | |
struct | MoreThuente |
Public Member Functions | |
bool | covariance (pcl::PointCloud< PointTarget > &target, pcl::PointCloud< PointSource > &source, Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > &T, Eigen::MatrixXd &cov) |
bool | covariance (NDTMap< PointTarget > &target, NDTMap< PointSource > &source, Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > &T, Eigen::MatrixXd &cov) |
virtual double | derivativesNDT (const std::vector< NDTCell< PointSource > * > &source, const NDTMap< PointTarget > &target, Eigen::MatrixXd &score_gradient, Eigen::MatrixXd &Hessian, bool computeHessian) |
bool | match (pcl::PointCloud< PointTarget > &target, pcl::PointCloud< PointSource > &source, Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > &T, bool useInitialGuess=false) |
bool | match (NDTMap< PointTarget > &target, NDTMap< PointSource > &source, Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > &T, bool useInitialGuess=false) |
NDTMatcherD2D (bool _isIrregularGrid, bool useDefaultGridResolutions, std::vector< double > _resolutions) | |
NDTMatcherD2D () | |
NDTMatcherD2D (const NDTMatcherD2D &other) | |
virtual double | scoreNDT (std::vector< NDTCell< PointSource > * > &source, NDTMap< PointTarget > &target) |
virtual double | scoreNDT_OM (NDTMap< PointSource > &source, NDTMap< PointTarget > &target) |
virtual double | scoreNDTPositive (std::vector< NDTCell< PointSource > * > &sourceNDT, NDTMap< PointTarget > &targetNDT, Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > &T) |
Public Attributes | |
double | current_resolution |
double | DELTA_SCORE |
the change in score after which we converge. Set to 1e-3 in constructor | |
double | finalscore |
int | ITR_MAX |
max iterations, set in constructor | |
bool | step_control |
sets step control on/off. set to true in constructor | |
Protected Member Functions | |
void | computeDerivatives (Eigen::Vector3d &m1, Eigen::Matrix3d C1, bool computeHessian=true) |
void | computeDerivativesLocal (Eigen::Vector3d &m1, Eigen::Matrix3d C1, Eigen::Matrix< double, 3, 6 > &_Jest, Eigen::Matrix< double, 18, 6 > &_Hest, Eigen::Matrix< double, 3, 18 > &_Zest, Eigen::Matrix< double, 18, 18 > &_ZHest, bool computeHessian) |
void | init (bool _isIrregularGrid, bool useDefaultGridResolutions, std::vector< double > _resolutions) |
double | lineSearchMT (Eigen::Matrix< double, 6, 1 > &increment, std::vector< NDTCell< PointSource > * > &source, NDTMap< PointTarget > &target) |
double | normalizeAngle (double a) |
virtual bool | update_gradient_hessian (Eigen::MatrixXd &score_gradient, Eigen::MatrixXd &Hessian, const Eigen::Vector3d &m1, const Eigen::Matrix3d &C1, const double &likelihood, bool computeHessian) |
virtual bool | update_gradient_hessian_local (Eigen::MatrixXd &score_gradient, Eigen::MatrixXd &Hessian, const Eigen::Vector3d &m1, const Eigen::Matrix3d &C1, const double &likelihood, const Eigen::Matrix< double, 3, 6 > &_Jest, const Eigen::Matrix< double, 18, 6 > &_Hest, const Eigen::Matrix< double, 3, 18 > &_Zest, const Eigen::Matrix< double, 18, 18 > &_ZHest, bool computeHessian) |
Protected Attributes | |
Eigen::Matrix< double, 3, 3 > | dRdx |
Eigen::Matrix< double, 3, 3 > | dRdxdx |
Eigen::Matrix< double, 3, 3 > | dRdxdy |
Eigen::Matrix< double, 3, 3 > | dRdxdz |
Eigen::Matrix< double, 3, 3 > | dRdy |
Eigen::Matrix< double, 3, 3 > | dRdydy |
Eigen::Matrix< double, 3, 3 > | dRdydz |
Eigen::Matrix< double, 3, 3 > | dRdz |
Eigen::Matrix< double, 3, 3 > | dRdzdz |
Eigen::Matrix< double, 18, 6 > | Hest |
bool | isIrregularGrid |
int | iteration_counter_internal |
Eigen::Matrix< double, 3, 6 > | Jest |
Eigen::Matrix< double, 6, 6 > | JtBJ |
double | lfd1 |
double | lfd2 |
int | NUMBER_OF_ACTIVE_CELLS |
int | NUMBER_OF_POINTS |
Eigen::Matrix< double, 6, 1 > | Q |
std::vector< double > | resolutions |
Eigen::Matrix< double, 1, 3 > | TMP1 |
Eigen::Matrix< double, 1, 3 > | xtB |
Eigen::Matrix< double, 6, 6 > | xtBH |
Eigen::Matrix< double, 6, 1 > | xtBJ |
Eigen::Matrix< double, 6, 6 > | xtBZBJ |
Eigen::Matrix< double, 6, 1 > | xtBZBx |
Eigen::Matrix< double, 6, 6 > | xtBZBZBx |
Eigen::Matrix< double, 6, 6 > | xtBZhBx |
Eigen::Matrix< double, 3, 18 > | Zest |
Eigen::Matrix< double, 18, 18 > | ZHest |
This class implements NDT registration for 3D point cloud scans.
Definition at line 53 of file ndt_matcher_d2d.h.
lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::NDTMatcherD2D | ( | bool | _isIrregularGrid, |
bool | useDefaultGridResolutions, | ||
std::vector< double > | _resolutions | ||
) | [inline] |
parametrized constructor. A default set is (false,false,true,empty_vector). parameters are:
_isIrregularGrid | --- experimental single pass through an irregular grid. also unstable |
useDefaultGridResolutions | --- if set, the following parameter is set to a preset value |
_resolutions | --- if previous bool is not set, these are the resolutions (in reverse order) that we will go through |
Definition at line 62 of file ndt_matcher_d2d.h.
lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::NDTMatcherD2D | ( | ) | [inline] |
Definition at line 67 of file ndt_matcher_d2d.h.
lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::NDTMatcherD2D | ( | const NDTMatcherD2D< PointSource, PointTarget > & | other | ) | [inline] |
Definition at line 71 of file ndt_matcher_d2d.h.
void lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::computeDerivatives | ( | Eigen::Vector3d & | m1, |
Eigen::Matrix3d | C1, | ||
bool | computeHessian = true |
||
) | [protected] |
Definition at line 1090 of file ndt_matcher_d2d.hpp.
void lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::computeDerivativesLocal | ( | Eigen::Vector3d & | m1, |
Eigen::Matrix3d | C1, | ||
Eigen::Matrix< double, 3, 6 > & | _Jest, | ||
Eigen::Matrix< double, 18, 6 > & | _Hest, | ||
Eigen::Matrix< double, 3, 18 > & | _Zest, | ||
Eigen::Matrix< double, 18, 18 > & | _ZHest, | ||
bool | computeHessian | ||
) | [protected] |
Definition at line 993 of file ndt_matcher_d2d.hpp.
bool lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::covariance | ( | pcl::PointCloud< PointTarget > & | target, |
pcl::PointCloud< PointSource > & | source, | ||
Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > & | T, | ||
Eigen::MatrixXd & | cov | ||
) |
computes the covariance of the match between moving and fixed, at T. note --- computes NDT distributions based on the resolution in res result is returned in cov
Definition at line 2542 of file ndt_matcher_d2d.hpp.
bool lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::covariance | ( | NDTMap< PointTarget > & | target, |
NDTMap< PointSource > & | source, | ||
Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > & | T, | ||
Eigen::MatrixXd & | cov | ||
) |
computes the covariance of the match between moving and fixed, at T. result is returned in cov
Definition at line 2412 of file ndt_matcher_d2d.hpp.
double lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::derivativesNDT | ( | const std::vector< NDTCell< PointSource > * > & | source, |
const NDTMap< PointTarget > & | target, | ||
Eigen::MatrixXd & | score_gradient, | ||
Eigen::MatrixXd & | Hessian, | ||
bool | computeHessian | ||
) | [virtual] |
Reimplemented in lslgeneric::NDTMatcherFeatureD2D< PointSource, PointTarget >.
Definition at line 1439 of file ndt_matcher_d2d.hpp.
void lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::init | ( | bool | _isIrregularGrid, |
bool | useDefaultGridResolutions, | ||
std::vector< double > | _resolutions | ||
) | [protected] |
Definition at line 15 of file ndt_matcher_d2d.hpp.
double lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::lineSearchMT | ( | Eigen::Matrix< double, 6, 1 > & | increment, |
std::vector< NDTCell< PointSource > * > & | source, | ||
NDTMap< PointTarget > & | target | ||
) | [protected] |
Definition at line 1680 of file ndt_matcher_d2d.hpp.
bool lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::match | ( | pcl::PointCloud< PointTarget > & | target, |
pcl::PointCloud< PointSource > & | source, | ||
Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > & | T, | ||
bool | useInitialGuess = false |
||
) |
Register two point clouds. This method builds an NDT representation of the "fixed" point cloud and uses that for registering the "moving" point cloud.
fixed | Reference data. NDT structure is built for this point cloud. |
moving | The output transformation registers this point cloud to fixed . |
T | This is an input/output parameter. The initial value of T gives the initial pose estimate of moving . When the algorithm terminates, T holds the registration result. |
Definition at line 47 of file ndt_matcher_d2d.hpp.
bool lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::match | ( | NDTMap< PointTarget > & | target, |
NDTMap< PointSource > & | source, | ||
Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > & | T, | ||
bool | useInitialGuess = false |
||
) |
Registers a point cloud to an NDT structure.
fixed | Reference data. |
moving | The output transformation registers this point cloud to fixed . |
T | This is an input/output parameter. The initial value of T gives the initial pose estimate of moving . When the algorithm terminates, T holds the registration result. |
Definition at line 269 of file ndt_matcher_d2d.hpp.
double lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::normalizeAngle | ( | double | a | ) | [protected] |
Definition at line 2248 of file ndt_matcher_d2d.hpp.
double lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::scoreNDT | ( | std::vector< NDTCell< PointSource > * > & | source, |
NDTMap< PointTarget > & | target | ||
) | [virtual] |
Definition at line 1182 of file ndt_matcher_d2d.hpp.
double lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::scoreNDT_OM | ( | NDTMap< PointSource > & | source, |
NDTMap< PointTarget > & | target | ||
) | [virtual] |
Definition at line 1243 of file ndt_matcher_d2d.hpp.
double lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::scoreNDTPositive | ( | std::vector< NDTCell< PointSource > * > & | sourceNDT, |
NDTMap< PointTarget > & | targetNDT, | ||
Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > & | T | ||
) | [virtual] |
Added by Jari - Uses 1-p as a score instead of -p
Definition at line 1356 of file ndt_matcher_d2d.hpp.
bool lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::update_gradient_hessian | ( | Eigen::MatrixXd & | score_gradient, |
Eigen::MatrixXd & | Hessian, | ||
const Eigen::Vector3d & | m1, | ||
const Eigen::Matrix3d & | C1, | ||
const double & | likelihood, | ||
bool | computeHessian | ||
) | [protected, virtual] |
Definition at line 903 of file ndt_matcher_d2d.hpp.
bool lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::update_gradient_hessian_local | ( | Eigen::MatrixXd & | score_gradient, |
Eigen::MatrixXd & | Hessian, | ||
const Eigen::Vector3d & | m1, | ||
const Eigen::Matrix3d & | C1, | ||
const double & | likelihood, | ||
const Eigen::Matrix< double, 3, 6 > & | _Jest, | ||
const Eigen::Matrix< double, 18, 6 > & | _Hest, | ||
const Eigen::Matrix< double, 3, 18 > & | _Zest, | ||
const Eigen::Matrix< double, 18, 18 > & | _ZHest, | ||
bool | computeHessian | ||
) | [protected, virtual] |
Definition at line 841 of file ndt_matcher_d2d.hpp.
double lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::current_resolution |
Definition at line 155 of file ndt_matcher_d2d.h.
double lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::DELTA_SCORE |
the change in score after which we converge. Set to 1e-3 in constructor
Definition at line 161 of file ndt_matcher_d2d.h.
Eigen::Matrix<double,3,3> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::dRdx [protected] |
Definition at line 170 of file ndt_matcher_d2d.h.
Eigen::Matrix<double,3,3> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::dRdxdx [protected] |
Definition at line 171 of file ndt_matcher_d2d.h.
Eigen::Matrix<double,3,3> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::dRdxdy [protected] |
Definition at line 171 of file ndt_matcher_d2d.h.
Eigen::Matrix<double,3,3> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::dRdxdz [protected] |
Definition at line 171 of file ndt_matcher_d2d.h.
Eigen::Matrix<double,3,3> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::dRdy [protected] |
Definition at line 170 of file ndt_matcher_d2d.h.
Eigen::Matrix<double,3,3> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::dRdydy [protected] |
Definition at line 171 of file ndt_matcher_d2d.h.
Eigen::Matrix<double,3,3> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::dRdydz [protected] |
Definition at line 171 of file ndt_matcher_d2d.h.
Eigen::Matrix<double,3,3> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::dRdz [protected] |
Definition at line 170 of file ndt_matcher_d2d.h.
Eigen::Matrix<double,3,3> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::dRdzdz [protected] |
Definition at line 171 of file ndt_matcher_d2d.h.
double lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::finalscore |
Definition at line 154 of file ndt_matcher_d2d.h.
Eigen::Matrix<double,18,6> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::Hest [protected] |
Definition at line 166 of file ndt_matcher_d2d.h.
bool lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::isIrregularGrid [protected] |
Definition at line 177 of file ndt_matcher_d2d.h.
int lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::iteration_counter_internal [protected] |
Definition at line 175 of file ndt_matcher_d2d.h.
int lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::ITR_MAX |
max iterations, set in constructor
Definition at line 157 of file ndt_matcher_d2d.h.
Eigen::Matrix<double,3,6> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::Jest [protected] |
Definition at line 165 of file ndt_matcher_d2d.h.
Eigen::Matrix<double,6,6> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::JtBJ [protected] |
Definition at line 258 of file ndt_matcher_d2d.h.
double lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::lfd1 [protected] |
Definition at line 173 of file ndt_matcher_d2d.h.
double lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::lfd2 [protected] |
Definition at line 173 of file ndt_matcher_d2d.h.
int lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::NUMBER_OF_ACTIVE_CELLS [protected] |
Definition at line 252 of file ndt_matcher_d2d.h.
int lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::NUMBER_OF_POINTS [protected] |
Definition at line 237 of file ndt_matcher_d2d.h.
Eigen::Matrix<double,6,1> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::Q [protected] |
Definition at line 256 of file ndt_matcher_d2d.h.
std::vector<double> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::resolutions [protected] |
Definition at line 178 of file ndt_matcher_d2d.h.
bool lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::step_control |
sets step control on/off. set to true in constructor
Definition at line 159 of file ndt_matcher_d2d.h.
Eigen::Matrix<double,1,3> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::TMP1 [protected] |
Definition at line 259 of file ndt_matcher_d2d.h.
Eigen::Matrix<double,1,3> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::xtB [protected] |
Definition at line 259 of file ndt_matcher_d2d.h.
Eigen::Matrix<double,6,6> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::xtBH [protected] |
Definition at line 258 of file ndt_matcher_d2d.h.
Eigen::Matrix<double,6,1> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::xtBJ [protected] |
Definition at line 256 of file ndt_matcher_d2d.h.
Eigen::Matrix<double,6,6> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::xtBZBJ [protected] |
Definition at line 258 of file ndt_matcher_d2d.h.
Eigen::Matrix<double,6,1> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::xtBZBx [protected] |
Definition at line 256 of file ndt_matcher_d2d.h.
Eigen::Matrix<double,6,6> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::xtBZBZBx [protected] |
Definition at line 258 of file ndt_matcher_d2d.h.
Eigen::Matrix<double,6,6> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::xtBZhBx [protected] |
Definition at line 258 of file ndt_matcher_d2d.h.
Eigen::Matrix<double,3,18> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::Zest [protected] |
Definition at line 167 of file ndt_matcher_d2d.h.
Eigen::Matrix<double,18,18> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::ZHest [protected] |
Definition at line 168 of file ndt_matcher_d2d.h.