#include <ndt_matcher_sequential_d2d.h>
Classes | |
struct | MoreThuente |
Public Member Functions | |
bool | add_cloud (pcl::PointCloud< PointSource > &cloud, Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > &Tref) |
virtual double | derivativesNDT (const std::vector< NDTCell< PointSource > * > &source, const std::vector< NDTCell< PointSource > * > &target, Eigen::MatrixXd &score_gradient, Eigen::MatrixXd &Hessian, bool computeHessian) |
bool | match_all (Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > &T_correction, bool useInitialGuess=false) |
NDTMatcherSequentialD2D (double _resolution) | |
NDTMatcherSequentialD2D () | |
NDTMatcherSequentialD2D (const NDTMatcherSequentialD2D &other) | |
Public Attributes | |
double | current_resolution |
int | ITR_MAX |
std::vector < lslgeneric::NDTMap < PointSource > *, Eigen::aligned_allocator < lslgeneric::NDTMap < PointSource > * > > | maps |
std::vector< pcl::PointCloud < PointSource > , Eigen::aligned_allocator < pcl::PointCloud< PointSource > * > > | pcs |
std::vector< Eigen::Transform < double, 3, Eigen::Affine, Eigen::ColMajor > , Eigen::aligned_allocator < Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > > > | transforms |
Protected Member Functions | |
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) |
double | lineSearchMT (Eigen::Matrix< double, 6, 1 > &increment, std::vector< NDTCell< PointSource > * > &source, std::vector< NDTCell< PointSource > * > &target) |
double | normalizeAngle (double a) |
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 | |
int | iteration_counter_internal |
double | lfd1 |
double | lfd2 |
This class implements NDT registration of a sequence of 3D point clouds to a reference system.
Definition at line 57 of file ndt_matcher_sequential_d2d.h.
lslgeneric::NDTMatcherSequentialD2D< PointSource >::NDTMatcherSequentialD2D | ( | double | _resolution | ) | [inline] |
Definition at line 60 of file ndt_matcher_sequential_d2d.h.
lslgeneric::NDTMatcherSequentialD2D< PointSource >::NDTMatcherSequentialD2D | ( | ) | [inline] |
Definition at line 67 of file ndt_matcher_sequential_d2d.h.
lslgeneric::NDTMatcherSequentialD2D< PointSource >::NDTMatcherSequentialD2D | ( | const NDTMatcherSequentialD2D< PointSource > & | other | ) | [inline] |
Definition at line 74 of file ndt_matcher_sequential_d2d.h.
bool lslgeneric::NDTMatcherSequentialD2D< PointSource >::add_cloud | ( | pcl::PointCloud< PointSource > & | cloud, |
Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > & | Tref | ||
) |
Definition at line 120 of file ndt_matcher_sequential_d2d.hpp.
void lslgeneric::NDTMatcherSequentialD2D< PointSource >::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 622 of file ndt_matcher_sequential_d2d.hpp.
double lslgeneric::NDTMatcherSequentialD2D< PointSource >::derivativesNDT | ( | const std::vector< NDTCell< PointSource > * > & | source, |
const std::vector< NDTCell< PointSource > * > & | target, | ||
Eigen::MatrixXd & | score_gradient, | ||
Eigen::MatrixXd & | Hessian, | ||
bool | computeHessian | ||
) | [virtual] |
Definition at line 721 of file ndt_matcher_sequential_d2d.hpp.
double lslgeneric::NDTMatcherSequentialD2D< PointSource >::lineSearchMT | ( | Eigen::Matrix< double, 6, 1 > & | increment, |
std::vector< NDTCell< PointSource > * > & | source, | ||
std::vector< NDTCell< PointSource > * > & | target | ||
) | [protected] |
Definition at line 895 of file ndt_matcher_sequential_d2d.hpp.
bool lslgeneric::NDTMatcherSequentialD2D< PointSource >::match_all | ( | Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > & | T_correction, |
bool | useInitialGuess = false |
||
) |
Definition at line 134 of file ndt_matcher_sequential_d2d.hpp.
double lslgeneric::NDTMatcherSequentialD2D< PointSource >::normalizeAngle | ( | double | a | ) | [protected] |
Definition at line 1463 of file ndt_matcher_sequential_d2d.hpp.
bool lslgeneric::NDTMatcherSequentialD2D< PointSource >::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 559 of file ndt_matcher_sequential_d2d.hpp.
double lslgeneric::NDTMatcherSequentialD2D< PointSource >::current_resolution |
Definition at line 97 of file ndt_matcher_sequential_d2d.h.
int lslgeneric::NDTMatcherSequentialD2D< PointSource >::iteration_counter_internal [protected] |
Definition at line 109 of file ndt_matcher_sequential_d2d.h.
int lslgeneric::NDTMatcherSequentialD2D< PointSource >::ITR_MAX |
Definition at line 98 of file ndt_matcher_sequential_d2d.h.
double lslgeneric::NDTMatcherSequentialD2D< PointSource >::lfd1 [protected] |
Definition at line 108 of file ndt_matcher_sequential_d2d.h.
double lslgeneric::NDTMatcherSequentialD2D< PointSource >::lfd2 [protected] |
Definition at line 108 of file ndt_matcher_sequential_d2d.h.
std::vector<lslgeneric::NDTMap<PointSource>*, Eigen::aligned_allocator<lslgeneric::NDTMap<PointSource>* > > lslgeneric::NDTMatcherSequentialD2D< PointSource >::maps |
Definition at line 103 of file ndt_matcher_sequential_d2d.h.
std::vector<pcl::PointCloud<PointSource>, Eigen::aligned_allocator<pcl::PointCloud<PointSource>* > > lslgeneric::NDTMatcherSequentialD2D< PointSource >::pcs |
Definition at line 105 of file ndt_matcher_sequential_d2d.h.
std::vector<Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor>, Eigen::aligned_allocator<Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> > > lslgeneric::NDTMatcherSequentialD2D< PointSource >::transforms |
Definition at line 101 of file ndt_matcher_sequential_d2d.h.