Implements an NDT based spatial index. More...

#include <ndt_map.h>

Inheritance diagram for lslgeneric::NDTMap:

Public Member Functions
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 bool	addMeasurement (const Eigen::Vector3d &origin, pcl::PointXYZ endpoint, double classifierTh, double maxz, double sensor_noise)
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)
void	addPointCloudSimple (const pcl::PointCloud< pcl::PointXYZ > &pc, double maxz=100.0)
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)
void	computeNDTCellsSimple ()
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
NDTCell *	getCellIdx (unsigned int idx) const
	return the cell using a specific index (not available for all spatialindexes), will return NULL if the idx is not valid.
virtual std::vector< NDTCell * >	getCellsForPoint (const pcl::PointXYZ pt, int n_neighbours, bool checkForGaussian=true) const
bool	getCellSizeInMeters (double &cx, double &cy, double &cz)
virtual bool	getCentroid (double &cx, double &cy, double &cz)
double	getDepth (Eigen::Vector3d origin, Eigen::Vector3d dir, double maxDepth=100)
double	getDepthSmooth (Eigen::Vector3d origin, Eigen::Vector3d dir, double maxDepth=20, int n_neigh=1, double weight=5.0, double threshold=0.2, Eigen::Vector3d *hit=NULL)
bool	getGridSize (int &cx, int &cy, int &cz)
bool	getGridSizeInMeters (double &cx, double &cy, double &cz)
virtual std::vector< NDTCell * >	getInitializedCellsForPoint (const pcl::PointXYZ pt) const
virtual double	getLikelihoodForPoint (pcl::PointXYZ pt)
SpatialIndex *	getMyIndex () const
int	getMyIndexInt () const
	return the spatial index used as an integer
std::string	getMyIndexStr () const
	return the spatial index used as a string
void	guessSize (float cenx, float ceny, float cenz, float sizex, float sizey, float sizez)
void	initialize (double cenx, double ceny, double cenz, double sizex, double sizey, double sizez)
void	loadDepthImage (const cv::Mat &depthImage, DepthCamera< pcl::PointXYZ > &cameraParams)
pcl::PointCloud< pcl::PointXYZ >	loadDepthImageFeatures (const cv::Mat &depthImage, std::vector< cv::KeyPoint > &keypoints, size_t &supportSize, double maxVar, DepthCamera< pcl::PointXYZ > &cameraParams, bool estimateParamsDI=false, bool nonMean=false)
int	loadFromJFF (const char *filename)
virtual void	loadPointCloud (const pcl::PointCloud< pcl::PointXYZ > &pc, double range_limit=-1)
void	loadPointCloud (const pcl::PointCloud< pcl::PointXYZ > &pc, const std::vector< std::vector< size_t > > &indices)
	each entry in the indices vector contains a set of indices to a NDC cell.
void	loadPointCloudCentroid (const pcl::PointCloud< pcl::PointXYZ > &pc, const Eigen::Vector3d &origin, const Eigen::Vector3d &old_centroid, const Eigen::Vector3d &map_size, double range_limit)
	NDTMap ()
	NDTMap (SpatialIndex *idx)
	NDTMap (const NDTMap &other)
	NDTMap (SpatialIndex *idx, float cenx, float ceny, float cenz, float sizex, float sizey, float sizez)
int	numberOfActiveCells ()
virtual std::vector< NDTCell * >	pseudoTransformNDT (Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > T)
NDTMap *	pseudoTransformNDTMap (Eigen::Transform< double, 3, Eigen::Affine, Eigen::ColMajor > T)
void	setMapSize (float sx, float sy, float sz)
void	setMode (bool is3D_)
int	writeCellVectorJFF (FILE *jffout)
int	writeLazyGridJFF (FILE *jffout)
int	writeOctTreeJFF (FILE *jffout)
int	writeToJFF (const char *filename)
virtual	~NDTMap ()
Public Attributes
EIGEN_MAKE_ALIGNED_OPERATOR_NEW pcl::PointCloud< pcl::PointXYZ >	conflictPoints
	points that were conflicting during update
Protected Attributes
float	centerx
float	centery
float	centerz
bool	guess_size_
SpatialIndex *	index_
bool	is3D
bool	isFirstLoad_
float	map_sizex
float	map_sizey
float	map_sizez
std::set< NDTCell * >	update_set

Detailed Description

Implements an NDT based spatial index.

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

Version:: 2.0

This class contains an interface to a SpatialIndex (custom defined) that contains NDT cells. Provides methods to create from a PointCloud.

This class implements two approaches to NDT mapping - "traditional" NDT approach, as well as a novel NDT Occupancy Map approach.

The "traditional" approach uses only the measurement points and a single scan in order to construct the NDT map.

The NDT-OM fuses incrementally measurement using Recursive Sample Covariance (RSC) approach. It also models the occupancy and free space, it adapts to changes in the cell etc.

Having these two versions combined also means that not all features are working with both. The known NDT-OM issues are

Only Lazy-Grid spatial index is supported

The old interface (e.g. used in registration) loadPointCloud(const pcl::PointCloud<PointT> &pc, double range_limit = -1); works as before: it computes an NDT map using only the samples and without tracking occupancy.

Since version 2.0 the ndt_map integrates also incremental update features. These are accessible through two methods: 1) void addPointCloudSimple(const pcl::PointCloud<PointT> &pc,double maxz=100.0); 2) void addPointCloud(const Eigen::Vector3d &origin, const pcl::PointCloud<PointT> &pc, double classifierTh=0.06, double maxz = 100.0, double sensor_noise = 0.25);

