CLandmarksMap.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */
#ifndef CLandmarksMap_H
#define CLandmarksMap_H
 
#include <mrpt/vision/CFeatureExtraction.h>
#include <mrpt/maps/CMetricMap.h>
#include <mrpt/maps/CLandmark.h>
#include <mrpt/obs/CObservationImage.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/obs/CObservationGPS.h>
#include <mrpt/obs/CObservationBearingRange.h>
#include <mrpt/serialization/CSerializable.h>
#include <mrpt/math/CMatrix.h>
#include <mrpt/containers/CDynamicGrid.h>
#include <mrpt/config/CLoadableOptions.h>
#include <mrpt/obs/obs_frwds.h>
 
namespace mrpt
{
namespace maps
{
namespace internal
{
using TSequenceLandmarks = std::vector<CLandmark>;
}
 
/** A class for storing a map of 3D probabilistic landmarks.
 * <br>
 *  Currently these types of landmarks are defined: (see mrpt::maps::CLandmark)
 *      - For "visual landmarks" from images: features with associated
 descriptors.
 *      - For laser scanners: each of the range measuremnts, as "occupancy"
 landmarks.
 *      - For grid maps: "Panoramic descriptor" feature points.
 *      - For range-only localization and SLAM: Beacons. It is also supported
 the simulation of expected beacon-to-sensor readings, observation
 likelihood,...
 * <br>
 * <b>How to load landmarks from observations:</b><br>
 *  When invoking CLandmarksMap::insertObservation(), the values in
 CLandmarksMap::insertionOptions will
 *     determinate the kind of landmarks that will be extracted and fused into
 the map. Supported feature
 *     extraction processes are listed next:
 *
  <table>
  <tr> <td><b>Observation class:</b></td> <td><b>Generated Landmarks:</b></td>
 <td><b>Comments:</b></td> </tr>
  <tr> <td>CObservationImage</td> <td>vlSIFT</td> <td>1) A SIFT feature is
 created for each SIFT detected in the image,
           <br>2) Correspondences between SIFTs features and existing ones are
 finded by computeMatchingWith3DLandmarks,
           <br>3) The corresponding feaures are fused, and the new ones added,
 with an initial uncertainty according to insertionOptions</td> </tr>
  <tr> <td>CObservationStereoImages</td> <td>vlSIFT</td> <td> Each image is
 separately procesed by the method for CObservationImage observations </td>
 </tr>
  <tr> <td>CObservationStereoImages</td> <td>vlColor</td> <td> TODO... </td>
 </tr>
  <tr> <td>CObservation2DRangeScan</td> <td>glOccupancy</td> <td> A landmark is
 added for each range in the scan, with its appropiate covariance matrix derived
 from the jacobians matrixes. </td> </tr>
  </table>
 *
 * \sa CMetricMap
 * \ingroup mrpt_vision_grp
 */
class CLandmarksMap : public mrpt::maps::CMetricMap
{
    DEFINE_SERIALIZABLE(CLandmarksMap)
 
   private:
    void internal_clear() override;
    bool internal_insertObservation(
        const mrpt::obs::CObservation* obs,
        const mrpt::poses::CPose3D* robotPose = nullptr) override;
 
