/* +------------------------------------------------------------------------+
   |                     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 "CFormRawMap.h"

#include <wx/app.h>
#include <wx/filedlg.h>
#include <wx/msgdlg.h>
#include <wx/progdlg.h>

#include "xRawLogViewerMain.h"

//(*InternalHeaders(CFormRawMap)
#include <wx/artprov.h>
#include <wx/bitmap.h>
#include <wx/font.h>
#include <wx/image.h>
#include <wx/intl.h>
#include <wx/string.h>
//*)

#include <wx/numdlg.h>

// General global variables:
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/io/CFileOutputStream.h>
#include <mrpt/maps/CColouredPointsMap.h>
#include <mrpt/maps/CMultiMetricMap.h>
#include <mrpt/maps/CPointsMap.h>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/math/geometry.h>
#include <mrpt/obs/CObservationOdometry.h>
#include <mrpt/opengl/CGridPlaneXY.h>
#include <mrpt/opengl/CSetOfLines.h>
#include <mrpt/poses/CPosePDFParticles.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>
#include <mrpt/topography.h>

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

// The map built from laser & odometry.
CMultiMetricMap theMap;
CPose3DInterpolator robot_path;
mrpt::topography::TPathFromRTKInfo rtk_path_info;

extern xRawLogViewerFrame* theMainWindow;

//(*IdInit(CFormRawMap)
const long CFormRawMap::ID_STATICTEXT7 = wxNewId();
const long CFormRawMap::ID_STATICTEXT6 = wxNewId();
const long CFormRawMap::ID_STATICTEXT5 = wxNewId();
const long CFormRawMap::ID_STATICTEXT1 = wxNewId();
const long CFormRawMap::ID_SLIDER1 = wxNewId();
const long CFormRawMap::ID_SPINCTRL1 = wxNewId();
const long CFormRawMap::ID_STATICTEXT3 = wxNewId();
const long CFormRawMap::ID_SLIDER2 = wxNewId();
const long CFormRawMap::ID_SPINCTRL2 = wxNewId();
const long CFormRawMap::ID_STATICTEXT10 = wxNewId();
const long CFormRawMap::ID_SLIDER3 = wxNewId();
const long CFormRawMap::ID_SPINCTRL3 = wxNewId();
const long CFormRawMap::ID_BUTTON2 = wxNewId();
const long CFormRawMap::ID_BUTTON6 = wxNewId();
const long CFormRawMap::ID_BUTTON5 = wxNewId();
const long CFormRawMap::ID_BUTTON1 = wxNewId();
const long CFormRawMap::ID_BUTTON3 = wxNewId();
const long CFormRawMap::ID_BUTTON7 = wxNewId();
const long CFormRawMap::ID_BUTTON8 = wxNewId();
const long CFormRawMap::ID_BUTTON9 = wxNewId();
const long CFormRawMap::ID_STATICTEXT8 = wxNewId();
const long CFormRawMap::ID_STATICTEXT2 = wxNewId();
const long CFormRawMap::ID_BUTTON4 = wxNewId();
const long CFormRawMap::ID_TEXTCTRL1 = wxNewId();
const long CFormRawMap::ID_PANEL1 = wxNewId();
const long CFormRawMap::ID_CUSTOM2 = wxNewId();
const long CFormRawMap::ID_PANEL3 = wxNewId();
//*)

BEGIN_EVENT_TABLE(CFormRawMap, wxDialog)
//(*EventTable(CFormRawMap)
//*)
END_EVENT_TABLE()

CFormRawMap::CFormRawMap(wxWindow* parent, wxWindowID)
{
  //(*Initialize(CFormRawMap)
  wxFlexGridSizer* FlexGridSizer4;
  wxFlexGridSizer* FlexGridSizer5;
  wxFlexGridSizer* FlexGridSizer9;
  wxStaticBoxSizer* boxResults;
  wxFlexGridSizer* FlexGridSizer7;
  wxFlexGridSizer* FlexGridSizer8;
  wxFlexGridSizer* FlexGridSizer6;
  wxStaticBoxSizer* StaticBoxSizer1;
  wxFlexGridSizer* FlexGridSizer11;

  Create(
      parent, wxID_ANY, _("Creation of \"raw map & paths\" from scans & odometry"),
      wxDefaultPosition, wxDefaultSize,
      wxCAPTION | wxDEFAULT_DIALOG_STYLE | wxSYSTEM_MENU | wxRESIZE_BORDER | wxMAXIMIZE_BOX,
      _T("wxID_ANY"));
  FlexGridSizer1 = new wxFlexGridSizer(2, 1, 0, 0);
  FlexGridSizer1->AddGrowableCol(0);
  FlexGridSizer1->AddGrowableRow(1);
  Panel2 = new wxPanel(
      this, ID_PANEL1, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL1"));
  FlexGridSizer2 = new wxFlexGridSizer(1, 2, 0, 0);
  FlexGridSizer2->AddGrowableCol(1);
  FlexGridSizer2->AddGrowableRow(0);
  FlexGridSizer4 = new wxFlexGridSizer(2, 1, 0, 0);
  FlexGridSizer3 = new wxFlexGridSizer(4, 3, 0, 0);
  FlexGridSizer3->AddGrowableCol(1);
  StaticText5 = new wxStaticText(
      Panel2, ID_STATICTEXT7, wxEmptyString, wxDefaultPosition, wxDefaultSize, 0,
      _T("ID_STATICTEXT7"));
  FlexGridSizer3->Add(
      StaticText5, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  StaticText4 = new wxStaticText(
      Panel2, ID_STATICTEXT6, _("Select which indexes to process:"), wxDefaultPosition,
      wxDefaultSize, 0, _T("ID_STATICTEXT6"));
  FlexGridSizer3->Add(
      StaticText4, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  StaticText2 = new wxStaticText(
      Panel2, ID_STATICTEXT5, wxEmptyString, wxDefaultPosition, wxDefaultSize, 0,
      _T("ID_STATICTEXT5"));
  FlexGridSizer3->Add(
      StaticText2, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  StaticText1 = new wxStaticText(
      Panel2, ID_STATICTEXT1, _("First entry:"), wxDefaultPosition, wxDefaultSize, 0,
      _T("ID_STATICTEXT1"));
  FlexGridSizer3->Add(StaticText1, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
  slFrom = new wxSlider(
      Panel2, ID_SLIDER1, 0, 0, 100, wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator,
      _T("ID_SLIDER1"));
  FlexGridSizer3->Add(slFrom, 1, wxEXPAND, 1);
  edFirst = new wxSpinCtrl(
      Panel2, ID_SPINCTRL1, _T("0"), wxDefaultPosition, wxDefaultSize, 0, 0, 100, 0,
      _T("ID_SPINCTRL1"));
  edFirst->SetValue(_T("0"));
  FlexGridSizer3->Add(edFirst, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  StaticText3 = new wxStaticText(
      Panel2, ID_STATICTEXT3, _("Last entry:"), wxDefaultPosition, wxDefaultSize, 0,
      _T("ID_STATICTEXT3"));
  FlexGridSizer3->Add(StaticText3, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
  slTo = new wxSlider(
      Panel2, ID_SLIDER2, 0, 0, 100, wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator,
      _T("ID_SLIDER2"));
  FlexGridSizer3->Add(slTo, 1, wxEXPAND, 1);
  edLast = new wxSpinCtrl(
      Panel2, ID_SPINCTRL2, _T("0"), wxDefaultPosition, wxDefaultSize, 0, 0, 100, 0,
      _T("ID_SPINCTRL2"));
  edLast->SetValue(_T("0"));
  FlexGridSizer3->Add(edLast, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  StaticText6 = new wxStaticText(
      Panel2, ID_STATICTEXT10, _("Decimation:"), wxDefaultPosition, wxDefaultSize, 0,
      _T("ID_STATICTEXT10"));
  FlexGridSizer3->Add(StaticText6, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
  slDecimate = new wxSlider(
      Panel2, ID_SLIDER3, 0, 1, 200, wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator,
      _T("ID_SLIDER3"));
  FlexGridSizer3->Add(slDecimate, 1, wxEXPAND, 0);
  edDec = new wxSpinCtrl(
      Panel2, ID_SPINCTRL3, _T("1"), wxDefaultPosition, wxDefaultSize, 0, 1, 200, 1,
      _T("ID_SPINCTRL3"));
  edDec->SetValue(_T("1"));
  FlexGridSizer3->Add(edDec, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  FlexGridSizer4->Add(FlexGridSizer3, 1, wxEXPAND, 0);
  FlexGridSizer5 = new wxFlexGridSizer(2, 1, 0, 0);
  FlexGridSizer5->AddGrowableCol(0);
  FlexGridSizer6 = new wxFlexGridSizer(1, 1, 0, 0);
  FlexGridSizer6->AddGrowableCol(0);
  FlexGridSizer6->AddGrowableRow(0);
  StaticBoxSizer1 = new wxStaticBoxSizer(wxHORIZONTAL, Panel2, _("Generate..."));
  FlexGridSizer8 = new wxFlexGridSizer(0, 3, 0, 0);
  FlexGridSizer8->AddGrowableCol(0);
  btnGenerate = new wxButton(
      Panel2, ID_BUTTON2, _("Map from odometry"), wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_BUTTON2"));
  btnGenerate->SetDefault();
  FlexGridSizer8->Add(btnGenerate, 1, wxEXPAND, 5);
  btnGenerateRTK = new wxButton(
      Panel2, ID_BUTTON6, _("Map from RTK GPS"), wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_BUTTON6"));
  btnGenerateRTK->SetDefault();
  FlexGridSizer8->Add(btnGenerateRTK, 1, wxEXPAND, 5);
  btnGeneratePaths = new wxButton(
      Panel2, ID_BUTTON5, _("Random paths..."), wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_BUTTON5"));
  btnGeneratePaths->SetDefault();
  FlexGridSizer8->Add(btnGeneratePaths, 1, wxEXPAND, 5);
  StaticBoxSizer1->Add(FlexGridSizer8, 1, wxEXPAND, 0);
  FlexGridSizer6->Add(StaticBoxSizer1, 1, wxEXPAND, 3);
  FlexGridSizer5->Add(FlexGridSizer6, 1, wxEXPAND, 0);
  FlexGridSizer7 = new wxFlexGridSizer(1, 1, 0, 0);
  FlexGridSizer7->AddGrowableCol(0);
  FlexGridSizer7->AddGrowableRow(0);
  boxResults = new wxStaticBoxSizer(wxHORIZONTAL, Panel2, _("Result"));
  FlexGridSizer9 = new wxFlexGridSizer(1, 1, 0, 0);
  FlexGridSizer11 = new wxFlexGridSizer(3, 3, 0, 0);
  FlexGridSizer11->AddGrowableCol(0);
  btnSaveTxt = new wxButton(
      Panel2, ID_BUTTON1, _("Save map as text..."), wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_BUTTON1"));
  btnSaveTxt->Disable();
  FlexGridSizer11->Add(btnSaveTxt, 1, wxEXPAND, 5);
  btnSave3D = new wxButton(
      Panel2, ID_BUTTON3, _("Save map as 3D scene..."), wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_BUTTON3"));
  btnSave3D->Disable();
  FlexGridSizer11->Add(btnSave3D, 1, wxEXPAND, 5);
  btnSavePath = new wxButton(
      Panel2, ID_BUTTON7, _("Save vehicle path..."), wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_BUTTON7"));
  btnSavePath->Disable();
  FlexGridSizer11->Add(btnSavePath, 1, wxEXPAND, 5);
  btnSaveObsPath = new wxButton(
      Panel2, ID_BUTTON8, _("Save observations poses..."), wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_BUTTON8"));
  btnSaveObsPath->Disable();
  FlexGridSizer11->Add(btnSaveObsPath, 1, wxEXPAND, 5);
  btnView3D = new wxButton(
      Panel2, ID_BUTTON9, _("Preview map in 3D"), wxDefaultPosition, wxDefaultSize, 0,
      wxDefaultValidator, _T("ID_BUTTON9"));
  btnView3D->Disable();
  FlexGridSizer11->Add(btnView3D, 1, wxEXPAND, 5);

  btnClose = new wxButton(
      Panel2, ID_BUTTON4, _("Close"), wxDefaultPosition, wxDefaultSize, 0, wxDefaultValidator,
      _T("ID_BUTTON4"));
  FlexGridSizer11->Add(btnClose, 1, wxEXPAND, 5);

  lbCount = new wxStaticText(
      Panel2, ID_STATICTEXT8, _("Point count=0 \n (No decimation)"), wxDefaultPosition,
      wxDefaultSize, 0, _T("ID_STATICTEXT8"));
  FlexGridSizer11->Add(lbCount, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  lbLength = new wxStaticText(
      Panel2, ID_STATICTEXT2, wxEmptyString, wxDefaultPosition, wxDefaultSize, 0,
      _T("ID_STATICTEXT2"));
  FlexGridSizer11->Add(lbLength, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  FlexGridSizer9->Add(FlexGridSizer11, 1, wxEXPAND, 0);
  boxResults->Add(FlexGridSizer9, 1, wxEXPAND, 0);
  FlexGridSizer7->Add(boxResults, 1, wxEXPAND, 0);
  FlexGridSizer5->Add(FlexGridSizer7, 1, wxEXPAND, 3);
  FlexGridSizer4->Add(FlexGridSizer5, 1, wxEXPAND, 0);
  FlexGridSizer2->Add(FlexGridSizer4, 1, wxEXPAND, 0);
  edOpts = new wxTextCtrl(
      Panel2, ID_TEXTCTRL1,
      _("; ====================================================\n; "
        "MULTIMETRIC MAP CONFIGURATION\n; "
        "====================================================\n[map]\n; "
        "Creation of maps:\noccupancyGrid_count = 0\ngasGrid_count = "
        "0\nbeaconMap_count = 0\npointsMap_count = "
        "1\nheightMap_count = 0\ncolourPointsMap_count=0\n\n; "
        "====================================================\n; MULTIMETRIC "
        "MAP: PointsMap #00\n; "
        "====================================================\n; Insertion "
        "Options for PointsMap "
        "00:\n[map_pointsMap_00_insertOpts]\nminDistBetweenLaserPoints = "
        "0.05\nisPlanarMap = 0\nalso_interpolate = 0\n\n; "
        "====================================================\n; MULTIMETRIC "
        "MAP: HeightMap #00\n; "
        "====================================================\n; Creation "
        "Options for HeightMap "
        "00:\n[map_heightGrid_00_creationOpts]\nmapType = 0 \t\t; See "
        "CHeightGridMap2D::CHeightGridMap2D\nmin_x = -10\nmax_x = 10\nmin_y "
        "= -10\nmax_y = 10\nresolution = 0.10\n\n; "
        "====================================================\n; MULTIMETRIC "
        "MAP: HeightMap #00\n; "
        "====================================================\n; Insertion "
        "Options for HeightMap "
        "00:\n[map_heightGrid_00_insertOpts]\nfilterByHeight = 0 ; 0/1: Do "
        "not/do filter.\nz_min =-0.10\nz_max = 0.10\n\n; "
        "====================================================\n; MULTIMETRIC "
        "MAP: ColourPointsMap #00\n; "
        "====================================================\n; Insertion "
        "Options for ColourPointsMap "
        "00:\n[map_colourPointsMap_00_insertOpts]\nminDistBetweenLaserPoints "
        "= 0.05\nisPlanarMap = 0\nalso_interpolate = 0\n\n; Additional "
        "options for use in RTK GPS-based map "
        "building\n[RTK_MAP]\ndisableGPSInterp = 0 // If set to 1, disable "
        "GPS interpolation for sequences of JAVAD readings\nsmooth_filter = "
        "3 // Smooth pitch & roll angles by averaging each value with a "
        "window of N previous and next elements (=0: Disable)\n"),
      wxDefaultPosition, wxSize(247, 94),
      wxTE_PROCESS_ENTER | wxTE_PROCESS_TAB | wxTE_MULTILINE | wxHSCROLL, wxDefaultValidator,
      _T("ID_TEXTCTRL1"));
  wxFont edOptsFont(
      8, wxFONTFAMILY_SWISS, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_NORMAL, false, _T("Courier New"),
      wxFONTENCODING_DEFAULT);
  edOpts->SetFont(edOptsFont);
  FlexGridSizer2->Add(edOpts, 1, wxEXPAND, 1);
  Panel2->SetSizer(FlexGridSizer2);
  FlexGridSizer2->Fit(Panel2);
  FlexGridSizer2->SetSizeHints(Panel2);
  FlexGridSizer1->Add(Panel2, 1, wxEXPAND, 0);
  Panel3 = new wxPanel(
      this, ID_PANEL3, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL3"));
  BoxSizer1 = new wxBoxSizer(wxHORIZONTAL);
  plotMap = new mpWindow(Panel3, ID_CUSTOM2, wxDefaultPosition, wxSize(742, 380), 0);
  BoxSizer1->Add(plotMap, 1, wxEXPAND, 0);
  Panel3->SetSizer(BoxSizer1);
  BoxSizer1->Fit(Panel3);
  BoxSizer1->SetSizeHints(Panel3);
  FlexGridSizer1->Add(Panel3, 1, wxEXPAND, 0);
  SetSizer(FlexGridSizer1);
  FlexGridSizer1->Fit(this);
  FlexGridSizer1->SetSizeHints(this);
  Center();

  Bind(wxEVT_SCROLL_THUMBTRACK, &CFormRawMap::OnslFromCmdScrollThumbTrack, this, ID_SLIDER1);
  Bind(wxEVT_SCROLL_CHANGED, &CFormRawMap::OnslFromCmdScrollThumbTrack, this, ID_SLIDER1);
  Bind(wxEVT_SCROLL_THUMBTRACK, &CFormRawMap::OnslToCmdScrollThumbTrack, this, ID_SLIDER2);
  Bind(wxEVT_SCROLL_CHANGED, &CFormRawMap::OnslToCmdScrollThumbTrack, this, ID_SLIDER2);
  Bind(wxEVT_SCROLL_THUMBTRACK, &CFormRawMap::OnslDecimateCmdScrollThumbTrack, this, ID_SLIDER3);
  Bind(wxEVT_SCROLL_CHANGED, &CFormRawMap::OnslDecimateCmdScrollThumbTrack, this, ID_SLIDER3);
  Bind(wxEVT_BUTTON, &CFormRawMap::OnbtnGenerateClick, this, ID_BUTTON2);
  Bind(wxEVT_BUTTON, &CFormRawMap::OnGenerateFromRTK, this, ID_BUTTON6);
  Bind(wxEVT_BUTTON, &CFormRawMap::OnbtnGeneratePathsClick, this, ID_BUTTON5);
  Bind(wxEVT_BUTTON, &CFormRawMap::OnbtnSaveTxtClick, this, ID_BUTTON1);
  Bind(wxEVT_BUTTON, &CFormRawMap::OnbtnSave3DClick, this, ID_BUTTON3);
  Bind(wxEVT_BUTTON, &CFormRawMap::OnbtnSavePathClick, this, ID_BUTTON7);
  Bind(wxEVT_BUTTON, &CFormRawMap::OnbtnSaveObsPathClick, this, ID_BUTTON8);
  Bind(wxEVT_BUTTON, &CFormRawMap::OnbtnView3DClick, this, ID_BUTTON9);
  Bind(wxEVT_BUTTON, &CFormRawMap::OnbtnCloseClick, this, ID_BUTTON4);
  //*)

  Maximize();
  FlexGridSizer1->Layout();
}

CFormRawMap::~CFormRawMap()
{
  //(*Destroy(CFormRawMap)
  //*)
}

void loadMapInto3DScene(Scene& scene)
{
  {
    TPoint3D minC, maxC;
    if (robot_path.size())
      robot_path.getBoundingBox(minC, maxC);
    else
    {
      minC = TPoint3D(-100, -100, 0);
      maxC = TPoint3D(100, 100, 0);
    }

    mrpt::opengl::CGridPlaneXY::Ptr gridobj = mrpt::opengl::CGridPlaneXY::Create(
        minC.x - 20, maxC.x + 20, minC.y - 20, maxC.y + 20, minC.z - 2, 5);
    gridobj->setColor(0.3, 0.3, 0.3, 1);
    scene.insert(gridobj);
  }

  // Camera points at path beginning:
  if (!robot_path.empty())
  {
    auto& p = robot_path.begin()->second;
    scene.getViewport("main")->getCamera().setPointingAt(mrpt::poses::CPose3D(p));
    scene.enableFollowCamera(true);
  }

  // The robot path:
  {
    mrpt::opengl::CSetOfLines::Ptr obj = mrpt::opengl::CSetOfLines::Create();

    obj->setColor(0, 1, 0, 0.5);
    obj->setLineWidth(4);

    mrpt::opengl::CSetOfLines::Ptr obj2 = mrpt::opengl::CSetOfLines::Create();
    obj2->setColor(1, 0, 0, 0.5);
    obj2->setLineWidth(2);

    double x0 = 0, y0 = 0, z0 = 0;
    std::optional<mrpt::Clock::time_point> last_t;

    for (auto& it : robot_path)
    {
      auto& p = it.second;
      auto this_t = it.first;

      if (distanceBetweenPoints(x0, y0, z0, p.x, p.y, p.z) < 5.5)
      {
        obj->appendLine(x0, y0, z0, p.x, p.y, p.z);
      }
      else if (last_t)
      {
        // We have a gap without GT:
        // Try to interpolate using the best GPS path:
        // map<Clock::time_point,CPoint3D> best_gps_path;		// time
        // -> 3D local coords
        auto i1 = rtk_path_info.best_gps_path.lower_bound(last_t.value());
        auto i2 = rtk_path_info.best_gps_path.upper_bound(this_t);

        // cout << mrpt::system::timeLocalToString(last_t) << " -> " <<
        // mrpt::system::timeLocalToString(this_t) << " D: " <<
        // std::distance(i1,i2) << endl;

        if (i1 != rtk_path_info.best_gps_path.end())
        {
          for (auto t = i1; t != i2 && t != rtk_path_info.best_gps_path.end(); ++t)
          {
            obj2->appendLine(x0, y0, z0, t->second.x, t->second.y, t->second.z);
            x0 = t->second.x;
            y0 = t->second.y;
            z0 = t->second.z;
          }
        }
      }
      // For the next loop:
      x0 = p.x;
      y0 = p.y;
      z0 = p.z;
      last_t = this_t;
    }

    // Perhaps we have one final segment of the path without GT:
    if (!rtk_path_info.best_gps_path.empty())
    {
      auto this_t = rtk_path_info.best_gps_path.rbegin()->first;
      // We have a gap without GT:
      // Try to interpolate using the best GPS path:
      // map<Clock::time_point,CPoint3D> best_gps_path;		// time ->
      // 3D local coords
      auto i1 = rtk_path_info.best_gps_path.lower_bound(last_t.value());
      auto i2 = rtk_path_info.best_gps_path.upper_bound(this_t);

      // cout << mrpt::system::timeLocalToString(last_t) << " -> " <<
      // mrpt::system::timeLocalToString(this_t) << " D: " <<
      // std::distance(i1,i2) << endl;

      if (i1 != rtk_path_info.best_gps_path.end())
      {
        for (auto t = i1; t != i2 && t != rtk_path_info.best_gps_path.end(); ++t)
        {
          obj2->appendLine(x0, y0, z0, t->second.x, t->second.y, t->second.z);
          x0 = t->second.x;
          y0 = t->second.y;
          z0 = t->second.z;
        }
      }
    }

    scene.insert(obj);
    scene.insert(obj2);
  }

  // The built maps:
  // ---------------------------
  scene.insert(theMap.getVisualization());
}

// From slider moved:
void CFormRawMap::OnslFromCmdScrollThumbTrack(wxScrollEvent&)
{
  int toVal = slTo->GetValue();
  int curVal = slFrom->GetValue();

  if (curVal > toVal) slFrom->SetValue(toVal);
  edFirst->SetValue(wxString::Format(_("%d"), slFrom->GetValue()));
}

// To slider moved:
void CFormRawMap::OnslToCmdScrollThumbTrack(wxScrollEvent&)
{
  int fromVal = slFrom->GetValue();
  int curVal = slTo->GetValue();

  if (curVal < fromVal) slTo->SetValue(fromVal);
  edLast->SetValue(wxString::Format(_("%d"), slTo->GetValue()));
}

// Generate the map:
void CFormRawMap::OnbtnGenerateClick(wxCommandEvent&)
{
  WX_START_TRY

  // Go: generate the map:
  size_t i;
  CPose2D curPose(0, 0, 0);

  size_t first = edFirst->GetValue();
  size_t last = edLast->GetValue();
  size_t decimate = edDec->GetValue();
  ASSERT_(first <= last);
  ASSERT_(last <= rawlog.size() - 1);
  ASSERT_(decimate > 0);

  // Create a memory "ini file" with the text in the window:
  CConfigFileMemory configSrc(std::string(edOpts->GetValue().mb_str()));

  TSetOfMetricMapInitializers lstMaps;
  lstMaps.loadFromConfigFile(configSrc, "map");
  theMap.setListOfMaps(lstMaps);

  CPointsMap::Ptr thePntsMap;
  if (auto p = theMap.mapByClass<CSimplePointsMap>(); p)
    thePntsMap = std::dynamic_pointer_cast<CPointsMap>(p);
  else if (auto p2 = theMap.mapByClass<CColouredPointsMap>(); p2)
    thePntsMap = std::dynamic_pointer_cast<CPointsMap>(p2);

  wxBusyCursor waitCursor;

  wxProgressDialog progDia(
      wxT("Creating raw map"), wxT("Working..."),
      (int)(last - first + 1),  // range
      this,                     // parent
      wxPD_CAN_ABORT | wxPD_APP_MODAL | wxPD_SMOOTH | wxPD_AUTO_HIDE | wxPD_ELAPSED_TIME |
          wxPD_ESTIMATED_TIME | wxPD_REMAINING_TIME);

  wxTheApp->Yield();  // Let the app. process messages
  size_t count = 0;

  vector<float> pathX, pathY;
  bool abort = false;

  robot_path.clear();

  // An (aprox) estimate of the final size of the map (great improve in
  // speed!)
  if (thePntsMap) thePntsMap->reserve((last - first + 1) * 800);

  std::optional<mrpt::Clock::time_point> last_tim;

  for (i = first; !abort && i <= last; i++)
  {
    bool addNewPathEntry = false;

    switch (rawlog.getType(i))
    {
      case CRawlog::etActionCollection:
      {
        CActionCollection::Ptr acts = rawlog.getAsAction(i);
        CPose2D poseIncrement;
        bool poseIncrementLoaded = false;

        for (size_t j = 0; j < acts->size(); j++)
        {
          CAction::Ptr act = acts->get(j);
          if (act->GetRuntimeClass() == CLASS_ID(CActionRobotMovement2D))
          {
            CActionRobotMovement2D::Ptr mov =
                std::dynamic_pointer_cast<CActionRobotMovement2D>(act);

            // Load an odometry estimation, but only if it is the
            // only movement
            //  estimation source: any other may be a better one:
            if (!poseIncrementLoaded || mov->estimationMethod != CActionRobotMovement2D::emOdometry)
            {
              poseIncrementLoaded = true;
              mov->poseChange->getMean(poseIncrement);
            }
          }
        }

        if (!poseIncrementLoaded && i < last)
          THROW_EXCEPTION_FMT("ERROR: Odometry not found at step %d!", (int)i);

        curPose = curPose + poseIncrement;
        addNewPathEntry = true;
      }
      break;
      case CRawlog::etSensoryFrame:
      {
        if (((i >> 1) % decimate) == 0)
        {
          CPose3D dumPose(curPose);
          rawlog.getAsObservations(i)->insertObservationsInto(theMap, dumPose);
        }
        addNewPathEntry = true;
      }
      break;
      case CRawlog::etObservation:
      {
        // Always, process odometry:
        const CObservation* obs = rawlog.getAsObservation(i).get();
        if (IS_CLASS(*obs, CObservationOdometry))
        {
          const auto* obsOdo = static_cast<const CObservationOdometry*>(obs);
          curPose = obsOdo->odometry;
        }

        if (((i >> 1) % decimate) == 0)
        {
          CPose3D dumPose(curPose);
          theMap.insertObservation(*rawlog.getAsObservation(i), dumPose);
          last_tim = rawlog.getAsObservation(i)->timestamp;
        }
        addNewPathEntry = true;
      }
      break;
      default:
        break;
    };  // end switch

    if (addNewPathEntry)
    {
      pathX.push_back(curPose.x());
      pathY.push_back(curPose.y());
      if (last_tim) robot_path.insert(last_tim.value(), CPose3D(curPose));
    }

    if ((count++ % 50) == 0)
    {
      if (!progDia.Update((int)(i - first))) abort = true;
      wxTheApp->Yield();
    }
  }  // end for i

  progDia.Update((int)(last - first + 1));

  // Load into the graphs:
  // ----------------------------------
  plotMap->DelAllLayers(true, false);

  mpFXYVector* lyPoints = new mpFXYVector();
  mpFXYVector* lyPath = new mpFXYVector();
  lyPath->SetPen(wxPen(wxColour(255, 0, 0), 2));
  lyPath->SetContinuity(true);
  lyPoints->SetPen(wxPen(wxColour(0, 0, 255), 1));

  plotMap->AddLayer(lyPoints);
  plotMap->AddLayer(lyPath);

  lyPath->SetData(pathX, pathY);

  if (thePntsMap)
  {
    size_t nPts = thePntsMap->size();
    size_t decimation = 1;

    if (nPts > 100000)
    {
      decimation = nPts / 100000;
    }

    vector<float> Xs, Ys;
    thePntsMap->getAllPoints(Xs, Ys, decimation);

    lyPoints->SetData(Xs, Ys);
    plotMap->LockAspect(false);
    plotMap->Fit();  // Update the window to show the new data fitted.
    plotMap->LockAspect(true);
    plotMap->AddLayer(new mpScaleX());
    plotMap->AddLayer(new mpScaleY());

    if (decimation > 1)
      lbCount->SetLabel(wxString::Format(
          _("Point count=%u\n(Decimation=%u)"), unsigned(Xs.size()), unsigned(decimation)));
    else
      lbCount->SetLabel(
          wxString::Format(_("Point count=%u\n(No decimation)"), unsigned(Xs.size())));
  }

  // Enable "results" buttons:
  btnSaveTxt->Enable();
  btnSave3D->Enable();
  btnSavePath->Enable();
  btnSaveTxt->Enable();
  btnSaveObsPath->Enable();
  btnView3D->Enable();

  WX_END_TRY
}

void CFormRawMap::OnbtnSaveTxtClick(wxCommandEvent&)
{
  wxString caption = wxT("Save as text file...");
  wxString wildcard = wxT("Text files (*.txt)|*.txt|All files (*.*)|*.*");

  wxString defaultDir((iniFile->read_string(iniFileSect, "LastDir", ".").c_str()));

  wxString defaultFilename = _("map.txt");
  wxFileDialog dialog(
      this, caption, defaultDir, defaultFilename, wildcard, wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

  if (dialog.ShowModal() == wxID_OK)
  {
    wxString fileName = dialog.GetPath();
    try
    {
      theMap.saveMetricMapRepresentationToFile(std::string(fileName.mb_str()));
    }
    catch (const std::exception& e)
    {
      wxMessageBox(mrpt::exception_to_str(e), _("Exception"), wxOK, this);
    }
  }
}

// Save as a 3D scene:
void CFormRawMap::OnbtnSave3DClick(wxCommandEvent&)
{
  wxString caption = wxT("Save as 3D scene file...");
  wxString wildcard = wxT("MRPT 3D scene files (*.3Dscene)|*.3Dscene|All files (*.*)|*.*");

  wxString defaultDir((iniFile->read_string(iniFileSect, "LastDir", ".").c_str()));

  wxString defaultFilename = _("map.3Dscene");
  wxFileDialog dialog(
      this, caption, defaultDir, defaultFilename, wildcard, wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

  if (dialog.ShowModal() == wxID_OK)
  {
    try
    {
      wxBusyCursor waitCursor;

      CFileGZOutputStream fil(std::string(dialog.GetPath().mb_str()));
      Scene scene;

      loadMapInto3DScene(scene);

      archiveFrom(fil) << scene;
    }
    catch (const std::exception& e)
    {
      wxMessageBox(mrpt::exception_to_str(e), _("Exception"), wxOK, this);
    }
  }
}

// Close:
void CFormRawMap::OnbtnCloseClick(wxCommandEvent&) { Close(); }
void CFormRawMap::OnslDecimateCmdScrollThumbTrack(wxScrollEvent&)
{
  edDec->SetValue(wxString::Format(_("%d"), slDecimate->GetValue()));
}

// Generate random paths from the motion model:
void CFormRawMap::OnbtnGeneratePathsClick(wxCommandEvent&)
{
  WX_START_TRY

  // Go: generate the map:
  size_t i;

  long nPaths = ::wxGetNumberFromUser(
      _("Each incremental pose will be drawn from its probabilistic motion "
        "model)"),
      _("How many random samples of the path to generate?\n"), _("Generation of random paths"),
      100 /*Default*/, 1 /*Min*/, 10000 /*Max*/);

  if (nPaths < 0) return;

  size_t first = edFirst->GetValue();
  size_t last = edLast->GetValue();
  size_t decimate = edDec->GetValue();
  ASSERT_(first <= last);
  ASSERT_(last <= rawlog.size() - 1);
  ASSERT_(decimate > 0);
  ASSERT_(nPaths > 0 && nPaths < 100000);

  // Load into the graphs:
  // ----------------------------------
  plotMap->DelAllLayers(true, false);

  wxBusyCursor waitCursor;

  size_t nComputationSteps = (last - first + 1) * nPaths;

  wxProgressDialog progDia(
      wxT("Generating paths"), wxT("Working..."),
      (int)nComputationSteps,  // range
      this,                    // parent
      wxPD_CAN_ABORT | wxPD_APP_MODAL | wxPD_SMOOTH | wxPD_AUTO_HIDE | wxPD_ELAPSED_TIME |
          wxPD_ESTIMATED_TIME | wxPD_REMAINING_TIME);

  wxTheApp->Yield();  // Let the app. process messages
  size_t count = 0;

  bool abort = false;

  CPosePDFParticles pdfParts(nPaths);

  for (int pathIter = 0; !abort && pathIter < nPaths; pathIter++)
  {
    vector<float> pathX, pathY, pathPhi;
    CPose2D curPose(0, 0, 0);

    // Initial pose:
    pathX.push_back(0);
    pathY.push_back(0);
    pathPhi.push_back(0);

    for (i = first; !abort && i <= last; i++)
    {
      switch (rawlog.getType(i))
      {
        case CRawlog::etActionCollection:
        {
          CActionCollection::Ptr acts = rawlog.getAsAction(i);
          CPose2D poseIncrement;
          bool poseIncrementLoaded = false;

          for (size_t j = 0; j < acts->size(); j++)
          {
            CAction::Ptr act = acts->get(j);
            if (act->GetRuntimeClass() == CLASS_ID(CActionRobotMovement2D))
            {
              CActionRobotMovement2D::Ptr mov =
                  std::dynamic_pointer_cast<CActionRobotMovement2D>(act);

              // Load an odometry estimation, but only if it is
              // the only movement
              //  estimation source: any other may be a better
              //  one:
              if (!poseIncrementLoaded ||
                  mov->estimationMethod != CActionRobotMovement2D::emOdometry)
              {
                poseIncrementLoaded = true;
                // Draw random pose:
                mov->drawSingleSample(poseIncrement);
              }
            }
          }

          if (!poseIncrementLoaded)
            THROW_EXCEPTION_FMT("ERROR: Odometry not found at step %d!", (int)i);

          curPose = curPose + poseIncrement;
          pathX.push_back(curPose.x());
          pathY.push_back(curPose.y());
          pathPhi.push_back(curPose.phi());
        }
        break;

        default:
        {
          // It's an observation... nothing to do.
        }
      };

      if ((count++ % 50) == 0)
      {
        if (!progDia.Update((int)count)) abort = true;
        wxTheApp->Yield();
      }
    }  // end for i

    if (!abort)
    {
      // Save for the covariance:
      // ----------------------------
      pdfParts.m_particles[pathIter].d =
          TPose2D(pathX[pathX.size() - 1], pathY[pathX.size() - 1], pathPhi[pathX.size() - 1]);

      // Add new layer with this path:
      // ----------------------------------
      mpFXYVector* lyPath = new mpFXYVector();
      lyPath->SetPen(wxPen(wxColour(255, 0, 0), 1));
      lyPath->SetContinuity(true);

      // Load into the graphs:
      lyPath->SetData(pathX, pathY);

      plotMap->AddLayer(lyPath, false);
    }

  }  // end for path samples

  progDia.Update((int)nComputationSteps);

  // Add a layer with a covariance with the last pose:
  const auto [COV, meanPose] = pdfParts.getCovarianceAndMean();

  mpCovarianceEllipse* lyCov = new mpCovarianceEllipse(COV(0, 0), COV(1, 1), COV(0, 1));
  lyCov->SetContinuity(true);
  lyCov->SetCoordinateBase(meanPose.x(), meanPose.y(), 0);
  plotMap->AddLayer(lyCov, false);

  //

  plotMap->Fit();  // Update the window to show the new data fitted.
  plotMap->LockAspect(true);
  plotMap->AddLayer(new mpScaleX());
  plotMap->AddLayer(new mpScaleY());

  lbCount->SetLabel(wxString::Format(_("%d paths"), nPaths));

  WX_END_TRY
}

