#include <mrpt/slam/CMetricMap.h>
Classes | |
struct | TMatchingPair |
An structure for returning the points pair-matchings. More... | |
class | TMatchingPairList |
A list of TMatchingPair. More... | |
Public Types | |
typedef TMatchingPair * | TMatchingPairPtr |
Public Member Functions | |
virtual void | clear ()=0 |
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 (CSensFrameProbSequence &sfSeq) |
Load the map contents from a CSensFrameProbSequence object, erasing all previous content of the map. | |
virtual bool | insertObservation (const CObservation *obs, const CPose3D *robotPose=NULL)=0 |
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. | |
void | alignBylikelihoodHillClimbing (CObservation *obs, CPose2D in_initialEstimatedPose, CPose2D &out_ResultingPose) |
Aligns an observation to its maximum likelihood pose (in 2D) into this map, by hill climbing in values computed with "computeObservationLikelihood". | |
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. | |
Public Attributes | |
bool | m_disableSaveAs3DObject |
When set to true (default=false), calling "getAs3DObject" will have no effects. |
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.
Definition at line 64 of file CMetricMap.h.
Definition at line 200 of file CMetricMap.h.
mrpt::slam::CMetricMap::CMetricMap | ( | ) |
Constructor.
virtual mrpt::slam::CMetricMap::~CMetricMap | ( | ) | [virtual] |
Destructor.
void mrpt::slam::CMetricMap::alignBylikelihoodHillClimbing | ( | CObservation * | obs, | |
CPose2D | in_initialEstimatedPose, | |||
CPose2D & | out_ResultingPose | |||
) |
Aligns an observation to its maximum likelihood pose (in 2D) into this map, by hill climbing in values computed with "computeObservationLikelihood".
obs | The observation to be aligned | |
in_initialEstimatedPose | The initial input to the algorithm, an initial "guess" for the observation pose in the map. | |
out_ResultingPose | The resulting pose from this algorithm |
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::CLandmarksMap, mrpt::slam::CMultiMetricMap, mrpt::slam::COccupancyGridMap2D, and mrpt::slam::CSimplePointsMap.
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).
obs | The observation. |
Reimplemented in mrpt::slam::CMultiMetricMap, and mrpt::slam::COccupancyGridMap2D.
Definition at line 136 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).
sf | The observations. |
virtual void mrpt::slam::CMetricMap::clear | ( | ) | [pure virtual] |
Erase all the contents of the map.
Implemented in mrpt::slam::CBeaconMap, mrpt::slam::CColouredPointsMap, mrpt::slam::CGasConcentrationGridMap2D, mrpt::slam::CHeightGridMap2D, mrpt::slam::CLandmarksMap, mrpt::slam::CMultiMetricMap, mrpt::slam::COccupancyGridMap2D, mrpt::slam::CPointsMap, and mrpt::slam::CSimplePointsMap.
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.
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. |
Implemented in mrpt::slam::CBeaconMap, mrpt::slam::CGasConcentrationGridMap2D, mrpt::slam::CHeightGridMap2D, mrpt::slam::CLandmarksMap, mrpt::slam::CMultiMetricMap, mrpt::slam::COccupancyGridMap2D, and mrpt::slam::CPointsMap.
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:
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. |
Reimplemented in mrpt::slam::CBeaconMap, mrpt::slam::CLandmarksMap, mrpt::slam::CMultiMetricMap, mrpt::slam::COccupancyGridMap2D, and mrpt::slam::CPointsMap.
Definition at line 279 of file CMetricMap.h.
References MRPT_TRY_END, MRPT_TRY_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.
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. |
Reimplemented in mrpt::slam::CPointsMap.
Definition at line 313 of file CMetricMap.h.
References MRPT_TRY_END, MRPT_TRY_START, and THROW_EXCEPTION.
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.
takenFrom | The robot's pose the observation is supposed to be taken from. | |
obs | The observation. |
Definition at line 127 of file CMetricMap.h.
References mrpt::poses::CPose3D.
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.
takenFrom | The robot's pose the observation is supposed to be taken from. | |
obs | The observation. |
Implemented in mrpt::slam::CBeaconMap, mrpt::slam::CColouredPointsMap, mrpt::slam::CGasConcentrationGridMap2D, mrpt::slam::CHeightGridMap2D, mrpt::slam::CLandmarksMap, mrpt::slam::CMultiMetricMap, mrpt::slam::COccupancyGridMap2D, and mrpt::slam::CSimplePointsMap.
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.
takenFrom | The robot's pose the observation is supposed to be taken from. | |
sf | The set of observations in a CSensoryFrame. |
virtual void mrpt::slam::CMetricMap::getAs3DObject | ( | mrpt::opengl::CSetOfObjectsPtr & | outObj | ) | const [pure virtual] |
Returns a 3D object representing the map.
Implemented in mrpt::slam::CBeaconMap, mrpt::slam::CColouredPointsMap, mrpt::slam::CGasConcentrationGridMap2D, mrpt::slam::CHeightGridMap2D, mrpt::slam::CLandmarksMap, mrpt::slam::CMultiMetricMap, mrpt::slam::COccupancyGridMap2D, and mrpt::slam::CPointsMap.
virtual bool mrpt::slam::CMetricMap::insertObservation | ( | const CObservation * | obs, | |
const CPose3D * | robotPose = NULL | |||
) | [pure 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 the origin. |
Implemented in mrpt::slam::CBeaconMap, mrpt::slam::CColouredPointsMap, mrpt::slam::CGasConcentrationGridMap2D, mrpt::slam::CHeightGridMap2D, mrpt::slam::CLandmarksMap, mrpt::slam::CMultiMetricMap, mrpt::slam::COccupancyGridMap2D, and mrpt::slam::CSimplePointsMap.
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 99 of file CMetricMap.h.
References MRPT_TRY_END, MRPT_TRY_START, and THROW_EXCEPTION.
virtual bool mrpt::slam::CMetricMap::isEmpty | ( | ) | const [pure virtual] |
Returns true if the map is empty/no observation has been inserted.
Implemented in mrpt::slam::CBeaconMap, mrpt::slam::CGasConcentrationGridMap2D, mrpt::slam::CHeightGridMap2D, mrpt::slam::CLandmarksMap, mrpt::slam::CMultiMetricMap, mrpt::slam::COccupancyGridMap2D, and mrpt::slam::CPointsMap.
void mrpt::slam::CMetricMap::loadFromProbabilisticPosesAndObservations | ( | CSensFrameProbSequence & | sfSeq | ) |
Load the map contents from a CSensFrameProbSequence 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 CSensFrameProbSequence object.
std::exception | Some internal steps in invoked methods can raise exceptions on invalid parameters, etc... |
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::CLandmarksMap, mrpt::slam::CMultiMetricMap, mrpt::slam::COccupancyGridMap2D, and mrpt::slam::CPointsMap.
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 370 of file CMetricMap.h.
References MRPT_TRY_END, MRPT_TRY_START, and THROW_EXCEPTION.
When set to true (default=false), calling "getAs3DObject" will have no effects.
Definition at line 360 of file CMetricMap.h.
Page generated by Doxygen 1.5.9 for MRPT 0.7.1 SVN: at Mon Aug 17 22:32:05 EDT 2009 |