   public:
    /** Computes the (logarithmic) likelihood that a given observation was taken
     *from a given pose in the world being modeled with this map.
     *
     *  In the current implementation, this method behaves in a different way
     *according to the nature of
     *   the observation's class:
     *      - "mrpt::obs::CObservation2DRangeScan": This calls
     *"computeLikelihood_RSLC_2007".
     *      - "mrpt::obs::CObservationStereoImages": This calls
     *"computeLikelihood_SIFT_LandmarkMap".
     * <table>
     * <tr> <td><b>Observation class:</b></td> <td><b>Generated
     *Landmarks:</b></td> <td><b>Comments:</b></td> </tr>
     * <tr> <td>CObservationImage</td> <td>vlSIFT</td> <td>1) A SIFT feature is
     *created for each SIFT detected in the image,
     *          <br>2) Correspondences between SIFTs features and existing ones
     *are found by computeMatchingWith3DLandmarks,
     *         <br>3) The corresponding feaures are fused, and the new ones
     * added,
     *with an initial uncertainty according to insertionOptions</td> </tr>
     * <tr> <td>CObservationStereoImages</td> <td>vlSIFT</td> <td> Each image is
     *separately procesed by the method for CObservationImage observations </td>
     *</tr>
     * <tr> <td>CObservationStereoImages</td> <td>vlColor</td> <td> TODO...
     *</td> </tr>
     * <tr> <td>CObservation2DRangeScan</td> <td>glOccupancy</td> <td> A
     *landmark is added for each range in the scan, with its appropiate
     *covariance matrix derived from the jacobians matrixes. </td> </tr>
     * </table>
     *
     * \param takenFrom The robot's pose the observation is supposed to be taken
     *from.
     * \param obs The observation.
     * \return This method returns a likelihood value > 0.
     *
     * \sa Used in particle filter algorithms, see: CMultiMetricMapPDF::update
     */
    double internal_computeObservationLikelihood(
        const mrpt::obs::CObservation* obs,
        const mrpt::poses::CPose3D& takenFrom) override;
 
    /** The color of landmark ellipsoids in CLandmarksMap::getAs3DObject */
    static mrpt::img::TColorf COLOR_LANDMARKS_IN_3DSCENES;
 
    using landmark_type = mrpt::maps::CLandmark;
 
    /** The list of landmarks: the wrapper class is just for maintaining the
     * KD-Tree representation
      */
    struct TCustomSequenceLandmarks
    {
       private:
        /** The actual list */
        internal::TSequenceLandmarks m_landmarks;
 
        /** A grid-map with the set of landmarks falling into each cell.
          *  \todo Use the KD-tree instead?
          */
        mrpt::containers::CDynamicGrid<std::vector<int32_t>> m_grid;
 
        /** Auxiliary variables used in "getLargestDistanceFromOrigin"
          * \sa getLargestDistanceFromOrigin
          */
        mutable float m_largestDistanceFromOrigin;
 
        /** Auxiliary variables used in "getLargestDistanceFromOrigin"
          * \sa getLargestDistanceFromOrigin
          */
        mutable bool m_largestDistanceFromOriginIsUpdated;
 
       public:
        /** Default constructor
          */
        TCustomSequenceLandmarks();
 
        using iterator = internal::TSequenceLandmarks::iterator;
        inline iterator begin() { return m_landmarks.begin(); };
        inline iterator end() { return m_landmarks.end(); };
        void clear();
        inline size_t size() const { return m_landmarks.size(); };
        using const_iterator = internal::TSequenceLandmarks::const_iterator;
        inline const_iterator begin() const { return m_landmarks.begin(); };
        inline const_iterator end() const { return m_landmarks.end(); };
        /** The object is copied, thus the original copy passed as a parameter
         * can be released.
          */
        void push_back(const CLandmark& lm);
        CLandmark* get(unsigned int indx);
        const CLandmark* get(unsigned int indx) const;
        void isToBeModified(unsigned int indx);
        void hasBeenModified(unsigned int indx);
        void hasBeenModifiedAll();
        void erase(unsigned int indx);
 
        mrpt::containers::CDynamicGrid<std::vector<int32_t>>* getGrid()
        {
            return &m_grid;
        }
        /** Returns the landmark with a given landmrk ID, or nullptr if not
         * found
          */
        const CLandmark* getByID(CLandmark::TLandmarkID ID) const;
 
        /** Returns the landmark with a given beacon ID, or nullptr if not found
          */
        const CLandmark* getByBeaconID(unsigned int ID) const;
 
        /** This method returns the largest distance from the origin to any of
         * the points, such as a sphere centered at the origin with this radius
         * cover ALL the points in the map (the results are buffered, such as,
         * if the map is not modified, the second call will be much faster than
         * the first one).
          */
        float getLargestDistanceFromOrigin() const;
 
    } landmarks;
 
