A mesh representation of a surface which keeps the estimated height for each (x,y) location.
Important implemented features are the insertion of 2D laser scans (from arbitrary 6D poses) and the exportation as 3D scenes.
Each cell contains the up-to-date average height from measured falling in that cell. Algorithms that can be used:
Definition at line 95 of file CHeightGridMap2D.h.
#include <mrpt/slam/CHeightGridMap2D.h>
Classes | |
struct | TInsertionOptions |
Parameters related with inserting observations into the map. More... | |
Public Types | |
enum | TMapRepresentation { mrSimpleAverage = 0 } |
The type of map representation to be used. More... | |
Public Member Functions | |
void | clear () |
Calls the base CMetricMap::clear Declared here to avoid ambiguity between the two clear() in both base classes. | |
float | cell2float (const THeightGridmapCell &c) const |
CHeightGridMap2D (TMapRepresentation mapType=mrSimpleAverage, float x_min=-2, float x_max=2, float y_min=-2, float y_max=2, float resolution=0.1) | |
Constructor. | |
bool | isEmpty () const |
Returns true if the map is empty/no observation has been inserted. | |
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. | |
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 |
The implementation in this class just calls all the corresponding method of the contained metric maps. | |
void | getAs3DObject (mrpt::opengl::CSetOfObjectsPtr &outObj) const |
Returns a 3D object representing the map: by default, it will be a mrpt::opengl::CMesh object, unless it is specified otherwise in mrpt:: | |
void | auxParticleFilterCleanUp () |
This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation". | |
TMapRepresentation | getMapType () |
Return the type of the gas distribution map, according to parameters passed on construction. | |
bool | intersectLine3D (const TLine3D &r1, TObject3D &obj) const |
Gets the intersection between a 3D line and a Height Grid map (taking into account the different heights of each individual cell). | |
bool | getMinMaxHeight (float &z_min, float &z_max) const |
Computes the minimum and maximum height in the grid. | |
size_t | countObservedCells () const |
Return the number of cells with at least one height data inserted. | |
Public Attributes | |
mrpt::slam::CHeightGridMap2D::TInsertionOptions | insertionOptions |
Protected Member Functions | |
virtual void | internal_clear () |
Erase all the contents of the map. | |
virtual bool | internal_insertObservation (const CObservation *obs, const CPose3D *robotPose=NULL) |
Insert the observation information into this map. | |
Protected Attributes | |
TMapRepresentation | m_mapType |
The map representation type of this map. | |
RTTI stuff | |
typedef CHeightGridMap2DPtr | SmartPtr |
static mrpt::utils::CLASSINIT | _init_CHeightGridMap2D |
static mrpt::utils::TRuntimeClassId | classCHeightGridMap2D |
static const mrpt::utils::TRuntimeClassId * | classinfo |
static const mrpt::utils::TRuntimeClassId * | _GetBaseClass () |
virtual const mrpt::utils::TRuntimeClassId * | GetRuntimeClass () const |
Returns information about the class of an object in runtime. | |
virtual mrpt::utils::CObject * | duplicate () const |
Returns a copy of the object, indepently of its class. | |
static mrpt::utils::CObject * | CreateObject () |
static CHeightGridMap2DPtr | Create () |
A typedef for the associated smart pointer
Definition at line 98 of file CHeightGridMap2D.h.
The type of map representation to be used.
See mrpt::slam::CHeightGridMap2D for discussion.
Definition at line 114 of file CHeightGridMap2D.h.
mrpt::slam::CHeightGridMap2D::CHeightGridMap2D | ( | TMapRepresentation | mapType = mrSimpleAverage , |
float | x_min = -2 , |
||
float | x_max = 2 , |
||
float | y_min = -2 , |
||
float | y_max = 2 , |
||
float | resolution = 0.1 |
||
) |
Constructor.
static const mrpt::utils::TRuntimeClassId* mrpt::slam::CHeightGridMap2D::_GetBaseClass | ( | ) | [static, protected] |
Reimplemented from mrpt::slam::CMetricMap.
void mrpt::slam::CHeightGridMap2D::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.
float mrpt::slam::CHeightGridMap2D::cell2float | ( | const THeightGridmapCell & | c | ) | const [inline] |
Definition at line 106 of file CHeightGridMap2D.h.
References mrpt::slam::THeightGridmapCell::h.
void mrpt::slam::CHeightGridMap2D::clear | ( | void | ) | [inline] |
Calls the base CMetricMap::clear Declared here to avoid ambiguity between the two clear() in both base classes.
Reimplemented from mrpt::slam::CMetricMap.
Definition at line 104 of file CHeightGridMap2D.h.
References mrpt::slam::CMetricMap::clear().
float mrpt::slam::CHeightGridMap2D::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.
double mrpt::slam::CHeightGridMap2D::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.
takenFrom | The robot's pose the observation is supposed to be taken from. |
obs | The observation. |
Implements mrpt::slam::CMetricMap.
size_t mrpt::slam::CHeightGridMap2D::countObservedCells | ( | ) | const |
Return the number of cells with at least one height data inserted.
static CHeightGridMap2DPtr mrpt::slam::CHeightGridMap2D::Create | ( | ) | [static] |
static mrpt::utils::CObject* mrpt::slam::CHeightGridMap2D::CreateObject | ( | ) | [static] |
virtual mrpt::utils::CObject* mrpt::slam::CHeightGridMap2D::duplicate | ( | ) | const [virtual] |
Returns a copy of the object, indepently of its class.
Implements mrpt::utils::CObject.
void mrpt::slam::CHeightGridMap2D::getAs3DObject | ( | mrpt::opengl::CSetOfObjectsPtr & | outObj | ) | const [virtual] |
Returns a 3D object representing the map: by default, it will be a mrpt::opengl::CMesh object, unless it is specified otherwise in mrpt::
Implements mrpt::slam::CMetricMap.
TMapRepresentation mrpt::slam::CHeightGridMap2D::getMapType | ( | ) |
Return the type of the gas distribution map, according to parameters passed on construction.
bool mrpt::slam::CHeightGridMap2D::getMinMaxHeight | ( | float & | z_min, |
float & | z_max | ||
) | const |
Computes the minimum and maximum height in the grid.
virtual const mrpt::utils::TRuntimeClassId* mrpt::slam::CHeightGridMap2D::GetRuntimeClass | ( | ) | const [virtual] |
Returns information about the class of an object in runtime.
Reimplemented from mrpt::slam::CMetricMap.
virtual void mrpt::slam::CHeightGridMap2D::internal_clear | ( | ) | [protected, virtual] |
Erase all the contents of the map.
Implements mrpt::slam::CMetricMap.
virtual bool mrpt::slam::CHeightGridMap2D::internal_insertObservation | ( | const CObservation * | obs, |
const CPose3D * | robotPose = NULL |
||
) | [protected, virtual] |
Insert the observation information into this map.
This method must be implemented in derived classes.
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.
Gets the intersection between a 3D line and a Height Grid map (taking into account the different heights of each individual cell).
bool mrpt::slam::CHeightGridMap2D::isEmpty | ( | ) | const [virtual] |
Returns true if the map is empty/no observation has been inserted.
Implements mrpt::slam::CMetricMap.
void mrpt::slam::CHeightGridMap2D::saveMetricMapRepresentationToFile | ( | const std::string & | filNamePrefix | ) | const [virtual] |
The implementation in this class just calls all the corresponding method of the contained metric maps.
Implements mrpt::slam::CMetricMap.
mrpt::utils::CLASSINIT mrpt::slam::CHeightGridMap2D::_init_CHeightGridMap2D [static, protected] |
Definition at line 98 of file CHeightGridMap2D.h.
Definition at line 98 of file CHeightGridMap2D.h.
const mrpt::utils::TRuntimeClassId* mrpt::slam::CHeightGridMap2D::classinfo [static] |
Definition at line 98 of file CHeightGridMap2D.h.
The map representation type of this map.
Definition at line 229 of file CHeightGridMap2D.h.
Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:40:17 UTC 2011 |