/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#include "vision-precomp.h"  // Precompiled headers
//
#include <mrpt/io/CFileOutputStream.h>
#include <mrpt/maps/CLandmark.h>
#include <mrpt/maps/CLandmarksMap.h>
#include <mrpt/math/geometry.h>
#include <mrpt/math/ops_matrices.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/obs/CObservationBeaconRanges.h>
#include <mrpt/obs/CObservationGPS.h>
#include <mrpt/obs/CObservationImage.h>
#include <mrpt/obs/CObservationRobotPose.h>
#include <mrpt/obs/CObservationStereoImages.h>
#include <mrpt/obs/CObservationVisualLandmarks.h>
#include <mrpt/opengl/CEllipsoid3D.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/Scene.h>
#include <mrpt/poses/CPointPDFGaussian.h>
#include <mrpt/random.h>
#include <mrpt/system/os.h>

#include <Eigen/Dense>

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::maps;
using namespace mrpt::obs;
using namespace mrpt::poses;
using namespace mrpt::tfest;
using namespace mrpt::random;
using namespace mrpt::system;
using namespace mrpt::vision;
using namespace mrpt::img;
using namespace mrpt::containers;
using namespace std;
using mrpt::maps::internal::TSequenceLandmarks;

//  =========== Begin of Map definition ============
MAP_DEFINITION_REGISTER("mrpt::maps::CLandmarksMap,landmarksMap", mrpt::maps::CLandmarksMap)

CLandmarksMap::TMapDefinition::TMapDefinition() = default;
void CLandmarksMap::TMapDefinition::loadFromConfigFile_map_specific(
    const mrpt::config::CConfigFileBase& source, const std::string& sectionNamePrefix)
{
  // [<sectionNamePrefix>+"_creationOpts"]
  const std::string sSectCreation = sectionNamePrefix + string("_creationOpts");
  this->initialBeacons.clear();
  const unsigned int nBeacons = source.read_int(sSectCreation, "nBeacons", 0);
  for (unsigned int q = 1; q <= nBeacons; q++)
  {
    TPairIdBeacon newPair;
    newPair.second = source.read_int(sSectCreation, format("beacon_%03u_ID", q), 0);

    newPair.first.x = source.read_float(sSectCreation, format("beacon_%03u_x", q), 0);
    newPair.first.y = source.read_float(sSectCreation, format("beacon_%03u_y", q), 0);
    newPair.first.z = source.read_float(sSectCreation, format("beacon_%03u_z", q), 0);

    this->initialBeacons.push_back(newPair);
  }

  insertionOpts.loadFromConfigFile(source, sectionNamePrefix + string("_insertOpts"));
  likelihoodOpts.loadFromConfigFile(source, sectionNamePrefix + string("_likelihoodOpts"));
}

void CLandmarksMap::TMapDefinition::dumpToTextStream_map_specific(std::ostream& out) const
{
  out << mrpt::format(
      "number of initial beacons                = %u\n", (int)initialBeacons.size());

  out << "      ID         (X,Y,Z)\n";
  out << "--------------------------------------------------------\n";
  for (const auto& initialBeacon : initialBeacons)
    out << mrpt::format(
        "      %03u         (%8.03f,%8.03f,%8.03f)\n", initialBeacon.second, initialBeacon.first.x,
        initialBeacon.first.y, initialBeacon.first.z);

  this->insertionOpts.dumpToTextStream(out);
  this->likelihoodOpts.dumpToTextStream(out);
}

mrpt::maps::CMetricMap::Ptr CLandmarksMap::internal_CreateFromMapDefinition(
    const mrpt::maps::TMetricMapInitializer& _def)
{
  const CLandmarksMap::TMapDefinition& def =
      *dynamic_cast<const CLandmarksMap::TMapDefinition*>(&_def);
  auto obj = CLandmarksMap::Create();

  for (const auto& initialBeacon : def.initialBeacons)
  {
    CLandmark lm;

    lm.createOneFeature();
    lm.features[0].type = featBeacon;

    lm.features[0].keypoint.ID = initialBeacon.second;
    lm.ID = initialBeacon.second;

    lm.pose_mean = initialBeacon.first;

    lm.pose_cov_11 = lm.pose_cov_22 = lm.pose_cov_33 = lm.pose_cov_12 = lm.pose_cov_13 =
        lm.pose_cov_23 = square(0.01f);

    obj->landmarks.push_back(lm);
  }

  obj->insertionOptions = def.insertionOpts;
  obj->likelihoodOptions = def.likelihoodOpts;
  return obj;
}
//  =========== End of Map definition Block =========

IMPLEMENTS_SERIALIZABLE(CLandmarksMap, CMetricMap, mrpt::maps)

/*---------------------------------------------------------------
        Static variables initialization
  ---------------------------------------------------------------*/
std::map<std::pair<mrpt::maps::CLandmark::TLandmarkID, mrpt::maps::CLandmark::TLandmarkID>, double>
    CLandmarksMap::_mEDD;
mrpt::maps::CLandmark::TLandmarkID CLandmarksMap::_mapMaxID;
bool CLandmarksMap::_maxIDUpdated = false;

void CLandmarksMap::internal_clear() { landmarks.clear(); }

size_t CLandmarksMap::size() const { return landmarks.size(); }

uint8_t CLandmarksMap::serializeGetVersion() const { return 0; }
void CLandmarksMap::serializeTo(mrpt::serialization::CArchive& out) const
{
  uint32_t n = landmarks.size();

  // First, write the number of landmarks:
  out << n;

  // Write all landmarks:
  for (const auto& landmark : landmarks) out << landmark;
}

void CLandmarksMap::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
{
  switch (version)
  {
    case 0:
    {
      uint32_t n, i;
      CLandmark lm;

      // Delete previous content of map:
      // -------------------------------------
      landmarks.clear();

      // Load from stream:
      // -------------------------------------
      in >> n;

      landmarks.clear();  // resize(n);

      // Read all landmarks:
      for (i = 0; i < n; i++)
      {
        in >> lm;
        landmarks.push_back(lm);
      }
    }
    break;
    default:
      MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
  };
}

/*---------------------------------------------------------------
          computeObservationLikelihood
  ---------------------------------------------------------------*/
double CLandmarksMap::internal_computeObservationLikelihood(
    const CObservation& obs, const CPose3D& robotPose3D) const
{
  MRPT_START

  if (CLASS_ID(CObservation2DRangeScan) == obs.GetRuntimeClass() &&
      insertionOptions.insert_Landmarks_from_range_scans)
  {
    /********************************************************************
            OBSERVATION TYPE: CObservation2DRangeScan
      ********************************************************************/
    const auto& o = dynamic_cast<const CObservation2DRangeScan&>(obs);
    CLandmarksMap auxMap;
    CPose2D sensorPose2D(robotPose3D + o.sensorPose);

    auxMap.loadOccupancyFeaturesFrom2DRangeScan(
        o, robotPose3D, likelihoodOptions.rangeScan2D_decimation);

    // And compute its likelihood:
    return computeLikelihood_RSLC_2007(&auxMap, sensorPose2D);
  }  // end of likelihood of 2D range scan:
  else if (CLASS_ID(CObservationStereoImages) == obs.GetRuntimeClass())
  {
    /********************************************************************
            OBSERVATION TYPE: CObservationStereoImages
        Lik. between "this" and "auxMap";
      ********************************************************************/
    const auto& o = dynamic_cast<const CObservationStereoImages&>(obs);

    CLandmarksMap auxMap;
    auxMap.insertionOptions = insertionOptions;
    auxMap.loadSiftFeaturesFromStereoImageObservation(
        o, CLandmarksMap::_mapMaxID, likelihoodOptions.SIFT_feat_options);
    auxMap.changeCoordinatesReference(robotPose3D);

    // ACCESS TO STATIC VARIABLE
    // std::cout << "_mapMaxID, from " << CLandmarksMap::_mapMaxID << " to
    // ";
    if (!CLandmarksMap::_maxIDUpdated)
    {
      CLandmarksMap::_mapMaxID += auxMap.size();
      CLandmarksMap::_maxIDUpdated = true;
    }  // end if

    // std::cout << CLandmarksMap::_mapMaxID <<std::endl;
    return computeLikelihood_SIFT_LandmarkMap(&auxMap);

  }  // end of likelihood of Stereo Images scan:
  else if (CLASS_ID(CObservationBeaconRanges) == obs.GetRuntimeClass())
  {
    /********************************************************************

            OBSERVATION TYPE: CObservationBeaconRanges

        Lik. between "this" and "auxMap";

      ********************************************************************/
    const auto& o = dynamic_cast<const CObservationBeaconRanges&>(obs);

    const double sensorStd = likelihoodOptions.beaconRangesUseObservationStd
                                 ? o.stdError
                                 : likelihoodOptions.beaconRangesStd;
    ASSERT_GT_(sensorStd, .0);
    const auto unif_val = std::log(1.0 / (o.maxSensorDistance - o.minSensorDistance));

    CPointPDFGaussian beaconPDF;
    double ret = 0;

    for (const auto& meas : o.sensedData)
    {
      // Look for the beacon in this map:
      unsigned int sensedID = meas.beaconID;
      bool found = false;

      if (std::isnan(meas.sensedDistance)) continue;

      // Compute the 3D position of the sensor:
      const auto point3D = robotPose3D + meas.sensorLocationOnRobot;

      for (const auto& lm : landmarks)
      {
        if ((lm.getType() != featBeacon) || (lm.ID != sensedID)) continue;  // Skip

        lm.getPose(beaconPDF);
        const auto& beacon3D = beaconPDF.mean;

        const double expectedRange = point3D.distanceTo(beacon3D);
        const double sensedDist = std::max<double>(.0, meas.sensedDistance);
        MRPT_CHECK_NORMAL_NUMBER(expectedRange);

        ret += (-0.5 * mrpt::square((expectedRange - sensedDist) / sensorStd));
        found = true;
        break;  // we found the beacon, skip the rest of landmarks
      }

      // If not found, uniform distribution:
      if (!found && o.maxSensorDistance > o.minSensorDistance)
      {
        ret += unif_val;
        MRPT_CHECK_NORMAL_NUMBER(ret);
      }

    }  // for each sensed beacon

    MRPT_CHECK_NORMAL_NUMBER(ret);
    return ret;

  }  // end of likelihood of CObservationBeaconRanges
  else if (CLASS_ID(CObservationRobotPose) == obs.GetRuntimeClass())
  {
    /********************************************************************

        OBSERVATION TYPE: CObservationRobotPose

        Lik. between "this" and "robotPose";

    ********************************************************************/
    const auto& o = dynamic_cast<const CObservationRobotPose&>(obs);

    // Compute the 3D position of the sensor:
    CPose3D sensorPose3D = robotPose3D + o.sensorPose;

    // Compute the likelihood according to mahalanobis distance between
    // poses:
    CMatrixD dij(1, 6), Cij(6, 6), Cij_1;
    dij(0, 0) = o.pose.mean.x() - sensorPose3D.x();
    dij(0, 1) = o.pose.mean.y() - sensorPose3D.y();
    dij(0, 2) = o.pose.mean.z() - sensorPose3D.z();
    dij(0, 3) = wrapToPi(o.pose.mean.yaw() - sensorPose3D.yaw());
    dij(0, 4) = wrapToPi(o.pose.mean.pitch() - sensorPose3D.pitch());
    dij(0, 5) = wrapToPi(o.pose.mean.roll() - sensorPose3D.roll());

    // Equivalent covariance from "i" to "j":
    Cij = CMatrixDouble(o.pose.cov);
    Cij_1 = Cij.inverse_LLt();

    double distMahaFlik2 = mrpt::math::multiply_HCHt_scalar(dij, Cij_1);
    double ret = -0.5 * (distMahaFlik2 / square(likelihoodOptions.extRobotPoseStd));

    MRPT_CHECK_NORMAL_NUMBER(ret);
    return ret;

  }  // end of likelihood of CObservation
  else if (CLASS_ID(CObservationGPS) == obs.GetRuntimeClass())
  {
    /********************************************************************

            OBSERVATION TYPE: CObservationGPS

    ********************************************************************/
    const auto& o = dynamic_cast<const CObservationGPS&>(obs);
    // Compute the 3D position of the sensor:
    CPoint3D point3D = CPoint3D(robotPose3D);
    CPoint3D GPSpose;
    double x, y;
    double earth_radius = 6378137;

    if ((o.has_GGA_datum()) && (likelihoodOptions.GPSOrigin.min_sat <=
                                o.getMsgByClass<gnss::Message_NMEA_GGA>().fields.satellitesUsed))
    {
      // Compose GPS robot position

      x = DEG2RAD(
              (o.getMsgByClass<gnss::Message_NMEA_GGA>().fields.longitude_degrees -
               likelihoodOptions.GPSOrigin.longitude)) *
          earth_radius * 1.03;
      y = DEG2RAD(
              (o.getMsgByClass<gnss::Message_NMEA_GGA>().fields.latitude_degrees -
               likelihoodOptions.GPSOrigin.latitude)) *
          earth_radius * 1.15;
      GPSpose.x(
          (x * cos(likelihoodOptions.GPSOrigin.ang) + y * sin(likelihoodOptions.GPSOrigin.ang) +
           likelihoodOptions.GPSOrigin.x_shift));
      GPSpose.y(
          (-x * sin(likelihoodOptions.GPSOrigin.ang) + y * cos(likelihoodOptions.GPSOrigin.ang) +
           likelihoodOptions.GPSOrigin.y_shift));
      GPSpose.z(
          (o.getMsgByClass<gnss::Message_NMEA_GGA>().fields.altitude_meters -
           likelihoodOptions.GPSOrigin.altitude));
      // std::cout<<"GPSpose calculo: "<<GPSpose.x<<","<<GPSpose.y<<"\n";

      //-------------------------------//
      // sigmaGPS =
      // f(o.getMsgByClass<gnss::Message_NMEA_GGA>().fields.satellitesUsed)
      // //funcion del numero de satelites
      //-------------------------------//

      // std::cout<<"datos de longitud y latitud:
      // "<<o.getMsgByClass<gnss::Message_NMEA_GGA>().fields.longitude_degrees<<","<<o.getMsgByClass<gnss::Message_NMEA_GGA>().fields.latitude_degrees<<","<<"\n";
      // std::cout<<"x,y sin rotar: "<<x<<","<<y<<","<<"\n";
      // std::cout<<"angulo: "<<likelihoodOptions.GPSOrigin.ang<<"\n";
      // std::cout<<"desp x,y:
      // "<<likelihoodOptions.GPSOrigin.x_shift<<","<<likelihoodOptions.GPSOrigin.y_shift<<"\n";
      // std::cout<<"GPS ORIGIN    :
      // "<<likelihoodOptions.GPSOrigin.longitude<<","<<likelihoodOptions.GPSOrigin.latitude<<","<<likelihoodOptions.GPSOrigin.altitude<<"\n";
      // std::cout<<"POSE DEL ROBOT:
      // "<<point3D.x<<","<<point3D.y<<","<<point3D.z<<"\n";
      // std::cout<<"POSE GPS      :
      // "<<GPSpose.x<<","<<GPSpose.y<<","<<GPSpose.z<<"\n";
      // std::cin.get();

      float distance = GPSpose.distanceTo(point3D);

      // std::cout<<"likel gps:"<<-0.5f*square( ( distance
      // )/likelihoodOptions.GPS_sigma)<<"\n";;
      double ret = -0.5f * square((distance) / likelihoodOptions.GPS_sigma);
      MRPT_CHECK_NORMAL_NUMBER(ret);
      return ret;
    }
    else
      return 0.5;
  }  // end of likelihood of CObservationGPS
  else
  {
    /********************************************************************

          OBSERVATION TYPE: Unknown

    ********************************************************************/
    return 0.5;
  }

  MRPT_END
}