// ----------------------------------------------------------------------------------------
// Generate a "Ground-truth" path from RTK GPS data, then build maps from those
// goos poses
// ----------------------------------------------------------------------------------------
void CFormRawMap::OnGenerateFromRTK(wxCommandEvent&)
{
  WX_START_TRY

  // Go: generate the map:
  size_t first = edFirst->GetValue();
  size_t last = edLast->GetValue();
  size_t decimate = slDecimate->GetValue();
  ASSERT_(first <= last);
  ASSERT_(last <= rawlog.size() - 1);
  ASSERT_(decimate > 0);

  // Create a memory "ini file" with the text in the window:
  CConfigFileMemory configSrc(std::string(edOpts->GetValue().mb_str()));

  TSetOfMetricMapInitializers lstMaps;
  lstMaps.loadFromConfigFile(configSrc, "map");
  theMap.setListOfMaps(lstMaps);

  // -------------------------------------------
  // Run path reconstruction:
  // -------------------------------------------
  mrpt::topography::path_from_rtk_gps(
      robot_path, rawlog, first, last,
      true,  // Is GUI
      configSrc.read_bool("RTK_MAP", "disableGPSInterp", false),
      configSrc.read_int("RTK_MAP", "smooth_filter", 1), &rtk_path_info);

  // ---------------------------------------------------
  //   Now, build the map using the interpolator
  // --------------------------------------------------
  wxBusyCursor waitCursor;

  CPointsMap::Ptr thePntsMap;
  if (auto p = theMap.mapByClass<CSimplePointsMap>(); p)
    thePntsMap = std::dynamic_pointer_cast<CPointsMap>(p);
  else if (auto p2 = theMap.mapByClass<CColouredPointsMap>(); p2)
    thePntsMap = std::dynamic_pointer_cast<CPointsMap>(p2);

  // An (aprox) estimate of the final size of the map (great improve in
  // speed!)
  if (thePntsMap) thePntsMap->reserve(last - first + 1);

  size_t count = 0;
  bool abort = false;

  wxProgressDialog progDia2(
      wxString::Format(wxT("Building map - %u GPS points"), (unsigned)robot_path.size()),
      wxT("Populating the map with observations..."),
      (int)(last - first + 1),  // range
      this,                     // parent
      wxPD_CAN_ABORT | wxPD_APP_MODAL | wxPD_SMOOTH | wxPD_AUTO_HIDE | wxPD_ELAPSED_TIME |
          wxPD_ESTIMATED_TIME | wxPD_REMAINING_TIME);

  progDia2.SetSize(400, progDia2.GetSize().GetHeight());
  wxTheApp->Yield();  // Let the app. process messages

  for (size_t i = first; !abort && i <= last; i++)
  {
    switch (rawlog.getType(i))
    {
      default:
        break;

      case CRawlog::etObservation:
      {
        CObservation::Ptr o = rawlog.getAsObservation(i);

        // Interpolate:
        CPose3D p;
        bool valid_interp;
        robot_path.interpolate(o->timestamp, p, valid_interp);

        if (valid_interp)
        {
          // Decimation counters for each sensor label:
          static std::map<string, size_t> decim_count;
          size_t& dec_cnt = decim_count[o->sensorLabel];

          if ((++dec_cnt % decimate) == 0) theMap.insertObservation(*o, p);
        }
      }
      break;
    }  // end switch type

    // Show progress:
    if ((count++ % 100) == 0)
    {
      if (!progDia2.Update((int)(i - first))) abort = true;
      wxTheApp->Yield();
    }
  }  // end for i

  progDia2.Update((int)(last - first + 1));

  // Load into the graphs:
  // ----------------------------------
  vector<float> pathX, pathY;
  pathX.reserve(robot_path.size());
  pathY.reserve(robot_path.size());

  double overall_dist = 0;
  double last_x = 0, last_y = 0, last_z = 0;

  for (auto i = robot_path.begin(); i != robot_path.end(); ++i)
  {
    if (i != robot_path.begin())
      overall_dist +=
          (mrpt::math::TPoint3D(i->second) - mrpt::math::TPoint3D(last_x, last_y, last_z)).norm();
    last_x = i->second.x;
    last_y = i->second.y;
    last_z = i->second.z;

    pathX.push_back(i->second.x);
    pathY.push_back(i->second.y);
  }

  plotMap->DelAllLayers(true, false);

  mpFXYVector* lyPoints = new mpFXYVector();
  mpFXYVector* lyPath = new mpFXYVector();
  lyPath->SetPen(wxPen(wxColour(255, 0, 0), 2));
  lyPath->SetContinuity(true);
  lyPoints->SetPen(wxPen(wxColour(0, 0, 255), 1));

  plotMap->AddLayer(lyPoints);
  plotMap->AddLayer(lyPath);

  lyPath->SetData(pathX, pathY);

  if (thePntsMap)
  {
    size_t nPts = thePntsMap->size();
    size_t decimation = 1;

    if (nPts > 100000)
    {
      decimation = nPts / 100000;
    }

    vector<float> Xs, Ys;
    thePntsMap->getAllPoints(Xs, Ys, decimation);

    lyPoints->SetData(Xs, Ys);
    plotMap->LockAspect(false);
    plotMap->Fit();  // Update the window to show the new data fitted.
    plotMap->LockAspect(true);
    plotMap->AddLayer(new mpScaleX());
    plotMap->AddLayer(new mpScaleY());

    if (decimation > 1)
      lbCount->SetLabel(wxString::Format(
          _("Point count=%u\n(Decimation=%u)"), unsigned(Xs.size()), unsigned(decimation)));
    else
      lbCount->SetLabel(
          wxString::Format(_("Point count=%u\n(No decimation)"), unsigned(Xs.size())));
  }

  lbLength->SetLabel(wxString::Format(_("Overall path length: %.03f meters"), overall_dist));

  // Enable "results" buttons:
  btnSaveTxt->Enable();
  btnSave3D->Enable();
  btnSavePath->Enable();
  btnSaveTxt->Enable();
  btnSaveObsPath->Enable();
  btnView3D->Enable();

  WX_END_TRY
}

