implements a normal distibution cell More...

#include <ndt_cell.h>

Inheritance diagram for lslgeneric::NDTCell< PointT >:

Public Types
enum	CellClass { HORIZONTAL = 0, VERTICAL, INCLINED, ROUGH, UNKNOWN }
Public Member Functions
virtual void	addPoint (PointT &pt)
virtual void	addPoints (pcl::PointCloud< PointT > &pt)
void	classify ()
virtual Cell< PointT > *	clone () const
	FIXME:: What is the difference between copy and clone?
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 PointT &p1, const PointT &p2, Eigen::Vector3d &out)
virtual Cell< PointT > *	copy () const
CellClass	getClass () const
Eigen::Matrix3d	getCov () 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 PointT &pt) const
Eigen::Vector3d	getMean () const
int	getN ()
float	getOccupancy ()
float	getOccupancyRescaled ()
void	getRGB (float &r, float &g, float &b)
int	loadFromJFF (FILE *jffin)
	NDTCell ()
	The points falling into the cell - cleared after update.
	NDTCell (PointT &center, double &xsize, double &ysize, double &zsize)
	NDTCell (const NDTCell &other)
void	rescaleCovariance ()
bool	rescaleCovariance (Eigen::Matrix3d &cov, Eigen::Matrix3d &invCov)
void	setCov (const Eigen::Matrix3d &cov)
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 ()
template<>
void	updateColorInformation ()
template<>
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)
void	writeToVRML (FILE *fout, Eigen::Vector3d col=Eigen::Vector3d(0, 0, 0))
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
	FIXME:: Am I used in some context? --> yes, ndt_wavefront planner.
std::vector< PointT >	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
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
Static Private Attributes
static double	EVAL_FACTOR
static double	EVAL_ROUGH_THR
static double	EVEC_INCLINED_THR
static bool	parametersSet_ = false

Detailed Description

template<typename PointT>
class lslgeneric::NDTCell< PointT >

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 76 of file ndt_cell.h.

Member Enumeration Documentation

template<typename PointT>

enum lslgeneric::NDTCell::CellClass

Enumerator:

HORIZONTAL
VERTICAL
INCLINED
ROUGH
UNKNOWN

Definition at line 83 of file ndt_cell.h.

Constructor & Destructor Documentation

template<typename PointT>

lslgeneric::NDTCell< PointT >::NDTCell ( ) [inline]

The points falling into the cell - cleared after update.

Definition at line 86 of file ndt_cell.h.

template<typename PointT>

virtual lslgeneric::NDTCell< PointT >::~NDTCell ( ) [inline, virtual]

Definition at line 107 of file ndt_cell.h.

template<typename PointT>

lslgeneric::NDTCell< PointT >::NDTCell	(	PointT &	center,
		double &	xsize,
		double &	ysize,
		double &	zsize
	)		`[inline]`

Definition at line 112 of file ndt_cell.h.

template<typename PointT>

lslgeneric::NDTCell< PointT >::NDTCell ( const NDTCell< PointT > & other ) [inline]

Definition at line 133 of file ndt_cell.h.

Member Function Documentation

template<typename PointT>

virtual void lslgeneric::NDTCell< PointT >::addPoint ( PointT & pt ) [inline, virtual]

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

Reimplemented from lslgeneric::Cell< PointT >.

Definition at line 260 of file ndt_cell.h.

template<typename PointT>

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

Definition at line 266 of file ndt_cell.h.

template<typename PointT >

void lslgeneric::NDTCell< PointT >::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 1170 of file ndt_cell.hpp.

template<typename PointT >

Cell< PointT > * lslgeneric::NDTCell< PointT >::clone ( ) const [virtual]

FIXME:: What is the difference between copy and clone?

produces a new Cell* of the same type

Reimplemented from lslgeneric::Cell< PointT >.

Definition at line 37 of file ndt_cell.hpp.

template<typename PointT >