/*---------------------------------------------------------------
            insertObservation
  ---------------------------------------------------------------*/
bool CLandmarksMap::internal_insertObservation(
    const CObservation& obs, const std::optional<const mrpt::poses::CPose3D>& robotPose)
{
  MRPT_START

  CPose2D robotPose2D;
  CPose3D robotPose3D;

  if (robotPose)
  {
    robotPose2D = CPose2D(*robotPose);
    robotPose3D = (*robotPose);
  }
  else
  {
    // Default values are (0,0,0)
  }

  if (CLASS_ID(CObservationImage) == obs.GetRuntimeClass() &&
      insertionOptions.insert_SIFTs_from_monocular_images)
  {
    /********************************************************************

            OBSERVATION TYPE: CObservationImage

      ********************************************************************/
    const auto& o = dynamic_cast<const CObservationImage&>(obs);
    CLandmarksMap tempMap;

    // 1) Load the features in a temporary 3D landmarks map:
    tempMap.loadSiftFeaturesFromImageObservation(o, insertionOptions.SIFT_feat_options);

    // 2) This temp. map must be moved to its real position on the global
    // reference coordinates:
    tempMap.changeCoordinatesReference(robotPose3D);

    // 3) Fuse that map with the current contents of "this" one:
    fuseWith(tempMap);

    // DONE!!

    // Observation was successfully inserted into the map
    // --------------------------------------------------------
    return true;
  }
  //	else
  //	if ( CLASS_ID(CObservation2DRangeScan)==obs.GetRuntimeClass() &&
  //		  insertionOptions.insert_Landmarks_from_range_scans)
  //	{
  /********************************************************************

          OBSERVATION TYPE: CObservation2DRangeScan

    ********************************************************************/
  /*		CObservation2DRangeScan	*o = (CObservation2DRangeScan*) obs;
      CLandmarksMap			tempMap;

      // Load into the map:
      tempMap.loadOccupancyFeaturesFrom2DRangeScan(*o, robotPose);
      fuseWith( tempMap );

      // Observation was successfully inserted into the map
      // --------------------------------------------------------
      return true;
    }       				*/
  else if (
      CLASS_ID(CObservationStereoImages) == obs.GetRuntimeClass() &&
      insertionOptions.insert_SIFTs_from_stereo_images)
  {
    /********************************************************************
            OBSERVATION TYPE: CObservationStereoImages
      ********************************************************************/
    const auto& o = dynamic_cast<const CObservationStereoImages&>(obs);

    // Change coordinates ref:
    CLandmarksMap auxMap;
    auxMap.insertionOptions = insertionOptions;
    auxMap.loadSiftFeaturesFromStereoImageObservation(
        o, CLandmarksMap::_mapMaxID, insertionOptions.SIFT_feat_options);
    auxMap.changeCoordinatesReference(robotPose3D);

    fuseWith(auxMap);

    // Observation was successfully inserted into the map
    // --------------------------------------------------------
    return true;
  }
  else if (CLASS_ID(CObservationVisualLandmarks) == obs.GetRuntimeClass())
  {
    /********************************************************************

            OBSERVATION TYPE:  CObservationVisualLandmarks

      ********************************************************************/
    const auto& o = dynamic_cast<const CObservationVisualLandmarks&>(obs);

    // Change coordinates ref:
    CLandmarksMap auxMap;
    CPose3D acumTransform(robotPose3D + o.refCameraPose);
    auxMap.changeCoordinatesReference(acumTransform, &o.landmarks);

    // Fuse with current:
    fuseWith(auxMap, true);

    // Observation was successfully inserted into the map
    // --------------------------------------------------------
    return true;
  }
  else
  {
    /********************************************************************
          OBSERVATION TYPE: Unknown
    ********************************************************************/
    return false;
  }

  MRPT_END
}

/*---------------------------------------------------------------
        computeMatchingWith2D
  ---------------------------------------------------------------*/
void CLandmarksMap::computeMatchingWith2D(
    [[maybe_unused]] const mrpt::maps::CMetricMap* otherMap,
    [[maybe_unused]] const CPose2D& otherMapPose,
    [[maybe_unused]] float maxDistForCorrespondence,
    [[maybe_unused]] float maxAngularDistForCorrespondence,
    [[maybe_unused]] const CPose2D& angularDistPivotPoint,
    [[maybe_unused]] TMatchingPairList& correspondences,
    [[maybe_unused]] float& correspondencesRatio,
    [[maybe_unused]] float* sumSqrDist,
    [[maybe_unused]] bool onlyKeepTheClosest,
    [[maybe_unused]] bool onlyUniqueRobust) const
{
  MRPT_START

  CLandmarksMap auxMap;
  CPose3D otherMapPose3D(otherMapPose);

  correspondencesRatio = 0;

  // Check the other map class:
  ASSERT_(otherMap->GetRuntimeClass() == CLASS_ID(CLandmarksMap));
  const auto* otherMap2 = dynamic_cast<const CLandmarksMap*>(otherMap);
  std::vector<bool> otherCorrespondences;

  // Coordinates change:
  auxMap.changeCoordinatesReference(otherMapPose3D, otherMap2);

  //// Use the 3D matching method:
  computeMatchingWith3DLandmarks(
      otherMap2, correspondences, correspondencesRatio, otherCorrespondences);

  MRPT_END
}

/*---------------------------------------------------------------
        loadSiftFeaturesFromImageObservation
  ---------------------------------------------------------------*/
void CLandmarksMap::loadSiftFeaturesFromImageObservation(
    const CObservationImage& obs, const mrpt::vision::CFeatureExtraction::TOptions& feat_options)
{
  CImage corImg;
  CPointPDFGaussian landmark3DPositionPDF;
  float d = insertionOptions.SIFTsLoadDistanceOfTheMean;
  float width = insertionOptions.SIFTsLoadEllipsoidWidth;
  CMatrixDouble33 P, D;
  CLandmark lm;

  vision::CFeatureExtraction fExt;
  vision::CFeatureList siftList;        // vision::TSIFTFeatureList siftList;
  vision::CFeatureList::iterator sift;  // vision::TSIFTFeatureList::iterator	sift;

  // Timestamp:
  lm.timestampLastSeen = obs.timestamp;
  lm.seenTimesCount = 1;

  // Remove distortion:
  corImg = obs.image;  // obs.image.correctDistortion(obs.intrinsicParams,obs.distortionParams);

  // Extract SIFT features:
  fExt.options = feat_options;
  fExt.detectFeatures(corImg, siftList);  // vision::computeSiftFeatures(corImg, siftList );

  // Save them as 3D landmarks:
  landmarks.clear();  // resize( siftList.size() );

  for (sift = siftList.begin(); sift != siftList.end(); sift++)
  {
    // Find the 3D position from the pixels
    //  coordinates and the camera intrinsic matrix:
    mrpt::math::TPoint3D dir =
        vision::pixelTo3D(sift->keypoint.pt, obs.cameraParams.intrinsicParams);

    // Compute the mean and covariance of the landmark gaussian 3D position,
    //  from the unitary direction vector and a given distance:
    // --------------------------------------------------------------------------
    landmark3DPositionPDF.mean = CPoint3D(dir);  // The mean is easy :-)
    landmark3DPositionPDF.mean *= d;

    // The covariance:
    P = math::generateAxisBaseFromDirection(dir.x, dir.y, dir.z);

    // Diagonal matrix (with the "size" of the ellipsoid)
    D(0, 0) = square(0.5 * d);
    D(1, 1) = square(width);
    D(2, 2) = square(width);

    // Finally, compute the covariance!
    landmark3DPositionPDF.cov = mrpt::math::multiply_HCHt(P, D);

    // Save into the landmarks vector:
    // --------------------------------------------
    lm.features.resize(1);
    lm.features[0] = (*sift);

    CPoint3D Normal = landmark3DPositionPDF.mean;
    Normal *= -1 / Normal.norm();

    lm.normal = Normal.asTPoint();

    lm.pose_mean = landmark3DPositionPDF.mean.asTPoint();

    lm.pose_cov_11 = landmark3DPositionPDF.cov(0, 0);
    lm.pose_cov_22 = landmark3DPositionPDF.cov(1, 1);
    lm.pose_cov_33 = landmark3DPositionPDF.cov(2, 2);
    lm.pose_cov_12 = landmark3DPositionPDF.cov(0, 1);
    lm.pose_cov_13 = landmark3DPositionPDF.cov(0, 2);
    lm.pose_cov_23 = landmark3DPositionPDF.cov(1, 2);

    landmarks.push_back(lm);
  }

}  // end loadSiftFeaturesFromImageObservation

/*---------------------------------------------------------------
        loadSiftFeaturesFromStereoImagesObservation
  ---------------------------------------------------------------*/
