#include <ndt_matcher_p2d.h>

Classes
struct	MoreThuente
Public Member Functions
void	check (pcl::PointCloud< pcl::PointXYZ > &fixed, pcl::PointCloud< pcl::PointXYZ > &moving, Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > &T)
bool	covariance (pcl::PointCloud< pcl::PointXYZ > &target, pcl::PointCloud< pcl::PointXYZ > &source, Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > &T, Eigen::Matrix< double, 6, 6 > &cov)
void	derivativesPointCloud (pcl::PointCloud< pcl::PointXYZ > &source, NDTMap &target, Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > &transform, Eigen::Matrix< double, 6, 1 > &score_gradient, Eigen::Matrix< double, 6, 6 > &Hessian, bool computeHessian)
void	generateScoreDebug (const char *out, pcl::PointCloud< pcl::PointXYZ > &target, pcl::PointCloud< pcl::PointXYZ > &source)
bool	match (pcl::PointCloud< pcl::PointXYZ > &target, pcl::PointCloud< pcl::PointXYZ > &source, Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > &T)
bool	match (NDTMap &target, pcl::PointCloud< pcl::PointXYZ > &source, Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > &T)
	NDTMatcherP2D (std::vector< double > _resolutions)
	NDTMatcherP2D ()
	NDTMatcherP2D (const NDTMatcherP2D &other)
double	scorePointCloud (pcl::PointCloud< pcl::PointXYZ > &source, NDTMap &target)
Public Attributes
double	finalscore
int	ITR_MAX
double	subsample_size
Private Member Functions
void	computeDerivatives (pcl::PointXYZ &pt)
void	init (bool useDefaultGridResolutions, std::vector< double > _resolutions)
double	lineSearch (double score_here, Eigen::Matrix< double, 6, 1 > &score_gradient, Eigen::Matrix< double, 6, 1 > &increment, pcl::PointCloud< pcl::PointXYZ > &source, NDTMap &target)
double	lineSearchMT (Eigen::Matrix< double, 6, 1 > &score_gradient_init, Eigen::Matrix< double, 6, 1 > &increment, pcl::PointCloud< pcl::PointXYZ > &source, Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > &globalT, NDTMap &target)
double	normalizeAngle (double a)
void	precomputeAngleDerivatives (Eigen::Vector3d &eulerAngles)
pcl::PointCloud< pcl::PointXYZ >	subsample (pcl::PointCloud< pcl::PointXYZ > &original)
void	update_hessian (Eigen::Matrix< double, 6, 6 > &Hessian, Eigen::Vector3d &transformed, Eigen::Matrix3d &Cinv)
bool	update_score_gradient (Eigen::Matrix< double, 6, 1 > &score_gradient, Eigen::Vector3d &transformed, Eigen::Matrix3d &Cinv)
Private Attributes
Eigen::Vector3d	a2
Eigen::Vector3d	a3
Eigen::Vector3d	b2
Eigen::Vector3d	b3
Eigen::Vector3d	c2
Eigen::Vector3d	c3
double	current_resolution
Eigen::Vector3d	d1
Eigen::Vector3d	d2
Eigen::Vector3d	d3
Eigen::Vector3d	e1
Eigen::Vector3d	e2
Eigen::Vector3d	e3
Eigen::Vector3d	f1
Eigen::Vector3d	f2
Eigen::Vector3d	f3
Eigen::Matrix< double, 18, 6 >	Hest
bool	isIrregularGrid
Eigen::Matrix< double, 3, 6 >	Jest
Eigen::Vector3d	jest04
Eigen::Vector3d	jest05
Eigen::Vector3d	jest13
Eigen::Vector3d	jest14
Eigen::Vector3d	jest15
Eigen::Vector3d	jest23
Eigen::Vector3d	jest24
Eigen::Vector3d	jest25
double	lfd1
double	lfd2
int	NUMBER_OF_ACTIVE_CELLS
int	NUMBER_OF_POINTS
std::vector< double >	resolutions
bool	useSimpleDerivatives

Detailed Description

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

Definition at line 49 of file ndt_matcher_p2d.h.

Constructor & Destructor Documentation

lslgeneric::NDTMatcherP2D::NDTMatcherP2D ( std::vector< double > _resolutions ) [inline]

Definition at line 52 of file ndt_matcher_p2d.h.

lslgeneric::NDTMatcherP2D::NDTMatcherP2D ( ) [inline]

Definition at line 56 of file ndt_matcher_p2d.h.

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

Definition at line 60 of file ndt_matcher_p2d.h.

Member Function Documentation

void lslgeneric::NDTMatcherP2D::check	(	pcl::PointCloud< pcl::PointXYZ > &	fixed,
		pcl::PointCloud< pcl::PointXYZ > &	moving,
		Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > &	T
	)

