10#define CLandmarksMap_H
66 bool internal_insertObservation( const
mrpt::obs::CObservation *obs, const
mrpt::poses::CPose3D *robotPose = NULL )
MRPT_OVERRIDE;
82 double internal_computeObservationLikelihood( const
mrpt::obs::CObservation *obs, const
mrpt::poses::CPose3D &takenFrom )
MRPT_OVERRIDE;
85 static
mrpt::utils::TColorf COLOR_LANDMARKS_IN_3DSCENES;
122 inline size_t size()
const {
return m_landmarks.size(); };
166 static std::map<std::pair<mrpt::maps::CLandmark::TLandmarkID, mrpt::maps::CLandmark::TLandmarkID>,
double>
_mEDD;
312 double ang, x_shift, y_shift;
368 const char *style=
"b",
369 float stdCount = 2.0f );
380 const char *style=
"b",
381 float confInterval = 0.95f )
const ;
424 unsigned int downSampleFactor = 1);
430 float maxDistForCorrespondence,
431 float maxAngularDistForCorrespondence,
434 float &correspondencesRatio,
435 float *sumSqrDist = NULL,
436 bool onlyKeepTheClosest =
false,
437 bool onlyUniqueRobust =
false )
const;
449 float &correspondencesRatio,
450 std::vector<bool> &otherCorrespondences)
const;
473 std::vector<bool> *otherCorrespondences = NULL);
485 void simulateBeaconReadings(
486 const
mrpt::poses::CPose3D &in_robotPose,
487 const
mrpt::poses::CPoint3D &in_sensorLocationOnRobot,
488 mrpt::obs::CObservationBeaconRanges &out_Observations ) const;
506 void simulateRangeBearingReadings(
507 const
mrpt::poses::CPose3D &robotPose,
508 const
mrpt::poses::CPose3D &sensorLocationOnRobot,
509 mrpt::obs::CObservationBearingRange &observations,
510 bool sensorDetectsIDs = true,
511 const
float stdRange = 0.01f,
515 const
double spurious_count_mean = 0,
516 const
double spurious_count_std = 0
525 void saveMetricMapRepresentationToFile(const
std::
string &filNamePrefix) const
MRPT_OVERRIDE;
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
#define MAP_DEFINITION_END(_CLASS_NAME_, _LINKAGE_)
#define MAP_DEFINITION_START(_CLASS_NAME_, _LINKAGE_)
Add a MAP_DEFINITION_START() ... MAP_DEFINITION_END() block inside the declaration of each metric map...
The class for storing "landmarks" (visual or laser-scan-extracted features,...)
int64_t TLandmarkID
The type for the IDs of landmarks.
A class for storing a map of 3D probabilistic landmarks.
double computeLikelihood_RSLC_2007(const CLandmarksMap *s, const mrpt::poses::CPose2D &sensorPose)
Computes the (logarithmic) likelihood function for a sensed observation "o" according to "this" map.
void internal_clear() MRPT_OVERRIDE
Internal method called by clear()
void changeCoordinatesReference(const mrpt::poses::CPose3D &newOrg)
Changes the reference system of the map to a given 3D pose.
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams ¶ms) const MRPT_OVERRIDE
Computes the ratio in [0,1] of correspondences between "this" and the "otherMap" map,...
void computeMatchingWith3DLandmarks(const mrpt::maps::CLandmarksMap *otherMap, mrpt::utils::TMatchingPairList &correspondences, float &correspondencesRatio, std::vector< bool > &otherCorrespondences) const
Perform a search for correspondences between "this" and another lansmarks map: In this class,...
void loadSiftFeaturesFromStereoImageObservation(const mrpt::obs::CObservationStereoImages &obs, mrpt::maps::CLandmark::TLandmarkID fID, const mrpt::vision::CFeatureExtraction::TOptions &feat_options=mrpt::vision::CFeatureExtraction::TOptions(mrpt::vision::featSIFT))
Loads into this landmarks map the SIFT features extracted from an observation consisting of a pair of...
size_t size() const
Returns the stored landmarks count.
bool saveToTextFile(std::string file)
Save to a text file.
void loadSiftFeaturesFromImageObservation(const mrpt::obs::CObservationImage &obs, const mrpt::vision::CFeatureExtraction::TOptions &feat_options=mrpt::vision::CFeatureExtraction::TOptions(mrpt::vision::featSIFT))
Loads into this landmarks map the SIFT features extracted from an image observation (Previous content...
void changeCoordinatesReference(const mrpt::poses::CPose3D &newOrg, const mrpt::maps::CLandmarksMap *otherMap)
Changes the reference system of the map "otherMap" and save the result in "this" map.
static mrpt::maps::CLandmark::TLandmarkID _mapMaxID
bool isEmpty() const MRPT_OVERRIDE
Returns true if the map is empty/no observation has been inserted.
void computeMatchingWith2D(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose2D &otherMapPose, float maxDistForCorrespondence, float maxAngularDistForCorrespondence, const mrpt::poses::CPose2D &angularDistPivotPoint, mrpt::utils::TMatchingPairList &correspondences, float &correspondencesRatio, float *sumSqrDist=NULL, bool onlyKeepTheClosest=false, bool onlyUniqueRobust=false) const
double computeLikelihood_SIFT_LandmarkMap(CLandmarksMap *map, mrpt::utils::TMatchingPairList *correspondences=NULL, std::vector< bool > *otherCorrespondences=NULL)
Returns the (logarithmic) likelihood of a set of landmarks "map" given "this" map.
bool saveToMATLABScript2D(std::string file, const char *style="b", float stdCount=2.0f)
Save to a MATLAB script which displays 2D error ellipses for the map (top-view, projection on the XY ...
CLandmarksMap()
Constructor.
virtual ~CLandmarksMap()
Virtual destructor.
mrpt::maps::CLandmark::TLandmarkID getMapMaxID()
bool saveToMATLABScript3D(std::string file, const char *style="b", float confInterval=0.95f) const
Save to a MATLAB script which displays 3D error ellipses for the map.
static bool _maxIDUpdated
static std::map< std::pair< mrpt::maps::CLandmark::TLandmarkID, mrpt::maps::CLandmark::TLandmarkID >, double > _mEDD
Map of the Euclidean Distance between the descriptors of two SIFT-based landmarks.
void fuseWith(CLandmarksMap &other, bool justInsertAllOfThem=false)
Fuses the contents of another map with this one, updating "this" object with the result.
void loadOccupancyFeaturesFrom2DRangeScan(const mrpt::obs::CObservation2DRangeScan &obs, const mrpt::poses::CPose3D *robotPose=NULL, unsigned int downSampleFactor=1)
Loads into this landmarks-map a set of occupancy features according to a 2D range scan (Previous cont...
Declares a virtual base class for all metric maps storage classes.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Declares a class derived from "CObservation" that encapsules an image from a camera,...
Observation class for either a pair of left+right or left+disparity images from a stereo camera.
A class used to store a 2D pose.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
This class allows loading and storing values and vectors of different types from a configuration text...
This is a virtual base class for sets of options than can be loaded from and/or saved to configuratio...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
const Scalar * const_iterator
std::vector< size_t > vector_size_t
@ featSIFT
Scale Invariant Feature Transform [LOWE'04].
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
std::vector< CLandmark > TSequenceLandmarks
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
The list of landmarks: the wrapper class is just for maintaining the KD-Tree representation.
mrpt::utils::CDynamicGrid< vector_int > m_grid
A grid-map with the set of landmarks falling into each cell.
const CLandmark * get(unsigned int indx) const
void hasBeenModifiedAll()
float m_largestDistanceFromOrigin
Auxiliary variables used in "getLargestDistanceFromOrigin".
float getLargestDistanceFromOrigin() const
This method returns the largest distance from the origin to any of the points, such as a sphere cente...
internal::TSequenceLandmarks m_landmarks
The actual list.
void isToBeModified(unsigned int indx)
const CLandmark * getByID(CLandmark::TLandmarkID ID) const
Returns the landmark with a given landmrk ID, or NULL if not found.
mrpt::utils::CDynamicGrid< vector_int > * getGrid()
void hasBeenModified(unsigned int indx)
const_iterator end() const
void push_back(const CLandmark &lm)
The object is copied, thus the original copy passed as a parameter can be released.
internal::TSequenceLandmarks::iterator iterator
TCustomSequenceLandmarks()
Default constructor.
const_iterator begin() const
CLandmark * get(unsigned int indx)
internal::TSequenceLandmarks::const_iterator const_iterator
void erase(unsigned int indx)
bool m_largestDistanceFromOriginIsUpdated
Auxiliary variables used in "getLargestDistanceFromOrigin".
const CLandmark * getByBeaconID(unsigned int ID) const
Returns the landmark with a given beacon ID, or NULL if not found.
With this struct options are provided to the fusion process.
float ellapsedTime
See "minTimesSeen".
TFuseOptions()
Initialization.
unsigned int minTimesSeen
Required number of times of a landmark to be seen not to be removed, in "ellapsedTime" seconds.
With this struct options are provided to the observation insertion process.
float SiftCorrRatioThreshold
[For SIFT landmarks only] The ratio between the best and second best descriptor distances to set as c...
bool PLOT_IMAGES
Indicates if the images (as well as the SIFT detected features) should be shown in a window.
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
float SIFTs_epipolar_TH
Maximum distance (in pixels) from a point to a certain epipolar line to be considered a potential mat...
TInsertionOptions()
Initilization of default parameters.
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
float SiftLikelihoodThreshold
[For SIFT landmarks only] The minimum likelihood value of a match to set as correspondence (Default=0...
bool insert_SIFTs_from_monocular_images
If set to true (default), the insertion of a CObservationImage in the map will insert SIFT 3D feature...
float SIFTsLoadEllipsoidWidth
[For SIFT landmarks only] The width (in meters, standard deviation) of the ellipsoid in the axis perp...
float SIFTs_stereo_maxDepth
Maximum depth of 3D landmarks when loading a landmarks map from a stereo image observation.
bool insert_Landmarks_from_range_scans
If set to true (default), inserting a CObservation2DRangeScan in the map will generate landmarks for ...
unsigned int SIFTMatching3DMethod
[For SIFT landmarks only] Method to compute 3D matching (Default = 0 (Our method)) 0: Our method -> E...
mrpt::vision::CFeatureExtraction::TOptions SIFT_feat_options
Parameters of the SIFT feature detector/descriptors while inserting images in the landmark map.
unsigned int SIFTLikelihoodMethod
[For SIFT landmarks only] Method to compute the likelihood (Default = 0 (Our method)) 0: Our method -...
int SIFTs_numberOfKLTKeypoints
Number of points to extract in the image.
float SiftEDDThreshold
[For SIFT landmarks only] The minimum Euclidean Descriptor Distance value of a match to set as corres...
float SIFTsLoadDistanceOfTheMean
[For SIFT landmarks only] The distance (in meters) of the mean value of landmarks,...
bool insert_SIFTs_from_stereo_images
If set to true (default), the insertion of a CObservationStereoImages in the map will insert SIFT 3D ...
This struct stores extra results from invoking insertObservation.
unsigned int nSiftL
The number of SIFT detected in left and right images respectively.
This struct store de GPS longitude, latitude (in degrees ) and altitude (in meters) for the first GPS...
double ang
These 3 params allow rotating and shifting GPS coordinates with other 2D maps (e.g.
unsigned int min_sat
Minimum number of sats to take into account the data.
With this struct options are provided to the likelihood computations.
double SIFTs_sigma_euclidean_dist
mrpt::vision::CFeatureExtraction::TOptions SIFT_feat_options
Parameters of the SIFT feature detector/descriptors while inserting images in the landmark map.
float SIFTnullCorrespondenceDistance
bool beaconRangesUseObservationStd
(Default: false) If true, beaconRangesStd is ignored and each individual CObservationBeaconRanges::st...
float beaconRangesStd
The standard deviation used for Beacon ranges likelihood (default=0.08) [meters].
unsigned int rangeScan2D_decimation
The number of rays from a 2D range scan will be decimated by this factor (default = 1,...
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form,...
int SIFTs_decimation
Considers 1 out of "SIFTs_decimation" visual landmarks in the observation during the likelihood compu...
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string §ion) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
float GPS_sigma
A constant "sigma" for GPS localization data (in meters)
double SIFTs_sigma_descriptor_dist
std::pair< mrpt::math::TPoint3D, unsigned int > TPairIdBeacon
Parameters for CMetricMap::compute3DMatchingRatio()