/* +------------------------------------------------------------------------+
   |                     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 "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();
  }
}
