/* +------------------------------------------------------------------------+
   |                     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          |
   +------------------------------------------------------------------------+ */

#pragma once

#include <mrpt/config/CConfigFile.h>
#include <mrpt/graphslam/CGraphSlamEngine.h>  // for IDE parsers
#include <mrpt/io/CFileInputStream.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/obs/CObservationOdometry.h>
#include <mrpt/opengl/CAxis.h>
#include <mrpt/opengl/CPlanarLaserScan.h>
#include <mrpt/system/filesystem.h>

namespace mrpt::graphslam
{
template <class GRAPH_T>
CGraphSlamEngine<GRAPH_T>::CGraphSlamEngine(
	const std::string& config_file, const std::string& rawlog_fname,
	const std::string& fname_GT, mrpt::graphslam::CWindowManager* win_manager,
	mrpt::graphslam::deciders::CNodeRegistrationDecider<GRAPH_T>* node_reg,
	mrpt::graphslam::deciders::CEdgeRegistrationDecider<GRAPH_T>* edge_reg,
	mrpt::graphslam::optimizers::CGraphSlamOptimizer<GRAPH_T>* optimizer)
	: m_node_reg(node_reg),
	  m_edge_reg(edge_reg),
	  m_optimizer(optimizer),
	  m_enable_visuals(win_manager != nullptr),
	  m_config_fname(config_file),
	  m_rawlog_fname(rawlog_fname),
	  m_fname_GT(fname_GT),
	  m_GT_poses_step(1),
	  m_win_manager(win_manager),
	  m_paused_message("Program is paused. Press \"p/P\" to resume."),
	  m_text_index_paused_message(345),	 // just a large number.
	  m_odometry_color(0, 0, 255),
	  m_GT_color(0, 255, 0),
	  m_estimated_traj_color(255, 165, 0),
	  m_optimized_map_color(255, 0, 0),
	  m_current_constraint_type_color(255, 255, 255),
	  m_robot_model_size(1),
	  m_class_name("CGraphSlamEngine"),
	  m_is_first_time_node_reg(true)
{
	this->initClass();
}

template <class GRAPH_T>
CGraphSlamEngine<GRAPH_T>::~CGraphSlamEngine()
{
	// change back the CImage path
	if (mrpt::system::strCmpI(m_GT_file_format, "rgbd_tum"))
	{
		MRPT_LOG_DEBUG_STREAM("Changing back the CImage PATH");
		mrpt::img::CImage::setImagesPathBase(m_img_prev_path_base);
	}
}

template <class GRAPH_T>
typename GRAPH_T::global_pose_t
	CGraphSlamEngine<GRAPH_T>::getCurrentRobotPosEstimation() const
{
	std::lock_guard<std::mutex> graph_lock(m_graph_section);
	return m_node_reg->getCurrentRobotPosEstimation();
}

template <class GRAPH_T>
typename GRAPH_T::global_poses_t
	CGraphSlamEngine<GRAPH_T>::getRobotEstimatedTrajectory() const
{
	std::lock_guard<std::mutex> graph_lock(m_graph_section);
	return m_graph.nodes;
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::getNodeIDsOfEstimatedTrajectory(
	std::set<mrpt::graphs::TNodeID>* nodes_set) const
{
	ASSERTDEB_(nodes_set);
	m_graph.getAllNodes(*nodes_set);
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::initClass()
{
	MRPT_START
	using namespace mrpt;
	using namespace mrpt::opengl;
	using namespace std;

	// logger instance properties
	m_time_logger.setName(m_class_name);
	this->logging_enable_keep_record = true;
	this->setLoggerName(m_class_name);

	// Assert that the deciders/optimizer pointers are valid
	ASSERTDEB_(m_node_reg);
	ASSERTDEB_(m_edge_reg);
	ASSERTDEB_(m_optimizer);

	// Assert that the graph class used is supported.
	{
		MRPT_LOG_INFO_STREAM("Verifying support for given MRPT graph class...");

		// TODO - initialize vector in a smarter way.
		m_supported_constraint_types.push_back("CPosePDFGaussianInf");
		m_supported_constraint_types.push_back("CPose3DPDFGaussianInf");

		constraint_t c;
		const string c_str(c.GetRuntimeClass()->className);

		bool found =
			(std::find(
				 m_supported_constraint_types.begin(),
				 m_supported_constraint_types.end(),
				 c_str) != m_supported_constraint_types.end());

		if (found) { MRPT_LOG_INFO_STREAM("[OK] Class: " << c_str); }
		else
		{
			MRPT_LOG_WARN_STREAM(
				"Given graph class " << c_str
									 << " has not been tested consistently yet."
									 << "Proceed at your own risk.");
			mrpt::system::pause();
		}

		// store it in a string for future use.
		m_current_constraint_type = c_str;
		m_current_constraint_type = "Constraints: " + m_current_constraint_type;
	}

	// If a valid CWindowManager pointer is given then visuals are on.
	if (m_enable_visuals)
	{
		m_win = m_win_manager->win;
		m_win_observer = m_win_manager->observer;
	}
	else
	{
		m_win = nullptr;
		m_win_observer = nullptr;

		MRPT_LOG_WARN_STREAM("Visualization is off. Running on headless mode");
	}

	// set the CDisplayWindowPlots pointer to null for starters, we don't know
	// if
	// we are using it

	m_observation_only_dataset = false;
	m_request_to_exit = false;

	// max node number already in the graph
	m_nodeID_max = mrpt::graphs::INVALID_NODEID;

	m_is_paused = false;
	m_GT_poses_index = 0;

	// pass the necessary variables/objects to the deciders/optimizes
	// pass a graph ptr after the instance initialization
	m_node_reg->setGraphPtr(&m_graph);
	m_edge_reg->setGraphPtr(&m_graph);
	m_optimizer->setGraphPtr(&m_graph);

	// pass the window manager pointer
	// note: m_win_manager contains a pointer to the CDisplayWindow3D instance
	if (m_enable_visuals)
	{
		m_node_reg->setWindowManagerPtr(m_win_manager);
		m_edge_reg->setWindowManagerPtr(m_win_manager);
		m_optimizer->setWindowManagerPtr(m_win_manager);
		m_edge_counter.setWindowManagerPtr(m_win_manager);
	}

	// pass a lock in case of multithreaded implementation
	m_node_reg->setCriticalSectionPtr(&m_graph_section);
	m_edge_reg->setCriticalSectionPtr(&m_graph_section);
	m_optimizer->setCriticalSectionPtr(&m_graph_section);

	// Load the parameters that each one of the self/deciders/optimizer classes
	// needs
	this->loadParams(m_config_fname);

	if (!m_enable_visuals)
	{
		MRPT_LOG_WARN_STREAM("Switching all visualization parameters off...");
		m_visualize_odometry_poses = 0;
		m_visualize_GT = 0;
		m_visualize_map = 0;
		m_visualize_estimated_trajectory = 0;
		m_visualize_SLAM_metric = 0;
		m_enable_curr_pos_viewport = 0;
		m_enable_range_viewport = 0;
		m_enable_intensity_viewport = 0;
	}

	m_use_GT = !m_fname_GT.empty();
	if (m_use_GT)
	{
		if (mrpt::system::strCmpI(m_GT_file_format, "rgbd_tum"))
		{
			THROW_EXCEPTION("Not Implemented Yet.");
			this->alignOpticalWithMRPTFrame();
			// this->readGTFileRGBD_TUM(m_fname_GT, &m_GT_poses);
		}
		else if (mrpt::system::strCmpI(m_GT_file_format, "navsimul"))
		{
			this->readGTFile(m_fname_GT, &m_GT_poses);
		}
	}

	// plot the GT related visuals only if ground-truth file is given
	if (!m_use_GT)
	{
		MRPT_LOG_WARN_STREAM(
			"Ground truth file was not provided. Switching the related "
			"visualization parameters off...");
		m_visualize_GT = 0;
		m_visualize_SLAM_metric = 0;
	}

	// timestamp
	if (m_enable_visuals)
	{
		m_win_manager->assignTextMessageParameters(
			&m_offset_y_timestamp, &m_text_index_timestamp);
	}

	if (m_visualize_map) { this->initMapVisualization(); }

	// Configuration of various trajectories visualization
	ASSERTDEB_(m_has_read_config);
	if (m_enable_visuals)
	{
		// odometry visualization
		if (m_visualize_odometry_poses) { this->initOdometryVisualization(); }
		// GT Visualization
		if (m_visualize_GT) { this->initGTVisualization(); }
		// estimated trajectory visualization
		this->initEstimatedTrajectoryVisualization();
		// current robot pose  viewport
		if (m_enable_curr_pos_viewport) { this->initCurrPosViewport(); }
	}

	// change the CImage path in case of RGBD datasets
	if (mrpt::system::strCmpI(m_GT_file_format, "rgbd_tum"))
	{
		// keep the last path - change back to it after rawlog parsing
		m_img_prev_path_base = mrpt::img::CImage::getImagesPathBase();

		std::string rawlog_fname_noext =
			system::extractFileName(m_rawlog_fname);
		std::string rawlog_dir = system::extractFileDirectory(m_rawlog_fname);
		m_img_external_storage_dir =
			rawlog_dir + rawlog_fname_noext + "_Images/";
		mrpt::img::CImage::setImagesPathBase(m_img_external_storage_dir);
	}

	// 3DRangeScans viewports initialization, in case of RGBD datasets
	if (mrpt::system::strCmpI(m_GT_file_format, "rgbd_tum"))
	{
		if (m_enable_range_viewport) { this->initRangeImageViewport(); }
		if (m_enable_intensity_viewport) { this->initIntensityImageViewport(); }
	}
	// axis
	if (m_enable_visuals)
	{
		Scene::Ptr scene = m_win->get3DSceneAndLock();

		CAxis::Ptr obj = std::make_shared<CAxis>();
		obj->setFrequency(5);
		obj->enableTickMarks();
		obj->setAxisLimits(-10, -10, -10, 10, 10, 10);
		obj->setName("axis");
		scene->insert(obj);

		m_win->unlockAccess3DScene();
		m_win->forceRepaint();
	}

	// Add additional keystrokes in the CDisplayWindow3D help textBox
	if (m_enable_visuals)
	{
		// keystrokes initialization
		m_keystroke_pause_exec = "p";
		m_keystroke_odometry = "o";
		m_keystroke_GT = "g";
		m_keystroke_estimated_trajectory = "t";
		m_keystroke_map = "m";

		// keystrokes registration
		m_win_observer->registerKeystroke(
			m_keystroke_pause_exec, "Pause/Resume program execution");
		m_win_observer->registerKeystroke(
			m_keystroke_odometry, "Toggle Odometry visualization");
		m_win_observer->registerKeystroke(
			m_keystroke_GT, "Toggle Ground-Truth visualization");
		m_win_observer->registerKeystroke(
			m_keystroke_estimated_trajectory,
			"Toggle Estimated trajectory visualization");
		m_win_observer->registerKeystroke(
			m_keystroke_map, "Toggle Map visualization");
	}

	// register the types of edges
	vector<string> vec_edge_types;
	vec_edge_types.emplace_back("Odometry");
	vec_edge_types.emplace_back("ICP2D");
	vec_edge_types.emplace_back("ICP3D");

	for (auto cit = vec_edge_types.begin(); cit != vec_edge_types.end(); ++cit)
	{
		m_edge_counter.addEdgeType(*cit);
	}

	// Visualize the edge statistics
	if (m_enable_visuals)
	{
		// total edges - loop closure edges
		double offset_y_total_edges, offset_y_loop_closures;
		int text_index_total_edges, text_index_loop_closures;

		m_win_manager->assignTextMessageParameters(
			&offset_y_total_edges, &text_index_total_edges);

		// build each one of these
		map<string, double> name_to_offset_y;
		map<string, int> name_to_text_index;
		for (auto it = vec_edge_types.begin(); it != vec_edge_types.end(); ++it)
		{
			m_win_manager->assignTextMessageParameters(
				&name_to_offset_y[*it], &name_to_text_index[*it]);
		}

		m_win_manager->assignTextMessageParameters(
			&offset_y_loop_closures, &text_index_loop_closures);

		// add all the parameters to the CEdgeCounter object
		m_edge_counter.setTextMessageParams(
			name_to_offset_y, name_to_text_index, offset_y_total_edges,
			text_index_total_edges, offset_y_loop_closures,
			text_index_loop_closures);
	}

	// Type of the generated graph
	if (m_enable_visuals)
	{
		m_win_manager->assignTextMessageParameters(
			&m_offset_y_current_constraint_type,
			&m_text_index_current_constraint_type);
		m_win_manager->addTextMessage(
			m_offset_x_left, -m_offset_y_current_constraint_type,
			m_current_constraint_type,
			mrpt::img::TColorf(m_current_constraint_type_color),
			m_text_index_current_constraint_type);
	}

	// query node/edge deciders for visual objects initialization
	if (m_enable_visuals)
	{
		std::lock_guard<std::mutex> graph_lock(m_graph_section);
		m_time_logger.enter("Visuals");
		m_node_reg->initializeVisuals();
		m_edge_reg->initializeVisuals();
		m_optimizer->initializeVisuals();
		m_time_logger.leave("Visuals");
	}

	m_init_timestamp = INVALID_TIMESTAMP;

	// COccupancyGridMap2D Initialization
	{
		mrpt::maps::COccupancyGridMap2D::Ptr gridmap =
			mrpt::maps::COccupancyGridMap2D::Create();

		gridmap->setSize(
			/* min_x = */ -20.0f,
			/* float max_x = */ 20.0f,
			/* float min_y = */ -20.0f,
			/* float max_y = */ 20.0f,
			/* float resolution = */ 0.05f);

		// TODO - Read these from the .ini file
		// observation insertion options
		gridmap->insertionOptions.maxOccupancyUpdateCertainty = 0.8f;
		gridmap->insertionOptions.maxDistanceInsertion = 5;
		gridmap->insertionOptions.wideningBeamsWithDistance = true;
		gridmap->insertionOptions.decimation = 2;

		m_gridmap_cached = gridmap;
		m_map_is_cached = false;
	}

	// COctoMap Initialization
	{
		mrpt::maps::COctoMap::Ptr octomap = mrpt::maps::COctoMap::Create();

		// TODO - adjust the insertionoptions...
		// TODO - Read these from the .ini file
		octomap->insertionOptions.setOccupancyThres(0.5);
		octomap->insertionOptions.setProbHit(0.7);
		octomap->insertionOptions.setProbMiss(0.4);
		octomap->insertionOptions.setClampingThresMin(0.1192);
		octomap->insertionOptions.setClampingThresMax(0.971);

		m_octomap_cached = octomap;
		m_map_is_cached = false;
	}

	// In case we are given an RGBD TUM Dataset - try and read the info file so
	// that we know how to play back the GT poses.
	try
	{
		m_info_params.setRawlogFile(m_rawlog_fname);
		m_info_params.parseFile();
		// set the rate at which we read from the GT poses vector
		int num_of_objects = std::atoi(
			m_info_params.fields["Overall number of objects"].c_str());
		m_GT_poses_step = m_GT_poses.size() / num_of_objects;

		MRPT_LOG_INFO_STREAM(
			"Overall number of objects in rawlog: " << num_of_objects);
		MRPT_LOG_INFO_STREAM(
			"Setting the Ground truth read step to: " << m_GT_poses_step);
	}
	catch (const std::exception& e)
	{
		MRPT_LOG_INFO_STREAM("RGBD_TUM info file was not found: " << e.what());
	}

	// SLAM evaluation metric
	m_curr_deformation_energy = 0;
	if (m_visualize_SLAM_metric) { this->initSlamMetricVisualization(); }

	// Message to be displayed on pause
	if (m_enable_visuals)
	{
		this->m_win->addTextMessage(0.5, 0.3, "", m_text_index_paused_message);
	}

	MRPT_END
}  // end of initClass

template <class GRAPH_T>
bool CGraphSlamEngine<GRAPH_T>::execGraphSlamStep(
	mrpt::obs::CObservation::Ptr& observation, size_t& rawlog_entry)
{
	using namespace mrpt::obs;

	CActionCollection::Ptr action;
	CSensoryFrame::Ptr observations;

	return this->_execGraphSlamStep(
		action, observations, observation, rawlog_entry);
}  // end of execGraphSlamStep

template <class GRAPH_T>
bool CGraphSlamEngine<GRAPH_T>::_execGraphSlamStep(
	mrpt::obs::CActionCollection::Ptr& action,
	mrpt::obs::CSensoryFrame::Ptr& observations,
	mrpt::obs::CObservation::Ptr& observation, size_t& rawlog_entry)
{
	MRPT_START

	using namespace std;
	using namespace mrpt;
	using namespace mrpt::poses;
	using namespace mrpt::obs;
	using namespace mrpt::obs::utils;
	using namespace mrpt::opengl;
	using namespace mrpt::system;

	m_time_logger.enter("proc_time");

	ASSERTDEBMSG_(
		m_has_read_config, "\nConfig file is not read yet.\nExiting... \n");
	// good to go..

	// read first measurement independently if we haven't already
	if (m_init_timestamp == INVALID_TIMESTAMP)
	{
		m_init_timestamp = getTimeStamp(action, observations, observation);
		MRPT_LOG_DEBUG_STREAM("execGraphSlamStep: first run");

		if (observation)
		{
			MRPT_LOG_DEBUG_STREAM("Observation only dataset!");
			m_observation_only_dataset = true;	// false by default
		}
		else
		{
			MRPT_LOG_DEBUG_STREAM("Action-observation dataset!");
			ASSERTDEB_(action);
			m_observation_only_dataset = false;

			CPose3D increment_pose_3d;
			action->getFirstMovementEstimationMean(increment_pose_3d);
			pose_t increment_pose(increment_pose_3d);
			m_curr_odometry_only_pose += increment_pose;
		}

		// TODO enable this and test this.
		// return true;
	}

	// NRD
	bool registered_new_node;
	{
		std::lock_guard<std::mutex> graph_lock(m_graph_section);
		m_time_logger.enter("node_registrar");
		registered_new_node =
			m_node_reg->updateState(action, observations, observation);
		m_time_logger.leave("node_registrar");
	}

	{  // get the 2D laser scan, if there
		CObservation2DRangeScan::Ptr scan =
			getObservation<CObservation2DRangeScan>(observations, observation);
		if (scan)
		{
			m_last_laser_scan2D = scan;

			if (!m_first_laser_scan2D)
			{  // capture first laser scan seperately
				m_first_laser_scan2D = m_last_laser_scan2D;
			}
		}
	}

	if (registered_new_node)
	{
		// At the first node registration, must have registered exactly 2 nodes
		// (root + first)
		if (m_is_first_time_node_reg)
		{
			std::lock_guard<std::mutex> graph_lock(m_graph_section);
			m_nodeID_max = 0;
			if (m_graph.nodeCount() != 2)
			{
				MRPT_LOG_ERROR_STREAM(
					"Expected [2] new registered nodes"
					<< " but got [" << m_graph.nodeCount() << "]");
				THROW_EXCEPTION("Illegal node registration");
			}

			m_nodes_to_laser_scans2D.insert(
				make_pair(m_nodeID_max, m_first_laser_scan2D));

			m_is_first_time_node_reg = false;
		}

		// going to be incremented in monitorNodeRegistration anyway.
		m_nodeID_max++;

		// either way add just one odometry edge
		m_edge_counter.addEdge("Odometry");
	}
	this->monitorNodeRegistration(
		registered_new_node, "NodeRegistrationDecider");

	// Edge registration procedure - Optimization
	// run this so that the ERD, GSO can be updated with the latest
	// observations even when no new nodes have been added to the graph
	{  // ERD
		std::lock_guard<std::mutex> graph_lock(m_graph_section);

		m_time_logger.enter("edge_registrar");
		m_edge_reg->updateState(action, observations, observation);
		m_time_logger.leave("edge_registrar");
	}
	this->monitorNodeRegistration(
		registered_new_node, "EdgeRegistrationDecider");

	{  // GSO
		std::lock_guard<std::mutex> graph_lock(m_graph_section);

		m_time_logger.enter("optimizer");
		m_optimizer->updateState(action, observations, observation);
		m_time_logger.leave("optimizer");
	}
	this->monitorNodeRegistration(registered_new_node, "GraphSlamOptimizer");

	// current timestamp - to be filled depending on the format
	m_curr_timestamp = getTimeStamp(action, observations, observation);

	if (observation)
	{
		// Read a single observation from the rawlog
		// (Format #2 rawlog file)

		// odometry
		if (IS_CLASS(*observation, CObservationOdometry))
		{
			CObservationOdometry::Ptr obs_odometry =
				std::dynamic_pointer_cast<CObservationOdometry>(observation);

			m_curr_odometry_only_pose = pose_t(obs_odometry->odometry);
			m_odometry_poses.push_back(m_curr_odometry_only_pose);
		}
		else if (IS_CLASS(*observation, CObservation3DRangeScan))
		{
			m_last_laser_scan3D =
				std::dynamic_pointer_cast<mrpt::obs::CObservation3DRangeScan>(
					observation);
		}
	}
	else
	{
		// action, observations should contain a pair of valid data
		// (Format #1 rawlog file)
		ASSERTDEB_(observations);
		ASSERTDEB_(action);

		CPose3D increment_pose_3d;
		action->getFirstMovementEstimationMean(increment_pose_3d);
		pose_t increment_pose(increment_pose_3d);
		m_curr_odometry_only_pose += increment_pose;
		m_odometry_poses.push_back(m_curr_odometry_only_pose);
	}  // ELSE FORMAT #1 - Action/Observations

	if (registered_new_node)
	{
		this->execDijkstraNodesEstimation();

		// keep track of the laser scans so that I can later visualize the map
		m_nodes_to_laser_scans2D[m_nodeID_max] = m_last_laser_scan2D;

		if (m_enable_visuals && m_visualize_map)
		{
			std::lock_guard<std::mutex> graph_lock(m_graph_section);
			// bool full_update = m_optimizer->justFullyOptimizedGraph();
			bool full_update = true;
			this->updateMapVisualization(m_nodes_to_laser_scans2D, full_update);
		}

		// query node/edge deciders for visual objects update
		if (m_enable_visuals) { this->updateAllVisuals(); }

		// update the edge counter
		std::map<std::string, int> edge_types_to_nums;
		m_edge_reg->getEdgesStats(&edge_types_to_nums);
		if (edge_types_to_nums.size())
		{
			for (auto it = edge_types_to_nums.begin();
				 it != edge_types_to_nums.end(); ++it)
			{
				// loop closure
				if (mrpt::system::strCmpI(it->first, "lc"))
				{
					m_edge_counter.setLoopClosureEdgesManually(it->second);
				}
				else
				{
					m_edge_counter.setEdgesManually(it->first, it->second);
				}
			}
		}

		// update the graph visualization

		if (m_enable_curr_pos_viewport) { updateCurrPosViewport(); }
		// update visualization of estimated trajectory
		if (m_enable_visuals)
		{
			bool full_update = true;  // don't care, do it anyway.
			m_time_logger.enter("Visuals");
			this->updateEstimatedTrajectoryVisualization(full_update);
			m_time_logger.leave("Visuals");
		}

		// refine the SLAM metric  and update its corresponding visualization
		// This is done only when the GT is available.
		if (m_use_GT)
		{
			m_time_logger.enter("SLAM_metric");
			this->computeSlamMetric(m_nodeID_max, m_GT_poses_index);
			m_time_logger.leave("SLAM_metric");
			if (m_visualize_SLAM_metric)
			{
				m_time_logger.enter("Visuals");
				this->updateSlamMetricVisualization();
				m_time_logger.leave("Visuals");
			}
		}

		// mark the map outdated
		m_map_is_cached = false;

	}  // IF REGISTERED_NEW_NODE

	//
	// Visualization related actions
	//
	m_time_logger.enter("Visuals");
	// Timestamp textMessage
	// Use the dataset timestamp otherwise fallback to
	// mrpt::system::getCurrentTime
	if (m_enable_visuals)
	{
		if (m_curr_timestamp != INVALID_TIMESTAMP)
		{
			m_win_manager->addTextMessage(
				m_offset_x_left, -m_offset_y_timestamp,
				format(
					"Simulated time: %s",
					timeToString(m_curr_timestamp).c_str()),
				mrpt::img::TColorf(1.0, 1.0, 1.0),
				/* unique_index = */ m_text_index_timestamp);
		}
		else
		{
			m_win_manager->addTextMessage(
				m_offset_x_left, -m_offset_y_timestamp,
				format(
					"Wall time: %s",
					timeToString(mrpt::system::getCurrentTime()).c_str()),
				mrpt::img::TColorf(1.0, 1.0, 1.0),
				/* unique_index = */ m_text_index_timestamp);
		}
	}

	// Odometry visualization
	if (m_visualize_odometry_poses && m_odometry_poses.size())
	{
		this->updateOdometryVisualization();
	}

	// ensure that the GT is visualized at the same rate as the SLAM procedure
	// handle RGBD-TUM datasets manually. Advance the GT index accordingly
	if (m_use_GT)
	{
		if (mrpt::system::strCmpI(m_GT_file_format, "rgbd_tum"))
		{  // 1/loop
			if (m_enable_visuals)
			{
				this->updateGTVisualization();	// I have already taken care of
				// the step
			}
			m_GT_poses_index += m_GT_poses_step;
		}
		else if (mrpt::system::strCmpI(m_GT_file_format, "navsimul"))
		{
			if (m_observation_only_dataset)
			{  // 1/2loops
				if (rawlog_entry % 2 == 0)
				{
					if (m_enable_visuals)
					{
						this->updateGTVisualization();	// I have already taken
						// care of the step
					}
					m_GT_poses_index += m_GT_poses_step;
				}
			}
			else
			{  // 1/loop
				// get both action and observation at a single step - same rate
				// as GT
				if (m_enable_visuals)
				{
					this->updateGTVisualization();	// I have already taken care
					// of the step
				}
				m_GT_poses_index += m_GT_poses_step;
			}
		}
	}

	// 3DRangeScans viewports update
	if (mrpt::system::strCmpI(m_GT_file_format, "rgbd_tum"))
	{
		if (m_enable_range_viewport && m_last_laser_scan3D)
		{
			this->updateRangeImageViewport();
		}

		if (m_enable_intensity_viewport && m_last_laser_scan3D)
		{
			this->updateIntensityImageViewport();
		}
	}

	// Query for events and take corresponding actions
	if (m_enable_visuals) { this->queryObserverForEvents(); }
	m_time_logger.leave("Visuals");

	m_dataset_grab_time =
		mrpt::system::timeDifference(m_init_timestamp, m_curr_timestamp);
	m_time_logger.leave("proc_time");

	return !m_request_to_exit;
	MRPT_END
}  // end of _execGraphSlamStep

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::execDijkstraNodesEstimation()
{
	{
		std::lock_guard<std::mutex> graph_lock(m_graph_section);
		m_time_logger.enter("dijkstra_nodes_estimation");
		m_graph.dijkstra_nodes_estimate();
		m_time_logger.leave("dijkstra_nodes_estimation");
	}
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::monitorNodeRegistration(
	bool registered /*=false*/, std::string class_name /*="Class"*/)
{
	MRPT_START
	using namespace mrpt;

	std::lock_guard<std::mutex> graph_lock(m_graph_section);
	size_t listed_nodeCount =
		(m_nodeID_max == mrpt::graphs::INVALID_NODEID ? 0 : m_nodeID_max + 1);

	if (!registered)
	{  // just check that it's the same.
		ASSERTDEBMSG_(
			listed_nodeCount == m_graph.nodeCount(),
			format(
				"listed_nodeCount [%lu] != nodeCount() [%lu]",
				static_cast<unsigned long>(listed_nodeCount),
				static_cast<unsigned long>(m_graph.nodeCount())));
	}
	else
	{
		if (listed_nodeCount != m_graph.nodeCount())
		{
			MRPT_LOG_ERROR_STREAM(
				class_name << " illegally added new nodes to the graph "
						   << ", wanted to see [" << listed_nodeCount
						   << "] but saw [" << m_graph.nodeCount() << "]");
			THROW_EXCEPTION(
				format("Illegal node registration by %s.", class_name.c_str()));
		}
	}
	MRPT_END
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::getMap(
	mrpt::maps::COccupancyGridMap2D::Ptr map,
	mrpt::system::TTimeStamp* acquisition_time) const
{
	MRPT_START

	if (!map) { map = mrpt::maps::COccupancyGridMap2D::Create(); }
	ASSERTDEB_(map);

	if (!m_map_is_cached) { this->computeMap(); }
	map->copyMapContentFrom(*m_gridmap_cached);

	// fill the timestamp if this is given
	if (acquisition_time) { *acquisition_time = m_map_acq_time; }
	MRPT_END
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::getMap(
	mrpt::maps::COctoMap::Ptr map,
	mrpt::system::TTimeStamp* acquisition_time) const
{
	MRPT_START
	THROW_EXCEPTION("Not Implemented Yet.");

	if (!m_map_is_cached) { this->computeMap(); }
	//	map =
	// dynamic_cast<mrpt::maps::COctoMap::Ptr>(m_octomap_cached->clone());
	ASSERTDEB_(map);

	// fill the timestamp if this is given
	if (acquisition_time) { *acquisition_time = m_map_acq_time; }

	MRPT_END
}

template <class GRAPH_T>
inline void CGraphSlamEngine<GRAPH_T>::computeMap() const
{
	MRPT_START
	using namespace std;
	using namespace mrpt;
	using namespace mrpt::maps;
	using namespace mrpt::poses;

	std::lock_guard<std::mutex> graph_lock(m_graph_section);

	if (!constraint_t::is_3D())
	{  // 2D Poses
		// MRPT_LOG_DEBUG_STREAM("Computing the occupancy gridmap...");
		mrpt::maps::COccupancyGridMap2D::Ptr gridmap = m_gridmap_cached;
		gridmap->clear();

		// traverse all the nodes - add their laser scans at their corresponding
		// poses
		for (auto it = m_nodes_to_laser_scans2D.begin();
			 it != m_nodes_to_laser_scans2D.end(); ++it)
		{
			const mrpt::graphs::TNodeID& curr_node = it->first;

			// fetch LaserScan
			const mrpt::obs::CObservation2DRangeScan::Ptr& curr_laser_scan =
				it->second;
			ASSERTDEBMSG_(
				curr_laser_scan,
				format(
					"LaserScan of nodeID: %lu is not present.",
					static_cast<unsigned long>(curr_node)));

			// Fetch pose at which to display the LaserScan
			CPose3D scan_pose = getLSPoseForGridMapVisualization(curr_node);

			// Add all to gridmap
			gridmap->insertObservation(*curr_laser_scan, scan_pose);
		}

		m_map_is_cached = true;
		m_map_acq_time = mrpt::system::now();
	}
	else
	{  // 3D Pose
		// MRPT_LOG_DEBUG_STREAM("Computing the Octomap...");
		THROW_EXCEPTION("Not Implemented Yet. Method is to compute a COctoMap");
		// MRPT_LOG_DEBUG_STREAM("Computed COctoMap successfully.");
	}

	MRPT_END
}  // end of computeMap

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::loadParams(const std::string& fname)
{
	MRPT_START

	ASSERTDEBMSG_(
		mrpt::system::fileExists(fname),
		mrpt::format("\nConfiguration file not found: \n%s\n", fname.c_str()));

	mrpt::config::CConfigFile cfg_file(fname);

	// Section: GeneralConfiguration

	m_user_decides_about_output_dir = cfg_file.read_bool(
		"GeneralConfiguration", "user_decides_about_output_dir", false, false);
	m_GT_file_format = cfg_file.read_string(
		"GeneralConfiguration", "ground_truth_file_format", "NavSimul", false);

	// Minimum verbosity level of the logger
	int min_verbosity_level =
		cfg_file.read_int("GeneralConfiguration", "class_verbosity", 1, false);
	this->setMinLoggingLevel(mrpt::system::VerbosityLevel(min_verbosity_level));

	// Section: VisualizationParameters

	// map visualization
	m_visualize_map = cfg_file.read_bool(
		"VisualizationParameters", "visualize_map", true, false);

	// odometry-only visualization
	m_visualize_odometry_poses = cfg_file.read_bool(
		"VisualizationParameters", "visualize_odometry_poses", true, false);
	m_visualize_estimated_trajectory = cfg_file.read_bool(
		"VisualizationParameters", "visualize_estimated_trajectory", true,
		false);

	// GT configuration visualization
	m_visualize_GT = cfg_file.read_bool(
		"VisualizationParameters", "visualize_ground_truth", true, false);
	// SLAM metric plot
	m_visualize_SLAM_metric = cfg_file.read_bool(
		"VisualizationParameters", "visualize_SLAM_metric", true, false);

	// Viewports flags
	m_enable_curr_pos_viewport = cfg_file.read_bool(
		"VisualizationParameters", "enable_curr_pos_viewport", true, false);
	m_enable_range_viewport = cfg_file.read_bool(
		"VisualizationParameters", "enable_range_viewport", false, false);
	m_enable_intensity_viewport = cfg_file.read_bool(
		"VisualizationParameters", "enable_intensity_viewport", false, false);

	m_node_reg->loadParams(fname);
	m_edge_reg->loadParams(fname);
	m_optimizer->loadParams(fname);

	m_has_read_config = true;
	MRPT_END
}
template <class GRAPH_T>
std::string CGraphSlamEngine<GRAPH_T>::getParamsAsString() const
{
	MRPT_START

	std::string str;
	this->getParamsAsString(&str);
	return str;

	MRPT_END
}
template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::getParamsAsString(std::string* params_out) const
{
	MRPT_START
	ASSERTDEB_(m_has_read_config);
	using namespace std;

	stringstream ss_out;

	ss_out
		<< "\n------------[ Graphslam_engine Problem Parameters ]------------"
		<< std::endl;
	ss_out << "Config filename                 = " << m_config_fname
		   << std::endl;

	ss_out << "Ground Truth File format        = " << m_GT_file_format
		   << std::endl;
	ss_out << "Ground Truth filename           = " << m_fname_GT << std::endl;

	ss_out << "Visualize odometry              = "
		   << (m_visualize_odometry_poses ? "TRUE" : "FALSE") << std::endl;
	ss_out << "Visualize estimated trajectory  = "
		   << (m_visualize_estimated_trajectory ? "TRUE" : "FALSE")
		   << std::endl;
	ss_out << "Visualize map                   = "
		   << (m_visualize_map ? "TRUE" : "FALSE") << std::endl;
	ss_out << "Visualize Ground Truth          = "
		   << (m_visualize_GT ? "TRUE" : "FALSE") << std::endl;

	ss_out << "Visualize SLAM metric plot      = "
		   << (m_visualize_SLAM_metric ? "TRUE" : "FALSE") << std::endl;

	ss_out << "Enable curr. position viewport  = "
		   << (m_enable_curr_pos_viewport ? "TRUE" : "FALSE") << endl;
	ss_out << "Enable range img viewport       = "
		   << (m_enable_range_viewport ? "TRUE" : "FALSE") << endl;
	ss_out << "Enable intensity img viewport   = "
		   << (m_enable_intensity_viewport ? "TRUE" : "FALSE") << endl;

	ss_out << "-----------------------------------------------------------"
		   << std::endl;
	ss_out << std::endl;

	// copy the stringstream contents to the passed in string
	*params_out = ss_out.str();

	MRPT_END
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::printParams() const
{
	MRPT_START
	std::cout << getParamsAsString();

	m_node_reg->printParams();
	m_edge_reg->printParams();
	m_optimizer->printParams();

	MRPT_END
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::initResultsFile(const std::string& fname)
{
	MRPT_START
	using namespace std;
	using namespace mrpt::system;

	MRPT_LOG_INFO_STREAM("Setting up file: " << fname);

	// current time vars
	mrpt::system::TTimeStamp cur_date(getCurrentTime());
	std::string time_spec = "UTC Time";
	string cur_date_str(dateTimeToString(cur_date));
	string cur_date_validstr(fileNameStripInvalidChars(cur_date_str));

	m_out_streams[fname].open(fname);
	ASSERTDEBMSG_(
		m_out_streams[fname].fileOpenCorrectly(),
		mrpt::format("\nError while trying to open %s\n", fname.c_str()));

	const std::string sep(80, '#');

	m_out_streams[fname].printf("# Mobile Robot Programming Toolkit (MRPT)\n");
	m_out_streams[fname].printf("# http::/www.mrpt.org\n");
	m_out_streams[fname].printf("# GraphSlamEngine Application\n");
	m_out_streams[fname].printf(
		"# Automatically generated file - %s: %s\n", time_spec.c_str(),
		cur_date_str.c_str());
	m_out_streams[fname].printf("%s\n\n", sep.c_str());

	MRPT_END
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::initRangeImageViewport()
{
	MRPT_START
	ASSERTDEB_(m_enable_visuals);
	using namespace mrpt::opengl;

	Scene::Ptr scene = m_win->get3DSceneAndLock();
	Viewport::Ptr viewp_range;

	viewp_range = scene->createViewport("viewp_range");
	double x, y, h, w;
	m_win_manager->assignViewportParameters(&x, &y, &w, &h);
	viewp_range->setViewportPosition(x, y, h, w);

	m_win->unlockAccess3DScene();
	m_win->forceRepaint();

	MRPT_END
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::updateAllVisuals()
{
	MRPT_START
	std::lock_guard<std::mutex> graph_lock(m_graph_section);
	m_time_logger.enter("Visuals");

	m_node_reg->updateVisuals();
	m_edge_reg->updateVisuals();
	m_optimizer->updateVisuals();

	m_time_logger.leave("Visuals");
	MRPT_END
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::updateRangeImageViewport()
{
	MRPT_START
	ASSERTDEB_(m_enable_visuals);
	using namespace mrpt::math;
	using namespace mrpt::opengl;

	if (m_last_laser_scan3D->hasRangeImage)
	{
		// TODO - make this a static class member - or at least a private member
		// of the class

		mrpt::img::CImage img;

		// load the image if not already loaded..
		m_last_laser_scan3D->load();
		img.setFromMatrix(
			m_last_laser_scan3D->rangeImage, false /* do normalize */);

		Scene::Ptr scene = m_win->get3DSceneAndLock();
		Viewport::Ptr viewp_range = scene->getViewport("viewp_range");
		viewp_range->setImageView(img);
		m_win->unlockAccess3DScene();
		m_win->forceRepaint();
	}

	MRPT_END
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::initIntensityImageViewport()
{
	MRPT_START
	ASSERTDEB_(m_enable_visuals);
	using namespace mrpt::opengl;

	Scene::Ptr scene = m_win->get3DSceneAndLock();
	Viewport::Ptr viewp_intensity;

	viewp_intensity = scene->createViewport("viewp_intensity");
	double x, y, w, h;
	m_win_manager->assignViewportParameters(&x, &y, &w, &h);
	viewp_intensity->setViewportPosition(x, y, w, h);

	m_win->unlockAccess3DScene();
	m_win->forceRepaint();

	MRPT_END
}
template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::updateIntensityImageViewport()
{
	MRPT_START
	ASSERTDEB_(m_enable_visuals);
	using namespace mrpt::opengl;

	if (m_last_laser_scan3D->hasIntensityImage)
	{
		mrpt::img::CImage img;

		// load the image if not already loaded..
		m_last_laser_scan3D->load();
		img = m_last_laser_scan3D->intensityImage;

		Scene::Ptr scene = m_win->get3DSceneAndLock();
		Viewport::Ptr viewp_intensity = scene->getViewport("viewp_intensity");
		viewp_intensity->setImageView(img);
		m_win->unlockAccess3DScene();
		m_win->forceRepaint();
	}

	MRPT_END
}  // end of updateIntensityImageViewport

template <class GRAPH_T>
mrpt::opengl::CSetOfObjects::Ptr
	CGraphSlamEngine<GRAPH_T>::initRobotModelVisualization()
{
	pose_t p;
	return this->initRobotModelVisualizationInternal(p);
}  // end of initRobotModelVisualization

template <class GRAPH_T>
mrpt::opengl::CSetOfObjects::Ptr
	CGraphSlamEngine<GRAPH_T>::initRobotModelVisualizationInternal(
		const mrpt::poses::CPose2D& p_unused)
{
	return mrpt::opengl::stock_objects::RobotPioneer();

}  // end of initRobotModelVisualizationInternal - CPose2D

template <class GRAPH_T>
mrpt::opengl::CSetOfObjects::Ptr
	CGraphSlamEngine<GRAPH_T>::initRobotModelVisualizationInternal(
		const mrpt::poses::CPose3D& p_unused)
{
	return mrpt::opengl::stock_objects::CornerXYZ();
}  // end of initRobotModelVisualizationInternal - CPose3D

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::initCurrPosViewport()
{
	MRPT_START
	ASSERTDEB_(m_enable_visuals);
	using namespace mrpt::opengl;

	Scene::Ptr scene = m_win->get3DSceneAndLock();
	Viewport::Ptr viewp = scene->createViewport("curr_robot_pose_viewport");
	// Add a clone viewport, using [0,1] factor X,Y,Width,Height coordinates:
	viewp->setCloneView("main");
	double x, y, h, w;
	m_win_manager->assignViewportParameters(&x, &y, &w, &h);
	viewp->setViewportPosition(x, y, h, w);
	viewp->setTransparent(false);
	viewp->getCamera().setAzimuthDegrees(90);
	viewp->getCamera().setElevationDegrees(90);
	viewp->setCustomBackgroundColor(
		mrpt::img::TColorf(205, 193, 197, /*alpha = */ 255));
	viewp->getCamera().setZoomDistance(30);
	viewp->getCamera().setOrthogonal();

	m_win->unlockAccess3DScene();
	m_win->forceRepaint();

	MRPT_END
}

template <class GRAPH_T>
inline void CGraphSlamEngine<GRAPH_T>::updateCurrPosViewport()
{
	MRPT_START
	ASSERTDEB_(m_enable_visuals);
	using namespace mrpt::opengl;
	using namespace mrpt::poses;

	ASSERTDEB_(m_enable_visuals);

	global_pose_t curr_robot_pose = this->getCurrentRobotPosEstimation();

	Scene::Ptr scene = m_win->get3DSceneAndLock();
	Viewport::Ptr viewp = scene->getViewport("curr_robot_pose_viewport");
	viewp->getCamera().setPointingAt(CPose3D(curr_robot_pose));

	m_win->unlockAccess3DScene();
	m_win->forceRepaint();
	MRPT_LOG_DEBUG_STREAM("Updated the \"current_pos\" viewport");

	MRPT_END
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::readGTFile(
	const std::string& fname_GT, std::vector<mrpt::poses::CPose2D>* gt_poses,
	std::vector<mrpt::system::TTimeStamp>* gt_timestamps)
{
	MRPT_START
	using namespace std;
	using namespace mrpt::system;

	// make sure file exists
	ASSERTDEBMSG_(
		fileExists(fname_GT),
		format(
			"\nGround-truth file %s was not found.\n"
			"Either specify a valid ground-truth filename or set set the "
			"m_visualize_GT flag to false\n",
			fname_GT.c_str()));

	mrpt::io::CFileInputStream file_GT(fname_GT);
	ASSERTDEBMSG_(file_GT.fileOpenCorrectly(), "\nCouldn't open GT file\n");
	ASSERTDEBMSG_(gt_poses, "\nNo valid std::vector<pose_t>* was given\n");

	string curr_line;

	// parse the file - get timestamp and pose and fill in the pose_t vector
	for (size_t line_num = 0; file_GT.readLine(curr_line); line_num++)
	{
		vector<string> curr_tokens;
		system::tokenize(curr_line, " ", curr_tokens);

		// check the current pose dimensions
		ASSERTDEBMSG_(
			curr_tokens.size() == constraint_t::state_length + 1,
			"\nGround Truth File doesn't seem to contain data as generated by "
			"the "
			"GridMapNavSimul application.\n Either specify the GT file format "
			"or set "
			"visualize_ground_truth to false in the .ini file\n");

		// timestamp
		if (gt_timestamps)
		{
			auto timestamp =
				mrpt::Clock::fromDouble(atof(curr_tokens[0].c_str()));
			gt_timestamps->push_back(timestamp);
		}

		// pose
		pose_t curr_pose(
			atof(curr_tokens[1].c_str()), atof(curr_tokens[2].c_str()),
			atof(curr_tokens[3].c_str()));
		gt_poses->push_back(curr_pose);
	}

	file_GT.close();

	MRPT_END
}
template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::readGTFile(
	const std::string& fname_GT, std::vector<mrpt::poses::CPose3D>* gt_poses,
	std::vector<mrpt::system::TTimeStamp>* gt_timestamps)
{
	THROW_EXCEPTION("Not implemented.");
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::readGTFileRGBD_TUM(
	const std::string& fname_GT, std::vector<mrpt::poses::CPose2D>* gt_poses,
	std::vector<mrpt::system::TTimeStamp>* gt_timestamps)
{
	MRPT_START
	using namespace std;
	using namespace mrpt::math;
	using namespace mrpt::system;

	// make sure file exists
	ASSERTDEBMSG_(
		fileExists(fname_GT),
		format(
			"\nGround-truth file %s was not found.\n"
			"Either specify a valid ground-truth filename or set set the "
			"m_visualize_GT flag to false\n",
			fname_GT.c_str()));

	mrpt::io::CFileInputStream file_GT(fname_GT);
	ASSERTDEBMSG_(
		file_GT.fileOpenCorrectly(),
		"\nreadGTFileRGBD_TUM: Couldn't openGT file\n");
	ASSERTDEBMSG_(gt_poses, "No valid std::vector<pose_t>* was given");

	string curr_line;

	// move to the first non-commented immediately - comments before this..
	for (size_t i = 0; file_GT.readLine(curr_line); i++)
	{
		if (curr_line.at(0) != '#') { break; }
	}

	// handle the first pose seperately
	// make sure that the ground-truth starts at 0.
	pose_t pose_diff;
	vector<string> curr_tokens;
	mrpt::system::tokenize(curr_line, " ", curr_tokens);

	// check the current pose dimensions
	ASSERTDEBMSG_(
		curr_tokens.size() == 8,
		"\nGround Truth File doesn't seem to contain data as specified in "
		"RGBD_TUM related "
		"datasets.\n Either specify correct the GT file format or set "
		"visualize_ground_truth to false in the .ini file\n");

	// quaternion
	CQuaternionDouble quat;
	quat.r(atof(curr_tokens[7].c_str()));
	quat.x(atof(curr_tokens[4].c_str()));
	quat.y(atof(curr_tokens[5].c_str()));
	quat.z(atof(curr_tokens[6].c_str()));
	double r, p, y;
	quat.rpy(r, p, y);

	CVectorDouble curr_coords(3);
	curr_coords[0] = atof(curr_tokens[1].c_str());
	curr_coords[1] = atof(curr_tokens[2].c_str());
	curr_coords[2] = atof(curr_tokens[3].c_str());

	// initial pose
	pose_t curr_pose(curr_coords[0], curr_coords[1], y);
	// pose_t curr_pose(0, 0, 0);

	pose_diff = curr_pose;

	// parse the file - get timestamp and pose and fill in the pose_t vector
	for (; file_GT.readLine(curr_line);)
	{
		system::tokenize(curr_line, " ", curr_tokens);
		ASSERTDEBMSG_(
			curr_tokens.size() == 8,
			"\nGround Truth File doesn't seem to contain data as specified in "
			"RGBD_TUM related "
			"datasets.\n Either specify correct the GT file format or set "
			"visualize_ground_truth to false in the .ini file\n");

		// timestamp
		if (gt_timestamps)
		{
			auto timestamp =
				mrpt::Clock::fromDouble(atof(curr_tokens[0].c_str()));
			gt_timestamps->push_back(timestamp);
		}

		// quaternion
		quat.r(atof(curr_tokens[7].c_str()));
		quat.x(atof(curr_tokens[4].c_str()));
		quat.y(atof(curr_tokens[5].c_str()));
		quat.z(atof(curr_tokens[6].c_str()));
		quat.rpy(r, p, y);

		// pose
		curr_coords[0] = atof(curr_tokens[1].c_str());
		curr_coords[1] = atof(curr_tokens[2].c_str());
		curr_coords[2] = atof(curr_tokens[3].c_str());

		// current ground-truth pose
		pose_t gt_pose(curr_coords[0], curr_coords[1], y);

		gt_pose.x() -= pose_diff.x();
		gt_pose.y() -= pose_diff.y();
		gt_pose.phi() -= pose_diff.phi();
		// curr_pose += -pose_diff;
		gt_poses->push_back(gt_pose);
	}

	file_GT.close();

	MRPT_END
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::alignOpticalWithMRPTFrame()
{
	MRPT_START
	using namespace std;
	using namespace mrpt::math;

	// aligning GT (optical) frame with the MRPT frame
	// Set the rotation matrix from the corresponding RPY angles
	// MRPT Frame: X->forward; Y->Left; Z->Upward
	// Optical Frame: X->Right; Y->Downward; Z->Forward
	ASSERTDEB_(m_has_read_config);
	// rotz
	double anglez = 0.0_deg;
	const double tmpz[] = {
		cos(anglez), -sin(anglez), 0, sin(anglez), cos(anglez), 0, 0, 0, 1};
	CMatrixDouble rotz(3, 3, tmpz);

	// roty
	double angley = 0.0_deg;
	// double angley = 90.0_deg;
	const double tmpy[] = {cos(angley),	 0, sin(angley), 0, 1, 0,
						   -sin(angley), 0, cos(angley)};
	CMatrixDouble roty(3, 3, tmpy);

	// rotx
	// double anglex = -90.0_deg;
	double anglex = 0.0_deg;
	const double tmpx[] = {
		1, 0, 0, 0, cos(anglex), -sin(anglex), 0, sin(anglex), cos(anglex)};
	CMatrixDouble rotx(3, 3, tmpx);

	stringstream ss_out;
	ss_out << "\nConstructing the rotation matrix for the GroundTruth Data..."
		   << endl;
	m_rot_TUM_to_MRPT = rotz * roty * rotx;

	ss_out << "Rotation matrices for optical=>MRPT transformation" << endl;
	ss_out << "rotz: " << endl << rotz << endl;
	ss_out << "roty: " << endl << roty << endl;
	ss_out << "rotx: " << endl << rotx << endl;
	ss_out << "Full rotation matrix: " << endl << m_rot_TUM_to_MRPT << endl;

	MRPT_LOG_DEBUG_STREAM(ss_out);

	MRPT_END
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::queryObserverForEvents()
{
	MRPT_START
	ASSERTDEB_(m_enable_visuals);
	ASSERTDEBMSG_(
		m_win_observer,
		"\nqueryObserverForEvents method was called even though no Observer "
		"object was provided\n");

	std::map<std::string, bool> events_occurred;
	m_win_observer->returnEventsStruct(&events_occurred);
	m_request_to_exit = events_occurred.find("Ctrl+c")->second;

	// odometry visualization
	if (events_occurred[m_keystroke_odometry])
	{
		this->toggleOdometryVisualization();
	}
	// GT visualization
	if (events_occurred[m_keystroke_GT]) { this->toggleGTVisualization(); }
	// Map visualization
	if (events_occurred[m_keystroke_map]) { this->toggleMapVisualization(); }
	// Estimated Trajectory Visualization
	if (events_occurred[m_keystroke_estimated_trajectory])
	{
		this->toggleEstimatedTrajectoryVisualization();
	}
	// pause/resume program execution
	if (events_occurred[m_keystroke_pause_exec]) { this->togglePause(); }

	// notify the deciders/optimizer of any events they may be interested in
	// MRPT_LOG_DEBUG_STREAM("Notifying deciders/optimizer for events");
	m_node_reg->notifyOfWindowEvents(events_occurred);
	m_edge_reg->notifyOfWindowEvents(events_occurred);
	m_optimizer->notifyOfWindowEvents(events_occurred);

	MRPT_END
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::toggleOdometryVisualization()
{
	MRPT_START
	ASSERTDEB_(m_enable_visuals);
	using namespace mrpt::opengl;
	MRPT_LOG_INFO_STREAM("Toggling Odometry visualization...");

	Scene::Ptr scene = m_win->get3DSceneAndLock();

	if (m_visualize_odometry_poses)
	{
		CRenderizable::Ptr obj = scene->getByName("odometry_poses_cloud");
		obj->setVisibility(!obj->isVisible());

		obj = scene->getByName("robot_odometry_poses");
		obj->setVisibility(!obj->isVisible());
	}
	else
	{
		dumpVisibilityErrorMsg("visualize_odometry_poses");
	}

	m_win->unlockAccess3DScene();
	m_win->forceRepaint();

	MRPT_END
}
template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::toggleGTVisualization()
{
	MRPT_START
	ASSERTDEB_(m_enable_visuals);
	using namespace mrpt::opengl;
	MRPT_LOG_INFO_STREAM("Toggling Ground Truth visualization");

	Scene::Ptr scene = m_win->get3DSceneAndLock();

	if (m_visualize_GT)
	{
		CRenderizable::Ptr obj = scene->getByName("GT_cloud");
		obj->setVisibility(!obj->isVisible());

		obj = scene->getByName("robot_GT");
		obj->setVisibility(!obj->isVisible());
	}
	else
	{
		dumpVisibilityErrorMsg("visualize_ground_truth");
	}

	m_win->unlockAccess3DScene();
	m_win->forceRepaint();

	MRPT_END
}
template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::toggleMapVisualization()
{
	MRPT_START
	ASSERTDEB_(m_enable_visuals);
	using namespace std;
	using namespace mrpt::opengl;
	MRPT_LOG_INFO_STREAM("Toggling Map visualization... ");

	Scene::Ptr scene = m_win->get3DSceneAndLock();

	// get total number of nodes
	int num_of_nodes;
	{
		std::lock_guard<std::mutex> graph_lock(m_graph_section);
		num_of_nodes = m_graph.nodeCount();
	}

	// name of gui object
	stringstream scan_name("");

	for (int node_cnt = 0; node_cnt != num_of_nodes; ++node_cnt)
	{
		// build the name of the potential corresponding object in the scene
		scan_name.str("");
		scan_name << "laser_scan_";
		scan_name << node_cnt;

		CRenderizable::Ptr obj = scene->getByName(scan_name.str());
		// current node may not have laserScans => may not have corresponding
		// obj
		if (obj) { obj->setVisibility(!obj->isVisible()); }
	}
	m_win->unlockAccess3DScene();
	m_win->forceRepaint();

	MRPT_END
}
template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::toggleEstimatedTrajectoryVisualization()
{
	MRPT_START
	ASSERTDEB_(m_enable_visuals);
	using namespace mrpt::opengl;
	MRPT_LOG_INFO_STREAM("Toggling Estimated Trajectory visualization... ");

	Scene::Ptr scene = m_win->get3DSceneAndLock();

	if (m_visualize_estimated_trajectory)
	{
		CRenderizable::Ptr obj = scene->getByName("estimated_traj_setoflines");
		obj->setVisibility(!obj->isVisible());

		obj = scene->getByName("robot_estimated_traj");
		obj->setVisibility(!obj->isVisible());
	}
	else
	{
		dumpVisibilityErrorMsg("visualize_estimated_trajectory");
	}

	m_win->unlockAccess3DScene();
	m_win->forceRepaint();

	MRPT_END
}
template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::dumpVisibilityErrorMsg(
	std::string viz_flag, int sleep_time)
{
	MRPT_START

	MRPT_LOG_ERROR_STREAM(
		"Cannot toggle visibility of specified object.\n"
		<< "Make sure that the corresponding visualization flag (" << viz_flag
		<< ") is set to true in the .ini file.");
	std::this_thread::sleep_for(std::chrono::milliseconds(sleep_time));

	MRPT_END
}

template <class GRAPH_T>
mrpt::system::TTimeStamp CGraphSlamEngine<GRAPH_T>::getTimeStamp(
	const mrpt::obs::CActionCollection::Ptr action,
	const mrpt::obs::CSensoryFrame::Ptr observations,
	const mrpt::obs::CObservation::Ptr observation)
{
	MRPT_START
	using namespace mrpt::obs;
	using namespace mrpt::system;

	// make sure that adequate data is given.
	ASSERTDEBMSG_(
		action || observation,
		"Neither action or observation contains valid data.");

	mrpt::system::TTimeStamp timestamp = INVALID_TIMESTAMP;
	if (observation) { timestamp = observation->timestamp; }
	else
	{
		// querry action part first
		timestamp = action->get(0)->timestamp;

		// if still not available query the observations in the CSensoryFrame
		if (timestamp == INVALID_TIMESTAMP)
		{
			for (auto sens_it = observations->begin();
				 sens_it != observations->end(); ++sens_it)
			{
				timestamp = (*sens_it)->timestamp;
				if (timestamp != INVALID_TIMESTAMP) { break; }
			}
		}
	}
	return timestamp;
	MRPT_END
}

template <class GRAPH_T>
mrpt::poses::CPose3D
	CGraphSlamEngine<GRAPH_T>::getLSPoseForGridMapVisualization(
		const mrpt::graphs::TNodeID nodeID) const
{
	return mrpt::poses::CPose3D(m_graph.nodes.at(nodeID));
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::initMapVisualization()
{
	using namespace mrpt::opengl;

	CSetOfObjects::Ptr map_obj = std::make_shared<CSetOfObjects>();
	map_obj->setName("map");
	Scene::Ptr& scene = this->m_win->get3DSceneAndLock();
	scene->insert(map_obj);
	this->m_win->unlockAccess3DScene();
	this->m_win->forceRepaint();
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::updateMapVisualization(
	const std::map<
		mrpt::graphs::TNodeID, mrpt::obs::CObservation2DRangeScan::Ptr>&
		nodes_to_laser_scans2D,
	bool full_update)
{
	MRPT_START
	ASSERTDEB_(m_enable_visuals);
	using namespace mrpt::obs;
	using namespace mrpt::opengl;
	using namespace std;
	using namespace mrpt::poses;
	Scene::Ptr scene = m_win->get3DSceneAndLock();
	CSetOfObjects::Ptr map_obj;
	{
		CRenderizable::Ptr obj = scene->getByName("map");
		map_obj = std::dynamic_pointer_cast<CSetOfObjects>(obj);
		ASSERTDEB_(map_obj);
	}

	mrpt::system::CTicTac map_update_timer;
	map_update_timer.Tic();

	// get the set of nodes for which to run the update
	std::set<mrpt::graphs::TNodeID> nodes_set;
	{
		if (full_update)
		{
			// for all the nodes get the node position and the corresponding
			// laser scan
			// if they were recorded and visualize them
			m_graph.getAllNodes(nodes_set);
			MRPT_LOG_DEBUG_STREAM("Executing full update of the map visuals");
			map_obj->clear();

		}  // end if - full update
		else
		{  // add only current nodeID
			nodes_set.insert(m_nodeID_max);
		}  // end if - partial update
	}

	// for all the nodes in the previously populated set
	for (mrpt::graphs::TNodeID node_it : nodes_set)
	{
		// name of gui object
		stringstream scan_name("");
		scan_name << "laser_scan_";
		scan_name << node_it;

		// get the node laser scan
		CObservation2DRangeScan::Ptr scan_content;
		auto search = nodes_to_laser_scans2D.find(node_it);

		// make sure that the laser scan exists and is valid
		if (search != nodes_to_laser_scans2D.end() && search->second)
		{
			scan_content = search->second;

			CObservation2DRangeScan scan_decimated;
			this->decimateLaserScan(
				*scan_content, &scan_decimated,
				/*keep_every_n_entries = */ 5);

			// if the scan doesn't already exist, add it to the map object,
			// otherwise just
			// adjust its pose
			CRenderizable::Ptr obj = map_obj->getByName(scan_name.str());
			CSetOfObjects::Ptr scan_obj =
				std::dynamic_pointer_cast<CSetOfObjects>(obj);
			if (!scan_obj)
			{
				scan_obj = std::make_shared<CSetOfObjects>();

				// creating and inserting the observation in the CSetOfObjects
				mrpt::maps::CSimplePointsMap m;
				m.insertObservation(scan_decimated);
				m.getVisualizationInto(*scan_obj);

				scan_obj->setName(scan_name.str());
				this->setObjectPropsFromNodeID(node_it, scan_obj);

				// set the visibility of the object the same value as the
				// visibility of
				// the previous - Needed for proper toggling of the visibility
				// of the
				// whole map
				{
					stringstream prev_scan_name("");
					prev_scan_name << "laser_scan_" << node_it - 1;
					CRenderizable::Ptr prev_obj =
						map_obj->getByName(prev_scan_name.str());
					if (prev_obj)
					{
						scan_obj->setVisibility(prev_obj->isVisible());
					}
				}

				map_obj->insert(scan_obj);
			}
			else
			{
				scan_obj = std::dynamic_pointer_cast<CSetOfObjects>(scan_obj);
			}

			// finally set the pose correctly - as computed by graphSLAM
			const CPose3D& scan_pose = CPose3D(m_graph.nodes.at(node_it));
			scan_obj->setPose(scan_pose);
		}
		else
		{
			MRPT_LOG_DEBUG_STREAM(
				"Laser scans of NodeID " << node_it << "are  empty/invalid");
		}

	}  // end for set of nodes

	m_win->unlockAccess3DScene();
	m_win->forceRepaint();

	double elapsed_time = map_update_timer.Tac();
	MRPT_LOG_DEBUG_STREAM(
		"updateMapVisualization took: " << elapsed_time << "s");
	MRPT_END
}  // end of updateMapVisualization

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::setObjectPropsFromNodeID(
	const mrpt::graphs::TNodeID nodeID,
	mrpt::opengl::CSetOfObjects::Ptr& viz_object)
{
	MRPT_START
	viz_object->setColor_u8(m_optimized_map_color);
	MRPT_END
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::decimateLaserScan(
	mrpt::obs::CObservation2DRangeScan& laser_scan_in,
	mrpt::obs::CObservation2DRangeScan* laser_scan_out,
	const int keep_every_n_entries /*= 2*/)
{
	MRPT_START

	size_t scan_size = laser_scan_in.getScanSize();

	// assign the decimated scans, ranges
	std::vector<float> new_scan(
		scan_size);	 // Was [], but can't use non-constant argument with arrays
	std::vector<char> new_validRange(scan_size);
	size_t new_scan_size = 0;
	for (size_t i = 0; i != scan_size; i++)
	{
		if (i % keep_every_n_entries == 0)
		{
			new_scan[new_scan_size] = laser_scan_in.getScanRange(i);
			new_validRange[new_scan_size] =
				laser_scan_in.getScanRangeValidity(i);
			new_scan_size++;
		}
	}
	laser_scan_out->loadFromVectors(
		new_scan_size, &new_scan[0], &new_validRange[0]);

	MRPT_END
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::initGTVisualization()
{
	MRPT_START
	ASSERTDEB_(m_enable_visuals);
	using namespace mrpt::opengl;

	// assertions
	ASSERTDEB_(m_has_read_config);
	ASSERTDEB_(
		m_win &&
		"Visualization of data was requested but no CDisplayWindow3D pointer "
		"was provided");

	// point cloud
	CPointCloud::Ptr GT_cloud = std::make_shared<CPointCloud>();
	GT_cloud->setPointSize(1.0);
	GT_cloud->enableColorFromX(false);
	GT_cloud->enableColorFromY(false);
	GT_cloud->enableColorFromZ(false);
	GT_cloud->setColor_u8(m_GT_color);
	GT_cloud->setName("GT_cloud");

	// robot model
	CSetOfObjects::Ptr robot_model = this->setCurrentPositionModel(
		"robot_GT", m_GT_color, m_robot_model_size);

	// insert them to the scene
	Scene::Ptr scene = m_win->get3DSceneAndLock();
	scene->insert(GT_cloud);
	scene->insert(robot_model);
	m_win->unlockAccess3DScene();

	m_win_manager->assignTextMessageParameters(
		&m_offset_y_GT, &m_text_index_GT);
	m_win_manager->addTextMessage(
		m_offset_x_left, -m_offset_y_GT, "Ground truth path",
		mrpt::img::TColorf(m_GT_color), m_text_index_GT);
	m_win->forceRepaint();

	MRPT_END
}  // end of initGTVisualization

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::updateGTVisualization()
{
	MRPT_START
	ASSERTDEB_(m_enable_visuals);
	using namespace mrpt::opengl;

	// add to the GT PointCloud and visualize it
	// check that GT vector is not depleted
	if (m_visualize_GT && m_GT_poses_index < m_GT_poses.size())
	{
		ASSERTDEB_(
			m_win &&
			"Visualization of data was requested but no CDisplayWindow3D "
			"pointer was given");

		Scene::Ptr scene = m_win->get3DSceneAndLock();

		CRenderizable::Ptr obj = scene->getByName("GT_cloud");
		CPointCloud::Ptr GT_cloud = std::dynamic_pointer_cast<CPointCloud>(obj);

		// add the latest GT pose
		mrpt::poses::CPose3D p(m_GT_poses[m_GT_poses_index]);
		GT_cloud->insertPoint(p.x(), p.y(), p.z());

		// robot model of GT trajectory
		obj = scene->getByName("robot_GT");
		CSetOfObjects::Ptr robot_obj =
			std::dynamic_pointer_cast<CSetOfObjects>(obj);
		robot_obj->setPose(p);
		m_win->unlockAccess3DScene();
		m_win->forceRepaint();
	}

	MRPT_END
}  // end of updateGTVisualization

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::initOdometryVisualization()
{
	MRPT_START
	ASSERTDEB_(m_has_read_config);
	ASSERTDEB_(m_enable_visuals);
	using namespace mrpt::opengl;

	// point cloud
	CPointCloud::Ptr odometry_poses_cloud = std::make_shared<CPointCloud>();
	odometry_poses_cloud->setPointSize(1.0);
	odometry_poses_cloud->enableColorFromX(false);
	odometry_poses_cloud->enableColorFromY(false);
	odometry_poses_cloud->enableColorFromZ(false);
	odometry_poses_cloud->setColor_u8(m_odometry_color);
	odometry_poses_cloud->setName("odometry_poses_cloud");

	// robot model
	CSetOfObjects::Ptr robot_model = this->setCurrentPositionModel(
		"robot_odometry_poses", m_odometry_color, m_robot_model_size);

	// insert them to the scene
	Scene::Ptr scene = m_win->get3DSceneAndLock();
	scene->insert(odometry_poses_cloud);
	scene->insert(robot_model);
	m_win->unlockAccess3DScene();

	m_win_manager->assignTextMessageParameters(
		&m_offset_y_odometry, &m_text_index_odometry);
	m_win_manager->addTextMessage(
		m_offset_x_left, -m_offset_y_odometry, "Odometry path",
		mrpt::img::TColorf(m_odometry_color), m_text_index_odometry);

	m_win->forceRepaint();

	MRPT_END
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::updateOdometryVisualization()
{
	MRPT_START
	ASSERTDEB_(m_enable_visuals);
	ASSERTDEBMSG_(
		m_win,
		"Visualization of data was requested but no CDisplayWindow3D pointer "
		"was given");
	using namespace mrpt::opengl;

	Scene::Ptr scene = m_win->get3DSceneAndLock();

	// point cloud
	CRenderizable::Ptr obj = scene->getByName("odometry_poses_cloud");
	CPointCloud::Ptr odometry_poses_cloud =
		std::dynamic_pointer_cast<CPointCloud>(obj);
	mrpt::poses::CPose3D p(m_odometry_poses.back());

	odometry_poses_cloud->insertPoint(p.x(), p.y(), p.z());

	// robot model
	obj = scene->getByName("robot_odometry_poses");
	CSetOfObjects::Ptr robot_obj =
		std::dynamic_pointer_cast<CSetOfObjects>(obj);
	robot_obj->setPose(p);

	m_win->unlockAccess3DScene();
	m_win->forceRepaint();

	MRPT_END
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::initEstimatedTrajectoryVisualization()
{
	MRPT_START
	ASSERTDEB_(m_enable_visuals);
	using namespace mrpt::opengl;

	// SetOfLines
	CSetOfLines::Ptr estimated_traj_setoflines =
		std::make_shared<CSetOfLines>();
	estimated_traj_setoflines->setColor_u8(m_estimated_traj_color);
	estimated_traj_setoflines->setLineWidth(1.5);
	estimated_traj_setoflines->setName("estimated_traj_setoflines");
	// append a dummy line so that you can later use append using
	// CSetOfLines::appendLienStrip method.
	estimated_traj_setoflines->appendLine(
		/* 1st */ 0, 0, 0,
		/* 2nd */ 0, 0, 0);

	// robot model
	// pose_t initial_pose;
	CSetOfObjects::Ptr robot_model = this->setCurrentPositionModel(
		"robot_estimated_traj", m_estimated_traj_color, m_robot_model_size);

	// insert objects in the graph
	Scene::Ptr scene = m_win->get3DSceneAndLock();
	if (m_visualize_estimated_trajectory)
	{
		scene->insert(estimated_traj_setoflines);
	}
	scene->insert(robot_model);
	m_win->unlockAccess3DScene();

	if (m_visualize_estimated_trajectory)
	{
		m_win_manager->assignTextMessageParameters(
			&m_offset_y_estimated_traj, &m_text_index_estimated_traj);
		m_win_manager->addTextMessage(
			m_offset_x_left, -m_offset_y_estimated_traj, "Estimated trajectory",
			mrpt::img::TColorf(m_estimated_traj_color),
			m_text_index_estimated_traj);
	}

	m_win->forceRepaint();

	MRPT_END
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::updateEstimatedTrajectoryVisualization(
	bool full_update)
{
	MRPT_START
	ASSERTDEB_(m_enable_visuals);
	using namespace mrpt::opengl;

	std::lock_guard<std::mutex> graph_lock(m_graph_section);
	ASSERTDEB_(m_graph.nodeCount() != 0);

	Scene::Ptr scene = m_win->get3DSceneAndLock();

	CRenderizable::Ptr obj;
	if (m_visualize_estimated_trajectory)
	{
		// set of lines
		obj = scene->getByName("estimated_traj_setoflines");
		CSetOfLines::Ptr estimated_traj_setoflines =
			std::dynamic_pointer_cast<CSetOfLines>(obj);

		// gather set of nodes for which to append lines - all of the nodes in
		// the graph or just the last inserted..
		std::set<mrpt::graphs::TNodeID> nodes_set;
		{
			if (full_update)
			{
				this->getNodeIDsOfEstimatedTrajectory(&nodes_set);
				estimated_traj_setoflines->clear();
				// dummy way so that I can use appendLineStrip afterwards.
				estimated_traj_setoflines->appendLine(
					/* 1st */ 0, 0, 0,
					/* 2nd */ 0, 0, 0);
			}
			else
			{
				nodes_set.insert(m_graph.nodeCount() - 1);
			}
		}
		// append line for each node in the set
		for (unsigned long it : nodes_set)
		{
			mrpt::poses::CPose3D p(m_graph.nodes.at(it));

			estimated_traj_setoflines->appendLineStrip(p.x(), p.y(), p.z());
		}
	}

	// robot model
	// set the robot position to the last recorded pose in the graph
	obj = scene->getByName("robot_estimated_traj");
	CSetOfObjects::Ptr robot_obj =
		std::dynamic_pointer_cast<CSetOfObjects>(obj);
	pose_t curr_estimated_pos = m_graph.nodes[m_graph.nodeCount() - 1];
	robot_obj->setPose(curr_estimated_pos);

	m_win->unlockAccess3DScene();
	m_win->forceRepaint();
	MRPT_END
}  // end of updateEstimatedTrajectoryVisualization

template <class GRAPH_T>
CGraphSlamEngine<GRAPH_T>::TRGBDInfoFileParams::TRGBDInfoFileParams(
	const std::string& rawlog_fname)
{
	this->setRawlogFile(rawlog_fname);
	this->initTRGBDInfoFileParams();
}
template <class GRAPH_T>
CGraphSlamEngine<GRAPH_T>::TRGBDInfoFileParams::TRGBDInfoFileParams()
{
	this->initTRGBDInfoFileParams();
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::TRGBDInfoFileParams::setRawlogFile(
	const std::string& rawlog_fname)
{
	// get the correct info filename from the rawlog_fname
	std::string dir = mrpt::system::extractFileDirectory(rawlog_fname);
	std::string rawlog_filename = mrpt::system::extractFileName(rawlog_fname);
	std::string name_prefix = "rawlog_";
	std::string name_suffix = "_info.txt";
	info_fname = dir + name_prefix + rawlog_filename + name_suffix;
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::TRGBDInfoFileParams::initTRGBDInfoFileParams()
{
	// fields to use
	fields["Overall number of objects"] = "";
	fields["Observations format"] = "";
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::TRGBDInfoFileParams::parseFile()
{
	ASSERT_FILE_EXISTS_(info_fname);
	using namespace std;

	// open file
	mrpt::io::CFileInputStream info_file(info_fname);
	ASSERTDEBMSG_(
		info_file.fileOpenCorrectly(),
		"\nTRGBDInfoFileParams::parseFile: Couldn't open info file\n");

	string curr_line;
	size_t line_cnt = 0;

	// parse until you find an empty line.
	while (true)
	{
		info_file.readLine(curr_line);
		line_cnt++;
		if (curr_line.size() == 0) break;
	}

	// parse the meaningful data
	while (info_file.readLine(curr_line))
	{
		// split current line at ":"
		vector<string> curr_tokens;
		mrpt::system::tokenize(curr_line, ":", curr_tokens);

		ASSERTDEB_EQUAL_(curr_tokens.size(), 2);

		// evaluate the name. if name in info struct then fill the corresponding
		// info struct parameter with the value_part in the file.
		std::string literal_part = mrpt::system::trim(curr_tokens[0]);
		std::string value_part = mrpt::system::trim(curr_tokens[1]);

		for (auto it = fields.begin(); it != fields.end(); ++it)
		{
			if (mrpt::system::strCmpI(it->first, literal_part))
			{
				it->second = value_part;
			}
		}

		line_cnt++;
	}
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::saveGraph(const std::string* fname_in) const
{
	MRPT_START

	// what's the name of the file to be saved?
	std::string fname;
	if (fname_in) { fname = *fname_in; }
	else
	{
		fname = "output_graph.graph";
	}

	MRPT_LOG_INFO_STREAM(
		"Saving generated graph in VERTEX/EDGE format: " << fname);
	m_graph.saveToTextFile(fname);

	MRPT_END
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::save3DScene(const std::string* fname_in) const
{
	MRPT_START
	ASSERTDEBMSG_(
		m_enable_visuals,
		"\nsave3DScene was called even though enable_visuals flag is "
		"off.\nExiting...\n");
	using namespace mrpt::opengl;

	Scene::Ptr scene = m_win->get3DSceneAndLock();

	if (!scene)
	{
		MRPT_LOG_ERROR_STREAM(
			"Could not fetch 3D Scene. Display window must already be closed.");
		return;
	}

	// TODO - what else is there to be excluded from the scene?
	// remove the CPlanarLaserScan if it exists
	{
		CPlanarLaserScan::Ptr laser_scan;
		for (; (laser_scan = scene->getByClass<CPlanarLaserScan>());)
		{
			MRPT_LOG_DEBUG_STREAM(
				"Removing CPlanarLaserScan from generated 3DScene...");
			scene->removeObject(laser_scan);
		}
	}

	// what's the name of the file to be saved?
	std::string fname;
	if (fname_in) { fname = *fname_in; }
	else
	{
		fname = "output_scene.3DScene";
	}

	MRPT_LOG_INFO_STREAM("Saving generated scene to " << fname);
	scene->saveToFile(fname);

	m_win->unlockAccess3DScene();
	m_win->forceRepaint();

	MRPT_END
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::computeSlamMetric(
	mrpt::graphs::TNodeID nodeID, size_t gt_index)
{
	MRPT_START
	using namespace mrpt::math;
	using namespace mrpt::poses;

	// start updating the metric after a certain number of nodes have been added
	if (m_graph.nodeCount() < 4) { return; }

	// add to the map - keep track of which gt index corresponds to which nodeID
	m_nodeID_to_gt_indices[nodeID] = gt_index;

	// initialize the loop variables only once
	pose_t curr_node_pos;
	pose_t curr_gt_pos;
	pose_t node_delta_pos;
	pose_t gt_delta;
	double trans_diff = 0;
	double rot_diff = 0;

	size_t indices_size = m_nodeID_to_gt_indices.size();

	// recompute the metric from scratch
	m_curr_deformation_energy = 0;

	// first element of map
	auto start_it = m_nodeID_to_gt_indices.begin();
	start_it++;

	// fetch the first node, gt positions separately
	auto prev_it = start_it;
	prev_it--;
	pose_t prev_node_pos = m_graph.nodes[prev_it->first];
	pose_t prev_gt_pos = m_GT_poses[prev_it->second];

	// temporary constraint type
	constraint_t c;

	for (auto index_it = start_it; index_it != m_nodeID_to_gt_indices.end();
		 index_it++)
	{
		curr_node_pos = m_graph.nodes[index_it->first];
		curr_gt_pos = m_GT_poses[index_it->second];

		node_delta_pos = curr_node_pos - prev_node_pos;
		gt_delta = curr_gt_pos - prev_gt_pos;

		trans_diff = gt_delta.distanceTo(node_delta_pos);
		rot_diff = this->accumulateAngleDiffs(gt_delta, node_delta_pos);

		m_curr_deformation_energy += (pow(trans_diff, 2) + pow(rot_diff, 2));
		m_curr_deformation_energy /= indices_size;

		// add it to the overall vector
		m_deformation_energy_vec.push_back(m_curr_deformation_energy);

		prev_node_pos = curr_node_pos;
		prev_gt_pos = curr_gt_pos;
	}

	MRPT_LOG_DEBUG_STREAM(
		"Total deformation energy: " << m_curr_deformation_energy);

	MRPT_END
}

template <class GRAPH_T>
double CGraphSlamEngine<GRAPH_T>::accumulateAngleDiffs(
	const mrpt::poses::CPose2D& p1, const mrpt::poses::CPose2D& p2)
{
	return mrpt::math::wrapToPi(p1.phi() - p2.phi());
}
template <class GRAPH_T>
double CGraphSlamEngine<GRAPH_T>::accumulateAngleDiffs(
	const mrpt::poses::CPose3D& p1, const mrpt::poses::CPose3D& p2)
{
	using mrpt::math::wrapToPi;
	double res = 0;

	res += wrapToPi(p1.roll() - p2.roll());
	res += wrapToPi(p1.pitch() - p2.pitch());
	res += wrapToPi(p1.yaw() - p2.yaw());

	return res;
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::initSlamMetricVisualization()
{
	ASSERTDEB_(m_enable_visuals);

	MRPT_START
	MRPT_LOG_DEBUG_STREAM("In initializeSlamMetricVisualization...");
	ASSERTDEB_(m_visualize_SLAM_metric);

	// initialize the m_win_plot on the stack
	m_win_plot = std::make_unique<mrpt::gui::CDisplayWindowPlots>(
		"Evolution of SLAM metric - Deformation Energy (1:1000)", 400, 300);

	m_win_plot->setPos(20, 50);
	m_win_plot->clf();
	// just plot the deformation points from scratch every time
	m_win_plot->hold_off();
	m_win_plot->enableMousePanZoom(true);

	MRPT_END
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::updateSlamMetricVisualization()
{
	MRPT_START
	ASSERTDEB_(m_enable_visuals);
	ASSERTDEB_(m_win_plot && m_visualize_SLAM_metric);

	// build the X, Y vectors for plotting - use log scale
	std::vector<double> x(m_deformation_energy_vec.size(), 0);
	std::vector<double> y(m_deformation_energy_vec.size(), 0);
	for (size_t i = 0; i != x.size(); i++)
	{
		x[i] = i;
		y[i] = m_deformation_energy_vec[i] * 1000;
	}

	m_win_plot->plot(x, y, "r-1", "Deformation Energy (x1000)");

	// set the limits so that he y-values can be monitored
	// set the xmin limit with respect to xmax, which is constantly growing
	std::vector<double>::const_iterator xmax, ymax;
	xmax = std::max_element(x.begin(), x.end());
	ymax = std::max_element(y.begin(), y.end());

	m_win_plot->axis(
		/*x_min = */ xmax != x.end() ? -(*xmax / 12) : -1,
		/*x_max = */ (xmax != x.end() ? *xmax : 1),
		/*y_min = */ -0.4f,
		/*y_max = */ (ymax != y.end() ? *ymax : 1));

	MRPT_END
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::getDescriptiveReport(
	std::string* report_str) const
{
	MRPT_START
	using namespace std;

	const static std::string report_sep(2, '\n');
	const static std::string header_sep(80, '#');

	// Summary of Results
	stringstream results_ss;
	results_ss << "Summary: " << std::endl;
	results_ss << header_sep << std::endl;
	results_ss << "\tProcessing time: "
			   << m_time_logger.getMeanTime("proc_time") << std::endl;
	;
	results_ss << "\tDataset Grab time: " << m_dataset_grab_time << std::endl;
	results_ss << "\tReal-time capable: "
			   << (m_time_logger.getMeanTime("proc_time") < m_dataset_grab_time
					   ? "TRUE"
					   : "FALSE")
			   << std::endl;
	results_ss << m_edge_counter.getAsString();
	results_ss << "\tNum of Nodes: " << m_graph.nodeCount() << std::endl;
	;

	// Class configuration parameters
	std::string config_params = this->getParamsAsString();

	// time and output logging
	const std::string time_res = m_time_logger.getStatsAsText();
	const std::string output_res = this->getLogAsString();

	// merge the individual reports
	report_str->clear();

	*report_str += results_ss.str();
	*report_str += report_sep;

	*report_str += config_params;
	*report_str += report_sep;

	*report_str += time_res;
	*report_str += report_sep;

	*report_str += output_res;
	*report_str += report_sep;

	MRPT_END
}

template <class GRAPH_T>
bool CGraphSlamEngine<GRAPH_T>::getGraphSlamStats(
	std::map<std::string, int>* node_stats,
	std::map<std::string, int>* edge_stats, mrpt::system::TTimeStamp* timestamp)
{
	MRPT_START
	using namespace std;
	using namespace mrpt::graphslam::detail;

	ASSERTDEBMSG_(node_stats, "Invalid pointer to node_stats is given");
	ASSERTDEBMSG_(edge_stats, "Invalid pointer to edge_stats is given");

	if (m_nodeID_max == 0) { return false; }

	// fill the node stats
	(*node_stats)["nodes_total"] = m_nodeID_max + 1;

	// fill the edge stats
	for (auto it = m_edge_counter.cbegin(); it != m_edge_counter.cend(); ++it)
	{
		(*edge_stats)[it->first] = it->second;
	}

	(*edge_stats)["loop_closures"] = m_edge_counter.getLoopClosureEdges();
	(*edge_stats)["edges_total"] = m_edge_counter.getTotalNumOfEdges();

	// fill the timestamp
	if (timestamp) { *timestamp = m_curr_timestamp; }

	return true;
	MRPT_END
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::generateReportFiles(
	const std::string& output_dir_fname)
{
	MRPT_START
	using namespace mrpt::system;
	using namespace mrpt;

	ASSERTDEBMSG_(
		directoryExists(output_dir_fname),
		format(
			"Output directory \"%s\" doesn't exist", output_dir_fname.c_str()));

	MRPT_LOG_INFO_STREAM("Generating detailed class report...");
	std::lock_guard<std::mutex> graph_lock(m_graph_section);

	std::string report_str;
	std::string fname;
	const std::string ext = ".log";

	{  // CGraphSlamEngine
		report_str.clear();
		fname = output_dir_fname + "/" + m_class_name + ext;
		// initialize the output file - refer to the stream through the
		// m_out_streams map
		this->initResultsFile(fname);
		this->getDescriptiveReport(&report_str);
		m_out_streams[fname].printf("%s", report_str.c_str());

		// write the timings into a separate file
		const std::string time_res = m_time_logger.getStatsAsText();
		fname = output_dir_fname + "/" + "timings" + ext;
		this->initResultsFile(fname);
		m_out_streams[fname].printf("%s", time_res.c_str());
	}
	{  // node_registrar
		report_str.clear();
		fname = output_dir_fname + "/" + "node_registrar" + ext;
		this->initResultsFile(fname);
		m_node_reg->getDescriptiveReport(&report_str);
		m_out_streams[fname].printf("%s", report_str.c_str());
	}
	{  // edge_registrar
		report_str.clear();
		fname = output_dir_fname + "/" + "edge_registrar" + ext;
		this->initResultsFile(fname);
		m_edge_reg->getDescriptiveReport(&report_str);
		m_out_streams[fname].printf("%s", report_str.c_str());
	}
	{  // optimizer
		report_str.clear();
		fname = output_dir_fname + "/" + "optimizer" + ext;
		this->initResultsFile(fname);
		m_optimizer->getDescriptiveReport(&report_str);
		m_out_streams[fname].printf("%s", report_str.c_str());
	}

	if (m_use_GT)
	{  // slam evaluation metric
		report_str.clear();
		const std::string desc(
			"# File includes the evolution of the SLAM metric.  Implemented "
			"metric computes the \"deformation energy\" that is needed to "
			"transfer the estimated trajectory into the ground-truth "
			"trajectory (computed as sum of the difference between estimated "
			"trajectory and ground truth consecutive poses See \"A comparison "
			"of SLAM algorithms based on a graph of relations - W.Burgard et "
			"al.\", for more details on the metric.\n");

		fname = output_dir_fname + "/" + "SLAM_evaluation_metric" + ext;
		this->initResultsFile(fname);

		m_out_streams[fname].printf("%s\n", desc.c_str());
		for (auto vec_it = m_deformation_energy_vec.begin();
			 vec_it != m_deformation_energy_vec.end(); ++vec_it)
		{
			m_out_streams[fname].printf("%f\n", *vec_it);
		}
	}

	MRPT_END
}
template <class GRAPH_T>
mrpt::opengl::CSetOfObjects::Ptr
	CGraphSlamEngine<GRAPH_T>::setCurrentPositionModel(
		const std::string& model_name, const mrpt::img::TColor& model_color,
		const size_t model_size, const pose_t& init_pose)
{
	mrpt::opengl::CSetOfObjects::Ptr model =
		this->initRobotModelVisualization();
	model->setName(model_name);
	model->setColor_u8(model_color);
	model->setScale(model_size);
	model->setPose(init_pose);

	return model;
}

template <class GRAPH_T>
void CGraphSlamEngine<GRAPH_T>::getDeformationEnergyVector(
	std::vector<double>* vec_out) const
{
	*vec_out = m_deformation_energy_vec;
}
}  // namespace mrpt::graphslam