void CLandmarksMap::loadSiftFeaturesFromStereoImageObservation(
    const CObservationStereoImages& obs,
    mrpt::maps::CLandmark::TLandmarkID fID,
    const mrpt::vision::CFeatureExtraction::TOptions& feat_options)
{
  MRPT_START

  vision::CFeatureExtraction fExt;
  vision::CFeatureList leftSiftList, rightSiftList;
  vision::CMatchedFeatureList matchesList;
  vision::TMatchingOptions matchingOptions;
  vision::TStereoSystemParams stereoParams;

  CLandmarksMap landmarkMap;

  // Extract SIFT features:
  fExt.options = feat_options;

  // Default: Hess implementation
  fExt.detectFeatures(
      obs.imageLeft, leftSiftList, fID, insertionOptions.SIFTs_numberOfKLTKeypoints);
  fExt.detectFeatures(
      obs.imageRight, rightSiftList, fID, insertionOptions.SIFTs_numberOfKLTKeypoints);

  matchingOptions.matching_method = vision::TMatchingOptions::mmDescriptorSIFT;
  matchingOptions.epipolar_TH = insertionOptions.SIFTs_epipolar_TH;
  matchingOptions.EDD_RATIO = insertionOptions.SiftCorrRatioThreshold;
  matchingOptions.maxEDD_TH = insertionOptions.SiftEDDThreshold;
  vision::matchFeatures(leftSiftList, rightSiftList, matchesList, matchingOptions);

  if (insertionOptions.PLOT_IMAGES)
  {
    std::cerr << "Warning: insertionOptions.PLOT_IMAGES has no effect "
                 "since MRPT 0.9.1\n";
  }

  // obs.imageLeft.saveToFile("LImage.jpg");
  // obs.imageRight.saveToFile("RImage.jpg");
  // FILE *fmt;
  // fmt = os::fopen( "matchesRBPF.txt", "at" );
  // os::fprintf( fmt, "%d\n", (unsigned int)matchesList.size() );
  // os::fclose(fmt);
  // matchesList.saveToTextFile("matches.txt");

  // Feature Projection to 3D
  // Parameters of the stereo rig

  stereoParams.K = obs.leftCamera.intrinsicParams;
  stereoParams.stdPixel = insertionOptions.SIFTs_stdXY;
  stereoParams.stdDisp = insertionOptions.SIFTs_stdDisparity;
  stereoParams.baseline = obs.rightCameraPose.x();
  stereoParams.minZ = 0.0f;
  stereoParams.maxZ = insertionOptions.SIFTs_stereo_maxDepth;

  size_t numM = matchesList.size();
  vision::projectMatchedFeatures(matchesList, stereoParams, *this);

  // Add Timestamp and the "Seen-Times" counter
  TCustomSequenceLandmarks::iterator ii;
  for (ii = landmarks.begin(); ii != landmarks.end(); ii++)
  {
    (*ii).timestampLastSeen = obs.timestamp;
    (*ii).seenTimesCount = 1;
  }

  printf(
      "%u (out of %u) corrs!\n", static_cast<unsigned>(landmarks.size()),
      static_cast<unsigned>(numM));

  // CLandmarksMap::_maxMapID = fID;

  // Project landmarks according to the ref. camera pose:
  changeCoordinatesReference(mrpt::poses::CPose3D(obs.cameraPose));

  MRPT_END
}

/*---------------------------------------------------------------
        changeCoordinatesReference
  ---------------------------------------------------------------*/
void CLandmarksMap::changeCoordinatesReference(const CPose3D& newOrg)
{
  TSequenceLandmarks::iterator lm;

  CMatrixDouble44 HM;
  newOrg.getHomogeneousMatrix(HM);

  // Build the rotation only transformation:
  double R11 = HM(0, 0);
  double R12 = HM(0, 1);
  double R13 = HM(0, 2);
  double R21 = HM(1, 0);
  double R22 = HM(1, 1);
  double R23 = HM(1, 2);
  double R31 = HM(2, 0);
  double R32 = HM(2, 1);
  double R33 = HM(2, 2);

  double c11, c22, c33, c12, c13, c23;

  // Change the reference of each individual landmark:
  // ----------------------------------------------------
  for (lm = landmarks.begin(); lm != landmarks.end(); lm++)
  {
    // Transform the pose mean & covariance:
    // ---------------------------------------------------------
    newOrg.composePoint(lm->pose_mean, lm->pose_mean);

    float C11 = lm->pose_cov_11;
    float C22 = lm->pose_cov_22;
    float C33 = lm->pose_cov_33;
    float C12 = lm->pose_cov_12;
    float C13 = lm->pose_cov_13;
    float C23 = lm->pose_cov_23;

    // The covariance:  cov = M * cov * (~M);
    c11 = R11 * (C11 * R11 + C12 * R12 + C13 * R13) + R12 * (C12 * R11 + C22 * R12 + C23 * R13) +
          R13 * (C13 * R11 + C23 * R12 + C33 * R13);
    c12 = (C11 * R11 + C12 * R12 + C13 * R13) * R21 + (C12 * R11 + C22 * R12 + C23 * R13) * R22 +
          (C13 * R11 + C23 * R12 + C33 * R13) * R23;
    c13 = (C11 * R11 + C12 * R12 + C13 * R13) * R31 + (C12 * R11 + C22 * R12 + C23 * R13) * R32 +
          (C13 * R11 + C23 * R12 + C33 * R13) * R33;
    c22 = R21 * (C11 * R21 + C12 * R22 + C13 * R23) + R22 * (C12 * R21 + C22 * R22 + C23 * R23) +
          R23 * (C13 * R21 + C23 * R22 + C33 * R23);
    c23 = (C11 * R21 + C12 * R22 + C13 * R23) * R31 + (C12 * R21 + C22 * R22 + C23 * R23) * R32 +
          (C13 * R21 + C23 * R22 + C33 * R23) * R33;
    c33 = R31 * (C11 * R31 + C12 * R32 + C13 * R33) + R32 * (C12 * R31 + C22 * R32 + C23 * R33) +
          R33 * (C13 * R31 + C23 * R32 + C33 * R33);

    // save into the landmark:
    lm->pose_cov_11 = c11;
    lm->pose_cov_22 = c22;
    lm->pose_cov_33 = c33;
    lm->pose_cov_12 = c12;
    lm->pose_cov_13 = c13;
    lm->pose_cov_23 = c23;

    // Rotate the normal:         lm->normal = rot + lm->normal;
    // ---------------------------------------------------------
    float Nx = lm->normal.x;
    float Ny = lm->normal.y;
    float Nz = lm->normal.z;

    lm->normal.x = Nx * R11 + Ny * R12 + Nz * R13;
    lm->normal.y = Nx * R21 + Ny * R22 + Nz * R23;
    lm->normal.z = Nx * R31 + Ny * R32 + Nz * R33;
  }

  // For updating the KD-Tree
  landmarks.hasBeenModifiedAll();
}

/*---------------------------------------------------------------
        changeCoordinatesReference
  ---------------------------------------------------------------*/
void CLandmarksMap::changeCoordinatesReference(
    const CPose3D& newOrg, const mrpt::maps::CLandmarksMap* otherMap)
{
  TSequenceLandmarks::const_iterator lm;
  CLandmark newLandmark;

  CMatrixDouble44 HM;
  newOrg.getHomogeneousMatrix(HM);

  // Build the rotation only transformation:
  double R11 = HM(0, 0);
  double R12 = HM(0, 1);
  double R13 = HM(0, 2);
  double R21 = HM(1, 0);
  double R22 = HM(1, 1);
  double R23 = HM(1, 2);
  double R31 = HM(2, 0);
  double R32 = HM(2, 1);
  double R33 = HM(2, 2);

  double c11, c22, c33, c12, c13, c23;

  landmarks.clear();

  // Change the reference of each individual landmark:
  // ----------------------------------------------------
  for (lm = otherMap->landmarks.begin(); lm != otherMap->landmarks.end(); lm++)
  {
    // Transform the pose mean & covariance:
    // ---------------------------------------------------------
    // The mean:     mean = newReferenceBase + mean;
    newOrg.composePoint(lm->pose_mean, newLandmark.pose_mean);

    float C11 = lm->pose_cov_11;
    float C22 = lm->pose_cov_22;
    float C33 = lm->pose_cov_33;
    float C12 = lm->pose_cov_12;
    float C13 = lm->pose_cov_13;
    float C23 = lm->pose_cov_23;

    // The covariance:  cov = M * cov * (~M);
    c11 = R11 * (C11 * R11 + C12 * R12 + C13 * R13) + R12 * (C12 * R11 + C22 * R12 + C23 * R13) +
          R13 * (C13 * R11 + C23 * R12 + C33 * R13);
    c12 = (C11 * R11 + C12 * R12 + C13 * R13) * R21 + (C12 * R11 + C22 * R12 + C23 * R13) * R22 +
          (C13 * R11 + C23 * R12 + C33 * R13) * R23;
    c13 = (C11 * R11 + C12 * R12 + C13 * R13) * R31 + (C12 * R11 + C22 * R12 + C23 * R13) * R32 +
          (C13 * R11 + C23 * R12 + C33 * R13) * R33;
    c22 = R21 * (C11 * R21 + C12 * R22 + C13 * R23) + R22 * (C12 * R21 + C22 * R22 + C23 * R23) +
          R23 * (C13 * R21 + C23 * R22 + C33 * R23);
    c23 = (C11 * R21 + C12 * R22 + C13 * R23) * R31 + (C12 * R21 + C22 * R22 + C23 * R23) * R32 +
          (C13 * R21 + C23 * R22 + C33 * R23) * R33;
    c33 = R31 * (C11 * R31 + C12 * R32 + C13 * R33) + R32 * (C12 * R31 + C22 * R32 + C23 * R33) +
          R33 * (C13 * R31 + C23 * R32 + C33 * R33);

    // save into the landmark:
    newLandmark.pose_cov_11 = c11;
    newLandmark.pose_cov_22 = c22;
    newLandmark.pose_cov_33 = c33;
    newLandmark.pose_cov_12 = c12;
    newLandmark.pose_cov_13 = c13;
    newLandmark.pose_cov_23 = c23;

    // Rotate the normal:         lm->normal = rot + lm->normal;
    // ---------------------------------------------------------
    float Nx = lm->normal.x;
    float Ny = lm->normal.y;
    float Nz = lm->normal.z;

    newLandmark.normal.x = Nx * R11 + Ny * R12 + Nz * R13;
    newLandmark.normal.y = Nx * R21 + Ny * R22 + Nz * R23;
    newLandmark.normal.z = Nx * R31 + Ny * R32 + Nz * R33;

    newLandmark.ID = lm->ID;

    newLandmark.features = lm->features;

    newLandmark.timestampLastSeen = lm->timestampLastSeen;
    newLandmark.seenTimesCount = lm->seenTimesCount;

    landmarks.push_back(newLandmark);
  }
}

/*---------------------------------------------------------------
            fuseWith
  ---------------------------------------------------------------*/
void CLandmarksMap::fuseWith(CLandmarksMap& other, bool justInsertAllOfThem)
{
  MRPT_START

  // std::cout << "Entrando en fuseWith" << std::endl;

  std::vector<bool> otherCorrs(other.size(), false);
  TMatchingPairList corrs;
  TMatchingPairList::iterator corrIt;
  float corrsRatio;
  CLandmark *thisLM, *otherLM;
  int i, n;
  bool verbose = true;  // false;
  TTimeStamp lastestTime;
  unsigned int nRemoved = 0;

  if (!justInsertAllOfThem)
  {
    // 1) Compute matching between the global and the new map:
    // ---------------------------------------------------------
    computeMatchingWith3DLandmarks(&other, corrs, corrsRatio, otherCorrs);

    // 2) Fuse correspondences
    // ---------------------------------------------------------
    for (corrIt = corrs.begin(); corrIt != corrs.end(); corrIt++)
    {
      thisLM = landmarks.get(corrIt->globalIdx);
      otherLM = other.landmarks.get(corrIt->localIdx);

      // Fuse their poses:
      CPointPDFGaussian P, P1, P2;

      thisLM->getPose(P1);
      otherLM->getPose(P2);

      P.bayesianFusion(P1, P2);

      landmarks.isToBeModified(corrIt->globalIdx);
      thisLM->setPose(P);

      // Update "seen" data:
      thisLM->seenTimesCount++;
      thisLM->timestampLastSeen = otherLM->timestampLastSeen;

      landmarks.hasBeenModified(corrIt->globalIdx);

    }  // end foreach corrs
  }

  // 3) Add new landmarks from the other map:
  // ---------------------------------------------------------
  n = other.size();
  for (i = 0; i < n; i++)
  {
    // Find the lastest time.
    lastestTime = max(lastestTime, other.landmarks.get(i)->timestampLastSeen);

    if (!otherCorrs[i])
    {
      // No corrs: A NEW LANDMARK!
      landmarks.push_back(*other.landmarks.get(i));
    }
  }  // end foreach other landmark

  if (!justInsertAllOfThem)
  {
    // 4) Remove landmarks that have been not seen the required
    //      number of times:
    // ---------------------------------------------------------
    n = landmarks.size();
    for (i = n - 1; i >= 0; i--)
    {
      if (landmarks.get(i)->getType() != featNotDefined)  // Occupancy features
      {
        const double dt = 1e-3 * std::chrono::duration_cast<std::chrono::milliseconds>(
                                     lastestTime - landmarks.get(i)->timestampLastSeen)
                                     .count();
        if (dt > fuseOptions.ellapsedTime &&
            landmarks.get(i)->seenTimesCount < fuseOptions.minTimesSeen)
        {
          landmarks.erase(i);
          nRemoved++;
        }
      }
    }
  }

  if (verbose)
  {
    printf(
        "[CLandmarksMap::fuseWith] %u fused/ %u new/ %u removed -> %u "
        "total\n",
        static_cast<unsigned int>(corrs.size()),
        static_cast<unsigned int>(other.size() - corrs.size()), static_cast<unsigned int>(nRemoved),
        static_cast<unsigned int>(landmarks.size()));
    FILE* f = os::fopen("Fused.txt", "at");
    fprintf(
        f, "%u\t%u\t%u\t%u\n", static_cast<unsigned int>(corrs.size()),
        static_cast<unsigned int>(other.size() - corrs.size()), static_cast<unsigned int>(nRemoved),
        static_cast<unsigned int>(landmarks.size()));
    os::fclose(f);
  }

  MRPT_END
}

