Implements an NDT based spatial index on top of an hybrid metric topological grid. More...

#include <ndt_map_hmt.h>

Inheritance diagram for lslgeneric::NDTMapHMT< PointT >:

Public Member Functions
virtual void	addDistributionToCell (const Eigen::Matrix3d &ucov, const Eigen::Vector3d &umean, unsigned int numpointsindistribution, float r=0, float g=0, float b=0, unsigned int maxnumpoints=1e9, float max_occupancy=1024)
virtual void	addPointCloud (const Eigen::Vector3d &origin, const pcl::PointCloud< PointT > &pc, double classifierTh=0.06, double maxz=100.0, double sensor_noise=0.25, double occupancy_limit=255)
virtual void	addPointCloudMeanUpdate (const Eigen::Vector3d &origin, const pcl::PointCloud< PointT > &pc, const Eigen::Vector3d &localmapsize, unsigned int maxnumpoints=1e9, float occupancy_limit=255, double maxz=100.0, double sensor_noise=0.25)
virtual void	computeNDTCells (int cellupdatemode=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)
virtual std::vector < lslgeneric::NDTCell< PointT > * >	getAllCells () const
virtual std::vector < lslgeneric::NDTCell< PointT > * >	getAllInitializedCells ()
virtual bool	getCellAtPoint (const PointT &refPoint, NDTCell< PointT > *&cell)
	Get the cell for which the point fall into (not the closest cell)
virtual bool	getCellForPoint (const PointT &refPoint, NDTCell< PointT > *&cell, bool checkForGaussian=true) const
virtual std::vector< NDTCell < PointT > * >	getCellsForPoint (const PointT pt, int n_neighbours, bool checkForGaussian=true) const
virtual bool	getCentroid (double &cx, double &cy, double &cz)
virtual std::vector< NDTCell < PointT > * >	getInitializedCellsForPoint (const PointT pt) const
virtual double	getLikelihoodForPoint (PointT pt)
int	loadFrom ()
virtual void	loadPointCloud (const pcl::PointCloud< PointT > &pc, double range_limit=-1)
	NDTMapHMT (double resolution_, float cenx, float ceny, float cenz, float sizex, float sizey, float sizez, double max_range, std::string directory, bool _saveOnDelete=false)
	NDTMapHMT (const NDTMapHMT< PointT > &other)
int	numberOfActiveCells ()
virtual std::vector< NDTCell < PointT > * >	pseudoTransformNDT (Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > T)
void	setInsertPosition (const Eigen::Vector3d &newPos)
bool	tryLoad (const double &cx, const double &cy, const double &cz, LazyGrid< PointT > *&grid)
bool	tryLoadPosition (const Eigen::Vector3d &newPos)
int	writeTo ()
virtual	~NDTMapHMT ()
Public Attributes
EIGEN_MAKE_ALIGNED_OPERATOR_NEW pcl::PointCloud< PointT >	conflictPoints
	points that were conflicting during update
Protected Member Functions
void	initializeGrids ()
Protected Attributes
float	centerx
float	centery
float	centerz
lslgeneric::LazyGrid< PointT > *	grid_ [3][3]
bool	grids_init
SpatialIndex< PointT > *	index_
bool	is3D
bool	isFirstLoad_
Eigen::Vector3d	last_insert
float	map_sizex
float	map_sizey
float	map_sizez
double	max_range_
std::string	my_directory
double	resolution
bool	saveOnDelete
std::set< NDTCell< PointT > * >	update_set

Detailed Description

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

Implements an NDT based spatial index on top of an hybrid metric topological grid.

Author:: Jari Saarinen (jari.saarinen@aalto.fi) and Todor Stoyanov (todor.stoyanov@oru.se)

Version:: 0.1

This class derives from NDTMap and extends with functionalities for tracking several overlapping grids. On top of NDTMap, this class assumes updates are coming from a vehicle that moves through an environment and scans with a limited range sensor. Border areas are tracked as separate LazyGrid maps and stored and loaded from disk when the robot switches the map region

Definition at line 65 of file ndt_map_hmt.h.

Constructor & Destructor Documentation

template<typename PointT>

lslgeneric::NDTMapHMT< PointT >::NDTMapHMT	(	double	resolution_,
		float	cenx,
		float	ceny,
		float	cenz,
		float	sizex,
		float	sizey,
		float	sizez,
		double	max_range,
		std::string	directory,
		bool	_saveOnDelete = `false`
	)		`[inline]`

Construct with given centroid and sizes

Parameters:

cenx,ceny,cenz;	(x,y,z) of the center of the map
sizex,sizey,sizez,:	The size of the map in each respective direction NOTE: Implementation only for the lazy gridSpatialIndex<PointT> *idx

Definition at line 75 of file ndt_map_hmt.h.

template<typename PointT>

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

Definition at line 118 of file ndt_map_hmt.h.

template<typename PointT>

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

destructor

Definition at line 133 of file ndt_map_hmt.h.