void CFormRawMap::OnbtnSavePathClick(wxCommandEvent&)
{
  WX_START_TRY

  if (!robot_path.size())
  {
    wxMessageBox(_("The robot path is empty."));
    return;
  }

  string outputPath;

  // ---------------------------------------------
  // Save the vehicle path
  // ---------------------------------------------
  wxString defaultDir((iniFile->read_string(iniFileSect, "LastDir", ".").c_str()));
  {
    wxFileDialog dialog(
        this, wxT("Save path as txt file..."), defaultDir, wxT("GT_path_vehicle.txt"),
        wxT("Text files (*.txt)|*.txt|All files (*.*)|*.*"), wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

    if (dialog.ShowModal() != wxID_OK) return;

    wxBusyCursor waitCursor;

    cout << robot_path.size() << endl;

    std::ofstream f(string(dialog.GetPath().mb_str()));
    for (const auto& rp : robot_path)
    {
      const double t = mrpt::Clock::toDouble(rp.first);
      const auto& p = rp.second;
      f << mrpt::format(
          "%.06f %.06f %.06f %.06f %.06f %.06f %.06f ", t, p.x, p.y, p.z, p.yaw, p.pitch, p.roll);

      // The uncertainty, if available:
      auto Wit = rtk_path_info.vehicle_uncertainty.find(rp.first);
      if (Wit != rtk_path_info.vehicle_uncertainty.end())
      {
        const CMatrixDouble66& C = Wit->second;

        f << mrpt::format(
            " %e %e %e %e %e %e %e %e %e %e %e %e %e %e %e %e %e %e %e "
            "%e %e\n",
            C(0, 0), C(0, 1), C(0, 2), C(0, 3), C(0, 4), C(0, 5), C(1, 1), C(1, 2), C(1, 3),
            C(1, 4), C(1, 5), C(2, 2), C(2, 3), C(2, 4), C(2, 5), C(3, 3), C(3, 4), C(3, 5),
            C(4, 4), C(4, 5), C(5, 5));
      }
      else
        f << mrpt::format(
            " %e %e %e %e %e %e %e %e %e %e %e %e %e %e %e %e %e %e %e "
            "%e %e\n",
            1e-4, 0.0, 0.0, 0.0, 0.0, 0.0, 1e-4, 0.0, 0.0, 0.0, 0.0, 1e-4, 0.0, 0.0, 0.0, 1e-4, 0.0,
            0.0, 1e-4, 0.0, 1e-4);
    }

    outputPath = mrpt::system::extractFileDirectory(string(dialog.GetPath().mb_str()));
  }

  // ---------------------------------------------
  // Save the vehicle path (binary)
  // ---------------------------------------------
  {
    wxFileDialog dialog(
        this, wxT("Save path as binary file..."), defaultDir, wxT("GT_path_vehicle.bin.gz"),
        wxT("Binary gz-compressed files (*.bin.gz)|*.bin.gz|All files "
            "(*.*)|*.*"),
        wxFD_SAVE | wxFD_OVERWRITE_PROMPT);
    if (dialog.ShowModal() != wxID_OK) return;

    wxBusyCursor waitCursor;
    CFileGZOutputStream f(string(dialog.GetPath().mb_str()));
    archiveFrom(f) << robot_path;
  }

  // ---------------------------------------------
  // Save the vehicle path, interpolated 100Hz
  // ---------------------------------------------
  {
    wxFileDialog dialog(
        this, wxT("Save interpolated path as txt file..."), defaultDir,
        wxT("GT_path_vehicle_interp.txt"), wxT("Text files (*.txt)|*.txt|All files (*.*)|*.*"),
        wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

    if (dialog.ShowModal() != wxID_OK) return;

    wxBusyCursor waitCursor;
    using namespace std::chrono_literals;
    const auto interval = 10ms;
    if (!robot_path.saveInterpolatedToTextFile(string(dialog.GetPath().mb_str()), interval))
      ::wxMessageBox(_("Error creating file."));
  }

  // ---------------------------------------------
  // Save quality measure
  // ---------------------------------------------
  if (!rtk_path_info.mahalabis_quality_measure.empty())
  {
    wxFileDialog dialog(
        this, wxT("Save path confidence measure..."), defaultDir,
        wxT("GT_path_vehicle_confidence.txt"), wxT("Text files (*.txt)|*.txt|All files (*.*)|*.*"),
        wxFD_SAVE | wxFD_OVERWRITE_PROMPT);
    if (dialog.ShowModal() != wxID_OK) return;

    wxBusyCursor waitCursor;
    std::ofstream f(string(dialog.GetPath().mb_str()));

    for (const auto& pi : rtk_path_info.mahalabis_quality_measure)
      f << mrpt::format("%.06f %.06e\n", mrpt::Clock::toDouble(pi.first), pi.second);
  }

  // ---------------------------------------------
  // Save other sensors GT predicted path
  // ---------------------------------------------
  {
    std::vector<std::string> the_labels = theMainWindow->AskForObservationByLabelMultiple(
        "Choose additional sensors to export GT path:");
    if (the_labels.size() == 0) return;

    // Standard matrix used for all the sensors on the vehicle:
    CMatrixDouble COV_sensor_local;
    COV_sensor_local.setZero(6, 6);

    {
      CConfigFileMemory cfg;
      rawlog.getCommentTextAsConfigFile(cfg);
      COV_sensor_local(0, 0) = square(cfg.read_double("ERROR_SENSOR_POSES", "std_x", 1e-3));
      COV_sensor_local(1, 1) = square(cfg.read_double("ERROR_SENSOR_POSES", "std_y", 1e-3));
      COV_sensor_local(2, 2) = square(cfg.read_double("ERROR_SENSOR_POSES", "std_z", 1e-3));
      COV_sensor_local(3, 3) =
          square(DEG2RAD(cfg.read_double("ERROR_SENSOR_POSES", "std_yaw", 1e-3)));
      COV_sensor_local(4, 4) =
          square(DEG2RAD(cfg.read_double("ERROR_SENSOR_POSES", "std_pitch", 1e-3)));
      COV_sensor_local(5, 5) =
          square(DEG2RAD(cfg.read_double("ERROR_SENSOR_POSES", "std_roll", 1e-3)));
    }

    ASSERT_(COV_sensor_local.rows() == 6 && COV_sensor_local.isSquare());

    std::vector<std::string>::iterator itStr;
    std::vector<FILE*> outFiles(the_labels.size());
    std::vector<FILE*>::iterator itOutFiles;

    // Generate output files
    for (itStr = the_labels.begin(), itOutFiles = outFiles.begin(); itStr != the_labels.end();
         itStr++, itOutFiles++)
    {
      *itOutFiles = mrpt::system::os::fopen(
          format("%s/GT_path_%s.txt", outputPath.c_str(), itStr->c_str()), "wt");
      ASSERT_(*itOutFiles);
    }

    wxBusyCursor waitCursor;

    CRawlog::iterator itRawlog;
    for (itRawlog = rawlog.begin(); itRawlog != rawlog.end(); itRawlog++)
    {
      if (itRawlog.getType() == mrpt::obs::CRawlog::etObservation)
      {
        CObservation::Ptr obs(std::dynamic_pointer_cast<CObservation>(*itRawlog));
        for (itStr = the_labels.begin(), itOutFiles = outFiles.begin(); itStr != the_labels.end();
             itStr++, itOutFiles++)
        {
          if (obs->sensorLabel == *itStr)
          {
            bool valid;
            CPose3D intRobotPose;
            robot_path.interpolate(obs->timestamp, intRobotPose, valid);
            if (!valid) continue;

            CPose3DPDFGaussian veh_pose;
            veh_pose.mean.setFromValues(0, 0, 0, 0, 0, 0);
            if (rtk_path_info.W_star.rows() == 6 && rtk_path_info.W_star.cols() == 6)
            {
              // Uncertainty estimation:
              veh_pose.cov = rtk_path_info.W_star;
            }
            else
            {
              veh_pose.cov.setIdentity();
              veh_pose.cov *= 1e-6;
            }

            // add the transformation of the vehicle:
            veh_pose.changeCoordinatesReference(intRobotPose);

            // add now that of the sensor:
            CPose3DPDFGaussian sensor_on_the_vehicle;
            obs->getSensorPose(sensor_on_the_vehicle.mean);
            sensor_on_the_vehicle.cov = COV_sensor_local;

            CPose3DPDFGaussian global_sensor_pose =
                veh_pose + sensor_on_the_vehicle;  // Global sensor
            // pose with
            // uncert.

            // save the composed path:
            {
              CPose3D& p = global_sensor_pose.mean;
              mrpt::system::os::fprintf(
                  *itOutFiles, "%.06f %.06f %.06f %.06f %.06f %.06f %.06f ",
                  mrpt::Clock::toDouble(obs->timestamp), p.x(), p.y(), p.z(), p.yaw(), p.pitch(),
                  p.roll());

              CMatrixDouble66& C = global_sensor_pose.cov;
              mrpt::system::os::fprintf(
                  *itOutFiles,
                  "%e %e %e %e %e %e %e %e %e %e %e %e %e %e %e "
                  "%e %e %e %e %e %e\n",
                  C(0, 0), C(0, 1), C(0, 2), C(0, 3), C(0, 4), C(0, 5), C(1, 1), C(1, 2), C(1, 3),
                  C(1, 4), C(1, 5), C(2, 2), C(2, 3), C(2, 4), C(2, 5), C(3, 3), C(3, 4), C(3, 5),
                  C(4, 4), C(4, 5), C(5, 5));
            }
          }  // end if
        }    // end for
      }      // end if
    }        // end for

    for (itOutFiles = outFiles.begin(); itOutFiles != outFiles.end(); itOutFiles++)
      mrpt::system::os::fclose(*itOutFiles);
  }

  WX_END_TRY
}

void CFormRawMap::OnbtnSaveObsPathClick(wxCommandEvent&) {}
void CFormRawMap::OnbtnView3DClick(wxCommandEvent&)
{
  Scene scene;

  loadMapInto3DScene(scene);

  win3Dmap = CDisplayWindow3D::Create("Raw-map 3D preview");
  Scene::Ptr the_scene = win3Dmap->get3DSceneAndLock();
  *the_scene = scene;
  win3Dmap->unlockAccess3DScene();
  win3Dmap->repaint();
}

void CFormRawMap::OnbtnHelpClick(wxCommandEvent&) {}
