implements a normal distibution cell More...

#include <ndt_cell.h>

Public Types
enum	CellClass { HORIZONTAL = 0, VERTICAL, INCLINED, ROUGH, UNKNOWN }
Public Member Functions
virtual void	addPoint (pcl::PointXYZ &pt)
virtual void	addPoints (pcl::PointCloud< pcl::PointXYZ > &pt)
void	classify ()
virtual NDTCell *	clone () const
void	computeGaussian (int mode=CELL_UPDATE_MODE_SAMPLE_VARIANCE, unsigned int maxnumpoints=1e9, float occupancy_limit=255, Eigen::Vector3d origin=Eigen::Vector3d(0, 0, 0), double sensor_noise=0.1)
void	computeGaussianSimple ()
	Just computes the normal distribution parameters from the points and leaves the points into the map.
double	computeMaximumLikelihoodAlongLine (const pcl::PointXYZ &p1, const pcl::PointXYZ &p2, Eigen::Vector3d &out)
virtual NDTCell *	copy () const
pcl::PointXYZ	getCenter () const
CellClass	getClass () const
Eigen::Matrix3d	getCov () const
virtual double	getDiagonal () const
void	getDimensions (double &xs, double &ys, double &zs) const
float	getDynamicLikelihood (unsigned int N)
int	getEmptyval ()
Eigen::Vector3d	getEvals () const
Eigen::Matrix3d	getEvecs () const
TEventData	getEventData ()
Eigen::Matrix3d	getInverseCov () const
double	getLikelihood (const pcl::PointXYZ &pt) const
Eigen::Vector3d	getMean () const
int	getN ()
float	getOccupancy ()
float	getOccupancyRescaled ()
void	getRGB (float &r, float &g, float &b)
bool	isInside (const pcl::PointXYZ pt) const
int	loadFromJFF (FILE *jffin)
	NDTCell ()
	The points falling into the cell - cleared after update.
	NDTCell (pcl::PointXYZ &center, double &xsize, double &ysize, double &zsize)
	NDTCell (const NDTCell &other)
void	rescaleCovariance ()
bool	rescaleCovariance (Eigen::Matrix3d &cov, Eigen::Matrix3d &invCov)
void	setCenter (const pcl::PointXYZ &cn)
void	setCov (const Eigen::Matrix3d &cov)
void	setDimensions (const double &xs, const double &ys, const double &zs)
void	setEmptyval (int emptyval_)
void	setEvals (const Eigen::Vector3d &ev)
void	setEventData (TEventData _ed)
void	setMean (const Eigen::Vector3d &mean)
void	setN (int N_)
void	setOccupancy (float occ_)
void	setRGB (float r, float g, float b)
	Set the RGB value for this cell.
void	updateColorInformation ()
void	updateEmpty (double elik, double dist)
void	updateOccupancy (float occ_val, float max_occu=255.0)
void	updateSampleVariance (const Eigen::Matrix3d &cov2, const Eigen::Vector3d &m2, unsigned int numpointsindistribution, bool updateOccupancyFlag=true, float max_occu=1024, unsigned int maxnumpoints=1e9)
int	writeToJFF (FILE *jffout)
virtual	~NDTCell ()
Static Public Member Functions
static void	setParameters (double _EVAL_ROUGH_THR=0.1, double _EVEC_INCLINED_THR=8 *M_PI/18, double _EVAL_FACTOR=100)
	use this to set the parameters for the NDTCell.
Public Attributes
double	consistency_score
double	cost
bool	hasGaussian_
	indicates if the cell has a gaussian in it
char	isEmpty
	ndt_wavefront planner
std::vector< pcl::PointXYZ, Eigen::aligned_allocator < pcl::PointXYZ > >	points_
Private Member Functions
int	loadJFFEventData (FILE *jffin, TEventData &evdata)
int	loadJFFMatrix (FILE *jffin, Eigen::Matrix3d &mat)
int	loadJFFVector (FILE *jffin, Eigen::Vector3d &vec)
double	squareSum (const Eigen::Matrix3d &C, const Eigen::Vector3d &x)
void	studentT ()
void	writeJFFEventData (FILE *jffout, TEventData &evdata)
void	writeJFFMatrix (FILE *jffout, Eigen::Matrix3d &mat)
void	writeJFFVector (FILE *jffout, Eigen::Vector3d &vec)
Private Attributes
float	B
pcl::PointXYZ	center_
CellClass	cl_
	Eigen values.
Eigen::Matrix3d	cov_
double	d1_
double	d2_
TEventData	edata
double	emptydist
double	emptylik
	The number of times a cell was observed empty (using ray casting)
int	emptyval
	Number of points used for Normal distribution estimation so far.
Eigen::Vector3d	evals_
	Mean of the normal distribution.