Member Function Documentation

template<typename PointT >

void lslgeneric::NDTMapHMT< PointT >::addDistributionToCell	(	const Eigen::Matrix3d &	ucov,
		const Eigen::Vector3d &	umean,
		unsigned int	numpointsindistribution,
		float	r = `0`,
		float	g = `0`,
		float	b = `0`,
		unsigned int	maxnumpoints = `1e9`,
		float	max_occupancy = `1024`
	)		`[virtual]`

Adds a sample mean and covariance to the map

Parameters:

&ucov	The covariance matrix to be added
&umean	The mean of the normal distribution
numpointsindistribution	The number of points used in computation of the sample mean and covariance
r,g,b	-- optional color parameters
maxnumpoints	-- optional adaptation of the gaussians

Add a distribution to the map

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 643 of file ndt_map_hmt.hpp.

template<typename PointT >

void lslgeneric::NDTMapHMT< PointT >::addPointCloud	(	const Eigen::Vector3d &	origin,
		const pcl::PointCloud< PointT > &	pc,
		double	classifierTh = `0.06`,
		double	maxz = `100.0`,
		double	sensor_noise = `0.25`,
		double	occupancy_limit = `255`
	)		`[virtual]`

Add new pointcloud to map - This is the main interface for NDT-OM! Performs raytracing, updates conflicts and adds points to cells computeNDTCells must be called after calling this

Parameters:

&origin	is the position of the sensor, from where the scan has been taken from.
&pc	is the pointcloud to be added
classifierTh	A treshold to judge if the ray passes through a gaussian (obsolete)
maxz	threshold for the maximum z-coordinate value for the measurement point_cloud
sensor_noise	The expected standard deviation of the sensor noise

Adds a new cloud: NDT-OM update step

< don't accept points further than the measurement

<distance to endpoint

test for distance based sensor noise

This is the probability of max lik point being endpoint

Evidence value for empty - alpha * p(x);

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 311 of file ndt_map_hmt.hpp.

template<typename PointT >

void lslgeneric::NDTMapHMT< PointT >::addPointCloudMeanUpdate	(	const Eigen::Vector3d &	origin,
		const pcl::PointCloud< PointT > &	pc,
		const Eigen::Vector3d &	localmapsize,
		unsigned int	maxnumpoints = `1e9`,
		float	occupancy_limit = `255`,
		double	maxz = `100.0`,
		double	sensor_noise = `0.25`
	)		`[virtual]`

Add new pointcloud to map - Updates the occupancy using the mean values of a local map generated from an observation

Performs raytracing, updates conflicts and adds points to cells computeNDTCells must be called after calling this

Parameters:

&origin	is the position of the sensor, from where the scan has been taken from.
&pc	is the pointcloud to be added
&localmapsize	The dimensions of the local map used for computing the gaussians
maxnumpoints	Defines the forgetting factor (default 100000) the smaller the value the faster the adaptation
occupancy_limit	Clamping threshold for log-odds value
maxz	threshold for the maximum z-coordinate value for the measurement point_cloud
sensor_noise	The expected standard deviation of the sensor noise

fprintf(stderr,"UPDATING\n");

Lets first create a local ndmap (this really works only if we have lazy grid as well)

TODO: This returns now copies --- should be pointers?

< don't accept points further than the measurement

<distance to endpoint

test for distance based sensor noise

This is the probability of max lik point being endpoint

Evidence value for empty - alpha * p(x);

Add fake point to initialize!

<FIXME:: local implementation can be faster?

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 473 of file ndt_map_hmt.hpp.

template<typename PointT >

void lslgeneric::NDTMapHMT< PointT >::computeNDTCells	(	int	cellupdatemode = `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`
	)		`[virtual]`

Computes the NDT-cells after a measurement has been added

Parameters:

cellupdatemode	Defines the update mode (default CELL_UPDATE_MODE_SAMPLE_VARIANCE)
maxnumpoints	Defines the forgetting factor (default 100000) the smaller the value the faster the adaptation

Helper function, computes the NDTCells

Process the conflict points

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 685 of file ndt_map_hmt.hpp.

template<typename PointT >

std::vector< lslgeneric::NDTCell< PointT > * > lslgeneric::NDTMapHMT< PointT >::getAllCells ( ) const [virtual]

Returns a transformed NDT as an NDT map with a CellVector data structure NDTMap<PointT>* pseudoTransformNDTMap(Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T); Returns all computed cells from the map This method gives all the vectors that contain a gaussian within a cell (hasGaussian is true).

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 1239 of file ndt_map_hmt.hpp.

template<typename PointT >

std::vector< lslgeneric::NDTCell< PointT > * > lslgeneric::NDTMapHMT< PointT >::getAllInitializedCells ( ) [virtual]

Returns all cells that have been initialized (including ones that do not contain gaussian at the moment). This is useful if you want to use the empty cells or dynamic cells

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 1269 of file ndt_map_hmt.hpp.

template<typename PointT >