/*---------------------------------------------------------------
            computeMatchingWith3DLandmarks
  ---------------------------------------------------------------*/
void CLandmarksMap::computeMatchingWith3DLandmarks(
    const mrpt::maps::CLandmarksMap* anotherMap,
    TMatchingPairList& correspondences,
    float& correspondencesRatio,
    std::vector<bool>& otherCorrespondences) const
{
  MRPT_START

  TSequenceLandmarks::const_iterator thisIt, otherIt;
  unsigned int nThis, nOther;
  int maxIdx;
  float desc;
  unsigned int i, n, j, k;
  TMatchingPair match;
  double lik_dist, lik_desc, lik, maxLik;
  // double									maxLikDist = -1, maxLikDesc =
  // -1;
  CPointPDFGaussian pointPDF_k, pointPDF_j;
  std::vector<bool> thisLandmarkAssigned;
  double K_desc = 0.0;
  double K_dist = 0.0;

  //	FILE									*f = os::fopen( "flik.txt", "wt"
  //);

  // Get the number of landmarks:
  nThis = landmarks.size();
  nOther = anotherMap->landmarks.size();

  // Initially no LM has a correspondence:
  thisLandmarkAssigned.resize(nThis, false);

  // Initially, set all landmarks without correspondences:
  correspondences.clear();
  otherCorrespondences.clear();
  otherCorrespondences.resize(nOther, false);
  correspondencesRatio = 0;

  // Method selection:
  // 0. Our Method.
  // 1. Sim, Elinas, Griffin, Little.

  switch (insertionOptions.SIFTMatching3DMethod)
  {
    case 0:
      // Our method: Filter out by the likelihood of the 3D position and
      // compute the likelihood of the Euclidean descriptor distance

      // "Constants" for the distance computation
      K_desc = -0.5 / square(likelihoodOptions.SIFTs_sigma_descriptor_dist);
      K_dist = -0.5 / square(likelihoodOptions.SIFTs_mahaDist_std);

      // CDynamicGrid<std::vector<int32_t>>		*gridLandmarks =
      // landmarks.getGrid();
      // std::vector<int32_t>						closeLandmarksList;

      for (k = 0, otherIt = anotherMap->landmarks.begin(); otherIt != anotherMap->landmarks.end();
           otherIt++, k++)
      {
        // Load into "pointPDF_k" the PDF of landmark "otherIt":
        otherIt->getPose(pointPDF_k);

        if (otherIt->getType() == featSIFT)
        {
          // minDist = minDist2 = 1e10f;
          maxLik = -1;
          maxIdx = -1;

          for (j = 0, thisIt = landmarks.begin(); thisIt != landmarks.end(); thisIt++, j++)
          {
            if (thisIt->getType() == featSIFT &&
                thisIt->features.size() == otherIt->features.size() && !thisIt->features.empty() &&
                thisIt->features[0].descriptors.SIFT->size() ==
                    otherIt->features[0].descriptors.SIFT->size())
            {
              // Compute "coincidence probability":
              // --------------------------------------
              // Load into "pointPDF_j" the PDF of landmark
              // "otherIt":
              thisIt->getPose(pointPDF_j);

              // Compute lik:
              // lik_dist =
              // pointPDF_k.productIntegralNormalizedWith(
              // &pointPDF_j );
              CMatrixDouble dij(1, 3), Cij(3, 3), Cij_1;
              double distMahaFlik2;

              // Distance between means:
              dij(0, 0) = pointPDF_k.mean.x() - pointPDF_j.mean.x();
              dij(0, 1) = pointPDF_k.mean.y() - pointPDF_j.mean.y();
              dij(0, 2) = pointPDF_k.mean.z() - pointPDF_j.mean.z();

              // Equivalent covariance from "i" to "j":
              Cij = CMatrixDouble(pointPDF_k.cov + pointPDF_j.cov);
              Cij_1 = Cij.inverse_LLt();

              distMahaFlik2 = mrpt::math::multiply_HCHt_scalar(dij, Cij_1);

              lik_dist = exp(K_dist * distMahaFlik2);
              // Likelihood regarding the spatial distance

              if (lik_dist > 1e-2)
              {
                // Compute distance between descriptors:
                // --------------------------------------

                // MODIFICATION 19-SEPT-2007
                // ONLY COMPUTE THE EUCLIDEAN DISTANCE BETWEEN
                // DESCRIPTORS IF IT HAS NOT BEEN COMPUTED
                // BEFORE
                // Make the pair of points
                std::pair<mrpt::maps::CLandmark::TLandmarkID, mrpt::maps::CLandmark::TLandmarkID>
                    mPair(thisIt->ID, otherIt->ID);

                if (CLandmarksMap::_mEDD[mPair] == 0)
                {
                  n = otherIt->features[0].descriptors.SIFT->size();
                  desc = 0;
                  for (i = 0; i < n; i++)
                    desc += square(
                        (*otherIt->features[0].descriptors.SIFT)[i] -
                        (*thisIt->features[0].descriptors.SIFT)[i]);

                  CLandmarksMap::_mEDD[mPair] = desc;
                }  // end if
                else
                {
                  desc = CLandmarksMap::_mEDD[mPair];
                }

                lik_desc = exp(K_desc * desc);  // Likelihood
                                                // regarding the
                                                // descriptor
              }
              else
              {
                lik_desc = 1e-3;
              }

              // Likelihood of the correspondence
              // --------------------------------------
              lik = lik_dist * lik_desc;

              //						os::fprintf( f,
              //"%i\t%i\t%f\t%f\t%f\n", k, j, lik_desc, lik_dist,
              // lik );

              if (lik > maxLik)
              {
                //							maxLikDist =
                // lik_dist;
                //							maxLikDesc =
                // lik_desc;
                maxLik = lik;
                maxIdx = j;
              }

            }  // end of this landmark is SIFT

          }  // End of for each "this", j
          // os::fprintf(f, "%i\t%i\t%f\t%f\t%f\n", maxIdx, k,
          // maxLikDist, maxLikDesc, maxLik);

          // Is it a correspondence?
          if (maxLik > insertionOptions.SiftLikelihoodThreshold)
          {
            // If a previous correspondence for this LM was found,
            // discard this one!
            if (!thisLandmarkAssigned[maxIdx])
            {
              thisLandmarkAssigned[maxIdx] = true;

              // OK: A correspondence found!!
              otherCorrespondences[k] = true;

              match.globalIdx = maxIdx;
              match.global.x = landmarks.get(maxIdx)->pose_mean.x;
              match.global.y = landmarks.get(maxIdx)->pose_mean.y;
              match.global.z = landmarks.get(maxIdx)->pose_mean.z;

              match.localIdx = k;
              match.local.x = anotherMap->landmarks.get(k)->pose_mean.x;
              match.local.y = anotherMap->landmarks.get(k)->pose_mean.y;
              match.local.z = anotherMap->landmarks.get(k)->pose_mean.z;

              correspondences.push_back(match);
            }
          }

        }  // end of "otherIt" is SIFT

      }  // end of other it., k

      // Compute the corrs ratio:
      correspondencesRatio = correspondences.size() / d2f(nOther);
      //		os::fclose(f);

      break;

    case 1:

      // IMPLEMENTATION OF THE METHOD DESCRIBED IN [VISION-BASED SLAM
      // USING THE RBPF][SIM, ELINAS, GRIFFIN, LITTLE]
      // 1. Compute Euclidean descriptor distance (EDD).
      // 2. Matching based on a Threshold.
      // 3. Compute likelihood based only on the position of the 3D
      // landmarks.

      // 1.- COMPUTE EDD

      ASSERT_(!anotherMap->landmarks.begin()->features.empty());
      ASSERT_(!landmarks.begin()->features.empty());
      unsigned int dLen = anotherMap->landmarks.begin()->features[0].descriptors.SIFT->size();
      for (k = 0, otherIt = anotherMap->landmarks.begin(); otherIt != anotherMap->landmarks.end();
           otherIt++, k++)
      {
        double mEDD = -1.0;
        unsigned int mEDDidx = 0;
        for (j = 0, thisIt = landmarks.begin(); thisIt != landmarks.end(); thisIt++, j++)
        {
          // Compute EDD
          double EDD = 0.0;
          for (i = 0; i < dLen; i++)
            EDD += square(
                (*otherIt->features[0].descriptors.SIFT)[i] -
                (*thisIt->features[0].descriptors.SIFT)[i]);

          EDD = sqrt(EDD);

          if (EDD > mEDD)
          {
            mEDD = EDD;
            mEDDidx = j;
          }  // end if
        }    // end for j

        if (mEDD > insertionOptions.SiftEDDThreshold)
        {
          // There is a correspondence
          if (!thisLandmarkAssigned[mEDDidx])  // If there is not
          // multiple
          // correspondence
          {
            thisLandmarkAssigned[mEDDidx] = true;

            // OK: A correspondence found!!
            otherCorrespondences[k] = true;

            otherIt->getPose(pointPDF_k);
            thisIt->getPose(pointPDF_j);

            match.globalIdx = j;
            match.global.x = landmarks.get(mEDDidx)->pose_mean.x;
            match.global.y = landmarks.get(mEDDidx)->pose_mean.y;
            match.global.z = landmarks.get(mEDDidx)->pose_mean.z;

            match.localIdx = k;
            match.local.x = anotherMap->landmarks.get(k)->pose_mean.x;
            match.local.y = anotherMap->landmarks.get(k)->pose_mean.y;
            match.local.z = anotherMap->landmarks.get(k)->pose_mean.z;

            correspondences.push_back(match);

          }  // end if multiple correspondence

        }  // end if mEDD

      }  // end for k

      correspondencesRatio = correspondences.size() / d2f(nOther);

      break;

  }  // end switch

  MRPT_END
}

/*---------------------------------------------------------------
            saveToTextFile
  ---------------------------------------------------------------*/
bool CLandmarksMap::saveToTextFile(std::string file)
{
  MRPT_START

  FILE* f = os::fopen(file.c_str(), "wt");
  if (!f) return false;

  // os::fprintf(f,"%% Map of landmarks - file dumped by
  // mrpt::maps::CLandmarksMap\n");
  // os::fprintf(f,"%%  Columns are: X Y Z TYPE(TKeyPointMethod) TIMES_SEEN
  // TIME_OF_LAST_OBSERVATION [SIFT DESCRIPTOR] ID\n");
  // os::fprintf(f,"%%
  // -----------------------------------------------------------------------------------------------------\n");

  for (auto it = landmarks.begin(); it != landmarks.end(); ++it)
  {
    os::fprintf(
        f, "%10f %10f %10f %4i %4u %10f", it->pose_mean.x, it->pose_mean.y, it->pose_mean.z,
        static_cast<int>(it->getType()), it->seenTimesCount,
        it->timestampLastSeen == INVALID_TIMESTAMP
            ? 0
            : mrpt::system::extractDayTimeFromTimestamp(it->timestampLastSeen));

    if (it->getType() == featSIFT)
    {
      ASSERT_(!it->features.empty());
      for (unsigned char i : *it->features[0].descriptors.SIFT) os::fprintf(f, " %u ", i);
    }
    os::fprintf(f, " %i ", (int)it->ID);

    os::fprintf(f, "\n");
  }

  os::fclose(f);

  return true;

  MRPT_END
}

