Public Member Functions | Public Attributes | Protected Member Functions | Protected Attributes
lslgeneric::NDTMapHMT< PointT > Class Template Reference

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 >:
Inheritance graph
[legend]

List of all members.

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:
&ucovThe covariance matrix to be added
&umeanThe mean of the normal distribution
numpointsindistributionThe 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:
&originis the position of the sensor, from where the scan has been taken from.
&pcis the pointcloud to be added
classifierThA treshold to judge if the ray passes through a gaussian (obsolete)
maxzthreshold for the maximum z-coordinate value for the measurement point_cloud
sensor_noiseThe 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:
&originis the position of the sensor, from where the scan has been taken from.
&pcis the pointcloud to be added
&localmapsizeThe dimensions of the local map used for computing the gaussians
maxnumpointsDefines the forgetting factor (default 100000) the smaller the value the faster the adaptation
occupancy_limitClamping threshold for log-odds value
maxzthreshold for the maximum z-coordinate value for the measurement point_cloud
sensor_noiseThe 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:
cellupdatemodeDefines the update mode (default CELL_UPDATE_MODE_SAMPLE_VARIANCE)
maxnumpointsDefines 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:
pcthe 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)

Definition at line 173 of file ndt_map_hmt.hpp.

template<typename PointT >
bool lslgeneric::NDTMapHMT< PointT >::tryLoad ( const double &  cx,
const double &  cy,
const double &  cz,
LazyGrid< PointT > *&  grid 
)

Definition at line 895 of file ndt_map_hmt.hpp.

template<typename PointT >
bool lslgeneric::NDTMapHMT< PointT >::tryLoadPosition ( const Eigen::Vector3d &  newPos)

Definition at line 64 of file ndt_map_hmt.hpp.

template<typename PointT >
int lslgeneric::NDTMapHMT< PointT >::writeTo ( )

Stuff for saving things to the directory of the map

output methods for saving the map in the jff format

Definition at line 712 of file ndt_map_hmt.hpp.


Member Data Documentation

template<typename PointT>
float lslgeneric::NDTMapHMT< PointT >::centerx [protected]

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 277 of file ndt_map_hmt.h.

template<typename PointT>
float lslgeneric::NDTMapHMT< PointT >::centery [protected]

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 277 of file ndt_map_hmt.h.

template<typename PointT>
float lslgeneric::NDTMapHMT< PointT >::centerz [protected]

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 277 of file ndt_map_hmt.h.

template<typename PointT>
EIGEN_MAKE_ALIGNED_OPERATOR_NEW pcl::PointCloud<PointT> lslgeneric::NDTMapHMT< PointT >::conflictPoints

points that were conflicting during update

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 292 of file ndt_map_hmt.h.

template<typename PointT>
lslgeneric::LazyGrid<PointT>* lslgeneric::NDTMapHMT< PointT >::grid_[3][3] [protected]

Definition at line 285 of file ndt_map_hmt.h.

template<typename PointT>
bool lslgeneric::NDTMapHMT< PointT >::grids_init [protected]

Definition at line 278 of file ndt_map_hmt.h.

template<typename PointT>
SpatialIndex<PointT>* lslgeneric::NDTMapHMT< PointT >::index_ [protected]

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 272 of file ndt_map_hmt.h.

template<typename PointT>
bool lslgeneric::NDTMapHMT< PointT >::is3D [protected]

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 270 of file ndt_map_hmt.h.

template<typename PointT>
bool lslgeneric::NDTMapHMT< PointT >::isFirstLoad_ [protected]

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 273 of file ndt_map_hmt.h.

template<typename PointT>
Eigen::Vector3d lslgeneric::NDTMapHMT< PointT >::last_insert [protected]

Definition at line 279 of file ndt_map_hmt.h.

template<typename PointT>
float lslgeneric::NDTMapHMT< PointT >::map_sizex [protected]

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 274 of file ndt_map_hmt.h.

template<typename PointT>
float lslgeneric::NDTMapHMT< PointT >::map_sizey [protected]

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 275 of file ndt_map_hmt.h.

template<typename PointT>
float lslgeneric::NDTMapHMT< PointT >::map_sizez [protected]

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 276 of file ndt_map_hmt.h.

template<typename PointT>
double lslgeneric::NDTMapHMT< PointT >::max_range_ [protected]

Definition at line 284 of file ndt_map_hmt.h.

template<typename PointT>
std::string lslgeneric::NDTMapHMT< PointT >::my_directory [protected]

Definition at line 280 of file ndt_map_hmt.h.

template<typename PointT>
double lslgeneric::NDTMapHMT< PointT >::resolution [protected]

Definition at line 281 of file ndt_map_hmt.h.

template<typename PointT>
bool lslgeneric::NDTMapHMT< PointT >::saveOnDelete [protected]

Definition at line 271 of file ndt_map_hmt.h.

template<typename PointT>
std::set<NDTCell<PointT>*> lslgeneric::NDTMapHMT< PointT >::update_set [protected]

Reimplemented from lslgeneric::NDTMap< PointT >.

Definition at line 282 of file ndt_map_hmt.h.


The documentation for this class was generated from the following files:


ndt_map
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Mon Oct 6 2014 03:18:54