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

//(*InternalHeaders(CFormChangeSensorPositions)
#include <wx/intl.h>
#include <wx/string.h>
//*)
#include <wx/app.h>
#include <wx/busyinfo.h>
#include <wx/filedlg.h>
#include <wx/imaglist.h>
#include <wx/msgdlg.h>
#include <wx/progdlg.h>

#include "xRawLogViewerMain.h"
// General global variables:

#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/obs/CActionCollection.h>
#include <mrpt/obs/CObservationImage.h>
#include <mrpt/obs/CSensoryFrame.h>
#include <mrpt/serialization/CArchive.h>
#include <mrpt/system/filesystem.h>
#include <mrpt/system/os.h>

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

// The index within the SF to process:
size_t idxToProcess = 0;
string labelToProcess;

bool sensorPoseReadOK;
TPose3D sensorPoseToSet, sensorPoseToRead;
bool changeOnlyXYZ = false;

bool camReadIsOk;
CMatrixDouble33 camIntrinsic;
std::vector<double> camDistortion;
float camFocalLen;

//(*IdInit(CFormChangeSensorPositions)
const long CFormChangeSensorPositions::ID_RADIOBUTTON1 = wxNewId();
const long CFormChangeSensorPositions::ID_RADIOBUTTON2 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT27 = wxNewId();
const long CFormChangeSensorPositions::ID_TEXTCTRL16 = wxNewId();
const long CFormChangeSensorPositions::ID_BUTTON9 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT28 = wxNewId();
const long CFormChangeSensorPositions::ID_TEXTCTRL17 = wxNewId();
const long CFormChangeSensorPositions::ID_BUTTON11 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICLINE1 = wxNewId();
const long CFormChangeSensorPositions::ID_RADIOBOX1 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT15 = wxNewId();
const long CFormChangeSensorPositions::ID_SPINCTRL1 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT30 = wxNewId();
const long CFormChangeSensorPositions::ID_COMBOBOX1 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT16 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT1 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT2 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT3 = wxNewId();
const long CFormChangeSensorPositions::ID_TEXTCTRL1 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT4 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT5 = wxNewId();
const long CFormChangeSensorPositions::ID_TEXTCTRL2 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT6 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT7 = wxNewId();
const long CFormChangeSensorPositions::ID_TEXTCTRL3 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT8 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT9 = wxNewId();
const long CFormChangeSensorPositions::ID_TEXTCTRL4 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT10 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT11 = wxNewId();
const long CFormChangeSensorPositions::ID_TEXTCTRL5 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT12 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT13 = wxNewId();
const long CFormChangeSensorPositions::ID_TEXTCTRL6 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT14 = wxNewId();
const long CFormChangeSensorPositions::ID_CHECKBOX1 = wxNewId();
const long CFormChangeSensorPositions::ID_BUTTON3 = wxNewId();
const long CFormChangeSensorPositions::ID_BUTTON1 = wxNewId();
const long CFormChangeSensorPositions::ID_PANEL1 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT17 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT18 = wxNewId();
const long CFormChangeSensorPositions::ID_TEXTCTRL7 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT19 = wxNewId();
const long CFormChangeSensorPositions::ID_TEXTCTRL8 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT20 = wxNewId();
const long CFormChangeSensorPositions::ID_TEXTCTRL9 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT21 = wxNewId();
const long CFormChangeSensorPositions::ID_TEXTCTRL10 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT23 = wxNewId();
const long CFormChangeSensorPositions::ID_TEXTCTRL12 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT24 = wxNewId();
const long CFormChangeSensorPositions::ID_TEXTCTRL13 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT25 = wxNewId();
const long CFormChangeSensorPositions::ID_TEXTCTRL14 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT26 = wxNewId();
const long CFormChangeSensorPositions::ID_TEXTCTRL15 = wxNewId();
const long CFormChangeSensorPositions::ID_STATICTEXT22 = wxNewId();
const long CFormChangeSensorPositions::ID_TEXTCTRL11 = wxNewId();
const long CFormChangeSensorPositions::ID_BUTTON4 = wxNewId();
const long CFormChangeSensorPositions::ID_BUTTON5 = wxNewId();
const long CFormChangeSensorPositions::ID_PANEL2 = wxNewId();
const long CFormChangeSensorPositions::ID_NOTEBOOK1 = wxNewId();
const long CFormChangeSensorPositions::ID_BUTTON2 = wxNewId();
//*)

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