    /** Constructor
      */
    CLandmarksMap();
 
    /** Virtual destructor.
      */
    virtual ~CLandmarksMap();
 
    /**** FAMD ***/
    /** Map of the Euclidean Distance between the descriptors of two SIFT-based
     * landmarks
     */
    static std::map<std::pair<mrpt::maps::CLandmark::TLandmarkID,
                              mrpt::maps::CLandmark::TLandmarkID>,
                    double>
        _mEDD;
    static mrpt::maps::CLandmark::TLandmarkID _mapMaxID;
    static bool _maxIDUpdated;
 
    mrpt::maps::CLandmark::TLandmarkID getMapMaxID();
    /**** END FAMD *****/
 
    // See docs in base class
    float compute3DMatchingRatio(
        const mrpt::maps::CMetricMap* otherMap,
        const mrpt::poses::CPose3D& otherMapPose,
        const TMatchingRatioParams& params) const override;
 
    /** With this struct options are provided to the observation insertion
     * process.
     */
    struct TInsertionOptions : public mrpt::config::CLoadableOptions
    {
       public:
        /** Initilization of default parameters
         */
        TInsertionOptions();
 
        void loadFromConfigFile(
            const mrpt::config::CConfigFileBase& source,
            const std::string& section) override;  // See base docs
        void dumpToTextStream(
            std::ostream& out) const override;  // See base docs
 
        /** If set to true (default), the insertion of a CObservationImage in
         * the map will insert SIFT 3D features.
          */
        bool insert_SIFTs_from_monocular_images;
 
        /** If set to true (default), the insertion of a
         * CObservationStereoImages in the map will insert SIFT 3D features.
          */
        bool insert_SIFTs_from_stereo_images;
 
        /** If set to true (default), inserting a CObservation2DRangeScan in the
         * map will generate landmarks for each range.
          */
        bool insert_Landmarks_from_range_scans;
 
        /** [For SIFT landmarks only] The ratio between the best and second best
         * descriptor distances to set as correspondence (Default=0.4)
          */
        float SiftCorrRatioThreshold;
 
        /** [For SIFT landmarks only] The minimum likelihood value of a match to
         * set as correspondence (Default=0.5)
          */
        float SiftLikelihoodThreshold;
 
        /****************************************** FAMD
         * ******************************************/
        /** [For SIFT landmarks only] The minimum Euclidean Descriptor Distance
         * value of a match to set as correspondence (Default=200)
          */
        float SiftEDDThreshold;
 
        /** [For SIFT landmarks only] Method to compute 3D matching (Default = 0
         * (Our method))
          * 0: Our method -> Euclidean Distance between Descriptors and 3D
         * position
          * 1: Sim, Elinas, Griffin, Little -> Euclidean Distance between
         * Descriptors
          */
        unsigned int SIFTMatching3DMethod;
 
        /** [For SIFT landmarks only] Method to compute the likelihood (Default
         * = 0 (Our method))
          * 0: Our method -> Euclidean Distance between Descriptors and 3D
         * position
          * 1: Sim, Elinas, Griffin, Little -> 3D position
          */
        unsigned int SIFTLikelihoodMethod;
 
        /****************************************** END FAMD
         * ******************************************/
 
        /** [For SIFT landmarks only] The distance (in meters) of the mean value
         * of landmarks, for the initial position PDF (Default = 3m)
          */
        float SIFTsLoadDistanceOfTheMean;
 
        /** [For SIFT landmarks only] The width (in meters, standard deviation)
         * of the ellipsoid in the axis perpendicular to the main directiom
         * (Default = 0.05f)
          */
        float SIFTsLoadEllipsoidWidth;
 
        /** [For SIFT landmarks only] The standard deviation (in pixels) for the
         * SIFTs detector (This is used for the Jacobbian to project stereo
         * images to 3D)
          */
        float SIFTs_stdXY, SIFTs_stdDisparity;
 
