/* +------------------------------------------------------------------------+
   |                     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/RBPF_SLAM_App.h>
#include <mrpt/gui/CDisplayWindow3D.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/io/CFileOutputStream.h>
#include <mrpt/io/vector_loadsave.h>
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/obs/CActionRobotMovement2D.h>
#include <mrpt/obs/CActionRobotMovement3D.h>
#include <mrpt/obs/CObservationGasSensors.h>
#include <mrpt/obs/CRawlog.h>
#include <mrpt/opengl/CEllipsoid2D.h>
#include <mrpt/opengl/CEllipsoid3D.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CSetOfLines.h>
#include <mrpt/opengl/stock_objects.h>
#include <mrpt/poses/CPosePDFGaussian.h>
#include <mrpt/random.h>
#include <mrpt/system/filesystem.h>	 // ASSERT_FILE_EXISTS_()
#include <mrpt/system/memory.h>	 // getMemoryUsage()

using namespace mrpt::apps;

constexpr auto sect = "MappingApplication";

// ---------------------------------------
//   RBPF_SLAM_App_Base
// ---------------------------------------
RBPF_SLAM_App_Base::RBPF_SLAM_App_Base()
{
	// Set logger display name:
	this->setLoggerName("RBPF_SLAM_App");
}

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

	MRPT_LOG_INFO_FMT(
		" rbpf-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_FMT("Usage: %s", impl_get_usage().c_str());
	}

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

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

	impl_initialize(argc, argv);

	MRPT_END
}

void RBPF_SLAM_App_Base::run()
{
	MRPT_START

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

	// ------------------------------------------
	//			Load config from file:
	// ------------------------------------------

	int LOG_FREQUENCY = 5;
	bool GENERATE_LOG_JOINT_H = false;
	bool GENERATE_LOG_INFO = false;
	bool SAVE_POSE_LOG = false;
	bool SAVE_MAP_IMAGES = false;
	bool SAVE_3D_SCENE = false;
	bool CAMERA_3DSCENE_FOLLOWS_ROBOT = true;
	bool SHOW_PROGRESS_IN_WINDOW = false;
	int SHOW_PROGRESS_IN_WINDOW_DELAY_MS = 1;
	std::string METRIC_MAP_CONTINUATION_GRIDMAP_FILE;
	std::string SIMPLEMAP_CONTINUATION;
	mrpt::math::TPose2D METRIC_MAP_CONTINUATION_START_POSE;
	int PROGRESS_WINDOW_WIDTH = 600, PROGRESS_WINDOW_HEIGHT = 500;
	int RANDOM_SEED = -1;  // <0: randomize
	const string OUT_DIR_STD =
		params.read_string(sect, "logOutput_dir", "log_out", true);

	if (params.keyExists(sect, "verbosity"))
		this->setMinLoggingLevel(params.read_enum<mrpt::system::VerbosityLevel>(
			sect, "verbosity", mrpt::system::VerbosityLevel::LVL_INFO));

	// This to allow using the shorter XXX_CS() macros to read params:
	{
		const auto& c = params;
		const auto& s = sect;

		MRPT_LOAD_CONFIG_VAR_CS(LOG_FREQUENCY, int);
		MRPT_LOAD_CONFIG_VAR_CS(GENERATE_LOG_JOINT_H, bool);
		MRPT_LOAD_CONFIG_VAR_CS(GENERATE_LOG_INFO, bool);
		MRPT_LOAD_CONFIG_VAR_CS(SAVE_POSE_LOG, bool);
		MRPT_LOAD_CONFIG_VAR_CS(SAVE_MAP_IMAGES, bool);
		MRPT_LOAD_CONFIG_VAR_CS(SAVE_3D_SCENE, bool);
		MRPT_LOAD_CONFIG_VAR_CS(CAMERA_3DSCENE_FOLLOWS_ROBOT, bool);
		MRPT_LOAD_CONFIG_VAR_CS(SHOW_PROGRESS_IN_WINDOW, bool);
		MRPT_LOAD_CONFIG_VAR_CS(SHOW_PROGRESS_IN_WINDOW_DELAY_MS, int);
		MRPT_LOAD_CONFIG_VAR_CS(METRIC_MAP_CONTINUATION_GRIDMAP_FILE, string);
		MRPT_LOAD_CONFIG_VAR_CS(SIMPLEMAP_CONTINUATION, string);
		METRIC_MAP_CONTINUATION_START_POSE.x =
			c.read_double(s, "METRIC_MAP_CONTINUATION_START_POSE_X", .0);
		METRIC_MAP_CONTINUATION_START_POSE.y =
			c.read_double(s, "METRIC_MAP_CONTINUATION_START_POSE_Y", .0);
		METRIC_MAP_CONTINUATION_START_POSE.phi = DEG2RAD(
			c.read_double(s, "METRIC_MAP_CONTINUATION_START_POSE_PHI_DEG", .0));
		MRPT_LOAD_CONFIG_VAR_CS(PROGRESS_WINDOW_WIDTH, int);
		MRPT_LOAD_CONFIG_VAR_CS(PROGRESS_WINDOW_HEIGHT, int);
		MRPT_LOAD_CONFIG_VAR_CS(RANDOM_SEED, int);
	}
	const char* OUT_DIR = OUT_DIR_STD.c_str();

	// Print params:
	MRPT_LOG_INFO_FMT("Output directory: `%s`", OUT_DIR);

	CTicTac tictac, tictacGlobal, tictac_JH;
	int step = 0;
	CSimpleMap finalMap;
	float t_exec;
	COccupancyGridMap2D::TEntropyInfo entropy;

	char strFil[1000];

	// ---------------------------------
	//		MapPDF opts
	// ---------------------------------
	CMetricMapBuilderRBPF::TConstructionOptions rbpfMappingOptions;
	rbpfMappingOptions.loadFromConfigFile(params, sect);

	// ---------------------------------
	//		Constructor
	// ---------------------------------
	mapBuilder = std::make_shared<CMetricMapBuilderRBPF>(rbpfMappingOptions);
	mapBuilder->setVerbosityLevel(this->getMinLoggingLevel());

	{
		std::stringstream ss;
		rbpfMappingOptions.dumpToTextStream(ss);
		MRPT_LOG_INFO(ss.str());
	}

	// handle the case of metric map continuation
	if (!METRIC_MAP_CONTINUATION_GRIDMAP_FILE.empty())
	{
		CSimpleMap dummySimpleMap;
		CPosePDFGaussian startPose;

		startPose.mean.x(METRIC_MAP_CONTINUATION_START_POSE.x);
		startPose.mean.y(METRIC_MAP_CONTINUATION_START_POSE.y);
		startPose.mean.phi(METRIC_MAP_CONTINUATION_START_POSE.phi);
		startPose.cov.setZero();

		mrpt::maps::COccupancyGridMap2D gridmap;
		{
			mrpt::io::CFileGZInputStream f(
				METRIC_MAP_CONTINUATION_GRIDMAP_FILE);
			mrpt::serialization::archiveFrom(f) >> gridmap;
		}

		mapBuilder->initialize(dummySimpleMap, &startPose);

		for (auto& m_particle : mapBuilder->mapPDF.m_particles)
		{
			CRBPFParticleData* part_d = m_particle.d.get();
			CMultiMetricMap& mmap = part_d->mapTillNow;
			mrpt::maps::COccupancyGridMap2D::Ptr it_grid =
				mmap.mapByClass<mrpt::maps::COccupancyGridMap2D>();
			ASSERTMSG_(
				it_grid,
				"No gridmap in multimetric map definition, but metric map "
				"continuation was set (!)");
			it_grid->copyMapContentFrom(gridmap);
		}
	}
	if (!SIMPLEMAP_CONTINUATION.empty())
	{
		mrpt::maps::CSimpleMap init_map;
		mrpt::io::CFileGZInputStream f(SIMPLEMAP_CONTINUATION);
		mrpt::serialization::archiveFrom(f) >> init_map;
		mapBuilder->initialize(init_map);
	}

	// ---------------------------------
	//   CMetricMapBuilder::TOptions
	// ---------------------------------
	// mapBuilder->setVerbosityLevel(  mrpt::system::LVL_DEBUG );  // default
	// value: as loaded from config file
	mapBuilder->options.enableMapUpdating = true;
	mapBuilder->options.debugForceInsertion = false;

	auto& rng = mrpt::random::getRandomGenerator();
	if (RANDOM_SEED >= 0) rng.randomize(RANDOM_SEED);
	else
		rng.randomize();

	// Prepare output directory:
	// --------------------------------
	deleteFilesInDirectory(OUT_DIR);
	createDirectory(OUT_DIR);

	string OUT_DIR_MAPS = format("%s/maps", OUT_DIR);
	string OUT_DIR_3D = format("%s/3D", OUT_DIR);

	deleteFilesInDirectory(OUT_DIR_MAPS);
	createDirectory(OUT_DIR_MAPS);

	deleteFilesInDirectory(OUT_DIR_3D);
	createDirectory(OUT_DIR_3D);

	// Open log files:
	// ----------------------------------
	CFileOutputStream f_log(format("%s/log_times.txt", OUT_DIR));
	CFileOutputStream f_info(format("%s/log_info.txt", OUT_DIR));
	CFileOutputStream f_jinfo(format("%s/log_jinfo.txt", OUT_DIR));
	CFileOutputStream f_path(format("%s/log_estimated_path.txt", OUT_DIR));
	CFileOutputStream f_pathOdo(format("%s/log_odometry_path.txt", OUT_DIR));
	CFileOutputStream f_partStats(format("%s/log_ParticlesStats.txt", OUT_DIR));

	f_log.printf(
		"%% time_step  execution_time(ms)  map_size(#frames)  frame_inserted? "
		"\n"
		"%%-------------------------------------------------------------------"
		"\n");

	f_info.printf(
		"%% EMI    H    EMMI    effecMappedArea  effecMappedCells \n"
		"%%-------------------------------------------------------\n");

	f_pathOdo.printf(
		"%% time_step  x  y z yaw pitch roll timestamp \n"
		"%%--------------------------------------------\n");

	f_pathOdo.printf(
		"%% time_step  x  y z yaw pitch roll \n"
		"%%----------------------------------\n");

	f_partStats.printf(
		"%% time_step   #particles   ESS \n"
		"%%------------------------------\n");

	// ----------------------------------------------------------
	//						Map Building
	// ----------------------------------------------------------
	CPose3D odoPose(0, 0, 0);

	CDisplayWindow3D::Ptr win3D;

	if (SHOW_PROGRESS_IN_WINDOW)
	{
		win3D = CDisplayWindow3D::Create(
			"RBPF-SLAM @ MRPT C++ Library", PROGRESS_WINDOW_WIDTH,
			PROGRESS_WINDOW_HEIGHT);
		win3D->setCameraZoom(40);
		win3D->setCameraAzimuthDeg(-50);
		win3D->setCameraElevationDeg(70);
	}

	tictacGlobal.Tic();
	for (;;)
	{
		if (quits_with_esc_key && os::kbhit())
		{
			char c = os::getch();
			if (c == 27) break;
		}

		CActionCollection::Ptr action;
		CSensoryFrame::Ptr observations;
		CObservation::Ptr observation;

		// Load action/observation pair from the rawlog:
		// --------------------------------------------------
		if (!impl_get_next_observations(action, observations, observation))
			break;	// EOF

		// Update odometry:
		{
			CActionRobotMovement2D::Ptr act =
				action->getBestMovementEstimation();
			if (act) odoPose = odoPose + CPose3D(act->poseChange->getMeanVal());
			else
			{
				CActionRobotMovement3D::Ptr act3D =
					action->getActionByClass<CActionRobotMovement3D>();
				if (act3D) odoPose = odoPose + act3D->poseChange.mean;
			}
		}

		mrpt::system::TTimeStamp observations_timestamp;
		if (observations && !observations->empty())
			observations_timestamp = (*observations->begin())->timestamp;

		// Execute:
		// ----------------------------------------
		tictac.Tic();
		mapBuilder->processActionObservation(*action, *observations);
		t_exec = tictac.Tac();
		MRPT_LOG_INFO_FMT("Map building executed in %.03fms", 1000.0f * t_exec);

		// Info log:
		// -----------
		f_log.printf(
			"%u %f %i %i\n", static_cast<unsigned int>(step), 1000.0f * t_exec,
			mapBuilder->getCurrentlyBuiltMapSize(),
			mapBuilder->m_statsLastIteration.observationsInserted ? int(1)
																  : int(0));

		CPose3DPDF::Ptr curPDFptr = mapBuilder->getCurrentPoseEstimation();
		CPose3DPDFParticles curPDF;

		if (IS_CLASS(*curPDFptr, CPose3DPDFParticles))
		{
			CPose3DPDFParticles::Ptr pp =
				std::dynamic_pointer_cast<CPose3DPDFParticles>(curPDFptr);
			curPDF = *pp;
		}

		if (LOG_FREQUENCY > 0 && 0 == (step % LOG_FREQUENCY))
		{
			const CMultiMetricMap* mostLikMap =
				mapBuilder->mapPDF.getCurrentMostLikelyMetricMap();

			if (GENERATE_LOG_INFO)
			{
				tictac_JH.Tic();

				const CMultiMetricMap* avrMap =
					mapBuilder->mapPDF.getAveragedMetricMapEstimation();
				COccupancyGridMap2D::Ptr grid =
					avrMap->mapByClass<COccupancyGridMap2D>();
				ASSERT_(grid);
				grid->computeEntropy(entropy);

				grid->saveAsBitmapFile(
					format("%s/EMMI_gridmap_%03u.png", OUT_DIR, step));

				f_info.printf(
					"%f %f %f %f %lu\n", entropy.I, entropy.H, entropy.mean_I,
					entropy.effectiveMappedArea, entropy.effectiveMappedCells);
				MRPT_LOG_INFO_FMT(
					"Log information saved. EMI = %.04f EMMI=%.04f (in "
					"%.03fms)\n",
					entropy.I, entropy.mean_I, 1000.0f * tictac_JH.Tac());
			}

			// Pose log:
			// -------------
			if (SAVE_POSE_LOG)
			{
				curPDF.saveToTextFile(
					format("%s/mapbuild_posepdf_%03u.txt", OUT_DIR, step));
			}

			// Map images:
			// -------------
			if (SAVE_MAP_IMAGES)
			{
				MRPT_LOG_DEBUG("Saving map images to files...");

				//  Most likely maps:
				// ----------------------------------------
				mostLikMap->saveMetricMapRepresentationToFile(
					format("%s/mapbuilt_%05u_", OUT_DIR_MAPS.c_str(), step));

				if (mostLikMap->countMapsByClass<COccupancyGridMap2D>() > 0)
				{
					mrpt::img::CImage img;
					mapBuilder->drawCurrentEstimationToImage(&img);
					img.saveToFile(
						format("%s/mapping_%05u.png", OUT_DIR, step));
				}
			}

			// Save a 3D scene view of the mapping process:
			Scene::Ptr scene;
			if (SAVE_3D_SCENE || SHOW_PROGRESS_IN_WINDOW)
			{
				scene = std::make_shared<Scene>();

				// The ground:
				mrpt::opengl::CGridPlaneXY::Ptr groundPlane =
					mrpt::opengl::CGridPlaneXY::Create(
						-200, 200, -200, 200, 0, 5);
				groundPlane->setColor(0.4f, 0.4f, 0.4f);
				scene->insert(groundPlane);

				// The camera pointing to the current robot pose:
				if (CAMERA_3DSCENE_FOLLOWS_ROBOT)
				{
					mrpt::opengl::CCamera::Ptr objCam =
						mrpt::opengl::CCamera::Create();
					CPose3D robotPose;
					curPDF.getMean(robotPose);

					objCam->setPointingAt(robotPose);
					objCam->setAzimuthDegrees(-30);
					objCam->setElevationDegrees(30);
					scene->insert(objCam);
				}

				// Draw the map(s):
				scene->insert(mostLikMap->getVisualization());

				// Draw the robot particles:
				size_t M = mapBuilder->mapPDF.particlesCount();
				mrpt::opengl::CSetOfLines::Ptr objLines =
					mrpt::opengl::CSetOfLines::Create();
				objLines->setColor(0, 1, 1);
				for (size_t i = 0; i < M; i++)
				{
					std::deque<TPose3D> path;
					mapBuilder->mapPDF.getPath(i, path);

					float x0 = 0, y0 = 0, z0 = 0;
					for (auto& k : path)
					{
						objLines->appendLine(
							x0, y0, z0 + 0.001, k.x, k.y, k.z + 0.001);
						x0 = k.x;
						y0 = k.y;
						z0 = k.z;
					}
				}
				scene->insert(objLines);

				// An ellipsoid:
				CPose3D lastMeanPose;
				float minDistBtwPoses = -1;
				std::deque<TPose3D> dummyPath;
				mapBuilder->mapPDF.getPath(0, dummyPath);
				for (int k = (int)dummyPath.size() - 1; k >= 0; k--)
				{
					CPose3DPDFParticles poseParts;
					mapBuilder->mapPDF.getEstimatedPosePDFAtTime(k, poseParts);

					const auto [COV, meanPose] =
						poseParts.getCovarianceAndMean();

					if (meanPose.distanceTo(lastMeanPose) > minDistBtwPoses)
					{
						CMatrixDouble33 COV3 = COV.blockCopy<3, 3>(0, 0);

						minDistBtwPoses = 6 * sqrt(COV3(0, 0) + COV3(1, 1));

						if (COV3(2, 2) == 0)
						{
							auto objEllip = opengl::CEllipsoid2D::Create();
							objEllip->setLocation(
								meanPose.x(), meanPose.y(),
								meanPose.z() + 0.001);
							objEllip->setCovMatrix(COV3.blockCopy<2, 2>());
							objEllip->setColor(0, 0, 1);
							objEllip->enableDrawSolid3D(false);
							scene->insert(objEllip);
						}
						else
						{
							auto objEllip = opengl::CEllipsoid3D::Create();
							objEllip->setLocation(
								meanPose.x(), meanPose.y(),
								meanPose.z() + 0.001);
							objEllip->setCovMatrix(COV3);
							objEllip->setColor(0, 0, 1);
							objEllip->enableDrawSolid3D(false);
							scene->insert(objEllip);
						}

						lastMeanPose = meanPose;
					}
				}
			}  // end if show or save 3D scene->

			if (SAVE_3D_SCENE)
			{  // Save as file:
				CFileGZOutputStream f(format(
					"%s/buildingmap_%05u.3Dscene", OUT_DIR_3D.c_str(), step));
				mrpt::serialization::archiveFrom(f) << *scene;
			}

			if (SHOW_PROGRESS_IN_WINDOW)
			{
				Scene::Ptr& scenePtr = win3D->get3DSceneAndLock();
				scenePtr = scene;
				win3D->unlockAccess3DScene();

				win3D->forceRepaint();
				int add_delay =
					SHOW_PROGRESS_IN_WINDOW_DELAY_MS - t_exec * 1000;
				if (add_delay > 0)
					std::this_thread::sleep_for(
						std::chrono::milliseconds(add_delay));
			}

			// Save the weighted entropy of each map:
			// ----------------------------------------
			if (GENERATE_LOG_JOINT_H)
			{
				tictac_JH.Tic();

				double H_joint = mapBuilder->getCurrentJointEntropy();
				double H_path = mapBuilder->mapPDF.getCurrentEntropyOfPaths();
				f_jinfo.printf("%e %e\n", H_joint, H_path);
				MRPT_LOG_INFO_FMT(
					"Saving joing H info. joint-H=%f\t(in %.03fms)", H_joint,
					1000.0f * tictac_JH.Tac());
			}

		}  // end of LOG_FREQ

		// Save the memory usage:
		// ------------------------------------------------------------------
		{
			unsigned long memUsage = mrpt::system::getMemoryUsage();
			FILE* f = os::fopen(
				format("%s/log_MemoryUsage.txt", OUT_DIR).c_str(), "at");
			if (f)
			{
				os::fprintf(f, "%u\t%lu\n", step, memUsage);
				os::fclose(f);
			}
			MRPT_LOG_INFO_FMT(
				"Saving memory usage: %.04f MiB", memUsage / (1024.0 * 1024.0));
		}

		// Save the parts stats:
		f_partStats.printf(
			"%u %u %f\n", (unsigned int)step, (unsigned int)curPDF.size(),
			curPDF.ESS());

		// Save the robot estimated pose for each step:
		CPose3D meanPose;
		mapBuilder->getCurrentPoseEstimation()->getMean(meanPose);

		f_path.printf(
			"%u %f %f %f %f %f %f %f\n", (unsigned int)step, meanPose.x(),
			meanPose.y(), meanPose.z(), meanPose.yaw(), meanPose.pitch(),
			meanPose.roll(), mrpt::Clock::toDouble(observations_timestamp));

		// Also keep the robot path as a vector, for the convenience of the app
		// user:
		out_estimated_path[observations_timestamp] = meanPose.asTPose();

		f_pathOdo.printf(
			"%i\t%f\t%f\t%f\t%f\t%f\t%f\n", step, odoPose.x(), odoPose.y(),
			odoPose.z(), odoPose.yaw(), odoPose.pitch(), odoPose.roll());

		step++;
		MRPT_LOG_INFO_FMT("------------- STEP %u ----------------", step);

	};	// end while

	MRPT_LOG_INFO_FMT(
		"----------- **END** (total time: %.03f sec) ---------",
		tictacGlobal.Tac());

	// Save map:
	mapBuilder->getCurrentlyBuiltMap(finalMap);

	CFileOutputStream filOut(format("%s/_finalmap_.simplemap", OUT_DIR));
	mrpt::serialization::archiveFrom(filOut) << finalMap;

	// Save gridmap extend (if exists):
	const CMultiMetricMap* mostLikMap =
		mapBuilder->mapPDF.getCurrentMostLikelyMetricMap();
	mostLikMap->saveMetricMapRepresentationToFile(
		format("%s/finalMap", OUT_DIR));

	// Save the most likely path of the particle set
	FILE* f_pathPart;

	os::sprintf(strFil, 1000, "%s/most_likely_path.txt", OUT_DIR);
	f_pathPart = os::fopen(strFil, "wt");

	ASSERT_(f_pathPart != nullptr);

	std::deque<TPose3D> outPath;
	std::deque<TPose3D>::iterator itPath;

	mapBuilder->getCurrentMostLikelyPath(outPath);

	for (itPath = outPath.begin(); itPath != outPath.end(); itPath++)
		os::fprintf(
			f_pathPart, "%.3f %.3f %.3f\n", itPath->x, itPath->y, itPath->yaw);

	os::fclose(f_pathPart);

	// Close 3D window, if any:
	if (win3D) win3D->waitForKey();

	MRPT_END
}

// ---------------------------------------
//   RBPF_SLAM_App_Rawlog
// ---------------------------------------
RBPF_SLAM_App_Rawlog::RBPF_SLAM_App_Rawlog()
{
	setLoggerName("RBPF_SLAM_App_Rawlog");
}

void RBPF_SLAM_App_Rawlog::init(
	const std::string& iniConfigFile, const std::string& rawlogFile)
{
	const int argc = 3;
	const char* argv[3] = {
		"rbpf-slam", iniConfigFile.c_str(), rawlogFile.c_str()};
	initialize(argc, argv);
}

void RBPF_SLAM_App_Rawlog::impl_initialize(int argc, const char** argv)
{
	MRPT_START
	// Rawlog file: from args. line or from config file:
	if (argc == 3) m_rawlogFileName = std::string(argv[2]);
	else
		m_rawlogFileName = params.read_string(
			sect, "rawlog_file", std::string("log.rawlog"), true);

	m_rawlog_offset = params.read_int(sect, "rawlog_offset", 0, true);

	ASSERT_FILE_EXISTS_(m_rawlogFileName);

	// Set relative path for externally-stored images in rawlogs:
	std::string rawlog_images_path =
		mrpt::system::extractFileDirectory(m_rawlogFileName);
	rawlog_images_path += "/Images";
	mrpt::img::CImage::setImagesPathBase(rawlog_images_path);

	MRPT_END
}