CFormChangeSensorPositions::CFormChangeSensorPositions(
	wxWindow* parent, wxWindowID id)
{
	//(*Initialize(CFormChangeSensorPositions)
	wxStaticBoxSizer* StaticBoxSizer2;
	wxFlexGridSizer* FlexGridSizer4;
	wxFlexGridSizer* FlexGridSizer16;
	wxFlexGridSizer* FlexGridSizer10;
	wxFlexGridSizer* FlexGridSizer3;
	wxFlexGridSizer* FlexGridSizer5;
	wxFlexGridSizer* FlexGridSizer9;
	wxFlexGridSizer* FlexGridSizer2;
	wxFlexGridSizer* FlexGridSizer7;
	wxStaticBoxSizer* StaticBoxSizer3;
	wxFlexGridSizer* FlexGridSizer15;
	wxFlexGridSizer* FlexGridSizer8;
	wxFlexGridSizer* FlexGridSizer13;
	wxFlexGridSizer* FlexGridSizer12;
	wxFlexGridSizer* FlexGridSizer6;
	wxStaticBoxSizer* StaticBoxSizer1;
	wxFlexGridSizer* FlexGridSizer1;
	wxFlexGridSizer* FlexGridSizer11;
	wxFlexGridSizer* FlexGridSizer17;

	Create(
		parent, id, _("Change sensor pose information:"), wxDefaultPosition,
		wxDefaultSize, wxDEFAULT_DIALOG_STYLE, _T("id"));
	FlexGridSizer1 = new wxFlexGridSizer(3, 1, 0, 0);
	FlexGridSizer1->AddGrowableCol(0);
	StaticBoxSizer1 = new wxStaticBoxSizer(wxHORIZONTAL, this, _("Apply to:"));
	FlexGridSizer13 = new wxFlexGridSizer(0, 1, 0, 0);
	FlexGridSizer13->AddGrowableCol(0);
	FlexGridSizer13->AddGrowableRow(0);
	FlexGridSizer14 = new wxFlexGridSizer(2, 4, 0, 0);
	FlexGridSizer14->AddGrowableCol(2);
	rbLoaded = new wxRadioButton(
		this, ID_RADIOBUTTON1, _("Loaded rawlog"), wxDefaultPosition,
		wxDefaultSize, 0, wxDefaultValidator, _T("ID_RADIOBUTTON1"));
	FlexGridSizer14->Add(
		rbLoaded, 1, wxALL | wxALIGN_LEFT | wxALIGN_CENTER_VERTICAL, 5);
	FlexGridSizer14->Add(
		-1, -1, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	FlexGridSizer14->Add(
		-1, -1, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	FlexGridSizer14->Add(
		-1, -1, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	rbFile = new wxRadioButton(
		this, ID_RADIOBUTTON2, _("Rawlog in file:"), wxDefaultPosition,
		wxDefaultSize, 0, wxDefaultValidator, _T("ID_RADIOBUTTON2"));
	FlexGridSizer14->Add(
		rbFile, 1, wxALL | wxALIGN_LEFT | wxALIGN_CENTER_VERTICAL, 5);
	StaticText27 = new wxStaticText(
		this, ID_STATICTEXT27, _("Input file:"), wxDefaultPosition,
		wxDefaultSize, 0, _T("ID_STATICTEXT27"));
	FlexGridSizer14->Add(
		StaticText27, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	txtInputFile = new wxTextCtrl(
		this, ID_TEXTCTRL16, wxEmptyString, wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL16"));
	FlexGridSizer14->Add(txtInputFile, 1, wxEXPAND, 5);
	btnPickInput = new wxButton(
		this, ID_BUTTON9, _("Select..."), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_BUTTON9"));
	FlexGridSizer14->Add(
		btnPickInput, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 0);
	FlexGridSizer14->Add(
		-1, -1, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	StaticText28 = new wxStaticText(
		this, ID_STATICTEXT28, _("Output file:"), wxDefaultPosition,
		wxDefaultSize, 0, _T("ID_STATICTEXT28"));
	FlexGridSizer14->Add(
		StaticText28, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	txtOutputFile = new wxTextCtrl(
		this, ID_TEXTCTRL17, wxEmptyString, wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL17"));
	FlexGridSizer14->Add(txtOutputFile, 1, wxEXPAND, 5);
	btnPickOut = new wxButton(
		this, ID_BUTTON11, _("Select..."), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_BUTTON11"));
	FlexGridSizer14->Add(
		btnPickOut, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 0);
	FlexGridSizer13->Add(FlexGridSizer14, 1, wxEXPAND, 0);
	StaticLine1 = new wxStaticLine(
		this, ID_STATICLINE1, wxDefaultPosition, wxSize(10, -1),
		wxLI_HORIZONTAL, _T("ID_STATICLINE1"));
	FlexGridSizer13->Add(StaticLine1, 1, wxEXPAND, 0);
	FlexGridSizer16 = new wxFlexGridSizer(0, 2, 0, 0);
	FlexGridSizer16->AddGrowableCol(1);
	FlexGridSizer16->AddGrowableRow(0);
	FlexGridSizer3 = new wxFlexGridSizer(1, 1, 0, 0);
	FlexGridSizer3->AddGrowableCol(0);
	FlexGridSizer3->AddGrowableRow(0);
	wxString __wxRadioBoxChoices_1[2] = {
		_("Select by index within SF"), _("Select by label")};
	rbApply = new wxRadioBox(
		this, ID_RADIOBOX1, _("Apply to..."), wxDefaultPosition,
		wxSize(202, 67), 2, __wxRadioBoxChoices_1, 1, wxRA_HORIZONTAL,
		wxDefaultValidator, _T("ID_RADIOBOX1"));
	FlexGridSizer3->Add(rbApply, 1, wxEXPAND, 5);
	FlexGridSizer16->Add(FlexGridSizer3, 1, wxEXPAND, 0);
	FlexGridSizer17 = new wxFlexGridSizer(2, 2, 0, 0);
	FlexGridSizer17->AddGrowableCol(1);
	StaticText15 = new wxStaticText(
		this, ID_STATICTEXT15, _("Index within CSensoryFrame"),
		wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT15"));
	FlexGridSizer17->Add(
		StaticText15, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	scIndex = new wxSpinCtrl(
		this, ID_SPINCTRL1, _T("0"), wxDefaultPosition, wxDefaultSize, 0, 0,
		100, 0, _T("ID_SPINCTRL1"));
	scIndex->SetValue(_T("0"));
	FlexGridSizer17->Add(scIndex, 1, wxEXPAND, 5);
	StaticText30 = new wxStaticText(
		this, ID_STATICTEXT30, _("Observations by label:"), wxDefaultPosition,
		wxDefaultSize, 0, _T("ID_STATICTEXT30"));
	FlexGridSizer17->Add(
		StaticText30, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	edLabel = new wxComboBox(
		this, ID_COMBOBOX1, wxEmptyString, wxDefaultPosition, wxDefaultSize, 0,
		nullptr, 0, wxDefaultValidator, _T("ID_COMBOBOX1"));
	edLabel->Disable();
	FlexGridSizer17->Add(edLabel, 1, wxEXPAND, 5);
	FlexGridSizer16->Add(FlexGridSizer17, 1, wxEXPAND, 0);
	FlexGridSizer13->Add(FlexGridSizer16, 1, wxEXPAND, 0);
	StaticBoxSizer1->Add(FlexGridSizer13, 1, wxEXPAND, 0);
	FlexGridSizer1->Add(StaticBoxSizer1, 1, wxEXPAND, 5);
	StaticText16 = new wxStaticText(
		this, ID_STATICTEXT16,
		_("What should be changed in those observations\?"), wxDefaultPosition,
		wxDefaultSize, 0, _T("ID_STATICTEXT16"));
	FlexGridSizer1->Add(
		StaticText16, 1, wxALL | wxALIGN_LEFT | wxALIGN_CENTER_VERTICAL, 2);
	Notebook1 = new wxNotebook(
		this, ID_NOTEBOOK1, wxDefaultPosition, wxDefaultSize, 0,
		_T("ID_NOTEBOOK1"));
	Panel1 = new wxPanel(
		Notebook1, ID_PANEL1, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
		_T("ID_PANEL1"));
	FlexGridSizer4 = new wxFlexGridSizer(2, 1, 0, 0);
	FlexGridSizer4->AddGrowableCol(0);
	FlexGridSizer5 = new wxFlexGridSizer(4, 6, 0, 0);
	FlexGridSizer5->AddGrowableCol(1);
	FlexGridSizer5->AddGrowableCol(4);
	FlexGridSizer5->Add(
		-1, -1, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	StaticText1 = new wxStaticText(
		Panel1, ID_STATICTEXT1, _("3D position:"), wxDefaultPosition,
		wxDefaultSize, 0, _T("ID_STATICTEXT1"));
	FlexGridSizer5->Add(StaticText1, 1, wxEXPAND, 5);
	FlexGridSizer5->Add(
		-1, -1, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	FlexGridSizer5->Add(
		-1, -1, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	StaticText2 = new wxStaticText(
		Panel1, ID_STATICTEXT2, _("3D angles (if applicable):"),
		wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT2"));
	FlexGridSizer5->Add(StaticText2, 1, wxEXPAND, 5);
	FlexGridSizer5->Add(
		-1, -1, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	StaticText3 = new wxStaticText(
		Panel1, ID_STATICTEXT3, _("x:"), wxDefaultPosition, wxDefaultSize, 0,
		_T("ID_STATICTEXT3"));
	FlexGridSizer5->Add(
		StaticText3, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	edX = new wxTextCtrl(
		Panel1, ID_TEXTCTRL1, _("0"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL1"));
	FlexGridSizer5->Add(edX, 1, wxEXPAND, 5);
	StaticText4 = new wxStaticText(
		Panel1, ID_STATICTEXT4, _("(meters)"), wxDefaultPosition, wxDefaultSize,
		0, _T("ID_STATICTEXT4"));
	FlexGridSizer5->Add(
		StaticText4, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	StaticText5 = new wxStaticText(
		Panel1, ID_STATICTEXT5, _("yaw:"), wxDefaultPosition, wxDefaultSize, 0,
		_T("ID_STATICTEXT5"));
	FlexGridSizer5->Add(
		StaticText5, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	edYaw = new wxTextCtrl(
		Panel1, ID_TEXTCTRL2, _("0"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL2"));
	FlexGridSizer5->Add(edYaw, 1, wxEXPAND, 5);
	StaticText6 = new wxStaticText(
		Panel1, ID_STATICTEXT6, _("(deg)"), wxDefaultPosition, wxDefaultSize, 0,
		_T("ID_STATICTEXT6"));
	FlexGridSizer5->Add(
		StaticText6, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	StaticText7 = new wxStaticText(
		Panel1, ID_STATICTEXT7, _("y:"), wxDefaultPosition, wxDefaultSize, 0,
		_T("ID_STATICTEXT7"));
	FlexGridSizer5->Add(
		StaticText7, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	edY = new wxTextCtrl(
		Panel1, ID_TEXTCTRL3, _("0"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL3"));
	FlexGridSizer5->Add(edY, 1, wxEXPAND, 5);
	StaticText8 = new wxStaticText(
		Panel1, ID_STATICTEXT8, _("(meters)"), wxDefaultPosition, wxDefaultSize,
		0, _T("ID_STATICTEXT8"));
	FlexGridSizer5->Add(
		StaticText8, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	StaticText9 = new wxStaticText(
		Panel1, ID_STATICTEXT9, _("pitch:"), wxDefaultPosition, wxDefaultSize,
		0, _T("ID_STATICTEXT9"));
	FlexGridSizer5->Add(
		StaticText9, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	edPitch = new wxTextCtrl(
		Panel1, ID_TEXTCTRL4, _("0"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL4"));
	FlexGridSizer5->Add(edPitch, 1, wxEXPAND, 5);
	StaticText10 = new wxStaticText(
		Panel1, ID_STATICTEXT10, _("(deg)"), wxDefaultPosition, wxDefaultSize,
		0, _T("ID_STATICTEXT10"));
	FlexGridSizer5->Add(
		StaticText10, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	StaticText11 = new wxStaticText(
		Panel1, ID_STATICTEXT11, _("z:"), wxDefaultPosition, wxDefaultSize, 0,
		_T("ID_STATICTEXT11"));
	FlexGridSizer5->Add(
		StaticText11, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	edZ = new wxTextCtrl(
		Panel1, ID_TEXTCTRL5, _("0"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL5"));
	FlexGridSizer5->Add(edZ, 1, wxEXPAND, 5);
	StaticText12 = new wxStaticText(
		Panel1, ID_STATICTEXT12, _("(meters)"), wxDefaultPosition,
		wxDefaultSize, 0, _T("ID_STATICTEXT12"));
	FlexGridSizer5->Add(
		StaticText12, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	StaticText13 = new wxStaticText(
		Panel1, ID_STATICTEXT13, _("roll:"), wxDefaultPosition, wxDefaultSize,
		0, _T("ID_STATICTEXT13"));
	FlexGridSizer5->Add(
		StaticText13, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	edRoll = new wxTextCtrl(
		Panel1, ID_TEXTCTRL6, _("0"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL6"));
	FlexGridSizer5->Add(edRoll, 1, wxEXPAND, 5);
	StaticText14 = new wxStaticText(
		Panel1, ID_STATICTEXT14, _("(deg)"), wxDefaultPosition, wxDefaultSize,
		0, _T("ID_STATICTEXT14"));
	FlexGridSizer5->Add(
		StaticText14, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	FlexGridSizer4->Add(FlexGridSizer5, 1, wxEXPAND, 0);
	FlexGridSizer15 = new wxFlexGridSizer(0, 3, 0, 0);
	cbOnlyXYZ = new wxCheckBox(
		Panel1, ID_CHECKBOX1, _("Change X,Y,Z only"), wxDefaultPosition,
		wxDefaultSize, 0, wxDefaultValidator, _T("ID_CHECKBOX1"));
	cbOnlyXYZ->SetValue(false);
	FlexGridSizer15->Add(
		cbOnlyXYZ, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	FlexGridSizer4->Add(FlexGridSizer15, 1, wxEXPAND, 0);
	FlexGridSizer6 = new wxFlexGridSizer(0, 3, 0, 0);
	FlexGridSizer6->AddGrowableCol(2);
	FlexGridSizer6->Add(
		-1, -1, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	btnGetCurPose = new wxButton(
		Panel1, ID_BUTTON3, _("Get current values..."), wxDefaultPosition,
		wxDefaultSize, 0, wxDefaultValidator, _T("ID_BUTTON3"));
	FlexGridSizer6->Add(
		btnGetCurPose, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	btnOK = new wxButton(
		Panel1, ID_BUTTON1, _("Apply changes..."), wxDefaultPosition,
		wxDefaultSize, 0, wxDefaultValidator, _T("ID_BUTTON1"));
	FlexGridSizer6->Add(btnOK, 1, wxEXPAND, 5);
	FlexGridSizer4->Add(FlexGridSizer6, 1, wxEXPAND, 0);
	Panel1->SetSizer(FlexGridSizer4);
	FlexGridSizer4->Fit(Panel1);
	FlexGridSizer4->SetSizeHints(Panel1);
	Panel2 = new wxPanel(
		Notebook1, ID_PANEL2, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL,
		_T("ID_PANEL2"));
	FlexGridSizer7 = new wxFlexGridSizer(0, 1, 0, 0);
	FlexGridSizer7->AddGrowableCol(0);
	FlexGridSizer7->AddGrowableRow(1);
	StaticText17 = new wxStaticText(
		Panel2, ID_STATICTEXT17,
		_("This applies to CObservationImages (\"Monocular cameras\" only)"),
		wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT17"));
	FlexGridSizer7->Add(
		StaticText17, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	FlexGridSizer9 = new wxFlexGridSizer(0, 2, 0, 0);
	FlexGridSizer9->AddGrowableCol(0);
	FlexGridSizer9->AddGrowableCol(1);
	FlexGridSizer9->AddGrowableRow(0);
	StaticBoxSizer2 = new wxStaticBoxSizer(
		wxHORIZONTAL, Panel2, _("Intrinsic parameters (in pixels)"));
	FlexGridSizer10 = new wxFlexGridSizer(0, 4, 0, 0);
	FlexGridSizer10->AddGrowableCol(1);
	FlexGridSizer10->AddGrowableCol(3);
	StaticText18 = new wxStaticText(
		Panel2, ID_STATICTEXT18, _("f_x="), wxDefaultPosition, wxDefaultSize, 0,
		_T("ID_STATICTEXT18"));
	FlexGridSizer10->Add(
		StaticText18, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	edFX = new wxTextCtrl(
		Panel2, ID_TEXTCTRL7, _("0"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL7"));
	FlexGridSizer10->Add(edFX, 1, wxEXPAND, 5);
	StaticText19 = new wxStaticText(
		Panel2, ID_STATICTEXT19, _("f_y="), wxDefaultPosition, wxDefaultSize, 0,
		_T("ID_STATICTEXT19"));
	FlexGridSizer10->Add(
		StaticText19, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	edFY = new wxTextCtrl(
		Panel2, ID_TEXTCTRL8, _("0"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL8"));
	FlexGridSizer10->Add(edFY, 1, wxEXPAND, 5);
	StaticText20 = new wxStaticText(
		Panel2, ID_STATICTEXT20, _("c_x="), wxDefaultPosition, wxDefaultSize, 0,
		_T("ID_STATICTEXT20"));
	FlexGridSizer10->Add(
		StaticText20, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	edCX = new wxTextCtrl(
		Panel2, ID_TEXTCTRL9, _("0"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL9"));
	FlexGridSizer10->Add(edCX, 1, wxEXPAND, 5);
	StaticText21 = new wxStaticText(
		Panel2, ID_STATICTEXT21, _("c_y="), wxDefaultPosition, wxDefaultSize, 0,
		_T("ID_STATICTEXT21"));
	FlexGridSizer10->Add(
		StaticText21, 1, wxALL | wxALIGN_RIGHT | wxALIGN_CENTER_VERTICAL, 5);
	edCY = new wxTextCtrl(
		Panel2, ID_TEXTCTRL10, _("0"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL10"));
	FlexGridSizer10->Add(edCY, 1, wxEXPAND, 5);
	StaticBoxSizer2->Add(FlexGridSizer10, 1, wxEXPAND, 0);
	FlexGridSizer9->Add(StaticBoxSizer2, 1, wxEXPAND, 5);
	StaticBoxSizer3 =
		new wxStaticBoxSizer(wxHORIZONTAL, Panel2, _("Distortion parameters"));
	FlexGridSizer11 = new wxFlexGridSizer(0, 4, 0, 0);
	StaticText23 = new wxStaticText(
		Panel2, ID_STATICTEXT23, _("k1="), wxDefaultPosition, wxDefaultSize, 0,
		_T("ID_STATICTEXT23"));
	FlexGridSizer11->Add(
		StaticText23, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	edK1 = new wxTextCtrl(
		Panel2, ID_TEXTCTRL12, _("0"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL12"));
	FlexGridSizer11->Add(
		edK1, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	StaticText24 = new wxStaticText(
		Panel2, ID_STATICTEXT24, _("k2="), wxDefaultPosition, wxDefaultSize, 0,
		_T("ID_STATICTEXT24"));
	FlexGridSizer11->Add(
		StaticText24, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	edK2 = new wxTextCtrl(
		Panel2, ID_TEXTCTRL13, _("0"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL13"));
	FlexGridSizer11->Add(
		edK2, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	StaticText25 = new wxStaticText(
		Panel2, ID_STATICTEXT25, _("p1="), wxDefaultPosition, wxDefaultSize, 0,
		_T("ID_STATICTEXT25"));
	FlexGridSizer11->Add(
		StaticText25, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	edP1 = new wxTextCtrl(
		Panel2, ID_TEXTCTRL14, _("0"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL14"));
	FlexGridSizer11->Add(
		edP1, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	StaticText26 = new wxStaticText(
		Panel2, ID_STATICTEXT26, _("p2="), wxDefaultPosition, wxDefaultSize, 0,
		_T("ID_STATICTEXT26"));
	FlexGridSizer11->Add(
		StaticText26, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	edP2 = new wxTextCtrl(
		Panel2, ID_TEXTCTRL15, _("0"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL15"));
	FlexGridSizer11->Add(
		edP2, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	StaticBoxSizer3->Add(FlexGridSizer11, 1, wxEXPAND, 0);
	FlexGridSizer9->Add(StaticBoxSizer3, 1, wxEXPAND, 5);
	FlexGridSizer7->Add(FlexGridSizer9, 1, wxEXPAND, 0);
	FlexGridSizer12 = new wxFlexGridSizer(0, 2, 0, 0);
	StaticText22 = new wxStaticText(
		Panel2, ID_STATICTEXT22, _("Focal length (in meters) ="),
		wxDefaultPosition, wxDefaultSize, 0, _T("ID_STATICTEXT22"));
	FlexGridSizer12->Add(
		StaticText22, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	edFocalLen = new wxTextCtrl(
		Panel2, ID_TEXTCTRL11, _("0"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_TEXTCTRL11"));
	FlexGridSizer12->Add(
		edFocalLen, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	FlexGridSizer7->Add(FlexGridSizer12, 1, wxEXPAND, 0);
	FlexGridSizer8 = new wxFlexGridSizer(0, 3, 0, 0);
	FlexGridSizer8->AddGrowableCol(2);
	FlexGridSizer8->Add(
		-1, -1, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	btnGetCurCamModel = new wxButton(
		Panel2, ID_BUTTON4, _("Get current values..."), wxDefaultPosition,
		wxDefaultSize, 0, wxDefaultValidator, _T("ID_BUTTON4"));
	FlexGridSizer8->Add(
		btnGetCurCamModel, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	btnApplyCameraParams = new wxButton(
		Panel2, ID_BUTTON5, _("Apply changes..."), wxDefaultPosition,
		wxDefaultSize, 0, wxDefaultValidator, _T("ID_BUTTON5"));
	FlexGridSizer8->Add(btnApplyCameraParams, 1, wxEXPAND, 5);
	FlexGridSizer7->Add(FlexGridSizer8, 1, wxEXPAND, 0);
	Panel2->SetSizer(FlexGridSizer7);
	FlexGridSizer7->Fit(Panel2);
	FlexGridSizer7->SetSizeHints(Panel2);
	Notebook1->AddPage(Panel1, _("Sensor pose on robot"), false);
	Notebook1->AddPage(Panel2, _("Camera parameters"), false);
	FlexGridSizer1->Add(Notebook1, 1, wxEXPAND, 5);
	FlexGridSizer2 = new wxFlexGridSizer(0, 3, 0, 0);
	FlexGridSizer2->AddGrowableCol(0);
	FlexGridSizer2->Add(
		-1, -1, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL,
		5);
	btnCancel = new wxButton(
		this, ID_BUTTON2, _("Cancel"), wxDefaultPosition, wxDefaultSize, 0,
		wxDefaultValidator, _T("ID_BUTTON2"));
	FlexGridSizer2->Add(
		btnCancel, 1,
		wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
	FlexGridSizer1->Add(FlexGridSizer2, 1, wxEXPAND, 0);
	SetSizer(FlexGridSizer1);
	FlexGridSizer1->Fit(this);
	FlexGridSizer1->SetSizeHints(this);
	Center();

	Bind(
		wxEVT_RADIOBUTTON, &CFormChangeSensorPositions::OnrbLoadedSelect, this,
		ID_RADIOBUTTON1);
	Bind(
		wxEVT_RADIOBUTTON, &CFormChangeSensorPositions::OnrbFileSelect, this,
		ID_RADIOBUTTON2);
	Bind(
		wxEVT_BUTTON, &CFormChangeSensorPositions::OnbtnPickInputClick, this,
		ID_BUTTON9);
	Bind(
		wxEVT_BUTTON, &CFormChangeSensorPositions::OnbtnPickOutClick, this,
		ID_BUTTON11);
	Bind(
		wxEVT_RADIOBOX, &CFormChangeSensorPositions::OnrbApplySelect, this,
		ID_RADIOBOX1);
	Bind(
		wxEVT_BUTTON, &CFormChangeSensorPositions::OnbtnGetCurPoseClick1, this,
		ID_BUTTON3);
	Bind(
		wxEVT_BUTTON, &CFormChangeSensorPositions::OnbtnOKClick, this,
		ID_BUTTON1);
	Bind(
		wxEVT_BUTTON, &CFormChangeSensorPositions::OnbtnGetCurCamModelClick,
		this, ID_BUTTON4);
	Bind(
		wxEVT_BUTTON, &CFormChangeSensorPositions::OnbtnApplyCameraParamsClick,
		this, ID_BUTTON5);
	Bind(
		wxEVT_BUTTON, &CFormChangeSensorPositions::OnbtnCancelClick, this,
		ID_BUTTON2);
	Bind(
		wxEVT_INIT_DIALOG, &CFormChangeSensorPositions::OnInit, this, wxID_ANY);
	//*)
}

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

void CFormChangeSensorPositions::OnbtnCancelClick(wxCommandEvent& event)
{
	Close();
}

void CFormChangeSensorPositions::OnrbLoadedSelect(wxCommandEvent& event)
{
	btnPickInput->Disable();
	btnPickOut->Disable();
	txtOutputFile->Disable();
	txtInputFile->Disable();
}

void CFormChangeSensorPositions::OnrbFileSelect(wxCommandEvent& event)
{
	btnPickInput->Enable();
	btnPickOut->Enable();
	txtOutputFile->Enable();
	txtInputFile->Enable();
}

void CFormChangeSensorPositions::OnbtnPickInputClick(wxCommandEvent& event)
{
	WX_START_TRY

	wxFileDialog dialog(
		this, _("Select input rawlog file") /* caption */,
		(iniFile->read_string(iniFileSect, "LastDir", ".").c_str()),
		_("*.rawlog") /* defaultFilename */,
		_("Rawlog files (*.rawlog,*.rawlog.gz)|*.rawlog;*.rawlog.gz|All files "
		  "(*.*)|*.*") /* wildcard */,
		wxFD_OPEN | wxFD_FILE_MUST_EXIST);

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

	// Save the path
	WX_START_TRY
	iniFile->write(
		iniFileSect, "LastDir", std::string(dialog.GetDirectory().mb_str()));
	WX_END_TRY

	txtInputFile->SetValue(dialog.GetPath());

	WX_END_TRY
}

void CFormChangeSensorPositions::OnbtnPickOutClick(wxCommandEvent& event)
{
	WX_START_TRY

	wxFileDialog dialog(
		this, _("Select output rawlog file") /* caption */,
		(iniFile->read_string(iniFileSect, "LastDir", ".").c_str()),
		_("*.rawlog") /* defaultFilename */,
		_("Rawlog files (*.rawlog,*.rawlog.gz)|*.rawlog;*.rawlog.gz|All files "
		  "(*.*)|*.*") /* wildcard */,
		wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

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

	// Save the path
	WX_START_TRY
	iniFile->write(
		iniFileSect, "LastDir", std::string(dialog.GetDirectory().mb_str()));
	WX_END_TRY

	txtOutputFile->SetValue(dialog.GetPath());

	WX_END_TRY
}

void CFormChangeSensorPositions::OnInit(wxInitDialogEvent& event)
{
	Center();
	wxCommandEvent dumm;

	// Is there any rawlog loaded??
	if (!rawlog.size())
	{
		// NO: Disable "modify current":
		OnrbFileSelect(dumm);
		rbFile->SetValue(true);
		rbLoaded->Disable();
	}
	else
	{
		// Select the loaded rawlog by default:
		OnrbLoadedSelect(dumm);
		rbLoaded->SetValue(true);
	}  // end there is loaded rawlog
}

/** This is the common function for all operations over a rawlog file ("filter"
 * a rawlog file into a new one) or over the loaded rawlog (depending on the
 * user selection in the GUI).
 */
void CFormChangeSensorPositions::executeOperationOnRawlog(
	TRawlogFilter operation, const char* endMsg)
{
	WX_START_TRY

	int processMax;
	bool isInMemory;
	CStream *in_fil = nullptr, *out_fil = nullptr;

	sensorPoseReadOK = false;
	camReadIsOk = false;

	if (rbLoaded->GetValue())
	{
		// APPLY TO rawlog in memory:
		isInMemory = true;

		processMax = (int)rawlog.size();
	}
	else
	{
		// APPLY TO rawlog files:
		isInMemory = false;

		if (!txtInputFile->GetValue().size())
			THROW_EXCEPTION("An input rawlog file must be selected");
		if (!txtOutputFile->GetValue().size())
			THROW_EXCEPTION("An output rawlog file must be selected");

		string fileName_IN(txtInputFile->GetValue().mbc_str());
		if (!mrpt::system::fileExists(fileName_IN))
			THROW_EXCEPTION("Input file does not exist!");

		string fileName_OUT(txtOutputFile->GetValue().mbc_str());

		if (!fileName_OUT.compare(fileName_IN))
			THROW_EXCEPTION("Input and output files must be different!");

		in_fil = new CFileGZInputStream(fileName_IN);
		out_fil = new CFileGZOutputStream(fileName_OUT);

		processMax = (int)in_fil->getTotalBytesCount();
	}

	wxProgressDialog progDia(
		wxT("Modifying rawlog"), wxT("Processing..."),
		processMax,	 // 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

	unsigned int countLoop = 0;
	bool keepLoading = true;
	string errorMsg;
	wxString auxStr;

	// Apply changes:
	int changes = 0;
	wxBusyCursor cursor;

	while (((!isInMemory && keepLoading) ||
			(isInMemory && countLoop < rawlog.size())) &&
		   !sensorPoseReadOK && !camReadIsOk)
	{
		CSerializable::Ptr newObj;
		try
		{
			if (isInMemory) { newObj = rawlog.getAsGeneric(countLoop); }
			else
			{
				archiveFrom(*in_fil) >> newObj;
			}

			// Check type:
			if (newObj->GetRuntimeClass() == CLASS_ID(CSensoryFrame))
			{
				// A sensory frame:
				CSensoryFrame::Ptr sf(
					std::dynamic_pointer_cast<CSensoryFrame>(newObj));

				// Process & save:
				operation(nullptr, sf.get(), changes);

				if (!isInMemory) archiveFrom(*out_fil) << *sf.get();
			}
			else if (newObj->GetRuntimeClass() == CLASS_ID(CActionCollection))
			{
				// This is an action:
				CActionCollection::Ptr acts =
					std::dynamic_pointer_cast<CActionCollection>(newObj);

				// Process & save:
				operation(
					dynamic_cast<CActionCollection*>(acts.get()), nullptr,
					changes);

				if (!isInMemory) archiveFrom(*out_fil) << *acts;
			}
			else if (newObj->GetRuntimeClass()->derivedFrom(
						 CLASS_ID(CObservation)))
			{
				// A sensory frame:
				CObservation::Ptr o(
					std::dynamic_pointer_cast<CObservation>(newObj));

				static CSensoryFrame sf;
				sf.clear();
				sf.insert(o);

				// Process & save:
				operation(nullptr, &sf, changes);

				if (!isInMemory) archiveFrom(*out_fil) << *o;
			}
			else
			{  // Unknown class:
				THROW_EXCEPTION(format(
					"Unexpected class found in the file: '%s'",
					newObj->GetRuntimeClass()->className));
			}
		}
		catch (exception& e)
		{
			errorMsg = mrpt::exception_to_str(e);
			keepLoading = false;
		}
		catch (...)
		{
			keepLoading = false;
		}

		// Step counter & update progress dialog
		if (countLoop++ % 300 == 0)
		{
			auxStr.sprintf(
				wxT("Processing... (%u objects processed)"), countLoop);
			int curProgr = isInMemory ? countLoop : (int)in_fil->getPosition();
			if (!progDia.Update(curProgr, auxStr)) keepLoading = false;
			wxTheApp->Yield();	// Let the app. process messages
		}

		// Delete only if processing file
		if (newObj && !isInMemory) { newObj.reset(); }

	}  // end while keep loading

	progDia.Update(processMax);	 // Close dialog.

	if (strlen(endMsg))
	{
		char tmpStr[1000];
		os::sprintf(
			tmpStr, sizeof(tmpStr), "%s %i\n\nEnd message:\n%s", endMsg,
			changes, errorMsg.c_str());
		wxMessageBox(tmpStr, _("Result:"), wxOK, this);
	}

	if (in_fil) delete in_fil;
	if (out_fil) delete out_fil;

	WX_END_TRY
}

// ------------------------------------------------------------
//    Set the sensor pose
// ------------------------------------------------------------
void exec_setPoseByIdx(
	mrpt::obs::CActionCollection* acts, mrpt::obs::CSensoryFrame* SF,
	int& changesCount)
{
	if (SF)
		if (SF->size() > idxToProcess)
		{
			CObservation::Ptr obs = SF->getObservationByIndex(idxToProcess);
			if (changeOnlyXYZ)
			{
				CPose3D tmpPose;
				obs->getSensorPose(tmpPose);
				tmpPose.setFromValues(
					sensorPoseToSet.x, sensorPoseToSet.y, sensorPoseToSet.z,
					tmpPose.yaw(), tmpPose.pitch(), tmpPose.roll());
				obs->setSensorPose(tmpPose);
			}
			else
			{
				obs->setSensorPose(sensorPoseToSet);
			}
			changesCount++;
		}
}

void exec_setPoseByLabel(
	mrpt::obs::CActionCollection* acts, mrpt::obs::CSensoryFrame* SF,
	int& changesCount)
{
	if (SF)
	{
		for (auto obs : *SF)
		{
			if (obs->sensorLabel == labelToProcess)
			{
				if (changeOnlyXYZ)
				{
					CPose3D tmpPose;
					obs->getSensorPose(tmpPose);
					tmpPose.setFromValues(
						sensorPoseToSet.x, sensorPoseToSet.y, sensorPoseToSet.z,
						tmpPose.yaw(), tmpPose.pitch(), tmpPose.roll());
					obs->setSensorPose(tmpPose);
				}
				else
				{
					obs->setSensorPose(sensorPoseToSet);
				}
				changesCount++;
			}
		}  // end for each obs.
	}
}

void CFormChangeSensorPositions::OnbtnOKClick(wxCommandEvent& event)
{
	WX_START_TRY

	// Set: sensorPoseToSet
	sensorPoseToSet.x = atof(string(edX->GetValue().mb_str()).c_str());
	sensorPoseToSet.y = atof(string(edY->GetValue().mb_str()).c_str());
	sensorPoseToSet.z = atof(string(edZ->GetValue().mb_str()).c_str());
	sensorPoseToSet.yaw =
		DEG2RAD(atof(string(edYaw->GetValue().mb_str()).c_str()));
	sensorPoseToSet.pitch =
		DEG2RAD(atof(string(edPitch->GetValue().mb_str()).c_str()));
	sensorPoseToSet.roll =
		DEG2RAD(atof(string(edRoll->GetValue().mb_str()).c_str()));

	changeOnlyXYZ = cbOnlyXYZ->GetValue();

	if (rbApply->GetSelection() == 0)
	{
		idxToProcess = scIndex->GetValue();
		executeOperationOnRawlog(
			exec_setPoseByIdx, "Sensor poses changed (by index): ");
	}
	else
	{
		labelToProcess = string(edLabel->GetValue().mb_str());
		executeOperationOnRawlog(
			exec_setPoseByLabel, "Sensor poses changed (by label): ");
	}

	WX_END_TRY
}

// ------------------------------------------------------------
//    Get the current sensor pose
// ------------------------------------------------------------
void exec_getCurrentPoseByIdx(
	mrpt::obs::CActionCollection* acts, mrpt::obs::CSensoryFrame* SF,
	int& changesCount)
{
	if (SF)
		if (SF->size() > idxToProcess)
		{
			SF->getObservationByIndex(idxToProcess)
				->getSensorPose(sensorPoseToRead);
			sensorPoseReadOK = true;
		}
}
void exec_getCurrentPoseByLabel(
	mrpt::obs::CActionCollection* acts, mrpt::obs::CSensoryFrame* SF,
	int& changesCount)
{
	if (SF && SF->size())
	{
		CObservation::Ptr o = SF->getObservationBySensorLabel(labelToProcess);
		if (o)
		{
			o->getSensorPose(sensorPoseToRead);
			sensorPoseReadOK = true;
		}
	}
}

void CFormChangeSensorPositions::OnbtnGetCurPoseClick1(wxCommandEvent& event)
{
	WX_START_TRY

	if (rbApply->GetSelection() == 0)
	{
		idxToProcess = scIndex->GetValue();
		executeOperationOnRawlog(exec_getCurrentPoseByIdx, "");
	}
	else
	{
		labelToProcess = string(edLabel->GetValue().mb_str());
		executeOperationOnRawlog(exec_getCurrentPoseByLabel, "");
	}

	if (sensorPoseReadOK)
	{
		// Put the sensor pose (stored in "sensorPoseToRead"):
		edX->SetValue((format("%f", sensorPoseToRead.x).c_str()));
		edY->SetValue((format("%f", sensorPoseToRead.y).c_str()));
		edZ->SetValue((format("%f", sensorPoseToRead.z).c_str()));
		edYaw->SetValue((format("%f", RAD2DEG(sensorPoseToRead.yaw)).c_str()));
		edPitch->SetValue(
			(format("%f", RAD2DEG(sensorPoseToRead.pitch)).c_str()));
		edRoll->SetValue(
			(format("%f", RAD2DEG(sensorPoseToRead.roll)).c_str()));
	}
	else
	{
		wxMessageBox(
			_("Error: No observation with the selected index has been found in "
			  "the rawlog!"));
	}

	WX_END_TRY
}

// ------------------------------------------------------------
//    Get the current camera model
// ------------------------------------------------------------
void exec_getCurrentCamCfgByIdx(
	mrpt::obs::CActionCollection* acts, mrpt::obs::CSensoryFrame* SF,
	int& changesCount)
{
	if (SF)
		if (SF->size() > idxToProcess)
		{
			if (SF->getObservationByIndex(idxToProcess)->GetRuntimeClass() ==
				CLASS_ID(CObservationImage))
			{
				auto obsIm =
					SF->getObservationByIndexAs<CObservationImage::Ptr>(
						idxToProcess);
				camDistortion =
					obsIm->cameraParams.getDistortionParamsAsVector();
				camIntrinsic = obsIm->cameraParams.intrinsicParams;
				camFocalLen = obsIm->cameraParams.focalLengthMeters;
				camReadIsOk = true;
			}
		}
}
void exec_getCurrentCamCfgByLabel(
	mrpt::obs::CActionCollection* acts, mrpt::obs::CSensoryFrame* SF,
	int& changesCount)
{
	if (SF && SF->size())
	{
		CObservation::Ptr o = SF->getObservationBySensorLabel(labelToProcess);

		if (o && o->GetRuntimeClass() == CLASS_ID(CObservationImage))
		{
			auto obsIm =
				SF->getObservationBySensorLabelAs<CObservationImage::Ptr>(
					labelToProcess);
			camDistortion = obsIm->cameraParams.getDistortionParamsAsVector();
			camIntrinsic = obsIm->cameraParams.intrinsicParams;
			camFocalLen = obsIm->cameraParams.focalLengthMeters;
			camReadIsOk = true;
		}
	}
}

void CFormChangeSensorPositions::OnbtnGetCurCamModelClick(wxCommandEvent& event)
{
	WX_START_TRY

	if (rbApply->GetSelection() == 0)
	{
		idxToProcess = scIndex->GetValue();
		executeOperationOnRawlog(exec_getCurrentCamCfgByIdx, "");
	}
	else
	{
		labelToProcess = string(edLabel->GetValue().mb_str());
		executeOperationOnRawlog(exec_getCurrentCamCfgByLabel, "");
	}

	if (camReadIsOk)
	{
		// Put the camara data:
		edFX->SetValue((format("%f", camIntrinsic(0, 0)).c_str()));
		edFY->SetValue((format("%f", camIntrinsic(1, 1)).c_str()));
		edCX->SetValue((format("%f", camIntrinsic(0, 2)).c_str()));
		edCY->SetValue((format("%f", camIntrinsic(1, 2)).c_str()));

		edK1->SetValue((format("%f", camDistortion[0]).c_str()));
		edK2->SetValue((format("%f", camDistortion[1]).c_str()));
		edP1->SetValue((format("%f", camDistortion[1]).c_str()));
		edP2->SetValue((format("%f", camDistortion[2]).c_str()));

		edFocalLen->SetValue((format("%f", camFocalLen).c_str()));
	}
	else
	{
		wxMessageBox(
			_("Error: No CObservationImage found in the rawlog with the "
			  "selected index!"));
	}

	WX_END_TRY
}

// ------------------------------------------------------------
//    Set the camera model
// ------------------------------------------------------------
void exec_setCurrentCamCfgByIdx(
	mrpt::obs::CActionCollection* acts, mrpt::obs::CSensoryFrame* SF,
	int& changesCount)
{
	if (SF)
		if (SF->size() > idxToProcess)
		{
			if (SF->getObservationByIndex(idxToProcess)->GetRuntimeClass() ==
				CLASS_ID(CObservationImage))
			{
				auto obsIm =
					SF->getObservationByIndexAs<CObservationImage::Ptr>(
						idxToProcess);
				obsIm->cameraParams.setDistortionParamsVector(camDistortion);
				obsIm->cameraParams.intrinsicParams = camIntrinsic;
				obsIm->cameraParams.focalLengthMeters = camFocalLen;
				changesCount++;
			}
		}
}
void exec_setCurrentCamCfgByLabel(
	mrpt::obs::CActionCollection* acts, mrpt::obs::CSensoryFrame* SF,
	int& changesCount)
{
	if (SF && SF->size())
	{
		CObservation::Ptr o = SF->getObservationBySensorLabel(labelToProcess);
		if (o && o->GetRuntimeClass() == CLASS_ID(CObservationImage))
		{
			auto obsIm =
				SF->getObservationBySensorLabelAs<CObservationImage::Ptr>(
					labelToProcess);
			obsIm->cameraParams.setDistortionParamsVector(camDistortion);
			obsIm->cameraParams.intrinsicParams = camIntrinsic;
			obsIm->cameraParams.focalLengthMeters = camFocalLen;
			changesCount++;
		}
	}
}

void CFormChangeSensorPositions::OnbtnApplyCameraParamsClick(
	wxCommandEvent& event)
{
	WX_START_TRY

	idxToProcess = scIndex->GetValue();

	// Put the camara data:
	camIntrinsic.setSize(3, 3);
	camIntrinsic.setZero();
	camIntrinsic(2, 2) = 1;
	camIntrinsic(0, 0) = atof(string(edFX->GetValue().mb_str()).c_str());
	camIntrinsic(1, 1) = atof(string(edFY->GetValue().mb_str()).c_str());
	camIntrinsic(0, 2) = atof(string(edCX->GetValue().mb_str()).c_str());
	camIntrinsic(1, 2) = atof(string(edCY->GetValue().mb_str()).c_str());

	camDistortion.resize(4);
	camDistortion[0] = atof(string(edK1->GetValue().mb_str()).c_str());
	camDistortion[1] = atof(string(edK2->GetValue().mb_str()).c_str());
	camDistortion[2] = atof(string(edP1->GetValue().mb_str()).c_str());
	camDistortion[3] = atof(string(edP2->GetValue().mb_str()).c_str());

	camFocalLen = atof(string(edFocalLen->GetValue().mb_str()).c_str());

	if (rbApply->GetSelection() == 0)
	{
		idxToProcess = scIndex->GetValue();
		executeOperationOnRawlog(
			exec_setCurrentCamCfgByIdx, "CObservationImage objects changed: ");
	}
	else
	{
		labelToProcess = string(edLabel->GetValue().mb_str());
		executeOperationOnRawlog(
			exec_setCurrentCamCfgByLabel,
			"CObservationImage objects changed: ");
	}

	WX_END_TRY
}

void CFormChangeSensorPositions::OnrbApplySelect(wxCommandEvent& event)
{
	if (rbApply->GetSelection() == 0)
	{
		scIndex->Enable();
		edLabel->Disable();
	}
	else
	{
		scIndex->Disable();
		edLabel->Enable();
	}
}