/*---------------------------------------------------------------
            saveToMATLABScript3D
  ---------------------------------------------------------------*/
bool CLandmarksMap::saveToMATLABScript3D(
    std::string file, const char* style, float confInterval) const
{
  FILE* f = os::fopen(file.c_str(), "wt");
  if (!f) return false;

  // Header:
  os::fprintf(f, "%%-------------------------------------------------------\n");
  os::fprintf(f, "%% File automatically generated using the MRPT method:\n");
  os::fprintf(f, "%%   'CLandmarksMap::saveToMATLABScript3D'\n");
  os::fprintf(f, "%%\n");
  os::fprintf(f, "%%                        ~ MRPT ~\n");
  os::fprintf(f, "%%  Jose Luis Blanco Claraco, University of Malaga @ 2006\n");
  os::fprintf(f, "%%  http://www.isa.uma.es/ \n");
  os::fprintf(f, "%%-------------------------------------------------------\n\n");

  // Main code:
  os::fprintf(f, "hold on;\n\n");

  for (const auto& landmark : landmarks)
  {
    os::fprintf(
        f, "m=[%.4f %.4f %.4f];", landmark.pose_mean.x, landmark.pose_mean.y, landmark.pose_mean.z);
    os::fprintf(
        f, "c=[%.8f %.8f %.8f;%.8f %.8f %.8f;%.8f %.8f %.8f]; ", landmark.pose_cov_11,
        landmark.pose_cov_12, landmark.pose_cov_13, landmark.pose_cov_12, landmark.pose_cov_22,
        landmark.pose_cov_23, landmark.pose_cov_13, landmark.pose_cov_23, landmark.pose_cov_33);

    os::fprintf(
        f,
        "error_ellipse(c,m,'conf',%f,'style','%s','numPointsEllipse',10);"
        "\n",
        confInterval, style);
  }

  os::fprintf(f, "axis equal;grid on;xlabel('x'); ylabel('y'); zlabel('z');");

  os::fclose(f);
  return true;
}
/**/

/*---------------------------------------------------------------
            saveToMATLABScript2D
  ---------------------------------------------------------------*/
bool CLandmarksMap::saveToMATLABScript2D(std::string file, const char* style, float stdCount)
{
  FILE* f = os::fopen(file.c_str(), "wt");
  if (!f) return false;

  const int ELLIPSE_POINTS = 30;
  std::vector<float> X, Y, COS, SIN;
  std::vector<float>::iterator x, y, Cos, Sin;
  double ang;
  CMatrixDouble22 cov, eigVal, eigVec, M;

  X.resize(ELLIPSE_POINTS);
  Y.resize(ELLIPSE_POINTS);
  COS.resize(ELLIPSE_POINTS);
  SIN.resize(ELLIPSE_POINTS);

  // Fill the angles:
  for (Cos = COS.begin(), Sin = SIN.begin(), ang = 0; Cos != COS.end();
       Cos++, Sin++, ang += (M_2PI / (ELLIPSE_POINTS - 1)))
  {
    *Cos = cos(ang);
    *Sin = sin(ang);
  }

  // Header:
  os::fprintf(f, "%%-------------------------------------------------------\n");
  os::fprintf(f, "%% File automatically generated using the MRPT method:\n");
  os::fprintf(f, "%%   'CLandmarksMap::saveToMATLABScript2D'\n");
  os::fprintf(f, "%%\n");
  os::fprintf(f, "%%                        ~ MRPT ~\n");
  os::fprintf(f, "%%  Jose Luis Blanco Claraco, University of Malaga @ 2006\n");
  os::fprintf(f, "%%  http://www.isa.uma.es/ \n");
  os::fprintf(f, "%%-------------------------------------------------------\n\n");

  // Main code:
  os::fprintf(f, "hold on;\n\n");

  for (auto& landmark : landmarks)
  {
    // Compute the eigen-vectors & values:
    cov(0, 0) = landmark.pose_cov_11;
    cov(1, 1) = landmark.pose_cov_22;
    cov(0, 1) = cov(1, 0) = landmark.pose_cov_12;

    std::vector<double> eigvals;
    cov.eig_symmetric(eigVec, eigvals);
    eigVal.setZero();
    eigVal.setDiagonal(eigvals);
    eigVal = eigVal.array().sqrt().matrix();
    M = eigVal.asEigen() * eigVec.transpose();

    // Compute the points of the ellipsoid:
    // ----------------------------------------------
    for (x = X.begin(), y = Y.begin(), Cos = COS.begin(), Sin = SIN.begin(); x != X.end();
         x++, y++, Cos++, Sin++)
    {
      *x = (landmark.pose_mean.x + stdCount * (*Cos * M(0, 0) + *Sin * M(1, 0)));
      *y = (landmark.pose_mean.y + stdCount * (*Cos * M(0, 1) + *Sin * M(1, 1)));
    }

    // Save the code to plot the ellipsoid:
    // ----------------------------------------------
    os::fprintf(f, "plot([ ");
    for (x = X.begin(); x != X.end(); x++)
    {
      os::fprintf(f, "%.4f", *x);
      if (x != (X.end() - 1)) os::fprintf(f, ",");
    }
    os::fprintf(f, "],[ ");
    for (y = Y.begin(); y != Y.end(); y++)
    {
      os::fprintf(f, "%.4f", *y);
      if (y != (Y.end() - 1)) os::fprintf(f, ",");
    }

    os::fprintf(f, "],'%s');\n", style);

    // os::fprintf(f,"error_ellipse(c,m,'conf',0.99,'style','%s','numPointsEllipse',10);\n",style);
  }

  os::fprintf(f, "\naxis equal;\n");
  os::fclose(f);
  return true;
}

/*---------------------------------------------------------------
            loadOccupancyFeaturesFrom2DRangeScan
  ---------------------------------------------------------------*/
void CLandmarksMap::loadOccupancyFeaturesFrom2DRangeScan(
    const CObservation2DRangeScan& obs,
    const std::optional<const mrpt::poses::CPose3D>& robotPose,
    unsigned int downSampleFactor)
{
  unsigned int i, n = obs.getScanSize();
  double Th, dTh;             // angle of the ray
  double J11, J12, J21, J22;  // The jacobian elements.
  double d;
  CPose3D sensorGlobalPose;

  // Empty the map:
  this->clear();

  // Sensor pose in 3D:
  if (!robotPose)
    sensorGlobalPose = obs.sensorPose;
  else
    sensorGlobalPose = *robotPose + obs.sensorPose;

  // Starting direction:
  if (obs.rightToLeft)
  {
    Th = -0.5 * obs.aperture;
    dTh = obs.aperture / n;
  }
  else
  {
    Th = +0.5 * obs.aperture;
    dTh = -obs.aperture / n;
  }

  // Measurement uncertainties:
  double var_d = square(0.005);  // square(obs.stdError);
  double var_th = square(dTh / 10.0);

  // For each range:
  for (i = 0; i < n; i += downSampleFactor, Th += downSampleFactor * dTh)
  {
    // If it is a valid ray:
    if (obs.getScanRangeValidity(i))
    {
      CLandmark newLandmark;

      // Timestamp:
      newLandmark.timestampLastSeen = obs.timestamp;
      newLandmark.seenTimesCount = 1;

      newLandmark.createOneFeature();
      newLandmark.features[0].type = featNotDefined;

      d = obs.getScanRange(i);

      // Compute the landmark in 2D:
      // -----------------------------------------------
      // Descriptor:
      newLandmark.features[0].orientation = Th;
      newLandmark.features[0].keypoint.octave = d;

      // Mean:
      newLandmark.pose_mean.x = (cos(Th) * d);
      newLandmark.pose_mean.y = (sin(Th) * d);
      newLandmark.pose_mean.z = 0;

      // Normal direction:
      newLandmark.normal = newLandmark.pose_mean;
      newLandmark.normal *= -1.0f / newLandmark.pose_mean.norm();

      // Jacobian:
      J11 = -d * sin(Th);
      J12 = cos(Th);
      J21 = d * cos(Th);
      J22 = sin(Th);

      // Covariance matrix:
      newLandmark.pose_cov_11 = (J11 * J11 * var_th + J12 * J12 * var_d);
      newLandmark.pose_cov_12 = (J11 * J21 * var_th + J12 * J22 * var_d);
      newLandmark.pose_cov_22 = (J21 * J21 * var_th + J22 * J22 * var_d);
      newLandmark.pose_cov_33 = (square(0.005));  // var_d;
      newLandmark.pose_cov_13 = newLandmark.pose_cov_23 = 0;

      // Append it:
      // -----------------------------------------------
      landmarks.push_back(newLandmark);

    }  // end of valid ray.

  }  // end for n

  // Take landmarks to 3D according to the robot & sensor 3D poses:
  // -----------------------------------------------
  changeCoordinatesReference(sensorGlobalPose);
}

/*---------------------------------------------------------------
        METHOD DESCRIBED IN PAPER
        -------------------------

    ICRA 2007,....

  ---------------------------------------------------------------*/
double CLandmarksMap::computeLikelihood_RSLC_2007(
    const CLandmarksMap* s, [[maybe_unused]] const CPose2D& sensorPose) const
{
  MRPT_START

  double lik = 1.0;
  TSequenceLandmarks::const_iterator itOther;
  double corr;
  double PrNoCorr;
  CPointPDFGaussian poseThis, poseOther;
  double nFoundCorrs = 0;
  unsigned int cx, cy, cx_1, cx_2, cy_1, cy_2;

  const auto grid = landmarks.getGrid();

  // For each landmark in the observations:
  for (itOther = s->landmarks.begin(); itOther != s->landmarks.end(); itOther++)
  {
    itOther->getPose(poseOther);

    cx = cy = grid->y2idx(itOther->pose_mean.y);

    cx_1 = max(0, grid->x2idx(itOther->pose_mean.x - 0.10f));
    cx_2 = min(static_cast<int>(grid->getSizeX()) - 1, grid->x2idx(itOther->pose_mean.x + 0.10f));
    cy_1 = max(0, grid->y2idx(itOther->pose_mean.y - 0.10f));
    cy_2 = min(static_cast<int>(grid->getSizeY()) - 1, grid->y2idx(itOther->pose_mean.y + 0.10f));

    // The likelihood of no correspondence starts at "1" and will be
    // modified next:
    PrNoCorr = 1;

    // For each landmark in this map: Compute its correspondence likelihood
    //   and conditioned observation likelihood:
    // itClosest = nullptr;

    // Look at landmarks in sourronding cells only:
    for (cx = cx_1; cx <= cx_2; cx++)
      for (cy = cy_1; cy <= cy_2; cy++)
      {
        auto corrs = grid->cellByIndex(cx, cy);
        ASSERT_(corrs != nullptr);
        if (!corrs->empty())
          for (auto it : *corrs)
          {
            auto lm = landmarks.get(it);

            // Compute the "correspondence" in the range [0,1]:
            // -------------------------------------------------------------

            // Shortcut:
            if (fabs(lm->pose_mean.x - itOther->pose_mean.x) > 0.15f ||
                fabs(lm->pose_mean.y - itOther->pose_mean.y) > 0.15f)
            {
              // Absolutely no correspondence, not need to compute
              // it:
              corr = 0;
            }
            else
            {
              lm->getPose(poseThis);
              corr = poseOther.productIntegralNormalizedWith2D(poseThis);
            }

            // The likelihood of no corresp. is proportional to the
            // product of all "1-CORRij":
            // -------------------------------------------------------------
            PrNoCorr *= 1 - corr;

          }  // end of each landmark in this map.
      }

    // Only consider this "point" if it has some real chance to has a
    // correspondence:
    nFoundCorrs += 1 - PrNoCorr;

    lik *= 1 - PrNoCorr;
  }  // enf for each landmark in the other map.

  lik = nFoundCorrs / static_cast<double>(s->size());

  lik = log(lik);

  MRPT_CHECK_NORMAL_NUMBER(lik);
  return lik;

  MRPT_END
}

/*---------------------------------------------------------------

          TCustomSequenceLandmarks

  ---------------------------------------------------------------*/
CLandmarksMap::TCustomSequenceLandmarks::TCustomSequenceLandmarks() :
    m_landmarks(), m_grid(-10.0f, 10.0f, -10.0f, 10.f, 0.20f)

{
}

void CLandmarksMap::TCustomSequenceLandmarks::clear()
{
  m_landmarks.clear();

  // Erase the grid:
  m_grid.clear();

  m_largestDistanceFromOriginIsUpdated = false;
}