        /** Number of points to extract in the image
          */
        int SIFTs_numberOfKLTKeypoints;
 
        /** Maximum depth of 3D landmarks when loading a landmarks map from a
         * stereo image observation.
          */
        float SIFTs_stereo_maxDepth;
 
        /** Maximum distance (in pixels) from a point to a certain epipolar line
         * to be considered a potential match.
          */
        float SIFTs_epipolar_TH;
 
        /** Indicates if the images (as well as the SIFT detected features)
         * should be shown in a window.
          */
        bool PLOT_IMAGES;
 
        /** Parameters of the SIFT feature detector/descriptors while inserting
         * images in the landmark map.
          *  \note There exists another \a SIFT_feat_options field in the \a
         * likelihoodOptions member.
          *  \note All parameters of this field can be loaded from a config
         * file. See mrpt::vision::CFeatureExtraction::TOptions for the names of
         * the expected fields.
          */
        mrpt::vision::CFeatureExtraction::TOptions SIFT_feat_options;
 
    } insertionOptions;
 
    /** With this struct options are provided to the likelihood computations.
     */
    struct TLikelihoodOptions : public mrpt::config::CLoadableOptions
    {
       public:
        TLikelihoodOptions();
 
        void loadFromConfigFile(
            const mrpt::config::CConfigFileBase& source,
            const std::string& section) override;  // See base docs
        void dumpToTextStream(
            std::ostream& out) const override;  // See base docs
 
        /** @name Parameters for: 2D LIDAR scans
          * @{ */
        /** The number of rays from a 2D range scan will be decimated by this
         * factor (default = 1, no decimation) */
        unsigned int rangeScan2D_decimation;
        /** @} */
 
        /** @name Parameters for: images
          * @{ */
        double SIFTs_sigma_euclidean_dist;
        double SIFTs_sigma_descriptor_dist;
        float SIFTs_mahaDist_std;
        float SIFTnullCorrespondenceDistance;
        /** Considers 1 out of "SIFTs_decimation" visual landmarks in the
         * observation during the likelihood computation. */
        int SIFTs_decimation;
        /** Parameters of the SIFT feature detector/descriptors while inserting
         * images in the landmark map.
          *  \note There exists another \a SIFT_feat_options field in the \a
         * insertionOptions member.
          *  \note All parameters of this field can be loaded from a config
         * file. See mrpt::vision::CFeatureExtraction::TOptions for the names of
         * the expected fields. */
        mrpt::vision::CFeatureExtraction::TOptions SIFT_feat_options;
        /** @} */
 
        /** @name Parameters for: Range-only observation
          * @{ */
        /** The standard deviation used for Beacon ranges likelihood
         * (default=0.08) [meters] \sa beaconRangesUseObservationStd */
        float beaconRangesStd;
        /** (Default: false) If true, `beaconRangesStd` is ignored and each
         * individual `CObservationBeaconRanges::stdError` field is used
         * instead. */
        bool beaconRangesUseObservationStd;
        /** @} */
 
        /** @name Parameters for: External robot poses observation
          * @{ */
        /** The standard deviation used for external robot poses likelihood
         * (default=0.05) [meters] */
        float extRobotPoseStd;
        /** @} */
 
        /** @name Parameters for: GPS readings
          * @{ */
 
        /** This struct store de GPS longitude, latitude (in degrees ) and
         * altitude (in meters) for the first GPS observation
          * compose with de sensor position on the robot */
        struct TGPSOrigin
        {
           public:
            TGPSOrigin();
            /** degrees */
            double longitude;
            /** degrees */
            double latitude;
            /** meters */
            double altitude;
            /** These 3 params allow rotating and shifting GPS coordinates with
             * other 2D maps (e.g. gridmaps).
              * - ang : Map rotation [deg]
              * - x_shift, y_shift: (x,y) offset [m] */
            double ang, x_shift, y_shift;
            /** Minimum number of sats to take into account the data */
            unsigned int min_sat;
        } GPSOrigin;
 
