text: /* +------------------------------------------------------------------------+
     |                     Mobile Robot Programming Toolkit (MRPT)            |
     |                          https://www.mrpt.org/                         |
     |                                                                        |
     | Copyright (c) 2005-2021, 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 "slam-precomp.h"  // Precompiled headers
  //
  #include <mrpt/math/TPose2D.h>
  #include <mrpt/math/utils.h>
  #include <mrpt/math/wrap2pi.h>
  #include <mrpt/obs/CActionRobotMovement3D.h>
  #include <mrpt/opengl/CEllipsoid2D.h>
  #include <mrpt/opengl/CEllipsoid3D.h>
  #include <mrpt/opengl/CSetOfObjects.h>
  #include <mrpt/opengl/stock_objects.h>
  #include <mrpt/poses/CPosePDF.h>
  #include <mrpt/poses/CPosePDFGaussian.h>
  #include <mrpt/slam/CRangeBearingKFSLAM2D.h>
  #include <mrpt/slam/data_association.h>
  #include <mrpt/system/CTicTac.h>
  #include <mrpt/system/os.h>
  
  using namespace mrpt::slam;
  using namespace mrpt::maps;
  using namespace mrpt::math;
  using namespace mrpt::obs;
  using namespace mrpt::poses;
  using namespace mrpt::system;
  using namespace mrpt;
  using namespace std;
  
  #define STATS_EXPERIMENT 0
  #define STATS_EXPERIMENT_ALSO_NC 1
  
  /*---------------------------------------------------------------
  							Constructor
    ---------------------------------------------------------------*/
  CRangeBearingKFSLAM2D::CRangeBearingKFSLAM2D()
  	: options(), m_action(), m_SF(), m_IDs(), m_SFs()
  {
  	reset();
  }
  
  /*---------------------------------------------------------------
  							reset
    ---------------------------------------------------------------*/
  void CRangeBearingKFSLAM2D::reset()
  {
  	m_action.reset();
  	m_SF.reset();
  	m_IDs.clear();
  	m_SFs.clear();
  
  	// INIT KF STATE
  	m_xkk.assign(3, 0);	 // State: 3D pose (x,y,phi)
  
  	// Initial cov:
  	m_pkk.setSize(3, 3);
  	m_pkk.setZero();
  }
  
  /*---------------------------------------------------------------
  							Destructor
    ---------------------------------------------------------------*/
  CRangeBearingKFSLAM2D::~CRangeBearingKFSLAM2D() = default;
  /*---------------------------------------------------------------
  							getCurrentRobotPose
    ---------------------------------------------------------------*/
  void CRangeBearingKFSLAM2D::getCurrentRobotPose(
  	CPosePDFGaussian& out_robotPose) const
  {
  	MRPT_START
  
  	ASSERT_(m_xkk.size() >= 3);
  
  	// Set 6D pose mean:
  	out_robotPose.mean = CPose2D(m_xkk[0], m_xkk[1], m_xkk[2]);
  
  	// and cov:
  	out_robotPose.cov = m_pkk.blockCopy<3, 3>(0, 0);
  
  	MRPT_END
  }
  
  /*---------------------------------------------------------------
  							getCurrentState
    ---------------------------------------------------------------*/
  void CRangeBearingKFSLAM2D::getCurrentState(
  	CPosePDFGaussian& out_robotPose,
  	std::vector<TPoint2D>& out_landmarksPositions,
  	std::map<unsigned int, CLandmark::TLandmarkID>& out_landmarkIDs,
  	CVectorDouble& out_fullState, CMatrixDouble& out_fullCovariance) const
  {
  	MRPT_START
  
  	ASSERT_(m_xkk.size() >= 3);
  
  	// Set 6D pose mean:
  	out_robotPose.mean = CPose2D(m_xkk[0], m_xkk[1], m_xkk[2]);
  
  	// and cov:
  	out_robotPose.cov = m_pkk.blockCopy<3, 3>(0, 0);
  
  	// Landmarks:
  	ASSERT_(((m_xkk.size() - 3) % 2) == 0);
  	size_t i, nLMs = (m_xkk.size() - 3) / 2;
  	out_landmarksPositions.resize(nLMs);
  	for (i = 0; i < nLMs; i++)
  	{
  		out_landmarksPositions[i].x = m_xkk[3 + i * 2 + 0];
  		out_landmarksPositions[i].y = m_xkk[3 + i * 2 + 1];
  	}  // end for i
  
  	// IDs:
  	out_landmarkIDs = m_IDs.getInverseMap();  // m_IDs_inverse;
  
  	// Full state:
  	out_fullState.resize(m_xkk.size());
  	std::copy(m_xkk.begin(), m_xkk.end(), out_fullState.begin());
  
  	// Full cov:
  	out_fullCovariance = m_pkk;
  
  	MRPT_END
  }
  
  /*---------------------------------------------------------------
  						processActionObservation
    ---------------------------------------------------------------*/
  void CRangeBearingKFSLAM2D::processActionObservation(
  	CActionCollection::Ptr& action, CSensoryFrame::Ptr& SF)
  {
  	MRPT_START
  
  	m_action = action;
  	m_SF = SF;
  
  	// Sanity check:
  	ASSERT_(m_IDs.size() == this->getNumberOfLandmarksInTheMap());
  
  	// ===================================================================================================================
  	// Here's the meat!: Call the main method for the KF algorithm, which will
  	// call all the callback methods as required:
  	// ===================================================================================================================
  	runOneKalmanIteration();
  
  	// =============================================================
  	//  ADD TO SFs SEQUENCE
  	// =============================================================
  	if (options.create_simplemap)
  	{
  		CPosePDFGaussian::Ptr auxPosePDF = std::make_shared<CPosePDFGaussian>();
  		getCurrentRobotPose(*auxPosePDF);
  		m_SFs.insert(auxPosePDF, SF);
  	}
  
  	MRPT_END
  }
  
  /** Must return the action vector u.
   * \param out_u The action vector which will be passed to OnTransitionModel
   */
  void CRangeBearingKFSLAM2D::OnGetAction(KFArray_ACT& u) const
  {
  	// Get odometry estimation:
  	CActionRobotMovement2D::Ptr actMov2D =
  		m_action->getBestMovementEstimation();
  	CActionRobotMovement3D::Ptr actMov3D =
  		m_action->getActionByClass<CActionRobotMovement3D>();
  	if (actMov3D)
  	{
  		u[0] = actMov3D->poseChange.mean.x();
  		u[1] = actMov3D->poseChange.mean.y();
  		u[2] = actMov3D->poseChange.mean.yaw();
  	}
  	else if (actMov2D)
  	{
  		CPose2D estMovMean;
  		actMov2D->poseChange->getMean(estMovMean);
  		u[0] = estMovMean.x();
  		u[1] = estMovMean.y();
  		u[2] = estMovMean.phi();
  	}
  	else
  	{
  		// Left u as zeros
  		for (size_t i = 0; i < 3; i++)
  			u[i] = 0;
  	}
  }
  
  /** This virtual function musts implement the prediction model of the Kalman
   * filter.
   */
  void CRangeBearingKFSLAM2D::OnTransitionModel(
  	const KFArray_ACT& u, KFArray_VEH& xv, bool& out_skipPrediction) const
  {
  	MRPT_START
  
  	// Do not update the vehicle pose & its covariance until we have some
  	// landmakrs in the map,
  	// otherwise, we are imposing a lower bound to the best uncertainty from now
  	// on:
  	if (size_t(m_xkk.size()) == get_vehicle_size())
  	{
  		out_skipPrediction = true;
  		return;
  	}
  
  	CPose2D robotPose(xv[0], xv[1], xv[2]);
  	CPose2D odoIncrement(u[0], u[1], u[2]);
  
  	// Pose composition:
  	robotPose = robotPose + odoIncrement;
  
  	xv[0] = robotPose.x();
  	xv[1] = robotPose.y();
  	xv[2] = robotPose.phi();
  
  	MRPT_END
  }
  
  /** This virtual function musts calculate the Jacobian F of the prediction
   * model.
   */
  void CRangeBearingKFSLAM2D::OnTransitionJacobian(KFMatrix_VxV& F) const
  {
  	MRPT_START
  
  	// The jacobian of the transition function:  dfv_dxv
  	CActionRobotMovement2D::Ptr act2D = m_action->getBestMovementEstimation();
  	CActionRobotMovement3D::Ptr act3D =
  		m_action->getActionByClass<CActionRobotMovement3D>();
  
  	if (act3D && act2D)
  		THROW_EXCEPTION("Both 2D & 3D odometry are present!?!?");
  
  	TPoint2D Ap;
  
  	if (act3D) { Ap = TPoint2D(CPose2D(act3D->poseChange.mean).asTPose()); }
  	else if (act2D)
  	{
  		Ap = TPoint2D(act2D->poseChange->getMeanVal().asTPose());
  	}
  	else
  	{
  		// No odometry at all:
  		F.setIdentity();  // Unit diagonal
  		return;
  	}
  
  	const kftype cy = cos(m_xkk[2]);
  	const kftype sy = sin(m_xkk[2]);
  
  	const kftype Ax = Ap.x;
  	const kftype Ay = Ap.y;
  
  	F.setIdentity();  // Unit diagonal
  
  	F(0, 2) = -Ax * sy - Ay * cy;
  	F(1, 2) = Ax * cy - Ay * sy;
  
  	MRPT_END
  }
  
  /** This virtual function musts calculate de noise matrix of the prediction
   * model.
   */
  void CRangeBearingKFSLAM2D::OnTransitionNoise(KFMatrix_VxV& Q) const
  {
  	MRPT_START
  
  	// The uncertainty of the 2D odometry, projected from the current position:
  	CActionRobotMovement2D::Ptr act2D = m_action->getBestMovementEstimation();
  	CActionRobotMovement3D::Ptr act3D =
  		m_action->getActionByClass<CActionRobotMovement3D>();
  
  	if (act3D && act2D)
  		THROW_EXCEPTION("Both 2D & 3D odometry are present!?!?");
  
  	CPosePDFGaussian odoIncr;
  
  	if (!act3D && !act2D)
  	{
  		// Use constant Q:
  		Q.setZero();
  		ASSERT_(int(options.stds_Q_no_odo.size()) == Q.cols());
  		for (size_t i = 0; i < 3; i++)
  			Q(i, i) = square(options.stds_Q_no_odo[i]);
  		return;
  	}
  	else
  	{
  		if (act2D)
  		{
  			// 2D odometry:
  			odoIncr = CPosePDFGaussian(*act2D->poseChange);
  		}
  		else
  		{
  			// 3D odometry:
  			odoIncr = CPosePDFGaussian(act3D->poseChange);
  		}
  	}
  
  	odoIncr.rotateCov(m_xkk[2]);
  
  	Q = odoIncr.cov;
  
  	MRPT_END
  }
  
  void CRangeBearingKFSLAM2D::OnObservationModel(
  	const std::vector<size_t>& idx_landmarks_to_predict,
  	vector_KFArray_OBS& out_predictions) const
  {
  	MRPT_START
  
  	// Get the sensor pose relative to the robot:
  	CObservationBearingRange::Ptr obs =
  		m_SF->getObservationByClass<CObservationBearingRange>();
  	ASSERTMSG_(
  		obs,
  		"*ERROR*: This method requires an observation of type "
  		"CObservationBearingRange");
  	const CPose2D sensorPoseOnRobot = CPose2D(obs->sensorLocationOnRobot);
  
  	/* -------------------------------------------
  	   Equations, obtained using matlab, of the relative 2D position of a
  	  landmark (xi,yi), relative
  		  to a robot 2D pose (x0,y0,phi)
  		Refer to technical report "6D EKF derivation...", 2008
  
  		x0 y0 phi0         % Robot's 2D pose
  		x0s y0s phis      % Sensor's 2D pose relative to robot
  		xi yi             % Absolute 2D landmark coordinates:
  
  		Hx : dh_dxv   -> Jacobian of the observation model wrt the robot pose
  		Hy : dh_dyi   -> Jacobian of the observation model wrt each landmark
  	  mean position
  
  		Sizes:
  		 h:  1x2
  		 Hx: 2x3
  		 Hy: 2x2
  	  ------------------------------------------- */
  	// Mean of the prior of the robot pose:
  	const CPose2D robotPose(m_xkk[0], m_xkk[1], m_xkk[2]);
  
  	const size_t vehicle_size = get_vehicle_size();
  	const size_t feature_size = get_feature_size();
  
  	const CPose2D sensorPoseAbs = robotPose + sensorPoseOnRobot;
  
  	// ---------------------------------------------------
  	// Observation prediction
  	// ---------------------------------------------------
  	const size_t N = idx_landmarks_to_predict.size();
  	out_predictions.resize(N);
  	for (size_t i = 0; i < N; i++)
  	{
  		const size_t idx_lm = idx_landmarks_to_predict[i];
  		ASSERTDEB_(idx_lm < this->getNumberOfLandmarksInTheMap());
  
  		// Landmark absolute position in the map:
  		const kftype xi = m_xkk[vehicle_size + feature_size * idx_lm + 0];
  		const kftype yi = m_xkk[vehicle_size + feature_size * idx_lm + 1];
  
  		const double Axi = xi - sensorPoseAbs.x();
  		const double Ayi = yi - sensorPoseAbs.y();
  
  		out_predictions[i][0] = sqrt(square(Axi) + square(Ayi));  // Range
  		out_predictions[i][1] =
  			mrpt::math::wrapToPi(atan2(Ayi, Axi) - sensorPoseAbs.phi());  // Yaw
  	}
  
  	MRPT_END
  }
  
  void CRangeBearingKFSLAM2D::OnObservationJacobians(
  	size_t idx_landmark_to_predict, KFMatrix_OxV& Hx, KFMatrix_OxF& Hy) const
  {
  	MRPT_START
  
  	// Get the sensor pose relative to the robot:
  	CObservationBearingRange::Ptr obs =
  		m_SF->getObservationByClass<CObservationBearingRange>();
  	ASSERTMSG_(
  		obs,
  		"*ERROR*: This method requires an observation of type "
  		"CObservationBearingRange");
  	const CPose2D sensorPoseOnRobot = CPose2D(obs->sensorLocationOnRobot);
  
  	/* -------------------------------------------
  	   Equations, obtained using matlab, of the relative 2D position of a
  	  landmark (xi,yi), relative
  		  to a robot 2D pose (x0,y0,phi)
  		Refer to technical report "6D EKF derivation...", 2008
  
  		x0 y0 phi0         % Robot's 2D pose
  		x0s y0s phis      % Sensor's 2D pose relative to robot
  		xi yi             % Absolute 2D landmark coordinates:
  
  		Hx : dh_dxv   -> Jacobian of the observation model wrt the robot pose
  		Hy : dh_dyi   -> Jacobian of the observation model wrt each landmark
  	  mean position
  
  		Sizes:
  		 h:  1x2
  		 Hx: 2x3
  		 Hy: 2x2
  	  ------------------------------------------- */
  	// Mean of the prior of the robot pose:
  	const CPose2D robotPose(m_xkk[0], m_xkk[1], m_xkk[2]);
  
  	const size_t vehicle_size = get_vehicle_size();
  	const size_t feature_size = get_feature_size();
  
  	// Robot 6D pose:
  	const kftype x0 = m_xkk[0];
  	const kftype y0 = m_xkk[1];
  	const kftype phi0 = m_xkk[2];
  
  	const kftype cphi0 = cos(phi0);
  	const kftype sphi0 = sin(phi0);
  
  	// Sensor 2D pose on robot:
  	const kftype x0s = sensorPoseOnRobot.x();
  	const kftype y0s = sensorPoseOnRobot.y();
  	const kftype phis = sensorPoseOnRobot.phi();
  
  	const kftype cphi0s = cos(phi0 + phis);
  	const kftype sphi0s = sin(phi0 + phis);
  
  	const CPose2D sensorPoseAbs = robotPose + sensorPoseOnRobot;
  
  	// Landmark absolute position in the map:
  	const kftype xi =
  		m_xkk[vehicle_size + feature_size * idx_landmark_to_predict + 0];
  	const kftype yi =
  		m_xkk[vehicle_size + feature_size * idx_landmark_to_predict + 1];
  
  	// ---------------------------------------------------
  	// Generate dhi_dxv: A 2x3 block
  	// ---------------------------------------------------
  	const kftype EXP1 = -2 * yi * y0s * cphi0 - 2 * yi * y0 +
  		2 * xi * y0s * sphi0 - 2 * xi * x0 - 2 * xi * x0s * cphi0 -
  		2 * yi * x0s * sphi0 + 2 * y0s * y0 * cphi0 - 2 * y0s * x0 * sphi0 +
  		2 * y0 * x0s * sphi0 + square(x0) + 2 * x0s * x0 * cphi0 + square(x0s) +
  		square(y0s) + square(xi) + square(yi) + square(y0);
  	const kftype sqrtEXP1_1 = kftype(1) / sqrt(EXP1);
  
  	const kftype EXP2 = cphi0s * xi + sphi0s * yi - sin(phis) * y0s -
  		y0 * sphi0s - x0s * cos(phis) - x0 * cphi0s;
  	const kftype EXP2sq = square(EXP2);
  
  	const kftype EXP3 = -sphi0s * xi + cphi0s * yi - cos(phis) * y0s -
  		y0 * cphi0s + x0s * sin(phis) + x0 * sphi0s;
  	const kftype EXP3sq = square(EXP3);
  
  	const kftype EXP4 = kftype(1) / (1 + EXP3sq / EXP2sq);
  
  	Hx(0, 0) = (-xi - sphi0 * y0s + cphi0 * x0s + x0) * sqrtEXP1_1;
  	Hx(0, 1) = (-yi + cphi0 * y0s + y0 + sphi0 * x0s) * sqrtEXP1_1;
  	Hx(0, 2) = (y0s * xi * cphi0 + y0s * yi * sphi0 - y0 * y0s * sphi0 -
  				x0 * y0s * cphi0 + x0s * xi * sphi0 - x0s * yi * cphi0 +
  				y0 * x0s * cphi0 - x0s * x0 * sphi0) *
  		sqrtEXP1_1;
  
  	Hx(1, 0) = (sphi0s / (EXP2) + (EXP3) / EXP2sq * cphi0s) * EXP4;
  	Hx(1, 1) = (-cphi0s / (EXP2) + (EXP3) / EXP2sq * sphi0s) * EXP4;
  	Hx(1, 2) =
  		((-cphi0s * xi - sphi0s * yi + y0 * sphi0s + x0 * cphi0s) / (EXP2) -
  		 (EXP3) / EXP2sq *
  			 (-sphi0s * xi + cphi0s * yi - y0 * cphi0s + x0 * sphi0s)) *
  		EXP4;
  
  	// ---------------------------------------------------
  	// Generate dhi_dyi: A 2x2 block
  	// ---------------------------------------------------
  	Hy(0, 0) = (xi + sphi0 * y0s - cphi0 * x0s - x0) * sqrtEXP1_1;
  	Hy(0, 1) = (yi - cphi0 * y0s - y0 - sphi0 * x0s) * sqrtEXP1_1;
  
  	Hy(1, 0) = (-sphi0s / (EXP2) - (EXP3) / EXP2sq * cphi0s) * EXP4;
  	Hy(1, 1) = (cphi0s / (EXP2) - (EXP3) / EXP2sq * sphi0s) * EXP4;
  
  	MRPT_END
  }
  
  /** This is called between the KF prediction step and the update step, and the
   * application must return the observations and, when applicable, the data
   * association between these observations and the current map.
   *
   * \param out_z N vectors, each for one "observation" of length OBS_SIZE, N
   * being the number of "observations": how many observed landmarks for a map, or
   * just one if not applicable.
   * \param out_data_association An empty vector or, where applicable, a vector
   * where the i'th element corresponds to the position of the observation in the
   * i'th row of out_z within the system state vector (in the range
   * [0,getNumberOfLandmarksInTheMap()-1]), or -1 if it is a new map element and
   * we want to insert it at the end of this KF iteration.
   * \param in_S The full covariance matrix of the observation predictions (i.e.
   * the "innovation covariance matrix"). This is a M·O x M·O matrix with M=length
   * of "in_lm_indices_in_S".
   * \param in_lm_indices_in_S The indices of the map landmarks (range
   * [0,getNumberOfLandmarksInTheMap()-1]) that can be found in the matrix in_S.
   *
   *  This method will be called just once for each complete KF iteration.
   * \note It is assumed that the observations are independent, i.e. there are NO
   * cross-covariances between them.
   */
  void CRangeBearingKFSLAM2D::OnGetObservationsAndDataAssociation(
  	vector_KFArray_OBS& Z, std::vector<int>& data_association,
  	const vector_KFArray_OBS& all_predictions, const KFMatrix& S,
  	const std::vector<size_t>& lm_indices_in_S, const KFMatrix_OxO& R)
  {
  	MRPT_START
  
  	constexpr size_t obs_size = get_observation_size();
  
  	// Z: Observations
  	CObservationBearingRange::TMeasurementList::const_iterator itObs;
  
  	CObservationBearingRange::Ptr obs =
  		m_SF->getObservationByClass<CObservationBearingRange>();
  	ASSERTMSG_(
  		obs,
  		"*ERROR*: This method requires an observation of type "
  		"CObservationBearingRange");
  
  	const size_t N = obs->sensedData.size();
  	Z.resize(N);
  
  	size_t row;
  	for (row = 0, itObs = obs->sensedData.begin();
  		 itObs != obs->sensedData.end(); ++itObs, ++row)
  	{
  		// Fill one row in Z:
  		Z[row][0] = itObs->range;
  		Z[row][1] = itObs->yaw;
  		ASSERTMSG_(
  			itObs->pitch == 0,
  			"ERROR: Observation contains pitch!=0 but this is 2D-SLAM!!!");
  	}
  
  	// Data association:
  	// ---------------------
  	data_association.assign(N, -1);	 // Initially, all new landmarks
  
  	// For each observed LM:
  	std::vector<size_t> obs_idxs_needing_data_assoc;
  	obs_idxs_needing_data_assoc.reserve(N);
  
  	{
  		std::vector<int>::iterator itDA;
  		for (row = 0, itObs = obs->sensedData.begin(),
  			itDA = data_association.begin();
  			 itObs != obs->sensedData.end(); ++itObs, ++itDA, ++row)
  		{
  			// Fill data asociation: Using IDs!
  			if (itObs->landmarkID < 0)
  				obs_idxs_needing_data_assoc.push_back(row);
  			else
  			{
  				mrpt::containers::bimap<
  					CLandmark::TLandmarkID, unsigned int>::iterator itID;
  				if ((itID = m_IDs.find_key(itObs->landmarkID)) != m_IDs.end())
  					*itDA = itID->second;  // This row in Z corresponds to the
  				// i'th map element in the state
  				// vector:
  			}
  		}
  	}
  
  	// ---- Perform data association ----
  	//  Only for observation indices in "obs_idxs_needing_data_assoc"
  	if (obs_idxs_needing_data_assoc.empty())
  	{
  		// We don't need to do DA:
  		m_last_data_association = TDataAssocInfo();
  
  		// Save them for the case the external user wants to access them:
  		for (size_t idxObs = 0; idxObs < data_association.size(); idxObs++)
  		{
  			int da = data_association[idxObs];
  			if (da >= 0)
  				m_last_data_association.results.associations[idxObs] =
  					data_association[idxObs];
  		}
  
  #if STATS_EXPERIMENT  // DEBUG: Generate statistic info
  		{
  			static CFileOutputStream fC("metric_stats_C.txt", true);
  #if STATS_EXPERIMENT_ALSO_NC
  			static CFileOutputStream fNC("metric_stats_NC.txt", true);
  #endif
  
  			std::vector<int>::iterator itDA;
  			size_t idx_obs = 0;
  			for (itDA = data_association.begin();
  				 itDA != data_association.end(); ++itDA, ++idx_obs)
  			{
  				if (*itDA == -1) continue;
  				const size_t lm_idx_in_map = *itDA;
  				size_t valid_idx_pred =
  					find_in_vector(lm_idx_in_map, lm_indices_in_S);
  
  				const KFArray_OBS& obs_mu = Z[idx_obs];
  
  				for (size_t idx_pred = 0; idx_pred < lm_indices_in_S.size();
  					 idx_pred++)
  				{
  					const KFArray_OBS& lm_mu =
  						all_predictions[lm_indices_in_S[idx_pred]];
  
  					const size_t base_idx_in_S = obs_size * idx_pred;
  					KFMatrix_OxO lm_cov;
  					S.extractMatrix(base_idx_in_S, base_idx_in_S, lm_cov);
  
  					double md, log_pdf;
  					mahalanobisDistance2AndLogPDF(
  						lm_mu - obs_mu, lm_cov, md, log_pdf);
  
  					if (valid_idx_pred == idx_pred)
  						fC.printf("%e %e\n", md, log_pdf);
  					else
  					{
  #if STATS_EXPERIMENT_ALSO_NC
  						fNC.printf("%e %e\n", md, log_pdf);
  #endif
  					}
  				}
  			}
  		}
  #endif
  	}
  	else
  	{
  		// Build a Z matrix with the observations that need dat.assoc:
  		const size_t nObsDA = obs_idxs_needing_data_assoc.size();
  
  		CMatrixDynamic<kftype> Z_obs_means(nObsDA, obs_size);
  		for (size_t i = 0; i < nObsDA; i++)
  		{
  			const size_t idx = obs_idxs_needing_data_assoc[i];
  			for (unsigned k = 0; k < obs_size; k++)
  				Z_obs_means(i, k) = Z[idx][k];
  		}
  
  		// Vehicle uncertainty
  		/*const KFMatrix_VxV Pxx = m_pkk.extractMatrix<get_vehicle_size(),
  		 * get_vehicle_size()>(0, 0);*/
  
  		// Build predictions:
  		// ---------------------------
  		const size_t nPredictions = lm_indices_in_S.size();
  		m_last_data_association.clear();
  
  		// S is the covariance of the predictions:
  		m_last_data_association.Y_pred_covs = S;
  
  		// The means:
  		m_last_data_association.Y_pred_means.setSize(nPredictions, obs_size);
  		for (size_t q = 0; q < nPredictions; q++)
  		{
  			const size_t i = lm_indices_in_S[q];
  			for (size_t w = 0; w < obs_size; w++)
  				m_last_data_association.Y_pred_means(q, w) =
  					all_predictions[i][w];
  			m_last_data_association.predictions_IDs.push_back(
  				i);	 // for the conversion of indices...
  		}
  
  		// Do Dat. Assoc :
  		// ---------------------------
  		if (nPredictions)
  		{
  			// CMatrixDouble Z_obs_cov = CMatrixDouble(R);
  
  			mrpt::slam::data_association_full_covariance(
  				Z_obs_means,  // Z_obs_cov,
  				m_last_data_association.Y_pred_means,
  				m_last_data_association.Y_pred_covs,
  				m_last_data_association.results, options.data_assoc_method,
  				options.data_assoc_metric, options.data_assoc_IC_chi2_thres,
  				true,  // Use KD-tree
  				m_last_data_association.predictions_IDs,
  				options.data_assoc_IC_metric,
  				options.data_assoc_IC_ml_threshold);
  
  			// Return pairings to the main KF algorithm:
  			for (auto it = m_last_data_association.results.associations.begin();
  				 it != m_last_data_association.results.associations.end(); ++it)
  				data_association[it->first] = it->second;
  		}
  	}
  	// ---- End of data association ----
  
  	MRPT_END
  }
  
  /** This virtual function musts normalize the state vector and covariance matrix
   * (only if its necessary).
   */
  void CRangeBearingKFSLAM2D::OnNormalizeStateVector()
  {
  	// Check angles:
  	mrpt::math::wrapToPiInPlace(m_xkk[2]);
  }
  
  /*---------------------------------------------------------------
  						loadOptions
    ---------------------------------------------------------------*/
  void CRangeBearingKFSLAM2D::loadOptions(
  	const mrpt::config::CConfigFileBase& ini)
  {
  	// Main
  	options.loadFromConfigFile(ini, "RangeBearingKFSLAM");
  	KF_options.loadFromConfigFile(ini, "RangeBearingKFSLAM_KalmanFilter");
  }
  
  /*---------------------------------------------------------------
  						loadOptions
    ---------------------------------------------------------------*/
  void CRangeBearingKFSLAM2D::TOptions::loadFromConfigFile(
  	const mrpt::config::CConfigFileBase& source, const std::string& section)
  {
  	stds_Q_no_odo[2] = RAD2DEG(stds_Q_no_odo[2]);
  
  	source.read_vector(section, "stds_Q_no_odo", stds_Q_no_odo, stds_Q_no_odo);
  	ASSERT_(stds_Q_no_odo.size() == 3);
  	stds_Q_no_odo[2] = DEG2RAD(stds_Q_no_odo[2]);
  
  	std_sensor_range =
  		source.read_float(section, "std_sensor_range", std_sensor_range);
  	std_sensor_yaw = DEG2RAD(source.read_float(
  		section, "std_sensor_yaw_deg", RAD2DEG(std_sensor_yaw)));
  
  	MRPT_LOAD_CONFIG_VAR(quantiles_3D_representation, float, source, section);
  	MRPT_LOAD_CONFIG_VAR(create_simplemap, bool, source, section);
  
  	data_assoc_method = source.read_enum<TDataAssociationMethod>(
  		section, "data_assoc_method", data_assoc_method);
  	data_assoc_metric = source.read_enum<TDataAssociationMetric>(
  		section, "data_assoc_metric", data_assoc_metric);
  	data_assoc_IC_metric = source.read_enum<TDataAssociationMetric>(
  		section, "data_assoc_IC_metric", data_assoc_IC_metric);
  
  	MRPT_LOAD_CONFIG_VAR(data_assoc_IC_chi2_thres, double, source, section);
  	MRPT_LOAD_CONFIG_VAR(data_assoc_IC_ml_threshold, double, source, section);
  }
  
  /*---------------------------------------------------------------
  						Constructor
    ---------------------------------------------------------------*/
  CRangeBearingKFSLAM2D::TOptions::TOptions()
  	: stds_Q_no_odo(3, 0),
  
  	  std_sensor_yaw(DEG2RAD(0.5f))
  
  {
  	stds_Q_no_odo[0] = 0.10f;
  	stds_Q_no_odo[1] = 0.10f;
  	stds_Q_no_odo[2] = DEG2RAD(4.0f);
  }
  
  /*---------------------------------------------------------------
  						dumpToTextStream
    ---------------------------------------------------------------*/
  void CRangeBearingKFSLAM2D::TOptions::dumpToTextStream(std::ostream& out) const
  {
  	using namespace mrpt::typemeta;
  
  	out << "\n----------- [CRangeBearingKFSLAM2D::TOptions] ------------ \n\n";
  
  	out << mrpt::format(
  		"data_assoc_method                       = %s\n",
  		TEnumType<TDataAssociationMethod>::value2name(data_assoc_method)
  			.c_str());
  	out << mrpt::format(
  		"data_assoc_metric                       = %s\n",
  		TEnumType<TDataAssociationMetric>::value2name(data_assoc_metric)
  			.c_str());
  	out << mrpt::format(
  		"data_assoc_IC_chi2_thres                = %.06f\n",
  		data_assoc_IC_chi2_thres);
  	out << mrpt::format(
  		"data_assoc_IC_metric                    = %s\n",
  		TEnumType<TDataAssociationMetric>::value2name(data_assoc_IC_metric)
  			.c_str());
  	out << mrpt::format(
  		"data_assoc_IC_ml_threshold              = %.06f\n",
  		data_assoc_IC_ml_threshold);
  
  	out << "\n";
  }
  
  void CRangeBearingKFSLAM2D::OnInverseObservationModel(
  	const KFArray_OBS& in_z, KFArray_FEAT& yn, KFMatrix_FxV& dyn_dxv,
  	KFMatrix_FxO& dyn_dhn) const
  {
  	MRPT_START
  
  	/* -------------------------------------------
  	   Equations, obtained using matlab
  
  		x0 y0 phi0      % Robot's 2D pose
  		x0s y0s phis    % Sensor's 2D pose relative to robot
  		hr ha         	% Observation hn: range, yaw
  
  		xi yi           % Absolute 2D landmark coordinates:
  
  		dyn_dxv   -> Jacobian of the inv. observation model wrt the robot pose
  		dyn_dhn   -> Jacobian of the inv. observation model wrt each landmark
  	  observation
  
  		Sizes:
  		 hn:      1x2  <--
  		 yn:      1x2  -->
  		 dyn_dxv: 2x3  -->
  		 dyn_dhn: 2x2  -->
  	  ------------------------------------------- */
  
  	CObservationBearingRange::Ptr obs =
  		m_SF->getObservationByClass<CObservationBearingRange>();
  	ASSERTMSG_(
  		obs,
  		"*ERROR*: This method requires an observation of type "
  		"CObservationBearingRange");
  	const CPose2D sensorPoseOnRobot = CPose2D(obs->sensorLocationOnRobot);
  
  	// Mean of the prior of the robot pose:
  	const TPose2D robotPose(m_xkk[0], m_xkk[1], m_xkk[2]);
  
  	// Robot 2D pose:
  	const kftype x0 = m_xkk[0];
  	const kftype y0 = m_xkk[1];
  	const kftype phi0 = m_xkk[2];
  
  	const kftype cphi0 = cos(phi0);
  	const kftype sphi0 = sin(phi0);
  
  	// Sensor 6D pose on robot:
  	const kftype x0s = sensorPoseOnRobot.x();
  	const kftype y0s = sensorPoseOnRobot.y();
  	const kftype phis = sensorPoseOnRobot.phi();
  
  	const kftype hr = in_z[0];
  	const kftype ha = in_z[1];
  
  	const kftype cphi_0sa = cos(phi0 + phis + ha);
  	const kftype sphi_0sa = sin(phi0 + phis + ha);
  
  	// Compute the mean 2D absolute coordinates:
  	yn[0] = hr * cphi_0sa + cphi0 * x0s - sphi0 * y0s + x0;
  	yn[1] = hr * sphi_0sa + sphi0 * x0s + cphi0 * y0s + y0;
  
  	// Jacobian wrt xv:
  	dyn_dxv(0, 0) = 1;
  	dyn_dxv(0, 1) = 0;
  	dyn_dxv(0, 2) = -hr * sphi_0sa - sphi0 * x0s - cphi0 * y0s;
  
  	dyn_dxv(1, 0) = 0;
  	dyn_dxv(1, 1) = 1;
  	dyn_dxv(1, 2) = hr * cphi_0sa + cphi0 * x0s - sphi0 * y0s;
  
  	// Jacobian wrt hn:
  	dyn_dhn(0, 0) = cphi_0sa;
  	dyn_dhn(0, 1) = -hr * sphi_0sa;
  
  	dyn_dhn(1, 0) = sphi_0sa;
  	dyn_dhn(1, 1) = hr * cphi_0sa;
  
  	MRPT_END
  }
  
  /** If applicable to the given problem, do here any special handling of adding a
   * new landmark to the map.
   * \param in_obsIndex The index of the observation whose inverse sensor is to
   * be computed. It corresponds to the row in in_z where the observation can be
   * found.
   * \param in_idxNewFeat The index that this new feature will have in the state
   * vector (0:just after the vehicle state, 1: after that,...). Save this number
   * so data association can be done according to these indices.
   * \sa OnInverseObservationModel
   */
  void CRangeBearingKFSLAM2D::OnNewLandmarkAddedToMap(
  	const size_t in_obsIdx, const size_t in_idxNewFeat)
  {
  	MRPT_START
  
  	CObservationBearingRange::Ptr obs =
  		m_SF->getObservationByClass<CObservationBearingRange>();
  	ASSERTMSG_(
  		obs,
  		"*ERROR*: This method requires an observation of type "
  		"CObservationBearingRange");
  
  	// ----------------------------------------------
  	// introduce in the lists of ID<->index in map:
  	// ----------------------------------------------
  	ASSERT_(in_obsIdx < obs->sensedData.size());
  	if (obs->sensedData[in_obsIdx].landmarkID >= 0)
  	{
  		// The sensor provides us a LM ID... use it:
  		m_IDs.insert(obs->sensedData[in_obsIdx].landmarkID, in_idxNewFeat);
  	}
  	else
  	{
  		// Features do not have IDs... use indices:
  		m_IDs.insert(in_idxNewFeat, in_idxNewFeat);
  	}
  
  	m_last_data_association.newly_inserted_landmarks[in_obsIdx] =
  		in_idxNewFeat;	// Just for stats, etc...
  
  	MRPT_END
  }
  
  /*---------------------------------------------------------------
  						getAs3DObject
    ---------------------------------------------------------------*/
  void CRangeBearingKFSLAM2D::getAs3DObject(
  	mrpt::opengl::CSetOfObjects::Ptr& outObj) const
  {
  	outObj->clear();
  
  	// ------------------------------------------------
  	//  Add the XYZ corner for the current area:
  	// ------------------------------------------------
  	outObj->insert(opengl::stock_objects::CornerXYZ());
  
  	// 2D ellipsoid for robot pose:
  	CPoint2DPDFGaussian pointGauss;
  	pointGauss.mean.x(m_xkk[0]);
  	pointGauss.mean.y(m_xkk[1]);
  	pointGauss.cov = m_pkk.blockCopy<2, 2>(0, 0);
  
  	{
  		auto ellip = opengl::CEllipsoid2D::Create();
  
  		ellip->setPose(pointGauss.mean);
  		ellip->setCovMatrix(pointGauss.cov);
  		ellip->enableDrawSolid3D(false);
  		ellip->setQuantiles(options.quantiles_3D_representation);
  		ellip->setColor(1, 0, 0);
  
  		outObj->insert(ellip);
  	}
  
  	// 2D ellipsoids for landmarks:
  	const size_t nLMs = (m_xkk.size() - 3) / 2;
  	for (size_t i = 0; i < nLMs; i++)
  	{
  		pointGauss.mean.x(m_xkk[3 + 2 * i + 0]);
  		pointGauss.mean.y(m_xkk[3 + 2 * i + 1]);
  		pointGauss.cov = m_pkk.blockCopy<2, 2>(3 + 2 * i, 3 + 2 * i);
  
  		auto ellip = opengl::CEllipsoid2D::Create();
  
  		ellip->setName(format("%u", static_cast<unsigned int>(i)));
  		ellip->enableShowName(true);
  		ellip->setPose(pointGauss.mean);
  		ellip->setCovMatrix(pointGauss.cov);
  		ellip->enableDrawSolid3D(false);
  		ellip->setQuantiles(options.quantiles_3D_representation);
  
  		ellip->setColor(0, 0, 1);
  
  		outObj->insert(ellip);
  	}
  }
  
  /*---------------------------------------------------------------
  			  saveMapAndPathRepresentationAsMATLABFile
    ---------------------------------------------------------------*/
  void CRangeBearingKFSLAM2D::saveMapAndPath2DRepresentationAsMATLABFile(
  	const string& fil, float stdCount, const string& styleLandmarks,
  	const string& stylePath, const string& styleRobot) const
  {
  	FILE* f = os::fopen(fil.c_str(), "wt");
  	if (!f) return;
  
  	CMatrixDouble cov(2, 2);
  	CVectorDouble mean(2);
  
  	// Header:
  	os::fprintf(
  		f,
  		"%%--------------------------------------------------------------------"
  		"\n");
  	os::fprintf(f, "%% File automatically generated using the MRPT method:\n");
  	os::fprintf(
  		f,
  		"%% "
  		"'CRangeBearingKFSLAM2D::saveMapAndPath2DRepresentationAsMATLABFile'"
  		"\n");
  	os::fprintf(f, "%%\n");
  	os::fprintf(f, "%%                        ~ MRPT ~\n");
  	os::fprintf(
  		f, "%%  Jose Luis Blanco Claraco, University of Malaga @ 2008\n");
  	os::fprintf(f, "%%      https://www.mrpt.org/     \n");
  	os::fprintf(
  		f,
  		"%%--------------------------------------------------------------------"
  		"\n");
  
  	// Main code:
  	os::fprintf(f, "hold on;\n\n");
  
  	size_t i, nLMs = (m_xkk.size() - get_vehicle_size()) / get_feature_size();
  
  	for (i = 0; i < nLMs; i++)
  	{
  		size_t idx = get_vehicle_size() + i * get_feature_size();
  
  		cov(0, 0) = m_pkk(idx + 0, idx + 0);
  		cov(1, 1) = m_pkk(idx + 1, idx + 1);
  		cov(0, 1) = cov(1, 0) = m_pkk(idx + 0, idx + 1);
  
  		mean[0] = m_xkk[idx + 0];
  		mean[1] = m_xkk[idx + 1];
  
  		// Command to draw the 2D ellipse:
  		os::fprintf(
  			f, "%s",
  			math::MATLAB_plotCovariance2D(cov, mean, stdCount, styleLandmarks)
  				.c_str());
  	}
  
  	// Now: the robot path:
  	// ------------------------------
  	if (m_SFs.size())
  	{
  		os::fprintf(f, "\nROB_PATH=[");
  		for (i = 0; i < m_SFs.size(); i++)
  		{
  			CSensoryFrame::Ptr dummySF;
  			CPose3DPDF::Ptr pdf3D;
  			m_SFs.get(i, pdf3D, dummySF);
  
  			CPose3D p;
  			pdf3D->getMean(p);
  			CPoint3D pnt3D(p);	// 6D -> 3D only
  
  			os::fprintf(f, "%.04f %.04f", pnt3D.x(), pnt3D.y());
  			if (i < (m_SFs.size() - 1)) os::fprintf(f, ";");
  		}
  		os::fprintf(f, "];\n");
  
  		os::fprintf(
  			f, "plot(ROB_PATH(:,1),ROB_PATH(:,2),'%s');\n", stylePath.c_str());
  	}
  
  	// The robot pose:
  	cov(0, 0) = m_pkk(0, 0);
  	cov(1, 1) = m_pkk(1, 1);
  	cov(0, 1) = cov(1, 0) = m_pkk(0, 1);
  
  	mean[0] = m_xkk[0];
  	mean[1] = m_xkk[1];
  
  	os::fprintf(
  		f, "%s",
  		math::MATLAB_plotCovariance2D(cov, mean, stdCount, styleRobot).c_str());
  
  	os::fprintf(f, "\naxis equal;\n");
  	os::fclose(f);
  }
  
  /** Computes A=A-B, which may need to be re-implemented depending on the
   * topology of the individual scalar components (eg, angles).
   */
  void CRangeBearingKFSLAM2D::OnSubstractObservationVectors(
  	KFArray_OBS& A, const KFArray_OBS& B) const
  {
  	A[0] -= B[0];
  	A[1] -= B[1];
  	mrpt::math::wrapToPiInPlace(A[1]);
  }
  
  /** Return the observation NOISE covariance matrix, that is, the model of the
   * Gaussian additive noise of the sensor.
   * \param out_R The noise covariance matrix. It might be non diagonal, but
   * it'll usually be.
   */
  void CRangeBearingKFSLAM2D::OnGetObservationNoise(KFMatrix_OxO& out_R) const
  {
  	out_R(0, 0) = square(options.std_sensor_range);
  	out_R(1, 1) = square(options.std_sensor_yaw);
  }
  
  /** This will be called before OnGetObservationsAndDataAssociation to allow the
   * application to reduce the number of covariance landmark predictions to be
   * made.
   *  For example, features which are known to be "out of sight" shouldn't be
   * added to the output list to speed up the calculations.
   * \param in_all_prediction_means The mean of each landmark predictions; the
   * computation or not of the corresponding covariances is what we're trying to
   * determined with this method.
   * \param out_LM_indices_to_predict The list of landmark indices in the map
   * [0,getNumberOfLandmarksInTheMap()-1] that should be predicted.
   * \note This is not a pure virtual method, so it should be implemented only if
   * desired. The default implementation returns a vector with all the landmarks
   * in the map.
   * \sa OnGetObservations, OnDataAssociation
   */
  void CRangeBearingKFSLAM2D::OnPreComputingPredictions(
  	const vector_KFArray_OBS& prediction_means,
  	std::vector<size_t>& out_LM_indices_to_predict) const
  {
  	CObservationBearingRange::Ptr obs =
  		m_SF->getObservationByClass<CObservationBearingRange>();
  	ASSERTMSG_(
  		obs,
  		"*ERROR*: This method requires an observation of type "
  		"CObservationBearingRange");
  
  	const double sensor_max_range = obs->maxSensorDistance;
  	const double fov_yaw = obs->fieldOfView_yaw;
  
  	const double max_vehicle_loc_uncertainty =
  		4 * std::sqrt(m_pkk(0, 0) + m_pkk(1, 1));
  	const double max_vehicle_ang_uncertainty = 4 * std::sqrt(m_pkk(2, 2));
  
  	out_LM_indices_to_predict.clear();
  	for (size_t i = 0; i < prediction_means.size(); i++)
  #if (!STATS_EXPERIMENT)	 // In the experiment we force errors too far and some
  		// predictions are out of range, so just generate all
  		// of them:
  		if (prediction_means[i][0] <
  				(1.5 + sensor_max_range + max_vehicle_loc_uncertainty +
  				 4 * options.std_sensor_range) &&
  			fabs(prediction_means[i][1]) <
  				(20.0_deg + 0.5 * fov_yaw + max_vehicle_ang_uncertainty +
  				 4 * options.std_sensor_yaw))
  #endif
  		{
  			out_LM_indices_to_predict.push_back(i);
  		}
  }
  
  /** Only called if using a numeric approximation of the transition Jacobian,
   * this method must return the increments in each dimension of the vehicle state
   * vector while estimating the Jacobian.
   */
  void CRangeBearingKFSLAM2D::OnTransitionJacobianNumericGetIncrements(
  	KFArray_VEH& out_increments) const
  {
  	for (size_t i = 0; i < get_vehicle_size(); i++)
  		out_increments[i] = 1e-6;
  }
  
  /** Only called if using a numeric approximation of the observation Jacobians,
   * this method must return the increments in each dimension of the vehicle state
   * vector while estimating the Jacobian.
   */
  void CRangeBearingKFSLAM2D::OnObservationJacobiansNumericGetIncrements(
  	KFArray_VEH& out_veh_increments, KFArray_FEAT& out_feat_increments) const
  {
  	for (size_t i = 0; i < get_vehicle_size(); i++)
  		out_veh_increments[i] = 1e-6;
  	for (size_t i = 0; i < get_feature_size(); i++)
  		out_feat_increments[i] = 1e-6;
  }
  