/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2023, 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 "apps-precomp.h"  // Precompiled headers
//
#include <mrpt/apps/KFSLAMApp.h>
#include <mrpt/config/CConfigFile.h>
#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/io/vector_loadsave.h>
#include <mrpt/math/ops_containers.h>
#include <mrpt/obs/CRawlog.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CSetOfLines.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/slam/CRangeBearingKFSLAM.h>
#include <mrpt/slam/CRangeBearingKFSLAM2D.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>
#include <mrpt/system/string_utils.h>

#include <fstream>

using namespace mrpt::apps;

KFSLAMApp::KFSLAMApp() : mrpt::system::COutputLogger("KFSLAMApp") {}

void KFSLAMApp::initialize(int argc, const char** argv)
{
	MRPT_START

	MRPT_LOG_INFO_FMT(
		" kf-slam - Part of the MRPT\n"
		" MRPT C++ Library: %s - Sources timestamp: %s\n\n",
		mrpt::system::MRPT_getVersion().c_str(),
		mrpt::system::MRPT_getCompilationDate().c_str());

	// Process arguments:
	if (argc < 2)
	{
		THROW_EXCEPTION("Usage: kf-slam <config_file> [dataset.rawlog]");
	}

	// Config file:
	const std::string configFile = std::string(argv[1]);

	ASSERT_FILE_EXISTS_(configFile);
	params.setContent(mrpt::io::file_get_contents(configFile));

	// Rawlog file: from args. line or from config file:
	if (argc == 3) rawlogFileName = std::string(argv[2]);
	else
		rawlogFileName = params.read_string(
			"MappingApplication", "rawlog_file", std::string("log.rawlog"));

	MRPT_END
}

void KFSLAMApp::run()
{
	MRPT_START

	// 2D or 3D implementation:
	const auto kf_implementation = mrpt::system::trim(params.read_string(
		"MappingApplication", "kf_implementation", "CRangeBearingKFSLAM"));

	if (kf_implementation == "CRangeBearingKFSLAM")
		Run_KF_SLAM<mrpt::slam::CRangeBearingKFSLAM>();
	else if (kf_implementation == "CRangeBearingKFSLAM2D")
		Run_KF_SLAM<mrpt::slam::CRangeBearingKFSLAM2D>();
	else
		throw std::runtime_error(
			"kf_implementation: Invalid value found in the config file.");

	MRPT_END
}

using namespace mrpt;
using namespace mrpt::slam;
using namespace mrpt::maps;
using namespace mrpt::opengl;
using namespace mrpt::system;
using namespace mrpt::math;
using namespace mrpt::poses;
using namespace mrpt::config;
using namespace mrpt::io;
using namespace mrpt::img;
using namespace mrpt::obs;
using namespace std;

// ------------------------------------------------------
//				traits
// ------------------------------------------------------
template <class IMPL>
struct kfslam_traits;

// Specialization for 2D (3D) SLAM:
template <>
struct kfslam_traits<CRangeBearingKFSLAM2D>
{
	using ekfslam_t = CRangeBearingKFSLAM2D;
	using posepdf_t = CPosePDFGaussian;
	using pose_t = CPose2D;
	using lm_t = TPoint2D;
	template <class ARR>
	static void landmark_to_3d(const ARR& lm, TPoint3D& p)
	{
		p.x = lm[0];
		p.y = lm[1];
		p.z = 0;
	}

	static void doPartitioningExperiment(
		[[maybe_unused]] ekfslam_t& mapping,
		[[maybe_unused]] CMatrixDouble& fullCov,
		[[maybe_unused]] const string& OUT_DIR)
	{
		// Nothing to do
	}
};

// Specialization for 3D (6D) SLAM:
template <>
struct kfslam_traits<CRangeBearingKFSLAM>
{
	using ekfslam_t = CRangeBearingKFSLAM;
	using posepdf_t = CPose3DQuatPDFGaussian;
	using pose_t = CPose3D;
	using lm_t = CPoint3D;

	template <class ARR>
	static void landmark_to_3d(const ARR& lm, TPoint3D& p)
	{
		p.x = lm[0];
		p.y = lm[1];
		p.z = lm[2];
	}