void CLandmarksMap::TCustomSequenceLandmarks::push_back(const CLandmark& l)
{
  // Resize grid if necessary:
  std::vector<int32_t> dummyEmpty;

  m_grid.resize(
      min(m_grid.getXMin(), l.pose_mean.x - 0.1), max(m_grid.getXMax(), l.pose_mean.x + 0.1),
      min(m_grid.getYMin(), l.pose_mean.y - 0.1), max(m_grid.getYMax(), l.pose_mean.y + 0.1),
      dummyEmpty);

  m_landmarks.push_back(l);

  // Add to the grid:
  std::vector<int32_t>* cell = m_grid.cellByPos(l.pose_mean.x, l.pose_mean.y);
  ASSERT_(cell);
  cell->push_back(m_landmarks.size() - 1);

  m_largestDistanceFromOriginIsUpdated = false;
}

CLandmark* CLandmarksMap::TCustomSequenceLandmarks::get(unsigned int indx)
{
  return &m_landmarks[indx];
}

const CLandmark* CLandmarksMap::TCustomSequenceLandmarks::get(unsigned int indx) const
{
  return &m_landmarks[indx];
}

void CLandmarksMap::TCustomSequenceLandmarks::isToBeModified(unsigned int indx)
{
  std::vector<int32_t>* cell =
      m_grid.cellByPos(m_landmarks[indx].pose_mean.x, m_landmarks[indx].pose_mean.y);

  std::vector<int32_t>::iterator it;
  for (it = cell->begin(); it != cell->end(); it++)
  {
    if (*it == static_cast<int>(indx))
    {
      cell->erase(it);
      return;
    }
  }

  m_largestDistanceFromOriginIsUpdated = false;
}

void CLandmarksMap::TCustomSequenceLandmarks::erase(unsigned int indx)
{
  m_landmarks.erase(m_landmarks.begin() + indx);
  m_largestDistanceFromOriginIsUpdated = false;
}

void CLandmarksMap::TCustomSequenceLandmarks::hasBeenModified(unsigned int indx)
{
  std::vector<int32_t> dummyEmpty;

  // Resize grid if necessary:
  m_grid.resize(
      min(m_grid.getXMin(), m_landmarks[indx].pose_mean.x),
      max(m_grid.getXMax(), m_landmarks[indx].pose_mean.x),
      min(m_grid.getYMin(), m_landmarks[indx].pose_mean.y),
      max(m_grid.getYMax(), m_landmarks[indx].pose_mean.y), dummyEmpty);

  // Add to the grid:
  std::vector<int32_t>* cell =
      m_grid.cellByPos(m_landmarks[indx].pose_mean.x, m_landmarks[indx].pose_mean.y);
  cell->push_back(indx);
  m_largestDistanceFromOriginIsUpdated = false;
}

void CLandmarksMap::TCustomSequenceLandmarks::hasBeenModifiedAll()
{
  MRPT_START

  TSequenceLandmarks::iterator it;
  unsigned int idx;
  double min_x = -10.0, max_x = 10.0;
  double min_y = -10.0, max_y = 10.0;
  std::vector<int32_t> dummyEmpty;

  // Clear cells:
  m_grid.clear();

  // Resize the grid to the outer limits of landmarks:
  for (idx = 0, it = m_landmarks.begin(); it != m_landmarks.end(); idx++, it++)
  {
    min_x = min(min_x, it->pose_mean.x);
    max_x = max(max_x, it->pose_mean.x);
    min_y = min(min_y, it->pose_mean.y);
    max_y = max(max_y, it->pose_mean.y);
  }
  m_grid.resize(min_x, max_x, min_y, max_y, dummyEmpty);

  // Add landmarks to cells:
  for (idx = 0, it = m_landmarks.begin(); it != m_landmarks.end(); idx++, it++)
  {
    std::vector<int32_t>* cell = m_grid.cellByPos(it->pose_mean.x, it->pose_mean.y);
    cell->push_back(idx);
  }

  m_largestDistanceFromOriginIsUpdated = false;
  MRPT_END
}

/*---------------------------------------------------------------
            getLargestDistanceFromOrigin
---------------------------------------------------------------*/
float CLandmarksMap::TCustomSequenceLandmarks::getLargestDistanceFromOrigin() const
{
  // Updated?
  if (!m_largestDistanceFromOriginIsUpdated)
  {
    // NO: Update it:
    float maxDistSq = 0, d;
    for (const auto& it : *this)
    {
      d = square(it.pose_mean.x) + square(it.pose_mean.y) + square(it.pose_mean.z);
      maxDistSq = max(d, maxDistSq);
    }

    m_largestDistanceFromOrigin = sqrt(maxDistSq);
    m_largestDistanceFromOriginIsUpdated = true;
  }
  return m_largestDistanceFromOrigin;
}

/*---------------------------------------------------------------
          computeLikelihood_SIFT_LandmarkMap
  ---------------------------------------------------------------*/
double CLandmarksMap::computeLikelihood_SIFT_LandmarkMap(
    CLandmarksMap* theMap,
    TMatchingPairList* correspondences,
    std::vector<bool>* otherCorrespondences) const
{
  double lik = 0;  // For 'traditional'
  double lik_i;
  unsigned long distDesc;
  double likByDist, likByDesc;

  std::vector<unsigned char>::const_iterator it1, it2;
  double K_dist = -0.5 / square(likelihoodOptions.SIFTs_mahaDist_std);
  double K_desc = -0.5 / square(likelihoodOptions.SIFTs_sigma_descriptor_dist);

  unsigned int idx1, idx2;
  CPointPDFGaussian lm1_pose, lm2_pose;
  CMatrixD dij(1, 3), Cij(3, 3), Cij_1;
  double distMahaFlik2;
  int decimation = likelihoodOptions.SIFTs_decimation;

  // USE INSERTOPTIONS METHOD
  switch (insertionOptions.SIFTLikelihoodMethod)
  {
    case 0:  // Our method
    {
      // lik = 1e-9;		// For consensus
      lik = 1.0;  // For traditional

      TSequenceLandmarks::const_iterator lm1, lm2;
      for (idx1 = 0, lm1 = theMap->landmarks.begin(); lm1 < theMap->landmarks.end();
           lm1 += decimation, idx1 += decimation)  // Other theMap LM1
      {
        if (lm1->getType() == featSIFT)
        {
          // Get the pose of lm1 as an object:
          lm1->getPose(lm1_pose);

          lik_i = 0;  // Counter

          for (idx2 = 0, lm2 = landmarks.begin(); lm2 != landmarks.end();
               lm2++, idx2++)  // This theMap LM2
          {
            if (lm2->getType() == featSIFT)
            {
              // Get the pose of lm2 as an object:
              lm2->getPose(lm2_pose);

              // Compute the likelihood according to mahalanobis
              // distance:
              // Distance between means:
              dij(0, 0) = lm1->pose_mean.x - lm2->pose_mean.x;
              dij(0, 1) = lm1->pose_mean.y - lm2->pose_mean.y;
              dij(0, 2) = lm1->pose_mean.z - lm2->pose_mean.z;

              // std::cout << "ED POSICION: " << sqrt(
              // dij(0,0)*dij(0,0) + dij(0,1)*dij(0,1) +
              // dij(0,2)*dij(0,2) ) << std::endl;
              // Equivalent covariance from "i" to "j":
              Cij = CMatrixDouble(lm1_pose.cov + lm2_pose.cov);
              Cij_1 = Cij.inverse_LLt();

              distMahaFlik2 = mrpt::math::multiply_HCHt_scalar(dij, Cij_1);

              likByDist = exp(K_dist * distMahaFlik2);

              if (likByDist > 1e-2)
              {
                // If the EUCLIDEAN distance is not too large,
                // we compute the Descriptor distance
                // Compute Likelihood by descriptor
                // Compute distance between descriptors

                // IF the EDD has been already computed, we skip
                // this step!
                std::pair<mrpt::maps::CLandmark::TLandmarkID, mrpt::maps::CLandmark::TLandmarkID>
                    mPair(lm2->ID, lm1->ID);
                // std::cout << "Par: (" << lm2->ID << "," <<
                // lm1->ID << ") -> ";

                if (CLandmarksMap::_mEDD[mPair] == 0)
                {
                  distDesc = 0;
                  ASSERT_(!lm1->features.empty() && !lm2->features.empty());
                  ASSERT_(
                      lm1->features[0].descriptors.SIFT->size() ==
                      lm2->features[0].descriptors.SIFT->size());

                  for (it1 = lm1->features[0].descriptors.SIFT->begin(),
                      it2 = lm2->features[0].descriptors.SIFT->begin();
                       it1 != lm1->features[0].descriptors.SIFT->end(); it1++, it2++)
                    distDesc += square(*it1 - *it2);

                  // Insert into the vector of Euclidean
                  // distances
                  CLandmarksMap::_mEDD[mPair] = distDesc;
                }  // end if
                else
                {
                  distDesc = (unsigned long)CLandmarksMap::_mEDD[mPair];
                }
                likByDesc = exp(K_desc * distDesc);
                lik_i += likByDist * likByDesc;  // Cumulative Likelihood
              }
              else
              {
                // If the EUCLIDEAN distance is too large, we
                // assume that the cumulative likelihood is
                // (almost) zero.
                lik_i += 1e-10f;
              }
            }                          // end if
          }                            // end for "lm2"
          lik *= (0.1 + 0.9 * lik_i);  // (TRADITIONAL) Total
        }
      }  // end for "lm1"
    }
    break;

    case 1:  // SIM, ELINAS, GRIFFIN, LITTLE
      double dist;
      TMatchingPairList::iterator itCorr;

      lik = 1.0f;

      // Check if the Matches are inserted into the function
      if (correspondences == nullptr)
        THROW_EXCEPTION(
            "When using this method with SIFTLikelihoodMethod = 1, "
            "TMatchingPairList *correspondence can not be NULL");

      if (otherCorrespondences == nullptr)
        THROW_EXCEPTION(
            "When using this method with SIFTLikelihoodMethod = 1, "
            "std::vector<bool> *otherCorrespondences can not be NULL");

      for (itCorr = correspondences->begin(); itCorr != correspondences->end(); itCorr++)
      {
        auto lm1 = theMap->landmarks.get(itCorr->localIdx);
        auto lm2 = landmarks.get(itCorr->globalIdx);

        dij(0, 0) = lm1->pose_mean.x - lm2->pose_mean.x;
        dij(0, 1) = lm1->pose_mean.y - lm2->pose_mean.y;
        dij(0, 2) = lm1->pose_mean.z - lm2->pose_mean.z;

        // Equivalent covariance from "i" to "j":
        Cij = CMatrixDouble(lm1_pose.cov + lm2_pose.cov);
        Cij_1 = Cij.inverse_LLt();

        distMahaFlik2 = mrpt::math::multiply_HCHt_scalar(dij, Cij_1);

        dist = min((double)likelihoodOptions.SIFTnullCorrespondenceDistance, distMahaFlik2);

        lik *= exp(-0.5 * dist);

      }  // end for correspondences

      // We complete the likelihood with the null correspondences
      for (size_t k = 1; k <= (theMap->size() - correspondences->size()); k++)
        lik *= likelihoodOptions.SIFTnullCorrespondenceDistance;

      break;

  }  // end switch

  lik = log(lik);
  MRPT_CHECK_NORMAL_NUMBER(lik);
  return lik;
}

/*---------------------------------------------------------------
          TInsertionOptions
  ---------------------------------------------------------------*/
CLandmarksMap::TInsertionOptions::TInsertionOptions() :

    SIFT_feat_options(vision::featSIFT)
{
}

/*---------------------------------------------------------------
          dumpToTextStream
  ---------------------------------------------------------------*/