Eigen::Matrix3d	evecs_
	Precomputed inverse covariance (updated every time the cell is updated)
float	G
Eigen::Matrix3d	icov_
	Contains the covatiance of the normal distribution.
float	max_occu_
	Occupancy value stored as "Log odds" (if you wish)
Eigen::Vector3d	mean_
	Eigen vectors.
unsigned int	N
float	occ
	RGB values [0..1] - Special implementations for PointXYZRGB & PointXYZI.
float	R
double	xsize_
double	ysize_
double	zsize_
Static Private Attributes
static double	EVAL_FACTOR
static double	EVAL_ROUGH_THR
static double	EVEC_INCLINED_THR
static bool	parametersSet_ = false

Detailed Description

implements a normal distibution cell

The base class for all NDT indeces, contains mean, covariance matrix as well as eigen decomposition of covariance

Definition at line 75 of file ndt_cell.h.

Member Enumeration Documentation

enum lslgeneric::NDTCell::CellClass

Enumerator:

HORIZONTAL
VERTICAL
INCLINED
ROUGH
UNKNOWN

Definition at line 82 of file ndt_cell.h.

Constructor & Destructor Documentation

lslgeneric::NDTCell::NDTCell ( ) [inline]

The points falling into the cell - cleared after update.

Definition at line 85 of file ndt_cell.h.

virtual lslgeneric::NDTCell::~NDTCell ( ) [inline, virtual]

Definition at line 106 of file ndt_cell.h.

lslgeneric::NDTCell::NDTCell	(	pcl::PointXYZ &	center,
		double &	xsize,
		double &	ysize,
		double &	zsize
	)		`[inline]`

Definition at line 111 of file ndt_cell.h.

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

Definition at line 135 of file ndt_cell.h.

Member Function Documentation

virtual void lslgeneric::NDTCell::addPoint ( pcl::PointXYZ & pt ) [inline, virtual]

Adds a new point to distribution (does not update the distribution) Call computeGaussian() to update the content

Definition at line 302 of file ndt_cell.h.

virtual void lslgeneric::NDTCell::addPoints ( pcl::PointCloud< pcl::PointXYZ > & pt ) [inline, virtual]

Definition at line 307 of file ndt_cell.h.

void lslgeneric::NDTCell::classify ( )

classifies the cell according to the covariance matrix properties if smallest eigenval is bigger then roughness thershold, it's a rough cell evaluate inclination of the corresponding evector and classify as vertica, horizontal or inclined

Definition at line 1006 of file ndt_cell.cpp.

NDTCell * lslgeneric::NDTCell::clone ( ) const [virtual]

produces a new Cell* of the same type

Definition at line 29 of file ndt_cell.cpp.

void lslgeneric::NDTCell::computeGaussian	(	int	mode = `CELL_UPDATE_MODE_SAMPLE_VARIANCE`,
		unsigned int	maxnumpoints = `1e9`,
		float	occupancy_limit = `255`,
		Eigen::Vector3d	origin = `Eigen::Vector3d(0,0,0)`,
		double	sensor_noise = `0.1`
	)

Fits and updates the sample mean and covariance for the cell after the scan has been added. This function updates the occupancy, it uses the given method for the update. The behavior of the update can be altered with axnumpoints and occupancy_limit

Parameters:

mode	Determines the mode of the cell update - the mode defines are in the beginning of the header
maxnumpoints	This adapts the cell content in the presence of non-stationary distribution. The lower the value the faster the adaptation
occupancy_limit	This sets the limit for the confidence (in log-odds) that the cell can obtain.
origin	The CELL_UPDATE_MODE_SAMPLE_VARIANCE_SURFACE_ESTIMATION requires to know the sensor origin, so in case You use that, you should provide the position of the sensor, from where the measurement was taken for the method
sensor_noise	A standard deviation of the sensor noise, used only by method CELL_UPDATE_MODE_SAMPLE_VARIANCE_SURFACE_ESTIMATION if (maxnumpoints<=0) then the cell adaptation strategy is not used

Attempts to fit a gaussian in the cell. computes covariance and mean using observations

Occupancy update part This part infers the given "inconsistency" check done on update phase and updates the values accordingly

Continous occupancy update

We have to trust that it is occupied if we have measurements from the cell!

