Main MRPT website > C++ reference
MRPT logo
Public Member Functions | Public Attributes | Private Member Functions

mrpt::slam::CMetricMap Class Reference


Detailed Description

Declares a virtual base class for all metric maps storage classes.

In this class virtual methods are provided to allow the insertion of any type of "CObservation" objects into the metric map, thus updating the map (doesn't matter if it is a 2D/3D grid or a points map). IMPORTANT: Observations doesn't include any information about the robot pose beliefs, just the raw observation and information about the sensor pose relative to the robot mobile base coordinates origin.

Note that all metric maps implement this mrpt::utils::CObservable interface, emitting the following events:

mrpt::slam::mrptEventMetricMapInsert: Upon insertion of an observation that effectively modifies the map (e.g. inserting an image into a grid map will NOT raise an event, inserting a laser scan will).

See also:
CObservation, CSensoryFrame, CMultiMetricMap

Definition at line 75 of file CMetricMap.h.

#include <mrpt/slam/CMetricMap.h>

Inheritance diagram for mrpt::slam::CMetricMap:
Inheritance graph
[legend]

List of all members.

Public Member Functions

void clear ()
 Erase all the contents of the map.
virtual bool isEmpty () const =0
 Returns true if the map is empty/no observation has been inserted.
void loadFromProbabilisticPosesAndObservations (const CSimpleMap &Map)
 Load the map contents from a CSimpleMap object, erasing all previous content of the map.
void loadFromSimpleMap (const CSimpleMap &Map)
 Load the map contents from a CSimpleMap object, erasing all previous content of the map.
bool insertObservation (const CObservation *obs, const CPose3D *robotPose=NULL)
 Insert the observation information into this map.
bool insertObservationPtr (const CObservationPtr &obs, const CPose3D *robotPose=NULL)
 A wrapper for smart pointers, just calls the non-smart pointer version.
virtual double computeObservationLikelihood (const CObservation *obs, const CPose3D &takenFrom)=0
 Computes the log-likelihood of a given observation given an arbitrary robot 3D pose.
double computeObservationLikelihood (const CObservation *obs, const CPose2D &takenFrom)
 Computes the log-likelihood of a given observation given an arbitrary robot 2D pose.
virtual bool canComputeObservationLikelihood (const CObservation *obs)
 Returns true if this map is able to compute a sensible likelihood function for this observation (i.e.
double computeObservationsLikelihood (const CSensoryFrame &sf, const CPose2D &takenFrom)
 Returns the sum of the log-likelihoods of each individual observation within a mrpt::slam::CSensoryFrame.
bool canComputeObservationsLikelihood (const CSensoryFrame &sf)
 Returns true if this map is able to compute a sensible likelihood function for this observation (i.e.
 CMetricMap ()
 Constructor.
virtual ~CMetricMap ()
 Destructor.
virtual void computeMatchingWith2D (const CMetricMap *otherMap, const CPose2D &otherMapPose, float maxDistForCorrespondence, float maxAngularDistForCorrespondence, const CPose2D &angularDistPivotPoint, TMatchingPairList &correspondences, float &correspondencesRatio, float *sumSqrDist=NULL, bool onlyKeepTheClosest=true, bool onlyUniqueRobust=false) const
 Computes the matchings between this and another 2D points map.
virtual void computeMatchingWith3D (const CMetricMap *otherMap, const CPose3D &otherMapPose, float maxDistForCorrespondence, float maxAngularDistForCorrespondence, const CPoint3D &angularDistPivotPoint, TMatchingPairList &correspondences, float &correspondencesRatio, float *sumSqrDist=NULL, bool onlyKeepTheClosest=true, bool onlyUniqueRobust=false) const
 Computes the matchings between this and another 3D points map - method used in 3D-ICP.
virtual float compute3DMatchingRatio (const CMetricMap *otherMap, const CPose3D &otherMapPose, float minDistForCorr=0.10f, float minMahaDistForCorr=2.0f) const =0
 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.
virtual void saveMetricMapRepresentationToFile (const std::string &filNamePrefix) const =0
 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).
virtual void getAs3DObject (mrpt::opengl::CSetOfObjectsPtr &outObj) const =0
 Returns a 3D object representing the map.
virtual void auxParticleFilterCleanUp ()=0
 This method is called at the end of each "prediction-update-map insertion" cycle within "mrpt::slam::CMetricMapBuilderRBPF::processActionObservation".
virtual float squareDistanceToClosestCorrespondence (const float &x0, const float &y0) const
 Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map.
virtual const CSimplePointsMapgetAsSimplePointsMap () const
 If the map is a simple points map or it's a multi-metric map that contains EXACTLY one simple points map, return it.
virtual CSimplePointsMapgetAsSimplePointsMap ()

Public Attributes

bool m_disableSaveAs3DObject
 When set to true (default=false), calling "getAs3DObject" will have no effects.

Private Member Functions

virtual void internal_clear ()=0
 Internal method called by clear()
virtual void OnPostSuccesfulInsertObs (const CObservation *)
 Hook for each time a "internal_insertObservation" returns "true" This is called automatically from insertObservation() when internal_insertObservation returns true.
virtual bool internal_insertObservation (const CObservation *obs, const CPose3D *robotPose=NULL)=0
 Internal method called by insertObservation()

RTTI stuff

static const
mrpt::utils::TRuntimeClassId 
classCMetricMap
class mrpt::utils::CStream
static const
mrpt::utils::TRuntimeClassId
_GetBaseClass ()
virtual const
mrpt::utils::TRuntimeClassId
GetRuntimeClass () const
 Returns information about the class of an object in runtime.

Constructor & Destructor Documentation

mrpt::slam::CMetricMap::CMetricMap ( )

Constructor.

virtual mrpt::slam::CMetricMap::~CMetricMap ( ) [virtual]

Destructor.


Member Function Documentation

static const mrpt::utils::TRuntimeClassId* mrpt::slam::CMetricMap::_GetBaseClass ( ) [static, protected]
virtual void mrpt::slam::CMetricMap::auxParticleFilterCleanUp ( ) [pure 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.

Implemented in mrpt::slam::CBeaconMap, mrpt::slam::CColouredPointsMap, mrpt::slam::CGasConcentrationGridMap2D, mrpt::slam::CHeightGridMap2D, mrpt::slam::COccupancyGridMap2D, mrpt::slam::CSimplePointsMap, mrpt::slam::CMultiMetricMap, and mrpt::slam::CLandmarksMap.

virtual bool mrpt::slam::CMetricMap::canComputeObservationLikelihood ( const CObservation obs) [inline, 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).

Parameters:
obsThe observation.
See also:
computeObservationLikelihood

Reimplemented in mrpt::slam::COccupancyGridMap2D, and mrpt::slam::CMultiMetricMap.

Definition at line 183 of file CMetricMap.h.

bool mrpt::slam::CMetricMap::canComputeObservationsLikelihood ( const CSensoryFrame sf)

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).

Parameters:
sfThe observations.
See also:
canComputeObservationLikelihood
void mrpt::slam::CMetricMap::clear ( )
virtual float mrpt::slam::CMetricMap::compute3DMatchingRatio ( const CMetricMap otherMap,
const CPose3D otherMapPose,
float  minDistForCorr = 0.10f,
float  minMahaDistForCorr = 2.0f 
) const [pure 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.

Parameters:
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.
Returns:
The matching ratio [0,1]
See also:
computeMatchingWith2D

Implemented in mrpt::slam::CBeaconMap, mrpt::slam::CGasConcentrationGridMap2D, mrpt::slam::CHeightGridMap2D, mrpt::slam::COccupancyGridMap2D, mrpt::slam::CPointsMap, mrpt::slam::CMultiMetricMap, and mrpt::slam::CLandmarksMap.

virtual void mrpt::slam::CMetricMap::computeMatchingWith2D ( const CMetricMap otherMap,
const CPose2D otherMapPose,
float  maxDistForCorrespondence,
float  maxAngularDistForCorrespondence,
const CPose2D angularDistPivotPoint,
TMatchingPairList correspondences,
float &  correspondencesRatio,
float *  sumSqrDist = NULL,
bool  onlyKeepTheClosest = true,
bool  onlyUniqueRobust = false 
) const [inline, virtual]

Computes the matchings between this and another 2D points map.

This includes finding:

  • The set of points pairs in each map
  • The mean squared distance between corresponding pairs. This method is the most time critical one into the ICP algorithm.
Parameters:
otherMap[IN] The other map to compute the matching with.
otherMapPose[IN] The pose of the other map as seen from "this".
maxDistForCorrespondence[IN] Maximum 2D linear distance between two points to be matched.
maxAngularDistForCorrespondence[IN] In radians: The aim is to allow larger distances to more distant correspondences.
angularDistPivotPoint[IN] The point used to calculate distances from in both maps.
correspondences[OUT] The detected matchings pairs.
correspondencesRatio[OUT] The ratio [0,1] of points in otherMap with at least one correspondence.
sumSqrDist[OUT] The sum of all matched points squared distances.If undesired, set to NULL, as default.
onlyKeepTheClosest[IN] If set to true, only the closest correspondence will be returned. If false (default) all are returned.
See also:
compute3DMatchingRatio

Reimplemented in mrpt::slam::CBeaconMap, mrpt::slam::COccupancyGridMap2D, mrpt::slam::CPointsMap, mrpt::slam::CMultiMetricMap, and mrpt::slam::CLandmarksMap.

Definition at line 235 of file CMetricMap.h.

References MRPT_END, MRPT_START, and THROW_EXCEPTION.

virtual void mrpt::slam::CMetricMap::computeMatchingWith3D ( const CMetricMap otherMap,
const CPose3D otherMapPose,
float  maxDistForCorrespondence,
float  maxAngularDistForCorrespondence,
const CPoint3D angularDistPivotPoint,
TMatchingPairList correspondences,
float &  correspondencesRatio,
float *  sumSqrDist = NULL,
bool  onlyKeepTheClosest = true,
bool  onlyUniqueRobust = false 
) const [inline, virtual]

Computes the matchings between this and another 3D points map - method used in 3D-ICP.

This method finds the set of point pairs in each map.

The method is the most time critical one into the ICP algorithm.

Parameters:
otherMap[IN] The other map to compute the matching with.
otherMapPose[IN] The pose of the other map as seen from "this".
maxDistForCorrespondence[IN] Maximum 2D linear distance between two points to be matched.
maxAngularDistForCorrespondence[IN] In radians: The aim is to allow larger distances to more distant correspondences.
angularDistPivotPoint[IN] The point used to calculate distances from in both maps.
correspondences[OUT] The detected matchings pairs.
correspondencesRatio[OUT] The ratio [0,1] of points in otherMap with at least one correspondence.
sumSqrDist[OUT] The sum of all matched points squared distances.If undesired, set to NULL, as default.
onlyKeepTheClosest[IN] If set to true, only the closest correspondence will be returned. If false (default) all are returned.
See also:
compute3DMatchingRatio

Reimplemented in mrpt::slam::CPointsMap.

Definition at line 269 of file CMetricMap.h.

References MRPT_END, MRPT_START, and THROW_EXCEPTION.

virtual double mrpt::slam::CMetricMap::computeObservationLikelihood ( const CObservation obs,
const CPose3D takenFrom 
) [pure virtual]

Computes the log-likelihood of a given observation given an arbitrary robot 3D pose.

Parameters:
takenFromThe robot's pose the observation is supposed to be taken from.
obsThe observation.
Returns:
This method returns a log-likelihood.
See also:
Used in particle filter algorithms, see: CMultiMetricMapPDF::update

Implemented in mrpt::slam::CBeaconMap, mrpt::slam::CGasConcentrationGridMap2D, mrpt::slam::CHeightGridMap2D, mrpt::slam::COccupancyGridMap2D, mrpt::slam::CPointsMap, mrpt::slam::CMultiMetricMap, and mrpt::slam::CLandmarksMap.

double mrpt::slam::CMetricMap::computeObservationLikelihood ( const CObservation obs,
const CPose2D takenFrom 
) [inline]

Computes the log-likelihood of a given observation given an arbitrary robot 2D pose.

Parameters:
takenFromThe robot's pose the observation is supposed to be taken from.
obsThe observation.
Returns:
This method returns a log-likelihood.
See also:
Used in particle filter algorithms, see: CMultiMetricMapPDF::update

Definition at line 174 of file CMetricMap.h.

References mrpt::poses::CPose3D.

double mrpt::slam::CMetricMap::computeObservationsLikelihood ( const CSensoryFrame sf,
const CPose2D takenFrom 
)

Returns the sum of the log-likelihoods of each individual observation within a mrpt::slam::CSensoryFrame.

Parameters:
takenFromThe robot's pose the observation is supposed to be taken from.
sfThe set of observations in a CSensoryFrame.
Returns:
This method returns a log-likelihood.
See also:
canComputeObservationsLikelihood
virtual void mrpt::slam::CMetricMap::getAs3DObject ( mrpt::opengl::CSetOfObjectsPtr outObj) const [pure virtual]
virtual CSimplePointsMap* mrpt::slam::CMetricMap::getAsSimplePointsMap ( ) [inline, virtual]

Reimplemented in mrpt::slam::CSimplePointsMap, and mrpt::slam::CMultiMetricMap.

Definition at line 339 of file CMetricMap.h.

virtual const CSimplePointsMap* mrpt::slam::CMetricMap::getAsSimplePointsMap ( ) const [inline, virtual]

If the map is a simple points map or it's a multi-metric map that contains EXACTLY one simple points map, return it.

Otherwise, return NULL

Reimplemented in mrpt::slam::CSimplePointsMap, and mrpt::slam::CMultiMetricMap.

Definition at line 338 of file CMetricMap.h.

virtual const mrpt::utils::TRuntimeClassId* mrpt::slam::CMetricMap::GetRuntimeClass ( ) const [virtual]
bool mrpt::slam::CMetricMap::insertObservation ( const CObservation obs,
const CPose3D robotPose = NULL 
) [inline]

Insert the observation information into this map.

This method must be implemented in derived classes.

Parameters:
obsThe observation
robotPoseThe 3D pose of the robot mobile base in the map reference system, or NULL (default) if you want to use the origin.
See also:
CObservation::insertObservationInto

Definition at line 132 of file CMetricMap.h.

bool mrpt::slam::CMetricMap::insertObservationPtr ( const CObservationPtr obs,
const CPose3D robotPose = NULL 
) [inline]

A wrapper for smart pointers, just calls the non-smart pointer version.

Definition at line 146 of file CMetricMap.h.

References MRPT_END, MRPT_START, mrpt::slam::CObservationPtr::pointer(), stlplus::smart_ptr_base< T, C, COUNTER >::present(), and THROW_EXCEPTION.

virtual void mrpt::slam::CMetricMap::internal_clear ( ) [private, pure virtual]
virtual bool mrpt::slam::CMetricMap::internal_insertObservation ( const CObservation obs,
const CPose3D robotPose = NULL 
) [private, pure virtual]
virtual bool mrpt::slam::CMetricMap::isEmpty ( ) const [pure virtual]
void mrpt::slam::CMetricMap::loadFromProbabilisticPosesAndObservations ( const CSimpleMap Map)

Load the map contents from a CSimpleMap object, erasing all previous content of the map.

This is automaticed invoking "insertObservation" for each observation at the mean 3D robot pose as given by the "poses::CPosePDF" in the CSimpleMap object.

See also:
insertObservation, CSimpleMap
Exceptions:
std::exceptionSome internal steps in invoked methods can raise exceptions on invalid parameters, etc...
void mrpt::slam::CMetricMap::loadFromSimpleMap ( const CSimpleMap Map) [inline]

Load the map contents from a CSimpleMap object, erasing all previous content of the map.

This is automaticed invoking "insertObservation" for each observation at the mean 3D robot pose as given by the "poses::CPosePDF" in the CSimpleMap object.

See also:
insertObservation, CSimpleMap
Exceptions:
std::exceptionSome internal steps in invoked methods can raise exceptions on invalid parameters, etc...

Definition at line 123 of file CMetricMap.h.

virtual void mrpt::slam::CMetricMap::OnPostSuccesfulInsertObs ( const CObservation ) [inline, private, virtual]

Hook for each time a "internal_insertObservation" returns "true" This is called automatically from insertObservation() when internal_insertObservation returns true.

Reimplemented in mrpt::slam::COccupancyGridMap2D.

Definition at line 89 of file CMetricMap.h.

virtual void mrpt::slam::CMetricMap::saveMetricMapRepresentationToFile ( const std::string &  filNamePrefix) const [pure 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).

Implemented in mrpt::slam::CBeaconMap, mrpt::slam::CGasConcentrationGridMap2D, mrpt::slam::CHeightGridMap2D, mrpt::slam::COccupancyGridMap2D, mrpt::slam::CPointsMap, mrpt::slam::CMultiMetricMap, and mrpt::slam::CLandmarksMap.

virtual float mrpt::slam::CMetricMap::squareDistanceToClosestCorrespondence ( const float &  x0,
const float &  y0 
) const [inline, virtual]

Returns the square distance from the 2D point (x0,y0) to the closest correspondence in the map.

Definition at line 325 of file CMetricMap.h.

References MRPT_END, MRPT_START, and THROW_EXCEPTION.


Friends And Related Function Documentation

friend class mrpt::utils::CStream [friend]

Reimplemented from mrpt::utils::CSerializable.

Reimplemented in mrpt::slam::CPointsMap.

Definition at line 80 of file CMetricMap.h.


Member Data Documentation

Definition at line 80 of file CMetricMap.h.

When set to true (default=false), calling "getAs3DObject" will have no effects.

Definition at line 316 of file CMetricMap.h.




Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN:exported at Tue Jan 25 21:56:31 UTC 2011