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:

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< pcl::PointXYZ > &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< pcl::PointXYZ > &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 * >	getAllCells () const
virtual std::vector < lslgeneric::NDTCell * >	getAllInitializedCells ()
virtual bool	getCellAtPoint (const pcl::PointXYZ &refPoint, NDTCell *&cell)
	Get the cell for which the point fall into (not the closest cell)
virtual bool	getCellForPoint (const pcl::PointXYZ &refPoint, NDTCell *&cell, bool checkForGaussian=true) const
virtual std::vector< NDTCell * >	getCellsForPoint (const pcl::PointXYZ pt, int n_neighbours, bool checkForGaussian=true) const
virtual bool	getCentroid (double &cx, double &cy, double &cz)
virtual std::vector< NDTCell * >	getInitializedCellsForPoint (const pcl::PointXYZ pt) const
virtual double	getLikelihoodForPoint (pcl::PointXYZ pt)
int	loadFrom ()
virtual void	loadPointCloud (const pcl::PointCloud< pcl::PointXYZ > &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 &other)
int	numberOfActiveCells ()
virtual std::vector< NDTCell * >	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 *&grid)
bool	tryLoadPosition (const Eigen::Vector3d &newPos)
int	writeTo ()
virtual	~NDTMapHMT ()
Public Attributes
EIGEN_MAKE_ALIGNED_OPERATOR_NEW pcl::PointCloud< pcl::PointXYZ >	conflictPoints
	points that were conflicting during update
Protected Member Functions
void	initializeGrids ()
Protected Attributes
float	centerx
float	centery
float	centerz
lslgeneric::LazyGrid *	grid_ [3][3]
bool	grids_init
SpatialIndex *	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 * >	update_set

Detailed Description

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 64 of file ndt_map_hmt.h.

Constructor & Destructor Documentation

lslgeneric::NDTMapHMT::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 74 of file ndt_map_hmt.h.

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

Definition at line 117 of file ndt_map_hmt.h.

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

destructor

Definition at line 132 of file ndt_map_hmt.h.

Member Function Documentation

void lslgeneric::NDTMapHMT::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.

Definition at line 631 of file ndt_map_hmt.cpp.

void lslgeneric::NDTMapHMT::addPointCloud	(	const Eigen::Vector3d &	origin,
		const pcl::PointCloud< pcl::PointXYZ > &	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.

Definition at line 301 of file ndt_map_hmt.cpp.

void lslgeneric::NDTMapHMT::addPointCloudMeanUpdate	(	const Eigen::Vector3d &	origin,
		const pcl::PointCloud< pcl::PointXYZ > &	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.

Definition at line 462 of file ndt_map_hmt.cpp.

void lslgeneric::NDTMapHMT::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.

Definition at line 672 of file ndt_map_hmt.cpp.

std::vector< lslgeneric::NDTCell * > lslgeneric::NDTMapHMT::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.

Definition at line 1181 of file ndt_map_hmt.cpp.

std::vector< lslgeneric::NDTCell * > lslgeneric::NDTMapHMT::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.

Definition at line 1210 of file ndt_map_hmt.cpp.

bool lslgeneric::NDTMapHMT::getCellAtPoint	(	const pcl::PointXYZ &	refPoint,
		NDTCell *&	cell
	)		`[virtual]`

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

Reimplemented from lslgeneric::NDTMap.

Definition at line 1069 of file ndt_map_hmt.cpp.

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

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

Reimplemented from lslgeneric::NDTMap.

Definition at line 1126 of file ndt_map_hmt.cpp.

std::vector< NDTCell * > lslgeneric::NDTMapHMT::getCellsForPoint	(	const pcl::PointXYZ	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.

Definition at line 1112 of file ndt_map_hmt.cpp.

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

Reimplemented from lslgeneric::NDTMap.

Definition at line 223 of file ndt_map_hmt.h.

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

Returns all the cells within radius

Reimplemented from lslgeneric::NDTMap.

Definition at line 1098 of file ndt_map_hmt.cpp.

double lslgeneric::NDTMapHMT::getLikelihoodForPoint ( pcl::PointXYZ pt ) [virtual]

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

Reimplemented from lslgeneric::NDTMap.

Definition at line 1087 of file ndt_map_hmt.cpp.

void lslgeneric::NDTMapHMT::initializeGrids ( ) [protected]

Definition at line 18 of file ndt_map_hmt.cpp.

int lslgeneric::NDTMapHMT::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 1019 of file ndt_map_hmt.cpp.

void lslgeneric::NDTMapHMT::loadPointCloud	(	const pcl::PointCloud< pcl::PointXYZ > &	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: