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

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

#include "CDlgParams.h"
#include "CLogView.h"
#include "slamdemoApp.h"

//(*InternalHeaders(slamdemoFrame)
#include <wx/artprov.h>
#include <wx/bitmap.h>
#include <wx/font.h>
#include <wx/icon.h>
#include <wx/image.h>
#include <wx/intl.h>
#include <wx/string.h>
//*)
#include <mrpt/gui/about_box.h>
#include <mrpt/gui/wx28-fixes.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/io/vector_loadsave.h>
#include <mrpt/math/ops_matrices.h>
#include <mrpt/math/wrap2pi.h>
#include <mrpt/obs/CObservationComment.h>
#include <mrpt/opengl/Scene.h>
#include <mrpt/random.h>
#include <mrpt/serialization/CArchive.h>

#include <memory>

using namespace std;
using namespace mrpt;
using namespace mrpt::io;
using namespace mrpt::bayes;
using namespace mrpt::random;
using namespace mrpt::serialization;
using namespace mrpt::system;
using namespace mrpt::slam;
using namespace mrpt::obs;
using namespace mrpt::maps;
using namespace mrpt::math;
using namespace mrpt::poses;

#include "../wx-common/mrpt_logo.xpm"
#include "imgs/icon_batch.xpm"
#include "imgs/icon_config.xpm"
#include "imgs/icon_exit.xpm"
#include "imgs/icon_help.xpm"
#include "imgs/icon_play.xpm"
#include "imgs/icon_reset.xpm"
#include "imgs/icon_step.xpm"
#include "imgs/icon_stop.xpm"
#include "imgs/icono_main.xpm"

static const double RAD2DEGSQ = square(180 / M_PI);

// A custom Art provider for customizing the icons:
class MyArtProvider : public wxArtProvider
{
 protected:
  wxBitmap CreateBitmap(const wxArtID& id, const wxArtClient& client, const wxSize& size) override;
};
wxBitmap MyArtProvider::CreateBitmap(
    const wxArtID& id, const wxArtClient& client, const wxSize& size)
{
  if (id == wxART_MAKE_ART_ID(MAIN_ICON)) return wxBitmap(icono_main_xpm);
  if (id == wxART_MAKE_ART_ID(IMG_MRPT_LOGO)) return wxBitmap(mrpt_logo_xpm);

  if (id == wxART_MAKE_ART_ID(ICON_BATCH)) return wxBitmap(icon_batch_xpm);
  if (id == wxART_MAKE_ART_ID(ICON_CONFIG)) return wxBitmap(icon_config_xpm);
  if (id == wxART_MAKE_ART_ID(ICON_HELP)) return wxBitmap(icon_help_xpm);
  if (id == wxART_MAKE_ART_ID(ICON_EXIT)) return wxBitmap(icon_exit_xpm);
  if (id == wxART_MAKE_ART_ID(ICON_PLAY)) return wxBitmap(icon_play_xpm);
  if (id == wxART_MAKE_ART_ID(ICON_RESET)) return wxBitmap(icon_reset_xpm);
  if (id == wxART_MAKE_ART_ID(ICON_STEP)) return wxBitmap(icon_step_xpm);
  if (id == wxART_MAKE_ART_ID(ICON_STOP)) return wxBitmap(icon_stop_xpm);

  // Any wxWidgets icons not implemented here
  // will be provided by the default art provider.
  return wxNullBitmap;
}

//(*IdInit(slamdemoFrame)
const long slamdemoFrame::ID_STATICTEXT1 = wxNewId();
const long slamdemoFrame::ID_PANEL3 = wxNewId();
const long slamdemoFrame::ID_CUSTOM1 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT2 = wxNewId();
const long slamdemoFrame::ID_PANEL4 = wxNewId();
const long slamdemoFrame::ID_CUSTOM2 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT10 = wxNewId();
const long slamdemoFrame::ID_PANEL5 = wxNewId();
const long slamdemoFrame::ID_CUSTOM3 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT3 = wxNewId();
const long slamdemoFrame::ID_PANEL6 = wxNewId();
const long slamdemoFrame::ID_CUSTOM4 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT4 = wxNewId();
const long slamdemoFrame::ID_PANEL7 = wxNewId();
const long slamdemoFrame::ID_GRID1 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT5 = wxNewId();
const long slamdemoFrame::ID_PANEL9 = wxNewId();
const long slamdemoFrame::ID_CUSTOM7 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT6 = wxNewId();
const long slamdemoFrame::ID_PANEL10 = wxNewId();
const long slamdemoFrame::ID_CUSTOM8 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT7 = wxNewId();
const long slamdemoFrame::ID_PANEL11 = wxNewId();
const long slamdemoFrame::ID_CUSTOM9 = wxNewId();
const long slamdemoFrame::ID_PANEL1 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT9 = wxNewId();
const long slamdemoFrame::ID_PANEL8 = wxNewId();
const long slamdemoFrame::ID_CUSTOM5 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT11 = wxNewId();
const long slamdemoFrame::ID_PANEL14 = wxNewId();
const long slamdemoFrame::ID_CUSTOM6 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT12 = wxNewId();
const long slamdemoFrame::ID_PANEL15 = wxNewId();
const long slamdemoFrame::ID_CUSTOM11 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT13 = wxNewId();
const long slamdemoFrame::ID_PANEL16 = wxNewId();
const long slamdemoFrame::ID_CUSTOM12 = wxNewId();
const long slamdemoFrame::ID_PANEL2 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT8 = wxNewId();
const long slamdemoFrame::ID_PANEL13 = wxNewId();
const long slamdemoFrame::ID_CUSTOM10 = wxNewId();
const long slamdemoFrame::ID_STATICTEXT14 = wxNewId();
const long slamdemoFrame::ID_PANEL17 = wxNewId();
const long slamdemoFrame::ID_CUSTOM13 = wxNewId();
const long slamdemoFrame::ID_PANEL12 = wxNewId();
const long slamdemoFrame::ID_NOTEBOOK1 = wxNewId();
const long slamdemoFrame::ID_MENUITEM1 = wxNewId();
const long slamdemoFrame::ID_MENUITEM2 = wxNewId();
const long slamdemoFrame::ID_MENUITEM3 = wxNewId();
const long slamdemoFrame::ID_MENUITEM6 = wxNewId();
const long slamdemoFrame::ID_MENUITEM4 = wxNewId();
const long slamdemoFrame::ID_MENUITEM5 = wxNewId();
const long slamdemoFrame::idMenuQuit = wxNewId();
const long slamdemoFrame::ID_MENUITEM8 = wxNewId();
const long slamdemoFrame::ID_MENUITEM11 = wxNewId();
const long slamdemoFrame::ID_MENUITEM_SAVE_RAWLOG = wxNewId();
const long slamdemoFrame::ID_MENUITEM9 = wxNewId();
const long slamdemoFrame::ID_MENUITEM10 = wxNewId();
const long slamdemoFrame::ID_MENUITEM7 = wxNewId();
const long slamdemoFrame::idMenuAbout = wxNewId();
const long slamdemoFrame::ID_STATUSBAR1 = wxNewId();
const long slamdemoFrame::ID_TOOLBARITEM1 = wxNewId();
const long slamdemoFrame::ID_TOOLBARITEM2 = wxNewId();
const long slamdemoFrame::ID_BTNRUN = wxNewId();
const long slamdemoFrame::ID_BTNSTOP = wxNewId();
const long slamdemoFrame::ID_TOOLBARITEM4 = wxNewId();
const long slamdemoFrame::ID_TOOLBARITEM3 = wxNewId();
const long slamdemoFrame::ID_TOOLBARITEM6 = wxNewId();
const long slamdemoFrame::ID_TOOLBARITEM7 = wxNewId();
const long slamdemoFrame::ID_TOOLBAR1 = wxNewId();
const long slamdemoFrame::ID_TIMER1 = wxNewId();
//*)

BEGIN_EVENT_TABLE(slamdemoFrame, wxFrame)
//(*EventTable(slamdemoFrame)
//*)
END_EVENT_TABLE()

