#include <ndt_matcher_d2d.h>

Inheritance diagram for lslgeneric::NDTMatcherD2D:

Classes
struct	MoreThuente
Public Member Functions
bool	covariance (pcl::PointCloud< pcl::PointXYZ > &target, pcl::PointCloud< pcl::PointXYZ > &source, Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > &T, Eigen::MatrixXd &cov)
bool	covariance (NDTMap &target, NDTMap &source, Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > &T, Eigen::MatrixXd &cov)
virtual double	derivativesNDT (const std::vector< NDTCell * > &source, const NDTMap &target, Eigen::MatrixXd &score_gradient, Eigen::MatrixXd &Hessian, bool computeHessian)
bool	match (pcl::PointCloud< pcl::PointXYZ > &target, pcl::PointCloud< pcl::PointXYZ > &source, Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > &T, bool useInitialGuess=false)
bool	match (NDTMap &target, NDTMap &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 * > &source, NDTMap &target)
virtual double	scoreNDT_OM (NDTMap &source, NDTMap &target)
virtual double	scoreNDTPositive (std::vector< NDTCell * > &sourceNDT, NDTMap &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
int	n_neighbours
bool	regularize
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 * > &source, NDTMap &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

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

Definition at line 52 of file ndt_matcher_d2d.h.

Constructor & Destructor Documentation

lslgeneric::NDTMatcherD2D::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 61 of file ndt_matcher_d2d.h.

lslgeneric::NDTMatcherD2D::NDTMatcherD2D ( ) [inline]

Definition at line 66 of file ndt_matcher_d2d.h.

lslgeneric::NDTMatcherD2D::NDTMatcherD2D ( const NDTMatcherD2D & other ) [inline]

Definition at line 70 of file ndt_matcher_d2d.h.

Member Function Documentation

void lslgeneric::NDTMatcherD2D::computeDerivatives	(	Eigen::Vector3d &	m1,
		Eigen::Matrix3d	C1,
		bool	computeHessian = `true`
	)		`[protected]`

Definition at line 685 of file ndt_matcher_d2d.cpp.

void lslgeneric::NDTMatcherD2D::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 589 of file ndt_matcher_d2d.cpp.

bool lslgeneric::NDTMatcherD2D::covariance	(	pcl::PointCloud< pcl::PointXYZ > &	target,
		pcl::PointCloud< pcl::PointXYZ > &	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 2123 of file ndt_matcher_d2d.cpp.

bool lslgeneric::NDTMatcherD2D::covariance	(	NDTMap &	target,
		NDTMap &	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 1994 of file ndt_matcher_d2d.cpp.

double lslgeneric::NDTMatcherD2D::derivativesNDT	(	const std::vector< NDTCell * > &	source,
		const NDTMap &	target,
		Eigen::MatrixXd &	score_gradient,
		Eigen::MatrixXd &	Hessian,
		bool	computeHessian
	)		`[virtual]`

Reimplemented in lslgeneric::NDTMatcherFeatureD2D.

Definition at line 1029 of file ndt_matcher_d2d.cpp.

void lslgeneric::NDTMatcherD2D::init	(	bool	_isIrregularGrid,
		bool	useDefaultGridResolutions,
		std::vector< double >	_resolutions
	)		`[protected]`

Definition at line 15 of file ndt_matcher_d2d.cpp.

double lslgeneric::NDTMatcherD2D::lineSearchMT	(	Eigen::Matrix< double, 6, 1 > &	increment,
		std::vector< NDTCell * > &	source,
		NDTMap &	target
	)		`[protected]`

Definition at line 1269 of file ndt_matcher_d2d.cpp.

bool lslgeneric::NDTMatcherD2D::match	(	pcl::PointCloud< pcl::PointXYZ > &	target,
		pcl::PointCloud< pcl::PointXYZ > &	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:

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 50 of file ndt_matcher_d2d.cpp.

bool lslgeneric::NDTMatcherD2D::match	(	NDTMap &	target,
		NDTMap &	source,
		Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > &	T,
		bool	useInitialGuess = `false`
	)

Registers a point cloud to an NDT structure.

Parameters:

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 150 of file ndt_matcher_d2d.cpp.

double lslgeneric::NDTMatcherD2D::normalizeAngle ( double a ) [protected]

Definition at line 1832 of file ndt_matcher_d2d.cpp.

double lslgeneric::NDTMatcherD2D::scoreNDT	(	std::vector< NDTCell * > &	source,
		NDTMap &	target
	)		`[virtual]`

Definition at line 776 of file ndt_matcher_d2d.cpp.

double lslgeneric::NDTMatcherD2D::scoreNDT_OM	(	NDTMap &	source,
		NDTMap &	target
	)		`[virtual]`

Definition at line 836 of file ndt_matcher_d2d.cpp.

double lslgeneric::NDTMatcherD2D::scoreNDTPositive	(	std::vector< NDTCell * > &	sourceNDT,
		NDTMap &	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 947 of file ndt_matcher_d2d.cpp.

bool lslgeneric::NDTMatcherD2D::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 500 of file ndt_matcher_d2d.cpp.

bool lslgeneric::NDTMatcherD2D::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]`