void CLandmarksMap::TInsertionOptions::dumpToTextStream(std::ostream& out) const
{
  out << "\n----------- [CLandmarksMap::TInsertionOptions] ------------ \n\n";

  out << mrpt::format(
      "insert_SIFTs_from_monocular_images      = %c\n",
      insert_SIFTs_from_monocular_images ? 'Y' : 'N');
  out << mrpt::format(
      "insert_SIFTs_from_stereo_images         = %c\n",
      insert_SIFTs_from_stereo_images ? 'Y' : 'N');
  out << mrpt::format(
      "insert_Landmarks_from_range_scans       = %c\n",
      insert_Landmarks_from_range_scans ? 'Y' : 'N');
  out << "\n";
  out << mrpt::format("SiftCorrRatioThreshold                  = %f\n", SiftCorrRatioThreshold);
  out << mrpt::format("SiftLikelihoodThreshold                 = %f\n", SiftLikelihoodThreshold);
  out << mrpt::format("SiftEDDThreshold                        = %f\n", SiftEDDThreshold);
  out << mrpt::format("SIFTMatching3DMethod                    = %d\n", SIFTMatching3DMethod);
  out << mrpt::format("SIFTLikelihoodMethod                    = %d\n", SIFTLikelihoodMethod);

  out << mrpt::format("SIFTsLoadDistanceOfTheMean              = %f\n", SIFTsLoadDistanceOfTheMean);
  out << mrpt::format("SIFTsLoadEllipsoidWidth                 = %f\n", SIFTsLoadEllipsoidWidth);
  out << "\n";
  out << mrpt::format("SIFTs_stdXY                             = %f\n", SIFTs_stdXY);
  out << mrpt::format("SIFTs_stdDisparity                      = %f\n", SIFTs_stdDisparity);
  out << "\n";
  out << mrpt::format("SIFTs_numberOfKLTKeypoints              = %i\n", SIFTs_numberOfKLTKeypoints);
  out << mrpt::format("SIFTs_stereo_maxDepth                   = %f\n", SIFTs_stereo_maxDepth);
  out << mrpt::format("SIFTs_epipolar_TH                       = %f\n", SIFTs_epipolar_TH);
  out << mrpt::format("PLOT_IMAGES                             = %c\n", PLOT_IMAGES ? 'Y' : 'N');

  SIFT_feat_options.dumpToTextStream(out);

  out << "\n";
}

/*---------------------------------------------------------------
          loadFromConfigFile
  ---------------------------------------------------------------*/
