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

#include "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);
          bool savedOk = img.saveToFile(format("%s/mapping_%05u.png", OUT_DIR, step));
          ASSERT_(savedOk);
        }
      }

      // 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
}