        /** A constant "sigma" for GPS localization data (in meters) */
        float GPS_sigma;
        /** @} */
 
    } likelihoodOptions;
 
    /** This struct stores extra results from invoking insertObservation
     */
    struct TInsertionResults
    {
        /** The number of SIFT detected in left and right images respectively
          */
 
        unsigned int nSiftL, nSiftR;
 
    } insertionResults;
 
    /** With this struct options are provided to the fusion process.
     */
    struct TFuseOptions
    {
        /** Initialization
          */
        TFuseOptions();
 
        /** Required number of times of a landmark to be seen not to be removed,
         * in "ellapsedTime" seconds.
          */
        unsigned int minTimesSeen;
 
        /** See "minTimesSeen"
          */
        float ellapsedTime;
 
    } fuseOptions;
 
    /** Save to a text file.
     *  In line "i" there are the (x,y,z) mean values of the i'th landmark +
     * type of landmark + # times seen + timestamp + RGB/descriptor + ID
     *
     *   Returns false if any error occured, true elsewere.
     */
    bool saveToTextFile(std::string file);
 
    /** Save to a MATLAB script which displays 2D error ellipses for the map
     *(top-view, projection on the XY plane).
     *  \param file     The name of the file to save the script to.
     *  \param style    The MATLAB-like string for the style of the lines (see
     *'help plot' in MATLAB for posibilities)
     *  \param stdCount The ellipsoids will be drawn from the center to
     *"stdCount" times the "standard deviations". (default is 2std = 95%
     *confidence intervals)
     *
     *  \return Returns false if any error occured, true elsewere.
     */
    bool saveToMATLABScript2D(
        std::string file, const char* style = "b", float stdCount = 2.0f);
 
    /** Save to a MATLAB script which displays 3D error ellipses for the map.
     *  \param file     The name of the file to save the script to.
     *  \param style    The MATLAB-like string for the style of the lines (see
     *'help plot' in MATLAB for posibilities)
     *  \param stdCount The ellipsoids will be drawn from the center to a given
     *confidence interval in [0,1], e.g. 2 sigmas=0.95 (default is 2std = 0.95
     *confidence intervals)
     *
     *  \return Returns false if any error occured, true elsewere.
     */
    bool saveToMATLABScript3D(
        std::string file, const char* style = "b",
        float confInterval = 0.95f) const;
 
    /** Returns the stored landmarks count.
     */
    size_t size() const;
 
    /** Computes the (logarithmic) likelihood function for a sensed observation
      "o" according to "this" map.
      *   This is the implementation of the algorithm reported in the paper:
            <em>J.L. Blanco, J. Gonzalez, and J.A. Fernandez-Madrigal, "A
      Consensus-based Approach for Estimating the Observation Likelihood of
      Accurate Range Sensors", in IEEE International Conference on Robotics and
      Automation (ICRA), Rome (Italy), Apr 10-14, 2007</em>
      */
    double computeLikelihood_RSLC_2007(
        const CLandmarksMap* s, const mrpt::poses::CPose2D& sensorPose);
 
    /** Loads into this landmarks map the SIFT features extracted from an image
     * observation (Previous contents of map will be erased)
      *  The robot is assumed to be at the origin of the map.
      *  Some options may be applicable from "insertionOptions"
     * (insertionOptions.SIFTsLoadDistanceOfTheMean)
      *
      *  \param feat_options Optionally, you can pass here parameters for
     * changing the default SIFT detector settings.
      */
    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
     * observation consisting of a pair of stereo-image (Previous contents of
     * map will be erased)
      *  The robot is assumed to be at the origin of the map.
      *  Some options may be applicable from "insertionOptions"
      *
      *  \param feat_options Optionally, you can pass here parameters for
     * changing the default SIFT detector settings.
      */
    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 a set of occupancy features according to a
     * 2D range scan (Previous contents of map will be erased)
      *  \param obs The observation
      *  \param robotPose   The robot pose in the map (Default = the origin)
      *  Some options may be applicable from "insertionOptions"
      */
    void loadOccupancyFeaturesFrom2DRangeScan(
        const mrpt::obs::CObservation2DRangeScan& obs,
        const mrpt::poses::CPose3D* robotPose = nullptr,
        unsigned int downSampleFactor = 1);
 
