Classes | Public Member Functions | Public Attributes | Protected Member Functions | Protected Attributes
lslgeneric::NDTMatcherD2D< PointSource, PointTarget > Class Template Reference

#include <ndt_matcher_d2d.h>

Inheritance diagram for lslgeneric::NDTMatcherD2D< PointSource, PointTarget >:
Inheritance graph
[legend]

List of all members.

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

Detailed Description

template<typename PointSource, typename PointTarget>
class lslgeneric::NDTMatcherD2D< PointSource, PointTarget >

This class implements NDT registration for 3D point cloud scans.

Definition at line 53 of file ndt_matcher_d2d.h.


Constructor & Destructor Documentation

template<typename PointSource, typename PointTarget>
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:

Parameters:
_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.

template<typename PointSource, typename PointTarget>
lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::NDTMatcherD2D ( ) [inline]

Definition at line 67 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::NDTMatcherD2D ( const NDTMatcherD2D< PointSource, PointTarget > &  other) [inline]

Definition at line 71 of file ndt_matcher_d2d.h.


Member Function Documentation

template<typename PointSource , typename PointTarget >
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.

template<typename PointSource , typename PointTarget >
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.

template<typename PointSource , typename PointTarget >
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.

template<typename PointSource , typename PointTarget >
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.

template<typename PointSource , typename PointTarget >
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]
template<typename PointSource , typename PointTarget >
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.

template<typename PointSource , typename PointTarget >
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.

template<typename PointSource , typename PointTarget >
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.

Parameters:
fixedReference data. NDT structure is built for this point cloud.
movingThe output transformation registers this point cloud to fixed.
TThis 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.

template<typename PointSource , typename PointTarget >
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.

Parameters:
fixedReference data.
movingThe output transformation registers this point cloud to fixed.
TThis 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.

template<typename PointSource , typename PointTarget >
double lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::normalizeAngle ( double  a) [protected]

Definition at line 2248 of file ndt_matcher_d2d.hpp.

template<typename PointSource , typename PointTarget >
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.

template<typename PointSource , typename PointTarget >
double lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::scoreNDT_OM ( NDTMap< PointSource > &  source,
NDTMap< PointTarget > &  target 
) [virtual]

Definition at line 1243 of file ndt_matcher_d2d.hpp.

template<typename PointSource , typename PointTarget >
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.

template<typename PointSource , typename PointTarget >
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.

template<typename PointSource , typename PointTarget >
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.


Member Data Documentation

template<typename PointSource, typename PointTarget>
double lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::current_resolution

Definition at line 155 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
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.

template<typename PointSource, typename PointTarget>
Eigen::Matrix<double,3,3> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::dRdx [protected]

Definition at line 170 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
Eigen::Matrix<double,3,3> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::dRdxdx [protected]

Definition at line 171 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
Eigen::Matrix<double,3,3> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::dRdxdy [protected]

Definition at line 171 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
Eigen::Matrix<double,3,3> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::dRdxdz [protected]

Definition at line 171 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
Eigen::Matrix<double,3,3> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::dRdy [protected]

Definition at line 170 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
Eigen::Matrix<double,3,3> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::dRdydy [protected]

Definition at line 171 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
Eigen::Matrix<double,3,3> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::dRdydz [protected]

Definition at line 171 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
Eigen::Matrix<double,3,3> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::dRdz [protected]

Definition at line 170 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
Eigen::Matrix<double,3,3> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::dRdzdz [protected]

Definition at line 171 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
double lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::finalscore

Definition at line 154 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
Eigen::Matrix<double,18,6> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::Hest [protected]

Definition at line 166 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
bool lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::isIrregularGrid [protected]

Definition at line 177 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
int lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::iteration_counter_internal [protected]

Definition at line 175 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
int lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::ITR_MAX

max iterations, set in constructor

Definition at line 157 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
Eigen::Matrix<double,3,6> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::Jest [protected]

Definition at line 165 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
Eigen::Matrix<double,6,6> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::JtBJ [protected]

Definition at line 258 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
double lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::lfd1 [protected]

Definition at line 173 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
double lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::lfd2 [protected]

Definition at line 173 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
int lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::NUMBER_OF_ACTIVE_CELLS [protected]

Definition at line 252 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
int lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::NUMBER_OF_POINTS [protected]

Definition at line 237 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
Eigen::Matrix<double,6,1> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::Q [protected]

Definition at line 256 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
std::vector<double> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::resolutions [protected]

Definition at line 178 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
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.

template<typename PointSource, typename PointTarget>
Eigen::Matrix<double,1,3> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::TMP1 [protected]

Definition at line 259 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
Eigen::Matrix<double,1,3> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::xtB [protected]

Definition at line 259 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
Eigen::Matrix<double,6,6> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::xtBH [protected]

Definition at line 258 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
Eigen::Matrix<double,6,1> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::xtBJ [protected]

Definition at line 256 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
Eigen::Matrix<double,6,6> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::xtBZBJ [protected]

Definition at line 258 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
Eigen::Matrix<double,6,1> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::xtBZBx [protected]

Definition at line 256 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
Eigen::Matrix<double,6,6> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::xtBZBZBx [protected]

Definition at line 258 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
Eigen::Matrix<double,6,6> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::xtBZhBx [protected]

Definition at line 258 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
Eigen::Matrix<double,3,18> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::Zest [protected]

Definition at line 167 of file ndt_matcher_d2d.h.

template<typename PointSource, typename PointTarget>
Eigen::Matrix<double,18,18> lslgeneric::NDTMatcherD2D< PointSource, PointTarget >::ZHest [protected]

Definition at line 168 of file ndt_matcher_d2d.h.


The documentation for this class was generated from the following files:


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