	static void doPartitioningExperiment(
		ekfslam_t& mapping, CMatrixDouble& fullCov, const string& OUT_DIR)
	{
		// Compute the "information" between partitions:
		if (mapping.options.doPartitioningExperiment)
		{
			// --------------------------------------------
			// PART I:
			//  Comparison to fixed partitioning every K obs.
			// --------------------------------------------

			// Compute the information matrix:
			for (size_t i = 0; i < 6; i++)
				fullCov(i, i) = max(fullCov(i, i), 1e-6);

			CMatrixF H(fullCov.inverse_LLt());
			H.saveToTextFile(OUT_DIR + string("/information_matrix_final.txt"));

			// Replace by absolute values:
			H = H.array().abs().matrix();
			CMatrixF H2(H);
			CImage imgF;
			imgF.setFromMatrix(H2, false /*it's not normalized*/);
			imgF.saveToFile(OUT_DIR + string("/information_matrix_final.png"));

			// ----------------------------------------
			// Compute the "approximation error factor" E:
			//  E = SUM() / SUM(ALL ELEMENTS IN MATRIX)
			// ----------------------------------------
			vector<std::vector<uint32_t>> landmarksMembership, partsInObsSpace;
			mrpt::math::CMatrixDouble ERRS(50, 3);

			for (int i = 0; i < ERRS.rows(); i++)
			{
				int K;

				if (i == 0)
				{
					K = 0;
					mapping.getLastPartitionLandmarks(landmarksMembership);
				}
				else
				{
					K = i + 1;
					mapping.getLastPartitionLandmarksAsIfFixedSubmaps(
						i + 1, landmarksMembership);
				}

				mapping.getLastPartition(partsInObsSpace);

				ERRS(i, 0) = K;
				ERRS(i, 1) = partsInObsSpace.size();
				ERRS(i, 2) = mapping.computeOffDiagonalBlocksApproximationError(
					landmarksMembership);
			}

			ERRS.saveToTextFile(OUT_DIR + string("/ERRORS.txt"));

			// --------------------------------------------
			// PART II:
			//  Sweep partitioning threshold:
			// --------------------------------------------
			size_t STEPS = 50;
			CVectorFloat ERRS_SWEEP(STEPS), ERRS_SWEEP_THRESHOLD(STEPS);

			// Compute the error for each partitioning-threshold
			for (size_t i = 0; i < STEPS; i++)
			{
				float th = (1.0f * i) / (STEPS - 1.0f);
				ERRS_SWEEP_THRESHOLD[i] = th;
				mapping.mapPartitionOptions()->partitionThreshold = th;

				mapping.reconsiderPartitionsNow();

				mapping.getLastPartitionLandmarks(landmarksMembership);
				ERRS_SWEEP[i] =
					mapping.computeOffDiagonalBlocksApproximationError(
						landmarksMembership);
			}

			ERRS_SWEEP.saveToTextFile(OUT_DIR + string("/ERRORS_SWEEP.txt"));
			ERRS_SWEEP_THRESHOLD.saveToTextFile(
				OUT_DIR + string("/ERRORS_SWEEP_THRESHOLD.txt"));

		}  // end if doPartitioningExperiment
	}
};