    // See docs in base class
    void computeMatchingWith2D(
        const mrpt::maps::CMetricMap* otherMap,
        const mrpt::poses::CPose2D& otherMapPose,
        float maxDistForCorrespondence, float maxAngularDistForCorrespondence,
        const mrpt::poses::CPose2D& angularDistPivotPoint,
        mrpt::tfest::TMatchingPairList& correspondences,
        float& correspondencesRatio, float* sumSqrDist = nullptr,
        bool onlyKeepTheClosest = false, bool onlyUniqueRobust = false) const;
 
    /** Perform a search for correspondences between "this" and another
     * lansmarks map:
      *  In this class, the matching is established solely based on the
     * landmarks' IDs.
      * \param otherMap [IN] The other map.
      * \param correspondences [OUT] The matched pairs between maps.
      * \param correspondencesRatio [OUT] This is NumberOfMatchings /
     * NumberOfLandmarksInTheAnotherMap
      * \param otherCorrespondences [OUT] Will be returned with a vector
     * containing "true" for the indexes of the other map's landmarks with a
     * correspondence.
      */
    void computeMatchingWith3DLandmarks(
        const mrpt::maps::CLandmarksMap* otherMap,
        mrpt::tfest::TMatchingPairList& correspondences,
        float& correspondencesRatio,
        std::vector<bool>& otherCorrespondences) const;
 
    /** Changes the reference system of the map to a given 3D pose.
      */
    void changeCoordinatesReference(const mrpt::poses::CPose3D& newOrg);
 
    /** Changes the reference system of the map "otherMap" and save the result
     * in "this" map.
      */
    void changeCoordinatesReference(
        const mrpt::poses::CPose3D& newOrg,
        const mrpt::maps::CLandmarksMap* otherMap);
 
    /** Fuses the contents of another map with this one, updating "this" object
     * with the result.
      *  This process involves fusing corresponding landmarks, then adding the
     * new ones.
      *  \param other The other landmarkmap, whose landmarks will be inserted
     * into "this"
      *  \param justInsertAllOfThem If set to "true", all the landmarks in
     * "other" will be inserted into "this" without checking for possible
     * correspondences (may appear duplicates ones, etc...)
      */
    void fuseWith(CLandmarksMap& other, bool justInsertAllOfThem = false);
 
    /** Returns the (logarithmic) likelihood of a set of landmarks "map" given
     * "this" map.
      *  See paper: JJAA 2006
      */
    double computeLikelihood_SIFT_LandmarkMap(
        CLandmarksMap* map,
        mrpt::tfest::TMatchingPairList* correspondences = nullptr,
        std::vector<bool>* otherCorrespondences = nullptr);
 
    /** Returns true if the map is empty/no observation has been inserted.
       */
    bool isEmpty() const override;
 
    /** Simulates a noisy reading toward each of the beacons in the landmarks
     * map, if any.
      * \param in_robotPose This robot pose is used to simulate the ranges to
     * each beacon.
      * \param in_sensorLocationOnRobot The 3D position of the sensor on the
     * robot
      * \param out_Observations The results will be stored here. NOTICE that the
     * fields
     * "CObservationBeaconRanges::minSensorDistance","CObservationBeaconRanges::maxSensorDistance"
     * and "CObservationBeaconRanges::stdError" MUST BE FILLED OUT before
     * calling this function.
      * An observation will be generated for each beacon in the map, but notice
     * that some of them may be missed if out of the sensor maximum range.
      */
    void simulateBeaconReadings(
        const mrpt::poses::CPose3D& in_robotPose,
        const mrpt::poses::CPoint3D& in_sensorLocationOnRobot,
        mrpt::obs::CObservationBeaconRanges& out_Observations) const;
 