The first one only updates the measurement points and thus is faster, but does not model free space and does not tolerate dynamics The second one uses ray tracing and a number of approaches to model the occupancy as well as adapts to the dynamics.

In all cases the procedure to use the ndt_map is the following: 1) Add the measurement (by above mentioned load or add methods) 2) call computeNDTCells

Afer this the map is updated. There are various methods to access the map elements documented in this header file that you may use.

This class implements now the following papers, which you hopefully cite if you find this useful: Normal Distributions Transform Occupancy Maps: Application to Large-Scale Online 3D Mapping. IEEE International Conference on Robotics and Automation (ICRA 2013), 2013. There is also an implementation of modeling of the dynamics (edata structure in ndt_cell): "Independent Markov chain occupancy grid maps for representation of dynamic environments," in IROS2012 Conference Proceedings, Vilamoura, Algarve, Portugal: IEEE, 2012, pp. 3489-3495.

In addition, this class provide the basis for NDT registration, which is further discussed in the ndt_registration package. The relevant publications are:

Definition at line 100 of file ndt_map.h.

Constructor & Destructor Documentation

lslgeneric::NDTMap::NDTMap ( ) [inline]

Definition at line 103 of file ndt_map.h.

lslgeneric::NDTMap::NDTMap ( SpatialIndex * idx ) [inline]

default constructor. The SpatialIndex sent as a paramter is used as a factory every time that loadPointCloud is called. it can/should be deallocated outside the class after the destruction of the NDTMap

Definition at line 112 of file ndt_map.h.

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

Definition at line 126 of file ndt_map.h.

lslgeneric::NDTMap::NDTMap	(	SpatialIndex *	idx,
		float	cenx,
		float	ceny,
		float	cenz,
		float	sizex,
		float	sizey,
		float	sizez
	)		`[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 laze grid

Definition at line 141 of file ndt_map.h.

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

Default destructor

Definition at line 205 of file ndt_map.h.

Member Function Documentation

void lslgeneric::NDTMap::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`
	)

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

bool lslgeneric::NDTMap::addMeasurement	(	const Eigen::Vector3d &	origin,
		pcl::PointXYZ	endpoint,
		double	classifierTh,
		double	maxz,
		double	sensor_noise
	)		`[virtual]`

void lslgeneric::NDTMap::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]`

&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

void lslgeneric::NDTMap::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 lslgeneric::NDTMap::addPointCloudSimple	(	const pcl::PointCloud< pcl::PointXYZ > &	pc,
		double	maxz = `100.0`
	)

void lslgeneric::NDTMap::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]`

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

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

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

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

double lslgeneric::NDTMap::getDepthSmooth	(	Eigen::Vector3d	origin,
		Eigen::Vector3d	dir,
		double	maxDepth = `20`,
		int	n_neigh = `1`,
		double	weight = `5.0`,
		double	threshold = `0.2`,
		Eigen::Vector3d *	hit = `NULL`
	)

bool lslgeneric::NDTMap::getCellSizeInMeters	(	double &	cx,
		double &	cy,
		double &	cz
	)		`[inline]`

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

double lslgeneric::NDTMap::getDepth	(	Eigen::Vector3d	origin,
		Eigen::Vector3d	dir,
		double	maxDepth = `100`
	)

bool lslgeneric::NDTMap::getGridSize	(	int &	cx,
		int &	cy,
		int &	cz
	)		`[inline]`

bool lslgeneric::NDTMap::getGridSizeInMeters	(	double &	cx,
		double &	cy,
		double &	cz
	)		`[inline]`

void lslgeneric::NDTMap::guessSize	(	float	cenx,
		float	ceny,
		float	cenz,
		float	sizex,
		float	sizey,
		float	sizez
	)		`[inline]`

void lslgeneric::NDTMap::initialize	(	double	cenx,
		double	ceny,
		double	cenz,
		double	sizex,
		double	sizey,
		double	sizez
	)		`[inline]`

void lslgeneric::NDTMap::loadDepthImage	(	const cv::Mat &	depthImage,
		DepthCamera< pcl::PointXYZ > &	cameraParams
	)

pcl::PointCloud< pcl::PointXYZ > lslgeneric::NDTMap::loadDepthImageFeatures	(	const cv::Mat &	depthImage,
		std::vector< cv::KeyPoint > &	keypoints,
		size_t &	supportSize,
		double	maxVar,
		DepthCamera< pcl::PointXYZ > &	cameraParams,
		bool	estimateParamsDI = `false`,
		bool	nonMean = `false`
	)

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

Public Member Functions

Public Attributes

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation

void lslgeneric::NDTMap::loadPointCloud	(	const pcl::PointCloud< pcl::PointXYZ > &	pc,
		const std::vector< std::vector< size_t > > &	indices
	)

&pc	the Point Cloud to use as input
&indices	a vector of the indeces that will be added for each cell. We add indices.size() number of cells, each cell c[i] contains the points, indexed by the vector indices[i]

void lslgeneric::NDTMap::loadPointCloudCentroid	(	const pcl::PointCloud< pcl::PointXYZ > &	pc,
		const Eigen::Vector3d &	origin,
		const Eigen::Vector3d &	old_centroid,
		const Eigen::Vector3d &	map_size,
		double	range_limit
	)

&pc	the PointCloud that is to be loaded
&origin	The desired origin of the map (will be fitted acording to old_centroid)
&old_centroid	The centroid to which we want to align
&map_size	The size of the new map
range_limit	The maximum range value for measurements

void lslgeneric::NDTMap::setMapSize	(	float	sx,
		float	sy,
		float	sz
	)		`[inline]`