slamdemoFrame::slamdemoFrame(wxWindow* parent, wxWindowID id)
{
  // Load my custom icons:
  wxArtProvider::Push(new MyArtProvider);

  //(*Initialize(slamdemoFrame)
  wxMenuItem* MenuItem2;
  wxGridSizer* GridSizer13;
  wxMenuItem* MenuItem1;
  wxFlexGridSizer* FlexGridSizer8;
  wxGridSizer* GridSizer1;
  wxFlexGridSizer* FlexGridSizer1;
  wxFlexGridSizer* FlexGridSizer2;
  wxMenu* Menu1;
  wxGridSizer* GridSizer4;
  wxGridSizer* GridSizer7;
  wxGridSizer* GridSizer3;
  wxGridSizer* GridSizer2;
  wxGridSizer* GridSizer6;
  wxFlexGridSizer* FlexGridSizer7;
  wxFlexGridSizer* FlexGridSizer4;
  wxFlexGridSizer* FlexGridSizer9;
  wxFlexGridSizer* FlexGridSizer6;
  wxGridSizer* GridSizer11;
  wxGridSizer* GridSizer5;
  wxFlexGridSizer* FlexGridSizer3;
  wxGridSizer* GridSizer14;
  wxGridSizer* GridSizer10;
  wxGridSizer* GridSizer8;
  wxMenuBar* MenuBar1;
  wxGridSizer* GridSizer9;
  wxMenuItem* MenuItem7;
  wxMenu* Menu2;
  wxGridSizer* GridSizer12;
  wxFlexGridSizer* FlexGridSizer5;

  Create(
      parent, wxID_ANY, _("2D SLAM Demo - Part of the MRPT project"), wxDefaultPosition,
      wxDefaultSize, wxDEFAULT_FRAME_STYLE, _T("wxID_ANY"));
  {
    wxIcon FrameIcon;
    FrameIcon.CopyFromBitmap(
        wxArtProvider::GetBitmap(wxART_MAKE_ART_ID_FROM_STR(_T("MAIN_ICON")), wxART_FRAME_ICON));
    SetIcon(FrameIcon);
  }
  FlexGridSizer1 = new wxFlexGridSizer(2, 1, 0, 0);
  FlexGridSizer1->AddGrowableCol(0);
  FlexGridSizer1->AddGrowableRow(0);
  FlexGridSizer3 = new wxFlexGridSizer(1, 4, 0, 0);
  FlexGridSizer3->AddGrowableCol(0);
  FlexGridSizer3->AddGrowableCol(1);
  FlexGridSizer3->AddGrowableCol(2);
  FlexGridSizer3->AddGrowableCol(3);
  FlexGridSizer3->AddGrowableRow(0);
  FlexGridSizer4 = new wxFlexGridSizer(4, 1, 0, 0);
  FlexGridSizer4->AddGrowableCol(0);
  FlexGridSizer4->AddGrowableRow(1);
  FlexGridSizer4->AddGrowableRow(3);
  Panel3 = new wxPanel(
      this, ID_PANEL3, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL3"));
  Panel3->SetBackgroundColour(wxColour(255, 255, 0));
  GridSizer1 = new wxGridSizer(0, 1, 0, 0);
  lbGT = new wxStaticText(
      Panel3, ID_STATICTEXT1, _("Ground truth"), wxDefaultPosition, wxDefaultSize, 0,
      _T("ID_STATICTEXT1"));
  wxFont lbGTFont(-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
  lbGT->SetFont(lbGTFont);
  GridSizer1->Add(lbGT, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  Panel3->SetSizer(GridSizer1);
  GridSizer1->Fit(Panel3);
  GridSizer1->SetSizeHints(Panel3);
  FlexGridSizer4->Add(Panel3, 1, wxEXPAND, 1);
  plotGT = new mpWindow(this, ID_CUSTOM1, wxDefaultPosition, wxDefaultSize, 0);
  FlexGridSizer4->Add(plotGT, 1, wxEXPAND, 3);
  Panel4 = new wxPanel(
      this, ID_PANEL4, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL4"));
  Panel4->SetBackgroundColour(wxColour(255, 255, 0));
  GridSizer2 = new wxGridSizer(0, 1, 0, 0);
  lbObs = new wxStaticText(
      Panel4, ID_STATICTEXT2, _("Observation"), wxDefaultPosition, wxDefaultSize, wxALIGN_CENTRE,
      _T("ID_STATICTEXT2"));
  wxFont lbObsFont(-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
  lbObs->SetFont(lbObsFont);
  GridSizer2->Add(lbObs, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  Panel4->SetSizer(GridSizer2);
  GridSizer2->Fit(Panel4);
  GridSizer2->SetSizeHints(Panel4);
  FlexGridSizer4->Add(Panel4, 1, wxEXPAND, 1);
  plotObs = new mpWindow(this, ID_CUSTOM2, wxDefaultPosition, wxDefaultSize, 0);
  FlexGridSizer4->Add(plotObs, 1, wxEXPAND, 3);
  FlexGridSizer3->Add(FlexGridSizer4, 1, wxEXPAND, 0);
  FlexGridSizer5 = new wxFlexGridSizer(4, 1, 0, 0);
  FlexGridSizer5->AddGrowableCol(0);
  FlexGridSizer5->AddGrowableRow(1);
  FlexGridSizer5->AddGrowableRow(3);
  Panel5 = new wxPanel(
      this, ID_PANEL5, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL5"));
  Panel5->SetBackgroundColour(wxColour(255, 255, 0));
  GridSizer3 = new wxGridSizer(0, 1, 0, 0);
  lbMap = new wxStaticText(
      Panel5, ID_STATICTEXT10, _("Map"), wxDefaultPosition, wxDefaultSize, wxALIGN_CENTRE,
      _T("ID_STATICTEXT10"));
  wxFont lbMapFont(-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
  lbMap->SetFont(lbMapFont);
  GridSizer3->Add(lbMap, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  Panel5->SetSizer(GridSizer3);
  GridSizer3->Fit(Panel5);
  GridSizer3->SetSizeHints(Panel5);
  FlexGridSizer5->Add(Panel5, 1, wxEXPAND, 1);
  plotMap = new mpWindow(this, ID_CUSTOM3, wxDefaultPosition, wxDefaultSize, 0);
  FlexGridSizer5->Add(plotMap, 1, wxEXPAND, 3);
  Panel6 = new wxPanel(
      this, ID_PANEL6, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL6"));
  Panel6->SetBackgroundColour(wxColour(255, 255, 0));
  GridSizer4 = new wxGridSizer(0, 1, 0, 0);
  lmIndCompat = new wxStaticText(
      Panel6, ID_STATICTEXT3, _("Indiv. compat (95% ellips.)"), wxDefaultPosition, wxDefaultSize,
      wxALIGN_CENTRE, _T("ID_STATICTEXT3"));
  wxFont lmIndCompatFont(-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
  lmIndCompat->SetFont(lmIndCompatFont);
  GridSizer4->Add(lmIndCompat, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  Panel6->SetSizer(GridSizer4);
  GridSizer4->Fit(Panel6);
  GridSizer4->SetSizeHints(Panel6);
  FlexGridSizer5->Add(Panel6, 1, wxEXPAND, 1);
  plotIndivCompat = new mpWindow(this, ID_CUSTOM4, wxDefaultPosition, wxDefaultSize, 0);
  FlexGridSizer5->Add(plotIndivCompat, 1, wxEXPAND, 3);
  FlexGridSizer3->Add(FlexGridSizer5, 1, wxEXPAND, 0);
  FlexGridSizer6 = new wxFlexGridSizer(2, 1, 0, 0);
  FlexGridSizer6->AddGrowableCol(0);
  FlexGridSizer6->AddGrowableRow(1);
  Panel7 = new wxPanel(
      this, ID_PANEL7, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL7"));
  Panel7->SetBackgroundColour(wxColour(255, 255, 0));
  GridSizer5 = new wxGridSizer(0, 1, 0, 0);
  lbDatAssoc = new wxStaticText(
      Panel7, ID_STATICTEXT4, _("Dat assoc"), wxDefaultPosition, wxDefaultSize, wxALIGN_CENTRE,
      _T("ID_STATICTEXT4"));
  wxFont lbDatAssocFont(-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
  lbDatAssoc->SetFont(lbDatAssocFont);
  GridSizer5->Add(lbDatAssoc, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  Panel7->SetSizer(GridSizer5);
  GridSizer5->Fit(Panel7);
  GridSizer5->SetSizeHints(Panel7);
  FlexGridSizer6->Add(Panel7, 1, wxEXPAND, 1);
  gridDA = new wxGrid(this, ID_GRID1, wxDefaultPosition, wxDefaultSize, 0, _T("ID_GRID1"));
  gridDA->CreateGrid(3, 3);
  gridDA->EnableEditing(false);
  gridDA->EnableGridLines(true);
  gridDA->SetColLabelSize(20);
  gridDA->SetRowLabelSize(40);
  gridDA->SetDefaultColSize(40, true);
  gridDA->SetDefaultCellFont(gridDA->GetFont());
  gridDA->SetDefaultCellTextColour(gridDA->GetForegroundColour());
  FlexGridSizer6->Add(gridDA, 1, wxEXPAND, 0);
  FlexGridSizer3->Add(FlexGridSizer6, 1, wxEXPAND, 0);
  FlexGridSizer7 = new wxFlexGridSizer(0, 1, 0, 0);
  FlexGridSizer7->AddGrowableCol(0);
  FlexGridSizer7->AddGrowableRow(0);
  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"));
  FlexGridSizer8 = new wxFlexGridSizer(6, 1, 0, 0);
  FlexGridSizer8->AddGrowableCol(0);
  FlexGridSizer8->AddGrowableRow(1);
  FlexGridSizer8->AddGrowableRow(3);
  FlexGridSizer8->AddGrowableRow(5);
  Panel8 = new wxPanel(
      Panel1, ID_PANEL9, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL9"));
  Panel8->SetBackgroundColour(wxColour(255, 255, 0));
  GridSizer7 = new wxGridSizer(0, 1, 0, 0);
  StaticText2 = new wxStaticText(
      Panel8, ID_STATICTEXT5, _("Vehicle X (err,99% bounds)"), wxDefaultPosition, wxDefaultSize,
      wxALIGN_CENTRE, _T("ID_STATICTEXT5"));
  wxFont StaticText2Font(-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
  StaticText2->SetFont(StaticText2Font);
  GridSizer7->Add(StaticText2, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  Panel8->SetSizer(GridSizer7);
  GridSizer7->Fit(Panel8);
  GridSizer7->SetSizeHints(Panel8);
  FlexGridSizer8->Add(Panel8, 1, wxEXPAND, 1);
  plotErrorX = new mpWindow(Panel1, ID_CUSTOM7, wxDefaultPosition, wxDefaultSize, 0);
  FlexGridSizer8->Add(plotErrorX, 1, wxEXPAND, 3);
  Panel9 = new wxPanel(
      Panel1, ID_PANEL10, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL10"));
  Panel9->SetBackgroundColour(wxColour(255, 255, 0));
  GridSizer8 = new wxGridSizer(0, 1, 0, 0);
  StaticText3 = new wxStaticText(
      Panel9, ID_STATICTEXT6, _("Vehicle Y (err,99% bounds)"), wxDefaultPosition, wxDefaultSize,
      wxALIGN_CENTRE, _T("ID_STATICTEXT6"));
  wxFont StaticText3Font(-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
  StaticText3->SetFont(StaticText3Font);
  GridSizer8->Add(StaticText3, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  Panel9->SetSizer(GridSizer8);
  GridSizer8->Fit(Panel9);
  GridSizer8->SetSizeHints(Panel9);
  FlexGridSizer8->Add(Panel9, 1, wxEXPAND, 1);
  plotErrorY = new mpWindow(Panel1, ID_CUSTOM8, wxDefaultPosition, wxDefaultSize, 0);
  FlexGridSizer8->Add(plotErrorY, 1, wxEXPAND, 3);
  Panel10 = new wxPanel(
      Panel1, ID_PANEL11, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL11"));
  Panel10->SetBackgroundColour(wxColour(255, 255, 0));
  GridSizer9 = new wxGridSizer(0, 1, 0, 0);
  StaticText4 = new wxStaticText(
      Panel10, ID_STATICTEXT7, _("Vehicle Phi (err,99% bounds)"), wxDefaultPosition, wxDefaultSize,
      wxALIGN_CENTRE, _T("ID_STATICTEXT7"));
  wxFont StaticText4Font(-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
  StaticText4->SetFont(StaticText4Font);
  GridSizer9->Add(StaticText4, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  Panel10->SetSizer(GridSizer9);
  GridSizer9->Fit(Panel10);
  GridSizer9->SetSizeHints(Panel10);
  FlexGridSizer8->Add(Panel10, 1, wxEXPAND, 1);
  plotErrorPhi = new mpWindow(Panel1, ID_CUSTOM9, wxDefaultPosition, wxDefaultSize, 0);
  FlexGridSizer8->Add(plotErrorPhi, 1, wxEXPAND, 3);
  Panel1->SetSizer(FlexGridSizer8);
  FlexGridSizer8->Fit(Panel1);
  FlexGridSizer8->SetSizeHints(Panel1);
  Panel2 = new wxPanel(
      Notebook1, ID_PANEL2, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL2"));
  FlexGridSizer9 = new wxFlexGridSizer(8, 1, 0, 0);
  FlexGridSizer9->AddGrowableCol(0);
  FlexGridSizer9->AddGrowableRow(1);
  FlexGridSizer9->AddGrowableRow(3);
  FlexGridSizer9->AddGrowableRow(5);
  FlexGridSizer9->AddGrowableRow(7);
  Panel13 = new wxPanel(
      Panel2, ID_PANEL8, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL8"));
  Panel13->SetBackgroundColour(wxColour(255, 255, 0));
  GridSizer6 = new wxGridSizer(0, 1, 0, 0);
  lbDaTP = new wxStaticText(
      Panel13, ID_STATICTEXT9, _("True positives:"), wxDefaultPosition, wxDefaultSize,
      wxALIGN_CENTRE, _T("ID_STATICTEXT9"));
  wxFont lbDaTPFont(-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
  lbDaTP->SetFont(lbDaTPFont);
  GridSizer6->Add(lbDaTP, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  Panel13->SetSizer(GridSizer6);
  GridSizer6->Fit(Panel13);
  GridSizer6->SetSizeHints(Panel13);
  FlexGridSizer9->Add(Panel13, 1, wxEXPAND, 1);
  plotDaTP = new mpWindow(Panel2, ID_CUSTOM5, wxDefaultPosition, wxDefaultSize, 0);
  FlexGridSizer9->Add(plotDaTP, 1, wxEXPAND, 3);
  Panel14 = new wxPanel(
      Panel2, ID_PANEL14, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL14"));
  Panel14->SetBackgroundColour(wxColour(255, 255, 0));
  GridSizer11 = new wxGridSizer(0, 1, 0, 0);
  lbDaTN = new wxStaticText(
      Panel14, ID_STATICTEXT11, _("True negatives:"), wxDefaultPosition, wxDefaultSize,
      wxALIGN_CENTRE, _T("ID_STATICTEXT11"));
  wxFont lbDaTNFont(-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
  lbDaTN->SetFont(lbDaTNFont);
  GridSizer11->Add(lbDaTN, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  Panel14->SetSizer(GridSizer11);
  GridSizer11->Fit(Panel14);
  GridSizer11->SetSizeHints(Panel14);
  FlexGridSizer9->Add(Panel14, 1, wxEXPAND, 0);
  plotDaTN = new mpWindow(Panel2, ID_CUSTOM6, wxDefaultPosition, wxDefaultSize, 0);
  FlexGridSizer9->Add(plotDaTN, 1, wxEXPAND, 5);
  Panel15 = new wxPanel(
      Panel2, ID_PANEL15, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL15"));
  Panel15->SetBackgroundColour(wxColour(255, 255, 0));
  GridSizer12 = new wxGridSizer(0, 1, 0, 0);
  StaticText6 = new wxStaticText(
      Panel15, ID_STATICTEXT12, _("False positives:"), wxDefaultPosition, wxDefaultSize,
      wxALIGN_CENTRE, _T("ID_STATICTEXT12"));
  wxFont StaticText6Font(-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
  StaticText6->SetFont(StaticText6Font);
  GridSizer12->Add(StaticText6, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  Panel15->SetSizer(GridSizer12);
  GridSizer12->Fit(Panel15);
  GridSizer12->SetSizeHints(Panel15);
  FlexGridSizer9->Add(Panel15, 1, wxEXPAND, 0);
  plotDaFP = new mpWindow(Panel2, ID_CUSTOM11, wxDefaultPosition, wxDefaultSize, 0);
  FlexGridSizer9->Add(plotDaFP, 1, wxEXPAND, 5);
  Panel16 = new wxPanel(
      Panel2, ID_PANEL16, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL16"));
  Panel16->SetBackgroundColour(wxColour(255, 255, 0));
  GridSizer13 = new wxGridSizer(0, 1, 0, 0);
  StaticText7 = new wxStaticText(
      Panel16, ID_STATICTEXT13, _("False negatives:"), wxDefaultPosition, wxDefaultSize,
      wxALIGN_CENTRE, _T("ID_STATICTEXT13"));
  wxFont StaticText7Font(-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
  StaticText7->SetFont(StaticText7Font);
  GridSizer13->Add(StaticText7, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  Panel16->SetSizer(GridSizer13);
  GridSizer13->Fit(Panel16);
  GridSizer13->SetSizeHints(Panel16);
  FlexGridSizer9->Add(Panel16, 1, wxEXPAND, 0);
  plotDaFN = new mpWindow(Panel2, ID_CUSTOM12, wxDefaultPosition, wxDefaultSize, 0);
  FlexGridSizer9->Add(plotDaFN, 1, wxEXPAND, 5);
  Panel2->SetSizer(FlexGridSizer9);
  FlexGridSizer9->Fit(Panel2);
  FlexGridSizer9->SetSizeHints(Panel2);
  Panel11 = new wxPanel(
      Notebook1, ID_PANEL12, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL12"));
  FlexGridSizer2 = new wxFlexGridSizer(4, 1, 0, 0);
  FlexGridSizer2->AddGrowableCol(0);
  FlexGridSizer2->AddGrowableRow(1);
  Panel12 = new wxPanel(
      Panel11, ID_PANEL13, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL13"));
  Panel12->SetBackgroundColour(wxColour(255, 255, 0));
  GridSizer10 = new wxGridSizer(0, 1, 0, 0);
  StaticText1 = new wxStaticText(
      Panel12, ID_STATICTEXT8, _("Computation time (ms)"), wxDefaultPosition, wxDefaultSize,
      wxALIGN_CENTRE, _T("ID_STATICTEXT8"));
  wxFont StaticText1Font(-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
  StaticText1->SetFont(StaticText1Font);
  GridSizer10->Add(StaticText1, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  Panel12->SetSizer(GridSizer10);
  GridSizer10->Fit(Panel12);
  GridSizer10->SetSizeHints(Panel12);
  FlexGridSizer2->Add(Panel12, 1, wxEXPAND, 1);
  plotStatTime = new mpWindow(Panel11, ID_CUSTOM10, wxDefaultPosition, wxDefaultSize, 0);
  FlexGridSizer2->Add(plotStatTime, 1, wxEXPAND, 3);
  Panel17 = new wxPanel(
      Panel11, ID_PANEL17, wxDefaultPosition, wxDefaultSize, wxTAB_TRAVERSAL, _T("ID_PANEL17"));
  Panel17->SetBackgroundColour(wxColour(255, 255, 0));
  GridSizer14 = new wxGridSizer(0, 1, 0, 0);
  StaticText5 = new wxStaticText(
      Panel17, ID_STATICTEXT14, _("JCBB iterations (if used)"), wxDefaultPosition, wxDefaultSize,
      wxALIGN_CENTRE, _T("ID_STATICTEXT14"));
  wxFont StaticText5Font(-1, wxFONTFAMILY_DEFAULT, wxFONTSTYLE_NORMAL, wxFONTWEIGHT_BOLD);
  StaticText5->SetFont(StaticText5Font);
  GridSizer14->Add(StaticText5, 1, wxALL | wxALIGN_CENTER_HORIZONTAL | wxALIGN_CENTER_VERTICAL, 5);
  Panel17->SetSizer(GridSizer14);
  GridSizer14->Fit(Panel17);
  GridSizer14->SetSizeHints(Panel17);
  FlexGridSizer2->Add(Panel17, 1, wxEXPAND | wxALIGN_LEFT | wxALIGN_BOTTOM, 0);
  plotDaJCBB = new mpWindow(Panel11, ID_CUSTOM13, wxDefaultPosition, wxDefaultSize, 0);
  FlexGridSizer2->Add(plotDaJCBB, 1, wxEXPAND | wxALIGN_LEFT | wxALIGN_BOTTOM, 3);
  Panel11->SetSizer(FlexGridSizer2);
  FlexGridSizer2->Fit(Panel11);
  FlexGridSizer2->SetSizeHints(Panel11);
  Notebook1->AddPage(Panel1, _("Errors"), true);
  Notebook1->AddPage(Panel2, _("Data assoc."), false);
  Notebook1->AddPage(Panel11, _("Stats"), false);
  FlexGridSizer7->Add(Notebook1, 1, wxEXPAND, 0);
  FlexGridSizer3->Add(FlexGridSizer7, 1, wxEXPAND, 0);
  FlexGridSizer1->Add(FlexGridSizer3, 1, wxEXPAND, 0);
  SetSizer(FlexGridSizer1);
  MenuBar1 = new wxMenuBar();
  Menu1 = new wxMenu();
  MenuItem3 = new wxMenuItem(
      Menu1, ID_MENUITEM1, _("Reset simulation\tCtrl+F2"), wxEmptyString, wxITEM_NORMAL);
  Menu1->Append(MenuItem3);
  Menu1->AppendSeparator();
  mnuOneStep =
      new wxMenuItem(Menu1, ID_MENUITEM2, _("Run one step\tF11"), wxEmptyString, wxITEM_NORMAL);
  Menu1->Append(mnuOneStep);
  mnuRun = new wxMenuItem(
      Menu1, ID_MENUITEM3, _("Run (interactive)...\tF5"), wxEmptyString, wxITEM_NORMAL);
  Menu1->Append(mnuRun);
  mnuStop = new wxMenuItem(Menu1, ID_MENUITEM6, _("Stop\tF2"), wxEmptyString, wxITEM_NORMAL);
  Menu1->Append(mnuStop);
  mnuRunBatch = new wxMenuItem(
      Menu1, ID_MENUITEM4, _("Run (non-interactive)...\tCtrl-F5"), wxEmptyString, wxITEM_NORMAL);
  Menu1->Append(mnuRunBatch);
  Menu1->AppendSeparator();
  MenuItem7 = new wxMenuItem(Menu1, ID_MENUITEM5, _("Parameters..."), wxEmptyString, wxITEM_NORMAL);
  Menu1->Append(MenuItem7);
  Menu1->AppendSeparator();
  MenuItem1 = new wxMenuItem(
      Menu1, idMenuQuit, _("Quit\tAlt-F4"), _("Quit the application"), wxITEM_NORMAL);
  Menu1->Append(MenuItem1);
  MenuBar1->Append(Menu1, _("&Simulation"));
  Menu3 = new wxMenu();
  MenuItem5 =
      new wxMenuItem(Menu3, ID_MENUITEM8, _("Save filter state..."), wxEmptyString, wxITEM_NORMAL);
  Menu3->Append(MenuItem5);
  mnuSaveLastDA = new wxMenuItem(
      Menu3, ID_MENUITEM11, _("Save last data association state..."), wxEmptyString, wxITEM_NORMAL);
  Menu3->Append(mnuSaveLastDA);
  Menu3->AppendSeparator();
  mnuItemSaveRawlog = new wxMenuItem(
      Menu3, ID_MENUITEM_SAVE_RAWLOG, _("Enable save rawlog..."), wxEmptyString, wxITEM_CHECK);
  Menu3->Append(mnuItemSaveRawlog);
  Menu3->AppendSeparator();
  MenuItem4 = new wxMenu();
  MenuItem6 =
      new wxMenuItem(MenuItem4, ID_MENUITEM9, _("View stats"), wxEmptyString, wxITEM_NORMAL);
  MenuItem4->Append(MenuItem6);
  MenuItem8 =
      new wxMenuItem(MenuItem4, ID_MENUITEM10, _("Reset stats"), wxEmptyString, wxITEM_NORMAL);
  MenuItem4->Append(MenuItem8);
  Menu3->Append(ID_MENUITEM7, _("Profiler"), MenuItem4, wxEmptyString);
  MenuBar1->Append(Menu3, _("&Tools"));
  Menu2 = new wxMenu();
  MenuItem2 = new wxMenuItem(
      Menu2, idMenuAbout, _("&About\tF1"), _("Show info about this application"), wxITEM_NORMAL);
  Menu2->Append(MenuItem2);
  MenuBar1->Append(Menu2, _("&Help"));
  SetMenuBar(MenuBar1);
  StatusBar1 = new wxStatusBar(this, ID_STATUSBAR1, 0, _T("ID_STATUSBAR1"));
  int __wxStatusBarWidths_1[1] = {-1};
  int __wxStatusBarStyles_1[1] = {wxSB_NORMAL};
  StatusBar1->SetFieldsCount(1, __wxStatusBarWidths_1);
  StatusBar1->SetStatusStyles(1, __wxStatusBarStyles_1);
  SetStatusBar(StatusBar1);
  ToolBar1 = new wxToolBar(
      this, ID_TOOLBAR1, wxDefaultPosition, wxDefaultSize,
      wxTB_FLAT | wxTB_HORIZONTAL | wxTB_TEXT | wxNO_BORDER, _T("ID_TOOLBAR1"));
  ToolBarItem1 = ToolBar1->AddTool(
      ID_TOOLBARITEM1, _("Reset"),
      wxArtProvider::GetBitmap(wxART_MAKE_ART_ID_FROM_STR(_T("ICON_RESET")), wxART_TOOLBAR),
      wxNullBitmap, wxITEM_NORMAL, _("Reset the simulation"), _("Reset the simulation"));
  ToolBar1->AddSeparator();
  ToolBarItem2 = ToolBar1->AddTool(
      ID_TOOLBARITEM2, _("One step"),
      wxArtProvider::GetBitmap(wxART_MAKE_ART_ID_FROM_STR(_T("ICON_STEP")), wxART_TOOLBAR),
      wxNullBitmap, wxITEM_NORMAL, _("Run one step"), _("Run one step"));
  ToolBarItem3 = ToolBar1->AddTool(
      ID_BTNRUN, _("Run..."),
      wxArtProvider::GetBitmap(wxART_MAKE_ART_ID_FROM_STR(_T("ICON_PLAY")), wxART_TOOLBAR),
      wxNullBitmap, wxITEM_CHECK, _("Continuous run with animations..."),
      _("Continuous run with animations..."));
  ToolBarItem4 = ToolBar1->AddTool(
      ID_BTNSTOP, _("Stop"),
      wxArtProvider::GetBitmap(wxART_MAKE_ART_ID_FROM_STR(_T("ICON_STOP")), wxART_TOOLBAR),
      wxNullBitmap, wxITEM_NORMAL, _("Stop continuous run"), _("Stop continuous run"));
  ToolBarItem5 = ToolBar1->AddTool(
      ID_TOOLBARITEM4, _("Run (batch)"),
      wxArtProvider::GetBitmap(wxART_MAKE_ART_ID_FROM_STR(_T("ICON_BATCH")), wxART_TOOLBAR),
      wxNullBitmap, wxITEM_NORMAL, _("Run the whole simulation without animate"),
      _("Run the whole simulation without animate"));
  ToolBar1->AddSeparator();
  ToolBarItem6 = ToolBar1->AddTool(
      ID_TOOLBARITEM3, _("Config..."),
      wxArtProvider::GetBitmap(wxART_MAKE_ART_ID_FROM_STR(_T("ICON_CONFIG")), wxART_TOOLBAR),
      wxNullBitmap, wxITEM_NORMAL, _("Change simulation parameters..."),
      _("Change simulation parameters..."));
  ToolBar1->AddSeparator();
  ToolBarItem7 = ToolBar1->AddTool(
      ID_TOOLBARITEM6, _("About..."),
      wxArtProvider::GetBitmap(wxART_MAKE_ART_ID_FROM_STR(_T("ICON_HELP")), wxART_TOOLBAR),
      wxNullBitmap, wxITEM_NORMAL, _("About this program"), _("About this program"));
  ToolBarItem8 = ToolBar1->AddTool(
      ID_TOOLBARITEM7, _("Quit"),
      wxArtProvider::GetBitmap(wxART_MAKE_ART_ID_FROM_STR(_T("ICON_EXIT")), wxART_TOOLBAR),
      wxNullBitmap, wxITEM_NORMAL, _("Exit the application"), _("Exit the application"));
  ToolBar1->Realize();
  SetToolBar(ToolBar1);
  timSimul.SetOwner(this, ID_TIMER1);
  FlexGridSizer1->Fit(this);
  FlexGridSizer1->SetSizeHints(this);

  Bind(wxEVT_MENU, &slamdemoFrame::OnbtnResetClicked, this, ID_MENUITEM1);
  Bind(wxEVT_MENU, &slamdemoFrame::OnbtnOneStepClicked, this, ID_MENUITEM2);
  Bind(wxEVT_MENU, &slamdemoFrame::OnbtnRunClicked, this, ID_MENUITEM3);
  Bind(wxEVT_MENU, &slamdemoFrame::OnbtnStopClicked, this, ID_MENUITEM6);
  Bind(wxEVT_MENU, &slamdemoFrame::OnbtnRunBatchClicked, this, ID_MENUITEM4);
  Bind(wxEVT_MENU, &slamdemoFrame::OnConfigClicked, this, ID_MENUITEM5);
  Bind(wxEVT_MENU, &slamdemoFrame::OnQuit, this, idMenuQuit);
  Bind(wxEVT_MENU, &slamdemoFrame::OnMenuSaveFilterState, this, ID_MENUITEM8);
  Bind(wxEVT_MENU, &slamdemoFrame::OnmnuSaveLastDASelected, this, ID_MENUITEM11);
  Bind(wxEVT_MENU, &slamdemoFrame::OnmnuItemSaveRawlogSelected, this, ID_MENUITEM_SAVE_RAWLOG);
  Bind(wxEVT_MENU, &slamdemoFrame::OnMenuProfilerViewStats, this, ID_MENUITEM9);
  Bind(wxEVT_MENU, &slamdemoFrame::OnMenuProfilerReset, this, ID_MENUITEM10);
  Bind(wxEVT_MENU, &slamdemoFrame::OnAbout, this, idMenuAbout);
  Bind(wxEVT_TOOL, &slamdemoFrame::OnbtnResetClicked, this, ID_TOOLBARITEM1);
  Bind(wxEVT_TOOL, &slamdemoFrame::OnbtnOneStepClicked, this, ID_TOOLBARITEM2);
  Bind(wxEVT_TOOL, &slamdemoFrame::OnbtnRunClicked, this, ID_BTNRUN);
  Bind(wxEVT_TOOL, &slamdemoFrame::OnbtnStopClicked, this, ID_BTNSTOP);
  Bind(wxEVT_TOOL, &slamdemoFrame::OnbtnRunBatchClicked, this, ID_TOOLBARITEM4);
  Bind(wxEVT_TOOL, &slamdemoFrame::OnConfigClicked, this, ID_TOOLBARITEM3);
  Bind(wxEVT_TOOL, &slamdemoFrame::OnAbout, this, ID_TOOLBARITEM6);
  Bind(wxEVT_TOOL, &slamdemoFrame::OnQuit, this, ID_TOOLBARITEM7);
  Bind(wxEVT_TIMER, &slamdemoFrame::OntimSimulTrigger, this, ID_TIMER1);
  //*)

  gridDA->SetSelectionMode(wxGrid::wxGridSelectCells);

  // buttons ------------
  this->ToolBar1->EnableTool(ID_BTNSTOP, false);
  mnuStop->Enable(false);

// Init graphs:
#define INIT_PLOT_LABELS(_PL, _LBX, _LBY) \
  _PL->AddLayer(new mpScaleX(wxT(_LBX))); \
  _PL->AddLayer(new mpScaleY(wxT(_LBY))); \
  _PL->LockAspect(true);                  \
  _PL->Fit(-10, 10, -10, 10);

#define INIT_PLOT(_PL)      INIT_PLOT_LABELS(_PL, "x", "y")
#define INIT_PLOT_TIME(_PL) INIT_PLOT_LABELS(_PL, "time", "")

  INIT_PLOT(plotGT)
  INIT_PLOT(plotMap)
  INIT_PLOT_TIME(plotErrorX)
  INIT_PLOT_TIME(plotErrorY)
  INIT_PLOT_TIME(plotErrorPhi)
  INIT_PLOT_LABELS(plotIndivCompat, "Bearing", "Range")
  INIT_PLOT(plotObs)

  INIT_PLOT_TIME(plotStatTime)

  INIT_PLOT_TIME(plotDaFN)
  INIT_PLOT_TIME(plotDaFP)
  INIT_PLOT_TIME(plotDaTN)
  INIT_PLOT_TIME(plotDaTP)
  INIT_PLOT_TIME(plotDaJCBB)

  vector<float> robot_shape_xs(3);
  vector<float> robot_shape_ys(3);
  robot_shape_xs[0] = 0;
  robot_shape_ys[0] = -0.10f;
  robot_shape_xs[1] = 0;
  robot_shape_ys[1] = 0.10f;
  robot_shape_xs[2] = 0.2f;
  robot_shape_ys[2] = 0;

  // GT plot ------------
  m_lyGTMap = new mpFXYVector();
  m_lyGTMap->SetPen(wxPen(wxColour(0, 0, 255), 5));
  m_lyGTMap->SetContinuity(false);
  plotGT->AddLayer(m_lyGTMap);

  m_lyGTRobot = new mpPolygon();
  m_lyGTRobot->SetPen(wxPen(wxColour(255, 0, 0), 2));
  m_lyGTRobot->SetContinuity(true);
  m_lyGTRobot->setPoints(robot_shape_xs, robot_shape_ys);
  m_lyGTRobot->SetCoordinateBase(0, 0, 0);
  plotGT->AddLayer(m_lyGTRobot);

  m_lyGTvisibleRange = new mpPolygon();
  m_lyGTvisibleRange->SetPen(wxPen(wxColour(0, 0, 0), 1));
  m_lyGTvisibleRange->SetContinuity(true);
  m_lyGTvisibleRange->SetCoordinateBase(0, 0, 0);
  plotGT->AddLayer(m_lyGTvisibleRange);

  plotGT->LockAspect();

  // Map plot ------------
  m_lyMapRobot = new mpPolygon();
  m_lyMapRobot->SetPen(wxPen(wxColour(255, 0, 0), 2));
  m_lyMapRobot->SetContinuity(true);
  m_lyMapRobot->setPoints(robot_shape_xs, robot_shape_ys);
  m_lyMapRobot->SetCoordinateBase(0, 0, 0);
  plotMap->AddLayer(m_lyMapRobot);
  plotMap->LockAspect();

  // Observations plot ------------
  m_lyObsRobot = new mpPolygon();
  m_lyObsRobot->SetPen(wxPen(wxColour(255, 0, 0), 2));
  m_lyObsRobot->SetContinuity(true);
  m_lyObsRobot->setPoints(robot_shape_xs, robot_shape_ys);
  m_lyObsRobot->SetCoordinateBase(0, 0, 0);
  plotObs->AddLayer(m_lyObsRobot);

  m_lyObsvisibleRange = new mpPolygon();
  m_lyObsvisibleRange->SetPen(wxPen(wxColour(0, 0, 0), 1));
  m_lyObsvisibleRange->SetContinuity(true);
  m_lyObsvisibleRange->SetCoordinateBase(0, 0, 0);
  plotObs->AddLayer(m_lyObsvisibleRange);

  plotObs->LockAspect();

  // IC plot ------------
  m_lyICvisibleRange = new mpPolygon();
  m_lyICvisibleRange->SetPen(wxPen(wxColour(0, 0, 0), 1, wxPENSTYLE_LONG_DASH));
  m_lyICvisibleRange->SetContinuity(true);
  m_lyICvisibleRange->SetCoordinateBase(0, 0, 0);
  plotIndivCompat->AddLayer(m_lyICvisibleRange);

  plotIndivCompat->LockAspect(false);

  // X ERROR plot ------------
  m_lyERRX_err = new mpFXYVector();
  m_lyERRX_err->SetPen(wxPen(wxColour(0, 0, 0), 4));
  m_lyERRX_err->SetContinuity(true);
  plotErrorX->AddLayer(m_lyERRX_err);

  m_lyERRX_boundUp = new mpFXYVector();
  m_lyERRX_boundUp->SetPen(wxPen(wxColour(0, 255, 0), 2));
  m_lyERRX_boundUp->SetContinuity(true);
  plotErrorX->AddLayer(m_lyERRX_boundUp);

  m_lyERRX_boundDown = new mpFXYVector();
  m_lyERRX_boundDown->SetPen(wxPen(wxColour(0, 255, 0), 2));
  m_lyERRX_boundDown->SetContinuity(true);
  plotErrorX->AddLayer(m_lyERRX_boundDown);

  plotErrorX->LockAspect(false);

  // Y ERROR plot ------------
  m_lyERRY_err = new mpFXYVector();
  m_lyERRY_err->SetPen(wxPen(wxColour(0, 0, 0), 4));
  m_lyERRY_err->SetContinuity(true);
  plotErrorY->AddLayer(m_lyERRY_err);

  m_lyERRY_boundUp = new mpFXYVector();
  m_lyERRY_boundUp->SetPen(wxPen(wxColour(0, 255, 0), 2));
  m_lyERRY_boundUp->SetContinuity(true);
  plotErrorY->AddLayer(m_lyERRY_boundUp);

  m_lyERRY_boundDown = new mpFXYVector();
  m_lyERRY_boundDown->SetPen(wxPen(wxColour(0, 255, 0), 2));
  m_lyERRY_boundDown->SetContinuity(true);
  plotErrorY->AddLayer(m_lyERRY_boundDown);

  plotErrorY->LockAspect(false);

  // Phi ERROR plot ------------
  m_lyERRPHI_err = new mpFXYVector();
  m_lyERRPHI_err->SetPen(wxPen(wxColour(0, 0, 0), 4));
  m_lyERRPHI_err->SetContinuity(true);
  plotErrorPhi->AddLayer(m_lyERRPHI_err);

  m_lyERRPHI_boundUp = new mpFXYVector();
  m_lyERRPHI_boundUp->SetPen(wxPen(wxColour(0, 255, 0), 2));
  m_lyERRPHI_boundUp->SetContinuity(true);
  plotErrorPhi->AddLayer(m_lyERRPHI_boundUp);

  m_lyERRPHI_boundDown = new mpFXYVector();
  m_lyERRPHI_boundDown->SetPen(wxPen(wxColour(0, 255, 0), 2));
  m_lyERRPHI_boundDown->SetContinuity(true);
  plotErrorPhi->AddLayer(m_lyERRPHI_boundDown);

  plotErrorPhi->LockAspect(false);

  // Stats Time plot ------------
  m_lyStatTimes = new mpFXYVector();
  m_lyStatTimes->SetPen(wxPen(wxColour(0, 0, 255), 3));
  m_lyStatTimes->SetContinuity(true);
  plotStatTime->AddLayer(m_lyStatTimes);

  plotStatTime->LockAspect(false);

// DA Stats plots ------------
#define INIT_DA_PLOT(CODE)                           \
  m_lyDa##CODE = new mpFXYVector();                  \
  m_lyDa##CODE->SetPen(wxPen(wxColour(0, 0, 0), 5)); \
  m_lyDa##CODE->SetContinuity(false);                \
  plotDa##CODE->AddLayer(m_lyDa##CODE);              \
  plotDa##CODE->LockAspect(false);

  INIT_DA_PLOT(FP);
  INIT_DA_PLOT(FN);
  INIT_DA_PLOT(TP);
  INIT_DA_PLOT(TN);

  INIT_DA_PLOT(JCBB);

  // Set some default params:
  m_SLAM.options.std_sensor_range = 0.03f;
  m_SLAM.options.std_sensor_yaw = DEG2RAD(0.5f);

  options.sensor_max_range = 5;
  options.sensor_min_range = 0.50;
  options.sensor_fov = 140.0_deg;

  options.sensorDistingishesLandmarks = false;

  m_SLAM.KF_options.method = kfEKFNaive;
  //	m_SLAM.KF_options.method = kfEKFAlaDavison;
  //	m_SLAM.KF_options.method = kfIKFFull;
  //	m_SLAM.KF_options.IKF_iterations = 4;

  m_SLAM.KF_options.enable_profiler = true;
  // m_SLAM.KF_options.debug_verify_analytic_jacobians = true;

  // Init everything:
  resetSimulator(options.map_generator);
  updateAllGraphs();

  Maximize();
}

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

/*---------------------------------------------------------------
            Quit
  ---------------------------------------------------------------*/
void slamdemoFrame::OnQuit(wxCommandEvent& event)
{
  // Stop simulation
  wxCommandEvent dumm;
  OnbtnStopClicked(dumm);

  Close();
}

/*---------------------------------------------------------------
            About
  ---------------------------------------------------------------*/
void slamdemoFrame::OnAbout(wxCommandEvent&)
{
  mrpt::gui::show_mrpt_about_box_wxWidgets(
      this, "2d-slam-demo",
      "Invoke with --help to see command line arguments.\n"
      "This program was inspired by a MATLAB version written by J. "
      "Neira and J. Tardos.\n");
}

/*---------------------------------------------------------------
            Reset btn
  ---------------------------------------------------------------*/
void slamdemoFrame::OnbtnResetClicked(wxCommandEvent& event)
{
  resetSimulator(options.map_generator);
  updateAllGraphs();
}

/*---------------------------------------------------------------
            1 step btn
  ---------------------------------------------------------------*/
void slamdemoFrame::OnbtnOneStepClicked(wxCommandEvent& event)
{
  CTicTac tictac;
  tictac.Tic();
  executeOneStep();
  const double T = tictac.Tac();
  StatusBar1->SetStatusText(
      wxString::Format(_("Step %u done in %.03fms"), (unsigned)m_historicData.size(), 1e3 * T));
  updateAllGraphs();
}

/*---------------------------------------------------------------
            Run btn
  ---------------------------------------------------------------*/
void slamdemoFrame::OnbtnRunClicked(wxCommandEvent& event)
{
  this->ToolBar1->EnableTool(ID_BTNSTOP, true);
  mnuStop->Enable(true);

  this->ToolBar1->ToggleTool(ID_BTNRUN, true);
  this->ToolBar1->EnableTool(ID_BTNRUN, false);
  mnuRun->Enable(false);

  // Prepare next step:
  timSimul.Start(10, true);
}

/*---------------------------------------------------------------
            Stop btn
  ---------------------------------------------------------------*/
void slamdemoFrame::OnbtnStopClicked(wxCommandEvent& event)
{
  this->ToolBar1->EnableTool(ID_BTNSTOP, false);
  mnuStop->Enable(false);

  this->ToolBar1->ToggleTool(ID_BTNRUN, false);
  this->ToolBar1->EnableTool(ID_BTNRUN, true);
  mnuRun->Enable(true);
}

/*---------------------------------------------------------------
            batch run btn
  ---------------------------------------------------------------*/
void slamdemoFrame::OnbtnRunBatchClicked(wxCommandEvent& event)
{
  static CTicTac tictac;

  wxBusyCursor info;
  wxTheApp->Yield();  // Let the app. process messages

  tictac.Tic();
  const size_t N = (options.path_square_len / options.robot_step_length) * 4 + 50;
  for (size_t i = 0; i < N; i++) executeOneStep();

  const double T = tictac.Tac();

  updateAllGraphs();

  StatusBar1->SetStatusText(wxString::Format(_("%u steps done in %.03f secs"), (unsigned)N, T));
}

/*---------------------------------------------------------------
            Reset simulator
  ---------------------------------------------------------------*/
void slamdemoFrame::resetSimulator(const std::string& map_type)
{
  if (options.random_seed >= 0)
    getRandomGenerator().randomize(options.random_seed);
  else
    getRandomGenerator().randomize();

  m_SLAM.reset();
  m_historicData.clear();
  m_estimatedIDX2realIDX.clear();
  m_realIDX_already_mapped.clear();

  m_lastObservation.sensedData.clear();
  m_lastObservation_GT_indices.clear();

  m_GT_pose = CPose2D(0, 0, 0);

  // The map:
  // -----------------
  m_GT_map.clear();

  if (map_type == "1")
  {
    // Default map:
    size_t ID = 0;
    CLandmark newLM;
    newLM.pose_mean.z = 0;

    for (int i = 0; i <= mrpt::round((options.path_square_len) / 2.0); i++)
    {
      // Bottom & top  corridors:
      newLM.pose_mean.x = 1 + 2 * i;
      newLM.pose_mean.y = -0.35;
      newLM.ID = ID++;
      m_GT_map.landmarks.push_back(newLM);

      newLM.pose_mean.y += options.path_square_len;  // Top corridor
      newLM.ID = ID++;
      m_GT_map.landmarks.push_back(newLM);

      newLM.pose_mean.x = 1 + 2 * i;
      newLM.pose_mean.y = -0.6;
      newLM.ID = ID++;
      m_GT_map.landmarks.push_back(newLM);

      newLM.pose_mean.y += options.path_square_len;  // Top corridor
      newLM.ID = ID++;
      m_GT_map.landmarks.push_back(newLM);

      newLM.pose_mean.x = 2 * (1 + i);
      newLM.pose_mean.y = 0.35;
      newLM.ID = ID++;
      m_GT_map.landmarks.push_back(newLM);

      newLM.pose_mean.x = 2 * i;
      newLM.pose_mean.y += options.path_square_len;  // Top corridor
      newLM.ID = ID++;
      m_GT_map.landmarks.push_back(newLM);

      newLM.pose_mean.x = 2 * (1 + i);
      newLM.pose_mean.y = 0.6;
      newLM.ID = ID++;
      m_GT_map.landmarks.push_back(newLM);

      newLM.pose_mean.x = 2 * i;
      newLM.pose_mean.y += options.path_square_len;  // Top corridor
      newLM.ID = ID++;
      m_GT_map.landmarks.push_back(newLM);

      // Left & right corridor:
      newLM.pose_mean.x = -0.35;
      newLM.pose_mean.y = 2 * i;
      newLM.ID = ID++;
      m_GT_map.landmarks.push_back(newLM);

      newLM.pose_mean.x += options.path_square_len;  // Right corridor
      // newLM.pose_mean.y -=1;
      newLM.ID = ID++;
      m_GT_map.landmarks.push_back(newLM);

      newLM.pose_mean.x = -0.6;
      newLM.pose_mean.y = 2 * i;
      newLM.ID = ID++;
      m_GT_map.landmarks.push_back(newLM);

      newLM.pose_mean.x += options.path_square_len;  // Right corridor
      // newLM.pose_mean.y -=1;
      newLM.ID = ID++;
      m_GT_map.landmarks.push_back(newLM);

      newLM.pose_mean.x = 0.35;
      newLM.pose_mean.y = 2 * i - 1;
      newLM.ID = ID++;
      m_GT_map.landmarks.push_back(newLM);

      newLM.pose_mean.x += options.path_square_len;  // Right  corridor
      newLM.pose_mean.y += 1 + 1;
      newLM.ID = ID++;
      m_GT_map.landmarks.push_back(newLM);

      newLM.pose_mean.x = 0.6;
      newLM.pose_mean.y = 2 * i - 1;
      newLM.ID = ID++;
      m_GT_map.landmarks.push_back(newLM);

      newLM.pose_mean.x += options.path_square_len;  // Right  corridor
      newLM.pose_mean.y += 1 + 1;
      newLM.ID = ID++;
      m_GT_map.landmarks.push_back(newLM);
    }
  }
  else if (map_type == "2")
  {
    const double extra_width = 5;
    // Random map:
    for (size_t i = 0; i < options.randomMap_nLMs; i++)
    {
      CLandmark newLM;
      newLM.pose_mean.x =
          getRandomGenerator().drawUniform(-extra_width, options.path_square_len + extra_width);
      newLM.pose_mean.y =
          getRandomGenerator().drawUniform(-extra_width, options.path_square_len + extra_width);
      newLM.pose_mean.z = 0;
      newLM.ID = i;
      m_GT_map.landmarks.push_back(newLM);
    }
  }

  // Refresh all:
  updateAllGraphs(true);
}

/*---------------------------------------------------------------
            updateAllGraphs
  ---------------------------------------------------------------*/
void slamdemoFrame::updateAllGraphs(bool alsoGTMap)
{
  // The area visible by the sensor (used in multiple graphs):
  const size_t AREA_SEGS = 100;
  vector<float> xs_area(AREA_SEGS + 1);
  vector<float> ys_area(AREA_SEGS + 1);
  for (size_t i = 0; i < AREA_SEGS; i++)
  {
    double a = options.sensor_fov * (-0.5 + double(i) / (AREA_SEGS - 1));
    xs_area[i] = options.sensorOnTheRobot.x() +
                 options.sensor_max_range * cos(a + options.sensorOnTheRobot.phi());
    ys_area[i] = options.sensorOnTheRobot.y() +
                 options.sensor_max_range * sin(a + options.sensorOnTheRobot.phi());
  }
  xs_area[AREA_SEGS] = options.sensorOnTheRobot.x();
  ys_area[AREA_SEGS] = options.sensorOnTheRobot.y();

  // GT Map ----------------------
  if (alsoGTMap)
  {
    vector<float> xs, ys;
    for (auto& landmark : m_GT_map.landmarks)
    {
      xs.push_back(landmark.pose_mean.x);
      ys.push_back(landmark.pose_mean.y);
    }

    lbGT->SetLabel(
        format("Ground truth (%u landmarks", (unsigned)m_GT_map.landmarks.size()).c_str());

    m_lyGTMap->Clear();
    m_lyGTMap->SetData(xs, ys);

    plotGT->Fit();
  }

  m_lyGTvisibleRange->setPoints(xs_area, ys_area);

  m_lyGTRobot->SetCoordinateBase(m_GT_pose.x(), m_GT_pose.y(), m_GT_pose.phi());
  m_lyGTvisibleRange->SetCoordinateBase(m_GT_pose.x(), m_GT_pose.y(), m_GT_pose.phi());
  plotGT->Refresh();

  // Observation ----------------------
  m_lyObsvisibleRange->setPoints(xs_area, ys_area);

  lbObs->SetLabel(
      format("Observation (%u landmarks", (unsigned)m_lastObservation.sensedData.size()).c_str());

  for (auto& m_lyObsLM : m_lyObsLMs) plotObs->DelLayer(m_lyObsLM, true);
  m_lyObsLMs.clear();

  CMatrixDouble22 NOISE;
  NOISE(0, 0) = square(m_SLAM.options.std_sensor_range);
  NOISE(1, 1) = square(m_SLAM.options.std_sensor_yaw);

  // Create an ellipse for each observed landmark:
  for (auto& i : m_lastObservation.sensedData)
  {
    mpCovarianceEllipse* cov = new mpCovarianceEllipse();
    cov->SetPen(wxPen(wxColour(255, 0, 0), 2));
    if (i.landmarkID != INVALID_LANDMARK_ID)
      cov->SetName(wxString::Format(_("#%u"), (unsigned)i.landmarkID));
    else
      cov->SetName(_("?"));

    // Compute mean & cov:
    const double hr = i.range;
    const double ha = i.yaw;
    const double cphi_0sa = cos(options.sensorOnTheRobot.phi() + ha);
    const double sphi_0sa = sin(options.sensorOnTheRobot.phi() + ha);

    const CPoint2D lm_xy = options.sensorOnTheRobot + CPoint2D(hr * cos(ha), hr * sin(ha));

    // Jacobian wrt hn:
    CMatrixDouble22 dyn_dhn;
    dyn_dhn(0, 0) = cphi_0sa;
    dyn_dhn(0, 1) = -hr * sphi_0sa;
    dyn_dhn(1, 0) = sphi_0sa;
    dyn_dhn(1, 1) = hr * cphi_0sa;

    const CMatrixDouble22 COV = mrpt::math::multiply_HCHt(dyn_dhn, NOISE);

    cov->SetQuantiles(3);
    cov->SetCoordinateBase(lm_xy.x(), lm_xy.y());
    cov->SetCovarianceMatrix(COV(0, 0), COV(0, 1), COV(1, 1));

    plotObs->AddLayer(cov);
    m_lyObsLMs.push_back(cov);
  }

  plotObs->Fit();
  plotObs->Refresh();

  // Map estimated by the SLAM filter ----------------------
  {
    CPosePDFGaussian estRobotPose;
    vector<TPoint2D> LMs;
    map<unsigned int, CLandmark::TLandmarkID> landmarkIDs;

    CVectorDouble Xkk;  // Full mean & cov
    CMatrixDouble Pkk;

    m_SLAM.getCurrentState(estRobotPose, LMs, landmarkIDs, Xkk, Pkk);

    lbMap->SetLabel(wxString::Format(_("Estimated map (%u landmarks)"), (unsigned)LMs.size()));

    // mean robot pose:
    m_lyMapRobot->SetCoordinateBase(
        estRobotPose.mean.x(), estRobotPose.mean.y(), estRobotPose.mean.phi());

    // Delete old covs:
    for (auto& m_lyMapEllipse : m_lyMapEllipses) plotMap->DelLayer(m_lyMapEllipse, true);
    m_lyMapEllipses.clear();

    // Robot ellipse:
    {
      mpCovarianceEllipse* cov = new mpCovarianceEllipse();
      cov->SetPen(wxPen(wxColour(255, 0, 0), 2));
      cov->SetName(_("Robot"));
      cov->SetQuantiles(3);

      cov->SetCoordinateBase(estRobotPose.mean.x(), estRobotPose.mean.y(), estRobotPose.mean.phi());
      cov->SetCovarianceMatrix(
          estRobotPose.cov(0, 0), estRobotPose.cov(0, 1), estRobotPose.cov(1, 1));

      plotMap->AddLayer(cov);
      m_lyMapEllipses.push_back(cov);
    }

    // Landmarks:
    for (size_t i = 0; i < LMs.size(); i++)
    {
      const size_t idx_in_real_map = m_estimatedIDX2realIDX.find(i)->second;

      mpCovarianceEllipse* cov = new mpCovarianceEllipse();
      cov->SetPen(wxPen(wxColour(0, 0, 255), 2));

      if (options.show_map_real_correspondences)
        cov->SetName(wxString::Format(_("#%u->%u"), (unsigned)i, (unsigned)idx_in_real_map));
      else
        cov->SetName(wxString::Format(_("#%u"), (unsigned)i));

      cov->SetQuantiles(3);

      cov->SetCoordinateBase(LMs[i].x, LMs[i].y);
      const size_t idx = 3 + 2 * i;
      cov->SetCovarianceMatrix(Pkk(idx + 0, idx + 0), Pkk(idx + 0, idx + 1), Pkk(idx + 1, idx + 1));

      plotMap->AddLayer(cov);
      m_lyMapEllipses.push_back(cov);
    }
  }

  plotMap->Fit();
  plotMap->Refresh();

  // Error plots -----------
  if (!m_historicData.empty())
  {
    const size_t N = m_historicData.size();

    // ERRORS IN X --------------
    if (m_lyERRX_err->GetDataLength() >= N)
    {
      m_lyERRX_err->Clear();
      m_lyERRX_boundUp->Clear();
      m_lyERRX_boundDown->Clear();
    }
    for (size_t i = m_lyERRX_err->GetDataLength(); i < N; i++)
    {
      THistoric& h = m_historicData[i];
      m_lyERRX_err->AppendDataPoint(i, h.GT_robot_pose.x() - h.estimate_robot_pose.mean.x());
      const double std_x = sqrt(h.estimate_robot_pose.cov(0, 0));
      m_lyERRX_boundUp->AppendDataPoint(i, 3 * std_x);
      m_lyERRX_boundDown->AppendDataPoint(i, -3 * std_x);
    }

    // ERRORS IN Y --------------
    if (m_lyERRY_err->GetDataLength() >= N)
    {
      m_lyERRY_err->Clear();
      m_lyERRY_boundUp->Clear();
      m_lyERRY_boundDown->Clear();
    }
    for (size_t i = m_lyERRY_err->GetDataLength(); i < N; i++)
    {
      THistoric& h = m_historicData[i];
      m_lyERRY_err->AppendDataPoint(i, h.GT_robot_pose.y() - h.estimate_robot_pose.mean.y());
      const double std_y = sqrt(h.estimate_robot_pose.cov(1, 1));
      m_lyERRY_boundUp->AppendDataPoint(i, 3 * std_y);
      m_lyERRY_boundDown->AppendDataPoint(i, -3 * std_y);
    }

    // ERRORS IN PHI --------------
    if (m_lyERRPHI_err->GetDataLength() >= N)
    {
      m_lyERRPHI_err->Clear();
      m_lyERRPHI_boundUp->Clear();
      m_lyERRPHI_boundDown->Clear();
    }
    for (size_t i = m_lyERRPHI_err->GetDataLength(); i < N; i++)
    {
      THistoric& h = m_historicData[i];
      m_lyERRPHI_err->AppendDataPoint(
          i,
          RAD2DEG(mrpt::math::wrapToPi(h.GT_robot_pose.phi() - h.estimate_robot_pose.mean.phi())));
      const double std_p = sqrt(h.estimate_robot_pose.cov(2, 2));
      m_lyERRPHI_boundUp->AppendDataPoint(i, RAD2DEG(3 * std_p));
      m_lyERRPHI_boundDown->AppendDataPoint(i, RAD2DEG(-3 * std_p));
    }

    plotErrorX->Fit(
        -0.05 * N, N * 1.05, m_lyERRX_boundDown->GetMinY() * 1.05,
        m_lyERRX_boundUp->GetMaxY() * 1.05);
    plotErrorX->Refresh();
    plotErrorY->Fit(
        -0.05 * N, N * 1.05, m_lyERRY_boundDown->GetMinY() * 1.05,
        m_lyERRY_boundUp->GetMaxY() * 1.05);
    plotErrorY->Refresh();
    plotErrorPhi->Fit(
        -0.05 * N, N * 1.05, m_lyERRPHI_boundDown->GetMinY() * 1.05,
        m_lyERRPHI_boundUp->GetMaxY() * 1.05);
    plotErrorPhi->Refresh();

    // Execution times: --------------
    if (m_lyStatTimes->GetDataLength() >= N)
    {
      m_lyStatTimes->Clear();
    }
    for (size_t i = m_lyStatTimes->GetDataLength(); i < N; i++)
    {
      THistoric& h = m_historicData[i];
      m_lyStatTimes->AppendDataPoint(i, 1e3 * h.run_time);
    }
    plotStatTime->Fit(
        -0.15 * N, N * 1.05, -0.10 * m_lyStatTimes->GetMaxY(), m_lyStatTimes->GetMaxY() * 1.05);
    plotStatTime->Refresh();
  }

  // Data association graphs ------------------------
  const CRangeBearingKFSLAM2D::TDataAssocInfo& da = m_SLAM.getLastDataAssociation();

  // IC graph ------------------------------------------
  {
    // Draw sensor ranges in the R-B plane:
    vector<float> xs_area_RG(5);
    vector<float> ys_area_RG(5);
    xs_area_RG[0] = -RAD2DEG(options.sensor_fov) * 0.5;
    ys_area_RG[0] = 0;
    xs_area_RG[1] = RAD2DEG(options.sensor_fov) * 0.5;
    ys_area_RG[1] = 0;
    xs_area_RG[2] = RAD2DEG(options.sensor_fov) * 0.5;
    ys_area_RG[2] = options.sensor_max_range;
    xs_area_RG[3] = -RAD2DEG(options.sensor_fov) * 0.5;
    ys_area_RG[3] = options.sensor_max_range;
    xs_area_RG[4] = xs_area_RG[0];
    ys_area_RG[4] = ys_area_RG[0];

    m_lyICvisibleRange->setPoints(xs_area_RG, ys_area_RG);

    // Delete old ellipses:
    for (auto& m_lyIC_LM : m_lyIC_LMs) plotIndivCompat->DelLayer(m_lyIC_LM, true);
    m_lyIC_LMs.clear();

    // Create an ellipse for each observed landmark, in
    //  the RANGE-BEARING plane:
    for (size_t i = 0; i < m_lastObservation.sensedData.size(); i++)
    {
      mpCovarianceEllipse* cov = new mpCovarianceEllipse();
      cov->SetPen(wxPen(wxColour(255, 0, 0), 2));
      if (m_lastObservation.sensedData[i].landmarkID != INVALID_LANDMARK_ID)
        cov->SetName(
            wxString::Format(_("O(%u)"), (unsigned)m_lastObservation.sensedData[i].landmarkID));
      else
        cov->SetName(wxString::Format(_("O%u"), (unsigned)i));

      // Compute mean & cov:
      const double hr = m_lastObservation.sensedData[i].range;
      const double ha = m_lastObservation.sensedData[i].yaw;

      cov->SetQuantiles(2);
      cov->SetCoordinateBase(RAD2DEG(ha), hr);
      cov->SetCovarianceMatrix(RAD2DEGSQ * NOISE(1, 1), 0, NOISE(0, 0));

      plotIndivCompat->AddLayer(cov);
      m_lyIC_LMs.push_back(cov);
    }

    const size_t obs_size = m_SLAM.get_observation_size();

    // Create an ellipse for each PREDICTED map landmark, in
    //  the RANGE-BEARING plane:
    for (size_t i = 0; i < da.predictions_IDs.size(); i++)
    {
      mpCovarianceEllipse* cov = new mpCovarianceEllipse();
      cov->SetPen(wxPen(wxColour(0, 0, 255), 2));
      cov->SetName(wxString::Format(_("P%u"), (unsigned)da.predictions_IDs[i]));

      const double hr = da.Y_pred_means(i, 0);
      const double ha = da.Y_pred_means(i, 1);

      cov->SetQuantiles(2);
      cov->SetCoordinateBase(RAD2DEG(ha), hr);
      if (size_t(da.Y_pred_covs.cols()) == obs_size)
      {  // Independent predictions:
        ASSERT_(size_t(da.Y_pred_covs.rows()) == obs_size * da.predictions_IDs.size());
        cov->SetCovarianceMatrix(
            RAD2DEGSQ * da.Y_pred_covs(obs_size * i + 1, 1),
            RAD2DEG(da.Y_pred_covs(obs_size * i + 1, 0)), da.Y_pred_covs(obs_size * i + 0, 0));
      }
      else
      {  // Full cov. predictions:
        ASSERT_(
            da.Y_pred_covs.isSquare() &&
            size_t(da.Y_pred_covs.cols()) == obs_size * da.predictions_IDs.size());
        cov->SetCovarianceMatrix(
            RAD2DEGSQ * da.Y_pred_covs(obs_size * i + 1, obs_size * i + 1),
            RAD2DEG(da.Y_pred_covs(obs_size * i + 1, obs_size * i + 0)),
            da.Y_pred_covs(obs_size * i + 0, obs_size * i + 0));
      }

      plotIndivCompat->AddLayer(cov);
      m_lyIC_LMs.push_back(cov);
    }

    // Draw lines between individually compatible covs:
    for (unsigned int o = 0; o < m_lastObservation.sensedData.size(); o++)
    {
      const double hr_o = m_lastObservation.sensedData[o].range;
      const double ha_o = m_lastObservation.sensedData[o].yaw;

      for (unsigned int p = 0; p < da.predictions_IDs.size(); p++)
      {
        bool is_final_assoc =
            (da.results.associations.find(o) != da.results.associations.end() &&
             da.results.associations.find(o)->second == da.predictions_IDs[p]);

        if (da.results.indiv_compatibility(p, o))
        {
          mpFXYVector* v = new mpFXYVector(wxEmptyString, mpALIGN_SW);
          v->SetPen(wxPen(wxColour(0, 0, 0), is_final_assoc ? 4 : 2));
          v->SetName(wxString::Format(_("%f"), da.results.indiv_distances(p, o)));

          const double hr = da.Y_pred_means(p, 0);
          const double ha = da.Y_pred_means(p, 1);

          vector<float> xs(2);
          vector<float> ys(2);
          xs[0] = RAD2DEG(ha);
          ys[0] = hr;
          xs[1] = RAD2DEG(ha_o);
          ys[1] = hr_o;

          v->SetData(xs, ys);
          v->SetContinuity(true);

          plotIndivCompat->AddLayer(v);
          m_lyIC_LMs.push_back(v);
        }
        // Set background if it's a final association:
        if (is_final_assoc)
        {
          gridDA->SetCellBackgroundColour(p, o, wxColor(255, 255, 0));
        }
      }
    }

    plotIndivCompat->Fit(
        -RAD2DEG(options.sensor_fov) * 0.5 - 10, RAD2DEG(options.sensor_fov) * 0.5 + 10, -0.10,
        options.sensor_max_range + 0.5);

    plotIndivCompat->Refresh();
  }

  // DA distances ------------------------------------------
  {
    gridDA->BeginBatch();

    // Rows: predictions; Cols: observations
    if (gridDA->GetNumberCols()) gridDA->DeleteCols(0, gridDA->GetNumberCols());
    gridDA->AppendCols(m_lastObservation.sensedData.size());

    for (unsigned int i = 0; i < m_lastObservation.sensedData.size(); i++)
      gridDA->SetColLabelValue(i, wxString::Format(wxT("O%u"), i));

    if (gridDA->GetNumberRows()) gridDA->DeleteRows(0, gridDA->GetNumberRows());
    gridDA->AppendRows(da.predictions_IDs.size());

    for (unsigned int i = 0; i < da.predictions_IDs.size(); i++)
      gridDA->SetRowLabelValue(i, wxString::Format(_("P%u"), (unsigned)da.predictions_IDs[i]));

    for (unsigned int o = 0; o < m_lastObservation.sensedData.size(); o++)
      for (unsigned int p = 0; p < da.predictions_IDs.size(); p++)
      {
        gridDA->SetCellAlignment(p, o, wxALIGN_RIGHT, wxALIGN_CENTRE);

        const double v = da.results.indiv_distances(p, o);
        if (v > 500)
          gridDA->SetCellValue(p, o, wxT("\u221E"));  // Infinity
        else if (v < -950)
          gridDA->SetCellValue(p, o, wxT("-\u221E"));  // -Infinity
        else
          gridDA->SetCellValue(p, o, wxString::Format(wxT("%.02f"), v));

        // Set background if it's a final association:
        if (da.results.associations.find(o) != da.results.associations.end() &&
            da.results.associations.find(o)->second == da.predictions_IDs[p])
        {
          gridDA->SetCellBackgroundColour(p, o, wxColor(255, 255, 0));
        }
      }

    gridDA->EndBatch();
  }

  // DA stats ------------------------------------------
  if (!m_historicData.empty())
  {
    const size_t N = m_historicData.size();

    // DA True positives --------------
    if (m_lyDaTP->GetDataLength() >= N) m_lyDaTP->Clear();
    for (size_t i = m_lyDaTP->GetDataLength(); i < N; i++)
      m_lyDaTP->AppendDataPoint(i, m_historicData[i].da_true_pos);

    unsigned int totalTP = 0;
    for (size_t i = 0; i < N; i++) totalTP += m_historicData[i].da_true_pos;
    lbDaTP->SetLabel(wxString::Format(wxT("True positives: %u"), totalTP));

    // DA True negatives --------------
    if (m_lyDaTN->GetDataLength() >= N) m_lyDaTN->Clear();
    for (size_t i = m_lyDaTN->GetDataLength(); i < N; i++)
      m_lyDaTN->AppendDataPoint(i, m_historicData[i].da_true_neg);

    unsigned int totalTN = 0;
    for (size_t i = 0; i < N; i++) totalTN += m_historicData[i].da_true_neg;
    lbDaTN->SetLabel(wxString::Format(wxT("True negatives: %u"), totalTN));

    // DA false positives --------------
    if (m_lyDaFP->GetDataLength() >= N) m_lyDaFP->Clear();
    for (size_t i = m_lyDaFP->GetDataLength(); i < N; i++)
      m_lyDaFP->AppendDataPoint(i, m_historicData[i].da_false_pos);

    unsigned int totalFP = 0;
    for (size_t i = 0; i < N; i++) totalFP += m_historicData[i].da_false_pos;
    StaticText6->SetLabel(wxString::Format(wxT("False positives: %u"), totalFP));

    // DA false negatives --------------
    if (m_lyDaFN->GetDataLength() >= N) m_lyDaFN->Clear();
    for (size_t i = m_lyDaFN->GetDataLength(); i < N; i++)
      m_lyDaFN->AppendDataPoint(i, m_historicData[i].da_false_neg);

    unsigned int totalFN = 0;
    for (size_t i = 0; i < N; i++) totalFN += m_historicData[i].da_false_neg;
    StaticText7->SetLabel(wxString::Format(wxT("False negatives: %u"), totalFN));

    // JCBB iterations --------------
    if (m_lyDaJCBB->GetDataLength() >= N) m_lyDaJCBB->Clear();
    for (size_t i = m_lyDaJCBB->GetDataLength(); i < N; i++)
      m_lyDaJCBB->AppendDataPoint(i, m_historicData[i].jcbb_iters);

    plotDaFN->Fit();
    plotDaFN->Refresh();
    plotDaFP->Fit();
    plotDaFP->Refresh();
    plotDaTN->Fit();
    plotDaTN->Refresh();
    plotDaTP->Fit();
    plotDaTP->Refresh();

    plotDaJCBB->Fit();
    plotDaJCBB->Refresh();
  }
}

void slamdemoFrame::TSimulationOptions::loadFromConfigFile(
    const mrpt::config::CConfigFileBase& f, const std::string& c)
{
  MRPT_LOAD_CONFIG_VAR(random_seed, int, f, c)
  MRPT_LOAD_CONFIG_VAR(map_generator, string, f, c)
  MRPT_LOAD_CONFIG_VAR(randomMap_nLMs, int, f, c)

  sensorOnTheRobot.x(f.read_double(c, "sensorOnTheRobot.x", sensorOnTheRobot.x()));
  sensorOnTheRobot.y(f.read_double(c, "sensorOnTheRobot.y", sensorOnTheRobot.y()));
  sensorOnTheRobot.phi(
      DEG2RAD(f.read_double(c, "sensorOnTheRobot.phi", RAD2DEG(sensorOnTheRobot.phi()))));

  MRPT_LOAD_CONFIG_VAR(sensor_max_range, double, f, c)
  MRPT_LOAD_CONFIG_VAR(sensor_min_range, double, f, c)
  MRPT_LOAD_CONFIG_VAR_DEGREES(sensor_fov, f, c)

  MRPT_LOAD_CONFIG_VAR(sensorDistingishesLandmarks, bool, f, c)

  MRPT_LOAD_CONFIG_VAR(path_square_len, double, f, c)
  MRPT_LOAD_CONFIG_VAR(robot_step_length, double, f, c)

  MRPT_LOAD_CONFIG_VAR(odometry_noise_std_xy, double, f, c)
  MRPT_LOAD_CONFIG_VAR_DEGREES(odometry_noise_std_phi, f, c)

  MRPT_LOAD_CONFIG_VAR(uncert_overestim_odom, double, f, c)
  MRPT_LOAD_CONFIG_VAR(uncert_overestim_sensor, double, f, c)

  MRPT_LOAD_CONFIG_VAR(show_map_real_correspondences, bool, f, c)

  MRPT_LOAD_CONFIG_VAR(spurious_count_mean, double, f, c)
  MRPT_LOAD_CONFIG_VAR(spurious_count_std, double, f, c)
}

void slamdemoFrame::TSimulationOptions::saveToConfigFile(
    mrpt::config::CConfigFileBase& f, const std::string& c) const
{
  MRPT_SAVE_CONFIG_VAR(random_seed, f, c)
  MRPT_SAVE_CONFIG_VAR(map_generator, f, c)
  MRPT_SAVE_CONFIG_VAR(randomMap_nLMs, f, c)

  f.write(c, "sensorOnTheRobot.x", sensorOnTheRobot.x());
  f.write(c, "sensorOnTheRobot.y", sensorOnTheRobot.y());
  f.write(c, "sensorOnTheRobot.phi", RAD2DEG(sensorOnTheRobot.phi()));

  MRPT_SAVE_CONFIG_VAR(sensor_max_range, f, c)
  MRPT_SAVE_CONFIG_VAR(sensor_min_range, f, c)
  MRPT_SAVE_CONFIG_VAR_DEGREES(sensor_fov, f, c)

  MRPT_SAVE_CONFIG_VAR(sensorDistingishesLandmarks, f, c)

  MRPT_SAVE_CONFIG_VAR(path_square_len, f, c)
  MRPT_SAVE_CONFIG_VAR(robot_step_length, f, c)

  MRPT_SAVE_CONFIG_VAR(odometry_noise_std_xy, f, c)
  MRPT_SAVE_CONFIG_VAR_DEGREES(odometry_noise_std_phi, f, c)

  MRPT_SAVE_CONFIG_VAR(uncert_overestim_odom, f, c)
  MRPT_SAVE_CONFIG_VAR(uncert_overestim_sensor, f, c)

  MRPT_SAVE_CONFIG_VAR(show_map_real_correspondences, f, c)

  MRPT_SAVE_CONFIG_VAR(spurious_count_mean, f, c)
  MRPT_SAVE_CONFIG_VAR(spurious_count_std, f, c)
}

void slamdemoFrame::TSimulationOptions::dumpToTextStream(std::ostream& out) const {}
/*---------------------------------------------------------------
            executeOneStep
  ---------------------------------------------------------------*/
void slamdemoFrame::executeOneStep()
{
  try
  {
    // Move the robot   ------------------------
    // Skip in the first step:
    CPose2D poseIncr(0, 0, 0);

    const int turnSteps = 10;
    const double Aphi = DEG2RAD(90.0 / turnSteps);

    if (!m_historicData.empty())
    {  // Ok, move:
      const double PATH_SQUARE_LEN = options.path_square_len;

      if (fabs(fmod(m_GT_pose.phi(), 90.0_deg)) < 1e-2)
      {
        int dir = mrpt::round(m_GT_pose.phi() / Aphi);

        // Continue in a straight line, unless we reach a corner:
        if ((m_GT_pose.x() > PATH_SQUARE_LEN && m_GT_pose.y() < PATH_SQUARE_LEN && dir == 0) ||
            (m_GT_pose.x() > PATH_SQUARE_LEN && m_GT_pose.y() > PATH_SQUARE_LEN &&
             dir == turnSteps) ||
            (m_GT_pose.x() <= 0 && std::abs(dir) == 2 * turnSteps) ||
            (m_GT_pose.y() <= 0 && dir == -turnSteps))
        {  // Turn
          poseIncr = CPose2D(0, 0, Aphi);
        }
        else
        {  // Straight:
          poseIncr = CPose2D(options.robot_step_length, 0, 0);
        }
      }
      else
      {  // Continue turning:
        poseIncr = CPose2D(0, 0, Aphi);
      }
    }

    m_GT_pose = m_GT_pose + poseIncr;

    // Round phi so we have always perfect square paths:
    m_GT_pose.phi(mrpt::round(m_GT_pose.phi() / Aphi) * Aphi);

    // Simulate observation ------------------------
    {
      const CPose3D robotPose = CPose3D(this->m_GT_pose);
      const CPose3D sensorOnRobot = CPose3D(options.sensorOnTheRobot);

      m_lastObservation.fieldOfView_yaw = options.sensor_fov;
      m_lastObservation.maxSensorDistance = options.sensor_max_range;
      m_lastObservation.minSensorDistance = options.sensor_min_range;

      m_GT_map.simulateRangeBearingReadings(
          robotPose, sensorOnRobot, m_lastObservation, options.sensorDistingishesLandmarks,
          m_SLAM.options.std_sensor_range / options.uncert_overestim_sensor,
          m_SLAM.options.std_sensor_yaw / options.uncert_overestim_sensor,
          0,  // sigma_pitch: we are in 2D
          &m_lastObservation_GT_indices, options.spurious_count_mean, options.spurious_count_std);
    }

    // before processing the observation:
    const std::set<size_t> old_realIDX_already_mapped = m_realIDX_already_mapped;

    // SLAM ------------------------
    double executionTime;
    static CTicTac tictac;
    {
      auto act = CActionCollection::Create();
      CActionRobotMovement2D actmov;
      CActionRobotMovement2D::TMotionModelOptions odo_opts;
      odo_opts.modelSelection = CActionRobotMovement2D::mmGaussian;

      // Model as a constant noise in X,Y,PHI:
      odo_opts.gaussianModel.a1 = 0;  // 0.01f;
      odo_opts.gaussianModel.a2 = 0;  // RAD2DEG( 0.0001f );
      odo_opts.gaussianModel.a3 = 0;  // DEG2RAD( 0.1f );
      odo_opts.gaussianModel.a4 = 0;  // 0.01; //0.05f;
      odo_opts.gaussianModel.minStdXY = options.odometry_noise_std_xy;
      odo_opts.gaussianModel.minStdPHI = options.odometry_noise_std_phi;

      // Add noise:
      CPose2D noisyPoseIncr = poseIncr;
      noisyPoseIncr.x_incr(getRandomGenerator().drawGaussian1D(
          0, options.odometry_noise_std_xy / options.uncert_overestim_odom));
      noisyPoseIncr.y_incr(getRandomGenerator().drawGaussian1D(
          0, options.odometry_noise_std_xy / options.uncert_overestim_odom));
      noisyPoseIncr.phi_incr(getRandomGenerator().drawGaussian1D(
          0, options.odometry_noise_std_phi / options.uncert_overestim_odom));

      actmov.computeFromOdometry(noisyPoseIncr, odo_opts);
      actmov.timestamp = mrpt::Clock::now();
      act->insert(actmov);

      CSensoryFrame::Ptr sf = CSensoryFrame::Create();
      m_lastObservation.timestamp = mrpt::Clock::now();
      m_lastObservation.sensorLabel = "SIMUL_2D_RB";

      sf->insert(CObservationBearingRange::Create(m_lastObservation));

      tictac.Tic();

      m_SLAM.processActionObservation(act, sf);

      executionTime = tictac.Tac();

      // Save dataset to file?
      if (m_rawlog_out_file.fileOpenCorrectly())
      {
        archiveFrom(m_rawlog_out_file) << act << sf;
      }
    }

    // For the case of doing D.A., save the correspondences REAL_MAP <->
    // ESTIMATED_MAP ----------
    const CRangeBearingKFSLAM2D::TDataAssocInfo& da = m_SLAM.getLastDataAssociation();

    for (auto newly_inserted_landmark : da.newly_inserted_landmarks)
    {
      const size_t obs_idx = newly_inserted_landmark.first;
      const size_t est_map_idx = newly_inserted_landmark.second;
      const size_t real_map_idx = m_lastObservation_GT_indices[obs_idx];

      m_estimatedIDX2realIDX[est_map_idx] = real_map_idx;

      // Do NOT remember all spureous readings, since they all look alike:
      if (real_map_idx != std::string::npos) m_realIDX_already_mapped.insert(real_map_idx);
    }

    // Save historic data ------------------------
    THistoric& hist = m_historicData.emplace_back();

    hist.GT_robot_pose = m_GT_pose;
    m_SLAM.getCurrentRobotPose(hist.estimate_robot_pose);
    hist.run_time = executionTime;
    hist.jcbb_iters = da.results.nNodesExploredInJCBB;

    // D.A. history data:
    if (!options.sensorDistingishesLandmarks)
    {
      ASSERT_(m_lastObservation.sensedData.size() == m_lastObservation_GT_indices.size());

      for (unsigned int o = 0; o < m_lastObservation.sensedData.size(); o++)
      {
        const bool o_has_assoc = (da.results.associations.find(o) != da.results.associations.end());
        const size_t o_realmap_idx = m_lastObservation_GT_indices[o];  // Note: This can be "-1"
        // for spurious readings!
        const bool o_was_mapped = (o_realmap_idx == std::string::npos)
                                      ? false
                                      : old_realIDX_already_mapped.find(o_realmap_idx) !=
                                            old_realIDX_already_mapped.end();
        const bool o_has_been_just_inserted =
            da.newly_inserted_landmarks.find(o) != da.newly_inserted_landmarks.end();

        if (o_was_mapped)
        {  // ALREADY OBSERVED IN THE PAST

          if (o_has_assoc)
          {
            if (m_estimatedIDX2realIDX[da.results.associations.find(o)->second] == o_realmap_idx)
            {
              // True positive: If the LM was already known and
              // it's been recognized as such...
              hist.da_true_pos++;
            }
            else
            {
              // False positive:
              hist.da_false_pos++;
            }
          }
          else
          {
            ASSERT_(o_has_been_just_inserted);
            // False negative: It was an already known LM but has
            // been wrongly classified as new:
            hist.da_false_neg++;
          }
        }
        else
        {  // OBSERVED FOR THE FIRST TIME

          if (o_has_assoc)
          {
            // False positive:
            hist.da_false_pos++;
          }
          else
          {
            ASSERT_(o_has_been_just_inserted);
            // True negative: If the LM is a new one and it's been
            // inserted as such...
            hist.da_true_neg++;
          }
        }
      }
    }
  }
  catch (const std::exception& e)
  {
    try
    {
      wxCommandEvent dum;
      this->OnbtnStopClicked(dum);
    }
    catch (...)
    {
    }
    cerr << endl << mrpt::exception_to_str(e) << endl;
    wxMessageBox(mrpt::exception_to_str(e), _("Exception"));
  }
}

/*---------------------------------------------------------------
            OntimSimulTrigger
  ---------------------------------------------------------------*/
void slamdemoFrame::OntimSimulTrigger(wxTimerEvent& event)
{
  static CTicTac tictac;

  if (!ToolBar1->GetToolState(ID_BTNRUN)) return;

  // Simulate one step:
  tictac.Tic();

  executeOneStep();

  const double T = tictac.Tac();

  updateAllGraphs();

  StatusBar1->SetStatusText(
      wxString::Format(_("Step %u done in %.03fms"), (unsigned)m_historicData.size(), 1e3 * T));

  wxTheApp->Yield(true);  // Let the app. process messages

  // Prepare next step:
  timSimul.Start(20, true);
}

/*---------------------------------------------------------------
            OnConfigClicked
  ---------------------------------------------------------------*/
void slamdemoFrame::OnConfigClicked(wxCommandEvent& event)
{
  CDlgParams dlg(this);

  // Put all params
  // ---------------------------------------
  dlg.rbKFnaiv->SetValue(m_SLAM.KF_options.method == kfEKFNaive);
  dlg.rbKFdavison->SetValue(m_SLAM.KF_options.method == kfEKFAlaDavison);
  dlg.rbIKF->SetValue(m_SLAM.KF_options.method == kfIKFFull);
  dlg.rbIKFdavison->SetValue(m_SLAM.KF_options.method == kfIKF);
  dlg.edIKFiters->SetValue(m_SLAM.KF_options.IKF_iterations);

  dlg.cbJacobTran->SetValue(!m_SLAM.KF_options.use_analytic_transition_jacobian);
  dlg.cbJacobObs->SetValue(!m_SLAM.KF_options.use_analytic_observation_jacobian);

  // dlg.rbFusion->SetSelection( (int)m_SLAM.KF_options.fusion_strategy );

  dlg.rbMapCorridor->SetValue(options.map_generator == "1");
  dlg.rbMapRandom->SetValue(options.map_generator == "2");

  if (options.map_generator != "1" && options.map_generator != "2") dlg.rbMapFile->SetValue(true);

  dlg.edLMs->SetValue(options.randomMap_nLMs);

  dlg.edPathLen->SetValue(wxString::Format(_("%f"), options.path_square_len));
  dlg.edPathStepSize->SetValue(wxString::Format(_("%f"), options.robot_step_length));
  dlg.edOdomStdXY->SetValue(wxString::Format(_("%f"), options.odometry_noise_std_xy));
  dlg.edStdOdomPhi->SetValue(wxString::Format(_("%f"), RAD2DEG(options.odometry_noise_std_phi)));

  dlg.cbSensorDistin->SetValue(options.sensorDistingishesLandmarks);
  dlg.edSeed->SetValue(options.random_seed);

  dlg.edStdRange->SetValue(wxString::Format(_("%f"), m_SLAM.options.std_sensor_range));
  dlg.edStdAngle->SetValue(wxString::Format(_("%f"), RAD2DEG(m_SLAM.options.std_sensor_yaw)));

  dlg.edSpuriousMean->SetValue(wxString::Format(_("%f"), options.spurious_count_mean));
  dlg.edSpuriousStd->SetValue(wxString::Format(_("%f"), options.spurious_count_std));

  dlg.edSenX->SetValue(wxString::Format(_("%f"), options.sensorOnTheRobot.x()));
  dlg.edSenY->SetValue(wxString::Format(_("%f"), options.sensorOnTheRobot.y()));
  dlg.edSenPhi->SetValue(wxString::Format(_("%f"), RAD2DEG(options.sensorOnTheRobot.phi())));

  dlg.edMaxR->SetValue(wxString::Format(_("%f"), options.sensor_max_range));
  dlg.edMinR->SetValue(wxString::Format(_("%f"), options.sensor_min_range));
  dlg.edFOV->SetValue(wxString::Format(_("%f"), RAD2DEG(options.sensor_fov)));

  dlg.edChi2->SetValue(wxString::Format(_("%.04f"), m_SLAM.options.data_assoc_IC_chi2_thres));
  dlg.rbDAMethod->SetSelection(int(m_SLAM.options.data_assoc_method));
  dlg.rbDAMetric->SetSelection(int(m_SLAM.options.data_assoc_metric));
  dlg.rbICmetric->SetSelection(int(m_SLAM.options.data_assoc_IC_metric));
  dlg.edICMLrefDist->SetValue(
      wxString::Format(_("%.04f"), m_SLAM.options.data_assoc_IC_ml_threshold));

  dlg.edOverOdom->SetValue(options.uncert_overestim_odom * 100);
  dlg.edOverSensor->SetValue(options.uncert_overestim_sensor * 100);

  {
    wxCommandEvent dum;
    dlg.OnUpdateControlsState(dum);
  }

  if (dlg.ShowModal() == wxID_OK)
  {
    // Get all params
    // ---------------------------------------
    if (dlg.rbKFnaiv->GetValue()) m_SLAM.KF_options.method = kfEKFNaive;
    if (dlg.rbKFdavison->GetValue()) m_SLAM.KF_options.method = kfEKFAlaDavison;
    if (dlg.rbIKF->GetValue()) m_SLAM.KF_options.method = kfIKFFull;
    if (dlg.rbIKFdavison->GetValue()) m_SLAM.KF_options.method = kfIKF;
    m_SLAM.KF_options.IKF_iterations = dlg.edIKFiters->GetValue();

    m_SLAM.KF_options.use_analytic_transition_jacobian = !dlg.cbJacobTran->GetValue();
    m_SLAM.KF_options.use_analytic_observation_jacobian = !dlg.cbJacobObs->GetValue();

    //		m_SLAM.KF_options.fusion_strategy =
    // TKFFusionMethod(dlg.rbFusion->GetSelection());

    if (dlg.rbMapCorridor->GetValue())
      options.map_generator = "1";
    else if (dlg.rbMapRandom->GetValue())
      options.map_generator = "2";
    else
      options.map_generator = string(dlg.edMapFile->GetValue().mb_str());

    options.randomMap_nLMs = dlg.edLMs->GetValue();
    options.random_seed = dlg.edSeed->GetValue();

    options.path_square_len = atof(dlg.edPathLen->GetValue().mb_str());
    options.robot_step_length = atof(dlg.edPathStepSize->GetValue().mb_str());
    options.odometry_noise_std_xy = atof(dlg.edOdomStdXY->GetValue().mb_str());
    options.odometry_noise_std_phi = DEG2RAD(atof(dlg.edStdOdomPhi->GetValue().mb_str()));

    options.sensorDistingishesLandmarks = dlg.cbSensorDistin->GetValue();

    m_SLAM.options.std_sensor_range = atof(dlg.edStdRange->GetValue().mb_str());
    m_SLAM.options.std_sensor_yaw = DEG2RAD(atof(dlg.edStdAngle->GetValue().mb_str()));

    options.spurious_count_mean = atof(dlg.edSpuriousMean->GetValue().mb_str());
    options.spurious_count_std = atof(dlg.edSpuriousStd->GetValue().mb_str());

    options.sensorOnTheRobot.x(atof(dlg.edSenX->GetValue().mb_str()));
    options.sensorOnTheRobot.y(atof(dlg.edSenY->GetValue().mb_str()));
    options.sensorOnTheRobot.phi(DEG2RAD(atof(dlg.edSenPhi->GetValue().mb_str())));

    options.sensor_max_range = atof(dlg.edMaxR->GetValue().mb_str());
    options.sensor_min_range = atof(dlg.edMinR->GetValue().mb_str());
    options.sensor_fov = DEG2RAD(atof(dlg.edFOV->GetValue().mb_str()));

    m_SLAM.options.data_assoc_IC_chi2_thres = atof(dlg.edChi2->GetValue().mb_str());
    m_SLAM.options.data_assoc_method = TDataAssociationMethod(dlg.rbDAMethod->GetSelection());
    m_SLAM.options.data_assoc_metric = TDataAssociationMetric(dlg.rbDAMetric->GetSelection());
    m_SLAM.options.data_assoc_IC_metric = TDataAssociationMetric(dlg.rbICmetric->GetSelection());
    m_SLAM.options.data_assoc_IC_ml_threshold = atof(dlg.edICMLrefDist->GetValue().mb_str());

    options.uncert_overestim_odom = 0.01 * dlg.edOverOdom->GetValue();
    options.uncert_overestim_sensor = 0.01 * dlg.edOverSensor->GetValue();
  }
}

void slamdemoFrame::OnMenuSaveFilterState(wxCommandEvent& event)
{
  CPosePDFGaussian estRobotPose;
  vector<TPoint2D> LMs;
  map<unsigned int, CLandmark::TLandmarkID> landmarkIDs;

  CVectorDouble Xkk;  // Full mean & cov
  CMatrixDouble Pkk;

  m_SLAM.getCurrentState(estRobotPose, LMs, landmarkIDs, Xkk, Pkk);

  {
    wxFileDialog dialog(
        this, _("Save state vector...") /*caption*/, _(".") /* defaultDir */,
        _("kf_mean.txt") /* defaultFilename */,
        _("Text files (*.txt)|*.txt|All files (*.*)|*.*") /* wildcard */,
        wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

    if (dialog.ShowModal() != wxID_OK) return;
    string filName(dialog.GetPath().mb_str());

    Xkk.saveToTextFile(filName);
  }
  {
    wxFileDialog dialog(
        this, _("Save covariance matrix...") /*caption*/, _(".") /* defaultDir */,
        _("kf_cov.txt") /* defaultFilename */,
        _("Text files (*.txt)|*.txt|All files (*.*)|*.*") /* wildcard */,
        wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

    if (dialog.ShowModal() != wxID_OK) return;
    string filName(dialog.GetPath().mb_str());

    Pkk.saveToTextFile(filName);
  }

  {
    wxFileDialog dialog(
        this, _("Save as 3D opengl scene...") /*caption*/, _(".") /* defaultDir */,
        _("slam.3Dscene") /* defaultFilename */,
        _("MRPT 3D scenes "
          "(*.3Dscene)|*."
          "3Dscene|All files "
          "(*.*)|*.*") /* wildcard */,
        wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

    if (dialog.ShowModal() != wxID_OK) return;
    string filName(dialog.GetPath().mb_str());

    // Save as 3D objects:
    auto obj3D = mrpt::opengl::CSetOfObjects::Create();
    m_SLAM.getAs3DObject(obj3D);

    mrpt::opengl::Scene scene;
    scene.insert(obj3D);

    scene.saveToFile(filName);
  }
}

void slamdemoFrame::OnMenuProfilerViewStats(wxCommandEvent& event)
{
  string profStats = m_SLAM.getProfiler().getStatsAsText();
  CLogView dlg(this);
  dlg.edLog->SetValue(profStats.c_str());
  dlg.ShowModal();
}

void slamdemoFrame::OnMenuProfilerReset(wxCommandEvent& event) { m_SLAM.getProfiler().clear(); }

void slamdemoFrame::OnmnuSaveLastDASelected(wxCommandEvent& event)
{
  // Data association graphs ------------------------
  const CRangeBearingKFSLAM2D::TDataAssocInfo& da = m_SLAM.getLastDataAssociation();

  {
    wxFileDialog dialog(
        this, _("Save prediction landmark IDs...") /*caption*/, _(".") /* defaultDir */,
        _("prediction_IDs.txt") /* defaultFilename */,
        _("Text files (*.txt)|*.txt|All files (*.*)|*.*") /* wildcard */,
        wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

    if (dialog.ShowModal() != wxID_OK) return;
    string filName(dialog.GetPath().mb_str());

    mrpt::io::vectorToTextFile(da.predictions_IDs, filName);
  }
  {
    wxFileDialog dialog(
        this, _("Save prediction means...") /*caption*/, _(".") /* defaultDir */,
        _("prediction_means.txt") /* defaultFilename */,
        _("Text files (*.txt)|*.txt|All files (*.*)|*.*") /* wildcard */,
        wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

    if (dialog.ShowModal() != wxID_OK) return;
    string filName(dialog.GetPath().mb_str());

    da.Y_pred_means.saveToTextFile(filName);
  }
  {
    wxFileDialog dialog(
        this, _("Save prediction covariance...") /*caption*/, _(".") /* defaultDir */,
        _("prediction_cov.txt") /* defaultFilename */,
        _("Text files (*.txt)|*.txt|All files (*.*)|*.*") /* wildcard */,
        wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

    if (dialog.ShowModal() != wxID_OK) return;
    string filName(dialog.GetPath().mb_str());

    da.Y_pred_covs.saveToTextFile(filName);
  }
}

void slamdemoFrame::OnmnuItemSaveRawlogSelected(wxCommandEvent& event)
{
  const bool saveMnuChkd = mnuItemSaveRawlog->IsChecked();

  if (m_rawlog_out_file.fileOpenCorrectly()) m_rawlog_out_file.close();

  if (saveMnuChkd)
  {
    // Start saving:
    wxFileDialog dialog(
        this, _("Save data set...") /*caption*/, _(".") /* defaultDir */,
        _("simul-range-bearing_dataset.rawlog") /* defaultFilename */,
        _("Rawlogs (*.rawlog)|*.rawlog|All files (*.*)|*.*") /* wildcard */,
        wxFD_SAVE | wxFD_OVERWRITE_PROMPT);

    if (dialog.ShowModal() != wxID_OK) return;
    const string filName(dialog.GetPath().mb_str());

    if (!m_rawlog_out_file.open(filName))
    {
      mnuItemSaveRawlog->Check(false);
      wxMessageBox(_("Cannot open output file..."), _("Error"));
      return;
    }

    // Save a first "observation" with historic data:
    CObservationComment obs;
    obs.text = std::string(
                   "Rawlog generated by 2d-slam-demo\n"
                   " MRPT version: ") +
               mrpt::system::MRPT_getVersion() +
               std::string(
                   "\n"
                   " Creation date: ") +
               mrpt::system::dateTimeLocalToString(mrpt::Clock::now()) + std::string("\n");

    mrpt::serialization::archiveFrom(m_rawlog_out_file) << obs;
  }
}
