apps/mm-viewer/main.cpp
Go to the documentation of this file.
1 /* -------------------------------------------------------------------------
2  * A repertory of multi primitive-to-primitive (MP2P) ICP algorithms in C++
3  * Copyright (C) 2018-2021 Jose Luis Blanco, University of Almeria
4  * See LICENSE for license information.
5  * ------------------------------------------------------------------------- */
6 
14 // The goal is to visualize these guys:
15 #include <mp2p_icp/metricmap.h>
16 // using this:
17 #include <mrpt/gui/CDisplayWindowGUI.h>
18 
19 // other deps:
20 #include <mp2p_icp/pointcloud_sanity_check.h>
21 #include <mrpt/3rdparty/tclap/CmdLine.h>
22 #include <mrpt/config.h>
23 #include <mrpt/config/CConfigFile.h>
24 #include <mrpt/core/round.h>
25 #include <mrpt/math/TObject3D.h>
26 #include <mrpt/math/geometry.h>
27 #include <mrpt/opengl/CArrow.h>
28 #include <mrpt/opengl/CGridPlaneXY.h>
29 #include <mrpt/opengl/COpenGLScene.h>
30 #include <mrpt/opengl/stock_objects.h>
31 #include <mrpt/poses/CPose3DInterpolator.h>
32 #include <mrpt/system/filesystem.h>
33 #include <mrpt/system/os.h> // loadPluginModules()
34 #include <mrpt/system/string_utils.h> // unitsFormat()
35 
36 #include <iostream>
37 
38 #include "../libcfgpath/cfgpath.h"
39 
40 constexpr const char* APP_NAME = "mm-viewer";
41 constexpr int MID_FONT_SIZE = 14;
42 constexpr int SMALL_FONT_SIZE = 13;
43 
44 // =========== Declare supported cli switches ===========
45 static TCLAP::CmdLine cmd(APP_NAME);
46 
47 static TCLAP::UnlabeledValueArg<std::string> argMapFile(
48  "input", "Load this metric map file (*.mm)", false, "myMap.mm", "myMap.mm",
49  cmd);
50 
51 static TCLAP::ValueArg<std::string> arg_plugins(
52  "l", "load-plugins",
53  "One or more (comma separated) *.so files to load as plugins", false,
54  "foobar.so", "foobar.so", cmd);
55 
56 static TCLAP::ValueArg<std::string> arg_tumTrajectory(
57  "t", "trajectory",
58  "Also draw a trajectory, given by a TUM file trajectory.", false,
59  "trajectory.tum", "trajectory.tum", cmd);
60 
61 // =========== Declare global variables ===========
62 #if MRPT_HAS_NANOGUI
63 
64 auto glVizMap = mrpt::opengl::CSetOfObjects::Create();
65 auto glGrid = mrpt::opengl::CGridPlaneXY::Create();
66 mrpt::opengl::CSetOfObjects::Ptr glENUCorner, glMapCorner;
67 mrpt::opengl::CSetOfObjects::Ptr glTrajectory;
68 
69 mrpt::gui::CDisplayWindowGUI::Ptr win;
70 
71 std::array<nanogui::TextBox*, 2> lbMapStats = {nullptr, nullptr};
72 nanogui::CheckBox* cbApplyGeoRef = nullptr;
73 nanogui::CheckBox* cbViewOrtho = nullptr;
74 nanogui::CheckBox* cbView2D = nullptr;
75 nanogui::CheckBox* cbViewVoxelsAsPoints = nullptr;
76 nanogui::CheckBox* cbViewVoxelsFreeSpace = nullptr;
77 nanogui::CheckBox* cbColorizeMap = nullptr;
78 nanogui::CheckBox* cbKeepOriginalCloudColors = nullptr;
79 nanogui::CheckBox* cbShowGroundGrid = nullptr;
80 nanogui::Slider* slPointSize = nullptr;
81 nanogui::Slider* slTrajectoryThickness = nullptr;
82 nanogui::Slider* slMidDepthField = nullptr;
83 nanogui::Slider* slThicknessDepthField = nullptr;
84 nanogui::Slider* slCameraFOV = nullptr;
85 nanogui::Label* lbCameraFOV = nullptr;
86 nanogui::Label* lbMousePos = nullptr;
87 nanogui::Label* lbCameraPointing = nullptr;
88 nanogui::Label *lbDepthFieldValues = nullptr, *lbDepthFieldMid = nullptr,
89  *lbDepthFieldThickness = nullptr, *lbPointSize = nullptr;
90 nanogui::Label* lbTrajThick = nullptr;
91 nanogui::Widget* panelLayers = nullptr;
92 nanogui::ComboBox* cbTravellingKeys = nullptr;
93 nanogui::TextBox* edAnimFPS = nullptr;
94 nanogui::Slider* slAnimProgress = nullptr;
95 nanogui::Button * btnAnimate = nullptr, *btnAnimStop = nullptr;
96 nanogui::ComboBox* cbTravellingInterp = nullptr;
97 
98 std::vector<std::string> layerNames;
99 std::map<std::string, nanogui::CheckBox*> cbLayersByName;
100 bool doFitView = false;
101 
102 mp2p_icp::metric_map_t theMap;
103 std::string theMapFileName = "unnamed.mm";
104 
105 // Robot path to display (optional):
106 mrpt::poses::CPose3DInterpolator trajectory;
107 
108 // Camera travelling trajectory:
109 mrpt::poses::CPose3DInterpolator camTravelling;
110 std::optional<double> camTravellingCurrentTime;
111 
112 constexpr float TRAV_ZOOM2ROLL = 1e-4;
113 
114 static void rebuild_3d_view();
115 static void onSaveLayers();
116 
117 namespace
118 {
119 void loadMapFile(const std::string& mapFile)
120 {
121  // Load one single file:
122  std::cout << "Loading map file: " << mapFile << std::endl;
123 
124  if (!theMap.load_from_file(mapFile))
125  {
126  std::cerr << "Error loading metric map from file!" << std::endl;
127  return;
128  }
129  theMapFileName = mapFile;
130 
131  // Obtain layer info:
132  std::cout << "Loaded map: " << theMap.contents_summary() << std::endl;
133 
134  for (const auto& [name, map] : theMap.layers) layerNames.push_back(name);
135 
136  // sanity checks:
137  for (const auto& [name, map] : theMap.layers)
138  {
139  const auto* pc = mp2p_icp::MapToPointsMap(*map);
140  if (!pc) continue; // not a point map
141  const bool sanityPassed = mp2p_icp::pointcloud_sanity_check(*pc);
142  ASSERTMSG_(
143  sanityPassed,
144  mrpt::format(
145  "sanity check did not pass for layer: '%s'", name.c_str()));
146  }
147 }
148 
149 void updateMouseCoordinates()
150 {
151  const auto mousexY = win->mousePos();
152 
153  mrpt::math::TLine3D mouse_ray;
154  win->background_scene->getViewport("main")->get3DRayForPixelCoord(
155  mousexY.x(), mousexY.y(), mouse_ray);
156 
157  // Create a 3D plane, e.g. Z=0
158  using mrpt::math::TPoint3D;
159 
160  const mrpt::math::TPlane ground_plane(
161  TPoint3D(0, 0, 0), TPoint3D(1, 0, 0), TPoint3D(0, 1, 0));
162  // Intersection of the line with the plane:
163  mrpt::math::TObject3D inters;
164  mrpt::math::intersect(mouse_ray, ground_plane, inters);
165  // Interpret the intersection as a point, if there is an intersection:
166  mrpt::math::TPoint3D inters_pt;
167  if (inters.getPoint(inters_pt))
168  {
169  lbMousePos->setCaption(mrpt::format(
170  "Mouse pointing to: X=%6.03f Y=%6.03f", inters_pt.x, inters_pt.y));
171  }
172 }
173 
174 void updateCameraLookCoordinates()
175 {
176  lbCameraPointing->setCaption(mrpt::format(
177  "Looking at: X=%6.03f Y=%6.03f Z=%6.03f",
178  win->camera().getCameraPointingX(), win->camera().getCameraPointingY(),
179  win->camera().getCameraPointingZ()));
180 }
181 
182 void observeViewOptions()
183 {
184  if (cbView2D->checked())
185  {
186  win->camera().setAzimuthDegrees(-90.0f);
187  win->camera().setElevationDegrees(90.0f);
188  }
189 }
190 
191 void updateMiniCornerView()
192 {
193  auto gl_view = win->background_scene->getViewport("small-view");
194  if (!gl_view) return;
195 
196  mrpt::opengl::CCamera& view_cam = gl_view->getCamera();
197 
198  view_cam.setAzimuthDegrees(win->camera().getAzimuthDegrees());
199  view_cam.setElevationDegrees(win->camera().getElevationDegrees());
200  view_cam.setZoomDistance(5);
201 }
202 
203 void rebuildLayerCheckboxes()
204 {
205  ASSERT_(panelLayers);
206  while (panelLayers->childCount())
207  { //
208  panelLayers->removeChild(0);
209  }
210 
211  for (size_t i = 0; i < layerNames.size(); i++)
212  {
213  auto cb = panelLayers->add<nanogui::CheckBox>(layerNames.at(i));
214  cb->setChecked(true);
215  cb->setCallback([](bool) { rebuild_3d_view(); });
216  cb->setFontSize(SMALL_FONT_SIZE);
217 
218  cbLayersByName[layerNames.at(i)] = cb;
219  }
220 }
221 
222 void rebuildCamTravellingCombo()
223 {
224  std::vector<std::string> lst, lstShort;
225  for (size_t i = 0; i < camTravelling.size(); i++)
226  {
227  auto it = camTravelling.begin();
228  std::advance(it, i);
229 
230  lstShort.push_back(std::to_string(i));
231  lst.push_back(mrpt::format(
232  "[%02u] t=%.02fs pose=%s", static_cast<unsigned int>(i),
233  mrpt::Clock::toDouble(it->first), it->second.asString().c_str()));
234  }
235  cbTravellingKeys->setItems(lst, lstShort);
236 
237  if (!lst.empty()) cbTravellingKeys->setSelectedIndex(lst.size() - 1);
238  win->performLayout();
239 }
240 
241 void onKeyboardAction(int key)
242 {
243  using mrpt::DEG2RAD;
244 
245  constexpr float SLIDE_VELOCITY = 0.01;
246  constexpr float SENSIBILITY_ROTATE = 1.0;
247 
248  auto cam = win->camera().cameraParams();
249 
250  switch (key)
251  {
252  case GLFW_KEY_UP:
253  case GLFW_KEY_DOWN:
254  case GLFW_KEY_S:
255  case GLFW_KEY_W:
256  {
257  const float dx = std::cos(DEG2RAD(cam.cameraAzimuthDeg));
258  const float dy = std::sin(DEG2RAD(cam.cameraAzimuthDeg));
259  const float d =
260  (key == GLFW_KEY_UP || key == GLFW_KEY_W) ? -1.0f : 1.0f;
261  cam.cameraPointingX +=
262  d * dx * cam.cameraZoomDistance * SLIDE_VELOCITY;
263  cam.cameraPointingY +=
264  d * dy * cam.cameraZoomDistance * SLIDE_VELOCITY;
265  win->camera().setCameraParams(cam);
266  }
267  break;
268 
269  case GLFW_KEY_A:
270  case GLFW_KEY_D:
271  {
272  const float dx = std::cos(DEG2RAD(cam.cameraAzimuthDeg + 90.f));
273  const float dy = std::sin(DEG2RAD(cam.cameraAzimuthDeg + 90.f));
274  const float d = key == GLFW_KEY_A ? -1.0f : 1.0f;
275  cam.cameraPointingX +=
276  d * dx * cam.cameraZoomDistance * SLIDE_VELOCITY;
277  cam.cameraPointingY +=
278  d * dy * cam.cameraZoomDistance * SLIDE_VELOCITY;
279  win->camera().setCameraParams(cam);
280  }
281  break;
282 
283  case GLFW_KEY_RIGHT:
284  case GLFW_KEY_LEFT:
285  {
286  int cmd_rot = key == GLFW_KEY_LEFT ? -1 : 1;
287  int cmd_elev = 0;
288 
289  const float dis = std::max(0.01f, (cam.cameraZoomDistance));
290  float eye_x =
291  cam.cameraPointingX + dis * cos(DEG2RAD(cam.cameraAzimuthDeg)) *
292  cos(DEG2RAD(cam.cameraElevationDeg));
293  float eye_y =
294  cam.cameraPointingY + dis * sin(DEG2RAD(cam.cameraAzimuthDeg)) *
295  cos(DEG2RAD(cam.cameraElevationDeg));
296  float eye_z = cam.cameraPointingZ +
297  dis * sin(DEG2RAD(cam.cameraElevationDeg));
298 
299  // Orbit camera:
300  float A_AzimuthDeg = -SENSIBILITY_ROTATE * cmd_rot;
301  cam.cameraAzimuthDeg += A_AzimuthDeg;
302 
303  float A_ElevationDeg = SENSIBILITY_ROTATE * cmd_elev;
304  cam.setElevationDeg(cam.cameraElevationDeg + A_ElevationDeg);
305 
306  // Move cameraPointing pos:
307  cam.cameraPointingX =
308  eye_x - dis * cos(DEG2RAD(cam.cameraAzimuthDeg)) *
309  cos(DEG2RAD(cam.cameraElevationDeg));
310  cam.cameraPointingY =
311  eye_y - dis * sin(DEG2RAD(cam.cameraAzimuthDeg)) *
312  cos(DEG2RAD(cam.cameraElevationDeg));
313  cam.cameraPointingZ =
314  eye_z - dis * sin(DEG2RAD(cam.cameraElevationDeg));
315 
316  win->camera().setCameraParams(cam);
317  }
318  break;
319  };
320 }
321 
322 void camTravellingStop()
323 {
324  camTravellingCurrentTime.reset();
325  btnAnimate->setEnabled(true);
326  btnAnimStop->setEnabled(false);
327 }
328 
329 void processCameraTravelling()
330 {
331  if (!camTravellingCurrentTime.has_value()) return;
332  double& t = camTravellingCurrentTime.value();
333 
334  // Time range:
335  const double t0 = mrpt::Clock::toDouble(camTravelling.begin()->first);
336  const double t1 = mrpt::Clock::toDouble(camTravelling.rbegin()->first);
337  slAnimProgress->setRange(std::make_pair<float>(t0, t1));
338  slAnimProgress->setValue(t);
339 
340  if (t >= t1)
341  {
342  camTravellingStop();
343  return;
344  }
345 
346  // Interpolate camera params:
347  const auto interpMethod =
348  cbTravellingInterp->selectedIndex() == 0
349  ? mrpt::poses::TInterpolatorMethod::imLinear2Neig
350  : mrpt::poses::TInterpolatorMethod::imSSLSLL;
351  camTravelling.setInterpolationMethod(interpMethod);
352 
353  mrpt::math::TPose3D p;
354  bool valid = false;
355  camTravelling.interpolate(mrpt::Clock::fromDouble(t), p, valid);
356  if (valid)
357  {
358  win->camera().setCameraPointing(p.x, p.y, p.z);
359  win->camera().setAzimuthDegrees(mrpt::RAD2DEG(p.yaw));
360  win->camera().setElevationDegrees(mrpt::RAD2DEG(p.pitch));
361  win->camera().setZoomDistance(p.roll / TRAV_ZOOM2ROLL);
362  }
363 
364  // Move to next time step:
365  const double FPS = std::stod(edAnimFPS->value());
366  ASSERT_(FPS > 0);
367  const double dt = 1.0 / FPS;
368  t += dt;
369 }
370 
371 void main_show_gui()
372 {
373  using namespace std::string_literals;
374 
375  if (argMapFile.isSet()) { loadMapFile(argMapFile.getValue()); }
376 
377  // Get user app config file
378  char appCfgFile[1024];
379  ::get_user_config_file(appCfgFile, sizeof(appCfgFile), APP_NAME);
380  mrpt::config::CConfigFile appCfg(appCfgFile);
381 
382  /*
383  * -------------------------------------------------------------------
384  * GUI
385  * --------------------------------------------------------------------
386  */
387  nanogui::init();
388 
389  mrpt::gui::CDisplayWindowGUI_Params cp;
390  cp.maximized = true;
391  win = mrpt::gui::CDisplayWindowGUI::Create(APP_NAME, 1024, 800, cp);
392 
393  // Add a background scene:
394  auto scene = mrpt::opengl::COpenGLScene::Create();
395  {
396  glGrid->setColor_u8(0xff, 0xff, 0xff, 0x10);
397  scene->insert(glGrid);
398  }
399 
400  glMapCorner = mrpt::opengl::stock_objects::CornerXYZ(1.0f);
401  glMapCorner->setName("map");
402  glMapCorner->enableShowName();
403 
404  glTrajectory = mrpt::opengl::CSetOfObjects::Create();
405 
406  glENUCorner = mrpt::opengl::stock_objects::CornerXYZ(2.0f);
407  glENUCorner->setName("ENU");
408  glENUCorner->enableShowName();
409  scene->insert(glENUCorner);
410 
411  scene->insert(glVizMap);
412 
413  {
414  std::lock_guard<std::mutex> lck(win->background_scene_mtx);
415  win->background_scene = std::move(scene);
416  }
417 
418  // Control GUI sub-window:
419  {
420  auto w = win->createManagedSubWindow("Map viewer");
421  w->setPosition({5, 25});
422  w->requestFocus();
423  w->setLayout(new nanogui::BoxLayout(
424  nanogui::Orientation::Vertical, nanogui::Alignment::Fill, 5, 2));
425  w->setFixedWidth(350);
426 
427  for (size_t i = 0; i < lbMapStats.size(); i++)
428  {
429  auto& lb = lbMapStats[i];
430 
431  if (i == 0)
432  {
433  auto pn = w->add<nanogui::Widget>();
434  pn->setLayout(new nanogui::BoxLayout(
435  nanogui::Orientation::Horizontal,
436  nanogui::Alignment::Fill));
437  lb = pn->add<nanogui::TextBox>(" ");
438  lb->setFixedWidth(300);
439  auto btnLoad =
440  pn->add<nanogui::Button>("", ENTYPO_ICON_ARCHIVE);
441  btnLoad->setCallback(
442  []()
443  {
444  try
445  {
446  const auto fil = nanogui::file_dialog(
447  {{"mm", "Metric maps (*.mm)"}}, false);
448  if (fil.empty()) return;
449 
450  loadMapFile(fil);
451  rebuildLayerCheckboxes();
452  win->performLayout();
453  rebuild_3d_view();
454  }
455  catch (const std::exception& e)
456  {
457  std::cerr << e.what() << std::endl;
458  }
459  });
460  }
461  else { lb = w->add<nanogui::TextBox>(" "); }
462 
463  lb->setFontSize(MID_FONT_SIZE);
464  lb->setAlignment(nanogui::TextBox::Alignment::Left);
465  lb->setEditable(true);
466  }
467 
468  //
469  w->add<nanogui::Label>(" "); // separator
470 
471  auto tabWidget = w->add<nanogui::TabWidget>();
472 
473  auto* tab1 = tabWidget->createTab("View");
474  tab1->setLayout(new nanogui::GroupLayout());
475 
476  auto* tab2 = tabWidget->createTab("Maps");
477  tab2->setLayout(new nanogui::BoxLayout(
478  nanogui::Orientation::Vertical, nanogui::Alignment::Fill));
479 
480  auto* tab3 = tabWidget->createTab("Travelling");
481  tab3->setLayout(new nanogui::BoxLayout(
482  nanogui::Orientation::Vertical, nanogui::Alignment::Fill));
483 
484  tabWidget->setActiveTab(0);
485 
486  // --------------------------------------------------------------
487  // Tab: Map layers
488  // --------------------------------------------------------------
489  tab2->add<nanogui::Label>("Render options:");
490 
491  {
492  auto pn = tab2->add<nanogui::Widget>();
493  pn->setLayout(new nanogui::GridLayout(
494  nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill));
495 
496  lbPointSize = pn->add<nanogui::Label>("Point size");
497  lbPointSize->setFontSize(MID_FONT_SIZE);
498 
499  slPointSize = pn->add<nanogui::Slider>();
500  slPointSize->setRange({1.0f, 10.0f});
501  slPointSize->setValue(2.0f);
502  slPointSize->setCallback([&](float) { rebuild_3d_view(); });
503  }
504 
505  cbViewVoxelsAsPoints =
506  tab2->add<nanogui::CheckBox>("Render voxel maps as point clouds");
507  cbViewVoxelsAsPoints->setFontSize(MID_FONT_SIZE);
508  cbViewVoxelsAsPoints->setChecked(false);
509  cbViewVoxelsAsPoints->setCallback([&](bool) { rebuild_3d_view(); });
510 
511  cbViewVoxelsFreeSpace =
512  tab2->add<nanogui::CheckBox>("Render free space of voxel maps");
513  cbViewVoxelsFreeSpace->setFontSize(MID_FONT_SIZE);
514  cbViewVoxelsFreeSpace->setChecked(false);
515  cbViewVoxelsFreeSpace->setCallback([&](bool) { rebuild_3d_view(); });
516 
517  cbColorizeMap = tab2->add<nanogui::CheckBox>("Recolorize map points");
518  cbColorizeMap->setFontSize(MID_FONT_SIZE);
519  cbColorizeMap->setChecked(true);
520  cbColorizeMap->setCallback([&](bool) { rebuild_3d_view(); });
521 
522  cbKeepOriginalCloudColors =
523  tab2->add<nanogui::CheckBox>("Keep original cloud colors");
524  cbKeepOriginalCloudColors->setFontSize(MID_FONT_SIZE);
525  cbKeepOriginalCloudColors->setChecked(false);
526  cbKeepOriginalCloudColors->setCallback([&](bool)
527  { rebuild_3d_view(); });
528 
529  tab2->add<nanogui::Label>(" ");
530  {
531  auto pn = tab2->add<nanogui::Widget>();
532  pn->setLayout(new nanogui::GridLayout(
533  nanogui::Orientation::Horizontal, 4, nanogui::Alignment::Fill));
534  pn->add<nanogui::Label>("Visible layers:");
535 
536  auto* btnCheckAll = pn->add<nanogui::Button>("", ENTYPO_ICON_CHECK);
537  btnCheckAll->setFontSize(SMALL_FONT_SIZE);
538  btnCheckAll->setCallback(
539  []()
540  {
541  for (auto& [name, cb] : cbLayersByName)
542  cb->setChecked(true);
543 
544  rebuild_3d_view();
545  });
546 
547  auto* btnCheckNone =
548  pn->add<nanogui::Button>("", ENTYPO_ICON_CIRCLE_WITH_CROSS);
549  btnCheckNone->setFontSize(SMALL_FONT_SIZE);
550  btnCheckNone->setCallback(
551  []()
552  {
553  for (auto& [name, cb] : cbLayersByName)
554  cb->setChecked(false);
555  rebuild_3d_view();
556  });
557  }
558 
559  panelLayers = tab2->add<nanogui::Widget>();
560  panelLayers->setLayout(new nanogui::BoxLayout(
561  nanogui::Orientation::Vertical, nanogui::Alignment::Fill));
562 
563  rebuildLayerCheckboxes();
564 
565  {
566  tab2->add<nanogui::Label>(" "); // separator
567  auto btnSave =
568  tab2->add<nanogui::Button>("Export marked layers...");
569  btnSave->setFontSize(MID_FONT_SIZE);
570  btnSave->setCallback([]() { onSaveLayers(); });
571  }
572 
573  // --------------------------------------------------------------
574  // Tab: View
575  // --------------------------------------------------------------
576  {
577  auto pn = tab1->add<nanogui::Widget>();
578  pn->setLayout(new nanogui::GridLayout(
579  nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill));
580 
581  lbTrajThick = pn->add<nanogui::Label>("Trajectory thickness:");
582  lbTrajThick->setFontSize(MID_FONT_SIZE);
583 
584  slTrajectoryThickness = pn->add<nanogui::Slider>();
585  slTrajectoryThickness->setEnabled(trajectory.size() >= 2);
586  slTrajectoryThickness->setRange({std::log(0.005f), std::log(2.0f)});
587  slTrajectoryThickness->setValue(std::log(0.05f));
588  slTrajectoryThickness->setCallback(
589  [&](float)
590  {
591  glTrajectory->clear(); // force rebuild
592  rebuild_3d_view();
593  });
594  }
595 
596  {
597  auto pn = tab1->add<nanogui::Widget>();
598  pn->setLayout(new nanogui::GridLayout(
599  nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill));
600 
601  lbDepthFieldMid =
602  pn->add<nanogui::Label>("Center depth clip plane:");
603  lbDepthFieldMid->setFontSize(MID_FONT_SIZE);
604 
605  slMidDepthField = pn->add<nanogui::Slider>();
606  slMidDepthField->setRange({-2.0, 3.0});
607  slMidDepthField->setValue(1.0f);
608  slMidDepthField->setCallback([&](float) { rebuild_3d_view(); });
609  }
610 
611  {
612  auto pn = tab1->add<nanogui::Widget>();
613  pn->setLayout(new nanogui::GridLayout(
614  nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill));
615 
616  lbDepthFieldThickness =
617  pn->add<nanogui::Label>("Max-Min depth thickness:");
618  lbDepthFieldThickness->setFontSize(MID_FONT_SIZE);
619 
620  slThicknessDepthField = pn->add<nanogui::Slider>();
621  slThicknessDepthField->setRange({-2.0, 6.0});
622  slThicknessDepthField->setValue(3.0);
623  slThicknessDepthField->setCallback([&](float)
624  { rebuild_3d_view(); });
625  }
626  {
627  auto pn = tab1->add<nanogui::Widget>();
628  pn->setLayout(new nanogui::GridLayout(
629  nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill));
630 
631  lbCameraFOV = pn->add<nanogui::Label>("Camera FOV:");
632  lbCameraFOV->setFontSize(MID_FONT_SIZE);
633  slCameraFOV = pn->add<nanogui::Slider>();
634  slCameraFOV->setRange({20.0f, 170.0f});
635  slCameraFOV->setValue(90.0f);
636  slCameraFOV->setCallback([&](float) { rebuild_3d_view(); });
637  }
638  lbDepthFieldValues = tab1->add<nanogui::Label>(" ");
639  lbDepthFieldValues->setFontSize(MID_FONT_SIZE);
640 
641  {
642  auto pn = tab1->add<nanogui::Widget>();
643  pn->setLayout(new nanogui::GridLayout(
644  nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill));
645 
646  cbViewOrtho = pn->add<nanogui::CheckBox>("Orthogonal view");
647  cbViewOrtho->setFontSize(MID_FONT_SIZE);
648  cbViewOrtho->setCallback([&](bool) { rebuild_3d_view(); });
649  cbViewOrtho->setFontSize(MID_FONT_SIZE);
650 
651  cbView2D = pn->add<nanogui::CheckBox>("Force 2D view");
652  cbView2D->setFontSize(MID_FONT_SIZE);
653  cbView2D->setCallback([&](bool) { rebuild_3d_view(); });
654  }
655 
656  {
657  auto pn = tab1->add<nanogui::Widget>();
658  pn->setLayout(new nanogui::GridLayout(
659  nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill));
660 
661  cbShowGroundGrid = pn->add<nanogui::CheckBox>("Show ground grid");
662  cbShowGroundGrid->setFontSize(MID_FONT_SIZE);
663  cbShowGroundGrid->setChecked(true);
664  cbShowGroundGrid->setCallback([&](bool) { rebuild_3d_view(); });
665 
666  auto btnFitView = pn->add<nanogui::Button>("Fit view to map");
667  btnFitView->setFontSize(MID_FONT_SIZE);
668  btnFitView->setCallback(
669  [&]()
670  {
671  doFitView = true;
672  rebuild_3d_view();
673  });
674  }
675 
676  cbApplyGeoRef = tab1->add<nanogui::CheckBox>(
677  "Apply georeferenced pose (if available)");
678  cbApplyGeoRef->setFontSize(MID_FONT_SIZE);
679  cbApplyGeoRef->setCallback([&](bool) { rebuild_3d_view(); });
680 
681  // --------------------------------------------------------------
682  // Tab: Travelling
683  // --------------------------------------------------------------
684  tab3->add<nanogui::Label>("Define camera travelling paths");
685 
686  {
687  auto pn = tab3->add<nanogui::Widget>();
688  pn->setLayout(new nanogui::GridLayout(
689  nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill));
690 
691  auto lb = pn->add<nanogui::Label>("Keyframes:");
692  lb->setFontSize(MID_FONT_SIZE);
693 
694  cbTravellingKeys = tab3->add<nanogui::ComboBox>();
695  cbTravellingKeys->setFontSize(MID_FONT_SIZE);
696  }
697  rebuildCamTravellingCombo();
698 
699  tab3->add<nanogui::Label>("");
700 
701  nanogui::TextBox* edTime;
702  {
703  auto pn = tab3->add<nanogui::Widget>();
704  pn->setLayout(new nanogui::GridLayout(
705  nanogui::Orientation::Horizontal, 3, nanogui::Alignment::Fill));
706 
707  pn->add<nanogui::Label>("New keyframe:")
708  ->setFontSize(MID_FONT_SIZE);
709 
710  edTime = pn->add<nanogui::TextBox>();
711  edTime->setAlignment(nanogui::TextBox::Alignment::Left);
712  edTime->setValue("0.0");
713  edTime->setEditable(true);
714  edTime->setPlaceholder("Time for this keyframe [s]");
715  edTime->setFormat("[0-9\\.]*");
716  edTime->setFontSize(MID_FONT_SIZE);
717 
718  auto btnAdd =
719  pn->add<nanogui::Button>("Add", ENTYPO_ICON_ADD_TO_LIST);
720  btnAdd->setFontSize(MID_FONT_SIZE);
721  btnAdd->setCallback(
722  [edTime]()
723  {
724  const auto p = mrpt::math::TPose3D(
725  win->camera().cameraParams().cameraPointingX,
726  win->camera().cameraParams().cameraPointingY,
727  win->camera().cameraParams().cameraPointingZ,
728  mrpt::DEG2RAD(
729  win->camera().cameraParams().cameraAzimuthDeg),
730  mrpt::DEG2RAD(
731  win->camera().cameraParams().cameraElevationDeg),
732  win->camera().cameraParams().cameraZoomDistance *
733  TRAV_ZOOM2ROLL);
734  camTravelling.insert(
735  mrpt::Clock::fromDouble(std::stod(edTime->value())), p);
736  rebuildCamTravellingCombo();
737 
738  edTime->setValue(
739  std::to_string(std::stod(edTime->value()) + 1));
740  });
741  }
742 
743  tab3->add<nanogui::Label>("");
744 
745  {
746  auto pn = tab3->add<nanogui::Widget>();
747  pn->setLayout(new nanogui::GridLayout(
748  nanogui::Orientation::Horizontal, 3, nanogui::Alignment::Fill));
749 
750  pn->add<nanogui::Label>("Playback:");
751  btnAnimate =
752  pn->add<nanogui::Button>("", ENTYPO_ICON_CONTROLLER_PLAY);
753  btnAnimate->setFontSize(MID_FONT_SIZE);
754  btnAnimate->setCallback(
755  []()
756  {
757  if (camTravelling.empty()) return;
758  camTravellingCurrentTime.emplace(
759  mrpt::Clock::toDouble(camTravelling.begin()->first));
760  btnAnimate->setEnabled(false);
761  btnAnimStop->setEnabled(true);
762  });
763  btnAnimStop =
764  pn->add<nanogui::Button>("", ENTYPO_ICON_CIRCLE_WITH_CROSS);
765  btnAnimStop->setFontSize(MID_FONT_SIZE);
766  btnAnimStop->setEnabled(false);
767  btnAnimStop->setCallback([]() { camTravellingStop(); });
768  }
769 
770  {
771  auto pn = tab3->add<nanogui::Widget>();
772  pn->setLayout(new nanogui::GridLayout(
773  nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill));
774 
775  auto lb = pn->add<nanogui::Label>("Animation FPS:");
776  lb->setFontSize(MID_FONT_SIZE);
777 
778  edAnimFPS = pn->add<nanogui::TextBox>("30.0");
779  edAnimFPS->setFontSize(MID_FONT_SIZE);
780  edAnimFPS->setFormat("[0-9\\.]*");
781  edAnimFPS->setEditable(true);
782  }
783  {
784  auto pn = tab3->add<nanogui::Widget>();
785  pn->setLayout(new nanogui::GridLayout(
786  nanogui::Orientation::Horizontal, 2, nanogui::Alignment::Fill));
787 
788  auto lb = pn->add<nanogui::Label>("Interpolation:");
789  lb->setFontSize(MID_FONT_SIZE);
790 
791  cbTravellingInterp = pn->add<nanogui::ComboBox>();
792  cbTravellingInterp->setFontSize(MID_FONT_SIZE);
793  cbTravellingInterp->setItems({"Linear", "Spline"});
794  cbTravellingInterp->setSelectedIndex(0);
795  }
796 
797  slAnimProgress = tab3->add<nanogui::Slider>();
798  slAnimProgress->setEnabled(false);
799 
800  // ---
801  lbMousePos = w->add<nanogui::Label>("Mouse pointing to:");
802  lbMousePos->setFontSize(MID_FONT_SIZE);
803  lbCameraPointing = w->add<nanogui::Label>("Camera looking at:");
804  lbCameraPointing->setFontSize(MID_FONT_SIZE);
805 
806  w->add<nanogui::Button>("Quit", ENTYPO_ICON_ARROW_BOLD_LEFT)
807  ->setCallback([]() { win->setVisible(false); });
808 
809  win->setKeyboardCallback(
810  [&](int key, [[maybe_unused]] int scancode, int action,
811  [[maybe_unused]] int modifiers)
812  {
813  if (action != GLFW_PRESS && action != GLFW_REPEAT) return false;
814 
815  onKeyboardAction(key);
816 
817  return false;
818  });
819  }
820 
821  // --------------------------------------------------------------
822  // ^^^^^^^^ GUI definition done ^^^^^^^^
823  // --------------------------------------------------------------
824  win->performLayout();
825  win->camera().setCameraPointing(8.0f, .0f, .0f);
826  win->camera().setAzimuthDegrees(110.0f);
827  win->camera().setElevationDegrees(15.0f);
828  win->camera().setZoomDistance(50.0f);
829 
830  // save and load UI state:
831 #define LOAD_CB_STATE(CB_NAME__) do_cb(CB_NAME__, #CB_NAME__)
832 #define SAVE_CB_STATE(CB_NAME__) \
833  appCfg.write("", #CB_NAME__, CB_NAME__->checked())
834 
835 #define LOAD_SL_STATE(SL_NAME__) do_sl(SL_NAME__, #SL_NAME__)
836 #define SAVE_SL_STATE(SL_NAME__) \
837  appCfg.write("", #SL_NAME__, SL_NAME__->value())
838 
839  auto load_UI_state_from_user_config = [&]()
840  {
841  auto do_cb = [&](nanogui::CheckBox* cb, const std::string& name)
842  { cb->setChecked(appCfg.read_bool("", name, cb->checked())); };
843  auto do_sl = [&](nanogui::Slider* sl, const std::string& name)
844  { sl->setValue(appCfg.read_float("", name, sl->value())); };
845 
846  LOAD_CB_STATE(cbApplyGeoRef);
847  LOAD_CB_STATE(cbViewOrtho);
848  LOAD_CB_STATE(cbView2D);
849  LOAD_CB_STATE(cbViewVoxelsAsPoints);
850  LOAD_CB_STATE(cbViewVoxelsFreeSpace);
851  LOAD_CB_STATE(cbColorizeMap);
852  LOAD_CB_STATE(cbKeepOriginalCloudColors);
853  LOAD_CB_STATE(cbShowGroundGrid);
854 
855  LOAD_SL_STATE(slPointSize);
856  LOAD_SL_STATE(slTrajectoryThickness);
857  LOAD_SL_STATE(slMidDepthField);
858  LOAD_SL_STATE(slThicknessDepthField);
859  LOAD_SL_STATE(slCameraFOV);
860 
861  win->camera().setCameraPointing(
862  appCfg.read_float("", "cam_x", win->camera().getCameraPointingX()),
863  appCfg.read_float("", "cam_y", win->camera().getCameraPointingY()),
864  appCfg.read_float("", "cam_z", win->camera().getCameraPointingZ()));
865  win->camera().setAzimuthDegrees(
866  appCfg.read_float("", "cam_az", win->camera().getAzimuthDegrees()));
867  win->camera().setElevationDegrees(appCfg.read_float(
868  "", "cam_el", win->camera().getElevationDegrees()));
869  win->camera().setZoomDistance(
870  appCfg.read_float("", "cam_d", win->camera().getZoomDistance()));
871  };
872  auto save_UI_state_to_user_config = [&]()
873  {
874  SAVE_CB_STATE(cbApplyGeoRef);
875  SAVE_CB_STATE(cbViewOrtho);
876  SAVE_CB_STATE(cbView2D);
877  SAVE_CB_STATE(cbViewVoxelsAsPoints);
878  SAVE_CB_STATE(cbViewVoxelsFreeSpace);
879  SAVE_CB_STATE(cbColorizeMap);
880  SAVE_CB_STATE(cbKeepOriginalCloudColors);
881  SAVE_CB_STATE(cbShowGroundGrid);
882 
883  SAVE_SL_STATE(slPointSize);
884  SAVE_SL_STATE(slTrajectoryThickness);
885  SAVE_SL_STATE(slMidDepthField);
886  SAVE_SL_STATE(slThicknessDepthField);
887  SAVE_SL_STATE(slCameraFOV);
888 
889  appCfg.write("", "cam_x", win->camera().getCameraPointingX());
890  appCfg.write("", "cam_y", win->camera().getCameraPointingY());
891  appCfg.write("", "cam_z", win->camera().getCameraPointingZ());
892  appCfg.write("", "cam_az", win->camera().getAzimuthDegrees());
893  appCfg.write("", "cam_el", win->camera().getElevationDegrees());
894  appCfg.write("", "cam_d", win->camera().getZoomDistance());
895  };
896 
897  // load UI state from last session:
898  load_UI_state_from_user_config();
899 
900  // Build 3D:
901  rebuild_3d_view();
902 
903  // Main loop
904  // ---------------------
905  win->drawAll();
906  win->setVisible(true);
907 
908  win->addLoopCallback(
909  [&]()
910  {
911  updateMouseCoordinates();
912  updateCameraLookCoordinates();
913  observeViewOptions();
914  updateMiniCornerView();
915  processCameraTravelling();
916  });
917 
918  nanogui::mainloop(1000 /*idleLoopPeriod ms*/, 25 /* minRepaintPeriod ms */);
919 
920  nanogui::shutdown();
921 
922  // save UI state:
923  save_UI_state_to_user_config();
924 }
925 
926 } // namespace
927 
928 // ==============================
929 // rebuild_3d_view
930 // ==============================
931 void rebuild_3d_view()
932 {
933  using namespace std::string_literals;
934 
935  lbMapStats[0]->setValue(theMapFileName);
936  lbMapStats[1]->setValue("Map: "s + theMap.contents_summary());
937 
938  cbApplyGeoRef->setEnabled(theMap.georeferencing.has_value());
939 
940  // 3D objects -------------------
941  std::optional<mrpt::math::TBoundingBoxf> mapBbox;
942 
943  // the map:
944  mp2p_icp::render_params_t rpMap;
945 
946  rpMap.points.visible = false;
947  for (const auto& [lyName, cb] : cbLayersByName)
948  {
949  // Update stats in the cb label:
950  cb->setCaption(lyName); // default
951  if (auto itL = theMap.layers.find(lyName); itL != theMap.layers.end())
952  {
953  if (auto pc = std::dynamic_pointer_cast<mrpt::maps::CPointsMap>(
954  itL->second);
955  pc)
956  {
957  cb->setCaption(
958  lyName + " | "s +
959  mrpt::system::unitsFormat(
960  static_cast<double>(pc->size()), 2, false) +
961  " points"s + " | class="s +
962  pc->GetRuntimeClass()->className);
963 
964  const auto bb = pc->boundingBox();
965  if (!mapBbox.has_value())
966  mapBbox = bb;
967  else { mapBbox = mapBbox->unionWith(bb); }
968  }
969  else
970  {
971  cb->setCaption(
972  lyName + " | class="s +
973  itL->second->GetRuntimeClass()->className);
974  }
975  }
976 
977  // show/hide:
978  if (!cb->checked()) continue; // hidden
979  rpMap.points.visible = true;
980 
981  auto& rpL = rpMap.points.perLayer[lyName];
982  rpL.pointSize = slPointSize->value();
983  rpL.render_voxelmaps_as_points = cbViewVoxelsAsPoints->checked();
984  rpL.render_voxelmaps_free_space = cbViewVoxelsFreeSpace->checked();
985 
986  lbPointSize->setCaption("Point size: " + std::to_string(rpL.pointSize));
987 
988  if (cbColorizeMap->checked())
989  {
990  auto& cm = rpL.colorMode.emplace();
991  cm.colorMap = mrpt::img::TColormap::cmJET;
992  cm.recolorizeByCoordinate = mp2p_icp::Coordinate::Z;
993  }
994  if (cbKeepOriginalCloudColors->checked())
995  {
996  auto& cm = rpL.colorMode.emplace();
997  cm.keep_original_cloud_color = true;
998  }
999  }
1000 
1001  // Default color:
1002  for (auto& [layer, rp] : rpMap.points.perLayer)
1003  rp.color = mrpt::img::TColor(0xff, 0x00, 0x00, 0xff);
1004 
1005  // Regenerate points opengl representation only if some parameter changed:
1006  static std::optional<mp2p_icp::render_params_t> prevRenderParams;
1007 
1008  if (!prevRenderParams.has_value() || prevRenderParams.value() != rpMap)
1009  {
1010  prevRenderParams = rpMap;
1011  glVizMap->clear();
1012 
1013  auto glPts = theMap.get_visualization(rpMap);
1014 
1015  // Show all or selected layers:
1016  rpMap.points.allLayers.color =
1017  mrpt::img::TColor(0xff, 0x00, 0x00, 0xff);
1018 
1019  glVizMap->insert(glPts);
1020  glVizMap->insert(glMapCorner);
1021  glVizMap->insert(glTrajectory);
1022  }
1023 
1024  if (cbApplyGeoRef->checked() && theMap.georeferencing.has_value())
1025  {
1026  glVizMap->setPose(theMap.georeferencing->T_enu_to_map.mean);
1027  glGrid->setPose(theMap.georeferencing->T_enu_to_map.mean);
1028  glENUCorner->setVisibility(true);
1029  }
1030  else
1031  {
1032  glVizMap->setPose(mrpt::poses::CPose3D::Identity());
1033  glGrid->setPose(mrpt::poses::CPose3D::Identity());
1034  glENUCorner->setVisibility(false);
1035  }
1036 
1037  // ground grid:
1038  if (mapBbox)
1039  {
1040  glGrid->setPlaneLimits(
1041  mapBbox->min.x, mapBbox->max.x, mapBbox->min.y, mapBbox->max.y);
1042  }
1043  glGrid->setVisibility(cbShowGroundGrid->checked());
1044 
1045  // Fit view to map:
1046  if (mapBbox && doFitView)
1047  {
1048  const auto midPt = (mapBbox->min + mapBbox->max) * 0.5;
1049  const auto mapLen = (mapBbox->max - mapBbox->min).norm();
1050 
1051  win->camera().setCameraPointing(midPt.x, midPt.y, midPt.z);
1052  win->camera().setZoomDistance(mapLen);
1053  }
1054  doFitView = false;
1055 
1056  // glTrajectory:
1057  if (glTrajectory->empty() && trajectory.size() >= 2)
1058  {
1059  const float trajCylRadius = std::exp(slTrajectoryThickness->value());
1060  lbTrajThick->setCaption(
1061  "Traj. thickness: " + std::to_string(trajCylRadius));
1062 
1063  std::optional<mrpt::math::TPose3D> prevPose;
1064  for (const auto& [t, p] : trajectory)
1065  {
1066  if (prevPose)
1067  {
1068  const auto& p0 = prevPose.value();
1069 
1070  auto glSegment = mrpt::opengl::CArrow::Create();
1071  glSegment->setArrowEnds(p0.translation(), p.translation());
1072  glSegment->setHeadRatio(.0);
1073  glSegment->setLargeRadius(trajCylRadius);
1074  glSegment->setSmallRadius(trajCylRadius);
1075  glSegment->setColor_u8(0x30, 0x30, 0x30, 0xff);
1076 
1077  glTrajectory->insert(glSegment);
1078  }
1079  prevPose = p;
1080  }
1081  }
1082 
1083  // XYZ corner overlay viewport:
1084  {
1085  auto gl_view = win->background_scene->createViewport("small-view");
1086 
1087  gl_view->setViewportPosition(0, 0, 0.1, 0.1 * 16.0 / 9.0);
1088  gl_view->setTransparent(true);
1089  {
1090  mrpt::opengl::CText::Ptr obj = mrpt::opengl::CText::Create("X");
1091  obj->setLocation(1.1, 0, 0);
1092  gl_view->insert(obj);
1093  }
1094  {
1095  mrpt::opengl::CText::Ptr obj = mrpt::opengl::CText::Create("Y");
1096  obj->setLocation(0, 1.1, 0);
1097  gl_view->insert(obj);
1098  }
1099  {
1100  mrpt::opengl::CText::Ptr obj = mrpt::opengl::CText::Create("Z");
1101  obj->setLocation(0, 0, 1.1);
1102  gl_view->insert(obj);
1103  }
1104  gl_view->insert(mrpt::opengl::stock_objects::CornerXYZ());
1105  }
1106 
1107  // Global view options:
1108  {
1109  std::lock_guard<std::mutex> lck(win->background_scene_mtx);
1110  win->camera().setCameraProjective(
1111  !cbViewOrtho->checked() && !cbView2D->checked());
1112 
1113  // clip planes:
1114  const auto depthFieldMid = std::pow(10.0, slMidDepthField->value());
1115  const auto depthFieldThickness =
1116  std::pow(10.0, slThicknessDepthField->value());
1117 
1118  const auto clipNear =
1119  std::max(1e-2, depthFieldMid - 0.5 * depthFieldThickness);
1120  const auto clipFar = depthFieldMid + 0.5 * depthFieldThickness;
1121 
1122  const float cameraFOV = slCameraFOV->value();
1123  win->camera().setCameraFOV(cameraFOV);
1124  win->camera().setMaximumZoom(std::max<double>(1000, 3.0 * clipFar));
1125 
1126  lbDepthFieldValues->setCaption(
1127  mrpt::format("Depth field: near=%g far=%g", clipNear, clipFar));
1128  lbDepthFieldMid->setCaption(
1129  mrpt::format("Frustrum center: %.03g", depthFieldMid));
1130  lbDepthFieldThickness->setCaption(
1131  mrpt::format("Frustum thickness: %.03g", depthFieldThickness));
1132 
1133  lbDepthFieldValues->setCaption(
1134  mrpt::format("Frustum: near=%.02g far=%.02g", clipNear, clipFar));
1135 
1136  lbCameraFOV->setCaption(
1137  mrpt::format("Camera FOV: %.02f deg", cameraFOV));
1138 
1139  win->background_scene->getViewport()->setViewportClipDistances(
1140  clipNear, clipFar);
1141  }
1142 }
1143 
1144 void onSaveLayers()
1145 {
1146  const std::string outFile =
1147  nanogui::file_dialog({{"txt", "(*.txt)"}}, true /*save*/);
1148  if (outFile.empty()) return;
1149 
1150  for (const auto& [lyName, cb] : cbLayersByName)
1151  {
1152  if (auto itL = theMap.layers.find(lyName); itL != theMap.layers.end())
1153  {
1154  itL->second->saveMetricMapRepresentationToFile(outFile);
1155  }
1156  }
1157 }
1158 
1159 #else // MRPT_HAS_NANOGUI
1160 static void main_show_gui()
1161 {
1162  THROW_EXCEPTION(
1163  "This application requires a version of MRPT built with nanogui "
1164  "support.");
1165 }
1166 
1167 #endif // MRPT_HAS_NANOGUI
1168 
1169 int main(int argc, char** argv)
1170 {
1171  try
1172  {
1173  // Parse arguments:
1174  if (!cmd.parse(argc, argv)) return 1; // should exit.
1175 
1176  // Load plugins:
1177  if (arg_plugins.isSet())
1178  {
1179  std::string errMsg;
1180  const auto plugins = arg_plugins.getValue();
1181  std::cout << "Loading plugin(s): " << plugins << std::endl;
1182  if (!mrpt::system::loadPluginModules(plugins, errMsg))
1183  {
1184  std::cerr << errMsg << std::endl;
1185  return 1;
1186  }
1187  }
1188 
1189  // load trajectory?
1190  if (arg_tumTrajectory.isSet())
1191  {
1192  ASSERT_FILE_EXISTS_(arg_tumTrajectory.getValue());
1193  bool trajectoryReadOk =
1194  trajectory.loadFromTextFile_TUM(arg_tumTrajectory.getValue());
1195  ASSERT_(trajectoryReadOk);
1196  std::cout << "Read trajectory with " << trajectory.size()
1197  << " keyframes.\n";
1198  }
1199 
1200  main_show_gui();
1201  return 0;
1202  }
1203  catch (std::exception& e)
1204  {
1205  std::cerr << "Exit due to exception:\n"
1206  << mrpt::exception_to_str(e) << std::endl;
1207  return 1;
1208  }
1209 }
main_show_gui
static void main_show_gui()
Definition: apps/mm-viewer/main.cpp:1160
mp2p_icp::render_params_t::points
render_params_points_t points
Definition: render_params.h:190
kitti-run-seq.f
string f
Definition: kitti-run-seq.py:12
mp2p_icp::Coordinate::Z
@ Z
mp2p_icp::pointcloud_sanity_check
bool pointcloud_sanity_check(const mrpt::maps::CPointsMap &pc, bool printWarnings=true)
Definition: pointcloud_sanity_check.cpp:19
pointcloud_sanity_check.h
Checks for consistent length of field vectors.
testing::internal::string
::std::string string
Definition: gtest.h:1979
arg_plugins
static TCLAP::ValueArg< std::string > arg_plugins("l", "load-plugins", "One or more (comma separated) *.so files to load as plugins", false, "foobar.so", "foobar.so", cmd)
cmd
static TCLAP::CmdLine cmd(APP_NAME)
mp2p_icp::render_params_points_t::allLayers
render_params_point_layer_t allLayers
Definition: render_params.h:166
argMapFile
static TCLAP::UnlabeledValueArg< std::string > argMapFile("input", "Load this metric map file (*.mm)", false, "myMap.mm", "myMap.mm", cmd)
arg_tumTrajectory
static TCLAP::ValueArg< std::string > arg_tumTrajectory("t", "trajectory", "Also draw a trajectory, given by a TUM file trajectory.", false, "trajectory.tum", "trajectory.tum", cmd)
APP_NAME
constexpr const char * APP_NAME
Definition: apps/mm-viewer/main.cpp:40
mp2p_icp::render_params_point_layer_t::color
mrpt::img::TColor color
Definition: render_params.h:133
mp2p_icp::MapToPointsMap
const mrpt::maps::CPointsMap * MapToPointsMap(const mrpt::maps::CMetricMap &map)
Definition: metricmap.cpp:648
main
int main(int argc, char **argv)
Definition: apps/mm-viewer/main.cpp:1169
d
d
MID_FONT_SIZE
constexpr int MID_FONT_SIZE
Definition: apps/mm-viewer/main.cpp:41
mp2p_icp::metric_map_t::contents_summary
virtual std::string contents_summary() const
Definition: metricmap.cpp:500
mp2p_icp::render_params_t
Definition: render_params.h:184
mp2p_icp::metric_map_t::get_visualization
virtual auto get_visualization(const render_params_t &p=render_params_t()) const -> std::shared_ptr< mrpt::opengl::CSetOfObjects >
Definition: metricmap.cpp:121
mp2p_icp::metric_map_t::load_from_file
bool load_from_file(const std::string &fileName)
Definition: metricmap.cpp:578
mp2p_icp::render_params_points_t::perLayer
std::map< layer_name_t, render_params_point_layer_t > perLayer
Definition: render_params.h:170
get_user_config_file
static void get_user_config_file(char *out, unsigned int maxlen, const char *appname)
Definition: cfgpath.h:96
metricmap.h
Generic representation of pointcloud(s) and/or extracted features.
mp2p_icp::metric_map_t
Generic container of pointcloud(s), extracted features and other maps.
Definition: metricmap.h:49
mp2p_icp::metric_map_t::georeferencing
std::optional< Georeferencing > georeferencing
Definition: metricmap.h:117
SMALL_FONT_SIZE
constexpr int SMALL_FONT_SIZE
Definition: apps/mm-viewer/main.cpp:42
mp2p_icp::metric_map_t::layers
std::map< layer_name_t, mrpt::maps::CMetricMap::Ptr > layers
Definition: metricmap.h:76
PointMatcherSupport::pow
constexpr T pow(const T base, const std::size_t exponent)
Definition: libpointmatcher/pointmatcher/DataPointsFilters/utils/utils.h:47
t
geometry_msgs::TransformStamped t
mp2p_icp::render_params_points_t::visible
bool visible
Definition: render_params.h:162

mp2p_icp
Author(s):
autogenerated on Thu Dec 26 2024 03:48:12