Implements an NDT based spatial index on top of an hybrid metric topological grid. More...
#include <ndt_map_hmt.h>
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 |
Implements an NDT based spatial index on top of an hybrid metric topological grid.
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.
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
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< PointT >::NDTMapHMT | ( | const NDTMapHMT< PointT > & | other | ) | [inline] |
Definition at line 117 of file ndt_map_hmt.h.
virtual lslgeneric::NDTMapHMT< PointT >::~NDTMapHMT | ( | ) | [inline, virtual] |
destructor
Definition at line 132 of file ndt_map_hmt.h.
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
&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.
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
&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.
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
&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.
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
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.
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.
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.
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.
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.
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.
virtual bool lslgeneric::NDTMapHMT< PointT >::getCentroid | ( | double & | cx, |
double & | cy, | ||
double & | cz | ||
) | [inline, virtual] |
Reimplemented from lslgeneric::NDTMap< PointT >.
Definition at line 223 of file ndt_map_hmt.h.
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.
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.
void lslgeneric::NDTMapHMT< PointT >::initializeGrids | ( | ) | [protected] |
Definition at line 24 of file ndt_map_hmt.hpp.
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.
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)
pc | the PointCloud that is to be loaded |
Reimplemented from lslgeneric::NDTMap< PointT >.
Definition at line 266 of file ndt_map_hmt.hpp.
int lslgeneric::NDTMapHMT< PointT >::numberOfActiveCells | ( | ) |
Reimplemented from lslgeneric::NDTMap< PointT >.
Definition at line 1295 of file ndt_map_hmt.hpp.
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.
void lslgeneric::NDTMapHMT< PointT >::setInsertPosition | ( | const Eigen::Vector3d & | newPos | ) |
Definition at line 173 of file ndt_map_hmt.hpp.
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.
bool lslgeneric::NDTMapHMT< PointT >::tryLoadPosition | ( | const Eigen::Vector3d & | newPos | ) |
Definition at line 64 of file ndt_map_hmt.hpp.
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.
float lslgeneric::NDTMapHMT< PointT >::centerx [protected] |
Reimplemented from lslgeneric::NDTMap< PointT >.
Definition at line 276 of file ndt_map_hmt.h.
float lslgeneric::NDTMapHMT< PointT >::centery [protected] |
Reimplemented from lslgeneric::NDTMap< PointT >.
Definition at line 276 of file ndt_map_hmt.h.
float lslgeneric::NDTMapHMT< PointT >::centerz [protected] |
Reimplemented from lslgeneric::NDTMap< PointT >.
Definition at line 276 of file ndt_map_hmt.h.
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 291 of file ndt_map_hmt.h.
lslgeneric::LazyGrid<PointT>* lslgeneric::NDTMapHMT< PointT >::grid_[3][3] [protected] |
Definition at line 284 of file ndt_map_hmt.h.
bool lslgeneric::NDTMapHMT< PointT >::grids_init [protected] |
Definition at line 277 of file ndt_map_hmt.h.
SpatialIndex<PointT>* lslgeneric::NDTMapHMT< PointT >::index_ [protected] |
Reimplemented from lslgeneric::NDTMap< PointT >.
Definition at line 271 of file ndt_map_hmt.h.
bool lslgeneric::NDTMapHMT< PointT >::is3D [protected] |
Reimplemented from lslgeneric::NDTMap< PointT >.
Definition at line 269 of file ndt_map_hmt.h.
bool lslgeneric::NDTMapHMT< PointT >::isFirstLoad_ [protected] |
Reimplemented from lslgeneric::NDTMap< PointT >.
Definition at line 272 of file ndt_map_hmt.h.
Eigen::Vector3d lslgeneric::NDTMapHMT< PointT >::last_insert [protected] |
Definition at line 278 of file ndt_map_hmt.h.
float lslgeneric::NDTMapHMT< PointT >::map_sizex [protected] |
Reimplemented from lslgeneric::NDTMap< PointT >.
Definition at line 273 of file ndt_map_hmt.h.
float lslgeneric::NDTMapHMT< PointT >::map_sizey [protected] |
Reimplemented from lslgeneric::NDTMap< PointT >.
Definition at line 274 of file ndt_map_hmt.h.
float lslgeneric::NDTMapHMT< PointT >::map_sizez [protected] |
Reimplemented from lslgeneric::NDTMap< PointT >.
Definition at line 275 of file ndt_map_hmt.h.
double lslgeneric::NDTMapHMT< PointT >::max_range_ [protected] |
Definition at line 283 of file ndt_map_hmt.h.
std::string lslgeneric::NDTMapHMT< PointT >::my_directory [protected] |
Definition at line 279 of file ndt_map_hmt.h.
double lslgeneric::NDTMapHMT< PointT >::resolution [protected] |
Definition at line 280 of file ndt_map_hmt.h.
bool lslgeneric::NDTMapHMT< PointT >::saveOnDelete [protected] |
Definition at line 270 of file ndt_map_hmt.h.
std::set<NDTCell<PointT>*> lslgeneric::NDTMapHMT< PointT >::update_set [protected] |
Reimplemented from lslgeneric::NDTMap< PointT >.
Definition at line 281 of file ndt_map_hmt.h.