30 #include "ui_mainWindow.h" 73 #include <QtGui/QCloseEvent> 74 #include <QtGui/QPixmap> 75 #include <QtCore/QDir> 76 #include <QtCore/QFile> 77 #include <QtCore/QTextStream> 78 #include <QtCore/QFileInfo> 79 #include <QMessageBox> 80 #include <QFileDialog> 81 #include <QGraphicsEllipseItem> 82 #include <QDockWidget> 83 #include <QtCore/QBuffer> 84 #include <QtCore/QTimer> 85 #include <QtCore/QTime> 86 #include <QActionGroup> 87 #include <QtGui/QDesktopServices> 88 #include <QtCore/QStringList> 89 #include <QtCore/QProcess> 90 #include <QSplashScreen> 91 #include <QInputDialog> 92 #include <QToolButton> 109 #include <pcl/visualization/cloud_viewer.h> 110 #include <pcl/common/transforms.h> 111 #include <pcl/common/common.h> 112 #include <pcl/io/pcd_io.h> 113 #include <pcl/io/ply_io.h> 114 #include <pcl/filters/filter.h> 115 #include <pcl/search/kdtree.h> 117 #ifdef RTABMAP_OCTOMAP 121 #ifdef HAVE_OPENCV_ARUCO 122 #include <opencv2/aruco.hpp> 125 #define LOG_FILE_NAME "LogRtabmap.txt" 126 #define SHARE_SHOW_LOG_FILE "share/rtabmap/showlogs.m" 127 #define SHARE_GET_PRECISION_RECALL_FILE "share/rtabmap/getPrecisionRecall.m" 128 #define SHARE_IMPORT_FILE "share/rtabmap/importfile.m" 144 _preferencesDialog(0),
146 _exportCloudsDialog(0),
147 _exportBundlerDialog(0),
151 _processingStatistics(
false),
152 _processingDownloadedMap(
false),
154 _odometryReceived(
false),
155 _newDatabasePath(
""),
156 _newDatabasePathOutput(
""),
157 _openedDatabasePath(
""),
158 _databaseUpdated(
false),
159 _odomImageShow(
true),
160 _odomImageDepthShow(
false),
161 _savedMaximized(
false),
163 _cachedMemoryUsage(0),
164 _createdCloudsMemoryUsage(0),
167 _odometryCorrection(
Transform::getIdentity()),
168 _processingOdometry(
false),
173 _rawLikelihoodCurve(0),
174 _exportPosesFrame(0),
175 _autoScreenCaptureOdomSync(
false),
176 _autoScreenCaptureRAM(
false),
177 _autoScreenCapturePNG(
false),
179 _progressCanceled(
false)
186 QSplashScreen * splash = 0;
187 if (showSplashScreen)
189 QPixmap pixmap(
":images/RTAB-Map.png");
190 splash =
new QSplashScreen(pixmap);
192 splash->showMessage(tr(
"Loading..."));
193 QApplication::processEvents();
208 _ui =
new Ui_mainWindow();
224 UDEBUG(
"Setup ui... end");
226 QString title(
"RTAB-Map[*]");
227 this->setWindowTitle(title);
228 this->setWindowIconText(tr(
"RTAB-Map"));
229 this->setObjectName(
"MainWindow");
234 _ui->widget_mainWindow->setVisible(
false);
250 bool statusBarShown =
false;
262 #ifdef RTABMAP_OCTOMAP 270 _ui->label_elapsedTime->setText(
"00:00:00");
276 _ui->imageView_source->setBackgroundColor(
_ui->imageView_source->getDefaultBackgroundColor());
277 _ui->imageView_loopClosure->setBackgroundColor(
_ui->imageView_loopClosure->getDefaultBackgroundColor());
278 _ui->imageView_odometry->setBackgroundColor(
_ui->imageView_odometry->getDefaultBackgroundColor());
279 _ui->imageView_odometry->setAlpha(200);
287 _ui->posteriorPlot->showLegend(
false);
288 _ui->posteriorPlot->setFixedYAxis(0,1);
295 _ui->likelihoodPlot->showLegend(
false);
299 _ui->rawLikelihoodPlot->showLegend(
false);
305 connect(
_ui->widget_mapVisibility, SIGNAL(visibilityChanged(
int,
bool)),
this, SLOT(
updateNodeVisibility(
int,
bool)));
308 connect(
_ui->actionExit, SIGNAL(triggered()),
this, SLOT(close()));
309 qRegisterMetaType<MainWindow::State>(
"MainWindow::State");
312 qRegisterMetaType<rtabmap::RtabmapEvent3DMap>(
"rtabmap::RtabmapEvent3DMap");
314 qRegisterMetaType<rtabmap::RtabmapGlobalPathEvent>(
"rtabmap::RtabmapGlobalPathEvent");
320 _ui->menuShow_view->addAction(
_ui->dockWidget_imageView->toggleViewAction());
321 _ui->menuShow_view->addAction(
_ui->dockWidget_posterior->toggleViewAction());
322 _ui->menuShow_view->addAction(
_ui->dockWidget_likelihood->toggleViewAction());
323 _ui->menuShow_view->addAction(
_ui->dockWidget_rawlikelihood->toggleViewAction());
324 _ui->menuShow_view->addAction(
_ui->dockWidget_statsV2->toggleViewAction());
325 _ui->menuShow_view->addAction(
_ui->dockWidget_console->toggleViewAction());
326 _ui->menuShow_view->addAction(
_ui->dockWidget_cloudViewer->toggleViewAction());
327 _ui->menuShow_view->addAction(
_ui->dockWidget_loopClosureViewer->toggleViewAction());
328 _ui->menuShow_view->addAction(
_ui->dockWidget_mapVisibility->toggleViewAction());
329 _ui->menuShow_view->addAction(
_ui->dockWidget_graphViewer->toggleViewAction());
330 _ui->menuShow_view->addAction(
_ui->dockWidget_odometry->toggleViewAction());
331 _ui->menuShow_view->addAction(
_ui->toolBar->toggleViewAction());
332 _ui->toolBar->setWindowTitle(tr(
"File toolbar"));
333 _ui->menuShow_view->addAction(
_ui->toolBar_2->toggleViewAction());
334 _ui->toolBar_2->setWindowTitle(tr(
"Control toolbar"));
335 QAction * a =
_ui->menuShow_view->addAction(
"Progress dialog");
336 a->setCheckable(
false);
338 QAction * statusBarAction =
_ui->menuShow_view->addAction(
"Status bar");
339 statusBarAction->setCheckable(
true);
340 statusBarAction->setChecked(statusBarShown);
341 connect(statusBarAction, SIGNAL(toggled(
bool)), this->statusBar(), SLOT(setVisible(
bool)));
344 connect(
_ui->actionSave_GUI_config, SIGNAL(triggered()),
this, SLOT(
saveConfigGUI()));
345 connect(
_ui->actionNew_database, SIGNAL(triggered()),
this, SLOT(
newDatabase()));
346 connect(
_ui->actionOpen_database, SIGNAL(triggered()),
this, SLOT(
openDatabase()));
347 connect(
_ui->actionClose_database, SIGNAL(triggered()),
this, SLOT(
closeDatabase()));
348 connect(
_ui->actionEdit_database, SIGNAL(triggered()),
this, SLOT(
editDatabase()));
352 connect(
_ui->actionDump_the_memory, SIGNAL(triggered()),
this, SLOT(
dumpTheMemory()));
353 connect(
_ui->actionDump_the_prediction_matrix, SIGNAL(triggered()),
this, SLOT(
dumpThePrediction()));
354 connect(
_ui->actionSend_goal, SIGNAL(triggered()),
this, SLOT(
sendGoal()));
355 connect(
_ui->actionSend_waypoints, SIGNAL(triggered()),
this, SLOT(
sendWaypoints()));
356 connect(
_ui->actionCancel_goal, SIGNAL(triggered()),
this, SLOT(
cancelGoal()));
357 connect(
_ui->actionLabel_current_location, SIGNAL(triggered()),
this, SLOT(
label()));
358 connect(
_ui->actionClear_cache, SIGNAL(triggered()),
this, SLOT(
clearTheCache()));
359 connect(
_ui->actionAbout, SIGNAL(triggered()),
_aboutDialog , SLOT(exec()));
360 connect(
_ui->actionHelp, SIGNAL(triggered()),
this , SLOT(
openHelp()));
361 connect(
_ui->actionPrint_loop_closure_IDs_to_console, SIGNAL(triggered()),
this, SLOT(
printLoopClosureIds()));
363 connect(
_ui->actionRaw_format_txt, SIGNAL(triggered()),
this , SLOT(
exportPosesRaw()));
366 connect(
_ui->actionKITTI_format_txt, SIGNAL(triggered()),
this , SLOT(
exportPosesKITTI()));
369 connect(
_ui->actionDelete_memory, SIGNAL(triggered()),
this , SLOT(
deleteMemory()));
375 connect(
_ui->actionDefault_views, SIGNAL(triggered(
bool)),
this, SLOT(
setDefaultViews()));
377 connect(
_ui->actionScreenshot, SIGNAL(triggered()),
this, SLOT(
takeScreenshot()));
387 connect(
_ui->actionSave_point_cloud, SIGNAL(triggered()),
this, SLOT(
exportClouds()));
388 connect(
_ui->actionExport_2D_Grid_map_bmp_png, SIGNAL(triggered()),
this, SLOT(
exportGridMap()));
389 connect(
_ui->actionExport_images_RGB_jpg_Depth_png, SIGNAL(triggered()),
this , SLOT(
exportImages()));
390 connect(
_ui->actionExport_cameras_in_Bundle_format_out, SIGNAL(triggered()), SLOT(
exportBundlerFormat()));
391 connect(
_ui->actionExport_octomap, SIGNAL(triggered()),
this, SLOT(
exportOctomap()));
392 connect(
_ui->actionView_high_res_point_cloud, SIGNAL(triggered()),
this, SLOT(
viewClouds()));
393 connect(
_ui->actionReset_Odometry, SIGNAL(triggered()),
this, SLOT(
resetOdometry()));
394 connect(
_ui->actionTrigger_a_new_map, SIGNAL(triggered()),
this, SLOT(
triggerNewMap()));
395 connect(
_ui->actionData_recorder, SIGNAL(triggered()),
this, SLOT(
dataRecorder()));
396 connect(
_ui->actionPost_processing, SIGNAL(triggered()),
this, SLOT(
postProcessing()));
397 connect(
_ui->actionDepth_Calibration, SIGNAL(triggered()),
this, SLOT(
depthCalibration()));
399 _ui->actionPause->setShortcut(Qt::Key_Space);
400 _ui->actionSave_GUI_config->setShortcut(QKeySequence::Save);
403 this->addAction(
_ui->actionSave_GUI_config);
404 _ui->actionReset_Odometry->setEnabled(
false);
405 _ui->actionPost_processing->setEnabled(
false);
406 _ui->actionAnchor_clouds_to_ground_truth->setEnabled(
false);
408 QToolButton* toolButton =
new QToolButton(
this);
409 toolButton->setMenu(
_ui->menuSelect_source);
410 toolButton->setPopupMode(QToolButton::InstantPopup);
411 toolButton->setIcon(QIcon(
":images/kinect_xbox_360.png"));
412 toolButton->setToolTip(
"Select sensor driver");
413 _ui->toolBar->addWidget(toolButton)->setObjectName(
"toolbar_source");
415 #if defined(Q_WS_MAC) || defined(Q_WS_WIN) 418 _ui->menuEdit->removeAction(
_ui->actionOpen_working_directory);
423 connect(
_ui->actionUsbCamera, SIGNAL(triggered()),
this, SLOT(
selectStream()));
424 connect(
_ui->actionOpenNI_PCL, SIGNAL(triggered()),
this, SLOT(
selectOpenni()));
425 connect(
_ui->actionOpenNI_PCL_ASUS, SIGNAL(triggered()),
this, SLOT(
selectOpenni()));
427 connect(
_ui->actionOpenNI_CV, SIGNAL(triggered()),
this, SLOT(
selectOpenniCv()));
429 connect(
_ui->actionOpenNI2, SIGNAL(triggered()),
this, SLOT(
selectOpenni2()));
430 connect(
_ui->actionOpenNI2_kinect, SIGNAL(triggered()),
this, SLOT(
selectOpenni2()));
431 connect(
_ui->actionOpenNI2_sense, SIGNAL(triggered()),
this, SLOT(
selectOpenni2()));
433 connect(
_ui->actionKinect_for_Windows_SDK_v2, SIGNAL(triggered()),
this, SLOT(
selectK4W2()));
434 connect(
_ui->actionKinect_for_Azure, SIGNAL(triggered()),
this, SLOT(
selectK4A()));
435 connect(
_ui->actionRealSense_R200, SIGNAL(triggered()),
this, SLOT(
selectRealSense()));
436 connect(
_ui->actionRealSense_ZR300, SIGNAL(triggered()),
this, SLOT(
selectRealSense()));
447 connect(
_ui->actionMYNT_EYE_S_SDK, SIGNAL(triggered()),
this, SLOT(
selectMyntEyeS()));
473 QActionGroup * modeGrp =
new QActionGroup(
this);
474 modeGrp->addAction(
_ui->actionSLAM_mode);
475 modeGrp->addAction(
_ui->actionLocalization_mode);
483 qRegisterMetaType<PreferencesDialog::PANEL_FLAGS>(
"PreferencesDialog::PANEL_FLAGS");
485 qRegisterMetaType<rtabmap::ParametersMap>(
"rtabmap::ParametersMap");
502 connect(
_ui->toolBar->toggleViewAction(), SIGNAL(toggled(
bool)),
this, SLOT(
configGUIModified()));
503 connect(
_ui->toolBar, SIGNAL(orientationChanged(Qt::Orientation)),
this, SLOT(
configGUIModified()));
504 connect(statusBarAction, SIGNAL(toggled(
bool)),
this, SLOT(
configGUIModified()));
505 QList<QDockWidget*> dockWidgets = this->findChildren<QDockWidget*>();
506 for(
int i=0; i<dockWidgets.size(); ++i)
508 connect(dockWidgets[i], SIGNAL(dockLocationChanged(Qt::DockWidgetArea)),
this, SLOT(
configGUIModified()));
509 connect(dockWidgets[i]->toggleViewAction(), SIGNAL(toggled(
bool)),
this, SLOT(
configGUIModified()));
511 connect(
_ui->dockWidget_graphViewer->toggleViewAction(), SIGNAL(triggered()),
this, SLOT(
updateGraphView()));
513 _ui->dockWidget_posterior->installEventFilter(
this);
514 _ui->dockWidget_likelihood->installEventFilter(
this);
515 _ui->dockWidget_rawlikelihood->installEventFilter(
this);
516 _ui->dockWidget_statsV2->installEventFilter(
this);
517 _ui->dockWidget_console->installEventFilter(
this);
518 _ui->dockWidget_loopClosureViewer->installEventFilter(
this);
519 _ui->dockWidget_mapVisibility->installEventFilter(
this);
520 _ui->dockWidget_graphViewer->installEventFilter(
this);
521 _ui->dockWidget_odometry->installEventFilter(
this);
522 _ui->dockWidget_cloudViewer->installEventFilter(
this);
523 _ui->dockWidget_imageView->installEventFilter(
this);
537 qRegisterMetaType<rtabmap::Statistics>(
"rtabmap::Statistics");
540 qRegisterMetaType<rtabmap::CameraInfo>(
"rtabmap::CameraInfo");
543 qRegisterMetaType<rtabmap::OdometryEvent>(
"rtabmap::OdometryEvent");
552 _ui->statsToolBox->setNewFigureMaxItems(50);
565 if(
_ui->statsToolBox->findChildren<
StatItem*>().size() == 0)
568 for(std::map<std::string, float>::const_iterator iter = statistics.begin(); iter != statistics.end(); ++iter)
571 if(!QString((*iter).first.c_str()).contains(
"Gt/"))
573 _ui->statsToolBox->updateStat(QString((*iter).first.c_str()).replace(
'_',
' '),
false);
578 _ui->statsToolBox->updateStat(
"Planning/From/",
false);
579 _ui->statsToolBox->updateStat(
"Planning/Time/ms",
false);
580 _ui->statsToolBox->updateStat(
"Planning/Goal/",
false);
581 _ui->statsToolBox->updateStat(
"Planning/Poses/",
false);
582 _ui->statsToolBox->updateStat(
"Planning/Length/m",
false);
584 _ui->statsToolBox->updateStat(
"Camera/Time capturing/ms",
false);
585 _ui->statsToolBox->updateStat(
"Camera/Time decimation/ms",
false);
586 _ui->statsToolBox->updateStat(
"Camera/Time disparity/ms",
false);
587 _ui->statsToolBox->updateStat(
"Camera/Time mirroring/ms",
false);
588 _ui->statsToolBox->updateStat(
"Camera/Time scan from depth/ms",
false);
590 _ui->statsToolBox->updateStat(
"Odometry/ID/",
false);
591 _ui->statsToolBox->updateStat(
"Odometry/Features/",
false);
592 _ui->statsToolBox->updateStat(
"Odometry/Matches/",
false);
593 _ui->statsToolBox->updateStat(
"Odometry/MatchesRatio/",
false);
594 _ui->statsToolBox->updateStat(
"Odometry/Inliers/",
false);
595 _ui->statsToolBox->updateStat(
"Odometry/InliersMeanDistance/m",
false);
596 _ui->statsToolBox->updateStat(
"Odometry/InliersDistribution/",
false);
597 _ui->statsToolBox->updateStat(
"Odometry/InliersRatio/",
false);
598 _ui->statsToolBox->updateStat(
"Odometry/ICPInliersRatio/",
false);
599 _ui->statsToolBox->updateStat(
"Odometry/ICPRotation/rad",
false);
600 _ui->statsToolBox->updateStat(
"Odometry/ICPTranslation/m",
false);
601 _ui->statsToolBox->updateStat(
"Odometry/ICPStructuralComplexity/",
false);
602 _ui->statsToolBox->updateStat(
"Odometry/ICPStructuralDistribution/",
false);
603 _ui->statsToolBox->updateStat(
"Odometry/ICPCorrespondences/",
false);
604 _ui->statsToolBox->updateStat(
"Odometry/StdDevLin/",
false);
605 _ui->statsToolBox->updateStat(
"Odometry/StdDevAng/",
false);
606 _ui->statsToolBox->updateStat(
"Odometry/VarianceLin/",
false);
607 _ui->statsToolBox->updateStat(
"Odometry/VarianceAng/",
false);
608 _ui->statsToolBox->updateStat(
"Odometry/TimeEstimation/ms",
false);
609 _ui->statsToolBox->updateStat(
"Odometry/TimeFiltering/ms",
false);
610 _ui->statsToolBox->updateStat(
"Odometry/GravityRollError/deg",
false);
611 _ui->statsToolBox->updateStat(
"Odometry/GravityPitchError/deg",
false);
612 _ui->statsToolBox->updateStat(
"Odometry/LocalMapSize/",
false);
613 _ui->statsToolBox->updateStat(
"Odometry/LocalScanMapSize/",
false);
614 _ui->statsToolBox->updateStat(
"Odometry/LocalKeyFrames/",
false);
615 _ui->statsToolBox->updateStat(
"Odometry/localBundleOutliers/",
false);
616 _ui->statsToolBox->updateStat(
"Odometry/localBundleConstraints/",
false);
617 _ui->statsToolBox->updateStat(
"Odometry/localBundleTime/ms",
false);
618 _ui->statsToolBox->updateStat(
"Odometry/KeyFrameAdded/",
false);
619 _ui->statsToolBox->updateStat(
"Odometry/Interval/ms",
false);
620 _ui->statsToolBox->updateStat(
"Odometry/Speed/kph",
false);
621 _ui->statsToolBox->updateStat(
"Odometry/Speed/mph",
false);
622 _ui->statsToolBox->updateStat(
"Odometry/Speed/mps",
false);
623 _ui->statsToolBox->updateStat(
"Odometry/SpeedGuess/kph",
false);
624 _ui->statsToolBox->updateStat(
"Odometry/SpeedGuess/mph",
false);
625 _ui->statsToolBox->updateStat(
"Odometry/SpeedGuess/mps",
false);
626 _ui->statsToolBox->updateStat(
"Odometry/Distance/m",
false);
627 _ui->statsToolBox->updateStat(
"Odometry/T/m",
false);
628 _ui->statsToolBox->updateStat(
"Odometry/Tx/m",
false);
629 _ui->statsToolBox->updateStat(
"Odometry/Ty/m",
false);
630 _ui->statsToolBox->updateStat(
"Odometry/Tz/m",
false);
631 _ui->statsToolBox->updateStat(
"Odometry/Troll/deg",
false);
632 _ui->statsToolBox->updateStat(
"Odometry/Tpitch/deg",
false);
633 _ui->statsToolBox->updateStat(
"Odometry/Tyaw/deg",
false);
634 _ui->statsToolBox->updateStat(
"Odometry/Px/m",
false);
635 _ui->statsToolBox->updateStat(
"Odometry/Py/m",
false);
636 _ui->statsToolBox->updateStat(
"Odometry/Pz/m",
false);
637 _ui->statsToolBox->updateStat(
"Odometry/Proll/deg",
false);
638 _ui->statsToolBox->updateStat(
"Odometry/Ppitch/deg",
false);
639 _ui->statsToolBox->updateStat(
"Odometry/Pyaw/deg",
false);
641 _ui->statsToolBox->updateStat(
"GUI/Refresh odom/ms",
false);
642 _ui->statsToolBox->updateStat(
"GUI/RGB-D cloud/ms",
false);
643 _ui->statsToolBox->updateStat(
"GUI/Graph Update/ms",
false);
644 #ifdef RTABMAP_OCTOMAP 645 _ui->statsToolBox->updateStat(
"GUI/Octomap Update/ms",
false);
646 _ui->statsToolBox->updateStat(
"GUI/Octomap Rendering/ms",
false);
648 _ui->statsToolBox->updateStat(
"GUI/Grid Update/ms",
false);
649 _ui->statsToolBox->updateStat(
"GUI/Grid Rendering/ms",
false);
650 _ui->statsToolBox->updateStat(
"GUI/Refresh stats/ms",
false);
651 _ui->statsToolBox->updateStat(
"GUI/Cache Data Size/MB",
false);
652 _ui->statsToolBox->updateStat(
"GUI/Cache Clouds Size/MB",
false);
653 #ifdef RTABMAP_OCTOMAP 654 _ui->statsToolBox->updateStat(
"GUI/Octomap Size/MB",
false);
681 #ifdef RTABMAP_OCTOMAP 692 qobject_cast<QHBoxLayout *>(
_ui->layout_imageview->layout())->setDirection(QBoxLayout::TopToBottom);
696 qobject_cast<QHBoxLayout *>(
_ui->layout_imageview->layout())->setDirection(QBoxLayout::LeftToRight);
735 bool processStopped =
true;
748 processStopped =
false;
756 if(this->isWindowModified())
758 QMessageBox::Button b=QMessageBox::question(
this,
760 tr(
"There are unsaved changed settings. Save them?"),
761 QMessageBox::Save | QMessageBox::Cancel | QMessageBox::Discard);
762 if(b == QMessageBox::Save)
766 else if(b != QMessageBox::Discard)
778 _ui->statsToolBox->closeFigures();
780 _ui->dockWidget_imageView->close();
781 _ui->dockWidget_likelihood->close();
782 _ui->dockWidget_rawlikelihood->close();
783 _ui->dockWidget_posterior->close();
784 _ui->dockWidget_statsV2->close();
785 _ui->dockWidget_console->close();
786 _ui->dockWidget_cloudViewer->close();
787 _ui->dockWidget_loopClosureViewer->close();
788 _ui->dockWidget_mapVisibility->close();
789 _ui->dockWidget_graphViewer->close();
790 _ui->dockWidget_odometry->close();
794 UERROR(
"Camera must be already deleted here!");
805 UERROR(
"OdomThread must be already deleted here!");
825 else if(anEvent->
getClassName().compare(
"RtabmapEvent") == 0)
829 int highestHypothesisId = int(
uValue(stats.
data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f));
830 int proximityClosureId = int(
uValue(stats.
data(), Statistics::kProximitySpace_last_detection_id(), 0.0f));
831 bool rejectedHyp = bool(
uValue(stats.
data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
832 float highestHypothesisValue =
uValue(stats.
data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
834 _ui->actionPause_on_match->isChecked())
837 highestHypothesisId > 0 &&
839 _ui->actionPause_when_a_loop_hypothesis_is_rejected->isChecked() &&
842 (proximityClosureId > 0 &&
843 _ui->actionPause_on_local_loop_detection->isChecked()))
849 QMetaObject::invokeMethod(
this,
"beep");
861 else if(anEvent->
getClassName().compare(
"RtabmapEventInit") == 0)
869 else if(anEvent->
getClassName().compare(
"RtabmapEvent3DMap") == 0)
874 else if(anEvent->
getClassName().compare(
"RtabmapGlobalPathEvent") == 0)
879 else if(anEvent->
getClassName().compare(
"RtabmapLabelErrorEvent") == 0)
884 else if(anEvent->
getClassName().compare(
"RtabmapGoalStatusEvent") == 0)
888 else if(anEvent->
getClassName().compare(
"CameraEvent") == 0)
895 QMetaObject::invokeMethod(
this,
"beep");
919 else if(anEvent->
getClassName().compare(
"OdometryEvent") == 0)
938 else if(anEvent->
getClassName().compare(
"ULogEvent") == 0)
943 QMetaObject::invokeMethod(
_ui->dockWidget_console,
"show");
950 QMetaObject::invokeMethod(
this,
"beep");
991 if(
_ui->imageView_odometry->toolTip().isEmpty())
993 _ui->imageView_odometry->setToolTip(
994 "Background Color Code:\n" 995 " Dark Red = Odometry Lost\n" 996 " Dark Yellow = Low Inliers\n" 997 "Feature Color code:\n" 999 " Yellow = Not matched features from previous frame(s)\n" 1005 bool lostStateChanged =
false;
1012 _ui->imageView_odometry->setBackgroundColor(Qt::darkRed);
1024 _ui->imageView_odometry->setBackgroundColor(Qt::darkYellow);
1031 _ui->imageView_odometry->setBackgroundColor(
_ui->imageView_odometry->getDefaultBackgroundColor());
1034 if(!pose.
isNull() && (
_ui->dockWidget_cloudViewer->isVisible() ||
_ui->graphicsView_graphView->isVisible()))
1039 if(
_ui->dockWidget_cloudViewer->isVisible())
1041 bool cloudUpdated =
false;
1042 bool scanUpdated =
false;
1043 bool featuresUpdated =
false;
1044 bool filteredGravityUpdated =
false;
1045 bool accelerationUpdated =
false;
1055 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
1056 pcl::IndicesPtr indices(
new std::vector<int>);
1071 pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(
new pcl::PointCloud<pcl::PointXYZRGB>);
1075 Eigen::Vector3f viewpoint(0.0
f,0.0
f,0.0
f);
1093 Eigen::Vector3f(pose.
x(), pose.
y(), pose.
z()) + viewpoint);
1098 pcl::TextureMesh::Ptr textureMesh(
new pcl::TextureMesh);
1099 pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
1100 textureMesh->tex_polygons.push_back(polygons);
1101 int w = cloud->width;
1102 int h = cloud->height;
1104 textureMesh->tex_coordinates.resize(1);
1105 int nPoints = (int)(textureMesh->cloud.data.size()/textureMesh->cloud.point_step);
1106 textureMesh->tex_coordinates[0].resize(nPoints);
1107 for(
int i=0; i<nPoints; ++i)
1110 textureMesh->tex_coordinates[0][i] = Eigen::Vector2f(
1111 float(i % w) /
float(w),
1112 float(h - i / w) /
float(h));
1115 pcl::TexMaterial mesh_material;
1116 mesh_material.tex_d = 1.0f;
1117 mesh_material.tex_Ns = 75.0f;
1118 mesh_material.tex_illum = 1;
1120 mesh_material.tex_name =
"material_odom";
1121 mesh_material.tex_file =
"";
1122 textureMesh->tex_materials.push_back(mesh_material);
1126 UERROR(
"Adding cloudOdom to viewer failed!");
1131 UERROR(
"Adding cloudOdom to viewer failed!");
1139 UERROR(
"Adding cloudOdom to viewer failed!");
1147 cloudUpdated =
true;
1159 bool scanAdded =
false;
1188 UERROR(
"Adding scanMapOdom to viewer failed!");
1215 bool scanAdded =
false;
1219 pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud;
1229 pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
1239 pcl::PointCloud<pcl::PointXYZI>::Ptr cloud;
1249 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
1260 UERROR(
"Adding scanOdom to viewer failed!");
1278 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZRGB>);
1281 for(std::map<int, cv::Point3f>::const_iterator iter=odom.
info().
localMap.begin(); iter!=odom.
info().
localMap.end(); ++iter)
1286 (*cloud)[i].x = iter->second.x;
1287 (*cloud)[i].y = iter->second.y;
1288 (*cloud)[i].z = iter->second.z;
1292 (*cloud)[i].r = inlier?0:255;
1293 (*cloud)[i].g = 255;
1307 featuresUpdated =
true;
1313 for(QMap<std::string, Transform>::iterator iter = addedFrustums.begin(); iter!=addedFrustums.end(); ++iter)
1315 std::list<std::string> splitted =
uSplitNumChar(iter.key());
1316 if(splitted.size() == 2)
1318 int id = std::atoi(splitted.back().c_str());
1319 if(splitted.front().compare(
"f_odom_") == 0 &&
1329 std::string frustumId =
uFormat(
"f_odom_%d", iter->first);
1340 QColor color = Qt::yellow;
1355 _cloudViewer->
addOrUpdateLine(
"odom_imu_orientation",
_odometryCorrection*pose, (
_odometryCorrection*pose).translation()*
Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0)*pose.
rotation().
inverse(), Qt::yellow,
true,
true);
1356 filteredGravityUpdated =
true;
1363 Eigen::Vector3f gravity(
1369 _cloudViewer->
addOrUpdateLine(
"odom_imu_acc",
_odometryCorrection*pose,
_odometryCorrection*pose*
Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0), Qt::red,
true,
true);
1370 accelerationUpdated =
true;
1415 #if PCL_VERSION_COMPARE(>=, 1, 7, 2) 1429 if(
_ui->graphicsView_graphView->isVisible())
1434 _ui->graphicsView_graphView->update();
1438 if(
_ui->dockWidget_odometry->isVisible() &&
1441 if(
_ui->imageView_odometry->isFeaturesShown())
1447 std::multimap<int, cv::KeyPoint> kpInliers;
1452 _ui->imageView_odometry->setFeatures(
1459 _ui->imageView_odometry->setFeatures(
1471 std::vector<cv::KeyPoint> kpts;
1473 _ui->imageView_odometry->setFeatures(
1481 bool monoInitialization =
false;
1484 monoInitialization =
true;
1487 _ui->imageView_odometry->clearLines();
1488 if(lost && !monoInitialization)
1490 if(lostStateChanged)
1497 _ui->imageView_odometry->setImageShown(
true);
1498 _ui->imageView_odometry->setImageDepthShown(
true);
1502 if(lostStateChanged)
1536 if(
_ui->imageView_odometry->isFeaturesShown() ||
_ui->imageView_odometry->isLinesShown())
1543 if(
_ui->imageView_odometry->isFeaturesShown() && inliers.find(i) != inliers.end())
1545 _ui->imageView_odometry->setFeatureColor(i, Qt::green);
1547 if(
_ui->imageView_odometry->isLinesShown())
1549 _ui->imageView_odometry->addLine(
1554 inliers.find(i) != inliers.end()?Qt::blue:Qt::yellow);
1565 _ui->imageView_odometry->update();
1722 QTime time, totalTime;
1732 int refMapId = -1, loopMapId = -1;
1737 int highestHypothesisId =
static_cast<float>(
uValue(stat.
data(), Statistics::kLoopHighest_hypothesis_id(), 0.0f));
1744 _ui->label_refId->setText(QString(
"New ID = %1 [%2]").arg(stat.
refImageId()).arg(refMapId));
1748 float totalTime =
static_cast<float>(
uValue(stat.
data(), Statistics::kTimingTotal(), 0.0f));
1755 bool highestHypothesisIsSaved = (bool)
uValue(stat.
data(), Statistics::kLoopHypothesis_reactivated(), 0.0f);
1757 bool smallMovement = (bool)
uValue(stat.
data(), Statistics::kMemorySmall_movement(), 0.0f);
1759 bool fastMovement = (bool)
uValue(stat.
data(), Statistics::kMemoryFast_movement(), 0.0f);
1770 _ui->imageView_source->isVisible() ||
1779 cv::Mat tmpRgb, tmpDepth, tmpG, tmpO, tmpE;
1782 uncompressImages?&tmpRgb:0,
1783 uncompressImages?&tmpDepth:0,
1784 uncompressScan?&tmpScan:0,
1785 0, &tmpG, &tmpO, &tmpE);
1790 if(smallMovement || fastMovement)
1798 unsigned int count = 0;
1801 for(std::multimap<int, int>::const_iterator jter=signature.
getWords().upper_bound(-1); jter!=signature.
getWords().end(); ++jter)
1818 _ui->imageView_source->clear();
1819 _ui->imageView_loopClosure->clear();
1821 _ui->imageView_source->setBackgroundColor(
_ui->imageView_source->getDefaultBackgroundColor());
1822 _ui->imageView_loopClosure->setBackgroundColor(
_ui->imageView_loopClosure->getDefaultBackgroundColor());
1824 _ui->label_matchId->clear();
1827 int rehearsalMerged = (int)
uValue(stat.
data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
1828 bool rehearsedSimilarity = (float)
uValue(stat.
data(), Statistics::kMemoryRehearsal_id(), 0.0f) != 0.0
f;
1829 int proximityTimeDetections = (int)
uValue(stat.
data(), Statistics::kProximityTime_detections(), 0.0f);
1830 bool scanMatchingSuccess = (bool)
uValue(stat.
data(), Statistics::kNeighborLinkRefiningAccepted(), 0.0f);
1831 _ui->label_stats_imageNumber->setText(QString(
"%1 [%2]").arg(stat.
refImageId()).arg(refMapId));
1833 if(rehearsalMerged > 0)
1835 _ui->imageView_source->setBackgroundColor(Qt::blue);
1837 else if(proximityTimeDetections > 0)
1839 _ui->imageView_source->setBackgroundColor(Qt::darkYellow);
1841 else if(scanMatchingSuccess)
1843 _ui->imageView_source->setBackgroundColor(Qt::darkCyan);
1845 else if(rehearsedSimilarity)
1847 _ui->imageView_source->setBackgroundColor(Qt::darkBlue);
1849 else if(smallMovement)
1851 _ui->imageView_source->setBackgroundColor(Qt::gray);
1853 else if(fastMovement)
1855 _ui->imageView_source->setBackgroundColor(Qt::magenta);
1858 if(
_ui->label_refId->toolTip().isEmpty())
1860 _ui->label_refId->setToolTip(
1861 "Background Color Code:\n" 1862 " Blue = Weight Update Merged\n" 1863 " Dark Blue = Weight Update\n" 1864 " Dark Yellow = Proximity Detection in Time\n" 1865 " Dark Cyan = Neighbor Link Refined\n" 1866 " Gray = Small Movement\n" 1867 " Magenta = Fast Movement\n" 1868 "Feature Color code:\n" 1870 " Yellow = New but Not Unique\n" 1871 " Red = In Vocabulary\n" 1872 " Blue = In Vocabulary and in Previous Signature\n" 1873 " Pink = In Vocabulary and in Loop Closure Signature\n" 1874 " Gray = Not Quantized to Vocabulary");
1877 if(
_ui->label_matchId->toolTip().isEmpty())
1879 _ui->label_matchId->setToolTip(
1880 "Background Color Code:\n" 1881 " Green = Accepted Loop Closure Detection\n" 1882 " Red = Rejected Loop Closure Detection\n" 1883 " Yellow = Proximity Detection in Space\n" 1884 "Feature Color code:\n" 1885 " Red = In Vocabulary\n" 1886 " Pink = In Vocabulary and in Loop Closure Signature\n" 1887 " Gray = Not Quantized to Vocabulary");
1890 UDEBUG(
"time= %d ms", time.restart());
1892 int rejectedHyp = bool(
uValue(stat.
data(), Statistics::kLoopRejectedHypothesis(), 0.0f));
1893 float highestHypothesisValue =
uValue(stat.
data(), Statistics::kLoopHighest_hypothesis_value(), 0.0f);
1894 int landmarkId =
static_cast<int>(
uValue(stat.
data(), Statistics::kLoopLandmark_detected(), 0.0f));
1895 int landmarkNodeRef =
static_cast<int>(
uValue(stat.
data(), Statistics::kLoopLandmark_detected_node_ref(), 0.0f));
1898 int shownLoopId = 0;
1904 _ui->imageView_loopClosure->setBackgroundColor(Qt::green);
1905 _ui->label_stats_loopClosuresDetected->setText(QString::number(
_ui->label_stats_loopClosuresDetected->text().toInt() + 1));
1906 if(highestHypothesisIsSaved)
1908 _ui->label_stats_loopClosuresReactivatedDetected->setText(QString::number(
_ui->label_stats_loopClosuresReactivatedDetected->text().toInt() + 1));
1910 _ui->label_matchId->setText(QString(
"Match ID = %1 [%2]").arg(stat.
loopClosureId()).arg(loopMapId));
1915 _ui->imageView_loopClosure->setBackgroundColor(Qt::yellow);
1916 _ui->label_matchId->setText(QString(
"Local match = %1 [%2]").arg(stat.
proximityDetectionId()).arg(loopMapId));
1919 else if(landmarkId!=0)
1921 _ui->imageView_loopClosure->setBackgroundColor(QColor(
"orange"));
1922 _ui->label_matchId->setText(QString(
"Landmark match = %1 with %2").arg(landmarkId).arg(landmarkNodeRef));
1923 matchId = landmarkNodeRef;
1930 _ui->imageView_loopClosure->setBackgroundColor(Qt::red);
1931 _ui->label_stats_loopClosuresRejected->setText(QString::number(
_ui->label_stats_loopClosuresRejected->text().toInt() + 1));
1932 _ui->label_matchId->setText(QString(
"Loop hypothesis %1 rejected!").arg(highestHypothesisId));
1940 _ui->label_matchId->setText(QString(
"Highest hypothesis (%1)").arg(highestHypothesisId));
1946 shownLoopId = matchId>0?matchId:highestHypothesisId;
1951 loopSignature = iter.value();
1952 bool uncompressImages =
_ui->imageView_source->isVisible() ||
1955 if(uncompressImages || uncompressScan)
1957 cv::Mat tmpRGB, tmpDepth;
1960 uncompressImages?&tmpRGB:0,
1961 uncompressImages?&tmpDepth:0,
1962 uncompressScan?&tmpScan:0);
1985 refImage = refImage.clone();
1990 loopImage = loopImage.clone();
1997 qimageThread.
start();
1998 qimageLoopThread.
start();
1999 qimageThread.
join();
2000 qimageLoopThread.
join();
2002 QImage lcImg = qimageLoopThread.
getQImage();
2003 UDEBUG(
"time= %d ms", time.restart());
2007 _ui->imageView_source->setImage(img);
2028 if(sceneRect.isValid())
2030 _ui->imageView_source->setSceneRect(sceneRect);
2035 _ui->imageView_loopClosure->setImage(lcImg);
2041 if(
_ui->imageView_loopClosure->sceneRect().isNull())
2043 _ui->imageView_loopClosure->setSceneRect(
_ui->imageView_source->sceneRect());
2047 UDEBUG(
"time= %d ms", time.restart());
2050 std::multimap<int, cv::KeyPoint> wordsA;
2051 std::multimap<int, cv::KeyPoint> wordsB;
2052 for(std::map<int, int>::const_iterator iter=signature.
getWords().begin(); iter!=signature.
getWords().end(); ++iter)
2054 wordsA.insert(wordsA.end(), std::make_pair(iter->first, signature.
getWordsKpts()[iter->second]));
2056 for(std::map<int, int>::const_iterator iter=loopSignature.
getWords().begin(); iter!=loopSignature.
getWords().end(); ++iter)
2058 wordsB.insert(wordsB.end(), std::make_pair(iter->first, loopSignature.
getWordsKpts()[iter->second]));
2062 UDEBUG(
"time= %d ms", time.restart());
2071 signature.
setPose(loopClosureTransform);
2073 if(
_ui->dockWidget_loopClosureViewer->isVisible())
2077 UINFO(
"Updating loop closure cloud view time=%fs", loopTimer.
elapsed());
2081 UDEBUG(
"time= %d ms", time.restart());
2086 if(!stat.
posterior().empty() &&
_ui->dockWidget_posterior->isVisible())
2095 if(!stat.
likelihood().empty() &&
_ui->dockWidget_likelihood->isVisible())
2099 if(!stat.
rawLikelihood().empty() &&
_ui->dockWidget_rawlikelihood->isVisible())
2105 const std::map<std::string, float> & statistics = stat.
data();
2106 std::string odomStr =
"Odometry/";
2107 for(std::map<std::string, float>::const_iterator iter = statistics.begin(); iter != statistics.end(); ++iter)
2110 if((*iter).first.size()<odomStr.size() || (*iter).first.substr(0, odomStr.size()).compare(odomStr)!=0)
2116 UDEBUG(
"time= %d ms", time.restart());
2122 if(stat.
poses().size())
2135 for(std::map<int, std::string>::const_iterator iter=stat.
labels().begin(); iter!=stat.
labels().end(); ++iter)
2137 uInsert(labels, std::pair<int, std::string>(*iter));
2141 _ui->graphicsView_graphView->getWorldMapRotation()==0.0f &&
2147 _ui->graphicsView_graphView->setWorldMapRotation(gpsRotationOffset);
2150 _ui->graphicsView_graphView->getWorldMapRotation() != 0.0f)
2152 _ui->graphicsView_graphView->setWorldMapRotation(0.0
f);
2156 std::map<int, Transform> poses = stat.
poses();
2158 UDEBUG(
"time= %d ms", time.restart());
2160 #if PCL_VERSION_COMPARE(>=, 1, 7, 2) 2191 if(
_ui->graphicsView_graphView->isVisible())
2193 _ui->graphicsView_graphView->updateReferentialPosition(poses.rbegin()->second);
2199 if(poses.find(stat.
refImageId())!=poses.end())
2201 poses.insert(std::make_pair(0, poses.at(stat.
refImageId())));
2204 if(groundTruth.find(stat.
refImageId())!=groundTruth.end())
2206 groundTruth.insert(std::make_pair(0, groundTruth.at(stat.
refImageId())));
2211 std::map<std::string, float> updateCloudSats;
2223 UDEBUG(
"time= %d ms", time.restart());
2225 for(std::map<std::string, float>::iterator iter=updateCloudSats.begin(); iter!=updateCloudSats.end(); ++iter)
2232 if(
_ui->graphicsView_graphView->isVisible())
2238 _ui->graphicsView_graphView->updatePosterior(stat.
posterior());
2254 _ui->graphicsView_graphView->updateLocalPath(stat.
localPath());
2258 _ui->graphicsView_graphView->setGlobalPath(std::vector<std::pair<int, Transform> >());
2284 _ui->label_stats_loopClosuresDetected->setText(QString::number(
_ui->label_stats_loopClosuresDetected->text().toInt() + 1));
2285 _ui->label_matchId->setText(QString(
"Match ID = %1 [%2]").arg(stat.
loopClosureId()).arg(loopMapId));
2289 _ui->label_matchId->clear();
2291 float elapsedTime =
static_cast<float>(totalTime.elapsed());
2292 UINFO(
"Updating GUI time = %fs", elapsedTime/1000.0
f);
2307 #ifdef RTABMAP_OCTOMAP 2323 const std::map<int, Transform> & posesIn,
2324 const std::multimap<int, Link> & constraints,
2325 const std::map<int, int> & mapIdsIn,
2326 const std::map<int, std::string> & labels,
2327 const std::map<int, Transform> & groundTruths,
2328 bool verboseProgress,
2329 std::map<std::string, float> * stats)
2332 std::map<int, Transform> nodePoses(posesIn.lower_bound(0), posesIn.end());
2333 UDEBUG(
"nodes=%d landmarks=%d constraints=%d mapIdsIn=%d labelsIn=%d",
2334 (
int)nodePoses.size(), (int)(posesIn.size() - nodePoses.size()), (
int)constraints.size(), (int)mapIdsIn.size(), (int)labels.size());
2353 std::map<int, Transform> poses;
2354 std::map<int, int> mapIds;
2359 bool hasZero = nodePoses.find(0) != nodePoses.end();
2362 std::map<int, Transform> posesInTmp = nodePoses;
2363 posesInTmp.erase(0);
2370 for(std::map<int, Transform>::iterator iter= poses.begin(); iter!=poses.end(); ++iter)
2372 std::map<int, int>::const_iterator jter = mapIdsIn.find(iter->first);
2373 if(jter!=mapIdsIn.end())
2375 mapIds.insert(*jter);
2381 poses.insert(*nodePoses.find(0));
2386 _progressDialog->
appendText(tr(
"Map update: %1 nodes shown of %2 (cloud filtering is on)").arg(poses.size()).arg(nodePoses.size()));
2387 QApplication::processEvents();
2396 std::map<int, bool> posesMask;
2397 for(std::map<int, Transform>::const_iterator iter = nodePoses.begin(); iter!=nodePoses.end(); ++iter)
2399 posesMask.insert(posesMask.end(), std::make_pair(iter->first, poses.find(iter->first) != poses.end()));
2401 _ui->widget_mapVisibility->setMap(nodePoses, posesMask);
2403 if(groundTruths.size() &&
_ui->actionAnchor_clouds_to_ground_truth->isChecked())
2405 for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
2407 std::map<int, Transform>::const_iterator gtIter = groundTruths.find(iter->first);
2408 if(gtIter!=groundTruths.end())
2410 iter->second = gtIter->second;
2414 UWARN(
"Not found ground truth pose for node %d", iter->first);
2420 _ui->actionAnchor_clouds_to_ground_truth->setChecked(
false);
2424 if(maxNodes > 0 && poses.size()>1)
2427 std::map<int, Transform> nearestPoses;
2428 nearestPoses.insert(*poses.rbegin());
2429 for(std::map<int, float>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
2431 std::map<int, Transform>::iterator pter = poses.find(iter->first);
2432 if(pter != poses.end())
2434 nearestPoses.insert(*pter);
2438 if(poses.find(0) != poses.end())
2440 nearestPoses.insert(*poses.find(0));
2446 UDEBUG(
"Update map with %d locations", poses.size());
2450 for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
2452 if(!iter->second.isNull())
2454 std::string cloudName =
uFormat(
"cloud%d", iter->first);
2456 if(iter->first == 0)
2458 viewerClouds.remove(cloudName);
2467 if(viewerClouds.contains(cloudName))
2472 if(tCloud.
isNull() || iter->second != tCloud)
2476 UERROR(
"Updating pose cloud %d failed!", iter->first);
2488 std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> createdCloud = this->
createAndAddCloudToMap(iter->first, iter->second,
uValue(mapIds, iter->first, -1));
2495 else if(viewerClouds.contains(cloudName))
2501 std::string scanName =
uFormat(
"scan%d", iter->first);
2502 if(iter->first == 0)
2504 viewerClouds.remove(scanName);
2509 if(viewerClouds.contains(scanName))
2514 if(tScan.
isNull() || iter->second != tScan)
2518 UERROR(
"Updating pose scan %d failed!", iter->first);
2529 if(!jter->sensorData().laserScanCompressed().isEmpty() || !jter->sensorData().laserScanRaw().isEmpty())
2535 else if(viewerClouds.contains(scanName))
2541 bool updateGridMap =
2542 ((
_ui->graphicsView_graphView->isVisible() &&
_ui->graphicsView_graphView->isGridMapVisible()) ||
2545 bool updateOctomap =
false;
2546 #ifdef RTABMAP_OCTOMAP 2552 if(updateGridMap || updateOctomap)
2561 jter->sensorData().uncompressDataConst(0, 0, 0, 0, &ground, &obstacles, &empty);
2565 #ifdef RTABMAP_OCTOMAP 2568 if((ground.empty() || ground.channels() > 2) &&
2569 (obstacles.empty() || obstacles.channels() > 2))
2571 cv::Point3f viewpoint = jter->sensorData().gridViewPoint();
2574 else if(!ground.empty() || !obstacles.empty())
2576 UWARN(
"Node %d: Cannot update octomap with 2D occupancy grids.", iter->first);
2584 std::string featuresName =
uFormat(
"features%d", iter->first);
2585 if(iter->first == 0)
2587 viewerClouds.remove(featuresName);
2592 if(viewerClouds.contains(featuresName))
2597 if(tFeatures.
isNull() || iter->second != tFeatures)
2601 UERROR(
"Updating pose features %d failed!", iter->first);
2610 if(!jter->getWords3().empty())
2616 else if(viewerClouds.contains(featuresName))
2622 std::string gravityName =
uFormat(
"gravity%d", iter->first);
2623 if(iter->first == 0)
2625 viewerLines.erase(gravityName);
2631 if(linkIter != constraints.end())
2633 Transform gravityT = linkIter->second.transform();
2635 gravity = (gravityT.
rotation()*(iter->second).
rotation().inverse()).toEigen3f()*gravity;
2636 _cloudViewer->
addOrUpdateLine(gravityName, iter->second, (iter->second).translation()*
Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0)*iter->second.
rotation().
inverse(), Qt::yellow,
false,
false);
2639 else if(viewerLines.find(gravityName)!=viewerLines.end())
2648 if(poses.size() < 200 || i % 100 == 0)
2650 QApplication::processEvents();
2663 for(QMap<std::string, Transform>::iterator iter = viewerClouds.begin(); iter!=viewerClouds.end(); ++iter)
2665 std::list<std::string> splitted =
uSplitNumChar(iter.key());
2667 if(splitted.size() == 2)
2669 id = std::atoi(splitted.back().c_str());
2670 if(poses.find(
id) == poses.end())
2674 UDEBUG(
"Hide %s", iter.key().c_str());
2681 for(std::set<std::string>::iterator iter = viewerLines.begin(); iter!=viewerLines.end(); ++iter)
2685 if(splitted.size() == 2)
2687 id = std::atoi(splitted.back().c_str());
2688 if(poses.find(
id) == poses.end())
2690 UDEBUG(
"Remove %s", iter->c_str());
2699 stats->insert(std::make_pair(
"GUI/RGB-D cloud/ms", (
float)timer.
restart()*1000.0f));
2708 for(QMap<std::string, Transform>::iterator iter = addedFrustums.begin(); iter!=addedFrustums.end(); ++iter)
2710 std::list<std::string> splitted =
uSplitNumChar(iter.key());
2711 if(splitted.size() == 2)
2713 if((splitted.front().compare(
"f_") == 0 || splitted.front().compare(
"f_gt_") == 0))
2731 std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr > graphs;
2739 std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator kter = graphs.find(mapId);
2740 if(kter == graphs.end())
2742 kter = graphs.insert(std::make_pair(mapId, pcl::PointCloud<pcl::PointXYZ>::Ptr(
new pcl::PointCloud<pcl::PointXYZ>))).first;
2744 pcl::PointXYZ pt(iter->second.x(), iter->second.y(), iter->second.z());
2745 kter->second->push_back(pt);
2751 std::string frustumId =
uFormat(
"f_%d", iter->first);
2766 QColor color = (Qt::GlobalColor)((mapId+3) % 12 + 7 );
2771 std::string gtFrustumId =
uFormat(
"f_gt_%d", iter->first);
2789 std::map<int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator kter = graphs.find(mapId);
2790 if(kter == graphs.end())
2792 kter = graphs.insert(std::make_pair(mapId, pcl::PointCloud<pcl::PointXYZ>::Ptr(
new pcl::PointCloud<pcl::PointXYZ>))).first;
2795 pcl::PointXYZ pt(t.
x(), t.
y(), t.
z());
2796 kter->second->push_back(pt);
2801 for(std::map<
int, pcl::PointCloud<pcl::PointXYZ>::Ptr >::iterator iter=graphs.begin(); iter!=graphs.end(); ++iter)
2803 QColor color = Qt::gray;
2804 if(iter->first >= 0)
2806 color = (Qt::GlobalColor)((iter->first+3) % 12 + 7 );
2814 UDEBUG(
"remove not used frustums");
2815 for(QMap<std::string, Transform>::iterator iter = addedFrustums.begin(); iter!=addedFrustums.end(); ++iter)
2817 std::list<std::string> splitted =
uSplitNumChar(iter.key());
2818 if(splitted.size() == 2)
2820 int id = std::atoi(splitted.back().c_str());
2821 if((splitted.front().compare(
"f_") == 0 || splitted.front().compare(
"f_gt_") == 0) &&
2833 UDEBUG(
"labels.size()=%d", (
int)labels.size());
2839 for(std::map<int, std::string>::const_iterator iter=labels.begin(); iter!=labels.end(); ++iter)
2841 if(nodePoses.find(iter->first)!=nodePoses.end())
2843 int mapId =
uValue(mapIdsIn, iter->first, -1);
2844 QColor color = Qt::gray;
2847 color = (Qt::GlobalColor)((mapId+3) % 12 + 7 );
2862 stats->insert(std::make_pair(
"GUI/Graph Update/ms", (
float)timer.
restart()*1000.0f));
2865 #ifdef RTABMAP_OCTOMAP 2873 UINFO(
"Octomap update time = %fs", time.
ticks());
2877 stats->insert(std::make_pair(
"GUI/Octomap Update/ms", (
float)timer.
restart()*1000.0f));
2889 pcl::IndicesPtr obstacles(
new std::vector<int>);
2891 if(obstacles->size())
2897 UINFO(
"Octomap show 3d map time = %fs", time.
ticks());
2902 stats->insert(std::make_pair(
"GUI/Octomap Rendering/ms", (
float)timer.
restart()*1000.0f));
2907 #if PCL_VERSION_COMPARE(>=, 1, 7, 2) 2912 for(std::map<int, Transform>::const_iterator iter=posesIn.begin(); iter!=posesIn.end() && iter->first<0; ++iter)
2914 #if PCL_VERSION_COMPARE(>=, 1, 7, 2) 2921 std::string(
"landmark_str_") + num,
2931 if(
_ui->graphicsView_graphView->isVisible())
2933 _ui->graphicsView_graphView->updateGraph(posesIn, constraints, mapIdsIn);
2937 for(std::map<int, Transform>::iterator iter=gtPoses.begin(); iter!=gtPoses.end(); ++iter)
2939 iter->second = mapToGt * iter->second;
2941 _ui->graphicsView_graphView->updateGTGraph(gtPoses);
2954 #ifdef RTABMAP_OCTOMAP 2969 stats->insert(std::make_pair(
"GUI/Grid Update/ms", (
float)timer.
restart()*1000.0f));
2983 if(
_ui->graphicsView_graphView->isVisible())
2985 _ui->graphicsView_graphView->updateMap(map8U, resolution, xMin, yMin);
2989 _ui->graphicsView_graphView->update();
2994 stats->insert(std::make_pair(
"GUI/Grid Rendering/ms", (
float)timer.
restart()*1000.0f));
3003 if(viewerClouds.contains(
"cloudOdom"))
3019 if(viewerClouds.contains(
"scanOdom"))
3035 if(viewerClouds.contains(
"scanMapOdom"))
3051 if(viewerClouds.contains(
"featuresOdom"))
3072 #ifdef RTABMAP_OCTOMAP 3075 _ui->actionExport_octomap->setEnabled(
false);
3087 std::string cloudName =
uFormat(
"cloud%d", nodeId);
3088 std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> outputPair;
3091 UERROR(
"Cloud %d already added to map.", nodeId);
3098 UERROR(
"Node %d is not in the cache.", nodeId);
3104 if((!iter->sensorData().imageCompressed().empty() || !iter->sensorData().imageRaw().empty()) &&
3105 (!iter->sensorData().depthOrRightCompressed().empty() || !iter->sensorData().depthOrRightRaw().empty()))
3107 cv::Mat image, depth;
3111 bool rectifyOnlyFeatures = Parameters::defaultRtabmapRectifyOnlyFeatures();
3112 bool imagesAlreadyRectified = Parameters::defaultRtabmapImagesAlreadyRectified();
3113 Parameters::parse(allParameters, Parameters::kRtabmapRectifyOnlyFeatures(), rectifyOnlyFeatures);
3114 Parameters::parse(allParameters, Parameters::kRtabmapImagesAlreadyRectified(), imagesAlreadyRectified);
3115 if(rectifyOnlyFeatures && !imagesAlreadyRectified)
3123 cv::Mat rectifiedImages = data.
imageRaw().clone();
3129 for(
unsigned int i=0; i<data.
cameraModels().size(); ++i)
3138 UWARN(
"Initializing rectification maps for camera %d (only done for the first image received)...", i);
3140 UWARN(
"Initializing rectification maps for camera %d (only done for the first image received)... done!", i);
3146 rectifiedImage.copyTo(cv::Mat(rectifiedImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.
imageRaw().rows)));
3150 UWARN(
"Camera %d of data %d is not valid for rectification (%dx%d).",
3156 UINFO(
"Time rectification: %fs", time.
ticks());
3158 image = rectifiedImages;
3162 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
3163 pcl::IndicesPtr indices(
new std::vector<int>);
3164 UASSERT_MSG(nodeId == 0 || nodeId == data.
id(),
uFormat(
"nodeId=%d data.id()=%d", nodeId, data.
id()).c_str());
3176 Eigen::Vector3f viewPoint(0.0
f,0.0
f,0.0
f);
3179 viewPoint[0] = data.
cameraModels()[0].localTransform().x();
3180 viewPoint[1] = data.
cameraModels()[0].localTransform().y();
3181 viewPoint[2] = data.
cameraModels()[0].localTransform().z();
3195 indices->resize(cloud->size());
3196 for(
unsigned int i=0; i<cloud->size(); ++i)
3203 if(indices->size() &&
3218 if(indices->size() &&
3229 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(
new pcl::PointCloud<pcl::PointXYZRGBNormal>);
3234 pcl::IndicesPtr beforeFiltering = indices;
3235 if( cloud->size() &&
3258 pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
3262 UWARN(
"Cloud subtraction with angle filtering is activated but " 3263 "cloud normal K search is 0. Subtraction is done with angle.");
3267 if(cloudWithNormals->size() &&
3294 UINFO(
"Time subtract filtering %d from %d -> %d (%fs)",
3296 (int)beforeFiltering->size(),
3297 (int)indices->size(),
3310 pcl::PointCloud<pcl::PointXYZRGB>::Ptr output;
3326 pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputFiltered(
new pcl::PointCloud<pcl::PointXYZRGB>);
3327 std::vector<pcl::Vertices> outputPolygons;
3332 pcl::TextureMesh::Ptr textureMesh(
new pcl::TextureMesh);
3333 pcl::toPCLPointCloud2(*outputFiltered, textureMesh->cloud);
3334 textureMesh->tex_polygons.push_back(outputPolygons);
3335 int w = cloud->width;
3336 int h = cloud->height;
3338 textureMesh->tex_coordinates.resize(1);
3339 int nPoints = (int)outputFiltered->size();
3340 textureMesh->tex_coordinates[0].resize(nPoints);
3341 for(
int i=0; i<nPoints; ++i)
3344 UASSERT(i < (
int)denseToOrganizedIndices.size());
3345 int originalVertex = denseToOrganizedIndices[i];
3346 textureMesh->tex_coordinates[0][i] = Eigen::Vector2f(
3347 float(originalVertex % w) /
float(w),
3348 float(h - originalVertex / w) /
float(h));
3351 pcl::TexMaterial mesh_material;
3352 mesh_material.tex_d = 1.0f;
3353 mesh_material.tex_Ns = 75.0f;
3354 mesh_material.tex_illum = 1;
3356 std::stringstream tex_name;
3357 tex_name <<
"material_" << nodeId;
3358 tex_name >> mesh_material.tex_name;
3360 mesh_material.tex_file =
"";
3362 textureMesh->tex_materials.push_back(mesh_material);
3366 UERROR(
"Adding texture mesh %d to viewer failed!", nodeId);
3375 UERROR(
"Adding mesh cloud %d to viewer failed!", nodeId);
3387 UWARN(
"Online meshing is activated but the generated cloud is " 3388 "dense (voxel filtering is used or multiple cameras are used). Disable " 3389 "online meshing in Preferences->3D Rendering to hide this warning.");
3395 pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
3398 QColor color = Qt::gray;
3401 color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
3406 if(cloudWithNormals->size())
3408 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr outputWithNormals;
3413 UERROR(
"Adding cloud %d to viewer failed!", nodeId);
3424 UERROR(
"Adding cloud %d to viewer failed!", nodeId);
3434 outputPair.first = output;
3435 outputPair.second = indices;
3461 std::string scanName =
uFormat(
"scan%d", nodeId);
3464 UERROR(
"Scan %d already added to map.", nodeId);
3471 UERROR(
"Node %d is not in the cache.", nodeId);
3475 if(!iter->sensorData().laserScanCompressed().isEmpty() || !iter->sensorData().laserScanRaw().isEmpty())
3478 iter->sensorData().uncompressData(0, 0, &scan);
3490 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
3491 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGB;
3492 pcl::PointCloud<pcl::PointXYZI>::Ptr cloudI;
3493 pcl::PointCloud<pcl::PointNormal>::Ptr cloudWithNormals;
3494 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudRGBWithNormals;
3495 pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIWithNormals;
3538 if((!scan.
is2d()) &&
3542 if(cloudRGBWithNormals.get())
3555 if(cloudIWithNormals.get())
3568 if(cloudWithNormals.get())
3622 if( (cloud.get() || cloudRGB.get() || cloudI.get()) &&
3625 Eigen::Vector3f scanViewpoint(
3630 pcl::PointCloud<pcl::Normal>::Ptr normals;
3631 if(cloud.get() && cloud->size())
3641 cloudWithNormals.reset(
new pcl::PointCloud<pcl::PointNormal>);
3642 pcl::concatenateFields(*cloud, *normals, *cloudWithNormals);
3645 else if(cloudRGB.get() && cloudRGB->size())
3649 cloudRGBWithNormals.reset(
new pcl::PointCloud<pcl::PointXYZRGBNormal>);
3650 pcl::concatenateFields(*cloudRGB, *normals, *cloudRGBWithNormals);
3653 else if(cloudI.get())
3663 cloudIWithNormals.reset(
new pcl::PointCloud<pcl::PointXYZINormal>);
3664 pcl::concatenateFields(*cloudI, *normals, *cloudIWithNormals);
3669 QColor color = Qt::gray;
3672 color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
3675 if(cloudRGBWithNormals.get())
3678 if(added && nodeId > 0)
3683 else if(cloudIWithNormals.get())
3686 if(added && nodeId > 0)
3698 else if(cloudWithNormals.get())
3701 if(added && nodeId > 0)
3713 else if(cloudRGB.get())
3716 if(added && nodeId > 0)
3721 else if(cloudI.get())
3724 if(added && nodeId > 0)
3740 if(added && nodeId > 0)
3754 UERROR(
"Adding cloud %d to viewer failed!", nodeId);
3774 std::string cloudName =
uFormat(
"features%d", nodeId);
3777 UERROR(
"Features cloud %d already added to map.", nodeId);
3784 UERROR(
"Node %d is not in the cache.", nodeId);
3790 UDEBUG(
"Features cloud %d already created.");
3794 if(iter->getWords3().size())
3796 UINFO(
"Create cloud from 3D words");
3797 QColor color = Qt::gray;
3800 color = (Qt::GlobalColor)(mapId+3 % 12 + 7 );
3804 if(!iter->sensorData().imageCompressed().empty() || !iter->sensorData().imageRaw().empty())
3810 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZRGB>);
3811 cloud->resize(iter->getWords3().size());
3813 UASSERT(iter->getWords().size() == iter->getWords3().size());
3815 UDEBUG(
"rgb.channels()=%d");
3816 if(!iter->getWords3().empty() && !iter->getWordsKpts().empty())
3819 if(iter.value().sensorData().cameraModels().size() == 1 && iter.value().sensorData().cameraModels().at(0).isValidForProjection())
3821 invLocalTransform = iter.value().sensorData().cameraModels()[0].localTransform().
inverse();
3823 else if(iter.value().sensorData().stereoCameraModel().isValidForProjection())
3825 invLocalTransform = iter.value().sensorData().stereoCameraModel().left().localTransform().
inverse();
3828 for(std::multimap<int, int>::const_iterator jter=iter->getWords().begin(); jter!=iter->getWords().end(); ++jter)
3830 const cv::Point3f & pt = iter->getWords3()[jter->second];
3832 (maxDepth == 0.0
f ||
3834 (iter.value().sensorData().cameraModels().size()<=1 &&
3837 (*cloud)[oi].
x = pt.x;
3838 (*cloud)[oi].y = pt.y;
3839 (*cloud)[oi].z = pt.z;
3840 const cv::KeyPoint & kpt = iter->getWordsKpts()[jter->second];
3841 int u = kpt.pt.x+0.5;
3842 int v = kpt.pt.y+0.5;
3847 if(rgb.channels() == 1)
3849 (*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = rgb.at<
unsigned char>(v, u);
3853 cv::Vec3b bgr = rgb.at<cv::Vec3b>(v, u);
3854 (*cloud)[oi].b = bgr.val[0];
3855 (*cloud)[oi].g = bgr.val[1];
3856 (*cloud)[oi].r = bgr.val[2];
3861 (*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = 255;
3870 UERROR(
"Adding features cloud %d to viewer failed!", nodeId);
3887 const std::map<int, Transform> & poses,
3888 const std::map<int, Transform> & groundTruth)
3891 if(groundTruth.size() && poses.size())
3893 float translational_rmse = 0.0f;
3894 float translational_mean = 0.0f;
3895 float translational_median = 0.0f;
3896 float translational_std = 0.0f;
3897 float translational_min = 0.0f;
3898 float translational_max = 0.0f;
3899 float rotational_rmse = 0.0f;
3900 float rotational_mean = 0.0f;
3901 float rotational_median = 0.0f;
3902 float rotational_std = 0.0f;
3903 float rotational_min = 0.0f;
3904 float rotational_max = 0.0f;
3911 translational_median,
3923 UINFO(
"translational_rmse=%f", translational_rmse);
3924 UINFO(
"translational_mean=%f", translational_mean);
3925 UINFO(
"translational_median=%f", translational_median);
3926 UINFO(
"translational_std=%f", translational_std);
3927 UINFO(
"translational_min=%f", translational_min);
3928 UINFO(
"translational_max=%f", translational_max);