bool lslgeneric::NDTMapHMT< PointT >::getCellAtPoint	(	const PointT &	refPoint,
		NDTCell< PointT > *&	cell
	)		`[virtual]`

Get the cell for which the point fall into (not the closest cell)

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 1086 of file ndt_map_hmt.hpp.

template<typename PointT >

bool lslgeneric::NDTMapHMT< PointT >::getCellForPoint	(	const PointT &	refPoint,
		NDTCell< PointT > *&	cell,
		bool	checkForGaussian = `true`
	)		const `[virtual]`

returns the closest cell to refPoint Does not work with NDT-OM

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 1147 of file ndt_map_hmt.hpp.

template<typename PointT >

std::vector< NDTCell< PointT > * > lslgeneric::NDTMapHMT< PointT >::getCellsForPoint	(	const PointT	pt,
		int	n_neighbours,
		bool	checkForGaussian = `true`
	)		const `[virtual]`

Returns all the cells within radius Does not work with NDT-OM

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 1132 of file ndt_map_hmt.hpp.

template<typename PointT>

virtual bool lslgeneric::NDTMapHMT< PointT >::getCentroid	(	double &	cx,
		double &	cy,
		double &	cz
	)		`[inline, virtual]`

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 224 of file ndt_map_hmt.h.

template<typename PointT >

std::vector< NDTCell< PointT > * > lslgeneric::NDTMapHMT< PointT >::getInitializedCellsForPoint ( const PointT pt ) const [virtual]

Returns all the cells within radius

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 1117 of file ndt_map_hmt.hpp.

template<typename PointT >

double lslgeneric::NDTMapHMT< PointT >::getLikelihoodForPoint ( PointT pt ) [virtual]

------------------- non-essential stuff --------------///

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 1105 of file ndt_map_hmt.hpp.

template<typename PointT >

void lslgeneric::NDTMapHMT< PointT >::initializeGrids ( ) [protected]

Definition at line 24 of file ndt_map_hmt.hpp.

template<typename PointT >

int lslgeneric::NDTMapHMT< PointT >::loadFrom ( )

method to load NDT maps from .jff files USAGE: create NDTMap with desired index and PointType (index type is checked, but Point type is NOT checked) via e.g.

lslgeneric::NDTMap<pcl::PointXYZ> nd1( new lslgeneric::LazyGrid<pcl::PointXYZ>(0.4)); --> (*)

and then call

nd1.loadFromJFF("map0027.jff");

) use this constructor so index is not initialized and attributes can be set manually

Definition at line 1035 of file ndt_map_hmt.hpp.

template<typename PointT >

void lslgeneric::NDTMapHMT< PointT >::loadPointCloud	(	const pcl::PointCloud< PointT > &	pc,
		double	range_limit = `-1`
	)		`[virtual]`

loadPointCloud - You can call this if you are only interested in dealing with one scan without need for fusing several ones or representing empty space and occupancy

Otherwise you should always call addPointCloud (or if you don't want occupancy then addPointCloudSimple)

Parameters:

pc	the PointCloud that is to be loaded

Note:: every subsequent call will destroy the previous map!

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 266 of file ndt_map_hmt.hpp.

template<typename PointT >

int lslgeneric::NDTMapHMT< PointT >::numberOfActiveCells ( )

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 1295 of file ndt_map_hmt.hpp.

template<typename PointT >

std::vector< NDTCell< PointT > * > lslgeneric::NDTMapHMT< PointT >::pseudoTransformNDT ( Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > T ) [virtual]

Returns a transformed NDT as a vector of NDT cells

TODO: can we port this? template<typename PointT> NDTMapHMT<PointT>* NDTMapHMT<PointT>::pseudoTransformNDTMap(Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> T) { NDTMap<PointT>* map = new NDTMap<PointT>(new CellVector<PointT>()); CellVector<PointT>* idx = dynamic_cast<CellVector<PointT>*> (map->getMyIndex()); typename SpatialIndex<PointT>::CellVectorItr it = index_->begin();

while (it != index_->end()) { NDTCell<PointT> *cell = dynamic_cast<NDTCell<PointT>*> (*it); if(cell!=NULL) { if(cell->hasGaussian_) { Eigen::Vector3d mean = cell->getMean(); Eigen::Matrix3d cov = cell->getCov(); mean = T*mean; /NOTE: The rotation of the covariance fixed by Jari 6.11.2012 cov = T.rotation()*cov*T.rotation().transpose(); NDTCell<PointT>* nd = (NDTCell<PointT>*)cell->clone(); nd->setMean(mean); nd->setCov(cov); idx->addNDTCell(nd); } } else { ERR("problem casting cell to NDT!\n"); } it++; } return map; }

NOTE: The rotation of the covariance fixed by Jari 6.11.2012

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 1202 of file ndt_map_hmt.hpp.

template<typename PointT >

void lslgeneric::NDTMapHMT< PointT >::setInsertPosition ( const Eigen::Vector3d & newPos )