    /** Simulates a noisy bearing-range observation of all the beacons
     * (landamrks with type glBeacon) in the landmarks map, if any.
      * \param[in]  robotPose The robot pose.
      * \param[in]  sensorLocationOnRobot The 3D position of the sensor on the
     * robot
      * \param[out] observations The results will be stored here.
      * \param[in]  sensorDetectsIDs If this is set to false, all the landmarks
     * will be sensed with INVALID_LANDMARK_ID as ID.
      * \param[in]  stdRange The sigma of the sensor noise in range (meters).
      * \param[in]  stdYaw The sigma of the sensor noise in yaw (radians).
      * \param[in]  stdPitch The sigma of the sensor noise in pitch (radians).
      * \param[out] real_associations If it's not a nullptr pointer, this will
     * contain at the return the real indices of the landmarks in the map in the
     * same order than they appear in out_Observations. Useful when
     * sensorDetectsIDs=false. Spurious readings are assigned a
     * std::string::npos (=-1) index.
      * \param[in]  spurious_count_mean The mean number of spurious measurements
     * (uniformly distributed in range & angle) to generate. The number of
     * spurious is generated by rounding a random Gaussin number. If both this
     * mean and the std are zero (the default) no spurious readings are
     * generated.
      * \param[in]  spurious_count_std  Read spurious_count_mean above.
      *
      * \note The fields
     * "CObservationBearingRange::fieldOfView_*","CObservationBearingRange::maxSensorDistance"
     * and "CObservationBearingRange::minSensorDistance" MUST BE FILLED OUT
     * before calling this function.
      * \note At output, the observation will have
     * CObservationBearingRange::validCovariances set to "false" and the 3
     * sensor_std_* members correctly set to their values.
      * An observation will be generated for each beacon in the map, but notice
     * that some of them may be missed if out of the sensor maximum range or
     * field of view-
      */
    void simulateRangeBearingReadings(
        const mrpt::poses::CPose3D& robotPose,
        const mrpt::poses::CPose3D& sensorLocationOnRobot,
        mrpt::obs::CObservationBearingRange& observations,
        bool sensorDetectsIDs = true, const float stdRange = 0.01f,
        const float stdYaw = mrpt::DEG2RAD(0.1f),
        const float stdPitch = mrpt::DEG2RAD(0.1f),
        std::vector<size_t>* real_associations = nullptr,
        const double spurious_count_mean = 0,
        const double spurious_count_std = 0) 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).
      *  In the case of this class, these files are generated:
      *     - "filNamePrefix"+"_3D.m": A script for MATLAB for drawing landmarks
      *as
      *3D ellipses.
      *     - "filNamePrefix"+"_3D.3DScene": A 3D scene with a "ground plane
      *grid"
      *and the set of ellipsoids in 3D.
      */
    void saveMetricMapRepresentationToFile(
        const std::string& filNamePrefix) const override;
 
    /** Returns a 3D object representing the map.
      * \sa COLOR_LANDMARKS_IN_3DSCENES
      */
    void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr& outObj) const override;
 
    // See base docs
    virtual void auxParticleFilterCleanUp() override;
 
    MAP_DEFINITION_START(CLandmarksMap)
    using TPairIdBeacon = std::pair<mrpt::math::TPoint3D, unsigned int>;
    /** Initial contents of the map, especified by a set of 3D Beacons with
     * associated IDs */
    std::deque<TPairIdBeacon> initialBeacons;
    mrpt::maps::CLandmarksMap::TInsertionOptions insertionOpts;
    mrpt::maps::CLandmarksMap::TLikelihoodOptions likelihoodOpts;
    MAP_DEFINITION_END(CLandmarksMap)
 
};  // End of class def.
 
}  // End of namespace
}  // End of namespace
 
#endif