void lslgeneric::NDTCell< PointT >::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`
	)		`[inline]`

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 279 of file ndt_cell.hpp.

template<typename PointT >

void lslgeneric::NDTCell< PointT >::computeGaussianSimple ( ) [inline]

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 152 of file ndt_cell.hpp.

template<typename PointT >

double lslgeneric::NDTCell< PointT >::computeMaximumLikelihoodAlongLine	(	const PointT &	p1,
		const PointT &	p2,
		Eigen::Vector3d &	out
	)		`[inline]`

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 1251 of file ndt_cell.hpp.

template<typename PointT >

Cell< PointT > * lslgeneric::NDTCell< PointT >::copy ( ) const [virtual]

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

Reimplemented from lslgeneric::Cell< PointT >.

Definition at line 55 of file ndt_cell.hpp.

template<typename PointT>

CellClass lslgeneric::NDTCell< PointT >::getClass ( ) const [inline]

Definition at line 208 of file ndt_cell.h.

template<typename PointT>

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

Definition at line 212 of file ndt_cell.h.

template<typename PointT>

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

Definition at line 320 of file ndt_cell.h.

template<typename PointT>

int lslgeneric::NDTCell< PointT >::getEmptyval ( ) [inline]

Definition at line 336 of file ndt_cell.h.

template<typename PointT>

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

Definition at line 228 of file ndt_cell.h.

template<typename PointT>

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

Definition at line 224 of file ndt_cell.h.

template<typename PointT>

TEventData lslgeneric::NDTCell< PointT >::getEventData ( ) [inline]

Definition at line 340 of file ndt_cell.h.

template<typename PointT>

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

Definition at line 216 of file ndt_cell.h.

template<typename PointT >

double lslgeneric::NDTCell< PointT >::getLikelihood ( const PointT & pt ) const

Get likelihood for a given point

Definition at line 1217 of file ndt_cell.hpp.

template<typename PointT>

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

Definition at line 220 of file ndt_cell.h.

template<typename PointT>

int lslgeneric::NDTCell< PointT >::getN ( ) [inline]

Definition at line 348 of file ndt_cell.h.

template<typename PointT>

float lslgeneric::NDTCell< PointT >::getOccupancy ( ) [inline]

Returns the current accumulated occupancy value

Definition at line 300 of file ndt_cell.h.

template<typename PointT>

float lslgeneric::NDTCell< PointT >::getOccupancyRescaled ( ) [inline]

Returns the current accumulated occupancy value (rescaled)

Definition at line 307 of file ndt_cell.h.

template<typename PointT>

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

Definition at line 278 of file ndt_cell.h.

template<typename PointT >

int lslgeneric::NDTCell< PointT >::loadFromJFF ( FILE * jffin )

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

Definition at line 1111 of file ndt_cell.hpp.

template<typename PointT >

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

yet another helper function for loadFromJFF()

Definition at line 1084 of file ndt_cell.hpp.

template<typename PointT >

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

helper function for loadFromJFF()

Definition at line 1042 of file ndt_cell.hpp.

template<typename PointT >

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

another helper function for loadFromJFF()

Definition at line 1067 of file ndt_cell.hpp.

template<typename PointT >

void lslgeneric::NDTCell< PointT >::rescaleCovariance ( )

Definition at line 764 of file ndt_cell.hpp.

template<typename PointT >

bool lslgeneric::NDTCell< PointT >::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 705 of file ndt_cell.hpp.

template<typename PointT >

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

setter for covariance

Definition at line 1234 of file ndt_cell.hpp.

template<typename PointT>

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

Definition at line 328 of file ndt_cell.h.

template<typename PointT>

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

Definition at line 240 of file ndt_cell.h.

template<typename PointT>

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

Definition at line 332 of file ndt_cell.h.

template<typename PointT>

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

Definition at line 236 of file ndt_cell.h.

template<typename PointT>

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

Definition at line 344 of file ndt_cell.h.

template<typename PointT>

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

Definition at line 324 of file ndt_cell.h.

template<typename PointT >

void lslgeneric::NDTCell< PointT >::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 20 of file ndt_cell.hpp.

template<typename PointT>

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

Set the RGB value for this cell.

Definition at line 272 of file ndt_cell.h.

template<typename PointT>

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

Definition at line 391 of file ndt_cell.h.

template<typename PointT >

void lslgeneric::NDTCell< PointT >::studentT ( ) [inline, private]

Cell estimation using student-t

Robust estimation using Student-T

The inverse does not exist -- exit

Definition at line 191 of file ndt_cell.hpp.

template<typename PointT >

void lslgeneric::NDTCell< PointT >::updateColorInformation ( ) [inline]

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 653 of file ndt_cell.hpp.

template<>

void lslgeneric::NDTCell< pcl::PointXYZRGB >::updateColorInformation ( ) [inline]

Definition at line 659 of file ndt_cell.hpp.

template<>

void lslgeneric::NDTCell< pcl::PointXYZI >::updateColorInformation ( ) [inline]

Definition at line 687 of file ndt_cell.hpp.

template<typename PointT>

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

Definition at line 313 of file ndt_cell.h.

template<typename PointT>

void lslgeneric::NDTCell< PointT >::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 289 of file ndt_cell.h.

template<typename PointT >

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