void CLandmarksMap::TInsertionOptions::loadFromConfigFile(
    const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
{
  insert_SIFTs_from_monocular_images = iniFile.read_bool(
      section.c_str(), "insert_SIFTs_from_monocular_images", insert_SIFTs_from_monocular_images);
  insert_SIFTs_from_stereo_images = iniFile.read_bool(
      section.c_str(), "insert_SIFTs_from_stereo_images", insert_SIFTs_from_stereo_images);
  insert_Landmarks_from_range_scans = iniFile.read_bool(
      section.c_str(), "insert_Landmarks_from_range_scans", insert_Landmarks_from_range_scans);
  SiftCorrRatioThreshold =
      iniFile.read_float(section.c_str(), "SiftCorrRatioThreshold", SiftCorrRatioThreshold);
  SiftLikelihoodThreshold =
      iniFile.read_float(section.c_str(), "SiftLikelihoodThreshold", SiftLikelihoodThreshold);
  SiftEDDThreshold = iniFile.read_float(section.c_str(), "SiftEDDThreshold", SiftEDDThreshold);
  SIFTMatching3DMethod =
      iniFile.read_int(section.c_str(), "SIFTMatching3DMethod", SIFTMatching3DMethod);
  SIFTLikelihoodMethod =
      iniFile.read_int(section.c_str(), "SIFTLikelihoodMethod", SIFTLikelihoodMethod);
  SIFTsLoadDistanceOfTheMean =
      iniFile.read_float(section.c_str(), "SIFTsLoadDistanceOfTheMean", SIFTsLoadDistanceOfTheMean);
  SIFTsLoadEllipsoidWidth =
      iniFile.read_float(section.c_str(), "SIFTsLoadEllipsoidWidth", SIFTsLoadEllipsoidWidth);
  SIFTs_stdXY = iniFile.read_float(section.c_str(), "SIFTs_stdXY", SIFTs_stdXY);
  SIFTs_stdDisparity =
      iniFile.read_float(section.c_str(), "SIFTs_stdDisparity", SIFTs_stdDisparity);
  SIFTs_numberOfKLTKeypoints =
      iniFile.read_int(section.c_str(), "SIFTs_numberOfKLTKeypoints", SIFTs_numberOfKLTKeypoints);
  SIFTs_stereo_maxDepth =
      iniFile.read_float(section.c_str(), "SIFTs_stereo_maxDepth", SIFTs_stereo_maxDepth);
  SIFTs_epipolar_TH = iniFile.read_float(section.c_str(), "SIFTs_epipolar_TH", SIFTs_epipolar_TH);
  PLOT_IMAGES = iniFile.read_bool(section.c_str(), "PLOT_IMAGES", PLOT_IMAGES);

  SIFT_feat_options.loadFromConfigFile(iniFile, section);
}

/*---------------------------------------------------------------
          TLikelihoodOptions
  ---------------------------------------------------------------*/
CLandmarksMap::TLikelihoodOptions::TLikelihoodOptions() :
    SIFT_feat_options(vision::featSIFT),

    GPSOrigin()

{
}

CLandmarksMap::TLikelihoodOptions::TGPSOrigin::TGPSOrigin()

    = default;

/*---------------------------------------------------------------
          dumpToTextStream
  ---------------------------------------------------------------*/
void CLandmarksMap::TLikelihoodOptions::dumpToTextStream(std::ostream& out) const
{
  out << "\n----------- [CLandmarksMap::TLikelihoodOptions] ------------ "
         "\n\n";

  out << mrpt::format("rangeScan2D_decimation                  = %i\n", rangeScan2D_decimation);
  out << mrpt::format("SIFTs_sigma_euclidean_dist              = %f\n", SIFTs_sigma_euclidean_dist);
  out << mrpt::format(
      "SIFTs_sigma_descriptor_dist             = %f\n", SIFTs_sigma_descriptor_dist);
  out << mrpt::format("SIFTs_mahaDist_std                      = %f\n", SIFTs_mahaDist_std);
  out << mrpt::format("SIFTs_decimation                        = %i\n", SIFTs_decimation);
  out << mrpt::format(
      "SIFTnullCorrespondenceDistance          = %f\n", SIFTnullCorrespondenceDistance);
  out << mrpt::format("beaconRangesStd                         = %f\n", beaconRangesStd);
  out << mrpt::format(
      "beaconRangesUseObservationStd           = %c\n", beaconRangesUseObservationStd ? 'Y' : 'N');
  out << mrpt::format("extRobotPoseStd                         = %f\n", extRobotPoseStd);

  out << mrpt::format("GPSOrigin:LATITUDE                      = %f\n", GPSOrigin.latitude);
  out << mrpt::format("GPSOrigin:LONGITUDE                     = %f\n", GPSOrigin.longitude);
  out << mrpt::format("GPSOrigin:ALTITUDE                      = %f\n", GPSOrigin.altitude);
  out << mrpt::format("GPSOrigin:Rotation_Angle                = %f\n", GPSOrigin.ang);
  out << mrpt::format("GPSOrigin:x_shift                       = %f\n", GPSOrigin.x_shift);
  out << mrpt::format("GPSOrigin:y_shift                       = %f\n", GPSOrigin.y_shift);
  out << mrpt::format("GPSOrigin:min_sat                       = %i\n", GPSOrigin.min_sat);

  out << mrpt::format("GPS_sigma                               = %f (m)\n", GPS_sigma);

  SIFT_feat_options.dumpToTextStream(out);

  out << "\n";
}

/*---------------------------------------------------------------
          loadFromConfigFile
  ---------------------------------------------------------------*/
void CLandmarksMap::TLikelihoodOptions::loadFromConfigFile(
    const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
{
  rangeScan2D_decimation =
      iniFile.read_int(section.c_str(), "rangeScan2D_decimation", rangeScan2D_decimation);
  SIFTs_sigma_euclidean_dist = iniFile.read_double(
      section.c_str(), "SIFTs_sigma_euclidean_dist", SIFTs_sigma_euclidean_dist);
  SIFTs_sigma_descriptor_dist = iniFile.read_double(
      section.c_str(), "SIFTs_sigma_descriptor_dist", SIFTs_sigma_descriptor_dist);
  SIFTs_mahaDist_std =
      iniFile.read_float(section.c_str(), "SIFTs_mahaDist_std", SIFTs_mahaDist_std);
  SIFTs_decimation = iniFile.read_int(section.c_str(), "SIFTs_decimation", SIFTs_decimation);
  SIFTnullCorrespondenceDistance = iniFile.read_float(
      section.c_str(), "SIFTnullCorrespondenceDistance", SIFTnullCorrespondenceDistance);

  GPSOrigin.latitude =
      iniFile.read_double(section.c_str(), "GPSOriginLatitude", GPSOrigin.latitude);
  GPSOrigin.longitude =
      iniFile.read_double(section.c_str(), "GPSOriginLongitude", GPSOrigin.longitude);
  GPSOrigin.altitude =
      iniFile.read_double(section.c_str(), "GPSOriginAltitude", GPSOrigin.altitude);
  GPSOrigin.ang =
      iniFile.read_double(section.c_str(), "GPSOriginAngle", GPSOrigin.ang) * M_PI / 180;
  GPSOrigin.x_shift = iniFile.read_double(section.c_str(), "GPSOriginXshift", GPSOrigin.x_shift);
  GPSOrigin.y_shift = iniFile.read_double(section.c_str(), "GPSOriginYshift", GPSOrigin.y_shift);
  GPSOrigin.min_sat = iniFile.read_int(section.c_str(), "GPSOriginMinSat", GPSOrigin.min_sat);

  GPS_sigma = iniFile.read_float(section.c_str(), "GPSSigma", GPS_sigma);

  beaconRangesStd = iniFile.read_float(section.c_str(), "beaconRangesStd", beaconRangesStd);
  beaconRangesUseObservationStd = iniFile.read_bool(
      section.c_str(), "beaconRangesUseObservationStd", beaconRangesUseObservationStd);

  extRobotPoseStd = iniFile.read_float(section.c_str(), "extRobotPoseStd", extRobotPoseStd);

  SIFT_feat_options.loadFromConfigFile(iniFile, section);
}

/*---------------------------------------------------------------
          TFuseOptions
  ---------------------------------------------------------------*/
CLandmarksMap::TFuseOptions::TFuseOptions()

    = default;

/*---------------------------------------------------------------
           isEmpty
  ---------------------------------------------------------------*/
bool CLandmarksMap::isEmpty() const { return size() == 0; }
/*---------------------------------------------------------------
           simulateBeaconReadings
  ---------------------------------------------------------------*/
void CLandmarksMap::simulateBeaconReadings(
    const CPose3D& in_robotPose,
    const CPoint3D& in_sensorLocationOnRobot,
    mrpt::obs::CObservationBeaconRanges& out_Observations) const
{
  TSequenceLandmarks::const_iterator it;
  mrpt::obs::CObservationBeaconRanges::TMeasurement newMeas;
  CPoint3D point3D, beacon3D;
  CPointPDFGaussian beaconPDF;

  // Compute the 3D position of the sensor:
  point3D = in_robotPose + in_sensorLocationOnRobot;

  // Clear output data:
  out_Observations.sensedData.clear();
  out_Observations.timestamp = mrpt::Clock::now();

  // For each BEACON landmark in the map:
  for (it = landmarks.begin(); it != landmarks.end(); it++)
  {
    if (it->getType() == featBeacon)
    {
      // Get the 3D position of the beacon (just the mean value):
      it->getPose(beaconPDF);
      beacon3D = beaconPDF.mean;

      float range = point3D.distanceTo(beacon3D);

      // Add noise:
      range += out_Observations.stdError * getRandomGenerator().drawGaussian1D_normalized();
      range = max(0.0f, range);

      if (range >= out_Observations.minSensorDistance &&
          range <= out_Observations.maxSensorDistance)
      {
        // Fill out:
        newMeas.beaconID = it->ID;
        newMeas.sensorLocationOnRobot = in_sensorLocationOnRobot;
        newMeas.sensedDistance = range;

        // Insert:
        out_Observations.sensedData.push_back(newMeas);
      }
    }  // end if beacon
  }    // end for it
       // Done!
}

/*---------------------------------------------------------------
           saveMetricMapRepresentationToFile
  ---------------------------------------------------------------*/
void CLandmarksMap::saveMetricMapRepresentationToFile(const std::string& filNamePrefix) const
{
  MRPT_START

  // Matlab:
  std::string fil1(filNamePrefix + std::string("_3D.m"));
  saveToMATLABScript3D(fil1);

  // 3D Scene:
  opengl::Scene scene;

  opengl::CGridPlaneXY::Ptr objGround =
      std::make_shared<opengl::CGridPlaneXY>(-100, 100, -100, 100, 0, 1);

  scene.insert(getVisualization());
  scene.insert(objGround);

  std::string fil2(filNamePrefix + std::string("_3D.3Dscene"));
  scene.saveToFile(fil2);

  MRPT_END
}

/*---------------------------------------------------------------
            getAs3DObject
  ---------------------------------------------------------------*/
void CLandmarksMap::getVisualizationInto(mrpt::opengl::CSetOfObjects& o) const
{
  if (!genericMapParams.enableSaveAs3DObject) return;

  // TODO: Generate patchs in 3D, etc...

  // Save 3D ellipsoids
  CPointPDFGaussian pointGauss;
  for (const auto& landmark : landmarks)
  {
    opengl::CEllipsoid3D::Ptr ellip = std::make_shared<opengl::CEllipsoid3D>();

    landmark.getPose(pointGauss);

    ellip->setPose(pointGauss.mean);
    ellip->setCovMatrix(pointGauss.cov);
    ellip->enableDrawSolid3D(false);
    ellip->setQuantiles(3.0);
    ellip->set3DsegmentsCount(10);
    ellip->setColor(0, 0, 1);
    ellip->setName(mrpt::format("LM.ID=%u", static_cast<unsigned int>(landmark.ID)));
    ellip->enableShowName(true);

    o.insert(ellip);
  }
}
/**** FAMD ****/
mrpt::maps::CLandmark::TLandmarkID CLandmarksMap::getMapMaxID() { return _mapMaxID; }
/**** END FAMD ****/

const CLandmark* CLandmarksMap::TCustomSequenceLandmarks::getByID(CLandmark::TLandmarkID ID) const
{
  for (const auto& m_landmark : m_landmarks)
  {
    if (m_landmark.ID == ID) return &m_landmark;
  }
  return nullptr;
}

// CLandmark* 	CLandmarksMap::TCustomSequenceLandmarks::getByID(
// CLandmark::TLandmarkID ID )
//{
//	for(size_t indx = 0; indx < m_landmarks.size(); indx++)
//	{
//		if( m_landmarks[indx].ID == ID )
//			return &m_landmarks[indx];
//	}
//	return nullptr;
//}

const CLandmark* CLandmarksMap::TCustomSequenceLandmarks::getByBeaconID(unsigned int ID) const
{
  for (const auto& m_landmark : m_landmarks)
  {
    if (m_landmark.ID == ID) return &m_landmark;
  }
  return nullptr;
}

/*---------------------------------------------------------------
   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.
 * \param  otherMap					  [IN] The other map to compute the matching
 with.
 * \param  otherMapPose				  [IN] The 6D pose of the other map as seen
 from "this".
 * \param  maxDistForCorr			  [IN] The minimum distance between 2
 non-probabilistic map elements for counting them as a correspondence.
 * \param  maxMahaDistForCorr		  [IN] The minimum Mahalanobis distance
 between 2 probabilistic map elements for counting them as a correspondence.
 *
 * \return The matching ratio [0,1]
 * \sa computeMatchingWith2D
 ----------------------------------------------------------------*/
float CLandmarksMap::compute3DMatchingRatio(
    const mrpt::maps::CMetricMap* otherMap2,
    const mrpt::poses::CPose3D& otherMapPose,
    const TMatchingRatioParams& params) const
{
  MRPT_START

  // Compare to a similar map only:
  const CLandmarksMap* otherMap = nullptr;

  if (otherMap2->GetRuntimeClass() == CLASS_ID(CLandmarksMap))
    otherMap = dynamic_cast<const CLandmarksMap*>(otherMap2);

  if (!otherMap) return 0;

  TCustomSequenceLandmarks::const_iterator itThis, itOther;
  std::deque<CPointPDFGaussian::Ptr> poses3DThis, poses3DOther;
  std::deque<CPointPDFGaussian::Ptr>::iterator itPoseThis, itPoseOther;
  CPointPDFGaussian tempPose;
  size_t nThis = landmarks.size();
  size_t nOther = otherMap->landmarks.size();
  size_t otherLandmarkWithCorrespondence = 0;

  // Checks:
  if (!nThis) return 0;
  if (!nOther) return 0;

  // The transformation:
  CMatrixDouble44 pose3DMatrix;
  otherMapPose.getHomogeneousMatrix(pose3DMatrix);
  float Tx = pose3DMatrix(0, 3);
  float Ty = pose3DMatrix(1, 3);
  float Tz = pose3DMatrix(2, 3);

  // ---------------------------------------------------------------------------------------------------------------
  // Is there any "contact" between the spheres that contain all the points
  // from each map after translating them??
  // (Note that we can avoid computing the rotation of all the points if this
  // test fail, with a great speed up!)
  // ---------------------------------------------------------------------------------------------------------------
  if (sqrt(square(Tx) + square(Ty) + square(Tz)) >=
      (landmarks.getLargestDistanceFromOrigin() +
       otherMap->landmarks.getLargestDistanceFromOrigin() + 1.0f))
    return 0;  // There is no contact!

  // Prepare:
  poses3DOther.resize(nOther);
  for (size_t i = 0; i < nOther; i++) poses3DOther[i] = std::make_shared<CPointPDFGaussian>();

  poses3DThis.resize(nThis);
  for (size_t i = 0; i < nThis; i++) poses3DThis[i] = std::make_shared<CPointPDFGaussian>();

  // Save 3D poses of the other map with transformed coordinates:
  for (itOther = otherMap->landmarks.begin(), itPoseOther = poses3DOther.begin();
       itOther != otherMap->landmarks.end(); itOther++, itPoseOther++)
  {
    itOther->getPose(**itPoseOther);
    (*itPoseOther)->changeCoordinatesReference(otherMapPose);
  }

  // And the 3D poses of "this" map:
  for (itThis = landmarks.begin(), itPoseThis = poses3DThis.begin(); itThis != landmarks.end();
       itThis++, itPoseThis++)
  {
    itThis->getPose(**itPoseThis);
  }

  // Check whether each "other"'s LM has a correspondence or not:
  for (itOther = otherMap->landmarks.begin(), itPoseOther = poses3DOther.begin();
       itOther != otherMap->landmarks.end(); itOther++, itPoseOther++)
  {
    for (itThis = landmarks.begin(), itPoseThis = poses3DThis.begin(); itThis != landmarks.end();
         itThis++, itPoseThis++)
    {
      CMatrixDouble COV = CMatrixDouble((*itPoseThis)->cov + (*itPoseOther)->cov);
      TPoint3D D(
          (*itPoseThis)->mean.x() - (*itPoseOther)->mean.x(),
          (*itPoseThis)->mean.y() - (*itPoseOther)->mean.y(),
          (*itPoseThis)->mean.z() - (*itPoseOther)->mean.z());
      CMatrixDouble d(1, 3);
      d(0, 0) = D.x;
      d(0, 1) = D.y;
      d(0, 2) = D.z;

      float distMaha = sqrt(mrpt::math::multiply_HCHt_scalar(d, COV.inverse_LLt()));

      if (distMaha < params.maxMahaDistForCorr)
      {
        // Now test the SIFT descriptors:
        if (!itThis->features.empty() && !itOther->features.empty() &&
            itThis->features[0].descriptors.SIFT->size() ==
                itOther->features[0].descriptors.SIFT->size())
        {
          unsigned long descrDist = 0;
          std::vector<unsigned char>::const_iterator it1, it2;
          for (it1 = itThis->features[0].descriptors.SIFT->begin(),
              it2 = itOther->features[0].descriptors.SIFT->begin();
               it1 != itThis->features[0].descriptors.SIFT->end(); it1++, it2++)
            descrDist += square(*it1 - *it2);

          float descrDist_f = sqrt(d2f(descrDist)) / itThis->features[0].descriptors.SIFT->size();

          if (descrDist_f < 1.5f)
          {
            otherLandmarkWithCorrespondence++;
            break;  // Go to the next "other" LM
          }
        }
      }
    }  // for each in "this"

  }  // for each in "other"

  return d2f(otherLandmarkWithCorrespondence) / nOther;

  MRPT_END
}

/*---------------------------------------------------------------
          auxParticleFilterCleanUp
 ---------------------------------------------------------------*/
void CLandmarksMap::auxParticleFilterCleanUp()
{
  _mEDD.clear();
  _maxIDUpdated = false;
}

/*---------------------------------------------------------------
          simulateRangeBearingReadings
 ---------------------------------------------------------------*/
void CLandmarksMap::simulateRangeBearingReadings(
    const CPose3D& in_robotPose,
    const CPose3D& in_sensorLocationOnRobot,
    CObservationBearingRange& out_Observations,
    bool sensorDetectsIDs,
    const float in_stdRange,
    const float in_stdYaw,
    const float in_stdPitch,
    std::vector<size_t>* out_real_associations,
    const double spurious_count_mean,
    const double spurious_count_std) const
{
  TSequenceLandmarks::const_iterator it;
  size_t idx;
  mrpt::obs::CObservationBearingRange::TMeasurement newMeas;
  CPoint3D beacon3D;
  CPointPDFGaussian beaconPDF;

  if (out_real_associations) out_real_associations->clear();

  // Compute the 3D position of the sensor:
  const CPose3D point3D = in_robotPose + CPose3D(in_sensorLocationOnRobot);

  // Clear output data:
  out_Observations.validCovariances = false;
  out_Observations.sensor_std_range = in_stdRange;
  out_Observations.sensor_std_yaw = in_stdYaw;
  out_Observations.sensor_std_pitch = in_stdPitch;

  out_Observations.sensedData.clear();
  out_Observations.timestamp = mrpt::Clock::now();
  out_Observations.sensorLocationOnRobot = in_sensorLocationOnRobot;

  // For each BEACON landmark in the map:
  // ------------------------------------------
  for (idx = 0, it = landmarks.begin(); it != landmarks.end(); ++it, ++idx)
  {
    // Get the 3D position of the beacon (just the mean value):
    it->getPose(beaconPDF);
    beacon3D = CPoint3D(beaconPDF.mean);

    // Compute yaw,pitch,range:
    double range, yaw, pitch;
    point3D.sphericalCoordinates(beacon3D.asTPoint(), range, yaw, pitch);

    // Add noises:
    range += in_stdRange * getRandomGenerator().drawGaussian1D_normalized();
    yaw += in_stdYaw * getRandomGenerator().drawGaussian1D_normalized();
    pitch += in_stdPitch * getRandomGenerator().drawGaussian1D_normalized();

    yaw = math::wrapToPi(yaw);
    range = max(0.0, range);

    if (range >= out_Observations.minSensorDistance &&
        range <= out_Observations.maxSensorDistance &&
        fabs(yaw) <= 0.5f * out_Observations.fieldOfView_yaw &&
        fabs(pitch) <= 0.5f * out_Observations.fieldOfView_pitch)
    {
      // Fill out:
      if (sensorDetectsIDs)
        newMeas.landmarkID = it->ID;
      else
        newMeas.landmarkID = INVALID_LANDMARK_ID;
      newMeas.range = range;
      newMeas.yaw = yaw;
      newMeas.pitch = pitch;

      // Insert:
      out_Observations.sensedData.push_back(newMeas);

      if (out_real_associations) out_real_associations->push_back(idx);  // Real indices.
    }
  }  // end for it

  const double fSpurious =
      getRandomGenerator().drawGaussian1D(spurious_count_mean, spurious_count_std);
  size_t nSpurious = 0;
  if (spurious_count_std != 0 || spurious_count_mean != 0)
    nSpurious = static_cast<size_t>(mrpt::round_long(std::max(0.0, fSpurious)));

  // For each spurios reading to generate:
  // ------------------------------------------
  for (size_t i = 0; i < nSpurious; i++)
  {
    // Make up yaw,pitch,range out from thin air:
    // (the conditionals on yaw & pitch are to generate 2D observations if
    // we are in 2D, which we learn from a null std.dev.)
    const double range = getRandomGenerator().drawUniform(
        out_Observations.minSensorDistance, out_Observations.maxSensorDistance);
    const double yaw = (out_Observations.sensor_std_yaw == 0)
                           ? 0
                           : getRandomGenerator().drawUniform(
                                 -0.5f * out_Observations.fieldOfView_yaw,
                                 0.5f * out_Observations.fieldOfView_yaw);
    const double pitch = (out_Observations.sensor_std_pitch == 0)
                             ? 0
                             : getRandomGenerator().drawUniform(
                                   -0.5f * out_Observations.fieldOfView_pitch,
                                   0.5f * out_Observations.fieldOfView_pitch);

    // Fill out:
    newMeas.landmarkID = INVALID_LANDMARK_ID;  // Always invalid ID since it's spurious
    newMeas.range = range;
    newMeas.yaw = yaw;
    newMeas.pitch = pitch;

    // Insert:
    out_Observations.sensedData.push_back(newMeas);

    if (out_real_associations)
      out_real_associations->push_back(std::string::npos);  // Real index: spurious
  }                                                         // end for it

  // Done!
}
