/* +------------------------------------------------------------------------+
   |                     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 "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\nlandmarksMap_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 = timestampTotime_t(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", timestampTotime_t(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 ",
								timestampTotime_t(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&) {}