if(points_.size()>=3) { updateOccupancy(1.0, occupancy_limit); edata.updateSimple(EVENTMAP_OCCU); ///The cell is observed occupied isEmpty = -1; emptyval = 0; emptylik = 0; emptydist = 0; } else { points_.clear(); if(emptyval>=6) { updateOccupancy(-1.0, occupancy_limit); edata.updateSimple(EVENTMAP_FREE); ///The cell is observed empty isEmpty = 1; if(occ<=0) { hasGaussian_ = false; } points_.clear(); emptyval = 0; emptylik = 0; emptydist = 0; return; } else ///No proper observation { isEmpty = 0; points_.clear(); emptyval = 0; emptylik = 0; emptydist = 0; if(occ<0) hasGaussian_ = false; //R = 0; return; //end } }

< not enough to compute the gaussian

Update using new information

If the timescaling is set. This does "Sliding Average" for the sample mean and covariance

test for distance based sensor noise

If the timescaling is set. This does "Sliding Average" for the sample mean and covariance

End of update ////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////

Definition at line 261 of file ndt_cell.cpp.

void lslgeneric::NDTCell::computeGaussianSimple ( )

Just computes the normal distribution parameters from the points and leaves the points into the map.

just updates the parameters based on points and leaves the points to cell

Definition at line 137 of file ndt_cell.cpp.

double lslgeneric::NDTCell::computeMaximumLikelihoodAlongLine	(	const pcl::PointXYZ &	p1,
		const pcl::PointXYZ &	p2,
		Eigen::Vector3d &	out
	)

Computes the maximum likelihood that a point moving along a line defined by two points p1 and p2, gets measured agains the normaldistribution that is within this cell. This is used in raytracing to check if a measurement ray passes through a previously mapped object (thus provides evidence of inconsistency)

Parameters:

p1	One point along the ray
p2	second point along the ray (it must hold that p1 != p2);
&out	Gives out the exact maximum likelihood point

Computes the maximum likelihood that a point moving along a line defined by two points p1 and p2, gets measured agains the normaldistribution that is within this cell. This is used in raytracing to check if a measurement ray passes through a previously mapped object (thus provides evidence of inconsistency)

Parameters:

p1	One point along the ray
p2	second point along the ray (it must hold that p1 != p2);

X marks the spot

Definition at line 1083 of file ndt_cell.cpp.

NDTCell * lslgeneric::NDTCell::copy ( ) const [virtual]

produces a new Cell* of the same type and sets it to have the same parameters as this cell.

Definition at line 46 of file ndt_cell.cpp.

pcl::PointXYZ lslgeneric::NDTCell::getCenter ( ) const [inline]

Definition at line 172 of file ndt_cell.h.

CellClass lslgeneric::NDTCell::getClass ( ) const [inline]

Definition at line 250 of file ndt_cell.h.

Eigen::Matrix3d lslgeneric::NDTCell::getCov ( ) const [inline]

Definition at line 254 of file ndt_cell.h.

virtual double lslgeneric::NDTCell::getDiagonal ( ) const [inline, virtual]

Definition at line 198 of file ndt_cell.h.

void lslgeneric::NDTCell::getDimensions	(	double &	xs,
		double &	ys,
		double &	zs
	)		const `[inline]`

Definition at line 176 of file ndt_cell.h.

float lslgeneric::NDTCell::getDynamicLikelihood ( unsigned int N ) [inline]

Definition at line 361 of file ndt_cell.h.

int lslgeneric::NDTCell::getEmptyval ( ) [inline]

Definition at line 377 of file ndt_cell.h.

Eigen::Vector3d lslgeneric::NDTCell::getEvals ( ) const [inline]

Definition at line 270 of file ndt_cell.h.

Eigen::Matrix3d lslgeneric::NDTCell::getEvecs ( ) const [inline]

Definition at line 266 of file ndt_cell.h.

TEventData lslgeneric::NDTCell::getEventData ( ) [inline]

Definition at line 381 of file ndt_cell.h.

Eigen::Matrix3d lslgeneric::NDTCell::getInverseCov ( ) const [inline]

Definition at line 258 of file ndt_cell.h.

double lslgeneric::NDTCell::getLikelihood ( const pcl::PointXYZ & pt ) const

Get likelihood for a given point

Definition at line 1052 of file ndt_cell.cpp.

Eigen::Vector3d lslgeneric::NDTCell::getMean ( ) const [inline]

Definition at line 262 of file ndt_cell.h.

int lslgeneric::NDTCell::getN ( ) [inline]

Definition at line 389 of file ndt_cell.h.

float lslgeneric::NDTCell::getOccupancy ( ) [inline]

Returns the current accumulated occupancy value

Definition at line 341 of file ndt_cell.h.

float lslgeneric::NDTCell::getOccupancyRescaled ( ) [inline]

Returns the current accumulated occupancy value (rescaled)

Definition at line 348 of file ndt_cell.h.

void lslgeneric::NDTCell::getRGB	(	float &	r,
		float &	g,
		float &	b
	)		`[inline]`

Definition at line 319 of file ndt_cell.h.

bool lslgeneric::NDTCell::isInside ( const pcl::PointXYZ pt ) const [inline]

Definition at line 182 of file ndt_cell.h.

int lslgeneric::NDTCell::loadFromJFF ( FILE * jffin )

input method to load the ndt cell from a jff v0.5 file

Definition at line 948 of file ndt_cell.cpp.

int lslgeneric::NDTCell::loadJFFEventData	(	FILE *	jffin,
		TEventData &	evdata
	)		`[private]`

yet another helper function for loadFromJFF()

Definition at line 922 of file ndt_cell.cpp.

int lslgeneric::NDTCell::loadJFFMatrix	(	FILE *	jffin,
		Eigen::Matrix3d &	mat
	)		`[private]`

helper function for loadFromJFF()

Definition at line 882 of file ndt_cell.cpp.

int lslgeneric::NDTCell::loadJFFVector	(	FILE *	jffin,
		Eigen::Vector3d &	vec
	)		`[private]`

another helper function for loadFromJFF()

Definition at line 906 of file ndt_cell.cpp.

void lslgeneric::NDTCell::rescaleCovariance ( )

Definition at line 742 of file ndt_cell.cpp.

bool lslgeneric::NDTCell::rescaleCovariance	(	Eigen::Matrix3d &	cov,
		Eigen::Matrix3d &	invCov
	)

Rescales the covariance to protect against near sigularities and computes the inverse - This does not change class member values

Returns:: true if success, false if eigen values were negative

Rescales the covariance to protect against near sigularities and computes the inverse - This does not change class member values

Definition at line 684 of file ndt_cell.cpp.

void lslgeneric::NDTCell::setCenter ( const pcl::PointXYZ & cn ) [inline]

Definition at line 161 of file ndt_cell.h.

void lslgeneric::NDTCell::setCov ( const Eigen::Matrix3d & _cov )

setter for covariance

Definition at line 1068 of file ndt_cell.cpp.

void lslgeneric::NDTCell::setDimensions	(	const double &	xs,
		const double &	ys,
		const double &	zs
	)		`[inline]`

Definition at line 165 of file ndt_cell.h.

void lslgeneric::NDTCell::setEmptyval ( int emptyval_ ) [inline]

Definition at line 369 of file ndt_cell.h.

void lslgeneric::NDTCell::setEvals ( const Eigen::Vector3d & ev ) [inline]

Definition at line 282 of file ndt_cell.h.

void lslgeneric::NDTCell::setEventData ( TEventData _ed ) [inline]

Definition at line 373 of file ndt_cell.h.

void lslgeneric::NDTCell::setMean ( const Eigen::Vector3d & mean ) [inline]

Definition at line 278 of file ndt_cell.h.

void lslgeneric::NDTCell::setN ( int N_ ) [inline]

Definition at line 385 of file ndt_cell.h.

void lslgeneric::NDTCell::setOccupancy ( float occ_ ) [inline]

Definition at line 365 of file ndt_cell.h.

void lslgeneric::NDTCell::setParameters	(	double	_EVAL_ROUGH_THR = `0.1`,
		double	_EVEC_INCLINED_THR = `8*M_PI/18`,
		double	_EVAL_FACTOR = `100`
	)		`[static]`

use this to set the parameters for the NDTCell.

Note:: be careful, remember that the parameters are static, thus global

Definition at line 14 of file ndt_cell.cpp.

void lslgeneric::NDTCell::setRGB	(	float	r,
		float	g,
		float	b
	)		`[inline]`

Set the RGB value for this cell.

Definition at line 313 of file ndt_cell.h.

double lslgeneric::NDTCell::squareSum	(	const Eigen::Matrix3d &	C,
		const Eigen::Vector3d &	x
	)		`[inline, private]`

Definition at line 434 of file ndt_cell.h.

void lslgeneric::NDTCell::studentT ( ) [private]

Cell estimation using student-t

Robust estimation using Student-T

The inverse does not exist -- exit

Definition at line 175 of file ndt_cell.cpp.

void lslgeneric::NDTCell::updateColorInformation ( )

Calculates the average color for cell if the point type is pcl::PointXYZI or pcl::PointXYZRGB

Update the color information (template specialization)

Default - Do nothing

Definition at line 634 of file ndt_cell.cpp.

void lslgeneric::NDTCell::updateEmpty	(	double	elik,
		double	dist
	)		`[inline]`

Definition at line 354 of file ndt_cell.h.

void lslgeneric::NDTCell::updateOccupancy	(	float	occ_val,
		float	max_occu = `255.0`
	)		`[inline]`

Updates the occupancy value of the cell by summing to class variable

Definition at line 330 of file ndt_cell.h.

void lslgeneric::NDTCell::updateSampleVariance	(	const Eigen::Matrix3d &	cov2,
		const Eigen::Vector3d &	m2,
		unsigned int	numpointsindistribution,
		bool	updateOccupancyFlag = `true`,
		float	max_occu = `1024`,
		unsigned int	maxnumpoints = `1e9`
	)