Check whether two point clouds are aligned, using the NDT score and Hessian.

Parameters:

fixed	Reference data. NDT structure is built for this point cloud (if needed).
moving	`T` will be applied to this point cloud.
T	Optional transformation that can be applied to `moving`. Defaults to zero.

Returns:

Definition at line 436 of file ndt_matcher_p2d.cpp.

void lslgeneric::NDTMatcherP2D::computeDerivatives ( pcl::PointXYZ & pt ) [private]

Definition at line 569 of file ndt_matcher_p2d.cpp.

bool lslgeneric::NDTMatcherP2D::covariance	(	pcl::PointCloud< pcl::PointXYZ > &	target,
		pcl::PointCloud< pcl::PointXYZ > &	source,
		Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > &	T,
		Eigen::Matrix< double, 6, 6 > &	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 209 of file ndt_matcher_p2d.cpp.

void lslgeneric::NDTMatcherP2D::derivativesPointCloud	(	pcl::PointCloud< pcl::PointXYZ > &	source,
		NDTMap &	target,
		Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > &	transform,
		Eigen::Matrix< double, 6, 1 > &	score_gradient,
		Eigen::Matrix< double, 6, 6 > &	Hessian,
		bool	computeHessian
	)

Definition at line 685 of file ndt_matcher_p2d.cpp.

void lslgeneric::NDTMatcherP2D::generateScoreDebug	(	const char *	out,
		pcl::PointCloud< pcl::PointXYZ > &	target,
		pcl::PointCloud< pcl::PointXYZ > &	source
	)

Definition at line 56 of file ndt_matcher_p2d.cpp.

void lslgeneric::NDTMatcherP2D::init	(	bool	useDefaultGridResolutions,
		std::vector< double >	_resolutions
	)		`[private]`

Definition at line 13 of file ndt_matcher_p2d.cpp.

double lslgeneric::NDTMatcherP2D::lineSearch	(	double	score_here,
		Eigen::Matrix< double, 6, 1 > &	score_gradient,
		Eigen::Matrix< double, 6, 1 > &	increment,
		pcl::PointCloud< pcl::PointXYZ > &	source,
		NDTMap &	target
	)		`[private]`

double lslgeneric::NDTMatcherP2D::lineSearchMT	(	Eigen::Matrix< double, 6, 1 > &	score_gradient_init,
		Eigen::Matrix< double, 6, 1 > &	increment,
		pcl::PointCloud< pcl::PointXYZ > &	source,
		Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > &	globalT,
		NDTMap &	target
	)		`[private]`

Definition at line 752 of file ndt_matcher_p2d.cpp.

bool lslgeneric::NDTMatcherP2D::match	(	pcl::PointCloud< pcl::PointXYZ > &	target,
		pcl::PointCloud< pcl::PointXYZ > &	source,
		Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > &	T
	)

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.

Returns:: False if registration terminated after the max number of iterations (before convergence), true if registration terminated because the convergence criteria were fulfilled.

Definition at line 167 of file ndt_matcher_p2d.cpp.

bool lslgeneric::NDTMatcherP2D::match	(	NDTMap &	target,
		pcl::PointCloud< pcl::PointXYZ > &	source,
		Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > &	T
	)

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.

Returns:: False if registration terminated after the max number of iterations (before convergence), true if registration terminated because the convergence criteria were fulfilled.

Definition at line 250 of file ndt_matcher_p2d.cpp.

double lslgeneric::NDTMatcherP2D::normalizeAngle ( double a ) [private]

Definition at line 1384 of file ndt_matcher_p2d.cpp.

void lslgeneric::NDTMatcherP2D::precomputeAngleDerivatives ( Eigen::Vector3d & eulerAngles ) [private]

Definition at line 505 of file ndt_matcher_p2d.cpp.

double lslgeneric::NDTMatcherP2D::scorePointCloud	(	pcl::PointCloud< pcl::PointXYZ > &	source,
		NDTMap &	target
	)

Definition at line 640 of file ndt_matcher_p2d.cpp.

pcl::PointCloud< pcl::PointXYZ > lslgeneric::NDTMatcherP2D::subsample ( pcl::PointCloud< pcl::PointXYZ > & original ) [private]

Definition at line 1289 of file ndt_matcher_p2d.cpp.

void lslgeneric::NDTMatcherP2D::update_hessian	(	Eigen::Matrix< double, 6, 6 > &	Hessian,
		Eigen::Vector3d &	transformed,
		Eigen::Matrix3d &	Cinv
	)		`[private]`

Definition at line 480 of file ndt_matcher_p2d.cpp.

bool lslgeneric::NDTMatcherP2D::update_score_gradient	(	Eigen::Matrix< double, 6, 1 > &	score_gradient,
		Eigen::Vector3d &	transformed,
		Eigen::Matrix3d &	Cinv
	)		`[private]`