template <class IMPL>
void KFSLAMApp::Run_KF_SLAM()
{
	MRPT_START

	auto& cfgFile = params;	 // for backwards compat of old code

	// The EKF-SLAM class:
	// Traits for this KF implementation (2D or 3D)
	using traits_t = kfslam_traits<IMPL>;
	using ekfslam_t = typename traits_t::ekfslam_t;

	ekfslam_t mapping;

	// The rawlog file:
	// ----------------------------------------
	const unsigned int rawlog_offset =
		cfgFile.read_int("MappingApplication", "rawlog_offset", 0);

	const unsigned int SAVE_LOG_FREQUENCY =
		cfgFile.read_int("MappingApplication", "SAVE_LOG_FREQUENCY", 1);

	const bool SAVE_DA_LOG =
		cfgFile.read_bool("MappingApplication", "SAVE_DA_LOG", true);

	const bool SAVE_3D_SCENES =
		cfgFile.read_bool("MappingApplication", "SAVE_3D_SCENES", true);
	const bool SAVE_MAP_REPRESENTATIONS = cfgFile.read_bool(
		"MappingApplication", "SAVE_MAP_REPRESENTATIONS", true);
	bool SHOW_3D_LIVE =
		cfgFile.read_bool("MappingApplication", "SHOW_3D_LIVE", false);
	const bool CAMERA_3DSCENE_FOLLOWS_ROBOT = cfgFile.read_bool(
		"MappingApplication", "CAMERA_3DSCENE_FOLLOWS_ROBOT", false);

#if !MRPT_HAS_WXWIDGETS
	SHOW_3D_LIVE = false;
#endif

	string OUT_DIR = cfgFile.read_string(
		"MappingApplication", "logOutput_dir", "OUT_KF-SLAM");
	string ground_truth_file =
		cfgFile.read_string("MappingApplication", "ground_truth_file", "");
	string ground_truth_file_robot = cfgFile.read_string(
		"MappingApplication", "ground_truth_file_robot", "");

	string ground_truth_data_association = cfgFile.read_string(
		"MappingApplication", "ground_truth_data_association", "");

	MRPT_LOG_INFO_STREAM("RAWLOG FILE: " << rawlogFileName);
	ASSERT_FILE_EXISTS_(rawlogFileName);
	CFileGZInputStream rawlogFile(rawlogFileName);

	deleteFilesInDirectory(OUT_DIR);
	createDirectory(OUT_DIR);

	// Load the config options for mapping:
	// ----------------------------------------
	mapping.loadOptions(cfgFile);
	{
		std::stringstream o;
		mapping.KF_options.dumpToTextStream(o);
		mapping.options.dumpToTextStream(o);
		MRPT_LOG_INFO(o.str());
	}

	// debug:
	// mapping.KF_options.use_analytic_observation_jacobian = true;
	// mapping.KF_options.use_analytic_transition_jacobian = true;
	// mapping.KF_options.debug_verify_analytic_jacobians = true;

	// Is there ground truth of the robot poses??
	CMatrixDouble GT_PATH(0, 0);
	if (ground_truth_file_robot.size() && fileExists(ground_truth_file_robot))
	{
		GT_PATH.loadFromTextFile(ground_truth_file_robot);
		ASSERT_(GT_PATH.rows() > 0 && GT_PATH.cols() == 6);
	}

	// Is there a ground truth file of the data association?
	// Map: timestamp -> vector(index in observation -> real index)
	std::map<double, std::vector<int>> GT_DA;
	// Landmark indices bimapping: SLAM DA <---> GROUND TRUTH DA
	mrpt::containers::bimap<int, int> DA2GTDA_indices;
	if (!ground_truth_data_association.empty() &&
		fileExists(ground_truth_data_association))
	{
		CMatrixDouble mGT_DA;
		mGT_DA.loadFromTextFile(ground_truth_data_association);
		ASSERT_GE_(mGT_DA.cols(), 3);
		// Convert the loaded matrix into a std::map in GT_DA:
		for (int i = 0; i < mGT_DA.rows(); i++)
		{
			std::vector<int>& v = GT_DA[mGT_DA(i, 0)];
			if (v.size() <= mGT_DA(i, 1)) v.resize(mGT_DA(i, 1) + 1);
			v[mGT_DA(i, 1)] = mGT_DA(i, 2);
		}
		MRPT_LOG_INFO_STREAM(
			"Loaded " << GT_DA.size() << " entries from DA ground truth file.");
	}

	// Create output file for DA perf:
	std::ofstream out_da_performance_log;
	{
		const std::string f = std::string(
			OUT_DIR + std::string("/data_association_performance.log"));
		out_da_performance_log.open(f.c_str());
		ASSERTMSG_(
			out_da_performance_log.is_open(),
			std::string("Error writing to: ") + f);

		// Header:
		out_da_performance_log
			<< "%           TIMESTAMP                INDEX_IN_OBS    TruePos "
			   "FalsePos TrueNeg FalseNeg  NoGroundTruthSoIDontKnow \n"
			<< "%--------------------------------------------------------------"
			   "--------------------------------------------------\n";
	}

	mrpt::gui::CDisplayWindow3D::Ptr win3d;

	if (SHOW_3D_LIVE)
	{
		win3d =
			mrpt::gui::CDisplayWindow3D::Create("KF-SLAM live view", 800, 500);

		win3d->addTextMessage(0.01, 0.96, "Red: Estimated path", 100);
		win3d->addTextMessage(0.01, 0.93, "Black: Ground truth path", 101);
	}

	// Create DA-log output file:
	std::ofstream out_da_log;
	if (SAVE_DA_LOG)
	{
		const std::string f =
			std::string(OUT_DIR + std::string("/data_association.log"));
		out_da_log.open(f.c_str());
		ASSERTMSG_(out_da_log.is_open(), std::string("Error writing to: ") + f);

		// Header:
		out_da_log << "%           TIMESTAMP                INDEX_IN_OBS    ID "
					  "   RANGE(m)    YAW(rad)   PITCH(rad) \n"
				   << "%-------------------------------------------------------"
					  "-------------------------------------\n";
	}

	// The main loop:
	// ---------------------------------------
	CActionCollection::Ptr action;
	CSensoryFrame::Ptr observations;
	size_t rawlogEntry = 0, step = 0;

	vector<TPose3D> meanPath;  // The estimated path
	typename traits_t::posepdf_t robotPose;
	const bool is_pose_3d = robotPose.state_length != 3;

	std::vector<typename IMPL::landmark_point_t> LMs;
	std::map<unsigned int, CLandmark::TLandmarkID> LM_IDs;
	CMatrixDouble fullCov;
	CVectorDouble fullState;
	CTicTac kftictac;

	auto rawlogArch = mrpt::serialization::archiveFrom(rawlogFile);

	for (;;)
	{
		if (os::kbhit())
		{
			char pushKey = os::getch();
			if (27 == pushKey) break;
		}

		// Load action/observation pair from the rawlog:
		// --------------------------------------------------
		if (!CRawlog::readActionObservationPair(
				rawlogArch, action, observations, rawlogEntry))
			break;	// file EOF

		if (rawlogEntry >= rawlog_offset)
		{
			// Process the action and observations:
			// --------------------------------------------
			kftictac.Tic();

			mapping.processActionObservation(action, observations);

			const double tim_kf_iter = kftictac.Tac();

			// Get current state:
			// -------------------------------
			mapping.getCurrentState(robotPose, LMs, LM_IDs, fullState, fullCov);
			MRPT_LOG_INFO_STREAM("Mean pose: " << robotPose.mean);
			MRPT_LOG_INFO_STREAM("# of landmarks in the map: " << LMs.size());

			// Get the mean robot pose as 3D:
			const CPose3D robotPoseMean3D = CPose3D(robotPose.mean);

			// Build the path:
			meanPath.push_back(robotPoseMean3D.asTPose());

			// Save mean pose:
			if (!(step % SAVE_LOG_FREQUENCY))
			{
				const auto p = robotPose.mean.asVectorVal();
				p.saveToTextFile(
					OUT_DIR +
					format("/robot_pose_%05u.txt", (unsigned int)step));
			}

			// Save full cov:
			if (!(step % SAVE_LOG_FREQUENCY))
			{
				fullCov.saveToTextFile(
					OUT_DIR + format("/full_cov_%05u.txt", (unsigned int)step));
			}

			// Generate Data Association log?
			if (SAVE_DA_LOG)
			{
				const typename ekfslam_t::TDataAssocInfo& da =
					mapping.getLastDataAssociation();

				const CObservationBearingRange::Ptr obs =
					observations
						->getObservationByClass<CObservationBearingRange>();
				if (obs)
				{
					const CObservationBearingRange* obsRB = obs.get();
					const double tim =
						mrpt::system::timestampToDouble(obsRB->timestamp);

					for (size_t i = 0; i < obsRB->sensedData.size(); i++)
					{
						auto it = da.results.associations.find(i);
						int assoc_ID_in_SLAM;
						if (it != da.results.associations.end())
							assoc_ID_in_SLAM = it->second;
						else
						{
							// It should be a newly created LM:
							auto itNewLM = da.newly_inserted_landmarks.find(i);
							if (itNewLM != da.newly_inserted_landmarks.end())
								assoc_ID_in_SLAM = itNewLM->second;
							else
								assoc_ID_in_SLAM = -1;
						}

						out_da_log << format(
							"%35.22f %8i %10i %10f %12f %12f\n", tim, (int)i,
							assoc_ID_in_SLAM,
							(double)obsRB->sensedData[i].range,
							(double)obsRB->sensedData[i].yaw,
							(double)obsRB->sensedData[i].pitch);
					}
				}
			}

			// Save report on DA performance:
			{
				const typename ekfslam_t::TDataAssocInfo& da =
					mapping.getLastDataAssociation();

				const CObservationBearingRange::Ptr obs =
					observations
						->getObservationByClass<CObservationBearingRange>();
				if (obs)
				{
					const CObservationBearingRange* obsRB = obs.get();
					const double tim =
						mrpt::system::timestampToDouble(obsRB->timestamp);

					auto itDA = GT_DA.find(tim);

					for (size_t i = 0; i < obsRB->sensedData.size(); i++)
					{
						bool is_FP = false, is_TP = false, is_FN = false,
							 is_TN = false;

						if (itDA != GT_DA.end())
						{
							const std::vector<int>& vDA = itDA->second;
							ASSERT_LT_(i, vDA.size());
							const int GT_ASSOC = vDA[i];

							auto it = da.results.associations.find(i);
							if (it != da.results.associations.end())
							{
								// This observation was assigned the already
								// existing LM in the map: "it->second"
								// TruePos -> If that LM index corresponds to
								// that in the GT (with index mapping):

								// mrpt::containers::bimap<int,int>
								// DA2GTDA_indices;
								// // Landmark indices bimapping: SLAM DA <--->
								// GROUND TRUTH DA
								if (DA2GTDA_indices.hasKey(it->second))
								{
									const int slam_asigned_LM_idx =
										DA2GTDA_indices.direct(it->second);
									if (slam_asigned_LM_idx == GT_ASSOC)
										is_TP = true;
									else
										is_FP = true;
								}
								else
								{
									// Is this case possible? Assigned to an
									// index not ever seen for the first time
									// with a GT....
									//  Just in case:
									is_FP = true;
								}
							}
							else
							{
								// No pairing, but should be a newly created LM:
								auto itNewLM =
									da.newly_inserted_landmarks.find(i);
								if (itNewLM !=
									da.newly_inserted_landmarks.end())
								{
									const int new_LM_in_SLAM = itNewLM->second;

									// Was this really a NEW LM not observed
									// before?
									if (DA2GTDA_indices.hasValue(GT_ASSOC))
									{
										// GT says this LM was already observed,
										// so it shouldn't appear here as new:
										is_FN = true;
									}
									else
									{
										// Really observed for the first time:
										is_TN = true;
										DA2GTDA_indices.insert(
											new_LM_in_SLAM, GT_ASSOC);
									}
								}
								else
								{
									// Not associated neither inserted:
									// Shouldn't really never arrive here.
								}
							}
						}

						// "%           TIMESTAMP                INDEX_IN_OBS
						// TruePos FalsePos TrueNeg FalseNeg
						// NoGroundTruthSoIDontKnow \n"
						out_da_performance_log << format(
							"%35.22f %13i %8i %8i %8i %8i %8i\n", tim, (int)i,
							(int)(is_TP ? 1 : 0), (int)(is_FP ? 1 : 0),
							(int)(is_TN ? 1 : 0), (int)(is_FN ? 1 : 0),
							(int)(!is_FP && !is_TP && !is_FN && !is_TN ? 1 : 0));
					}
				}
			}

			// Save map to file representations?
			if (SAVE_MAP_REPRESENTATIONS && !(step % SAVE_LOG_FREQUENCY))
			{
				mapping.saveMapAndPath2DRepresentationAsMATLABFile(
					OUT_DIR + format("/slam_state_%05u.m", (unsigned int)step));
			}

			// Save 3D view of the filter state:
			if (win3d || (SAVE_3D_SCENES && !(step % SAVE_LOG_FREQUENCY)))
			{
				Scene::Ptr scene3D = std::make_shared<Scene>();
				{
					opengl::CGridPlaneXY::Ptr grid =
						std::make_shared<opengl::CGridPlaneXY>(
							-1000, 1000, -1000, 1000, 0, 5);
					grid->setColor(0.4f, 0.4f, 0.4f);
					scene3D->insert(grid);
				}

				// Robot path:
				{
					opengl::CSetOfLines::Ptr linesPath =
						std::make_shared<opengl::CSetOfLines>();
					linesPath->setColor(1, 0, 0);

					TPose3D init_pose;
					if (!meanPath.empty())
						init_pose = CPose3D(meanPath[0]).asTPose();

					int path_decim = 0;
					for (auto& it : meanPath)
					{
						linesPath->appendLine(init_pose, it);
						init_pose = it;

						if (++path_decim > 10)
						{
							path_decim = 0;
							mrpt::opengl::CSetOfObjects::Ptr xyz =
								mrpt::opengl::stock_objects::CornerXYZSimple(
									0.3f, 2.0f);
							xyz->setPose(CPose3D(it));
							scene3D->insert(xyz);
						}
					}
					scene3D->insert(linesPath);

					// finally a big corner for the latest robot pose:
					{
						mrpt::opengl::CSetOfObjects::Ptr xyz =
							mrpt::opengl::stock_objects::CornerXYZSimple(
								1.0, 2.5);
						xyz->setPose(robotPoseMean3D);
						scene3D->insert(xyz);
					}

					// The camera pointing to the current robot pose:
					if (win3d && CAMERA_3DSCENE_FOLLOWS_ROBOT)
					{
						win3d->setCameraPointingToPoint(
							robotPoseMean3D.x(), robotPoseMean3D.y(),
							robotPoseMean3D.z());
					}
				}

				// Do we have a ground truth?
				if (GT_PATH.cols() == 6 || GT_PATH.cols() == 3)
				{
					opengl::CSetOfLines::Ptr GT_path =
						std::make_shared<opengl::CSetOfLines>();
					GT_path->setColor(0, 0, 0);
					size_t N =
						std::min((int)GT_PATH.rows(), (int)meanPath.size());

					if (GT_PATH.cols() == 6)
					{
						double gtx0 = 0, gty0 = 0, gtz0 = 0;
						for (size_t i = 0; i < N; i++)
						{
							const CPose3D p(
								GT_PATH(i, 0), GT_PATH(i, 1), GT_PATH(i, 2),
								GT_PATH(i, 3), GT_PATH(i, 4), GT_PATH(i, 5));

							GT_path->appendLine(
								gtx0, gty0, gtz0, p.x(), p.y(), p.z());
							gtx0 = p.x();
							gty0 = p.y();
							gtz0 = p.z();
						}
					}
					else if (GT_PATH.cols() == 3)
					{
						double gtx0 = 0, gty0 = 0;
						for (size_t i = 0; i < N; i++)
						{
							const CPose2D p(
								GT_PATH(i, 0), GT_PATH(i, 1), GT_PATH(i, 2));

							GT_path->appendLine(gtx0, gty0, 0, p.x(), p.y(), 0);
							gtx0 = p.x();
							gty0 = p.y();
						}
					}
					scene3D->insert(GT_path);
				}

				// Draw latest data association:
				{
					const typename ekfslam_t::TDataAssocInfo& da =
						mapping.getLastDataAssociation();

					mrpt::opengl::CSetOfLines::Ptr lins =
						mrpt::opengl::CSetOfLines::Create();
					lins->setLineWidth(1.2f);
					lins->setColor(1, 1, 1);
					for (auto it = da.results.associations.begin();
						 it != da.results.associations.end(); ++it)
					{
						const prediction_index_t idxPred = it->second;
						// This index must match the internal list of features
						// in the map:
						typename ekfslam_t::KFArray_FEAT featMean;
						mapping.getLandmarkMean(idxPred, featMean);

						TPoint3D featMean3D;
						traits_t::landmark_to_3d(featMean, featMean3D);

						// Line: robot -> landmark:
						lins->appendLine(
							robotPoseMean3D.x(), robotPoseMean3D.y(),
							robotPoseMean3D.z(), featMean3D.x, featMean3D.y,
							featMean3D.z);
					}
					scene3D->insert(lins);
				}

				// The current state of KF-SLAM:
				{
					opengl::CSetOfObjects::Ptr objs =
						std::make_shared<opengl::CSetOfObjects>();
					mapping.getAs3DObject(objs);
					scene3D->insert(objs);
				}

				if (win3d)
				{
					mrpt::opengl::Scene::Ptr& scn = win3d->get3DSceneAndLock();
					scn = scene3D;

					// Update text messages:
					win3d->addTextMessage(
						0.02, 0.02,
						format(
							"Step %u - Landmarks in the map: %u",
							(unsigned int)step, (unsigned int)LMs.size()),
						0);

					win3d->addTextMessage(
						0.02, 0.06,
						format(
							is_pose_3d
								? "Estimated pose: (x y z qr qx qy qz) = %s"
								: "Estimated pose: (x y yaw) = %s",
							robotPose.mean.asString().c_str()),
						1);

					static vector<double> estHz_vals;
					const double curHz = 1.0 / std::max(1e-9, tim_kf_iter);
					estHz_vals.push_back(curHz);
					if (estHz_vals.size() > 50)
						estHz_vals.erase(estHz_vals.begin());
					const double meanHz = mrpt::math::mean(estHz_vals);

					win3d->addTextMessage(
						0.02, 0.10,
						format(
							"Iteration time: %7ss",
							mrpt::system::unitsFormat(tim_kf_iter).c_str()),
						2);

					win3d->addTextMessage(
						0.02, 0.14,
						format(
							"Execution rate: %7sHz",
							mrpt::system::unitsFormat(meanHz).c_str()),
						3);

					win3d->unlockAccess3DScene();
					win3d->repaint();
				}

				if (SAVE_3D_SCENES && !(step % SAVE_LOG_FREQUENCY))
				{
					// Save to file:
					CFileGZOutputStream f(
						OUT_DIR +
						format("/kf_state_%05u.3Dscene", (unsigned int)step));
					mrpt::serialization::archiveFrom(f) << *scene3D;
				}
			}

			// Free rawlog items memory:
			// --------------------------------------------
			action.reset();
			observations.reset();

		}  // (rawlogEntry>=rawlog_offset)

		MRPT_LOG_INFO_STREAM(
			"Step " << step << " - Rawlog entries processed: " << rawlogEntry);

		step++;
	};	// end "while(1)"

	// Partitioning experiment: Only for 6D SLAM:
	traits_t::doPartitioningExperiment(mapping, fullCov, OUT_DIR);

	// Is there ground truth of landmarks positions??
	if (ground_truth_file.size() && fileExists(ground_truth_file))
	{
		CMatrixFloat GT(0, 0);
		try
		{
			GT.loadFromTextFile(ground_truth_file);
		}
		catch (const std::exception& e)
		{
			MRPT_LOG_ERROR_STREAM(
				"Ignoring the following error loading ground truth file: "
				<< mrpt::exception_to_str(e));
		}

		if (GT.rows() > 0 && !LMs.empty())
		{
			// Each row has:
			//   [0] [1] [2]  [6]
			//    x   y   z    ID
			CVectorDouble ERRS(0);
			for (size_t i = 0; i < LMs.size(); i++)
			{
				// Find the entry in the GT for this mapped LM:
				bool found = false;
				for (int r = 0; r < GT.rows(); r++)
				{
					if (std::abs(LM_IDs[i] - GT(r, 6)) < 1e-9)
					{
						const CPoint3D gtPt(GT(r, 0), GT(r, 1), GT(r, 2));
						// All these conversions are to make it work with either
						// CPoint3D & TPoint2D:
						ERRS.push_back(
							gtPt.distanceTo(CPoint3D(TPoint3D(LMs[i]))));
						found = true;
						break;
					}
				}
				if (!found)
				{
					MRPT_LOG_ERROR_STREAM(
						"Ground truth entry not found for landmark ID:"
						<< LM_IDs[i]);
				}
			}

			loc_error_wrt_gt = math::mean(ERRS);

			MRPT_LOG_INFO("ERRORS VS. GROUND TRUTH:");
			MRPT_LOG_INFO_FMT("Mean Error: %f meters\n", loc_error_wrt_gt);
			MRPT_LOG_INFO_FMT(
				"Minimum error: %f meters\n", math::minimum(ERRS));
			MRPT_LOG_INFO_FMT(
				"Maximum error: %f meters\n", math::maximum(ERRS));
		}
	}  // end if GT

	MRPT_LOG_INFO("********* KF-SLAM finished! **********");

	if (win3d)
	{
		MRPT_LOG_WARN("Close the 3D window to quit the application.");
		win3d->waitForKey();
	}

	MRPT_END
}
