#include <mrpt/slam/COccupancyGridMap2D.h>
Classes | |
struct | TCriticalPointsList |
The structure used to store the set of Voronoi diagram critical points. More... | |
struct | TEntropyInfo |
Used for returning entropy related information. More... | |
class | TInsertionOptions |
With this struct options are provided to the observation insertion process. More... | |
class | TLikelihoodOptions |
With this struct options are provided to the observation likelihood computation process. More... | |
class | TLikelihoodOutput |
Some members of this struct will contain intermediate or output data after calling "computeObservationLikelihood" for some likelihood functions. More... | |
struct | TUpdateCellsInfoChangeOnly |
An internal structure for storing data related to counting the new information apported by some observation. More... | |
Public Types | |
enum | TLikelihoodMethod { lmMeanInformation = 0, lmRayTracing, lmConsensus, lmCellsDifference, lmLikelihoodField_Thrun, lmLikelihoodField_II, lmConsensusOWA } |
The type for selecting a likelihood computation method. More... | |
typedef std::pair< double, CPoint2D > | TPairLikelihoodIndex |
Auxiliary private class. | |
Public Member Functions | |
void | updateCell (int x, int y, float v) |
Performs the Bayesian fusion of a new observation of a cell. | |
COccupancyGridMap2D (float min_x=-20.0f, float max_x=20.0f, float min_y=-20.0f, float max_y=20.0f, float resolution=0.05f) | |
Constructor. | |
void | clear () |
Clear the map: It set all cells to their default occupancy value (0.5), without changing the resolution (the grid extension is reset to the default values). | |
void | fill (float default_value=0.5f) |
Fills all the cells with a default value. | |
virtual | ~COccupancyGridMap2D () |
Destructor. | |
void | setSize (float x_min, float x_max, float y_min, float y_max, float resolution, float default_value=0.5f) |
Change the size of gridmap, erasing all its previous contents. | |
void | resizeGrid (float new_x_min, float new_x_max, float new_y_min, float new_y_max, float new_cells_default_value=0.5f, bool additionalMargin=true) MRPT_NO_THROWS |
Change the size of gridmap, maintaining previous contents. | |
unsigned int | getSizeX () const |
Returns the horizontal size of grid map in cells count. | |
unsigned int | getSizeY () const |
Returns the vertical size of grid map in cells count. | |
float | getXMin () const |
Returns the "x" coordinate of left side of grid map. | |
float | getXMax () const |
Returns the "x" coordinate of right side of grid map. | |
float | getYMin () const |
Returns the "y" coordinate of top side of grid map. | |
float | getYMax () const |
Returns the "y" coordinate of bottom side of grid map. | |
float | getResolution () const |
Returns the resolution of the grid map. | |
int | x2idx (float x) const |
Transform a coordinate value into a cell index. | |
int | y2idx (float y) const |
int | x2idx (double x) const |
int | y2idx (double y) const |
float | idx2x (const size_t &cx) const |
Transform a cell index into a coordinate value. | |
float | idx2y (const size_t &cy) const |
int | x2idx (float x, float x_min) const |
Transform a coordinate value into a cell index, using a diferent "x_min" value. | |
int | y2idx (float y, float y_min) const |
void | setCell (int x, int y, float value) |
Change the contents [0,1] of a cell, given its index. | |
float | getCell (int x, int y) const |
Read the real valued [0,1] contents of a cell, given its index. | |
cellType * | getRow (int cy) |
Access to a "row": mainly used for drawing grid as a bitmap efficiently, do not use it normally. | |
const cellType * | getRow (int cy) const |
Access to a "row": mainly used for drawing grid as a bitmap efficiently, do not use it normally. | |
void | setPos (float x, float y, float value) |
Change the contents [0,1] of a cell, given its coordinates. | |
float | getPos (float x, float y) const |
Read the real valued [0,1] contents of a cell, given its coordinates. | |
bool | isStaticPos (float x, float y, float threshold=0.7f) const |
Returns "true" if cell is "static", i.e.if its occupancy is below a given threshold. | |
bool | isStaticCell (int cx, int cy, float threshold=0.7f) const |
void | setBasisCell (int x, int y, unsigned char value) |
Change a cell in the "basis" maps.Used for Voronoi calculation. | |
unsigned char | getBasisCell (int x, int y) const |
Reads a cell in the "basis" maps.Used for Voronoi calculation. | |
void | computeCriticalPoints (CPointsMap &outPoints, int nPoints=100) const |
Returns a set of "critical" points in the grid, according to Computer Vision techniques (. | |
void | extractPanoramicFeatures (mrpt::slam::CLandmarksMap &outMap, unsigned int nFeatures=100, unsigned int nDirectionSectors=32, unsigned int nRangeSectors=8, float startDistance=0.1f, float endDistance=3.0f) const |
Computes a set of very distinctive landmarks from the occupancy grid, and store them (previous content is not erased!) into the given landmarks map. | |
void | extractPanoramicFeaturesCached (mrpt::slam::CLandmarksMap &outMap, unsigned int nFeatures=100, unsigned int nDirectionSectors=32, unsigned int nRangeSectors=8, float startDistance=0.1f, float endDistance=3.0f) const |
The same that "extractPanoramicFeatures", but this method saves the computed landmark in an internal buffer, so it is only computed the first time (REMEMBER to call "resetPanoramicFeaturesCache" if the gridmap has been modified and you really want the features to be recomputed!!!). | |
void | resetPanoramicFeaturesCache () |
Reset the internal cache for "extractPanoramicFeaturesCached", you must call this method if the gridmap has changed to the landmarks to be recomputed. | |
void | subSample (int downRatio) |
Performs a downsampling of the gridmap, by a given factor: resolution/=ratio. | |
bool | insertObservation (const CObservation *obs, const CPose3D *robotPose=NULL) |
Insert the observation information into this map. | |
void | computeEntropy (TEntropyInfo &info) const |
Computes the entropy and related values of this grid map. | |
int | getVoroniClearance (int cx, int cy) |
Reads a the clearance of a cell, after building the Voronoi diagram. | |
void | setVoroniClearance (int cx, int cy, int dist) |
Used to set the clearance of a cell, while building the Voronoi diagram. | |
void | buildVoronoiDiagram (float threshold, float robot_size, int x1=0, int x2=0, int y1=0, int y2=0) |
Build the Voronoi diagram of the grid map. | |
void | findCriticalPoints (float filter_distance) |
Builds a list with the critical points from Voronoi diagram, which must must be built before calling this method. | |
int | computeClearance (int cx, int cy, int *basis_x, int *basis_y, int *nBasis, bool GetContourPoint=false) const |
Compute the clearance of a given cell, and returns its two first basis (closest obstacle) points.Used to build Voronoi and critical points. | |
float | computeClearance (float x, float y, float maxSearchDistance) const |
An alternative method for computing the clearance of a given location (in meters). | |
float | computePathCost (float x1, float y1, float x2, float y2) const |
Compute the 'cost' of traversing a segment of the map according to the occupancy of traversed cells. | |
void | laserScanSimulator (CObservation2DRangeScan &inout_Scan, const CPose2D &robotPose, float threshold=0.5f, int N=361, float noiseStd=0, unsigned int decimation=1, float angleNoiseStd=DEG2RAD(0)) const |
Simulates a range scan into the current grid map. | |
double | computeObservationLikelihood (const CObservation *obs, const CPose3D &takenFrom) |
Computes the likelihood that a given observation was taken from a given pose in the world being modeled with this map. | |
bool | canComputeObservationLikelihood (const CObservation *obs) |
Returns true if this map is able to compute a sensible likelihood function for this observation (i.e. | |
double | computeLikelihoodField_Thrun (const CPointsMap *pm, const CPose2D *relativePose=NULL) |
Computes the likelihood [0,1] of a set of points, given the current grid map as reference. | |
double | computeLikelihoodField_II (const CPointsMap *pm, const CPose2D *relativePose=NULL) |
Computes the likelihood [0,1] of a set of points, given the current grid map as reference. | |
bool | saveAsBitmapFile (const std::string &file) const |
Saves the gridmap as a graphical file (BMP,PNG,. | |
bool | saveAsBitmapFileWithLandmarks (const std::string &file, const mrpt::slam::CLandmarksMap *landmarks) const |
Saves the gridmap as a graphical bitmap file, 8 bit gray scale, 1 pixel is 1 cell, and with an overlay of landmarks. | |
void | getAsImage (utils::CMRPTImageFloat &img, bool verticalFlip=false) const |
Returns the grid as a float image, where each pixel is a cell. | |
void | getAsImage (utils::CMRPTImage &img, bool verticalFlip=false, bool forceRGB=false, bool tricolor=false) const |
Returns the grid as a 8-bit graylevel image, where each pixel is a cell (output image is RGB only if forceRGB is true) If "tricolor" is true, only three gray levels will appear in the image: gray for unobserved cells, and black/white for occupied/empty cells respectively. | |
void | getAsImageFiltered (utils::CMRPTImage &img, bool verticalFlip=false, bool forceRGB=false) const |
Returns the grid as a 8-bit graylevel image, where each pixel is a cell (output image is RGB only if forceRGB is true) - This method filters the image for easy feature detection If "tricolor" is true, only three gray levels will appear in the image: gray for unobserved cells, and black/white for occupied/empty cells respectively. | |
void | getAs3DObject (mrpt::opengl::CSetOfObjectsPtr &outObj) const |
Returns a 3D plane with its texture being the occupancy grid and transparency proportional to "uncertainty" (i.e. | |
bool | isEmpty () const |
Returns true if the map is empty/no observation has been inserted. | |
bool | loadFromBitmapFile (const std::string &file, float resolution, float xCentralPixel=-1, float yCentralPixel=-1) |
Load the gridmap from a image in a file (the format can be any supported by CMRPTImage::loadFromFile). | |
bool | loadFromBitmap (const mrpt::utils::CMRPTImage &img, float resolution, float xCentralPixel=-1, float yCentralPixel=-1) |
Load the gridmap from a image in a file (the format can be any supported by CMRPTImage::loadFromFile). | |
void | computeMatchingWith2D (const CMetricMap *otherMap, const CPose2D &otherMapPose, float maxDistForCorrespondence, float maxAngularDistForCorrespondence, const CPose2D &angularDistPivotPoint, TMatchingPairList &correspondences, float &correspondencesRatio, float *sumSqrDist=NULL, bool onlyKeepTheClosest=false, bool onlyUniqueRobust=false) const |
See the base class for more details: In this class it is implemented as correspondences of the passed points map to occupied cells. | |
float | compute3DMatchingRatio (const CMetricMap *otherMap, const CPose3D &otherMapPose, float minDistForCorr=0.10f, float minMahaDistForCorr=2.0f) const |
Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose" In the case of a multi-metric map, this returns the average between the maps. | |
void | saveMetricMapRepresentationToFile (const std::string &filNamePrefix) const |
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >, as an image or in any other applicable way (Notice that other methods to save the map may be implemented in classes implementing this virtual interface). | |
void | auxParticleFilterCleanUp () |
This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation". | |
Static Public Member Functions | |
static void | updateCell_fast_occupied (const unsigned &x, const unsigned &y, const cellType &logodd_obs, const cellType &thres, cellType *mapArray, const unsigned &_size_x) |
Performs the Bayesian fusion of a new observation of a cell, without checking for grid limits nor updateInfoChangeOnly. | |
static void | updateCell_fast_occupied (cellType *theCell, const cellType &logodd_obs, const cellType &thres) |
Performs the Bayesian fusion of a new observation of a cell, without checking for grid limits nor updateInfoChangeOnly. | |
static void | updateCell_fast_free (const unsigned &x, const unsigned &y, const cellType &logodd_obs, const cellType &thres, cellType *mapArray, const unsigned &_size_x) |
Performs the Bayesian fusion of a new observation of a cell, without checking for grid limits nor updateInfoChangeOnly. | |
static void | updateCell_fast_free (cellType *theCell, const cellType &logodd_obs, const cellType &thres) |
Performs the Bayesian fusion of a new observation of a cell, without checking for grid limits nor updateInfoChangeOnly. | |
static float | l2p (const cellType &l) |
Scales an integer representation of the log-odd into a real valued probability in [0,1], using p=exp(l)/(1+exp(l)). | |
static uint8_t | l2p_255 (const cellType &l) |
Scales an integer representation of the log-odd into a linear scale [0,255], using p=exp(l)/(1+exp(l)). | |
static cellType | p2l (const float &p) |
Scales a real valued probability in [0,1] to an integer representation of: log(p)-log(1-p) in the valid range of cellType. | |
static bool | saveAsBitmapTwoMapsWithCorrespondences (const std::string &fileName, const COccupancyGridMap2D *m1, const COccupancyGridMap2D *m2, const CMetricMap::TMatchingPairList &corrs) |
Saves a composite image with two gridmaps and lines representing a set of correspondences between them. | |
static bool | saveAsEMFTwoMapsWithCorrespondences (const std::string &fileName, const COccupancyGridMap2D *m1, const COccupancyGridMap2D *m2, const CMetricMap::TMatchingPairList &corrs) |
Saves a composite image with two gridmaps and numbers for the correspondences between them. | |
Public Attributes | |
struct MRPTDLLIMPEXP mrpt::slam::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly | updateInfoChangeOnly |
An internal structure for storing data related to counting the new information apported by some observation. | |
TInsertionOptions | insertionOptions |
With this struct options are provided to the observation insertion process. | |
mrpt::slam::COccupancyGridMap2D::TLikelihoodOptions | likelihoodOptions |
With this struct options are provided to the observation likelihood computation process. | |
class mrpt::slam::COccupancyGridMap2D::TLikelihoodOutput | likelihoodOutputs |
Some members of this struct will contain intermediate or output data after calling "computeObservationLikelihood" for some likelihood functions. | |
struct MRPTDLLIMPEXP mrpt::slam::COccupancyGridMap2D::TCriticalPointsList | CriticalPointsList |
The structure used to store the set of Voronoi diagram critical points. | |
Protected Member Functions | |
void | freeMap () |
Frees the dynamic memory buffers of map. | |
void | setCell_nocheck (int x, int y, float value) |
Change the contents [0,1] of a cell, given its index. | |
float | getCell_nocheck (int x, int y) const |
Read the real valued [0,1] contents of a cell, given its index. | |
void | setRawCell (unsigned int cellIndex, cellType b) |
Changes a cell by its absolute index (Do not use it normally). | |
double | computeObservationLikelihood_Consensus (const CObservation *obs, const CPose2D &takenFrom) |
One of the methods that can be selected for implementing "computeObservationLikelihood" (This method is the Range-Scan Likelihood Consensus for gridmaps, see the ICRA2007 paper by Blanco et al. | |
double | computeObservationLikelihood_ConsensusOWA (const CObservation *obs, const CPose2D &takenFrom) |
One of the methods that can be selected for implementing "computeObservationLikelihood" TODO: This method is described in. | |
double | computeObservationLikelihood_CellsDifference (const CObservation *obs, const CPose2D &takenFrom) |
One of the methods that can be selected for implementing "computeObservationLikelihood". | |
double | computeObservationLikelihood_MI (const CObservation *obs, const CPose2D &takenFrom) |
One of the methods that can be selected for implementing "computeObservationLikelihood". | |
double | computeObservationLikelihood_rayTracing (const CObservation *obs, const CPose2D &takenFrom) |
One of the methods that can be selected for implementing "computeObservationLikelihood". | |
double | computeObservationLikelihood_likelihoodField_Thrun (const CObservation *obs, const CPose2D &takenFrom) |
One of the methods that can be selected for implementing "computeObservationLikelihood". | |
double | computeObservationLikelihood_likelihoodField_II (const CObservation *obs, const CPose2D &takenFrom) |
One of the methods that can be selected for implementing "computeObservationLikelihood". | |
Static Protected Member Functions | |
static double | H (double p) |
Entropy computation internal function:. | |
Protected Attributes | |
std::vector< cellType > | map |
This is the buffer for storing the cells.In this dynamic size buffer are stored the cell values as "bytes", stored row by row, from left to right cells. | |
uint32_t | size_x |
The size of the grid in cells. | |
uint32_t | size_y |
float | x_min |
The limits of the grid in "units" (meters). | |
float | x_max |
float | y_min |
float | y_max |
float | resolution |
Cell size, i.e. | |
std::vector< double > | precomputedLikelihood |
These are auxiliary variables to speed up the computation of observation likelihood values for LF method among others, at a high cost in memory (see TLikelihoodOptions::enableLikelihoodCache). | |
bool | precomputedLikelihoodToBeRecomputed |
std::vector< unsigned char > | basis_map |
Used for Voronoi calculation.Same struct as "map", but contains a "0" if not a basis point. | |
std::vector< int > | voronoi_diagram |
Used to store the Voronoi diagram.Same struct as "map", but contains the distance of each cell to its closer obstacles in 1/100 of "units", or 0 if not into the Voronoi diagram. | |
float | voroni_free_threshold |
The free-cells threshold used to compute the Voronoi diagram. | |
Static Protected Attributes | |
static std::vector< float > | entropyTable |
Internally used to speed-up entropy calculation. | |
static std::vector< float > | logoddsTable |
A lookup table to compute occupancy probabilities in [0,1] from integer log-odds values in the cells, using ![]() | |
static std::vector< uint8_t > | logoddsTable_255 |
A lookup table to compute occupancy probabilities in the range [0,255] from integer log-odds values in the cells, using ![]() | |
static float * | logoddsTablePtr |
A pointer to the vector logoddsTable. | |
static uint8_t * | logoddsTable_255Ptr |
A pointer to the vector logoddsTable_255. | |
static std::vector< cellType > | p2lTable |
A lookup table for passing from float to log-odds as cellType. | |
static cellType * | p2lTablePtr |
A pointer to the vector p2lTable. | |
Private Member Functions | |
unsigned char | GetNeighborhood (int cx, int cy) const |
Returns a byte with the occupancy of the 8 sorrounding cells. | |
int | direction2idx (int dx, int dy) |
Returns the index [0,7] of the given movement, or -1 if invalid one. | |
Private Attributes | |
mrpt::slam::CLandmarksMapPtr | m_panoramicFeaturesCache |
Internally used in "extractPanoramicFeaturesCached". | |
int | direccion_vecino_x [8] |
Used to store the 8 possible movements from a cell to the sorrounding ones.Filled in the constructor. | |
int | direccion_vecino_y [8] |
Friends | |
class | CMultiMetricMap |
class | CMultiMetricMapPDF |
COccupancyGridMap2D is a class for storing a metric map representation in the form of a probabilistic occupancy grid map: value of 0 means certainly occupied, 1 means a certainly empty cell. Initially 0.5 means uncertainty.
Since MRPT 0.5.5, the cells keep the log-odd representation of probabilities instead of the probabilities themselves. More details can be found at http://babel.isa.uma.es/mrpt/index.php/Occupancy_Grids
The algorithm for updating the grid from a laser scanner takes into account the progressive widening of the beams, as described in the wiki (this feature was introduced in MRPT 0.6.4).
Some implemented methods are:
Definition at line 109 of file COccupancyGridMap2D.h.
typedef std::pair<double,CPoint2D> mrpt::slam::COccupancyGridMap2D::TPairLikelihoodIndex |
The type for selecting a likelihood computation method.
lmMeanInformation | |
lmRayTracing | |
lmConsensus | |
lmCellsDifference | |
lmLikelihoodField_Thrun | |
lmLikelihoodField_II | |
lmConsensusOWA |
Definition at line 689 of file COccupancyGridMap2D.h.
mrpt::slam::COccupancyGridMap2D::COccupancyGridMap2D | ( | float | min_x = -20.0f , |
|
float | max_x = 20.0f , |
|||
float | min_y = -20.0f , |
|||
float | max_y = 20.0f , |
|||
float | resolution = 0.05f | |||
) |
Constructor.
virtual mrpt::slam::COccupancyGridMap2D::~COccupancyGridMap2D | ( | ) | [virtual] |
Destructor.
void mrpt::slam::COccupancyGridMap2D::auxParticleFilterCleanUp | ( | ) | [virtual] |
This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation".
This method should normally do nothing, but in some cases can be used to free auxiliary cached variables.
Implements mrpt::slam::CMetricMap.
void mrpt::slam::COccupancyGridMap2D::buildVoronoiDiagram | ( | float | threshold, | |
float | robot_size, | |||
int | x1 = 0 , |
|||
int | x2 = 0 , |
|||
int | y1 = 0 , |
|||
int | y2 = 0 | |||
) |
Build the Voronoi diagram of the grid map.
threshold | The threshold for binarizing the map. | |
robot_size | Size in "units" (meters) of robot, approx. | |
x1 | Left coordinate of area to be computed. Default, entire map. | |
x2 | Right coordinate of area to be computed. Default, entire map. | |
y1 | Top coordinate of area to be computed. Default, entire map. | |
y2 | Bottom coordinate of area to be computed. Default, entire map. |
bool mrpt::slam::COccupancyGridMap2D::canComputeObservationLikelihood | ( | const CObservation * | obs | ) | [virtual] |
Returns true if this map is able to compute a sensible likelihood function for this observation (i.e.
an occupancy grid map cannot with an image).
obs | The observation. |
Reimplemented from mrpt::slam::CMetricMap.
void mrpt::slam::COccupancyGridMap2D::clear | ( | ) | [virtual] |
Clear the map: It set all cells to their default occupancy value (0.5), without changing the resolution (the grid extension is reset to the default values).
Implements mrpt::slam::CMetricMap.
float mrpt::slam::COccupancyGridMap2D::compute3DMatchingRatio | ( | const CMetricMap * | otherMap, | |
const CPose3D & | otherMapPose, | |||
float | minDistForCorr = 0.10f , |
|||
float | minMahaDistForCorr = 2.0f | |||
) | const [virtual] |
Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map, whose 6D pose relative to "this" is "otherMapPose" In the case of a multi-metric map, this returns the average between the maps.
This method always return 0 for grid maps.
otherMap | [IN] The other map to compute the matching with. | |
otherMapPose | [IN] The 6D pose of the other map as seen from "this". | |
minDistForCorr | [IN] The minimum distance between 2 non-probabilistic map elements for counting them as a correspondence. | |
minMahaDistForCorr | [IN] The minimum Mahalanobis distance between 2 probabilistic map elements for counting them as a correspondence. |
Implements mrpt::slam::CMetricMap.
float mrpt::slam::COccupancyGridMap2D::computeClearance | ( | float | x, | |
float | y, | |||
float | maxSearchDistance | |||
) | const |
An alternative method for computing the clearance of a given location (in meters).
int mrpt::slam::COccupancyGridMap2D::computeClearance | ( | int | cx, | |
int | cy, | |||
int * | basis_x, | |||
int * | basis_y, | |||
int * | nBasis, | |||
bool | GetContourPoint = false | |||
) | const |
Compute the clearance of a given cell, and returns its two first basis (closest obstacle) points.Used to build Voronoi and critical points.
cx | The cell index | |
cy | The cell index | |
basis_x | Target buffer for coordinates of basis, having a size of two "ints". | |
basis_y | Target buffer for coordinates of basis, having a size of two "ints". | |
nBasis | The number of found basis: Can be 0,1 or 2. | |
GetContourPoint | If "true" the basis are not returned, but the closest free cells.Default at false. |
void mrpt::slam::COccupancyGridMap2D::computeCriticalPoints | ( | CPointsMap & | outPoints, | |
int | nPoints = 100 | |||
) | const |
Returns a set of "critical" points in the grid, according to Computer Vision techniques (.
..)
void mrpt::slam::COccupancyGridMap2D::computeEntropy | ( | TEntropyInfo & | info | ) | const |
Computes the entropy and related values of this grid map.
The entropy is computed as the summed entropy of each cell, taking them as discrete random variables following a Bernoulli distribution:
info | The output information is returned here. |
double mrpt::slam::COccupancyGridMap2D::computeLikelihoodField_II | ( | const CPointsMap * | pm, | |
const CPose2D * | relativePose = NULL | |||
) |
Computes the likelihood [0,1] of a set of points, given the current grid map as reference.
pm | The points map | |
relativePose | The relative pose of the points map in this map's coordinates, or NULL for (0,0,0). See "likelihoodOptions" for configuration parameters. |
double mrpt::slam::COccupancyGridMap2D::computeLikelihoodField_Thrun | ( | const CPointsMap * | pm, | |
const CPose2D * | relativePose = NULL | |||
) |
Computes the likelihood [0,1] of a set of points, given the current grid map as reference.
pm | The points map | |
relativePose | The relative pose of the points map in this map's coordinates, or NULL for (0,0,0). See "likelihoodOptions" for configuration parameters. |
void mrpt::slam::COccupancyGridMap2D::computeMatchingWith2D | ( | const CMetricMap * | otherMap, | |
const CPose2D & | otherMapPose, | |||
float | maxDistForCorrespondence, | |||
float | maxAngularDistForCorrespondence, | |||
const CPose2D & | angularDistPivotPoint, | |||
TMatchingPairList & | correspondences, | |||
float & | correspondencesRatio, | |||
float * | sumSqrDist = NULL , |
|||
bool | onlyKeepTheClosest = false , |
|||
bool | onlyUniqueRobust = false | |||
) | const [virtual] |
See the base class for more details: In this class it is implemented as correspondences of the passed points map to occupied cells.
NOTICE: That the "z" dimension is ignored in the points. Clip the points as appropiated if needed before calling this method.
Reimplemented from mrpt::slam::CMetricMap.
double mrpt::slam::COccupancyGridMap2D::computeObservationLikelihood | ( | const CObservation * | obs, | |
const CPose3D & | takenFrom | |||
) | [virtual] |
Computes the likelihood that a given observation was taken from a given pose in the world being modeled with this map.
See "likelihoodOptions" for configuration parameters.
takenFrom | The robot's pose the observation is supposed to be taken from. | |
obs | The observation. |
Implements mrpt::slam::CMetricMap.
double mrpt::slam::COccupancyGridMap2D::computeObservationLikelihood_CellsDifference | ( | const CObservation * | obs, | |
const CPose2D & | takenFrom | |||
) | [protected] |
One of the methods that can be selected for implementing "computeObservationLikelihood".
double mrpt::slam::COccupancyGridMap2D::computeObservationLikelihood_Consensus | ( | const CObservation * | obs, | |
const CPose2D & | takenFrom | |||
) | [protected] |
One of the methods that can be selected for implementing "computeObservationLikelihood" (This method is the Range-Scan Likelihood Consensus for gridmaps, see the ICRA2007 paper by Blanco et al.
)
double mrpt::slam::COccupancyGridMap2D::computeObservationLikelihood_ConsensusOWA | ( | const CObservation * | obs, | |
const CPose2D & | takenFrom | |||
) | [protected] |
One of the methods that can be selected for implementing "computeObservationLikelihood" TODO: This method is described in.
...
double mrpt::slam::COccupancyGridMap2D::computeObservationLikelihood_likelihoodField_II | ( | const CObservation * | obs, | |
const CPose2D & | takenFrom | |||
) | [protected] |
One of the methods that can be selected for implementing "computeObservationLikelihood".
double mrpt::slam::COccupancyGridMap2D::computeObservationLikelihood_likelihoodField_Thrun | ( | const CObservation * | obs, | |
const CPose2D & | takenFrom | |||
) | [protected] |
One of the methods that can be selected for implementing "computeObservationLikelihood".
double mrpt::slam::COccupancyGridMap2D::computeObservationLikelihood_MI | ( | const CObservation * | obs, | |
const CPose2D & | takenFrom | |||
) | [protected] |
One of the methods that can be selected for implementing "computeObservationLikelihood".
double mrpt::slam::COccupancyGridMap2D::computeObservationLikelihood_rayTracing | ( | const CObservation * | obs, | |
const CPose2D & | takenFrom | |||
) | [protected] |
One of the methods that can be selected for implementing "computeObservationLikelihood".
float mrpt::slam::COccupancyGridMap2D::computePathCost | ( | float | x1, | |
float | y1, | |||
float | x2, | |||
float | y2 | |||
) | const |
Compute the 'cost' of traversing a segment of the map according to the occupancy of traversed cells.
int mrpt::slam::COccupancyGridMap2D::direction2idx | ( | int | dx, | |
int | dy | |||
) | [private] |
Returns the index [0,7] of the given movement, or -1 if invalid one.
void mrpt::slam::COccupancyGridMap2D::extractPanoramicFeatures | ( | mrpt::slam::CLandmarksMap & | outMap, | |
unsigned int | nFeatures = 100 , |
|||
unsigned int | nDirectionSectors = 32 , |
|||
unsigned int | nRangeSectors = 8 , |
|||
float | startDistance = 0.1f , |
|||
float | endDistance = 3.0f | |||
) | const |
Computes a set of very distinctive landmarks from the occupancy grid, and store them (previous content is not erased!) into the given landmarks map.
Landmarks type is "mrpt::slam::CLandmark::glPanoramicDescriptor" This technique was reported in the paper "..."
void mrpt::slam::COccupancyGridMap2D::extractPanoramicFeaturesCached | ( | mrpt::slam::CLandmarksMap & | outMap, | |
unsigned int | nFeatures = 100 , |
|||
unsigned int | nDirectionSectors = 32 , |
|||
unsigned int | nRangeSectors = 8 , |
|||
float | startDistance = 0.1f , |
|||
float | endDistance = 3.0f | |||
) | const |
The same that "extractPanoramicFeatures", but this method saves the computed landmark in an internal buffer, so it is only computed the first time (REMEMBER to call "resetPanoramicFeaturesCache" if the gridmap has been modified and you really want the features to be recomputed!!!).
void mrpt::slam::COccupancyGridMap2D::fill | ( | float | default_value = 0.5f |
) |
Fills all the cells with a default value.
void mrpt::slam::COccupancyGridMap2D::findCriticalPoints | ( | float | filter_distance | ) |
Builds a list with the critical points from Voronoi diagram, which must must be built before calling this method.
filter_distance | The minimum distance between two critical points. |
void mrpt::slam::COccupancyGridMap2D::freeMap | ( | ) | [protected] |
Frees the dynamic memory buffers of map.
void mrpt::slam::COccupancyGridMap2D::getAs3DObject | ( | mrpt::opengl::CSetOfObjectsPtr & | outObj | ) | const [virtual] |
Returns a 3D plane with its texture being the occupancy grid and transparency proportional to "uncertainty" (i.e.
a value of 0.5 is fully transparent)
Implements mrpt::slam::CMetricMap.
void mrpt::slam::COccupancyGridMap2D::getAsImage | ( | utils::CMRPTImage & | img, | |
bool | verticalFlip = false , |
|||
bool | forceRGB = false , |
|||
bool | tricolor = false | |||
) | const |
Returns the grid as a 8-bit graylevel image, where each pixel is a cell (output image is RGB only if forceRGB is true) If "tricolor" is true, only three gray levels will appear in the image: gray for unobserved cells, and black/white for occupied/empty cells respectively.
void mrpt::slam::COccupancyGridMap2D::getAsImage | ( | utils::CMRPTImageFloat & | img, | |
bool | verticalFlip = false | |||
) | const |
Returns the grid as a float image, where each pixel is a cell.
void mrpt::slam::COccupancyGridMap2D::getAsImageFiltered | ( | utils::CMRPTImage & | img, | |
bool | verticalFlip = false , |
|||
bool | forceRGB = false | |||
) | const |
Returns the grid as a 8-bit graylevel image, where each pixel is a cell (output image is RGB only if forceRGB is true) - This method filters the image for easy feature detection If "tricolor" is true, only three gray levels will appear in the image: gray for unobserved cells, and black/white for occupied/empty cells respectively.
unsigned char mrpt::slam::COccupancyGridMap2D::getBasisCell | ( | int | x, | |
int | y | |||
) | const [inline] |
Reads a cell in the "basis" maps.Used for Voronoi calculation.
Definition at line 546 of file COccupancyGridMap2D.h.
float mrpt::slam::COccupancyGridMap2D::getCell | ( | int | x, | |
int | y | |||
) | const [inline] |
Read the real valued [0,1] contents of a cell, given its index.
Definition at line 511 of file COccupancyGridMap2D.h.
float mrpt::slam::COccupancyGridMap2D::getCell_nocheck | ( | int | x, | |
int | y | |||
) | const [inline, protected] |
Read the real valued [0,1] contents of a cell, given its index.
Definition at line 188 of file COccupancyGridMap2D.h.
unsigned char mrpt::slam::COccupancyGridMap2D::GetNeighborhood | ( | int | cx, | |
int | cy | |||
) | const [inline, private] |
Returns a byte with the occupancy of the 8 sorrounding cells.
cx | The cell index | |
cy | The cell index |
float mrpt::slam::COccupancyGridMap2D::getPos | ( | float | x, | |
float | y | |||
) | const [inline] |
Read the real valued [0,1] contents of a cell, given its coordinates.
Definition at line 533 of file COccupancyGridMap2D.h.
float mrpt::slam::COccupancyGridMap2D::getResolution | ( | ) | const [inline] |
const cellType* mrpt::slam::COccupancyGridMap2D::getRow | ( | int | cy | ) | const [inline] |
Access to a "row": mainly used for drawing grid as a bitmap efficiently, do not use it normally.
Definition at line 525 of file COccupancyGridMap2D.h.
cellType* mrpt::slam::COccupancyGridMap2D::getRow | ( | int | cy | ) | [inline] |
Access to a "row": mainly used for drawing grid as a bitmap efficiently, do not use it normally.
Definition at line 521 of file COccupancyGridMap2D.h.
unsigned int mrpt::slam::COccupancyGridMap2D::getSizeX | ( | ) | const [inline] |
Returns the horizontal size of grid map in cells count.
Definition at line 434 of file COccupancyGridMap2D.h.
unsigned int mrpt::slam::COccupancyGridMap2D::getSizeY | ( | ) | const [inline] |
Returns the vertical size of grid map in cells count.
Definition at line 438 of file COccupancyGridMap2D.h.
int mrpt::slam::COccupancyGridMap2D::getVoroniClearance | ( | int | cx, | |
int | cy | |||
) | [inline] |
Reads a the clearance of a cell, after building the Voronoi diagram.
Definition at line 843 of file COccupancyGridMap2D.h.
float mrpt::slam::COccupancyGridMap2D::getXMax | ( | ) | const [inline] |
Returns the "x" coordinate of right side of grid map.
Definition at line 446 of file COccupancyGridMap2D.h.
float mrpt::slam::COccupancyGridMap2D::getXMin | ( | ) | const [inline] |
Returns the "x" coordinate of left side of grid map.
Definition at line 442 of file COccupancyGridMap2D.h.
float mrpt::slam::COccupancyGridMap2D::getYMax | ( | ) | const [inline] |
Returns the "y" coordinate of bottom side of grid map.
Definition at line 454 of file COccupancyGridMap2D.h.
float mrpt::slam::COccupancyGridMap2D::getYMin | ( | ) | const [inline] |
Returns the "y" coordinate of top side of grid map.
Definition at line 450 of file COccupancyGridMap2D.h.
static double mrpt::slam::COccupancyGridMap2D::H | ( | double | p | ) | [static, protected] |
Entropy computation internal function:.
float mrpt::slam::COccupancyGridMap2D::idx2x | ( | const size_t & | cx | ) | const [inline] |
Transform a cell index into a coordinate value.
Definition at line 470 of file COccupancyGridMap2D.h.
float mrpt::slam::COccupancyGridMap2D::idx2y | ( | const size_t & | cy | ) | const [inline] |
Definition at line 471 of file COccupancyGridMap2D.h.
bool mrpt::slam::COccupancyGridMap2D::insertObservation | ( | const CObservation * | obs, | |
const CPose3D * | robotPose = NULL | |||
) | [virtual] |
Insert the observation information into this map.
obs | The observation | |
robotPose | The 3D pose of the robot mobile base in the map reference system, or NULL (default) if you want to use CPose2D(0,0,deg) |
Implements mrpt::slam::CMetricMap.
bool mrpt::slam::COccupancyGridMap2D::isEmpty | ( | ) | const [virtual] |
Returns true if the map is empty/no observation has been inserted.
Implements mrpt::slam::CMetricMap.
bool mrpt::slam::COccupancyGridMap2D::isStaticCell | ( | int | cx, | |
int | cy, | |||
float | threshold = 0.7f | |||
) | const [inline] |
Definition at line 538 of file COccupancyGridMap2D.h.
bool mrpt::slam::COccupancyGridMap2D::isStaticPos | ( | float | x, | |
float | y, | |||
float | threshold = 0.7f | |||
) | const [inline] |
Returns "true" if cell is "static", i.e.if its occupancy is below a given threshold.
Definition at line 537 of file COccupancyGridMap2D.h.
static float mrpt::slam::COccupancyGridMap2D::l2p | ( | const cellType & | l | ) | [inline, static] |
Scales an integer representation of the log-odd into a real valued probability in [0,1], using p=exp(l)/(1+exp(l)).
Definition at line 480 of file COccupancyGridMap2D.h.
References OCCGRID_CELLTYPE_MIN.
static uint8_t mrpt::slam::COccupancyGridMap2D::l2p_255 | ( | const cellType & | l | ) | [inline, static] |
Scales an integer representation of the log-odd into a linear scale [0,255], using p=exp(l)/(1+exp(l)).
Definition at line 487 of file COccupancyGridMap2D.h.
References OCCGRID_CELLTYPE_MIN.
void mrpt::slam::COccupancyGridMap2D::laserScanSimulator | ( | CObservation2DRangeScan & | inout_Scan, | |
const CPose2D & | robotPose, | |||
float | threshold = 0.5f , |
|||
int | N = 361 , |
|||
float | noiseStd = 0 , |
|||
unsigned int | decimation = 1 , |
|||
float | angleNoiseStd = DEG2RAD(0) | |||
) | const |
Simulates a range scan into the current grid map.
The simulated scan is stored in a CObservation2DRangeScan object, which is also used to pass some parameters: all previously stored characteristics (as aperture,...) are taken into account for simulation. Only a few more parameters are needed. Additive gaussian noise can be optionally added to the simulated scan.
inout_Scan | [IN/OUT] This must be filled with desired parameters before calling, and will contain the scan samples on return. | |
robotPose | [IN] The robot pose in this map coordinates. Recall that sensor pose relative to this robot pose must be specified in the observation object. | |
threshold | [IN] The minimum occupancy threshold to consider a cell to be occupied, for example 0.5. | |
N | [IN] The count of range scan "rays", by default to 361. | |
noiseStd | [IN] The standard deviation of measurement noise. If not desired, set to 0. | |
decimation | [IN] The rays that will be simulated are at indexes: 0, D, 2D, 3D, ... Default is D=1 | |
angleNoiseStd | [IN] The sigma of an optional Gaussian noise added to the angles at which ranges are measured (in radians). |
bool mrpt::slam::COccupancyGridMap2D::loadFromBitmap | ( | const mrpt::utils::CMRPTImage & | img, | |
float | resolution, | |||
float | xCentralPixel = -1 , |
|||
float | yCentralPixel = -1 | |||
) |
Load the gridmap from a image in a file (the format can be any supported by CMRPTImage::loadFromFile).
img | The image. Only a grayscale image will be used, so RGB components will be mixed if a color image is passed. | |
resolution | The size of a pixel (cell), in meters. Recall cells are always squared, so just a dimension is needed. | |
xCentralPixel | The "x" coordinate (0=first) for the pixel which will be taken at coordinates origin (0,0). If not supplied, it will be used the middle of the map. | |
yCentralPixel | The "y" coordinate (0=first) for the pixel which will be taken at coordinates origin (0,0). If not supplied, it will be used the middle of the map. |
bool mrpt::slam::COccupancyGridMap2D::loadFromBitmapFile | ( | const std::string & | file, | |
float | resolution, | |||
float | xCentralPixel = -1 , |
|||
float | yCentralPixel = -1 | |||
) |
Load the gridmap from a image in a file (the format can be any supported by CMRPTImage::loadFromFile).
file | The file to be loaded. | |
resolution | The size of a pixel (cell), in meters. Recall cells are always squared, so just a dimension is needed. | |
xCentralPixel | The "x" coordinate (0=first) for the pixel which will be taken at coordinates origin (0,0). If not supplied, it will be used the middle of the map. | |
yCentralPixel | The "y" coordinate (0=first) for the pixel which will be taken at coordinates origin (0,0). If not supplied, it will be used the middle of the map. |
static cellType mrpt::slam::COccupancyGridMap2D::p2l | ( | const float & | p | ) | [inline, static] |
Scales a real valued probability in [0,1] to an integer representation of: log(p)-log(1-p) in the valid range of cellType.
Definition at line 494 of file COccupancyGridMap2D.h.
References OCCGRID_P2LTABLE_SIZE.
void mrpt::slam::COccupancyGridMap2D::resetPanoramicFeaturesCache | ( | ) |
Reset the internal cache for "extractPanoramicFeaturesCached", you must call this method if the gridmap has changed to the landmarks to be recomputed.
void mrpt::slam::COccupancyGridMap2D::resizeGrid | ( | float | new_x_min, | |
float | new_x_max, | |||
float | new_y_min, | |||
float | new_y_max, | |||
float | new_cells_default_value = 0.5f , |
|||
bool | additionalMargin = true | |||
) |
Change the size of gridmap, maintaining previous contents.
new_x_min | The "x" coordinates of new left most side of grid. | |
new_x_max | The "x" coordinates of new right most side of grid. | |
new_y_min | The "y" coordinates of new top most side of grid. | |
new_y_max | The "y" coordinates of new bottom most side of grid. | |
new_cells_default_value | The value of the new cells, tipically 0.5. | |
additionalMargin | If set to true (default), an additional margin of a few meters will be added to the grid, ONLY if the new coordinates are larger than current ones. |
bool mrpt::slam::COccupancyGridMap2D::saveAsBitmapFile | ( | const std::string & | file | ) | const |
Saves the gridmap as a graphical file (BMP,PNG,.
..). The format will be derived from the file extension (see CMRPTImage::saveToFile )
bool mrpt::slam::COccupancyGridMap2D::saveAsBitmapFileWithLandmarks | ( | const std::string & | file, | |
const mrpt::slam::CLandmarksMap * | landmarks | |||
) | const |
Saves the gridmap as a graphical bitmap file, 8 bit gray scale, 1 pixel is 1 cell, and with an overlay of landmarks.
static bool mrpt::slam::COccupancyGridMap2D::saveAsBitmapTwoMapsWithCorrespondences | ( | const std::string & | fileName, | |
const COccupancyGridMap2D * | m1, | |||
const COccupancyGridMap2D * | m2, | |||
const CMetricMap::TMatchingPairList & | corrs | |||
) | [static] |
Saves a composite image with two gridmaps and lines representing a set of correspondences between them.
static bool mrpt::slam::COccupancyGridMap2D::saveAsEMFTwoMapsWithCorrespondences | ( | const std::string & | fileName, | |
const COccupancyGridMap2D * | m1, | |||
const COccupancyGridMap2D * | m2, | |||
const CMetricMap::TMatchingPairList & | corrs | |||
) | [static] |
Saves a composite image with two gridmaps and numbers for the correspondences between them.
void mrpt::slam::COccupancyGridMap2D::saveMetricMapRepresentationToFile | ( | const std::string & | filNamePrefix | ) | const [virtual] |
This virtual method saves the map to a file "filNamePrefix"+< some_file_extension >, as an image or in any other applicable way (Notice that other methods to save the map may be implemented in classes implementing this virtual interface).
Implements mrpt::slam::CMetricMap.
void mrpt::slam::COccupancyGridMap2D::setBasisCell | ( | int | x, | |
int | y, | |||
unsigned char | value | |||
) | [inline] |
Change a cell in the "basis" maps.Used for Voronoi calculation.
Definition at line 542 of file COccupancyGridMap2D.h.
void mrpt::slam::COccupancyGridMap2D::setCell | ( | int | x, | |
int | y, | |||
float | value | |||
) | [inline] |
Change the contents [0,1] of a cell, given its index.
Definition at line 501 of file COccupancyGridMap2D.h.
void mrpt::slam::COccupancyGridMap2D::setCell_nocheck | ( | int | x, | |
int | y, | |||
float | value | |||
) | [inline, protected] |
Change the contents [0,1] of a cell, given its index.
Definition at line 181 of file COccupancyGridMap2D.h.
void mrpt::slam::COccupancyGridMap2D::setPos | ( | float | x, | |
float | y, | |||
float | value | |||
) | [inline] |
Change the contents [0,1] of a cell, given its coordinates.
Definition at line 529 of file COccupancyGridMap2D.h.
void mrpt::slam::COccupancyGridMap2D::setRawCell | ( | unsigned int | cellIndex, | |
cellType | b | |||
) | [inline, protected] |
Changes a cell by its absolute index (Do not use it normally).
Definition at line 195 of file COccupancyGridMap2D.h.
void mrpt::slam::COccupancyGridMap2D::setSize | ( | float | x_min, | |
float | x_max, | |||
float | y_min, | |||
float | y_max, | |||
float | resolution, | |||
float | default_value = 0.5f | |||
) |
Change the size of gridmap, erasing all its previous contents.
x_min | The "x" coordinates of left most side of grid. | |
x_max | The "x" coordinates of right most side of grid. | |
y_min | The "y" coordinates of top most side of grid. | |
y_max | The "y" coordinates of bottom most side of grid. | |
resolution | The new size of cells. | |
default_value | The value of cells, tipically 0.5. |
void mrpt::slam::COccupancyGridMap2D::setVoroniClearance | ( | int | cx, | |
int | cy, | |||
int | dist | |||
) | [inline] |
Used to set the clearance of a cell, while building the Voronoi diagram.
Definition at line 847 of file COccupancyGridMap2D.h.
void mrpt::slam::COccupancyGridMap2D::subSample | ( | int | downRatio | ) |
Performs a downsampling of the gridmap, by a given factor: resolution/=ratio.
void mrpt::slam::COccupancyGridMap2D::updateCell | ( | int | x, | |
int | y, | |||
float | v | |||
) |
Performs the Bayesian fusion of a new observation of a cell.
static void mrpt::slam::COccupancyGridMap2D::updateCell_fast_free | ( | cellType * | theCell, | |
const cellType & | logodd_obs, | |||
const cellType & | thres | |||
) | [inline, static] |
Performs the Bayesian fusion of a new observation of a cell, without checking for grid limits nor updateInfoChangeOnly.
This method increases the "free-ness" of a cell, managing possible saturation.
x | Cell index in X axis. | |
y | Cell index in Y axis. | |
logodd_obs | Observation of the cell, in log-odd form as transformed by p2l. | |
thres | This must be OCCGRID_CELLTYPE_MAX-logodd_obs |
Definition at line 349 of file COccupancyGridMap2D.h.
References OCCGRID_CELLTYPE_MAX.
static void mrpt::slam::COccupancyGridMap2D::updateCell_fast_free | ( | const unsigned & | x, | |
const unsigned & | y, | |||
const cellType & | logodd_obs, | |||
const cellType & | thres, | |||
cellType * | mapArray, | |||
const unsigned & | _size_x | |||
) | [inline, static] |
Performs the Bayesian fusion of a new observation of a cell, without checking for grid limits nor updateInfoChangeOnly.
This method increases the "free-ness" of a cell, managing possible saturation.
x | Cell index in X axis. | |
y | Cell index in Y axis. | |
logodd_obs | Observation of the cell, in log-odd form as transformed by p2l. | |
thres | This must be OCCGRID_CELLTYPE_MAX-logodd_obs |
Definition at line 327 of file COccupancyGridMap2D.h.
References OCCGRID_CELLTYPE_MAX.
static void mrpt::slam::COccupancyGridMap2D::updateCell_fast_occupied | ( | cellType * | theCell, | |
const cellType & | logodd_obs, | |||
const cellType & | thres | |||
) | [inline, static] |
Performs the Bayesian fusion of a new observation of a cell, without checking for grid limits nor updateInfoChangeOnly.
This method increases the "occupancy-ness" of a cell, managing possible saturation.
theCell | The cell to modify | |
logodd_obs | Observation of the cell, in log-odd form as transformed by p2l. | |
thres | This must be OCCGRID_CELLTYPE_MIN+logodd_obs |
Definition at line 309 of file COccupancyGridMap2D.h.
References OCCGRID_CELLTYPE_MIN.
static void mrpt::slam::COccupancyGridMap2D::updateCell_fast_occupied | ( | const unsigned & | x, | |
const unsigned & | y, | |||
const cellType & | logodd_obs, | |||
const cellType & | thres, | |||
cellType * | mapArray, | |||
const unsigned & | _size_x | |||
) | [inline, static] |
Performs the Bayesian fusion of a new observation of a cell, without checking for grid limits nor updateInfoChangeOnly.
This method increases the "occupancy-ness" of a cell, managing possible saturation.
x | Cell index in X axis. | |
y | Cell index in Y axis. | |
logodd_obs | Observation of the cell, in log-odd form as transformed by p2l. | |
thres | This must be OCCGRID_CELLTYPE_MIN+logodd_obs |
Definition at line 288 of file COccupancyGridMap2D.h.
References OCCGRID_CELLTYPE_MIN.
int mrpt::slam::COccupancyGridMap2D::x2idx | ( | float | x, | |
float | x_min | |||
) | const [inline] |
Transform a coordinate value into a cell index, using a diferent "x_min" value.
Definition at line 475 of file COccupancyGridMap2D.h.
int mrpt::slam::COccupancyGridMap2D::x2idx | ( | double | x | ) | const [inline] |
Definition at line 465 of file COccupancyGridMap2D.h.
int mrpt::slam::COccupancyGridMap2D::x2idx | ( | float | x | ) | const [inline] |
Transform a coordinate value into a cell index.
Definition at line 462 of file COccupancyGridMap2D.h.
int mrpt::slam::COccupancyGridMap2D::y2idx | ( | float | y, | |
float | y_min | |||
) | const [inline] |
Definition at line 476 of file COccupancyGridMap2D.h.
int mrpt::slam::COccupancyGridMap2D::y2idx | ( | double | y | ) | const [inline] |
Definition at line 466 of file COccupancyGridMap2D.h.
int mrpt::slam::COccupancyGridMap2D::y2idx | ( | float | y | ) | const [inline] |
Definition at line 463 of file COccupancyGridMap2D.h.
friend class CMultiMetricMap [friend] |
Definition at line 130 of file COccupancyGridMap2D.h.
friend class CMultiMetricMapPDF [friend] |
Definition at line 131 of file COccupancyGridMap2D.h.
std::vector<unsigned char> mrpt::slam::COccupancyGridMap2D::basis_map [protected] |
Used for Voronoi calculation.Same struct as "map", but contains a "0" if not a basis point.
Definition at line 159 of file COccupancyGridMap2D.h.
struct MRPTDLLIMPEXP mrpt::slam::COccupancyGridMap2D::TCriticalPointsList mrpt::slam::COccupancyGridMap2D::CriticalPointsList |
The structure used to store the set of Voronoi diagram critical points.
int mrpt::slam::COccupancyGridMap2D::direccion_vecino_x[8] [private] |
Used to store the 8 possible movements from a cell to the sorrounding ones.Filled in the constructor.
Definition at line 1104 of file COccupancyGridMap2D.h.
int mrpt::slam::COccupancyGridMap2D::direccion_vecino_y[8] [private] |
Definition at line 1104 of file COccupancyGridMap2D.h.
std::vector<float> mrpt::slam::COccupancyGridMap2D::entropyTable [static, protected] |
Internally used to speed-up entropy calculation.
Definition at line 205 of file COccupancyGridMap2D.h.
With this struct options are provided to the observation insertion process.
Definition at line 685 of file COccupancyGridMap2D.h.
mrpt::slam::COccupancyGridMap2D::TLikelihoodOptions mrpt::slam::COccupancyGridMap2D::likelihoodOptions |
With this struct options are provided to the observation likelihood computation process.
class mrpt::slam::COccupancyGridMap2D::TLikelihoodOutput mrpt::slam::COccupancyGridMap2D::likelihoodOutputs |
Some members of this struct will contain intermediate or output data after calling "computeObservationLikelihood" for some likelihood functions.
std::vector<float> mrpt::slam::COccupancyGridMap2D::logoddsTable [static, protected] |
A lookup table to compute occupancy probabilities in [0,1] from integer log-odds values in the cells, using .
Definition at line 209 of file COccupancyGridMap2D.h.
std::vector<uint8_t> mrpt::slam::COccupancyGridMap2D::logoddsTable_255 [static, protected] |
A lookup table to compute occupancy probabilities in the range [0,255] from integer log-odds values in the cells, using .
This is used to speed-up conversions to grayscale images.
Definition at line 214 of file COccupancyGridMap2D.h.
uint8_t* mrpt::slam::COccupancyGridMap2D::logoddsTable_255Ptr [static, protected] |
float* mrpt::slam::COccupancyGridMap2D::logoddsTablePtr [static, protected] |
mrpt::slam::CLandmarksMapPtr mrpt::slam::COccupancyGridMap2D::m_panoramicFeaturesCache [mutable, private] |
Internally used in "extractPanoramicFeaturesCached".
Definition at line 1091 of file COccupancyGridMap2D.h.
std::vector<cellType> mrpt::slam::COccupancyGridMap2D::map [protected] |
This is the buffer for storing the cells.In this dynamic size buffer are stored the cell values as "bytes", stored row by row, from left to right cells.
Definition at line 137 of file COccupancyGridMap2D.h.
std::vector<cellType> mrpt::slam::COccupancyGridMap2D::p2lTable [static, protected] |
A lookup table for passing from float to log-odds as cellType.
Definition at line 223 of file COccupancyGridMap2D.h.
cellType* mrpt::slam::COccupancyGridMap2D::p2lTablePtr [static, protected] |
std::vector<double> mrpt::slam::COccupancyGridMap2D::precomputedLikelihood [protected] |
These are auxiliary variables to speed up the computation of observation likelihood values for LF method among others, at a high cost in memory (see TLikelihoodOptions::enableLikelihoodCache).
Definition at line 153 of file COccupancyGridMap2D.h.
bool mrpt::slam::COccupancyGridMap2D::precomputedLikelihoodToBeRecomputed [protected] |
Definition at line 154 of file COccupancyGridMap2D.h.
float mrpt::slam::COccupancyGridMap2D::resolution [protected] |
uint32_t mrpt::slam::COccupancyGridMap2D::size_x [protected] |
uint32_t mrpt::slam::COccupancyGridMap2D::size_y [protected] |
Definition at line 141 of file COccupancyGridMap2D.h.
struct MRPTDLLIMPEXP mrpt::slam::COccupancyGridMap2D::TUpdateCellsInfoChangeOnly mrpt::slam::COccupancyGridMap2D::updateInfoChangeOnly |
An internal structure for storing data related to counting the new information apported by some observation.
float mrpt::slam::COccupancyGridMap2D::voroni_free_threshold [protected] |
The free-cells threshold used to compute the Voronoi diagram.
Definition at line 169 of file COccupancyGridMap2D.h.
std::vector<int> mrpt::slam::COccupancyGridMap2D::voronoi_diagram [protected] |
Used to store the Voronoi diagram.Same struct as "map", but contains the distance of each cell to its closer obstacles in 1/100 of "units", or 0 if not into the Voronoi diagram.
Definition at line 165 of file COccupancyGridMap2D.h.
float mrpt::slam::COccupancyGridMap2D::x_max [protected] |
Definition at line 145 of file COccupancyGridMap2D.h.
float mrpt::slam::COccupancyGridMap2D::x_min [protected] |
float mrpt::slam::COccupancyGridMap2D::y_max [protected] |
Definition at line 145 of file COccupancyGridMap2D.h.
float mrpt::slam::COccupancyGridMap2D::y_min [protected] |
Definition at line 145 of file COccupancyGridMap2D.h.
Page generated by Doxygen 1.5.8 for MRPT 0.6.5 SVN: at Thu Feb 26 02:19:01 EST 2009 |