30 #include "ui_DatabaseViewer.h"
31 #include <QMessageBox>
32 #include <QFileDialog>
33 #include <QInputDialog>
34 #include <QColorDialog>
35 #include <QGraphicsLineItem>
36 #include <QtGui/QCloseEvent>
37 #include <QGraphicsOpacityEffect>
38 #include <QtCore/QBuffer>
39 #include <QtCore/QTextStream>
40 #include <QtCore/QDateTime>
41 #include <QtCore/QSettings>
46 #include <opencv2/core/core_c.h>
47 #include <opencv2/imgproc/types_c.h>
48 #include <opencv2/highgui/highgui.hpp>
90 #include <pcl/io/pcd_io.h>
91 #include <pcl/io/ply_io.h>
92 #include <pcl/io/obj_io.h>
93 #include <pcl/filters/voxel_grid.h>
94 #include <pcl/filters/crop_box.h>
95 #include <pcl/common/transforms.h>
96 #include <pcl/common/common.h>
99 #ifdef RTABMAP_OCTOMAP
103 #ifdef RTABMAP_GRIDMAP
114 editDepthDialog_(new QDialog(
this)),
115 editMapDialog_(new QDialog(
this)),
117 savedMaximized_(
false),
120 infoReducedGraph_(
false),
131 ui_ =
new Ui_DatabaseViewer();
133 ui_->buttonBox->setVisible(
false);
134 connect(
ui_->buttonBox->button(QDialogButtonBox::Close), SIGNAL(clicked()),
this, SLOT(close()));
136 ui_->comboBox_logger_level->setVisible(parent==0);
137 ui_->label_logger_level->setVisible(parent==0);
138 connect(
ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(
int)),
this, SLOT(
updateLoggerLevel()));
139 connect(
ui_->actionVertical_Layout, SIGNAL(toggled(
bool)),
this, SLOT(
setupMainLayout(
bool)));
144 vLayout->setContentsMargins(0,0,0,0);
145 vLayout->setSpacing(0);
147 QDialogButtonBox * buttonBox =
new QDialogButtonBox(QDialogButtonBox::Save | QDialogButtonBox::Cancel | QDialogButtonBox::Reset,
Qt::Horizontal,
editDepthDialog_);
148 vLayout->addWidget(buttonBox);
151 connect(buttonBox->button(QDialogButtonBox::Reset), SIGNAL(clicked()),
editDepthArea_, SLOT(resetChanges()));
158 vLayout->setContentsMargins(0,0,0,0);
159 vLayout->setSpacing(0);
161 buttonBox =
new QDialogButtonBox(QDialogButtonBox::Save | QDialogButtonBox::Cancel | QDialogButtonBox::Reset,
Qt::Horizontal,
editMapDialog_);
162 vLayout->addWidget(buttonBox);
163 connect(buttonBox, SIGNAL(accepted()),
editMapDialog_, SLOT(accept()));
164 connect(buttonBox, SIGNAL(rejected()),
editMapDialog_, SLOT(reject()));
165 connect(buttonBox->button(QDialogButtonBox::Reset), SIGNAL(clicked()),
editMapArea_, SLOT(resetChanges()));
169 QString title(
"RTAB-Map Database Viewer[*]");
170 this->setWindowTitle(title);
172 ui_->dockWidget_constraints->setVisible(
false);
173 ui_->dockWidget_graphView->setVisible(
false);
174 ui_->dockWidget_occupancyGridView->setVisible(
false);
175 ui_->dockWidget_guiparameters->setVisible(
false);
176 ui_->dockWidget_coreparameters->setVisible(
false);
177 ui_->dockWidget_info->setVisible(
false);
178 ui_->dockWidget_stereoView->setVisible(
false);
179 ui_->dockWidget_view3d->setVisible(
false);
180 ui_->dockWidget_statistics->setVisible(
false);
200 ui_->graphicsView_stereo->setAlpha(255);
202 #ifndef RTABMAP_OCTOMAP
203 ui_->checkBox_octomap->setEnabled(
false);
204 ui_->checkBox_octomap->setChecked(
false);
207 #ifndef RTABMAP_GRIDMAP
208 ui_->checkBox_showElevation->setEnabled(
false);
209 ui_->checkBox_showElevation->setChecked(
false);
210 ui_->checkBox_grid_elevation->setEnabled(
false);
211 ui_->checkBox_grid_elevation->setChecked(
false);
242 ui_->parameters_toolbox->setupUi(parameters);
248 ui_->comboBox_octomap_rendering_type->setVisible(
ui_->checkBox_octomap->isChecked());
249 ui_->spinBox_grid_depth->setVisible(
ui_->checkBox_octomap->isChecked());
250 ui_->checkBox_grid_empty->setVisible(!
ui_->checkBox_octomap->isChecked() ||
ui_->comboBox_octomap_rendering_type->currentIndex()==0);
251 ui_->checkBox_grid_frontiers->setVisible(
ui_->checkBox_octomap->isChecked() &&
ui_->comboBox_octomap_rendering_type->currentIndex()==0);
252 ui_->label_octomap_cubes->setVisible(
ui_->checkBox_octomap->isChecked());
253 ui_->label_octomap_depth->setVisible(
ui_->checkBox_octomap->isChecked());
254 ui_->label_octomap_empty->setVisible(!
ui_->checkBox_octomap->isChecked() ||
ui_->comboBox_octomap_rendering_type->currentIndex()==0);
255 ui_->label_octomap_frontiers->setVisible(
ui_->checkBox_octomap->isChecked() &&
ui_->comboBox_octomap_rendering_type->currentIndex()==0);
257 ui_->menuView->addAction(
ui_->dockWidget_constraints->toggleViewAction());
258 ui_->menuView->addAction(
ui_->dockWidget_graphView->toggleViewAction());
259 ui_->menuView->addAction(
ui_->dockWidget_occupancyGridView->toggleViewAction());
260 ui_->menuView->addAction(
ui_->dockWidget_stereoView->toggleViewAction());
261 ui_->menuView->addAction(
ui_->dockWidget_view3d->toggleViewAction());
262 ui_->menuView->addAction(
ui_->dockWidget_guiparameters->toggleViewAction());
263 ui_->menuView->addAction(
ui_->dockWidget_coreparameters->toggleViewAction());
264 ui_->menuView->addAction(
ui_->dockWidget_info->toggleViewAction());
265 ui_->menuView->addAction(
ui_->dockWidget_statistics->toggleViewAction());
266 connect(
ui_->dockWidget_graphView->toggleViewAction(), SIGNAL(triggered()),
this, SLOT(
updateGraphView()));
267 connect(
ui_->dockWidget_occupancyGridView->toggleViewAction(), SIGNAL(triggered()),
this, SLOT(
updateGraphView()));
268 connect(
ui_->dockWidget_statistics->toggleViewAction(), SIGNAL(triggered()),
this, SLOT(
updateStatistics()));
269 connect(
ui_->dockWidget_info->toggleViewAction(), SIGNAL(triggered()),
this, SLOT(
updateInfo()));
272 connect(
ui_->graphViewer, SIGNAL(linkSelected(
int,
int)),
this , SLOT(
graphLinkSelected(
int,
int)));
274 connect(
ui_->parameters_toolbox, SIGNAL(parametersChanged(
const QStringList &)),
this, SLOT(
notifyParametersChanged(
const QStringList &)));
276 connect(
ui_->actionQuit, SIGNAL(triggered()),
this, SLOT(close()));
278 ui_->actionOpen_database->setEnabled(
true);
279 ui_->actionClose_database->setEnabled(
false);
282 ui_->actionSave_config->setShortcut(QKeySequence::Save);
283 connect(
ui_->actionSave_config, SIGNAL(triggered()),
this, SLOT(
writeSettings()));
284 connect(
ui_->actionOpen_database, SIGNAL(triggered()),
this, SLOT(
openDatabase()));
285 connect(
ui_->actionClose_database, SIGNAL(triggered()),
this, SLOT(
closeDatabase()));
286 connect(
ui_->actionDatabase_recovery, SIGNAL(triggered()),
this, SLOT(
recoverDatabase()));
288 connect(
ui_->actionExtract_images, SIGNAL(triggered()),
this, SLOT(
extractImages()));
289 connect(
ui_->actionEdit_depth_image, SIGNAL(triggered()),
this, SLOT(
editDepthImage()));
290 connect(
ui_->actionGenerate_graph_dot, SIGNAL(triggered()),
this, SLOT(
generateGraph()));
292 connect(
ui_->actionRaw_format_txt, SIGNAL(triggered()),
this , SLOT(
exportPosesRaw()));
296 connect(
ui_->actionKITTI_format_txt, SIGNAL(triggered()),
this , SLOT(
exportPosesKITTI()));
299 connect(
ui_->actionPoses_KML, SIGNAL(triggered()),
this , SLOT(
exportPosesKML()));
300 connect(
ui_->actionGPS_TXT, SIGNAL(triggered()),
this , SLOT(
exportGPS_TXT()));
301 connect(
ui_->actionGPS_KML, SIGNAL(triggered()),
this , SLOT(
exportGPS_KML()));
302 connect(
ui_->actionEdit_optimized_2D_map, SIGNAL(triggered()),
this , SLOT(
editSaved2DMap()));
303 connect(
ui_->actionExport_saved_2D_map, SIGNAL(triggered()),
this , SLOT(
exportSaved2DMap()));
304 connect(
ui_->actionImport_2D_map, SIGNAL(triggered()),
this , SLOT(
import2DMap()));
305 connect(
ui_->actionRegenerate_optimized_2D_map, SIGNAL(triggered()),
this , SLOT(
regenerateSavedMap()));
309 connect(
ui_->actionView_3D_map, SIGNAL(triggered()),
this, SLOT(
view3DMap()));
310 connect(
ui_->actionGenerate_3D_map_pcd, SIGNAL(triggered()),
this, SLOT(
generate3DMap()));
315 connect(
ui_->actionRefine_links, SIGNAL(triggered()),
this, SLOT(
refineLinks()));
318 connect(
ui_->actionReset_all_changes, SIGNAL(triggered()),
this, SLOT(
resetAllChanges()));
323 connect(
ui_->pushButton_add, SIGNAL(clicked()),
this, SLOT(
addConstraint()));
326 ui_->pushButton_refine->setEnabled(
false);
327 ui_->pushButton_add->setEnabled(
false);
328 ui_->pushButton_reset->setEnabled(
false);
329 ui_->pushButton_reject->setEnabled(
false);
331 ui_->menuEdit->setEnabled(
false);
332 ui_->actionGenerate_3D_map_pcd->setEnabled(
false);
333 ui_->actionExport->setEnabled(
false);
334 ui_->actionExtract_images->setEnabled(
false);
335 ui_->menuExport_poses->setEnabled(
false);
336 ui_->menuExport_GPS->setEnabled(
false);
337 ui_->actionPoses_KML->setEnabled(
false);
338 ui_->actionEdit_optimized_2D_map->setEnabled(
false);
339 ui_->actionExport_saved_2D_map->setEnabled(
false);
340 ui_->actionImport_2D_map->setEnabled(
false);
341 ui_->actionRegenerate_optimized_2D_map->setEnabled(
false);
342 ui_->actionView_optimized_mesh->setEnabled(
false);
343 ui_->actionExport_optimized_mesh->setEnabled(
false);
344 ui_->actionUpdate_optimized_mesh->setEnabled(
false);
346 ui_->horizontalSlider_A->setTracking(
false);
347 ui_->horizontalSlider_B->setTracking(
false);
348 ui_->horizontalSlider_A->setEnabled(
false);
349 ui_->horizontalSlider_B->setEnabled(
false);
352 connect(
ui_->horizontalSlider_A, SIGNAL(sliderMoved(
int)),
this, SLOT(
sliderAMoved(
int)));
353 connect(
ui_->horizontalSlider_B, SIGNAL(sliderMoved(
int)),
this, SLOT(
sliderBMoved(
int)));
354 ui_->spinBox_indexA->setEnabled(
false);
355 ui_->spinBox_indexB->setEnabled(
false);
356 connect(
ui_->spinBox_indexA, SIGNAL(valueChanged(
int)),
ui_->horizontalSlider_A, SLOT(setValue(
int)));
357 connect(
ui_->spinBox_indexB, SIGNAL(valueChanged(
int)),
ui_->horizontalSlider_B, SLOT(setValue(
int)));
359 connect(
ui_->toolButton_edit_priorA, SIGNAL(clicked(
bool)),
this, SLOT(
editConstraint()));
360 connect(
ui_->toolButton_edit_priorB, SIGNAL(clicked(
bool)),
this, SLOT(
editConstraint()));
361 connect(
ui_->toolButton_remove_priorA, SIGNAL(clicked(
bool)),
this, SLOT(
rejectConstraint()));
362 connect(
ui_->toolButton_remove_priorB, SIGNAL(clicked(
bool)),
this, SLOT(
rejectConstraint()));
364 connect(
ui_->spinBox_mesh_angleTolerance, SIGNAL(valueChanged(
int)),
this, SLOT(
update3dView()));
365 connect(
ui_->spinBox_mesh_minClusterSize, SIGNAL(valueChanged(
int)),
this, SLOT(
update3dView()));
366 connect(
ui_->spinBox_mesh_fillDepthHoles, SIGNAL(valueChanged(
int)),
this, SLOT(
update3dView()));
367 connect(
ui_->spinBox_mesh_depthError, SIGNAL(valueChanged(
int)),
this, SLOT(
update3dView()));
368 connect(
ui_->checkBox_mesh_quad, SIGNAL(toggled(
bool)),
this, SLOT(
update3dView()));
369 connect(
ui_->spinBox_mesh_triangleSize, SIGNAL(valueChanged(
int)),
this, SLOT(
update3dView()));
370 connect(
ui_->checkBox_showWords, SIGNAL(toggled(
bool)),
this, SLOT(
update3dView()));
371 connect(
ui_->checkBox_showCloud, SIGNAL(toggled(
bool)),
this, SLOT(
update3dView()));
372 connect(
ui_->checkBox_showMesh, SIGNAL(toggled(
bool)),
this, SLOT(
update3dView()));
373 connect(
ui_->checkBox_showScan, SIGNAL(toggled(
bool)),
this, SLOT(
update3dView()));
374 connect(
ui_->checkBox_showMap, SIGNAL(toggled(
bool)),
this, SLOT(
update3dView()));
375 connect(
ui_->checkBox_showGrid, SIGNAL(toggled(
bool)),
this, SLOT(
update3dView()));
376 connect(
ui_->checkBox_showElevation, SIGNAL(stateChanged(
int)),
this, SLOT(
update3dView()));
377 connect(
ui_->checkBox_odomFrame_3dview, SIGNAL(toggled(
bool)),
this, SLOT(
update3dView()));
378 connect(
ui_->checkBox_gravity_3dview, SIGNAL(toggled(
bool)),
this, SLOT(
update3dView()));
380 ui_->horizontalSlider_neighbors->setTracking(
false);
381 ui_->horizontalSlider_loops->setTracking(
false);
382 ui_->horizontalSlider_neighbors->setEnabled(
false);
383 ui_->horizontalSlider_loops->setEnabled(
false);
393 ui_->checkBox_showOptimized->setEnabled(
false);
394 connect(
ui_->toolButton_constraint, SIGNAL(clicked(
bool)),
this, SLOT(
editConstraint()));
397 ui_->horizontalSlider_iterations->setTracking(
false);
398 ui_->horizontalSlider_iterations->setEnabled(
false);
399 ui_->spinBox_optimizationsFrom->setEnabled(
false);
402 connect(
ui_->spinBox_optimizationsFrom, SIGNAL(editingFinished()),
this, SLOT(
updateGraphView()));
403 connect(
ui_->comboBox_optimizationFlavor, SIGNAL(activated(
int)),
this, SLOT(
updateGraphView()));
404 connect(
ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(
int)),
this, SLOT(
updateGraphView()));
405 connect(
ui_->checkBox_wmState, SIGNAL(stateChanged(
int)),
this, SLOT(
updateGraphView()));
406 connect(
ui_->graphViewer, SIGNAL(mapShownRequested()),
this, SLOT(
updateGraphView()));
407 connect(
ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(
int)),
this, SLOT(
updateGraphView()));
409 connect(
ui_->checkBox_ignoreGlobalLoop, SIGNAL(stateChanged(
int)),
this, SLOT(
updateGraphView()));
410 connect(
ui_->checkBox_ignoreLocalLoopSpace, SIGNAL(stateChanged(
int)),
this, SLOT(
updateGraphView()));
411 connect(
ui_->checkBox_ignoreLocalLoopTime, SIGNAL(stateChanged(
int)),
this, SLOT(
updateGraphView()));
412 connect(
ui_->checkBox_ignoreUserLoop, SIGNAL(stateChanged(
int)),
this, SLOT(
updateGraphView()));
413 connect(
ui_->checkBox_ignoreLandmarks, SIGNAL(stateChanged(
int)),
this, SLOT(
updateGraphView()));
414 connect(
ui_->doubleSpinBox_optimizationScale, SIGNAL(editingFinished()),
this, SLOT(
updateGraphView()));
415 connect(
ui_->checkBox_octomap, SIGNAL(stateChanged(
int)),
this, SLOT(
updateGrid()));
416 connect(
ui_->checkBox_grid_grid, SIGNAL(stateChanged(
int)),
this, SLOT(
updateGrid()));
417 connect(
ui_->checkBox_grid_2d, SIGNAL(stateChanged(
int)),
this, SLOT(
updateGrid()));
418 connect(
ui_->checkBox_grid_elevation, SIGNAL(stateChanged(
int)),
this, SLOT(
updateGrid()));
419 connect(
ui_->comboBox_octomap_rendering_type, SIGNAL(currentIndexChanged(
int)),
this, SLOT(
updateOctomapView()));
421 connect(
ui_->checkBox_grid_empty, SIGNAL(stateChanged(
int)),
this, SLOT(
updateGrid()));
422 connect(
ui_->checkBox_grid_frontiers, SIGNAL(stateChanged(
int)),
this, SLOT(
updateGrid()));
423 connect(
ui_->doubleSpinBox_gainCompensationRadius, SIGNAL(valueChanged(
double)),
this, SLOT(
updateConstraintView()));
425 connect(
ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(
double)),
this, SLOT(
update3dView()));
426 connect(
ui_->checkBox_cameraProjection, SIGNAL(stateChanged(
int)),
this, SLOT(
update3dView()));
427 connect(
ui_->checkBox_showDisparityInsteadOfRight, SIGNAL(stateChanged(
int)),
this, SLOT(
update3dView()));
429 connect(
ui_->spinBox_decimation, SIGNAL(valueChanged(
int)),
this, SLOT(
update3dView()));
430 connect(
ui_->groupBox_posefiltering, SIGNAL(clicked(
bool)),
this, SLOT(
updateGraphView()));
431 connect(
ui_->doubleSpinBox_posefilteringRadius, SIGNAL(editingFinished()),
this, SLOT(
updateGraphView()));
432 connect(
ui_->doubleSpinBox_posefilteringAngle, SIGNAL(editingFinished()),
this, SLOT(
updateGraphView()));
434 ui_->label_stereo_inliers_name->setStyleSheet(
"QLabel {color : blue; }");
435 ui_->label_stereo_flowOutliers_name->setStyleSheet(
"QLabel {color : red; }");
436 ui_->label_stereo_slopeOutliers_name->setStyleSheet(
"QLabel {color : yellow; }");
437 ui_->label_stereo_disparityOutliers_name->setStyleSheet(
"QLabel {color : magenta; }");
441 connect(
ui_->graphViewer, SIGNAL(configChanged()),
this, SLOT(
configModified()));
442 connect(
ui_->graphicsView_A, SIGNAL(configChanged()),
this, SLOT(
configModified()));
443 connect(
ui_->graphicsView_B, SIGNAL(configChanged()),
this, SLOT(
configModified()));
444 connect(
ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(
int)),
this, SLOT(
configModified()));
445 connect(
ui_->actionVertical_Layout, SIGNAL(toggled(
bool)),
this, SLOT(
configModified()));
446 connect(
ui_->actionConcise_Layout, SIGNAL(toggled(
bool)),
this, SLOT(
configModified()));
447 connect(
ui_->checkBox_alignPosesWithGPS, SIGNAL(stateChanged(
int)),
this, SLOT(
updateGraphView()));
448 connect(
ui_->checkBox_alignPosesWithGroundTruth, SIGNAL(stateChanged(
int)),
this, SLOT(
updateGraphView()));
449 connect(
ui_->checkBox_alignScansCloudsWithGroundTruth, SIGNAL(stateChanged(
int)),
this, SLOT(
updateGraphView()));
450 connect(
ui_->checkBox_ignoreIntermediateNodes, SIGNAL(stateChanged(
int)),
this, SLOT(
configModified()));
451 connect(
ui_->checkBox_ignoreIntermediateNodes, SIGNAL(stateChanged(
int)),
this, SLOT(
updateGraphView()));
452 connect(
ui_->checkBox_timeStats, SIGNAL(stateChanged(
int)),
this, SLOT(
configModified()));
453 connect(
ui_->checkBox_timeStats, SIGNAL(stateChanged(
int)),
this, SLOT(
updateStatistics()));
455 connect(
ui_->doubleSpinBox_gainCompensationRadius, SIGNAL(valueChanged(
double)),
this, SLOT(
configModified()));
456 connect(
ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(
double)),
this, SLOT(
configModified()));
457 connect(
ui_->checkBox_cameraProjection, SIGNAL(stateChanged(
int)),
this, SLOT(
configModified()));
458 connect(
ui_->checkBox_showDisparityInsteadOfRight, SIGNAL(stateChanged(
int)),
this, SLOT(
configModified()));
459 connect(
ui_->spinBox_decimation, SIGNAL(valueChanged(
int)),
this, SLOT(
configModified()));
460 connect(
ui_->groupBox_posefiltering, SIGNAL(clicked(
bool)),
this, SLOT(
configModified()));
461 connect(
ui_->doubleSpinBox_posefilteringRadius, SIGNAL(valueChanged(
double)),
this, SLOT(
configModified()));
462 connect(
ui_->doubleSpinBox_posefilteringAngle, SIGNAL(valueChanged(
double)),
this, SLOT(
configModified()));
464 connect(
ui_->pushButton_applyRotation, SIGNAL(clicked()),
this, SLOT(
updateGraphView()));
466 connect(
ui_->spinBox_icp_decimation, SIGNAL(valueChanged(
int)),
this, SLOT(
configModified()));
467 connect(
ui_->doubleSpinBox_icp_maxDepth, SIGNAL(valueChanged(
double)),
this, SLOT(
configModified()));
468 connect(
ui_->doubleSpinBox_icp_minDepth, SIGNAL(valueChanged(
double)),
this, SLOT(
configModified()));
469 connect(
ui_->checkBox_icp_from_depth, SIGNAL(stateChanged(
int)),
this, SLOT(
configModified()));
471 connect(
ui_->doubleSpinBox_detectMore_radius, SIGNAL(valueChanged(
double)),
this, SLOT(
configModified()));
472 connect(
ui_->doubleSpinBox_detectMore_radiusMin, SIGNAL(valueChanged(
double)),
this, SLOT(
configModified()));
473 connect(
ui_->doubleSpinBox_detectMore_angle, SIGNAL(valueChanged(
double)),
this, SLOT(
configModified()));
474 connect(
ui_->spinBox_detectMore_iterations, SIGNAL(valueChanged(
int)),
this, SLOT(
configModified()));
475 connect(
ui_->checkBox_detectMore_intraSession, SIGNAL(stateChanged(
int)),
this, SLOT(
configModified()));
476 connect(
ui_->checkBox_detectMore_interSession, SIGNAL(stateChanged(
int)),
this, SLOT(
configModified()));
477 connect(
ui_->checkBox_opt_graph_as_guess, SIGNAL(stateChanged(
int)),
this, SLOT(
configModified()));
479 connect(
ui_->lineEdit_obstacleColor, SIGNAL(textChanged(
const QString &)),
this, SLOT(
configModified()));
480 connect(
ui_->lineEdit_groundColor, SIGNAL(textChanged(
const QString &)),
this, SLOT(
configModified()));
481 connect(
ui_->lineEdit_emptyColor, SIGNAL(textChanged(
const QString &)),
this, SLOT(
configModified()));
482 connect(
ui_->lineEdit_frontierColor, SIGNAL(textChanged(
const QString &)),
this, SLOT(
configModified()));
483 connect(
ui_->lineEdit_obstacleColor, SIGNAL(textChanged(
const QString &)),
this, SLOT(
updateGrid()));
484 connect(
ui_->lineEdit_groundColor, SIGNAL(textChanged(
const QString &)),
this, SLOT(
updateGrid()));
485 connect(
ui_->lineEdit_emptyColor, SIGNAL(textChanged(
const QString &)),
this, SLOT(
updateGrid()));
486 connect(
ui_->lineEdit_frontierColor, SIGNAL(textChanged(
const QString &)),
this, SLOT(
updateGrid()));
489 connect(
ui_->toolButton_emptyColor, SIGNAL(clicked(
bool)),
this, SLOT(
selectEmptyColor()));
491 connect(
ui_->spinBox_cropRadius, SIGNAL(valueChanged(
int)),
this, SLOT(
configModified()));
492 connect(
ui_->checkBox_grid_showProbMap, SIGNAL(stateChanged(
int)),
this, SLOT(
configModified()));
493 connect(
ui_->checkBox_grid_showProbMap, SIGNAL(stateChanged(
int)),
this, SLOT(
updateGraphView()));
498 QList<QDockWidget*> dockWidgets = this->findChildren<QDockWidget*>();
499 for(
int i=0;
i<dockWidgets.size(); ++
i)
501 connect(dockWidgets[
i], SIGNAL(dockLocationChanged(Qt::DockWidgetArea)),
this, SLOT(
configModified()));
502 connect(dockWidgets[
i]->toggleViewAction(), SIGNAL(toggled(
bool)),
this, SLOT(
configModified()));
504 ui_->dockWidget_constraints->installEventFilter(
this);
505 ui_->dockWidget_graphView->installEventFilter(
this);
506 ui_->dockWidget_occupancyGridView->installEventFilter(
this);
507 ui_->dockWidget_stereoView->installEventFilter(
this);
508 ui_->dockWidget_view3d->installEventFilter(
this);
509 ui_->dockWidget_guiparameters->installEventFilter(
this);
510 ui_->dockWidget_coreparameters->installEventFilter(
this);
511 ui_->dockWidget_info->installEventFilter(
this);
512 ui_->dockWidget_statistics->installEventFilter(
this);
519 #ifdef RTABMAP_OCTOMAP
528 qobject_cast<QHBoxLayout *>(
ui_->horizontalLayout_imageViews->layout())->setDirection(QBoxLayout::TopToBottom);
532 qobject_cast<QHBoxLayout *>(
ui_->horizontalLayout_imageViews->layout())->setDirection(QBoxLayout::LeftToRight);
542 ui_->buttonBox->setVisible(visible);
547 if(
ui_->actionConcise_Layout->isChecked())
549 ui_->graphicsView_B->setVisible(
false);
550 ui_->scrollArea->setVisible(
false);
551 ui_->scrollArea_2->setVisible(
false);
552 ui_->spinBox_indexB->setVisible(
false);
553 ui_->widget_imageControls_B->setVisible(
false);
554 ui_->widget_graphControl->setVisible(
false);
555 ui_->graphicsView_A->clearLines();
556 ui_->graphicsView_B->clearLines();
560 ui_->graphicsView_B->setVisible(
true);
561 ui_->scrollArea->setVisible(
true);
562 ui_->scrollArea_2->setVisible(
true);
563 ui_->spinBox_indexB->setVisible(
true);
564 ui_->widget_imageControls_B->setVisible(
true);
565 ui_->widget_graphControl->setVisible(
true);
568 this->setWindowModified(
true);
577 QString privatePath = QDir::homePath() +
"/.rtabmap";
578 if(!QDir(privatePath).exists())
580 QDir::home().mkdir(
".rtabmap");
582 return privatePath +
"/rtabmap.ini";
588 QSettings settings(
path, QSettings::IniFormat);
589 settings.beginGroup(
"DatabaseViewer");
593 bytes = settings.value(
"geometry", QByteArray()).toByteArray();
596 this->restoreGeometry(
bytes);
598 bytes = settings.value(
"state", QByteArray()).toByteArray();
601 this->restoreState(
bytes);
605 ui_->comboBox_logger_level->setCurrentIndex(settings.value(
"loggerLevel",
ui_->comboBox_logger_level->currentIndex()).toInt());
606 ui_->actionVertical_Layout->setChecked(settings.value(
"verticalLayout",
ui_->actionVertical_Layout->isChecked()).toBool());
607 ui_->actionConcise_Layout->setChecked(settings.value(
"conciseLayout",
ui_->actionConcise_Layout->isChecked()).toBool());
608 ui_->checkBox_ignoreIntermediateNodes->setChecked(settings.value(
"ignoreIntermediateNodes",
ui_->checkBox_ignoreIntermediateNodes->isChecked()).toBool());
609 ui_->checkBox_timeStats->setChecked(settings.value(
"timeStats",
ui_->checkBox_timeStats->isChecked()).toBool());
612 ui_->graphViewer->loadSettings(settings,
"GraphView");
613 ui_->graphViewer->setReferentialVisible(
false);
615 settings.beginGroup(
"optimization");
616 ui_->doubleSpinBox_gainCompensationRadius->setValue(settings.value(
"gainCompensationRadius",
ui_->doubleSpinBox_gainCompensationRadius->value()).toDouble());
617 ui_->doubleSpinBox_voxelSize->setValue(settings.value(
"voxelSize",
ui_->doubleSpinBox_voxelSize->value()).toDouble());
618 ui_->spinBox_decimation->setValue(settings.value(
"decimation",
ui_->spinBox_decimation->value()).toInt());
619 ui_->checkBox_cameraProjection->setChecked(settings.value(
"camProj",
ui_->checkBox_cameraProjection->isChecked()).toBool());
620 ui_->checkBox_showDisparityInsteadOfRight->setChecked(settings.value(
"showDisp",
ui_->checkBox_showDisparityInsteadOfRight->isChecked()).toBool());
623 settings.beginGroup(
"grid");
624 ui_->groupBox_posefiltering->setChecked(settings.value(
"poseFiltering",
ui_->groupBox_posefiltering->isChecked()).toBool());
625 ui_->doubleSpinBox_posefilteringRadius->setValue(settings.value(
"poseFilteringRadius",
ui_->doubleSpinBox_posefilteringRadius->value()).toDouble());
626 ui_->doubleSpinBox_posefilteringAngle->setValue(settings.value(
"poseFilteringAngle",
ui_->doubleSpinBox_posefilteringAngle->value()).toDouble());
627 ui_->lineEdit_obstacleColor->setText(settings.value(
"colorObstacle",
ui_->lineEdit_obstacleColor->text()).toString());
628 ui_->lineEdit_groundColor->setText(settings.value(
"colorGround",
ui_->lineEdit_groundColor->text()).toString());
629 ui_->lineEdit_emptyColor->setText(settings.value(
"colorEmpty",
ui_->lineEdit_emptyColor->text()).toString());
630 ui_->lineEdit_frontierColor->setText(settings.value(
"colorFrontier",
ui_->lineEdit_frontierColor->text()).toString());
631 ui_->spinBox_cropRadius->setValue(settings.value(
"cropRadius",
ui_->spinBox_cropRadius->value()).toInt());
632 ui_->checkBox_grid_showProbMap->setChecked(settings.value(
"probMap",
ui_->checkBox_grid_showProbMap->isChecked()).toBool());
635 settings.beginGroup(
"mesh");
636 ui_->checkBox_mesh_quad->setChecked(settings.value(
"quad",
ui_->checkBox_mesh_quad->isChecked()).toBool());
637 ui_->spinBox_mesh_angleTolerance->setValue(settings.value(
"angleTolerance",
ui_->spinBox_mesh_angleTolerance->value()).toInt());
638 ui_->spinBox_mesh_minClusterSize->setValue(settings.value(
"minClusterSize",
ui_->spinBox_mesh_minClusterSize->value()).toInt());
639 ui_->spinBox_mesh_fillDepthHoles->setValue(settings.value(
"fillDepthHolesSize",
ui_->spinBox_mesh_fillDepthHoles->value()).toInt());
640 ui_->spinBox_mesh_depthError->setValue(settings.value(
"fillDepthHolesError",
ui_->spinBox_mesh_depthError->value()).toInt());
641 ui_->spinBox_mesh_triangleSize->setValue(settings.value(
"triangleSize",
ui_->spinBox_mesh_triangleSize->value()).toInt());
645 ui_->graphicsView_A->loadSettings(settings,
"ImageViewA");
646 ui_->graphicsView_B->loadSettings(settings,
"ImageViewB");
649 settings.beginGroup(
"icp");
650 ui_->spinBox_icp_decimation->setValue(settings.value(
"decimation",
ui_->spinBox_icp_decimation->value()).toInt());
651 ui_->doubleSpinBox_icp_maxDepth->setValue(settings.value(
"maxDepth",
ui_->doubleSpinBox_icp_maxDepth->value()).toDouble());
652 ui_->doubleSpinBox_icp_minDepth->setValue(settings.value(
"minDepth",
ui_->doubleSpinBox_icp_minDepth->value()).toDouble());
653 ui_->checkBox_icp_from_depth->setChecked(settings.value(
"icpFromDepth",
ui_->checkBox_icp_from_depth->isChecked()).toBool());
659 settings.beginGroup(
"Gui");
661 settings.beginGroup(
"PostProcessingDialog");
662 ui_->doubleSpinBox_detectMore_radius->setValue(settings.value(
"cluster_radius",
ui_->doubleSpinBox_detectMore_radius->value()).toDouble());
663 ui_->doubleSpinBox_detectMore_radiusMin->setValue(settings.value(
"cluster_radius_min",
ui_->doubleSpinBox_detectMore_radiusMin->value()).toDouble());
664 ui_->doubleSpinBox_detectMore_angle->setValue(settings.value(
"cluster_angle",
ui_->doubleSpinBox_detectMore_angle->value()).toDouble());
665 ui_->spinBox_detectMore_iterations->setValue(settings.value(
"iterations",
ui_->spinBox_detectMore_iterations->value()).toInt());
666 ui_->checkBox_detectMore_intraSession->setChecked(settings.value(
"intra_session",
ui_->checkBox_detectMore_intraSession->isChecked()).toBool());
667 ui_->checkBox_detectMore_interSession->setChecked(settings.value(
"inter_session",
ui_->checkBox_detectMore_interSession->isChecked()).toBool());
668 ui_->checkBox_opt_graph_as_guess->setChecked(settings.value(
"opt_graph_as_guess",
ui_->checkBox_opt_graph_as_guess->isChecked()).toBool());
675 for(ParametersMap::iterator
iter = parameters.begin();
iter!= parameters.end(); ++
iter)
677 ui_->parameters_toolbox->updateParameter(
iter->first,
iter->second);
684 QSettings settings(
path, QSettings::IniFormat);
685 settings.beginGroup(
"DatabaseViewer");
688 if(!this->isMaximized())
690 settings.setValue(
"geometry", this->saveGeometry());
692 settings.setValue(
"state", this->saveState());
693 settings.setValue(
"maximized", this->isMaximized());
696 settings.setValue(
"loggerLevel",
ui_->comboBox_logger_level->currentIndex());
697 settings.setValue(
"verticalLayout",
ui_->actionVertical_Layout->isChecked());
698 settings.setValue(
"conciseLayout",
ui_->actionConcise_Layout->isChecked());
699 settings.setValue(
"ignoreIntermediateNodes",
ui_->checkBox_ignoreIntermediateNodes->isChecked());
700 settings.setValue(
"timeStats",
ui_->checkBox_timeStats->isChecked());
703 ui_->graphViewer->saveSettings(settings,
"GraphView");
706 settings.beginGroup(
"optimization");
707 settings.setValue(
"gainCompensationRadius",
ui_->doubleSpinBox_gainCompensationRadius->value());
708 settings.setValue(
"voxelSize",
ui_->doubleSpinBox_voxelSize->value());
709 settings.setValue(
"decimation",
ui_->spinBox_decimation->value());
710 settings.setValue(
"camProj",
ui_->checkBox_cameraProjection->isChecked());
711 settings.setValue(
"showDisp",
ui_->checkBox_showDisparityInsteadOfRight->isChecked());
715 settings.beginGroup(
"grid");
716 settings.setValue(
"poseFiltering",
ui_->groupBox_posefiltering->isChecked());
717 settings.setValue(
"poseFilteringRadius",
ui_->doubleSpinBox_posefilteringRadius->value());
718 settings.setValue(
"poseFilteringAngle",
ui_->doubleSpinBox_posefilteringAngle->value());
719 settings.setValue(
"colorObstacle",
ui_->lineEdit_obstacleColor->text());
720 settings.setValue(
"colorGround",
ui_->lineEdit_groundColor->text());
721 settings.setValue(
"colorEmpty",
ui_->lineEdit_emptyColor->text());
722 settings.setValue(
"colorFrontier",
ui_->lineEdit_frontierColor->text());
723 settings.setValue(
"cropRadius",
ui_->spinBox_cropRadius->value());
724 settings.setValue(
"probMap",
ui_->checkBox_grid_showProbMap->isChecked());
727 settings.beginGroup(
"mesh");
728 settings.setValue(
"quad",
ui_->checkBox_mesh_quad->isChecked());
729 settings.setValue(
"angleTolerance",
ui_->spinBox_mesh_angleTolerance->value());
730 settings.setValue(
"minClusterSize",
ui_->spinBox_mesh_minClusterSize->value());
731 settings.setValue(
"fillDepthHolesSize",
ui_->spinBox_mesh_fillDepthHoles->value());
732 settings.setValue(
"fillDepthHolesError",
ui_->spinBox_mesh_depthError->value());
733 settings.setValue(
"triangleSize",
ui_->spinBox_mesh_triangleSize->value());
737 ui_->graphicsView_A->saveSettings(settings,
"ImageViewA");
738 ui_->graphicsView_B->saveSettings(settings,
"ImageViewB");
741 settings.beginGroup(
"icp");
742 settings.setValue(
"decimation",
ui_->spinBox_icp_decimation->value());
743 settings.setValue(
"maxDepth",
ui_->doubleSpinBox_icp_maxDepth->value());
744 settings.setValue(
"minDepth",
ui_->doubleSpinBox_icp_minDepth->value());
745 settings.setValue(
"icpFromDepth",
ui_->checkBox_icp_from_depth->isChecked());
751 settings.beginGroup(
"Gui");
753 settings.beginGroup(
"PostProcessingDialog");
754 settings.setValue(
"cluster_radius",
ui_->doubleSpinBox_detectMore_radius->value());
755 settings.setValue(
"cluster_radius_min",
ui_->doubleSpinBox_detectMore_radiusMin->value());
756 settings.setValue(
"cluster_angle",
ui_->doubleSpinBox_detectMore_angle->value());
757 settings.setValue(
"iterations",
ui_->spinBox_detectMore_iterations->value());
758 settings.setValue(
"intra_session",
ui_->checkBox_detectMore_intraSession->isChecked());
759 settings.setValue(
"inter_session",
ui_->checkBox_detectMore_interSession->isChecked());
760 settings.setValue(
"opt_graph_as_guess",
ui_->checkBox_opt_graph_as_guess->isChecked());
765 for(ParametersMap::iterator
iter=parameters.begin();
iter!=parameters.end();)
767 if(!
ui_->parameters_toolbox->getParameterWidget(
iter->first.c_str()))
769 parameters.erase(
iter++);
778 this->setWindowModified(
false);
784 ui_->comboBox_logger_level->setCurrentIndex(1);
785 ui_->checkBox_alignPosesWithGPS->setChecked(
true);
786 ui_->checkBox_alignPosesWithGroundTruth->setChecked(
true);
787 ui_->checkBox_alignScansCloudsWithGroundTruth->setChecked(
false);
788 ui_->checkBox_ignoreIntermediateNodes->setChecked(
false);
789 ui_->checkBox_timeStats->setChecked(
true);
791 ui_->comboBox_optimizationFlavor->setCurrentIndex(0);
792 ui_->checkBox_spanAllMaps->setChecked(
true);
793 ui_->checkBox_wmState->setChecked(
false);
794 ui_->checkBox_ignorePoseCorrection->setChecked(
false);
795 ui_->checkBox_ignoreGlobalLoop->setChecked(
false);
796 ui_->checkBox_ignoreLocalLoopSpace->setChecked(
false);
797 ui_->checkBox_ignoreLocalLoopTime->setChecked(
false);
798 ui_->checkBox_ignoreUserLoop->setChecked(
false);
799 ui_->checkBox_ignoreLandmarks->setChecked(
false);
800 ui_->doubleSpinBox_optimizationScale->setValue(1.0);
801 ui_->doubleSpinBox_gainCompensationRadius->setValue(0.0);
802 ui_->doubleSpinBox_voxelSize->setValue(0.0);
803 ui_->spinBox_decimation->setValue(1);
804 ui_->checkBox_cameraProjection->setChecked(
false);
805 ui_->checkBox_showDisparityInsteadOfRight->setChecked(
false);
807 ui_->groupBox_posefiltering->setChecked(
false);
808 ui_->doubleSpinBox_posefilteringRadius->setValue(0.1);
809 ui_->doubleSpinBox_posefilteringAngle->setValue(30);
810 ui_->checkBox_grid_empty->setChecked(
true);
811 ui_->checkBox_grid_frontiers->setChecked(
false);
812 ui_->checkBox_octomap->setChecked(
false);
813 ui_->lineEdit_obstacleColor->setText(QColor(Qt::red).
name());
814 ui_->lineEdit_groundColor->setText(QColor(Qt::green).
name());
815 ui_->lineEdit_emptyColor->setText(QColor(Qt::yellow).
name());
816 ui_->lineEdit_frontierColor->setText(QColor(Qt::cyan).
name());
817 ui_->spinBox_cropRadius->setValue(1);
818 ui_->checkBox_grid_showProbMap->setChecked(
false);
820 ui_->checkBox_mesh_quad->setChecked(
true);
821 ui_->spinBox_mesh_angleTolerance->setValue(15);
822 ui_->spinBox_mesh_minClusterSize->setValue(0);
823 ui_->spinBox_mesh_fillDepthHoles->setValue(
false);
824 ui_->spinBox_mesh_depthError->setValue(10);
825 ui_->spinBox_mesh_triangleSize->setValue(2);
827 ui_->spinBox_icp_decimation->setValue(1);
828 ui_->doubleSpinBox_icp_maxDepth->setValue(0.0);
829 ui_->doubleSpinBox_icp_minDepth->setValue(0.0);
830 ui_->checkBox_icp_from_depth->setChecked(
false);
832 ui_->doubleSpinBox_detectMore_radius->setValue(1.0);
833 ui_->doubleSpinBox_detectMore_radiusMin->setValue(0.0);
834 ui_->doubleSpinBox_detectMore_angle->setValue(30.0);
835 ui_->spinBox_detectMore_iterations->setValue(5);
836 ui_->checkBox_detectMore_intraSession->setChecked(
true);
837 ui_->checkBox_detectMore_interSession->setChecked(
true);
838 ui_->checkBox_opt_graph_as_guess->setChecked(
true);
843 QString
path = QFileDialog::getOpenFileName(
this, tr(
"Select file"),
pathDatabase_, tr(
"Databases (*.db)"));
852 UDEBUG(
"Open database \"%s\"",
path.toStdString().c_str());
853 if(QFile::exists(
path))
855 if(QFileInfo(
path).isFile())
857 std::string driverType =
"sqlite3";
863 ui_->actionClose_database->setEnabled(
false);
864 ui_->actionOpen_database->setEnabled(
true);
867 QMessageBox::warning(
this,
"Database error", tr(
"Can't open database \"%1\"").
arg(
path));
871 ui_->actionClose_database->setEnabled(
true);
872 ui_->actionOpen_database->setEnabled(
false);
886 uInsert(parameters, overriddenParameters);
888 if(parameters.size())
890 const ParametersMap & currentParameters =
ui_->parameters_toolbox->getParameters();
892 for(ParametersMap::iterator
iter=parameters.begin();
iter!=parameters.end(); ++
iter)
894 ParametersMap::const_iterator jter = currentParameters.find(
iter->first);
895 if(jter!=currentParameters.end() &&
896 ui_->parameters_toolbox->getParameterWidget(QString(
iter->first.c_str())) != 0 &&
897 iter->second.compare(jter->second) != 0 &&
898 iter->first.compare(Parameters::kRtabmapWorkingDirectory()) != 0)
900 bool different =
true;
911 differentParameters.insert(*
iter);
912 QString
msg = tr(
"Parameter \"%1\": database=\"%2\" Preferences=\"%3\"")
913 .arg(
iter->first.c_str())
914 .arg(
iter->second.c_str())
915 .arg(jter->second.c_str());
921 if(differentParameters.size())
923 int r = QMessageBox::question(
this,
924 tr(
"Update parameters..."),
925 tr(
"The database is using %1 different parameter(s) than "
926 "those currently set in Core parameters panel. Do you want "
927 "to use database's parameters?").
arg(differentParameters.size()),
928 QMessageBox::Yes | QMessageBox::No,
930 if(r == QMessageBox::Yes)
933 for(rtabmap::ParametersMap::const_iterator
iter = differentParameters.begin();
iter!=differentParameters.end(); ++
iter)
935 ui_->parameters_toolbox->updateParameter(
iter->first,
iter->second);
936 str.push_back(
iter->first.c_str());
944 this->setWindowTitle(
"RTAB-Map Database Viewer - " +
path +
"[*]");
960 QMessageBox::warning(
this,
"Database error", tr(
"Database \"%1\" does not exist.").
arg(
path));
967 this->setWindowTitle(
"RTAB-Map Database Viewer[*]");
972 QMessageBox::StandardButton button = QMessageBox::question(
this,
973 tr(
"Links modified"),
974 tr(
"Some links are modified (%1 added, %2 refined, %3 removed), do you want to save them?")
976 QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
977 QMessageBox::Cancel);
979 if(button == QMessageBox::Yes)
988 if(refinedIter->second.from() != refinedIter->second.to())
994 if(
iter->second.from() !=
iter->second.to())
1005 if(
iter->second.from() !=
iter->second.to())
1014 if(
iter->second.from() !=
iter->second.to())
1032 if(button != QMessageBox::Yes && button != QMessageBox::No)
1041 QMessageBox::StandardButton button = QMessageBox::question(
this,
1042 tr(
"Local occupancy grid maps modified"),
1043 tr(
"%1 occupancy grid maps are modified, do you want to "
1044 "save them? This will overwrite occupancy grids saved in the database.")
1046 QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
1047 QMessageBox::Cancel);
1049 if(button == QMessageBox::Yes)
1057 mapIter->second.groundCells,
1058 mapIter->second.obstacleCells,
1059 mapIter->second.emptyCells,
1060 mapIter->second.cellSize,
1061 mapIter->second.viewPoint);
1070 if(button != QMessageBox::Yes && button != QMessageBox::No)
1078 QMessageBox::StandardButton button = QMessageBox::question(
this,
1079 tr(
"Laser scans modified"),
1080 tr(
"%1 laser scans are modified, do you want to "
1081 "save them? This will overwrite laser scans saved in the database.")
1083 QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
1084 QMessageBox::Cancel);
1086 if(button == QMessageBox::Yes)
1095 if(button != QMessageBox::Yes && button != QMessageBox::No)
1124 ui_->graphViewer->clearAll();
1126 ui_->menuEdit->setEnabled(
false);
1127 ui_->actionGenerate_3D_map_pcd->setEnabled(
false);
1128 ui_->actionExport->setEnabled(
false);
1129 ui_->actionExtract_images->setEnabled(
false);
1130 ui_->menuExport_poses->setEnabled(
false);
1131 ui_->menuExport_GPS->setEnabled(
false);
1132 ui_->actionPoses_KML->setEnabled(
false);
1133 ui_->actionEdit_optimized_2D_map->setEnabled(
false);
1134 ui_->actionExport_saved_2D_map->setEnabled(
false);
1135 ui_->actionImport_2D_map->setEnabled(
false);
1136 ui_->actionRegenerate_optimized_2D_map->setEnabled(
false);
1137 ui_->actionView_optimized_mesh->setEnabled(
false);
1138 ui_->actionExport_optimized_mesh->setEnabled(
false);
1139 ui_->actionUpdate_optimized_mesh->setEnabled(
false);
1140 ui_->checkBox_showOptimized->setEnabled(
false);
1141 ui_->toolBox_statistics->clear();
1143 ui_->checkBox_alignPosesWithGPS->setVisible(
false);
1144 ui_->checkBox_alignPosesWithGPS->setEnabled(
false);
1145 ui_->checkBox_alignPosesWithGroundTruth->setVisible(
false);
1146 ui_->checkBox_alignPosesWithGroundTruth->setEnabled(
false);
1147 ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(
false);
1148 ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(
false);
1149 ui_->doubleSpinBox_optimizationScale->setVisible(
false);
1150 ui_->label_scale_title->setVisible(
false);
1151 ui_->label_rmse->setVisible(
false);
1152 ui_->label_rmse_title->setVisible(
false);
1153 ui_->checkBox_ignoreIntermediateNodes->setVisible(
false);
1154 ui_->label_ignoreINtermediateNdoes->setVisible(
false);
1155 ui_->label_alignPosesWithGPS->setVisible(
false);
1156 ui_->label_alignPosesWithGroundTruth->setVisible(
false);
1157 ui_->label_alignScansCloudsWithGroundTruth->setVisible(
false);
1158 ui_->label_optimizeFrom->setText(tr(
"Root"));
1159 ui_->textEdit_info->clear();
1161 ui_->pushButton_refine->setEnabled(
false);
1162 ui_->pushButton_add->setEnabled(
false);
1163 ui_->pushButton_reset->setEnabled(
false);
1164 ui_->pushButton_reject->setEnabled(
false);
1166 ui_->horizontalSlider_loops->setEnabled(
false);
1167 ui_->horizontalSlider_loops->setMaximum(0);
1168 ui_->horizontalSlider_iterations->setEnabled(
false);
1169 ui_->horizontalSlider_iterations->setMaximum(0);
1170 ui_->horizontalSlider_neighbors->setEnabled(
false);
1171 ui_->horizontalSlider_neighbors->setMaximum(0);
1172 ui_->label_constraint->clear();
1173 ui_->label_constraint_opt->clear();
1174 ui_->label_variance->clear();
1175 ui_->lineEdit_covariance->clear();
1176 ui_->label_type->clear();
1177 ui_->label_type_name->clear();
1178 ui_->checkBox_showOptimized->setEnabled(
false);
1180 ui_->horizontalSlider_A->setEnabled(
false);
1181 ui_->horizontalSlider_A->setMaximum(0);
1182 ui_->horizontalSlider_B->setEnabled(
false);
1183 ui_->horizontalSlider_B->setMaximum(0);
1184 ui_->label_idA->setText(
"NaN");
1185 ui_->label_idB->setText(
"NaN");
1189 ui_->spinBox_indexA->setEnabled(
false);
1190 ui_->spinBox_indexA->setMaximum(0);
1191 ui_->spinBox_indexB->setEnabled(
false);
1192 ui_->spinBox_indexB->setMaximum(0);
1203 ui_->graphViewer->clearAll();
1204 ui_->label_loopClosures->clear();
1205 ui_->label_timeOptimization->clear();
1206 ui_->label_pathLength->clear();
1207 ui_->label_poses->clear();
1208 ui_->label_rmse->clear();
1209 ui_->spinBox_optimizationsFrom->setEnabled(
false);
1211 ui_->graphicsView_A->clear();
1212 ui_->graphicsView_B->clear();
1214 ui_->graphicsView_stereo->clear();
1218 ui_->toolBox_statistics->clear();
1234 QString
path = QFileDialog::getOpenFileName(
this, tr(
"Select file"),
pathDatabase_, tr(
"Databases (*.db)"));
1239 QMessageBox::information(
this,
"Database recovery", tr(
"The selected database is already opened, close it first."));
1242 std::string errorMsg;
1244 progressDialog->setAttribute(Qt::WA_DeleteOnClose);
1246 progressDialog->show();
1251 QMessageBox::information(
this,
"Database recovery", tr(
"Database \"%1\" recovered! Try opening it again.").
arg(
path));
1255 QMessageBox::warning(
this,
"Database recovery", tr(
"Database recovery failed: \"%1\".").
arg(errorMsg.c_str()));
1265 if(this->isWindowModified())
1267 QMessageBox::Button
b=QMessageBox::question(
this,
1268 tr(
"Database Viewer"),
1269 tr(
"There are unsaved changed settings. Save them?"),
1270 QMessageBox::Save | QMessageBox::Cancel | QMessageBox::Discard);
1271 if(
b == QMessageBox::Save)
1275 else if(
b != QMessageBox::Discard)
1294 if(event->isAccepted())
1296 ui_->toolBox_statistics->closeFigures();
1307 this->setWindowModified(
false);
1317 if(this->isVisible())
1330 if(this->isVisible())
1339 if((event->modifiers() & Qt::ControlModifier) && event->key() == Qt::Key_S)
1347 if (event->type() == QEvent::Resize && qobject_cast<QDockWidget*>(obj))
1349 this->setWindowModified(
true);
1351 return QWidget::eventFilter(obj, event);
1375 double previousStamp = 0;
1376 std::vector<double> delays(
ids_.size());
1378 std::map<int, Transform> poses;
1379 std::map<int, double> stamps;
1380 std::map<int, Transform> groundTruths;
1381 std::map<int, GPS> gpsValues;
1382 std::map<int, EnvSensors> sensorsValues;
1383 for(
int i=0;
i<
ids_.size();
i+=1+framesIgnored)
1395 if(frameRate == 0 ||
1396 previousStamp == 0 ||
1398 stamp - previousStamp >= 1.0/frameRate)
1400 if(sessionExported < 0 || sessionExported == mapId)
1402 ids.push_back(
ids_[
i]);
1404 if(previousStamp && stamp)
1406 delays[oi++] = stamp - previousStamp;
1408 previousStamp = stamp;
1410 poses.insert(std::make_pair(
ids_[
i], odomPose));
1411 stamps.insert(std::make_pair(
ids_[
i], stamp));
1412 groundTruths.insert(std::make_pair(
ids_[
i], groundTruth));
1413 if(gps.
stamp() > 0.0)
1415 gpsValues.insert(std::make_pair(
ids_[
i], gps));
1419 sensorsValues.insert(std::make_pair(
ids_[
i], sensors));
1423 if(sessionExported >= 0 && mapId > sessionExported)
1434 progressDialog->setAttribute(Qt::WA_DeleteOnClose);
1436 progressDialog->show();
1438 UINFO(
"Decompress: rgb=%d depth=%d scan=%d userData=%d",
1444 for(
int i=0;
i<ids.size() && !progressDialog->
isCanceled(); ++
i)
1450 cv::Mat depth, rgb, userData;
1452 data.uncompressDataConst(
1457 cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
1460 std::multimap<int, Link> links;
1462 if(links.size() && links.begin()->first <
id)
1464 covariance = links.begin()->second.infMatrix().inv();
1469 if(
data.cameraModels().size())
1475 data.cameraModels(),
1486 data.stereoCameraModels(),
1491 if(groundTruths.find(
id)!=groundTruths.end())
1495 if(gpsValues.find(
id)!=gpsValues.end())
1497 sensorData.
setGPS(gpsValues.at(
id));
1499 if(sensorsValues.find(
id)!=sensorsValues.end())
1508 QApplication::processEvents();
1513 progressDialog->
appendText(tr(
"Average frame rate=%1 Hz (Min=%2, Max=%3)")
1520 UERROR(
"DataRecorder init failed?!");
1525 QMessageBox::warning(
this, tr(
"Cannot export database"), tr(
"An output path must be set!"));
1537 QStringList formats;
1538 formats.push_back(
"id.jpg");
1539 formats.push_back(
"id.png");
1540 formats.push_back(
"timestamp.jpg");
1541 formats.push_back(
"timestamp.png");
1543 QString
format = QInputDialog::getItem(
this, tr(
"Which RGB format?"), tr(
"Format:"), formats, 0,
false, &ok);
1549 QString ext =
format.split(
'.').back();
1550 bool useStamp =
format.split(
'.').front().compare(
"timestamp") == 0;
1551 bool directoriesCreated =
false;
1552 QString
path = QFileDialog::getExistingDirectory(
this, tr(
"Select directory where to save images..."),
pathDatabase_);
1556 progressDialog->setAttribute(Qt::WA_DeleteOnClose);
1560 progressDialog->show();
1562 int imagesExported = 0;
1563 for(
int i=0;
i<
ids_.size(); ++
i)
1565 QString
id = QString::number(
ids_.at(
i));
1569 data.uncompressData();
1571 if(!directoriesCreated)
1574 if(!
data.imageRaw().empty() && !
data.rightRaw().empty())
1577 dir.mkdir(QString(
"%1/left").
arg(
path));
1578 dir.mkdir(QString(
"%1/right").
arg(
path));
1579 dir.mkdir(QString(
"%1/calib").
arg(
path));
1580 directoriesCreated =
true;
1582 else if(!
data.imageRaw().empty())
1584 if(!
data.depthRaw().empty())
1587 dir.mkdir(QString(
"%1/rgb").
arg(
path));
1588 dir.mkdir(QString(
"%1/depth").
arg(
path));
1589 dir.mkdir(QString(
"%1/calib").
arg(
path));
1590 directoriesCreated =
true;
1595 if(!
data.imageRaw().empty() && useStamp)
1601 std::vector<float>
v;
1607 UWARN(
"Node %d has null timestamp! Using id instead!",
ids_.at(
i));
1611 id = QString::number(stamp,
'f');
1615 if(!
data.imageRaw().empty())
1617 if(!
data.rightRaw().empty())
1619 if(!cv::imwrite(QString(
"%1/left/%2.%3").
arg(
path).
arg(
id).
arg(ext).toStdString(),
data.imageRaw()))
1621 if(!cv::imwrite(QString(
"%1/right/%2.%3").
arg(
path).
arg(
id).
arg(ext).toStdString(),
data.rightRaw()))
1623 UINFO(QString(
"Saved left/%1.%2 and right/%1.%2").
arg(
id).
arg(ext).toStdString().
c_str());
1627 UERROR(
"Cannot save calibration file, database name is empty!");
1629 else if(
data.stereoCameraModels().size()>=1 &&
data.stereoCameraModels().front().isValidForProjection())
1631 for(
size_t i=0;
i<
data.stereoCameraModels().
size(); ++
i)
1633 std::string cameraName =
id.toStdString();
1634 if(
data.stereoCameraModels().size()>1)
1640 data.imageRaw().size(),
1641 data.stereoCameraModels()[
i].left().K_raw(),
1642 data.stereoCameraModels()[
i].left().D_raw(),
1643 data.stereoCameraModels()[
i].left().R(),
1644 data.stereoCameraModels()[
i].left().P(),
1645 data.rightRaw().size(),
1646 data.stereoCameraModels()[
i].right().K_raw(),
1647 data.stereoCameraModels()[
i].right().D_raw(),
1648 data.stereoCameraModels()[
i].right().R(),
1649 data.stereoCameraModels()[
i].right().P(),
1650 data.stereoCameraModels()[
i].R(),
1651 data.stereoCameraModels()[
i].T(),
1652 data.stereoCameraModels()[
i].E(),
1653 data.stereoCameraModels()[
i].F(),
1654 data.stereoCameraModels()[
i].left().localTransform());
1655 if(
model.save(
path.toStdString() +
"/calib"))
1657 UINFO(
"Saved stereo calibration \"%s\"", (
path.toStdString()+
"/calib/"+cameraName+
".yaml").c_str());
1661 UERROR(
"Failed saving calibration \"%s\"", (
path.toStdString()+
"/calib/"+cameraName+
".yaml").c_str());
1668 if(!
data.depthRaw().empty())
1670 if(!cv::imwrite(QString(
"%1/rgb/%2.%3").
arg(
path).
arg(
id).
arg(ext).toStdString(),
data.imageRaw()))
1673 UWARN(
"Failed saving \"%s\"", QString(
"%1/depth/%2.png").
arg(
path).
arg(
id).toStdString().
c_str());
1674 UINFO(QString(
"Saved rgb/%1.%2 and depth/%1.png").
arg(
id).
arg(ext).toStdString().
c_str());
1678 if(!cv::imwrite(QString(
"%1/%2.%3").
arg(
path).
arg(
id).
arg(ext).toStdString(),
data.imageRaw()))
1686 UERROR(
"Cannot save calibration file, database name is empty!");
1688 else if(
data.cameraModels().size() >= 1 &&
data.cameraModels().front().isValidForProjection())
1690 for(
size_t i=0;
i<
data.cameraModels().
size(); ++
i)
1692 std::string cameraName =
id.toStdString();
1693 if(
data.cameraModels().size()>1)
1698 data.imageRaw().size(),
1699 data.cameraModels()[
i].K_raw(),
1700 data.cameraModels()[
i].D_raw(),
1701 data.cameraModels()[
i].R(),
1702 data.cameraModels()[
i].P(),
1703 data.cameraModels()[
i].localTransform());
1704 std::string dirPrefix =
"";
1705 if(!
data.depthRaw().empty())
1707 dirPrefix =
"/calib";
1709 if(
model.save(
path.toStdString()+dirPrefix))
1711 UINFO(
"Saved calibration \"%s\"", (
path.toStdString()+dirPrefix+
"/"+cameraName+
".yaml").c_str());
1715 UERROR(
"Failed saving calibration \"%s\"", (
path.toStdString()+
"/"+cameraName+
".yaml").c_str());
1723 QApplication::processEvents();
1733 QMessageBox::information(
this, tr(
"Exporting"), tr(
"%1 images exported!").
arg(imagesExported));
1745 progressDialog->setAttribute(Qt::WA_DeleteOnClose);
1746 int progressSteps = 5;
1747 if(
ui_->graphViewer->isVisible() ||
ui_->dockWidget_occupancyGridView->isVisible())
1751 if(
ui_->textEdit_info->isVisible())
1755 if(
ui_->toolBox_statistics->isVisible())
1760 progressDialog->show();
1763 progressDialog->
appendText(tr(
"Loading all ids..."));
1764 QApplication::processEvents();
1766 QApplication::processEvents();
1768 UINFO(
"Loading all IDs...");
1771 #if QT_VERSION >= QT_VERSION_CHECK(5, 15, 3)
1772 ids_ = QList<int>(ids.begin(), ids.end());
1774 ids_ = QList<int>::fromStdList(std::list<int>(ids.begin(), ids.end()));
1787 ui_->checkBox_wmState->setVisible(
false);
1788 ui_->checkBox_alignPosesWithGPS->setVisible(
false);
1789 ui_->checkBox_alignPosesWithGPS->setEnabled(
false);
1790 ui_->checkBox_alignPosesWithGroundTruth->setVisible(
false);
1791 ui_->checkBox_alignPosesWithGroundTruth->setEnabled(
false);
1792 ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(
false);
1793 ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(
false);
1794 ui_->doubleSpinBox_optimizationScale->setVisible(
false);
1795 ui_->label_scale_title->setVisible(
false);
1796 ui_->label_rmse->setVisible(
false);
1797 ui_->label_rmse_title->setVisible(
false);
1798 ui_->checkBox_ignoreIntermediateNodes->setVisible(
false);
1799 ui_->label_ignoreINtermediateNdoes->setVisible(
false);
1800 ui_->label_alignPosesWithGPS->setVisible(
false);
1801 ui_->label_alignPosesWithGroundTruth->setVisible(
false);
1802 ui_->label_alignScansCloudsWithGroundTruth->setVisible(
false);
1803 ui_->toolButton_edit_priorA->setVisible(
false);
1804 ui_->toolButton_edit_priorB->setVisible(
false);
1805 ui_->toolButton_remove_priorA->setVisible(
false);
1806 ui_->toolButton_remove_priorB->setVisible(
false);
1807 ui_->menuEdit->setEnabled(
true);
1808 ui_->actionGenerate_3D_map_pcd->setEnabled(
true);
1809 ui_->actionExport->setEnabled(
true);
1810 ui_->actionExtract_images->setEnabled(
true);
1811 ui_->menuExport_poses->setEnabled(
false);
1812 ui_->menuExport_GPS->setEnabled(
false);
1813 ui_->actionPoses_KML->setEnabled(
false);
1814 ui_->actionEdit_optimized_2D_map->setEnabled(
false);
1815 ui_->actionExport_saved_2D_map->setEnabled(
false);
1816 ui_->actionImport_2D_map->setEnabled(
false);
1817 ui_->actionRegenerate_optimized_2D_map->setEnabled(
false);
1818 ui_->actionView_optimized_mesh->setEnabled(
false);
1819 ui_->actionExport_optimized_mesh->setEnabled(
false);
1825 ui_->toolBox_statistics->clear();
1826 ui_->label_optimizeFrom->setText(tr(
"Root"));
1828 progressDialog->
appendText(tr(
"%1 ids loaded!").
arg(ids.size()));
1830 progressDialog->
appendText(tr(
"Loading all links..."));
1831 QApplication::processEvents();
1833 QApplication::processEvents();
1835 std::multimap<int, Link> unilinks;
1837 UDEBUG(
"%d total links loaded", (
int)unilinks.size());
1839 std::multimap<int, Link> links;
1840 for(std::multimap<int, Link>::iterator
iter=unilinks.begin();
iter!=unilinks.end(); ++
iter)
1842 links.insert(*
iter);
1845 links.insert(std::make_pair(
iter->second.to(),
iter->second.inverse()));
1849 progressDialog->
appendText(tr(
"%1 links loaded!").
arg(unilinks.size()));
1851 progressDialog->
appendText(
"Loading Working Memory state...");
1852 QApplication::processEvents();
1854 QApplication::processEvents();
1860 double previousStamp = 0.0;
1864 progressDialog->
appendText(
"Loading Working Memory state... done!");
1866 progressDialog->
appendText(
"Loading info for all nodes...");
1867 QApplication::processEvents();
1869 QApplication::processEvents();
1871 int lastValidNodeId = 0;
1872 for(
int i=0;
i<
ids_.size(); ++
i)
1881 std::vector<float>
v;
1894 lastValidNodeId =
ids_[
i];
1898 if(wmStates.find(
ids_[
i]) != wmStates.end())
1904 ui_->checkBox_ignoreIntermediateNodes->setVisible(
true);
1905 ui_->label_ignoreINtermediateNdoes->setVisible(
true);
1911 if(!
p.isNull() && !previousPose.
isNull())
1916 if(previousStamp > 0.0 &&
s > 0.0)
1930 for(std::multimap<int, Link>::iterator jter=links.find(
ids_[
i]); jter!=links.end() && jter->first ==
ids_[
i]; ++jter)
1937 std::multimap<int, Link>::iterator invertedLinkIter =
graph::findLink(links, jter->second.to(), jter->second.from(),
false, jter->second.type());
1938 if( jter->second.isValid() &&
1939 ids.find(jter->second.from()) != ids.end() &&
1940 (ids.find(jter->second.to()) != ids.end() || jter->second.to()<0) &&
1942 invertedLinkIter != links.end() &&
1946 if(jter->second.userDataCompressed().cols == 0 &&
1947 invertedLinkIter->second.userDataCompressed().cols != 0)
1949 links_.insert(std::make_pair(invertedLinkIter->second.from(), invertedLinkIter->second));
1953 links_.insert(std::make_pair(
ids_[
i], jter->second));
1963 if(gps.
stamp() > 0.0)
1967 cv::Point3f
p(0.0
f,0.0
f,0.0
f);
1981 progressDialog->
appendText(
"Loading info for all nodes... done!");
1983 progressDialog->
appendText(
"Loading optimized poses and maps...");
1984 QApplication::processEvents();
1986 QApplication::processEvents();
1993 ui_->checkBox_alignPosesWithGPS->setVisible(!
gpsPoses_.empty());
1994 ui_->checkBox_alignPosesWithGPS->setEnabled(!
gpsPoses_.empty());
1995 ui_->label_alignPosesWithGPS->setVisible(!
gpsPoses_.empty());
2005 ui_->menuExport_GPS->setEnabled(
true);
2006 ui_->actionPoses_KML->setEnabled(
true);
2009 float xMin, yMin, cellSize;
2011 ui_->actionEdit_optimized_2D_map->setEnabled(hasMap);
2012 ui_->actionExport_saved_2D_map->setEnabled(hasMap);
2013 ui_->actionImport_2D_map->setEnabled(hasMap);
2018 ui_->actionView_optimized_mesh->setEnabled(
true);
2019 ui_->actionExport_optimized_mesh->setEnabled(
true);
2024 progressDialog->
appendText(
"Loading optimized poses and maps... done!");
2026 QApplication::processEvents();
2028 QApplication::processEvents();
2030 if(
ids_.size() &&
ui_->toolBox_statistics->isVisible())
2032 progressDialog->
appendText(
"Loading statistics...");
2033 QApplication::processEvents();
2035 QApplication::processEvents();
2037 UINFO(
"Update statistics...");
2040 progressDialog->
appendText(
"Loading statistics... done!");
2042 QApplication::processEvents();
2044 QApplication::processEvents();
2048 ui_->textEdit_info->clear();
2049 if(
ui_->textEdit_info->isVisible())
2051 progressDialog->
appendText(
"Update database info...");
2052 QApplication::processEvents();
2054 QApplication::processEvents();
2058 progressDialog->
appendText(
"Update database info... done!");
2060 QApplication::processEvents();
2062 QApplication::processEvents();
2069 bool nullPoses =
odomPoses_.begin()->second.isNull();
2072 if((!
iter->second.isNull() && nullPoses) ||
2073 (
iter->second.isNull() && !nullPoses))
2075 if(
iter->second.isNull())
2079 UWARN(
"Mixed valid and null poses! Ignoring graph...");
2094 ui_->spinBox_optimizationsFrom->setValue(
odomPoses_.begin()->first);
2099 if(lastValidNodeId>0)
2104 std::map<int, rtabmap::Transform> posesOut;
2105 std::multimap<int, rtabmap::Link> linksOut;
2106 UINFO(
"Get connected graph from %d (%d poses, %d links)", lastValidNodeId, (
int)
odomPoses_.size(), (
int)
links_.size());
2114 if(!posesOut.empty())
2116 bool optimizeFromGraphEnd = Parameters::defaultRGBDOptimizeFromGraphEnd();
2118 if(optimizeFromGraphEnd)
2120 ui_->spinBox_optimizationsFrom->setValue(posesOut.rbegin()->first);
2124 ui_->spinBox_optimizationsFrom->setValue(posesOut.lower_bound(1)->first);
2138 ui_->spinBox_indexA->setMinimum(0);
2139 ui_->spinBox_indexB->setMinimum(0);
2140 ui_->spinBox_indexA->setMaximum(
ids_.size()-1);
2141 ui_->spinBox_indexB->setMaximum(
ids_.size()-1);
2142 ui_->spinBox_indexA->setEnabled(
true);
2143 ui_->spinBox_indexB->setEnabled(
true);
2145 ui_->horizontalSlider_A->setMinimum(0);
2146 ui_->horizontalSlider_B->setMinimum(0);
2147 ui_->horizontalSlider_A->setMaximum(
ids_.size()-1);
2148 ui_->horizontalSlider_B->setMaximum(
ids_.size()-1);
2149 ui_->horizontalSlider_A->setEnabled(
true);
2150 ui_->horizontalSlider_B->setEnabled(
true);
2151 ui_->horizontalSlider_A->setSliderPosition(0);
2152 ui_->horizontalSlider_B->setSliderPosition(0);
2158 ui_->horizontalSlider_A->setEnabled(
false);
2159 ui_->horizontalSlider_B->setEnabled(
false);
2161 ui_->spinBox_indexA->setEnabled(
false);
2162 ui_->spinBox_indexB->setEnabled(
false);
2164 ui_->label_idA->setText(
"NaN");
2165 ui_->label_idB->setText(
"NaN");
2172 if(
ui_->graphViewer->isVisible() ||
ui_->dockWidget_occupancyGridView->isVisible())
2174 progressDialog->
appendText(
"Updating Graph View...");
2175 QApplication::processEvents();
2177 QApplication::processEvents();
2181 progressDialog->
appendText(
"Updating Graph View... done!");
2183 QApplication::processEvents();
2185 QApplication::processEvents();
2193 UINFO(
"Update database info...");
2196 if(
ui_->textEdit_info->toPlainText().isEmpty())
2203 ui_->textEdit_info->append(tr(
"Total odometry length:\t%1 m (approx. as graph has been reduced)").
arg(
infoTotalOdom_));
2210 int lastWordIdId = 0;
2217 ids.insert(lastWordIdId);
2218 std::list<VisualWord *> vws;
2222 wordsDim = vws.front()->getDescriptor().cols;
2223 wordsType = vws.front()->getDescriptor().type();
2229 ui_->textEdit_info->append(tr(
"Total time:\t\t%1").
arg(QDateTime::fromMSecsSinceEpoch(
infoTotalTime_*1000).toUTC().toString(
"hh:mm:ss.zzz")));
2235 ui_->textEdit_info->append(
"");
2239 ui_->textEdit_info->append(tr(
"Database size:\t%1 %2").
arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).
arg(mem>1000000?
"MB":mem>1000?
"KB":
"Bytes"));
2242 ui_->textEdit_info->append(tr(
"Nodes size:\t\t%1 %2\t%3%").
arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).
arg(mem>1000000?
"MB":mem>1000?
"KB":
"Bytes").
arg(dbSize>0?QString::number(
double(mem)/
double(dbSize)*100.0,
'f', 2 ):
"0"));
2245 ui_->textEdit_info->append(tr(
"Links size:\t\t%1 %2\t%3%").
arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).
arg(mem>1000000?
"MB":mem>1000?
"KB":
"Bytes").
arg(dbSize>0?QString::number(
double(mem)/
double(dbSize)*100.0,
'f', 2 ):
"0"));
2248 ui_->textEdit_info->append(tr(
"RGB Images size:\t%1 %2\t%3%").
arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).
arg(mem>1000000?
"MB":mem>1000?
"KB":
"Bytes").
arg(dbSize>0?QString::number(
double(mem)/
double(dbSize)*100.0,
'f', 2 ):
"0"));
2251 ui_->textEdit_info->append(tr(
"Depth Images size:\t%1 %2\t%3%").
arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).
arg(mem>1000000?
"MB":mem>1000?
"KB":
"Bytes").
arg(dbSize>0?QString::number(
double(mem)/
double(dbSize)*100.0,
'f', 2 ):
"0"));
2254 ui_->textEdit_info->append(tr(
"Calibrations size:\t%1 %2\t%3%").
arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).
arg(mem>1000000?
"MB":mem>1000?
"KB":
"Bytes").
arg(dbSize>0?QString::number(
double(mem)/
double(dbSize)*100.0,
'f', 2 ):
"0"));
2257 ui_->textEdit_info->append(tr(
"Grids size:\t\t%1 %2\t%3%").
arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).
arg(mem>1000000?
"MB":mem>1000?
"KB":
"Bytes").
arg(dbSize>0?QString::number(
double(mem)/
double(dbSize)*100.0,
'f', 2 ):
"0"));
2260 ui_->textEdit_info->append(tr(
"Scans size:\t\t%1 %2\t%3%").
arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).
arg(mem>1000000?
"MB":mem>1000?
"KB":
"Bytes").
arg(dbSize>0?QString::number(
double(mem)/
double(dbSize)*100.0,
'f', 2 ):
"0"));
2263 ui_->textEdit_info->append(tr(
"User data size:\t%1 %2\t%3%").
arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).
arg(mem>1000000?
"MB":mem>1000?
"KB":
"Bytes").
arg(dbSize>0?QString::number(
double(mem)/
double(dbSize)*100.0,
'f', 2 ):
"0"));
2266 ui_->textEdit_info->append(tr(
"Dictionary size:\t%1 %2\t%3%").
arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).
arg(mem>1000000?
"MB":mem>1000?
"KB":
"Bytes").
arg(dbSize>0?QString::number(
double(mem)/
double(dbSize)*100.0,
'f', 2 ):
"0"));
2269 ui_->textEdit_info->append(tr(
"Features size:\t%1 %2\t%3%").
arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).
arg(mem>1000000?
"MB":mem>1000?
"KB":
"Bytes").
arg(dbSize>0?QString::number(
double(mem)/
double(dbSize)*100.0,
'f', 2 ):
"0"));
2272 ui_->textEdit_info->append(tr(
"Statistics size:\t%1 %2\t%3%").
arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).
arg(mem>1000000?
"MB":mem>1000?
"KB":
"Bytes").
arg(dbSize>0?QString::number(
double(mem)/
double(dbSize)*100.0,
'f', 2 ):
"0"));
2273 mem = dbSize - total;
2274 ui_->textEdit_info->append(tr(
"Other (indexing, unused):\t%1 %2\t%3%").
arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).
arg(mem>1000000?
"MB":mem>1000?
"KB":
"Bytes").
arg(dbSize>0?QString::number(
double(mem)/
double(dbSize)*100.0,
'f', 2 ):
"0"));
2275 ui_->textEdit_info->append(
"");
2276 std::set<int> idsWithoutBad;
2278 int infoBadcountInLTM = 0;
2279 int infoBadCountInGraph = 0;
2280 for(
int i=0;
i<
ids_.size(); ++
i)
2282 if(idsWithoutBad.find(
ids_[
i]) == idsWithoutBad.end())
2284 ++infoBadcountInLTM;
2287 ++infoBadCountInGraph;
2291 ui_->textEdit_info->append(tr(
"%1 bad signatures in LTM").
arg(infoBadcountInLTM));
2292 ui_->textEdit_info->append(tr(
"%1 bad signatures in the global graph").
arg(infoBadCountInGraph));
2293 ui_->textEdit_info->append(
"");
2295 QFontMetrics metrics(
ui_->textEdit_info->font());
2296 #if QT_VERSION >= QT_VERSION_CHECK(5, 15, 0)
2297 int tabW =
ui_->textEdit_info->tabStopDistance();
2299 int tabW =
ui_->textEdit_info->tabStopWidth();
2301 for(ParametersMap::iterator
iter=parameters.begin();
iter!=parameters.end(); ++
iter)
2303 #if QT_VERSION >= QT_VERSION_CHECK(5, 15, 0)
2304 int strW = metrics.horizontalAdvance(QString(
iter->first.c_str()) +
"=");
2306 int strW = metrics.width(QString(
iter->first.c_str()) +
"=");
2308 ui_->textEdit_info->append(tr(
"%1=%2%3")
2310 .
arg(strW < tabW?
"\t\t\t\t":strW < tabW*2?
"\t\t\t":strW < tabW*3?
"\t\t":
"\t")
2311 .arg(
iter->second.c_str()));
2315 ui_->textEdit_info->moveCursor(QTextCursor::Start) ;
2316 ui_->textEdit_info->ensureCursorVisible() ;
2321 ui_->textEdit_info->clear();
2330 ui_->toolBox_statistics->clear();
2331 double firstStamp = 0.0;
2334 std::map<std::string, std::pair<std::vector<qreal>, std::vector<qreal> > > allData;
2335 std::map<std::string, int > allDataOi;
2337 for(std::map<
int, std::pair<std::map<std::string, float>,
double> >::
iterator jter=allStats.begin(); jter!=allStats.end(); ++jter)
2339 double stamp=jter->second.second;
2340 std::map<std::string, float> & statistics = jter->second.first;
2345 for(std::map<std::string, float>::iterator
iter=statistics.begin();
iter!=statistics.end(); ++
iter)
2347 if(allData.find(
iter->first) == allData.end())
2350 allData.insert(std::make_pair(
iter->first, std::make_pair(std::vector<qreal>(allStats.size(), 0.0f), std::vector<qreal>(allStats.size(), 0.0f) )));
2351 allDataOi.insert(std::make_pair(
iter->first, 0));
2354 int & oi = allDataOi.at(
iter->first);
2355 allData.at(
iter->first).first[oi] =
ui_->checkBox_timeStats->isChecked()?qreal(stamp-firstStamp):jter->first;
2356 allData.at(
iter->first).second[oi] =
iter->second;
2361 for(std::map<std::string, std::pair<std::vector<qreal>, std::vector<qreal> > >::
iterator iter=allData.begin();
iter!=allData.end(); ++
iter)
2363 int oi = allDataOi.at(
iter->first);
2364 iter->second.first.resize(oi);
2365 iter->second.second.resize(oi);
2366 ui_->toolBox_statistics->updateStat(
iter->first.c_str(),
iter->second.first,
iter->second.second,
true);
2374 QColor
c = QColorDialog::getColor(
ui_->lineEdit_obstacleColor->text(),
this);
2377 ui_->lineEdit_obstacleColor->setText(
c.name());
2382 QColor
c = QColorDialog::getColor(
ui_->lineEdit_groundColor->text(),
this);
2385 ui_->lineEdit_groundColor->setText(
c.name());
2390 QColor
c = QColorDialog::getColor(
ui_->lineEdit_emptyColor->text(),
this);
2393 ui_->lineEdit_emptyColor->setText(
c.name());
2398 QColor
c = QColorDialog::getColor(
ui_->lineEdit_frontierColor->text(),
this);
2401 ui_->lineEdit_frontierColor->setText(
c.name());
2415 data.uncompressData();
2416 if(!
data.depthRaw().empty())
2470 types.push_back(
"Map's graph (see Graph View)");
2471 types.push_back(
"Odometry");
2474 types.push_back(
"Ground Truth");
2477 QString
type = QInputDialog::getItem(
this, tr(
"Which poses?"), tr(
"Poses:"), types, 0,
false, &ok);
2483 bool groundTruth =
type.compare(
"Ground Truth") == 0;
2487 QMessageBox::warning(
this, tr(
"Cannot export poses"), tr(
"No ground truth poses in database?!"));
2493 if(
graphes_.empty() ||
ui_->horizontalSlider_iterations->maximum() != (
int)
graphes_.size()-1)
2495 QMessageBox::warning(
this, tr(
"Cannot export poses"), tr(
"No graph in database?!"));
2501 QMessageBox::warning(
this, tr(
"Cannot export poses"), tr(
"No odometry poses in database?!"));
2507 QMessageBox::warning(
this, tr(
"Cannot export poses in KML format"), tr(
"No GPS in database?!"));
2511 std::map<int, Transform> optimizedPoses;
2526 (
ui_->checkBox_alignPosesWithGPS->isEnabled() &&
2527 ui_->checkBox_alignPosesWithGPS->isChecked()) ||
2531 (
ui_->checkBox_alignPosesWithGroundTruth->isEnabled() &&
ui_->checkBox_alignPosesWithGroundTruth->isChecked()))
2542 float translational_rmse = 0.0f;
2543 float translational_mean = 0.0f;
2544 float translational_median = 0.0f;
2545 float translational_std = 0.0f;
2546 float translational_min = 0.0f;
2547 float translational_max = 0.0f;
2548 float rotational_rmse = 0.0f;
2549 float rotational_mean = 0.0f;
2550 float rotational_median = 0.0f;
2551 float rotational_std = 0.0f;
2552 float rotational_min = 0.0f;
2553 float rotational_max = 0.0f;
2560 translational_median,
2574 for(std::map<int, Transform>::iterator
iter=optimizedPoses.begin();
iter!=optimizedPoses.end(); ++
iter)
2576 iter->second = gtToMap *
iter->second;
2578 if(alignToGPS &&
format != 5 && optimizedPoses.find(
gpsValues_.begin()->first)!=optimizedPoses.end())
2582 Transform offset = optimizedPoses.at(originId).translation().inverse();
2583 for(std::map<int, Transform>::iterator
iter=optimizedPoses.begin();
iter!=optimizedPoses.end(); ++
iter)
2594 std::map<int, GPS>
values;
2596 for(std::map<int, Transform>::iterator
iter=optimizedPoses.begin();
iter!=optimizedPoses.end(); ++
iter)
2606 std::vector<float>
v;
2613 QString output =
pathDatabase_ + QDir::separator() +
"poses.kml";
2614 QString
path = QFileDialog::getSaveFileName(
2618 tr(
"Google Earth file (*.kml)"));
2626 QMessageBox::information(
this,
2627 tr(
"Export poses..."),
2628 tr(
"GPS coordinates saved to \"%1\".")
2633 QMessageBox::information(
this,
2634 tr(
"Export poses..."),
2635 tr(
"Failed to save GPS coordinates to \"%1\"!")
2642 if(optimizedPoses.size())
2644 std::map<int, Transform> localTransforms;
2646 items.push_back(
"Robot (base frame)");
2647 items.push_back(
"Camera");
2648 items.push_back(
"Scan");
2650 QString item = QInputDialog::getItem(
this, tr(
"Export Poses"), tr(
"Frame: "), items, 0,
false, &ok);
2651 if(!ok || item.isEmpty())
2655 if(item.compare(
"Robot (base frame)") != 0)
2657 bool cameraFrame = item.compare(
"Camera") == 0;
2658 for(std::map<int, Transform>::iterator
iter=optimizedPoses.begin();
iter!=optimizedPoses.end(); ++
iter)
2663 std::vector<CameraModel> models;
2664 std::vector<StereoCameraModel> stereoModels;
2667 if((models.size() == 1 &&
2668 !models.at(0).localTransform().isNull()))
2670 localTransform = models.at(0).localTransform();
2672 else if(stereoModels.size() == 1 &&
2673 !stereoModels[0].localTransform().isNull())
2675 localTransform = stereoModels[0].localTransform();
2677 else if(models.size()>1)
2679 UWARN(
"Multi-camera is not supported (node %d)",
iter->first);
2683 UWARN(
"Calibration not valid for node %d",
iter->first);
2688 UWARN(
"Missing calibration for node %d",
iter->first);
2696 localTransform =
info.localTransform();
2700 UWARN(
"Missing scan info for node %d",
iter->first);
2704 if(!localTransform.
isNull())
2706 localTransforms.insert(std::make_pair(
iter->first, localTransform));
2709 if(localTransforms.empty())
2711 QMessageBox::warning(
this,
2713 tr(
"Could not find any \"%1\" frame, exporting in Robot frame instead.").
arg(item));
2717 std::map<int, Transform> poses;
2718 std::multimap<int, Link> links;
2719 if(localTransforms.empty())
2721 poses = optimizedPoses;
2730 for(std::map<int, Transform>::iterator
iter=localTransforms.begin();
iter!=localTransforms.end(); ++
iter)
2732 poses.insert(std::make_pair(
iter->first, optimizedPoses.at(
iter->first) *
iter->second));
2740 std::multimap<int, Link>::iterator inserted = links.insert(*
iter);
2741 int from =
iter->second.from();
2742 int to =
iter->second.to();
2743 inserted->second.setTransform(localTransforms.at(from).inverse()*
iter->second.transform()*localTransforms.at(to));
2749 if(
format != 4 &&
format != 11 && !poses.empty() && poses.begin()->first<0)
2751 UWARN(
"Only g2o format (4) and RGBD format with ID format can export landmarks, they are ignored with format %d",
format);
2752 for(std::map<int, Transform>::iterator
iter=poses.begin();
iter!=poses.end() &&
iter->first < 0;)
2754 poses.erase(
iter++);
2756 for(std::multimap<int, Link>::iterator
iter=links.begin();
iter!=links.end();)
2758 if(
iter->second.from() < 0 ||
iter->second.to() < 0)
2760 links.erase(
iter++);
2769 std::map<int, double> stamps;
2772 for(std::map<int, Transform>::iterator
iter=poses.begin();
iter!=poses.end(); ++
iter)
2776 stamps.insert(std::make_pair(
iter->first, 0));
2785 std::vector<float>
v;
2790 stamps.insert(std::make_pair(
iter->first, stamp));
2794 if(stamps.size()!=poses.size())
2796 QMessageBox::warning(
this, tr(
"Export poses..."), tr(
"Poses (%1) and stamps (%2) have not the same size! Cannot export in RGB-D SLAM format.")
2797 .
arg(poses.size()).
arg(stamps.size()));
2803 QString suffix =
odometry?
"_odom":groundTruth?
"_gt":
"";
2804 output = output.arg(suffix);
2806 QString
path = QFileDialog::getSaveFileName(
2810 format == 3?tr(
"TORO file (*.graph)"):
format==4?tr(
"g2o file (*.g2o)"):tr(
"Text file (*.txt)"));
2814 if(QFileInfo(
path).suffix() ==
"")
2834 QMessageBox::information(
this,
2835 tr(
"Export poses..."),
2836 tr(
"%1 saved to \"%2\".")
2842 QMessageBox::information(
this,
2843 tr(
"Export poses..."),
2844 tr(
"Failed to save %1 to \"%2\"!")
2866 QString
path = QFileDialog::getSaveFileName(
2870 format==0?tr(
"Raw format (*.txt)"):tr(
"Google Earth file (*.kml)"));
2878 QMessageBox::information(
this,
2879 tr(
"Export poses..."),
2880 tr(
"GPS coordinates saved to \"%1\".")
2885 QMessageBox::information(
this,
2886 tr(
"Export poses..."),
2887 tr(
"Failed to save GPS coordinates to \"%1\"!")
2898 QMessageBox::warning(
this, tr(
"Cannot edit 2D map"), tr(
"A database must must loaded first...\nUse File->Open database."));
2904 QMessageBox::warning(
this, tr(
"Cannot edit 2D map"),
2905 tr(
"The database has too old version (%1) to saved "
2912 QMessageBox::warning(
this, tr(
"Cannot edit 2D map"),
2913 tr(
"The database has modified links and/or modified local "
2914 "occupancy grids, the 2D optimized map cannot be modified."));
2918 float xMin, yMin, cellSize;
2922 QMessageBox::warning(
this, tr(
"Cannot export 2D map"), tr(
"The database doesn't contain a saved 2D map."));
2927 cv::Mat map8UFlip, map8URotated;
2928 cv::flip(map8U, map8UFlip, 0);
2929 if(!
ui_->graphViewer->isOrientationENU())
2932 cv::transpose(map8UFlip, map8URotated);
2933 cv::flip(map8URotated, map8URotated, 0);
2937 map8URotated = map8UFlip;
2946 if(!
ui_->graphViewer->isOrientationENU())
2949 cv::transpose(mapModified, map8URotated);
2950 cv::flip(map8URotated, map8URotated, 1);
2954 map8URotated = mapModified;
2956 cv::flip(map8URotated, map8UFlip, 0);
2958 UASSERT(map8UFlip.type() == map8U.type());
2959 UASSERT(map8UFlip.cols == map8U.cols);
2960 UASSERT(map8UFlip.rows == map8U.rows);
2967 QMessageBox::information(
this, tr(
"Edit 2D map"), tr(
"Map updated!"));
2970 int cropRadius =
ui_->spinBox_cropRadius->value();
2971 QMessageBox::StandardButton
b = QMessageBox::question(
this,
2972 tr(
"Crop empty space"),
2973 tr(
"Do you want to clear empty space from local occupancy grids and laser scans?\n\n"
2975 " * If the map needs to be regenerated in the future (e.g., when we re-use the map in SLAM mode), removed obstacles won't reappear.\n"
2976 " * The cropped laser scans will be also used for localization, so if dynamic obstacles have been removed, localization won't try to match them anymore.\n\n"
2978 " * Cropping the laser scans cannot be reverted after the viewer is closed and changes have been saved.\n\n"
2980 " Crop radius = %1 pixels\n\n"
2981 "Press \"Yes\" to filter only grids.\n"
2982 "Press \"Yes to All\" to filter both grids and laser scans.\n").
arg(cropRadius),
2983 QMessageBox::Yes | QMessageBox::YesToAll | QMessageBox::No, QMessageBox::No);
2984 if(
b == QMessageBox::Yes ||
b == QMessageBox::YesToAll)
2992 progressDialog.show();
2994 progressDialog.
appendText(QString(
"Cropping empty space... %1 scans to filter").
arg(poses.size()));
2995 progressDialog.setMinimumWidth(800);
2996 QApplication::processEvents();
2998 UINFO(
"Cropping empty space... poses=%d cropRadius=%d", poses.size(), cropRadius);
3000 for(std::map<int, Transform>::iterator
iter=poses.begin();
iter!=poses.end() && !progressDialog.
isCanceled(); ++
iter)
3004 cv::Mat gridObstacles;
3011 data.uncompressData(0,0,&scan,0,&gridGround,&gridObstacles,&gridEmpty);
3017 if(!gridObstacles.empty())
3019 cv::Mat filtered = cv::Mat(1, gridObstacles.cols, gridObstacles.type());
3021 for(
int i=0;
i<gridObstacles.cols; ++
i)
3023 const float * ptr = gridObstacles.ptr<
float>(0,
i);
3024 cv::Point3f pt(ptr[0], ptr[1], gridObstacles.channels()==2?0:ptr[2]);
3027 int x =
int((pt.x - xMin) / cellSize + 0.5f);
3028 int y =
int((pt.y - yMin) / cellSize + 0.5f);
3030 if(
x>=0 &&
x<map8S.cols &&
3031 y>=0 &&
y<map8S.rows)
3033 bool obstacleDetected =
false;
3035 for(
int j=-cropRadius;
j<=cropRadius && !obstacleDetected; ++
j)
3037 for(
int k=-cropRadius; k<=cropRadius && !obstacleDetected; ++k)
3039 if(
x+
j>=0 &&
x+
j<map8S.cols &&
3040 y+k>=0 &&
y+k<map8S.rows &&
3041 map8S.at<
unsigned char>(
y+k,
x+
j) == 100)
3043 obstacleDetected =
true;
3048 if(map8S.at<
unsigned char>(
y,
x) != 0 || obstacleDetected)
3057 if(oi != gridObstacles.cols)
3059 progressDialog.
appendText(QString(
"Grid %1 filtered %2 pts -> %3 pts").
arg(
iter->first).
arg(gridObstacles.cols).arg(oi));
3060 UINFO(
"Grid %d filtered %d -> %d",
iter->first, gridObstacles.cols, oi);
3063 cv::Mat newObstacles = cv::Mat(filtered,
cv::Range::all(), cv::Range(0, oi));
3067 value.obstacleCells = newObstacles;
3079 if(
b == QMessageBox::YesToAll && !scan.
isEmpty())
3083 cv::Mat filtered = cv::Mat(1, scan.
size(), scan.
dataType());
3085 for(
int i=0;
i<scan.
size(); ++
i)
3087 const float * ptr = scan.
data().ptr<
float>(0,
i);
3088 cv::Point3f pt(ptr[0], ptr[1], scan.
is2d()?0:ptr[2]);
3091 int x =
int((pt.x - xMin) / cellSize + 0.5f);
3092 int y =
int((pt.y - yMin) / cellSize + 0.5f);
3094 if(
x>=0 &&
x<map8S.cols &&
3095 y>=0 &&
y<map8S.rows)
3097 bool obstacleDetected =
false;
3099 for(
int j=-cropRadius;
j<=cropRadius && !obstacleDetected; ++
j)
3101 for(
int k=-cropRadius; k<=cropRadius && !obstacleDetected; ++k)
3103 if(
x+
j>=0 &&
x+
j<map8S.cols &&
3104 y+k>=0 &&
y+k<map8S.rows &&
3105 map8S.at<
unsigned char>(
y+k,
x+
j) == 100)
3107 obstacleDetected =
true;
3112 if(map8S.at<
unsigned char>(
y,
x) != 0 || obstacleDetected)
3121 if(oi != scan.
size())
3124 UINFO(
"Scan %d filtered %d -> %d",
iter->first, scan.
size(), oi);
3154 QApplication::processEvents();
3174 QMessageBox::warning(
this, tr(
"Cannot export 2D map"), tr(
"A database must must loaded first...\nUse File->Open database."));
3178 float xMin, yMin, cellSize;
3182 QMessageBox::warning(
this, tr(
"Cannot export 2D map"), tr(
"The database doesn't contain a saved 2D map."));
3188 QString
path = QFileDialog::getSaveFileName(
3196 if(QFileInfo(
path).suffix() ==
"")
3200 cv::imwrite(
path.toStdString(), map8U);
3203 QString yaml =
info.absolutePath() +
"/" +
info.baseName() +
".yaml";
3205 float occupancyThr = Parameters::defaultGridGlobalOccupancyThr();
3206 Parameters::parse(
ui_->parameters_toolbox->getParameters(), Parameters::kGridGlobalOccupancyThr(), occupancyThr);
3209 file.open (yaml.toStdString());
3210 file <<
"image: " <<
info.baseName().toStdString() <<
".pgm" << std::endl;
3211 file <<
"resolution: " << cellSize << std::endl;
3212 file <<
"origin: [" << xMin <<
", " << yMin <<
", 0.0]" << std::endl;
3213 file <<
"negate: 0" << std::endl;
3214 file <<
"occupied_thresh: " << occupancyThr << std::endl;
3215 file <<
"free_thresh: 0.196" << std::endl;
3220 QMessageBox::information(
this, tr(
"Export 2D map"), tr(
"Exported %1 and %2!").
arg(
path).
arg(yaml));
3229 QMessageBox::warning(
this, tr(
"Cannot import 2D map"), tr(
"A database must must loaded first...\nUse File->Open database."));
3235 QMessageBox::warning(
this, tr(
"Cannot edit 2D map"),
3236 tr(
"The database has too old version (%1) to saved "
3243 QMessageBox::warning(
this, tr(
"Cannot import 2D map"),
3244 tr(
"The database has modified links and/or modified local "
3245 "occupancy grids, the 2D optimized map cannot be modified."));
3249 float xMin, yMin, cellSize;
3253 QMessageBox::warning(
this, tr(
"Cannot import 2D map"), tr(
"The database doesn't contain a saved 2D map."));
3257 QString
path = QFileDialog::getOpenFileName(
3264 cv::Mat map8U = cv::imread(
path.toStdString(), cv::IMREAD_UNCHANGED);
3267 if(mapOrg.cols == map.cols && mapOrg.rows == map8U.rows)
3270 QMessageBox::information(
this, tr(
"Import 2D map"), tr(
"Imported %1!").
arg(
path));
3274 QMessageBox::warning(
this, tr(
"Import 2D map"), tr(
"Cannot import %1 as its size doesn't match the current saved map. Import 2D Map action should only be used to modify the map saved in the database.").
arg(
path));
3284 QMessageBox::warning(
this, tr(
"Cannot import 2D map"), tr(
"A database must must loaded first...\nUse File->Open database."));
3290 QMessageBox::warning(
this, tr(
"Cannot edit 2D map"),
3291 tr(
"The database has too old version (%1) to saved "
3298 QMessageBox::warning(
this, tr(
"Cannot import 2D map"),
3299 tr(
"The database has modified links and/or modified local "
3300 "occupancy grids, the 2D optimized map cannot be modified. Try "
3301 "closing the database and re-open it to save the changes."));
3307 QMessageBox::warning(
this, tr(
"Cannot regenerate 2D map"),
3308 tr(
"Graph is empty, make sure you opened the "
3309 "Graph View and there is a map shown."));
3314 #ifdef RTABMAP_OCTOMAP
3316 types.push_back(
"Default occupancy grid");
3317 types.push_back(
"From OctoMap projection");
3319 QString
type = QInputDialog::getItem(
this, tr(
"Which type?"), tr(
"Type:"), types, 0,
false, &ok);
3327 UINFO(
"Update local maps list...");
3330 float gridCellSize = Parameters::defaultGridCellSize();
3331 Parameters::parse(
ui_->parameters_toolbox->getParameters(), Parameters::kGridCellSize(), gridCellSize);
3332 #ifdef RTABMAP_OCTOMAP
3333 if(
type.compare(
"From OctoMap projection") == 0)
3346 map = grid.
getMap(xMin, yMin);
3351 QMessageBox::information(
this, tr(
"Regenerate 2D map"), tr(
"Failed to renegerate the map, resulting map is empty!"));
3361 lastlocalizationPose =
graphes_.back().rbegin()->second;
3366 QMessageBox::information(
this, tr(
"Regenerate 2D map"), tr(
"Map regenerated!"));
3367 ui_->actionEdit_optimized_2D_map->setEnabled(
true);
3368 ui_->actionExport_saved_2D_map->setEnabled(
true);
3369 ui_->actionImport_2D_map->setEnabled(
true);
3377 QMessageBox::warning(
this, tr(
"Cannot view optimized mesh"), tr(
"A database must must loaded first...\nUse File->Open database."));
3381 std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
3382 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
3383 std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
3385 std::vector<std::vector<Eigen::Vector2f> > texCoords;
3389 if(cloudMat.empty())
3391 QMessageBox::warning(
this, tr(
"Cannot view optimized mesh"), tr(
"The database doesn't contain a saved optimized mesh."));
3396 viewer->setWindowFlags(Qt::Window);
3397 viewer->setAttribute(Qt::WA_DeleteOnClose);
3399 if(!textures.empty())
3403 viewer->setWindowTitle(
"Optimized Textured Mesh");
3407 else if(polygons.size() == 1)
3410 viewer->setWindowTitle(
"Optimized Mesh");
3418 viewer->setWindowTitle(
"Optimized Point Cloud");
3429 QMessageBox::warning(
this, tr(
"Cannot export optimized mesh"), tr(
"A database must must loaded first...\nUse File->Open database."));
3433 std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
3434 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
3435 std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
3437 std::vector<std::vector<Eigen::Vector2f> > texCoords;
3441 if(cloudMat.empty())
3443 QMessageBox::warning(
this, tr(
"Cannot export optimized mesh"), tr(
"The database doesn't contain a saved optimized mesh."));
3449 if(!textures.empty())
3452 QString
path = QFileDialog::getSaveFileName(
3456 tr(
"Mesh (*.obj)"));
3460 if(QFileInfo(
path).suffix() ==
"")
3464 QString baseName = QFileInfo(
path).baseName();
3465 if(mesh->tex_materials.size() == 1)
3467 mesh->tex_materials.at(0).tex_file = baseName.toStdString() +
".png";
3468 cv::imwrite((QFileInfo(
path).absoluteDir().absolutePath()+QDir::separator()+baseName).toStdString() +
".png", textures);
3472 for(
unsigned int i=0;
i<mesh->tex_materials.size(); ++
i)
3474 mesh->tex_materials.at(
i).tex_file = (baseName+QDir::separator()+QString::number(
i)+
".png").toStdString();
3475 UASSERT((
i+1)*textures.rows <= (
unsigned int)textures.cols);
3476 cv::imwrite((QFileInfo(
path).absoluteDir().absolutePath()+QDir::separator()+baseName+QDir::separator()+QString::number(
i)+
".png").toStdString(), textures(
cv::Range::all(), cv::Range(
i*textures.rows, (
i+1)*textures.rows)));
3479 pcl::io::saveOBJFile(
path.toStdString(), *mesh);
3481 QMessageBox::information(
this, tr(
"Export Textured Mesh"), tr(
"Exported %1!").
arg(
path));
3484 else if(polygons.size() == 1)
3487 QString
path = QFileDialog::getSaveFileName(
3491 tr(
"Mesh (*.ply)"));
3495 if(QFileInfo(
path).suffix() ==
"")
3499 pcl::io::savePLYFileBinary(
path.toStdString(), *mesh);
3500 QMessageBox::information(
this, tr(
"Export Mesh"), tr(
"Exported %1!").
arg(
path));
3505 QString
path = QFileDialog::getSaveFileName(
3509 tr(
"Point cloud data (*.ply *.pcd)"));
3513 if(QFileInfo(
path).suffix() ==
"")
3517 bool success =
false;
3519 if(QFileInfo(
path).suffix() ==
"pcd")
3521 success = pcl::io::savePCDFile(
path.toStdString(), *cloud) == 0;
3525 success = pcl::io::savePLYFile(
path.toStdString(), *cloud) == 0;
3529 QMessageBox::information(
this, tr(
"Export Point Cloud"), tr(
"Exported %1!").
arg(
path));
3533 QMessageBox::critical(
this, tr(
"Export Point Cloud"), tr(
"Failed exporting %1!").
arg(
path));
3544 QMessageBox::warning(
this, tr(
"Cannot generate a graph"), tr(
"The database is empty..."));
3551 if(
graphes_.empty() ||
ui_->horizontalSlider_iterations->maximum() != (
int)
graphes_.size()-1)
3553 QMessageBox::warning(
this, tr(
"Cannot generate a graph"), tr(
"No graph in database?!"));
3558 std::map<int, Transform> optimizedPoses;
3559 if(
ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() &&
3560 ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked()
3569 if(
ui_->groupBox_posefiltering->isChecked())
3572 ui_->doubleSpinBox_posefilteringRadius->value(),
3573 ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
3579 if(optimizedPoses.size() > 0)
3585 std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> clouds;
3586 std::map<int, pcl::PolygonMesh::Ptr> meshes;
3587 std::map<int, pcl::TextureMesh::Ptr> textureMeshes;
3588 std::vector<std::map<int, pcl::PointXY> > textureVertexToPixels;
3594 QMap<int, Signature>(),
3595 std::map<
int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
3596 std::map<int, LaserScan>(),
3598 ui_->parameters_toolbox->getParameters(),
3602 textureVertexToPixels))
3604 if(textureMeshes.size())
3608 cv::Mat globalTextures;
3609 pcl::TextureMeshPtr textureMesh = textureMeshes.at(0);
3610 if(textureMesh->tex_materials.size()>1)
3614 std::map<int, cv::Mat>(),
3615 std::map<
int, std::vector<CameraModel> >(),
3620 textureVertexToPixels,
3633 textureMesh->tex_coordinates,
3635 QMessageBox::information(
this, tr(
"Update Optimized Textured Mesh"), tr(
"Updated!"));
3636 ui_->actionView_optimized_mesh->setEnabled(
true);
3637 ui_->actionExport_optimized_mesh->setEnabled(
true);
3640 else if(meshes.size())
3643 std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons(1);
3646 QMessageBox::information(
this, tr(
"Update Optimized Mesh"), tr(
"Updated!"));
3647 ui_->actionView_optimized_mesh->setEnabled(
true);
3648 ui_->actionExport_optimized_mesh->setEnabled(
true);
3651 else if(clouds.size())
3655 QMessageBox::information(
this, tr(
"Update Optimized PointCloud"), tr(
"Updated!"));
3656 ui_->actionView_optimized_mesh->setEnabled(
true);
3657 ui_->actionExport_optimized_mesh->setEnabled(
true);
3662 QMessageBox::critical(
this, tr(
"Update Optimized Mesh"), tr(
"Nothing to save!"));
3669 QMessageBox::critical(
this, tr(
"Error"), tr(
"No neighbors found for node %1.").
arg(
ui_->spinBox_optimizationsFrom->value()));
3677 QMessageBox::warning(
this, tr(
"Cannot generate a graph"), tr(
"A database must must loaded first...\nUse File->Open database."));
3681 QString
path = QFileDialog::getSaveFileName(
this, tr(
"Save File"),
pathDatabase_+
"/Graph.dot", tr(
"Graphiz file (*.dot)"));
3692 QMessageBox::warning(
this, tr(
"Cannot generate a graph"), tr(
"The database is empty..."));
3696 int id = QInputDialog::getInt(
this, tr(
"Around which location?"), tr(
"Location ID"),
ids_.first(),
ids_.first(),
ids_.last(), 1, &ok);
3700 int margin = QInputDialog::getInt(
this, tr(
"Depth around the location?"), tr(
"Margin"), 4, 1, 100, 1, &ok);
3703 QString
path = QFileDialog::getSaveFileName(
this, tr(
"Save File"),
pathDatabase_+
"/Graph" + QString::number(
id) +
".dot", tr(
"Graphiz file (*.dot)"));
3704 if(!
path.isEmpty() &&
id>0)
3706 std::map<int, int> ids;
3707 std::list<int> curentMarginList;
3708 std::set<int> currentMargin;
3709 std::set<int> nextMargin;
3710 nextMargin.insert(
id);
3712 while((margin == 0 ||
m < margin) && nextMargin.size())
3714 curentMarginList = std::list<int>(nextMargin.rbegin(), nextMargin.rend());
3717 for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
3719 if(ids.find(*jter) == ids.end())
3721 std::multimap<int, Link> links;
3722 ids.insert(std::pair<int, int>(*jter,
m));
3728 for(std::multimap<int, Link>::const_iterator
iter=links.begin();
iter!=links.end(); ++
iter)
3736 nextMargin.insert(
iter->first);
3741 if(currentMargin.insert(
iter->first).second)
3743 curentMarginList.push_back(
iter->first);
3755 ids.insert(std::pair<int,int>(
id, 0));
3756 std::set<int> idsSet;
3757 for(std::map<int, int>::iterator
iter = ids.begin();
iter!=ids.end(); ++
iter)
3759 idsSet.insert(idsSet.end(),
iter->first);
3762 UINFO(
"idsSet=%d", idsSet.size());
3767 QMessageBox::critical(
this, tr(
"Error"), tr(
"No neighbors found for signature %1.").
arg(
id));
3782 progressDialog.show();
3786 plot->setWindowFlags(Qt::Window);
3787 plot->setWindowTitle(
"Local Occupancy Grid Generation Time (ms)");
3788 plot->setAttribute(Qt::WA_DeleteOnClose);
3794 plotCells->setWindowFlags(Qt::Window);
3795 plotCells->setWindowTitle(
"Occupancy Cells");
3796 plotCells->setAttribute(Qt::WA_DeleteOnClose);
3803 double decompressionTime = 0;
3804 double gridCreationTime = 0;
3811 data.uncompressData();
3812 decompressionTime =
timer.ticks()*1000.0;
3825 s.setPose(odomPose);
3826 cv::Mat ground, obstacles,
empty;
3827 cv::Point3f viewpoint;
3830 if(
ui_->checkBox_grid_regenerateFromSavedGrid->isChecked() &&
s.sensorData().gridCellSize() > 0.0f)
3838 if(
s.sensorData().cameraModels().size())
3842 for(
unsigned int i=0;
i<
s.sensorData().cameraModels().
size(); ++
i)
3844 const Transform &
t =
s.sensorData().cameraModels()[
i].localTransform();
3847 viewpoint.x +=
t.
x();
3848 viewpoint.y +=
t.
y();
3849 viewpoint.z +=
t.z();
3860 else if(
s.sensorData().cameraModels().size())
3864 for(
unsigned int i=0;
i<
s.sensorData().stereoCameraModels().
size(); ++
i)
3866 const Transform &
t =
s.sensorData().stereoCameraModels()[
i].localTransform();
3869 viewpoint.x +=
t.
x();
3870 viewpoint.y +=
t.
y();
3871 viewpoint.z +=
t.z();
3891 gridCreationTime =
timer.ticks()*1000.0;
3893 msg = QString(
"Generated local occupancy grid map %1/%2").arg(
i+1).arg((
int)
ids_.size());
3904 decompressionCurve->
addValue(
ids_.at(
i), decompressionTime);
3907 if(
ids_.size() < 50 || (
i+1) % 25 == 0)
3909 QApplication::processEvents();
3930 if(
ids_.size() == 0)
3932 UWARN(
"ids_ is empty!");
3937 idsSet.insert(
ids_.at(
ui_->horizontalSlider_A->value()));
3938 idsSet.insert(
ids_.at(
ui_->horizontalSlider_B->value()));
3939 #if QT_VERSION >= QT_VERSION_CHECK(5, 15, 3)
3940 QList<int> ids(idsSet.begin(), idsSet.end());
3942 QList<int> ids = idsSet.toList();
3947 progressDialog.show();
3949 for(
int i =0;
i<ids.size(); ++
i)
3953 data.uncompressData();
3966 s.setPose(odomPose);
3967 cv::Mat ground, obstacles,
empty;
3968 cv::Point3f viewpoint;
3970 if(
ui_->checkBox_grid_regenerateFromSavedGrid->isChecked() &&
s.sensorData().gridCellSize() > 0.0f)
3978 if(
s.sensorData().cameraModels().size())
3982 for(
unsigned int i=0;
i<
s.sensorData().cameraModels().
size(); ++
i)
3984 const Transform &
t =
s.sensorData().cameraModels()[
i].localTransform();
3987 viewpoint.x +=
t.
x();
3988 viewpoint.y +=
t.
y();
3989 viewpoint.z +=
t.z();
4000 else if(
s.sensorData().stereoCameraModels().size())
4004 for(
unsigned int i=0;
i<
s.sensorData().stereoCameraModels().
size(); ++
i)
4006 const Transform &
t =
s.sensorData().stereoCameraModels()[
i].localTransform();
4009 viewpoint.x +=
t.
x();
4010 viewpoint.y +=
t.
y();
4011 viewpoint.z +=
t.z();
4032 msg = QString(
"Generated local occupancy grid map %1/%2 (%3s)").arg(
i+1).arg((
int)ids.size()).arg(
time.ticks());
4037 QApplication::processEvents();
4056 QMessageBox::warning(
this, tr(
"Cannot view 3D map"), tr(
"The database is empty..."));
4063 if(
graphes_.empty() ||
ui_->horizontalSlider_iterations->maximum() != (
int)
graphes_.size()-1)
4065 QMessageBox::warning(
this, tr(
"Cannot generate a graph"), tr(
"No graph in database?!"));
4070 std::map<int, Transform> optimizedPoses;
4071 if(
ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() &&
4072 ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() &&
4081 if(
ui_->groupBox_posefiltering->isChecked())
4084 ui_->doubleSpinBox_posefilteringRadius->value(),
4085 ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
4087 if(optimizedPoses.size() > 0)
4093 QMap<int, Signature>(),
4094 std::map<
int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
4095 std::map<int, LaserScan>(),
4097 ui_->parameters_toolbox->getParameters());
4101 QMessageBox::critical(
this, tr(
"Error"), tr(
"No neighbors found for node %1.").
arg(
ui_->spinBox_optimizationsFrom->value()));
4109 QMessageBox::warning(
this, tr(
"Cannot generate a graph"), tr(
"The database is empty..."));
4116 if(
graphes_.empty() ||
ui_->horizontalSlider_iterations->maximum() != (
int)
graphes_.size()-1)
4118 QMessageBox::warning(
this, tr(
"Cannot generate a graph"), tr(
"No graph in database?!"));
4123 std::map<int, Transform> optimizedPoses;
4124 if(
ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() &&
4125 ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() &&
4135 ui_->checkBox_alignPosesWithGPS->isEnabled() &&
4136 ui_->checkBox_alignPosesWithGPS->isChecked();
4139 (
ui_->checkBox_alignPosesWithGroundTruth->isEnabled() &&
ui_->checkBox_alignPosesWithGroundTruth->isChecked()))
4150 float translational_rmse = 0.0f;
4151 float translational_mean = 0.0f;
4152 float translational_median = 0.0f;
4153 float translational_std = 0.0f;
4154 float translational_min = 0.0f;
4155 float translational_max = 0.0f;
4156 float rotational_rmse = 0.0f;
4157 float rotational_mean = 0.0f;
4158 float rotational_median = 0.0f;
4159 float rotational_std = 0.0f;
4160 float rotational_min = 0.0f;
4161 float rotational_max = 0.0f;
4168 translational_median,
4182 for(std::map<int, Transform>::iterator
iter=optimizedPoses.begin();
iter!=optimizedPoses.end(); ++
iter)
4184 iter->second = gtToMap *
iter->second;
4186 if(alignToGPS && optimizedPoses.find(
gpsValues_.begin()->first)!=optimizedPoses.end())
4190 Transform offset = optimizedPoses.at(originId).translation().inverse();
4191 for(std::map<int, Transform>::iterator
iter=optimizedPoses.begin();
iter!=optimizedPoses.end(); ++
iter)
4200 if(
ui_->groupBox_posefiltering->isChecked())
4203 ui_->doubleSpinBox_posefilteringRadius->value(),
4204 ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
4206 if(optimizedPoses.size() > 0)
4212 QMap<int, Signature>(),
4213 std::map<
int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
4214 std::map<int, LaserScan>(),
4216 ui_->parameters_toolbox->getParameters());
4220 QMessageBox::critical(
this, tr(
"Error"), tr(
"No neighbors found for node %1.").
arg(
ui_->spinBox_optimizationsFrom->value()));
4229 if(
graphes_.empty() ||
ui_->horizontalSlider_iterations->maximum() != (
int)
graphes_.size()-1)
4231 QMessageBox::warning(
this, tr(
"Cannot generate a graph"), tr(
"No graph in database?!"));
4236 std::map<int, Transform> optimizedPoses =
graphes_.back();
4239 progressDialog->setAttribute(Qt::WA_DeleteOnClose);
4242 progressDialog->setMinimumWidth(800);
4243 progressDialog->show();
4245 const ParametersMap & parameters =
ui_->parameters_toolbox->getParameters();
4246 bool loopCovLimited = Parameters::defaultRGBDLoopCovLimited();
4247 Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), loopCovLimited);
4253 int iterations =
ui_->spinBox_detectMore_iterations->value();
4256 std::multimap<int, int> checkedLoopClosures;
4257 std::pair<int, int> lastAdded(0,0);
4258 bool intraSession =
ui_->checkBox_detectMore_intraSession->isChecked();
4259 bool interSession =
ui_->checkBox_detectMore_interSession->isChecked();
4260 bool useOptimizedGraphAsGuess =
ui_->checkBox_opt_graph_as_guess->isChecked();
4261 if(!interSession && !intraSession)
4263 QMessageBox::warning(
this, tr(
"Cannot detect more loop closures"), tr(
"Intra and inter session parameters are disabled! Enable one or both."));
4267 for(
int n=0;
n<iterations; ++
n)
4269 UINFO(
"iteration %d/%d",
n+1, iterations);
4271 std::map<int, Transform>(optimizedPoses.upper_bound(0), optimizedPoses.end()),
4272 ui_->doubleSpinBox_detectMore_radius->value(),
4273 ui_->doubleSpinBox_detectMore_angle->value()*CV_PI/180.0);
4276 progressDialog->
appendText(tr(
"Looking for more loop closures, %1 clusters found.").
arg(clusters.size()));
4277 QApplication::processEvents();
4283 std::set<int> addedLinks;
4285 for(std::multimap<int, int>::iterator
iter=clusters.begin();
iter!= clusters.end() && !progressDialog->
isCanceled(); ++
iter, ++
i)
4287 int from =
iter->first;
4288 int to =
iter->second;
4291 from =
iter->second;
4298 if((interSession && mapIdFrom != mapIdTo) ||
4299 (intraSession && mapIdFrom == mapIdTo))
4305 addedLinks.find(from) == addedLinks.end() &&
4306 addedLinks.find(to) == addedLinks.end())
4309 Transform delta = optimizedPoses.at(from).inverse() * optimizedPoses.at(to);
4310 if(
delta.getNorm() <
ui_->doubleSpinBox_detectMore_radius->value() &&
4311 delta.getNorm() >=
ui_->doubleSpinBox_detectMore_radiusMin->value())
4313 checkedLoopClosures.insert(std::make_pair(from, to));
4316 UINFO(
"Added new loop closure between %d and %d.", from, to);
4318 addedLinks.insert(from);
4319 addedLinks.insert(to);
4320 lastAdded.first = from;
4321 lastAdded.second = to;
4323 progressDialog->
appendText(tr(
"Detected loop closure %1->%2! (%3/%4)").
arg(from).
arg(to).
arg(
i+1).
arg(clusters.size()));
4324 QApplication::processEvents();
4335 QApplication::processEvents();
4338 UINFO(
"Iteration %d/%d: added %d loop closures.",
n+1, iterations, (
int)addedLinks.size()/2);
4339 progressDialog->
appendText(tr(
"Iteration %1/%2: Detected %3 loop closures!").
arg(
n+1).
arg(iterations).
arg(addedLinks.size()/2));
4340 if(addedLinks.size() == 0)
4359 UINFO(
"Total added %d loop closures.", added);
4361 progressDialog->
appendText(tr(
"Total new loop closures detected=%1").
arg(added));
4368 QList<rtabmap::Link> links;
4369 for(std::multimap<int, Link>::iterator
iter=allLinks.begin();
iter!=allLinks.end(); ++
iter)
4374 links.push_back(
iter->second);
4382 QList<rtabmap::Link> links;
4383 for(std::multimap<int, Link>::iterator
iter=allLinks.begin();
iter!=allLinks.end(); ++
iter)
4388 iter->second.from() !=
iter->second.to())
4390 links.push_back(
iter->second);
4398 QList<rtabmap::Link> links;
4399 for(std::multimap<int, Link>::iterator
iter=allLinks.begin();
iter!=allLinks.end(); ++
iter)
4403 links.push_back(
iter->second);
4413 cv::Mat infMatrix = links.first().infMatrix();
4416 if(dialog.exec() != QDialog::Accepted)
4422 progressDialog->setAttribute(Qt::WA_DeleteOnClose);
4425 progressDialog->setMinimumWidth(800);
4426 progressDialog->show();
4430 for(
int i=0;
i<links.size(); ++
i)
4432 int from = links[
i].from();
4433 int to = links[
i].to();
4438 UERROR(
"Not found link! (%d->%d)", from, to);
4452 if(
iter->second.to() == currentLink.
to() &&
4453 iter->second.type() == currentLink.
type())
4455 iter->second = currentLink;
4468 QApplication::processEvents();
4477 progressDialog->
appendText(
"Refining links finished!");
4488 for(std::multimap<int, Link>::iterator
iter=allLinks.begin();
iter!=allLinks.end(); ++
iter)
4492 int minId =
iter->second.from()>
iter->second.to()?
iter->second.to():
iter->second.from();
4493 int maxId =
iter->second.from()<
iter->second.to()?
iter->second.to():
iter->second.from();
4494 if(minNodeId == 0 || minNodeId > minId)
4498 if(maxNodeId == 0 || maxNodeId < maxId)
4523 for(std::multimap<int, Link>::iterator
iter=allLinks.begin();
iter!=allLinks.end(); ++
iter)
4528 int from =
iter->second.from();
4529 int to =
iter->second.to();
4533 ((from >= minNodeId && from <= maxNodeId) ||
4534 (to >= minNodeId && to <= maxNodeId))) ||
4536 ((mapFrom >= minMapId && mapFrom <= maxMapId) ||
4537 (mapTo >= minMapId && mapTo <= maxMapId)))) &&
4538 ((intra && mapTo == mapFrom) ||
4539 (inter && mapTo != mapFrom)))
4541 links.push_back(
iter->second);
4547 QMessageBox::warning(
this, tr(
"Refine links"), tr(
"No links found matching the requested parameters."));
4558 UWARN(
"No links can be refined!");
4566 progressDialog->setAttribute(Qt::WA_DeleteOnClose);
4569 progressDialog->setMinimumWidth(800);
4570 progressDialog->show();
4572 for(
int i=0;
i<links.size(); ++
i)
4574 int from = links[
i].from();
4575 int to = links[
i].to();
4576 if(from > 0 && to > 0)
4583 progressDialog->
appendText(tr(
"Ignored link %1->%2 (landmark)").
arg(from).
arg(to));
4586 QApplication::processEvents();
4595 progressDialog->
appendText(
"Refining links finished!");
4601 if(QMessageBox::question(
this,
4602 tr(
"Reset all changes"),
4603 tr(
"You are about to reset all changes you've made so far, do you want to continue?"),
4604 QMessageBox::Yes | QMessageBox::No,
4605 QMessageBox::No) == QMessageBox::Yes)
4622 static bool updateA =
true;
4623 if(updateA ||
ui_->actionConcise_Layout->isChecked())
4627 updateA = !updateA ||
ui_->actionConcise_Layout->isChecked();
4642 ui_->spinBox_indexA,
4643 ui_->label_parentsA,
4644 ui_->label_childrenA,
4648 ui_->graphicsView_A,
4652 ui_->label_optposeA,
4656 ui_->label_gravityA,
4658 ui_->toolButton_edit_priorA,
4659 ui_->toolButton_remove_priorA,
4662 ui_->label_sensorsA,
4669 ui_->spinBox_indexB,
4670 ui_->label_parentsB,
4671 ui_->label_childrenB,
4675 ui_->graphicsView_B,
4679 ui_->label_optposeB,
4683 ui_->label_gravityB,
4685 ui_->toolButton_edit_priorB,
4686 ui_->toolButton_remove_priorB,
4689 ui_->label_sensorsB,
4694 QSpinBox * spinBoxIndex,
4695 QLabel * labelParents,
4696 QLabel * labelChildren,
4702 QLabel * labelMapId,
4704 QLabel * labelOptPose,
4705 QLabel * labelVelocity,
4706 QLabel * labelCalib,
4708 QLabel * labelGravity,
4709 QLabel * labelPrior,
4710 QToolButton * editPriorButton,
4711 QToolButton * removePriorButton,
4714 QLabel * labelSensors,
4715 bool updateConstraintView)
4720 spinBoxIndex->blockSignals(
true);
4721 spinBoxIndex->setValue(
value);
4722 spinBoxIndex->blockSignals(
false);
4723 labelParents->clear();
4724 labelChildren->clear();
4727 labelMapId->clear();
4729 labelOptPose->clear();
4730 labelVelocity->clear();
4732 labelCalib->clear();
4733 labelScan ->clear();
4734 labelGravity->clear();
4735 labelPrior->clear();
4736 editPriorButton->setVisible(
false);
4737 removePriorButton->setVisible(
false);
4740 labelSensors->clear();
4746 labelId->setText(QString::number(
id));
4749 if(
ui_->dockWidget_graphView->isVisible()) {
4750 ui_->graphViewer->highlightNode(
id, spinBoxIndex==
ui_->spinBox_indexB?1:0);
4760 data.uncompressData();
4761 if(!
data.imageRaw().empty())
4765 if(!
data.depthOrRightRaw().empty())
4767 cv::Mat depth =
data.depthOrRightRaw();
4768 if(!
data.depthRaw().empty())
4770 if(
ui_->spinBox_mesh_fillDepthHoles->value() > 0)
4775 if( !
data.imageRaw().empty() &&
4776 !
data.rightRaw().empty() &&
4777 data.stereoCameraModels().size()==1 &&
4778 data.stereoCameraModels()[0].isValidForProjection() &&
4779 ui_->checkBox_showDisparityInsteadOfRight->isChecked())
4804 if(!imgDepth.empty())
4806 view->setImageDepth(imgDepth);
4809 rect.setWidth(imgDepth.cols);
4810 rect.setHeight(imgDepth.rows);
4819 if(
data.cameraModels().size())
4821 for(
unsigned int i=0;
i<
data.cameraModels().
size(); ++
i)
4823 rect.setWidth(rect.width()+
data.cameraModels()[
i].imageWidth());
4824 rect.setHeight(
std::max((
int)rect.height(),
data.cameraModels()[
i].imageHeight()));
4827 else if(
data.stereoCameraModels().size())
4829 for(
unsigned int i=0;
i<
data.cameraModels().
size(); ++
i)
4831 rect.setWidth(rect.width()+
data.stereoCameraModels()[
i].left().imageWidth());
4832 rect.setHeight(
std::max((
int)rect.height(),
data.stereoCameraModels()[
i].left().imageHeight()));
4838 view->setSceneRect(rect);
4843 std::list<Signature*> signatures;
4846 if(signatures.size() && signatures.front()!=0 && !signatures.front()->getWordsKpts().empty())
4848 std::multimap<int, cv::KeyPoint> keypoints;
4849 for(std::map<int, int>::const_iterator
iter=signatures.front()->getWords().begin();
iter!=signatures.front()->getWords().end(); ++
iter)
4851 keypoints.insert(std::make_pair(
iter->first, signatures.front()->getWordsKpts()[
iter->second]));
4853 view->setFeatures(keypoints,
data.depthOrRightRaw().type() == CV_8UC1?cv::Mat():
data.depthOrRightRaw(), Qt::yellow);
4860 std::vector<float>
v;
4866 label->setText(l.c_str());
4869 labelPose->setText(QString(
"%1xyz=(%2,%3,%4)\nrpy=(%5,%6,%7)").
arg(odomPose.
isIdentity()?
"* ":
"").
arg(
x).arg(
y).arg(
z).arg(
roll).arg(
pitch).arg(
yaw));
4874 labelOptPose->setText(
"<Not in optimized graph>");
4884 stamp->setText(QString::number(
s,
'f'));
4885 stamp->setToolTip(QDateTime::fromMSecsSinceEpoch(
s*1000.0).toString(
"dd.MM.yyyy hh:mm:ss.zzz"));
4889 labelVelocity->setText(QString(
"vx=%1 vy=%2 vz=%3 vroll=%4 vpitch=%5 vyaw=%6").
arg(
v[0]).
arg(
v[1]).
arg(
v[2]).
arg(
v[3]).
arg(
v[4]).
arg(
v[5]));
4892 std::multimap<int, Link> gravityLink;
4894 if(!gravityLink.empty())
4896 Eigen::Vector3f
v = gravityLink.begin()->second.transform().inverse().toEigen3f() * -Eigen::Vector3f::UnitZ();
4897 labelGravity->setText(QString(
"x=%1 y=%2 z=%3").
arg(
v[0]).
arg(
v[1]).
arg(
v[2]));
4907 std::stringstream
out;
4909 labelPrior->setToolTip(QString(
"%1").
arg(
out.str().c_str()));
4910 removePriorButton->setVisible(
true);
4911 editPriorButton->setToolTip(tr(
"Edit Prior"));
4912 editPriorButton->setText(
"...");
4913 editPriorButton->setVisible(
true);
4915 else if(!odomPose.
isNull())
4917 editPriorButton->setToolTip(tr(
"Add Prior"));
4918 editPriorButton->setText(
"+");
4919 editPriorButton->setVisible(
true);
4924 labelGps->setText(QString(
"stamp=%1 longitude=%2 latitude=%3 altitude=%4m error=%5m bearing=%6deg").
arg(QString::number(gps.
stamp(),
'f')).
arg(gps.
longitude()).arg(gps.
latitude()).arg(gps.
altitude()).arg(gps.
error()).arg(gps.
bearing()));
4925 labelGps->setToolTip(QDateTime::fromMSecsSinceEpoch(gps.
stamp()*1000.0).toString(
"dd.MM.yyyy hh:mm:ss.zzz"));
4929 labelGt->setText(QString(
"%1").
arg(
g.prettyPrint().c_str()));
4935 for(EnvSensors::iterator
iter=sensors.begin();
iter!=sensors.end(); ++
iter)
4937 if(
iter != sensors.begin())
4939 sensorsStr +=
" | ";
4940 tooltipStr +=
" | ";
4945 sensorsStr +=
uFormat(
"%.1f dbm",
iter->second.value()).c_str();
4946 tooltipStr +=
"Wifi signal strength";
4950 sensorsStr +=
uFormat(
"%.1f \u00B0C",
iter->second.value()).c_str();
4951 tooltipStr +=
"Ambient Temperature";
4955 sensorsStr +=
uFormat(
"%.1f hPa",
iter->second.value()).c_str();
4956 tooltipStr +=
"Ambient Air Pressure";
4960 sensorsStr +=
uFormat(
"%.0f lx",
iter->second.value()).c_str();
4961 tooltipStr +=
"Ambient Light";
4965 sensorsStr +=
uFormat(
"%.0f %%",
iter->second.value()).c_str();
4966 tooltipStr +=
"Ambient Relative Humidity";
4970 sensorsStr +=
uFormat(
"%.2f",
iter->second.value()).c_str();
4971 tooltipStr += QString(
"Type %1").arg((
int)
iter->first);
4975 labelSensors->setText(sensorsStr);
4976 labelSensors->setToolTip(tooltipStr);
4978 if(
data.cameraModels().size() ||
data.stereoCameraModels().size())
4980 std::stringstream calibrationDetails;
4981 if(
data.cameraModels().size())
4983 if(!
data.depthRaw().empty() &&
data.depthRaw().cols!=
data.imageRaw().cols &&
data.imageRaw().cols)
4985 labelCalib->setText(tr(
"%1 %2x%3 [%8x%9] fx=%4 fy=%5 cx=%6 cy=%7 T=%10 [%11 %12 %13 %14; %15 %16 %17 %18; %19 %20 %21 %22]")
4986 .
arg(
data.cameraModels().size())
4987 .
arg(
data.cameraModels()[0].imageWidth()>0?
data.cameraModels()[0].imageWidth():
data.imageRaw().cols/
data.cameraModels().size())
4988 .arg(
data.cameraModels()[0].imageHeight()>0?
data.cameraModels()[0].imageHeight():
data.imageRaw().rows)
4989 .arg(
data.cameraModels()[0].fx())
4990 .arg(
data.cameraModels()[0].fy())
4991 .arg(
data.cameraModels()[0].cx())
4992 .arg(
data.cameraModels()[0].cy())
4993 .arg(
data.depthRaw().cols/
data.cameraModels().size())
4994 .arg(
data.depthRaw().rows)
4995 .arg(
data.cameraModels()[0].localTransform().prettyPrint().c_str())
4996 .arg(
data.cameraModels()[0].localTransform().r11()).arg(
data.cameraModels()[0].localTransform().r12()).arg(
data.cameraModels()[0].localTransform().r13()).arg(
data.cameraModels()[0].localTransform().o14())
4997 .arg(
data.cameraModels()[0].localTransform().r21()).arg(
data.cameraModels()[0].localTransform().r22()).arg(
data.cameraModels()[0].localTransform().r23()).arg(
data.cameraModels()[0].localTransform().o24())
4998 .arg(
data.cameraModels()[0].localTransform().r31()).arg(
data.cameraModels()[0].localTransform().r32()).arg(
data.cameraModels()[0].localTransform().r33()).arg(
data.cameraModels()[0].localTransform().o34()));
5002 labelCalib->setText(tr(
"%1 %2x%3 fx=%4 fy=%5 cx=%6 cy=%7 T=%8 [%9 %10 %11 %12; %13 %14 %15 %16; %17 %18 %19 %20]")
5003 .
arg(
data.cameraModels().size())
5004 .
arg(
data.cameraModels()[0].imageWidth()>0?
data.cameraModels()[0].imageWidth():
data.imageRaw().cols/
data.cameraModels().size())
5005 .arg(
data.cameraModels()[0].imageHeight()>0?
data.cameraModels()[0].imageHeight():
data.imageRaw().rows)
5006 .arg(
data.cameraModels()[0].fx())
5007 .arg(
data.cameraModels()[0].fy())
5008 .arg(
data.cameraModels()[0].cx())
5009 .arg(
data.cameraModels()[0].cy())
5010 .arg(
data.cameraModels()[0].localTransform().prettyPrint().c_str())
5011 .arg(
data.cameraModels()[0].localTransform().r11()).arg(
data.cameraModels()[0].localTransform().r12()).arg(
data.cameraModels()[0].localTransform().r13()).arg(
data.cameraModels()[0].localTransform().o14())
5012 .arg(
data.cameraModels()[0].localTransform().r21()).arg(
data.cameraModels()[0].localTransform().r22()).arg(
data.cameraModels()[0].localTransform().r23()).arg(
data.cameraModels()[0].localTransform().o24())
5013 .arg(
data.cameraModels()[0].localTransform().r31()).arg(
data.cameraModels()[0].localTransform().r32()).arg(
data.cameraModels()[0].localTransform().r33()).arg(
data.cameraModels()[0].localTransform().o34()));
5016 for(
unsigned int i=0;
i<
data.cameraModels().
size();++
i)
5018 if(
i!=0) calibrationDetails << std::endl;
5019 calibrationDetails <<
"Id: " <<
i <<
" Size=" <<
data.cameraModels()[
i].imageWidth() <<
"x" <<
data.cameraModels()[
i].imageHeight() << std::endl;
5020 if(
data.cameraModels()[
i].K_raw().total()) calibrationDetails <<
"K=" <<
data.cameraModels()[
i].K_raw() << std::endl;
5021 if(
data.cameraModels()[
i].D_raw().total()) calibrationDetails <<
"D=" <<
data.cameraModels()[
i].D_raw() << std::endl;
5022 if(
data.cameraModels()[
i].R().total()) calibrationDetails <<
"R=" <<
data.cameraModels()[
i].R() << std::endl;
5023 if(
data.cameraModels()[
i].P().total()) calibrationDetails <<
"P=" <<
data.cameraModels()[
i].P() << std::endl;
5028 else if(
data.stereoCameraModels().size())
5031 labelCalib->setText(tr(
"%1x%2 fx=%3 fy=%4 cx=%5 cy=%6 baseline=%7m T=%8 [%9 %10 %11 %12; %13 %14 %15 %16; %17 %18 %19 %20]")
5032 .arg(
data.stereoCameraModels()[0].left().imageWidth()>0?
data.stereoCameraModels()[0].left().imageWidth():
data.imageRaw().cols)
5033 .arg(
data.stereoCameraModels()[0].left().imageHeight()>0?
data.stereoCameraModels()[0].left().imageHeight():
data.imageRaw().rows)
5034 .arg(
data.stereoCameraModels()[0].left().fx())
5035 .arg(
data.stereoCameraModels()[0].left().fy())
5036 .arg(
data.stereoCameraModels()[0].left().cx())
5037 .arg(
data.stereoCameraModels()[0].left().cy())
5038 .arg(
data.stereoCameraModels()[0].baseline())
5039 .arg(
data.stereoCameraModels()[0].localTransform().prettyPrint().c_str())
5040 .arg(
data.stereoCameraModels()[0].localTransform().r11()).arg(
data.stereoCameraModels()[0].localTransform().r12()).arg(
data.stereoCameraModels()[0].localTransform().r13()).arg(
data.stereoCameraModels()[0].localTransform().o14())
5041 .arg(
data.stereoCameraModels()[0].localTransform().r21()).arg(
data.stereoCameraModels()[0].localTransform().r22()).arg(
data.stereoCameraModels()[0].localTransform().r23()).arg(
data.stereoCameraModels()[0].localTransform().o24())
5042 .arg(
data.stereoCameraModels()[0].localTransform().r31()).arg(
data.stereoCameraModels()[0].localTransform().r32()).arg(
data.stereoCameraModels()[0].localTransform().r33()).arg(
data.stereoCameraModels()[0].localTransform().o34()));
5044 for(
unsigned int i=0;
i<
data.stereoCameraModels().size();++
i)
5046 calibrationDetails <<
"Id: " <<
i << std::endl;
5047 calibrationDetails <<
" Left:" <<
" Size=" <<
data.stereoCameraModels()[
i].left().imageWidth() <<
"x" <<
data.stereoCameraModels()[
i].left().imageHeight() << std::endl;
5048 if(
data.stereoCameraModels()[
i].left().K_raw().total()) calibrationDetails <<
" K=" <<
data.stereoCameraModels()[
i].left().K_raw() << std::endl;
5049 if(
data.stereoCameraModels()[
i].left().D_raw().total()) calibrationDetails <<
" D=" <<
data.stereoCameraModels()[
i].left().D_raw() << std::endl;
5050 if(
data.stereoCameraModels()[
i].left().R().total()) calibrationDetails <<
" R=" <<
data.stereoCameraModels()[
i].left().R() << std::endl;
5051 if(
data.stereoCameraModels()[
i].left().P().total()) calibrationDetails <<
" P=" <<
data.stereoCameraModels()[
i].left().P() << std::endl;
5052 calibrationDetails <<
" Right:" <<
" Size=" <<
data.stereoCameraModels()[
i].right().imageWidth() <<
"x" <<
data.stereoCameraModels()[
i].right().imageHeight() << std::endl;
5053 if(
data.stereoCameraModels()[
i].right().K_raw().total()) calibrationDetails <<
" K=" <<
data.stereoCameraModels()[
i].right().K_raw() << std::endl;
5054 if(
data.stereoCameraModels()[
i].right().D_raw().total()) calibrationDetails <<
" D=" <<
data.stereoCameraModels()[
i].right().D_raw() << std::endl;
5055 if(
data.stereoCameraModels()[
i].right().R().total()) calibrationDetails <<
" R=" <<
data.stereoCameraModels()[
i].right().R() << std::endl;
5056 if(
data.stereoCameraModels()[
i].right().P().total()) calibrationDetails <<
" P=" <<
data.stereoCameraModels()[
i].right().P() << std::endl;
5057 if(
data.stereoCameraModels()[
i].R().total()) calibrationDetails <<
" R=" <<
data.stereoCameraModels()[
i].R() << std::endl;
5058 if(
data.stereoCameraModels()[
i].T().total()) calibrationDetails <<
" T=" <<
data.stereoCameraModels()[
i].T() << std::endl;
5059 if(
data.stereoCameraModels()[
i].F().total()) calibrationDetails <<
" F=" <<
data.stereoCameraModels()[
i].F() << std::endl;
5060 if(
data.stereoCameraModels()[
i].E().total()) calibrationDetails <<
" E=" <<
data.stereoCameraModels()[
i].E() << std::endl;
5064 labelCalib->setToolTip(calibrationDetails.str().c_str());
5069 labelCalib->setText(
"NA");
5072 if(
data.laserScanRaw().size())
5074 labelScan->setText(tr(
"Format=%1 Points=%2 [max=%3] Range=[%4->%5 m] Angle=[%6->%7 rad inc=%8] Has [Color=%9 2D=%10 Normals=%11 Intensity=%12] %13")
5075 .arg(
data.laserScanRaw().formatName().c_str())
5076 .arg(
data.laserScanRaw().size())
5077 .arg(
data.laserScanRaw().maxPoints())
5078 .arg(
data.laserScanRaw().rangeMin())
5079 .arg(
data.laserScanRaw().rangeMax())
5080 .arg(
data.laserScanRaw().angleMin())
5081 .arg(
data.laserScanRaw().angleMax())
5082 .arg(
data.laserScanRaw().angleIncrement())
5083 .arg(
data.laserScanRaw().hasRGB()?1:0)
5084 .arg(
data.laserScanRaw().is2d()?1:0)
5085 .arg(
data.laserScanRaw().hasNormals()?1:0)
5086 .arg(
data.laserScanRaw().hasIntensity()?1:0)
5087 .arg(
data.laserScanRaw().localTransform().prettyPrint().c_str()));
5091 if(!
data.depthOrRightRaw().empty() &&
data.depthOrRightRaw().type() == CV_8UC1)
5098 ui_->graphicsView_stereo->clear();
5112 if(signatures.size() &&
ui_->checkBox_odomFrame_3dview->isChecked())
5119 if(!gravityLink.empty() &&
ui_->checkBox_gravity_3dview->isChecked())
5121 Transform gravityT = gravityLink.begin()->second.transform();
5122 Eigen::Vector3f gravity = gravityT.
inverse().
toEigen3f()*-Eigen::Vector3f::UnitZ();
5123 cloudViewer_->
addOrUpdateLine(
"gravity", pose, pose*
Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0), Qt::yellow,
true,
false);
5132 if(
ui_->checkBox_showScan->isChecked() && laserScanRaw.
size())
5149 else if(laserScanRaw.
hasRGB())
5167 if(
ui_->checkBox_showCloud->isChecked() &&
ui_->checkBox_cameraProjection->isChecked() &&
5168 !
data.imageRaw().empty() && !laserScanRaw.
empty() && !laserScanRaw.
is2d())
5171 std::vector<CameraModel> models =
data.cameraModels();
5172 if(!
data.stereoCameraModels().empty())
5175 for(
size_t i=0;
i<
data.stereoCameraModels().
size(); ++
i)
5177 models.push_back(
data.stereoCameraModels()[
i].left());
5181 if(!models.empty() && !models[0].isValidForProjection())
5186 if(!models.empty() && models[0].imageWidth() != 0 && models[0].imageHeight() != 0)
5189 std::map<int, std::vector<CameraModel> > cameraModels;
5191 cameraModels.insert(std::make_pair(
data.id(), models));
5193 std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;
5199 UASSERT(pointToPixel.empty() || pointToPixel.size() == cloud->size());
5200 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudValidPoints(
new pcl::PointCloud<pcl::PointXYZRGB>);
5201 cloudValidPoints->resize(cloud->size());
5203 for(
size_t i=0;
i<pointToPixel.size(); ++
i)
5205 pcl::PointXYZINormal & pt = cloud->at(
i);
5206 pcl::PointXYZRGB ptColor;
5207 int nodeID = pointToPixel[
i].first.first;
5208 int cameraIndex = pointToPixel[
i].first.second;
5209 if(nodeID>0 && cameraIndex>=0)
5211 cv::Mat image =
data.imageRaw();
5213 int subImageWidth = image.cols / cameraModels.at(nodeID).size();
5214 image = image(
cv::Range::all(), cv::Range(cameraIndex*subImageWidth, (cameraIndex+1)*subImageWidth));
5217 int x = pointToPixel[
i].second.x * (
float)image.cols;
5218 int y = pointToPixel[
i].second.y * (
float)image.rows;
5222 if(image.type()==CV_8UC3)
5224 cv::Vec3b bgr = image.at<cv::Vec3b>(
y,
x);
5231 UASSERT(image.type()==CV_8UC1);
5232 ptColor.r = ptColor.g = ptColor.b = image.at<
unsigned char>(pointToPixel[
i].second.y * image.rows, pointToPixel[
i].second.x * image.cols);
5238 cloudValidPoints->at(oi++) = ptColor;
5241 cloudValidPoints->resize(oi);
5242 if(!cloudValidPoints->empty())
5244 if(
ui_->doubleSpinBox_voxelSize->value() > 0.0)
5246 cloudValidPoints =
util3d::voxelize(cloudValidPoints,
ui_->doubleSpinBox_voxelSize->value());
5253 UWARN(
"Camera projection to scan returned an empty cloud, no visible points from cameras...");
5258 UERROR(
"Node has invalid camera models, camera projection on scan cannot be done!.");
5261 else if(
ui_->checkBox_showCloud->isChecked() ||
ui_->checkBox_showMesh->isChecked())
5263 if(!
data.depthOrRightRaw().empty())
5265 if(!
data.imageRaw().empty())
5267 std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
5268 std::vector<pcl::IndicesPtr> allIndices;
5269 if(!
data.depthRaw().empty() &&
data.cameraModels().size()==1)
5271 cv::Mat depth =
data.depthRaw();
5272 if(
ui_->spinBox_mesh_fillDepthHoles->value() > 0)
5276 pcl::IndicesPtr
indices(
new std::vector<int>);
5280 data.cameraModels()[0],
5281 ui_->spinBox_decimation->value(),0,0,
indices.get());
5285 allIndices.push_back(
indices);
5292 UASSERT(clouds.size() == allIndices.size());
5293 for(
size_t i=0;
i<allIndices.size(); ++
i)
5295 if(allIndices[
i]->
size())
5297 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = clouds[
i];
5298 pcl::IndicesPtr
indices = allIndices[
i];
5299 if(
ui_->doubleSpinBox_voxelSize->value() > 0.0)
5304 if(
ui_->checkBox_showMesh->isChecked() && !cloud->is_dense)
5306 Eigen::Vector3f viewpoint(0.0
f,0.0
f,0.0
f);
5307 if(
data.cameraModels().size() && !
data.cameraModels()[0].localTransform().isNull())
5309 viewpoint[0] =
data.cameraModels()[0].localTransform().x();
5310 viewpoint[1] =
data.cameraModels()[0].localTransform().y();
5311 viewpoint[2] =
data.cameraModels()[0].localTransform().z();
5313 else if(
data.stereoCameraModels().size() && !
data.stereoCameraModels()[0].localTransform().isNull())
5315 viewpoint[0] =
data.stereoCameraModels()[0].localTransform().x();
5316 viewpoint[1] =
data.stereoCameraModels()[0].localTransform().y();
5317 viewpoint[2] =
data.stereoCameraModels()[0].localTransform().z();
5321 float(
ui_->spinBox_mesh_angleTolerance->value())*
M_PI/180.0f,
5322 ui_->checkBox_mesh_quad->isChecked(),
5323 ui_->spinBox_mesh_triangleSize->value(),
5326 if(
ui_->spinBox_mesh_minClusterSize->value())
5329 std::vector<std::set<int> > neighbors;
5330 std::vector<std::set<int> > vertexToPolygons;
5337 ui_->spinBox_mesh_minClusterSize->value());
5338 std::vector<pcl::Vertices> filteredPolygons(polygons.size());
5342 for(std::list<int>::iterator jter=
iter->begin(); jter!=
iter->end(); ++jter)
5344 filteredPolygons[oi++] = polygons.at(*jter);
5347 filteredPolygons.resize(oi);
5348 polygons = filteredPolygons;
5353 if(
ui_->checkBox_showCloud->isChecked())
5360 else if(
ui_->checkBox_showCloud->isChecked())
5362 std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds;
5363 std::vector<pcl::IndicesPtr> allIndices;
5366 UASSERT(clouds.size() == allIndices.size());
5367 for(
size_t i=0;
i<allIndices.size(); ++
i)
5369 if(allIndices[
i]->
size())
5371 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = clouds[
i];
5372 pcl::IndicesPtr
indices = allIndices[
i];
5373 if(
ui_->doubleSpinBox_voxelSize->value() > 0.0)
5388 if(
data.cameraModels().size())
5399 if(
ui_->checkBox_showWords->isChecked() &&
5400 !signatures.empty() &&
5401 !(*signatures.begin())->getWords3().empty())
5403 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZ>);
5404 cloud->resize((*signatures.begin())->getWords3().size());
5406 for(std::multimap<int, int>::const_iterator
iter=(*signatures.begin())->getWords().begin();
5407 iter!=(*signatures.begin())->getWords().end();
5410 const cv::Point3f & pt = (*signatures.begin())->getWords3()[
iter->second];
5411 cloud->at(
i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
5426 if(
ui_->checkBox_showMap->isChecked() ||
5427 ui_->checkBox_showGrid->isChecked() ||
5428 ui_->checkBox_showElevation->checkState() != Qt::Unchecked)
5435 else if(!
data.gridGroundCellsRaw().empty() || !
data.gridObstacleCellsRaw().empty())
5437 combinedLocalMaps.
add(
data.id(),
data.gridGroundCellsRaw(),
data.gridObstacleCellsRaw(),
data.gridEmptyCellsRaw(),
data.gridCellSize(),
data.gridViewPoint());
5439 if(!combinedLocalMaps.
empty())
5441 std::map<int, Transform> poses;
5442 poses.insert(std::make_pair(
data.id(), pose));
5444 #ifdef RTABMAP_OCTOMAP
5446 if(
ui_->checkBox_octomap->isChecked() &&
5447 (!combinedLocalMaps.
localGrids().begin()->second.groundCells.empty() || !combinedLocalMaps.
localGrids().begin()->second.obstacleCells.empty()) &&
5448 combinedLocalMaps.
localGrids().begin()->second.is3D() &&
5449 combinedLocalMaps.
localGrids().begin()->second.cellSize > 0.0f)
5459 if(
ui_->checkBox_showMap->isChecked())
5461 float xMin=0.0f, yMin=0.0f;
5463 float gridCellSize = Parameters::defaultGridCellSize();
5467 #ifdef RTABMAP_OCTOMAP
5477 map8S = grid.
getMap(xMin, yMin);
5486 if(
ui_->checkBox_showGrid->isChecked())
5488 #ifdef RTABMAP_OCTOMAP
5491 if(
ui_->comboBox_octomap_rendering_type->currentIndex()== 0)
5493 pcl::IndicesPtr obstacles(
new std::vector<int>);
5494 pcl::IndicesPtr
empty(
new std::vector<int>);
5495 pcl::IndicesPtr ground(
new std::vector<int>);
5496 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->
createCloud(
ui_->spinBox_grid_depth->value(), obstacles.get(),
empty.get(), ground.get());
5499 pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(
new pcl::PointCloud<pcl::PointXYZRGB>);
5500 pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
5504 pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(
new pcl::PointCloud<pcl::PointXYZRGB>);
5505 pcl::copyPointCloud(*cloud, *ground, *groundCloud);
5511 pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(
new pcl::PointCloud<pcl::PointXYZ>);
5512 pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
5516 pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(
new pcl::PointCloud<pcl::PointXYZ>);
5517 pcl::copyPointCloud(*cloud, *ground, *groundCloud);
5522 if(
ui_->checkBox_grid_empty->isChecked())
5524 pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCloud(
new pcl::PointCloud<pcl::PointXYZ>);
5525 pcl::copyPointCloud(*cloud, *
empty, *emptyCloud);
5562 if(
ui_->checkBox_grid_empty->isChecked())
5567 QColor(
ui_->lineEdit_emptyColor->text()));
5573 #ifdef RTABMAP_OCTOMAP
5580 #ifdef RTABMAP_GRIDMAP
5581 if(
ui_->checkBox_showElevation->checkState() != Qt::Unchecked)
5583 GridMap gridMap(&combinedLocalMaps, parameters);
5585 if(combinedLocalMaps.
localGrids().begin()->second.is3D())
5588 if(
ui_->checkBox_showElevation->checkState() == Qt::PartiallyChecked)
5590 float xMin, yMin, gridCellSize;
5591 cv::Mat elevationMap = gridMap.
createHeightMap(xMin, yMin, gridCellSize);
5603 UWARN(
"Local grid is not 3D, cannot generate an elevation map");
5614 if(signatures.size())
5616 UASSERT(signatures.front() != 0 && signatures.size() == 1);
5617 delete signatures.front();
5623 std::multimap<int, rtabmap::Link> links;
5627 QString strParents, strChildren;
5628 for(std::multimap<int, rtabmap::Link>::iterator
iter=links.begin();
iter!=links.end(); ++
iter)
5633 if(
iter->first <
id)
5635 strChildren.append(QString(
"%1 ").
arg(
iter->first));
5639 strParents.append(QString(
"%1 ").
arg(
iter->first));
5643 labelParents->setText(strParents);
5644 labelChildren->setText(strChildren);
5650 labelMapId->setText(QString::number(mapId));
5664 int from =
ids_.at(
ui_->horizontalSlider_A->value());
5665 int to =
ids_.at(
ui_->horizontalSlider_B->value());
5674 if(
i !=
ui_->horizontalSlider_loops->value())
5676 ui_->horizontalSlider_loops->blockSignals(
true);
5677 ui_->horizontalSlider_loops->setValue(
i);
5678 ui_->horizontalSlider_loops->blockSignals(
false);
5681 ui_->horizontalSlider_neighbors->blockSignals(
true);
5682 ui_->horizontalSlider_neighbors->setValue(0);
5683 ui_->horizontalSlider_neighbors->blockSignals(
false);
5693 if(
i !=
ui_->horizontalSlider_neighbors->value())
5695 ui_->horizontalSlider_neighbors->blockSignals(
true);
5696 ui_->horizontalSlider_neighbors->setValue(
i);
5697 ui_->horizontalSlider_neighbors->blockSignals(
false);
5700 ui_->horizontalSlider_loops->blockSignals(
true);
5701 ui_->horizontalSlider_loops->setValue(0);
5702 ui_->horizontalSlider_loops->blockSignals(
false);
5710 ui_->horizontalSlider_loops->blockSignals(
true);
5711 ui_->horizontalSlider_neighbors->blockSignals(
true);
5712 ui_->horizontalSlider_loops->setValue(0);
5713 ui_->horizontalSlider_neighbors->setValue(0);
5714 ui_->horizontalSlider_loops->blockSignals(
false);
5715 ui_->horizontalSlider_neighbors->blockSignals(
false);
5720 bool constraintViewUpdated =
false;
5724 constraintViewUpdated =
true;
5729 std::map<int, Transform> optimizedPoses =
uValueAt(
graphes_,
ui_->horizontalSlider_iterations->value());
5730 if(optimizedPoses.size() > 0)
5732 std::map<int, Transform>::iterator fromIter = optimizedPoses.find(from);
5733 std::map<int, Transform>::iterator toIter = optimizedPoses.find(to);
5734 if(fromIter != optimizedPoses.end() &&
5735 toIter != optimizedPoses.end())
5737 Link link(from, to,
Link::kUndef, fromIter->second.inverse() * toIter->second);
5739 constraintViewUpdated =
true;
5743 if(!constraintViewUpdated)
5745 ui_->label_constraint->clear();
5746 ui_->label_constraint_opt->clear();
5747 ui_->label_variance->clear();
5748 ui_->lineEdit_covariance->clear();
5749 ui_->label_type->clear();
5750 ui_->label_type_name->clear();
5751 ui_->checkBox_showOptimized->setEnabled(
false);
5761 if(this->parent() == 0)
5769 if(
ui_->horizontalSlider_A->maximum())
5771 int id =
ids_.at(
ui_->horizontalSlider_A->value());
5774 data.uncompressData();
5782 ui_->dockWidget_stereoView->isVisible() &&
5783 !
data->imageRaw().empty() &&
5784 !
data->depthOrRightRaw().empty() &&
5785 data->depthOrRightRaw().type() == CV_8UC1 &&
5786 data->stereoCameraModels().size()==1 &&
5787 data->stereoCameraModels()[0].isValidForProjection())
5790 if(
data->imageRaw().channels() == 3)
5792 cv::cvtColor(
data->imageRaw(), leftMono, CV_BGR2GRAY);
5796 leftMono =
data->imageRaw();
5804 std::vector<cv::KeyPoint> kpts;
5805 uInsert(parameters,
ParametersPair(Parameters::kKpMaxFeatures(), parameters.at(Parameters::kVisMaxFeatures())));
5807 uInsert(parameters,
ParametersPair(Parameters::kKpMinDepth(), parameters.at(Parameters::kVisMinDepth())));
5808 uInsert(parameters,
ParametersPair(Parameters::kKpMaxDepth(), parameters.at(Parameters::kVisMaxDepth())));
5809 uInsert(parameters,
ParametersPair(Parameters::kKpDetectorStrategy(), parameters.at(Parameters::kVisFeatureType())));
5810 uInsert(parameters,
ParametersPair(Parameters::kKpRoiRatios(), parameters.at(Parameters::kVisRoiRatios())));
5811 uInsert(parameters,
ParametersPair(Parameters::kKpSubPixEps(), parameters.at(Parameters::kVisSubPixEps())));
5812 uInsert(parameters,
ParametersPair(Parameters::kKpSubPixIterations(), parameters.at(Parameters::kVisSubPixIterations())));
5813 uInsert(parameters,
ParametersPair(Parameters::kKpSubPixWinSize(), parameters.at(Parameters::kVisSubPixWinSize())));
5818 float timeKpt =
timer.ticks();
5820 std::vector<cv::Point2f> leftCorners;
5821 cv::KeyPoint::convert(kpts, leftCorners);
5824 std::vector<unsigned char> status;
5825 std::vector<cv::Point2f> rightCorners;
5834 float timeStereo =
timer.ticks();
5836 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
new pcl::PointCloud<pcl::PointXYZ>);
5837 cloud->resize(kpts.size());
5838 float bad_point = std::numeric_limits<float>::quiet_NaN ();
5839 UASSERT(status.size() == kpts.size());
5842 int flowOutliers= 0;
5843 int slopeOutliers= 0;
5844 int negativeDisparityOutliers = 0;
5845 for(
unsigned int i=0;
i<status.size(); ++
i)
5847 cv::Point3f pt(bad_point, bad_point, bad_point);
5850 float disparity = leftCorners[
i].x - rightCorners[
i].x;
5851 if(disparity > 0.0
f)
5856 data->stereoCameraModels()[0]);
5863 cloud->at(oi++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
5869 ++negativeDisparityOutliers;
5879 UINFO(
"correspondences = %d/%d (%f) (time kpt=%fs stereo=%fs)",
5880 (
int)cloud->size(), (
int)leftCorners.size(),
float(cloud->size())/
float(leftCorners.size()), timeKpt, timeStereo);
5886 ui_->label_stereo_inliers->setNum(inliers);
5887 ui_->label_stereo_flowOutliers->setNum(flowOutliers);
5888 ui_->label_stereo_slopeOutliers->setNum(slopeOutliers);
5889 ui_->label_stereo_disparityOutliers->setNum(negativeDisparityOutliers);
5891 std::vector<cv::KeyPoint> rightKpts;
5892 cv::KeyPoint::convert(rightCorners, rightKpts);
5893 std::vector<cv::DMatch> good_matches(kpts.size());
5894 for(
unsigned int i=0;
i<good_matches.size(); ++
i)
5896 good_matches[
i].trainIdx =
i;
5897 good_matches[
i].queryIdx =
i;
5909 ui_->graphicsView_stereo->clear();
5910 ui_->graphicsView_stereo->setLinesShown(
true);
5911 ui_->graphicsView_stereo->setFeaturesShown(
false);
5912 ui_->graphicsView_stereo->setImageDepthShown(
true);
5915 ui_->graphicsView_stereo->setImageDepth(
data->depthOrRightRaw());
5918 for(
unsigned int i=0;
i<kpts.size(); ++
i)
5920 if(rightKpts[
i].pt.x > 0 && rightKpts[
i].pt.y > 0)
5922 QColor
c = Qt::green;
5927 else if(status[
i] == 100)
5931 else if(status[
i] == 101)
5935 else if(status[
i] == 102)
5939 else if(status[
i] == 110)
5943 ui_->graphicsView_stereo->addLine(
5949 QString(
"%1: (%2,%3) -> (%4,%5) d=%6").
arg(
i).
arg(kpts[
i].pt.x).
arg(kpts[
i].pt.y).arg(rightKpts[
i].pt.x).arg(rightKpts[
i].pt.y).arg(kpts[
i].pt.x - rightKpts[
i].pt.x));
5952 ui_->graphicsView_stereo->update();
5958 if(
ui_->actionConcise_Layout->isChecked()) {
5962 int from =
ids_.at(
ui_->horizontalSlider_A->value());
5963 int to =
ids_.at(
ui_->horizontalSlider_B->value());
5966 ui_->graphicsView_A->clearLines();
5967 ui_->graphicsView_A->setFeaturesColor(
ui_->graphicsView_A->getDefaultFeatureColor());
5968 ui_->graphicsView_B->clearLines();
5969 ui_->graphicsView_B->setFeaturesColor(
ui_->graphicsView_B->getDefaultFeatureColor());
5971 const QMultiMap<int, KeypointItem*> & wordsA =
ui_->graphicsView_A->getFeatures();
5972 const QMultiMap<int, KeypointItem*> & wordsB =
ui_->graphicsView_B->getFeatures();
5973 std::set<int> inliersSet(inliers.begin(), inliers.end());
5974 if(wordsA.size() && wordsB.size())
5976 QList<int> ids = wordsA.uniqueKeys();
5977 for(
int i=0;
i<ids.size(); ++
i)
5979 if(ids[
i] > 0 && wordsA.count(ids[
i]) == 1 && wordsB.count(ids[
i]) == 1)
5983 float scaleAX =
ui_->graphicsView_A->viewScale();
5984 float scaleBX =
ui_->graphicsView_B->viewScale();
5986 float marginAX = (
ui_->graphicsView_A->width() -
ui_->graphicsView_A->sceneRect().width()*scaleAX)/2.0
f;
5987 float marginAY = (
ui_->graphicsView_A->height() -
ui_->graphicsView_A->sceneRect().height()*scaleAX)/2.0
f;
5988 float marginBX = (
ui_->graphicsView_B->width() -
ui_->graphicsView_B->sceneRect().width()*scaleBX)/2.0
f;
5989 float marginBY = (
ui_->graphicsView_B->height() -
ui_->graphicsView_B->sceneRect().height()*scaleBX)/2.0
f;
5994 if(
ui_->actionVertical_Layout->isChecked())
5996 deltaY =
ui_->graphicsView_A->height();
6000 deltaX =
ui_->graphicsView_A->width();
6006 QColor cA =
ui_->graphicsView_A->getDefaultMatchingLineColor();
6007 QColor cB =
ui_->graphicsView_B->getDefaultMatchingLineColor();
6008 if(inliersSet.find(ids[
i])!=inliersSet.end())
6010 cA =
ui_->graphicsView_A->getDefaultMatchingFeatureColor();
6011 cB =
ui_->graphicsView_B->getDefaultMatchingFeatureColor();
6012 ui_->graphicsView_A->setFeatureColor(ids[
i],
ui_->graphicsView_A->getDefaultMatchingFeatureColor());
6013 ui_->graphicsView_B->setFeatureColor(ids[
i],
ui_->graphicsView_B->getDefaultMatchingFeatureColor());
6017 ui_->graphicsView_A->setFeatureColor(ids[
i],
ui_->graphicsView_A->getDefaultMatchingLineColor());
6018 ui_->graphicsView_B->setFeatureColor(ids[
i],
ui_->graphicsView_B->getDefaultMatchingLineColor());
6021 ui_->graphicsView_A->addLine(
6024 (kptB->
keypoint().pt.x*scaleBX+marginBX+deltaX-marginAX)/scaleAX,
6025 (kptB->
keypoint().pt.y*scaleBX+marginBY+deltaY-marginAY)/scaleAX,
6028 ui_->graphicsView_B->addLine(
6029 (kptA->
keypoint().pt.x*scaleAX+marginAX-deltaX-marginBX)/scaleBX,
6030 (kptA->
keypoint().pt.y*scaleAX+marginAY-deltaY-marginBY)/scaleBX,
6036 ui_->graphicsView_A->update();
6037 ui_->graphicsView_B->update();
6044 ui_->spinBox_indexA->blockSignals(
true);
6045 ui_->spinBox_indexA->setValue(
value);
6046 ui_->spinBox_indexA->blockSignals(
false);
6049 ui_->label_idA->setText(QString::number(
ids_.at(
value)));
6059 ui_->spinBox_indexB->blockSignals(
true);
6060 ui_->spinBox_indexB->setValue(
value);
6061 ui_->spinBox_indexB->blockSignals(
false);
6064 ui_->label_idB->setText(QString::number(
ids_.at(
value)));
6105 int priorId = sender() ==
ui_->toolButton_edit_priorA?
ids_.at(
ui_->horizontalSlider_A->value()):
6106 sender() ==
ui_->toolButton_edit_priorB?
ids_.at(
ui_->horizontalSlider_B->value()):0;
6123 QMessageBox::warning(
this, tr(
""), tr(
"Node %1 doesn't have odometry pose, cannot add a prior for it!").
arg(priorId));
6133 link = findIter->second;
6143 cv::Mat covBefore = link.
infMatrix().inv();
6145 if(dialog.exec() == QDialog::Accepted)
6152 if(
iter->second.to() == link.
to() &&
6153 iter->second.type() == link.
type())
6155 iter->second = newLink;
6176 link.
transform(), cv::Mat::eye(6,6,CV_64FC1) * (priorId>0?0.00001:1));
6177 if(dialog.exec() == QDialog::Accepted)
6180 int from = priorId>0?priorId:
ids_.at(
ui_->horizontalSlider_A->value());
6181 int to = priorId>0?priorId:
ids_.at(
ui_->horizontalSlider_B->value());
6188 if(newLink.
from() < newLink.
to())
6204 bool priorsIgnored = Parameters::defaultOptimizerPriorsIgnored();
6205 Parameters::parse(
ui_->parameters_toolbox->getParameters(), Parameters::kOptimizerPriorsIgnored(), priorsIgnored);
6208 if(QMessageBox::question(
this,
6209 tr(
"Updating Prior"),
6210 tr(
"Parameter %1 is true, do you want to turn it off to update the graph with the updated prior?").
arg(Parameters::kOptimizerPriorsIgnored().
c_str()),
6211 QMessageBox::Yes | QMessageBox::No,
6212 QMessageBox::Yes) == QMessageBox::Yes)
6214 priorsIgnored =
false;
6215 ui_->parameters_toolbox->updateParameter(Parameters::kOptimizerPriorsIgnored(),
"false");
6218 int indexA =
ui_->horizontalSlider_A->value();
6219 int indexB =
ui_->horizontalSlider_B->value();
6224 if(
ui_->horizontalSlider_A->value() != indexA)
6225 ui_->horizontalSlider_A->setValue(indexA);
6228 if(
ui_->horizontalSlider_B->value() != indexB)
6229 ui_->horizontalSlider_B->setValue(indexB);
6247 ui_->horizontalSlider_neighbors->value() >= 0 &&
ui_->horizontalSlider_neighbors->value() <
neighborLinks_.size())
6252 ui_->horizontalSlider_loops->value() >= 0 &&
ui_->horizontalSlider_loops->value() <
loopLinks_.size())
6266 bool updateImageSliders,
6276 if(iterLink->second.from() == link.
to())
6278 link = iterLink->second.
inverse();
6282 link = iterLink->second;
6285 else if(
ui_->checkBox_ignorePoseCorrection->isChecked())
6313 ui_->label_constraint->clear();
6314 ui_->label_constraint_opt->clear();
6315 ui_->checkBox_showOptimized->setEnabled(
false);
6318 ui_->label_type->setText(QString::number(link.
type()));
6319 ui_->label_type_name->setText(tr(
"(%1)")
6329 ui_->label_variance->setText(QString(
"%1, %2")
6332 std::stringstream
out;
6334 ui_->lineEdit_covariance->setText(
out.str().c_str());
6335 ui_->label_constraint->setText(QString(
"%1").
arg(
t.prettyPrint().c_str()).replace(
" ",
"\n"));
6337 (
int)
graphes_.size()-1 ==
ui_->horizontalSlider_iterations->maximum())
6341 std::map<int, rtabmap::Transform>::iterator iterFrom =
graph.find(link.
from());
6342 std::map<int, rtabmap::Transform>::iterator iterTo =
graph.find(link.
to());
6343 if(iterFrom !=
graph.end() && iterTo !=
graph.end())
6345 ui_->checkBox_showOptimized->setEnabled(
true);
6350 float a = pcl::getAngle3D(Eigen::Vector4f(
v1.x(),
v1.y(),
v1.z(), 0), Eigen::Vector4f(
v2.x(),
v2.y(),
v2.z(), 0));
6351 a = (
a *180.0f) / CV_PI;
6352 ui_->label_constraint_opt->setText(QString(
"%1\n(error=%2% a=%3)").arg(QString(topt.
prettyPrint().c_str()).replace(
" ",
"\n")).arg((
t.getNorm()>0?
diff/
t.getNorm():0)*100.0f).arg(
a));
6354 if(
ui_->checkBox_showOptimized->isChecked())
6361 if(updateImageSliders)
6363 ui_->horizontalSlider_A->blockSignals(
true);
6364 ui_->horizontalSlider_B->blockSignals(
true);
6370 ui_->horizontalSlider_A->blockSignals(
false);
6371 ui_->horizontalSlider_B->blockSignals(
false);
6374 ui_->spinBox_indexA,
6375 ui_->label_parentsA,
6376 ui_->label_childrenA,
6380 ui_->graphicsView_A,
6384 ui_->label_optposeA,
6388 ui_->label_gravityA,
6390 ui_->toolButton_edit_priorA,
6391 ui_->toolButton_remove_priorA,
6394 ui_->label_sensorsA,
6399 ui_->spinBox_indexB,
6400 ui_->label_parentsB,
6401 ui_->label_childrenB,
6405 ui_->graphicsView_B,
6409 ui_->label_optposeB,
6413 ui_->label_gravityB,
6415 ui_->toolButton_edit_priorB,
6416 ui_->toolButton_remove_priorB,
6419 ui_->label_sensorsB,
6428 if(signatureFrom.
id()>0)
6440 if(signatureTo.
id()>0)
6444 else if(link.
to()>0)
6454 if(
ui_->checkBox_odomFrame->isChecked())
6460 std::vector<float>
v;
6476 if(
ui_->checkBox_show3Dclouds->isChecked())
6478 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom, cloudTo;
6479 pcl::IndicesPtr indicesFrom(
new std::vector<int>);
6480 pcl::IndicesPtr indicesTo(
new std::vector<int>);
6490 if(cloudTo.get() && indicesTo->size())
6496 if(
ui_->doubleSpinBox_gainCompensationRadius->value()>0.0 &&
6497 cloudFrom.get() && indicesFrom->size() &&
6498 cloudTo.get() && indicesTo->size())
6503 compensator.
apply(0, cloudFrom, indicesFrom);
6504 compensator.
apply(1, cloudTo, indicesTo);
6505 UINFO(
"Gain compensation time = %fs",
t.ticks());
6508 if(cloudFrom.get() && indicesFrom->size())
6510 if(
ui_->doubleSpinBox_voxelSize->value() > 0.0)
6512 cloudFrom =
util3d::voxelize(cloudFrom, indicesFrom,
ui_->doubleSpinBox_voxelSize->value());
6516 if(cloudTo.get() && indicesTo->size())
6518 if(
ui_->doubleSpinBox_voxelSize->value() > 0.0)
6528 if(
ui_->checkBox_show3DWords->isChecked())
6531 ids.push_back(link.
from());
6534 ids.push_back(link.
to());
6536 std::list<Signature*> signatures;
6538 if(signatures.size() == 2 || (link.
to()<0 && signatures.size()==1))
6540 const Signature * sFrom = signatureFrom.
id()>0?&signatureFrom:signatures.front();
6542 if(signatures.size()==2)
6544 sTo = signatureTo.
id()>0?&signatureTo:signatures.back();
6548 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom(
new pcl::PointCloud<pcl::PointXYZ>);
6549 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo(
new pcl::PointCloud<pcl::PointXYZ>);
6550 cloudFrom->resize(sFrom->
getWords3().size());
6553 cloudTo->resize(sTo->
getWords3().size());
6558 for(std::multimap<int, int>::const_iterator
iter=sFrom->
getWords().begin();
6563 cloudFrom->at(
i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
6569 for(std::multimap<int, int>::const_iterator
iter=sTo->
getWords().begin();
6574 cloudTo->at(
i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
6578 if(cloudFrom->size())
6591 if(cloudFrom->size())
6597 UWARN(
"Empty 3D words for node %d", link.
from());
6608 UWARN(
"Empty 3D words for node %d", link.
to());
6615 UERROR(
"Not found signature %d or %d in RAM", link.
from(), link.
to());
6620 for(std::list<Signature*>::iterator
iter=signatures.begin();
iter!=signatures.end(); ++
iter)
6630 if(
ui_->checkBox_show2DScans->isChecked())
6635 signatureTo.
id()==0)
6637 std::vector<int> ids;
6639 if(userData.type() == CV_8SC1 &&
6640 userData.rows == 1 &&
6641 userData.cols >= 8 &&
6642 userData.at<
char>(userData.cols-1) == 0 &&
6643 memcmp(userData.data,
"SCANS:", 6) == 0)
6645 std::string scansStr = (
const char *)userData.data;
6646 UINFO(
"Detected \"%s\" in links's user data", scansStr.c_str());
6647 if(!scansStr.empty())
6649 std::list<std::string> strs =
uSplit(scansStr,
':');
6650 if(strs.size() == 2)
6652 std::list<std::string> strIds =
uSplit(strs.rbegin()->c_str(),
';');
6653 for(std::list<std::string>::iterator
iter=strIds.begin();
iter!=strIds.end(); ++
iter)
6655 ids.push_back(atoi(
iter->c_str()));
6656 if(ids.back() == link.
from())
6669 std::map<int, rtabmap::Transform> poses;
6670 for(
unsigned int i=0;
i<ids.size(); ++
i)
6678 UERROR(
"Not found %d node!", ids[
i]);
6686 std::map<int, rtabmap::Transform> posesOut;
6687 std::multimap<int, rtabmap::Link> linksOut;
6695 if(poses.size() != posesOut.size())
6697 UWARN(
"Scan poses input and output are different! %d vs %d", (
int)poses.size(), (
int)posesOut.size());
6701 for(std::map<int, Transform>::iterator
iter=posesOut.begin();
iter!=posesOut.end(); ++
iter)
6703 UDEBUG(
" %d=%s",
iter->first,
iter->second.prettyPrint().c_str());
6706 for(std::multimap<int, Link>::iterator
iter=linksOut.begin();
iter!=linksOut.end(); ++
iter)
6708 UDEBUG(
" %d->%d (type=%s) %s",
iter->second.from(),
iter->second.to(),
iter->second.typeName().c_str(),
iter->second.transform().prettyPrint().c_str());
6711 std::map<int, rtabmap::Transform> finalPoses = optimizer->
optimize(link.
to(), posesOut, linksOut);
6714 UDEBUG(
"Output poses: ");
6715 for(std::map<int, Transform>::iterator
iter=finalPoses.begin();
iter!=finalPoses.end(); ++
iter)
6717 UDEBUG(
" %d=%s",
iter->first,
iter->second.prettyPrint().c_str());
6722 pcl::PointCloud<pcl::PointXYZ>::Ptr assembledScans(
new pcl::PointCloud<pcl::PointXYZ>);
6723 pcl::PointCloud<pcl::PointNormal>::Ptr assembledNormalScans(
new pcl::PointCloud<pcl::PointNormal>);
6724 pcl::PointCloud<pcl::PointXYZI>::Ptr assembledIScans(
new pcl::PointCloud<pcl::PointXYZI>);
6725 pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembledINormalScans(
new pcl::PointCloud<pcl::PointXYZINormal>);
6726 pcl::PointCloud<pcl::PointXYZ>::Ptr
graph(
new pcl::PointCloud<pcl::PointXYZ>);
6727 for(std::map<int, Transform>::iterator
iter=finalPoses.begin();
iter!=finalPoses.end(); ++
iter)
6730 if(
iter->first != link.
to())
6736 data.uncompressDataConst(0, 0, &scan, 0);
6760 if(assembledNormalScans->size())
6765 if(assembledScans->size())
6770 if(assembledINormalScans->size())
6775 if(assembledIScans->size())
6795 pcl::PointCloud<pcl::PointXYZINormal>::Ptr scan;
6802 pcl::PointCloud<pcl::PointNormal>::Ptr scan;
6809 pcl::PointCloud<pcl::PointXYZI>::Ptr scan;
6816 pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
6826 pcl::PointCloud<pcl::PointXYZINormal>::Ptr scan;
6833 pcl::PointCloud<pcl::PointNormal>::Ptr scan;
6840 pcl::PointCloud<pcl::PointXYZI>::Ptr scan;
6847 pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
6883 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
6904 ui_->pushButton_refine->setEnabled(
false);
6905 ui_->pushButton_reset->setEnabled(
false);
6906 ui_->pushButton_add->setEnabled(
false);
6907 ui_->pushButton_reject->setEnabled(
false);
6908 ui_->toolButton_constraint->setEnabled(
false);
6916 currentLink =
loopLinks_.at(
ui_->horizontalSlider_loops->value());
6917 from = currentLink.
from();
6918 to = currentLink.
to();
6922 from =
ids_.at(
ui_->horizontalSlider_A->value());
6923 to =
ids_.at(
ui_->horizontalSlider_B->value());
6924 if(from!=to && from && to &&
6927 (
ui_->checkBox_enableForAll->isChecked() ||
6934 ui_->pushButton_add->setEnabled(
true);
6937 else if(
ui_->checkBox_enableForAll->isChecked())
6941 UWARN(
"Button \"Add\" cannot be enabled even if \"all\" checkbox is checked, as node %d doesn't have odometry set.", from);
6945 UWARN(
"Button \"Add\" cannot be enabled even if \"all\" checkbox is checked, as node %d doesn't have odometry set.", to);
6953 ((currentLink.
from() == from && currentLink.
to() == to) || (currentLink.
from() == to && currentLink.
to() == from)))
6957 ui_->pushButton_reject->setEnabled(
true);
6964 currentLink =
iter->second;
6965 ui_->pushButton_reset->setEnabled(
true);
6968 ui_->toolButton_constraint->setEnabled(
true);
6979 bool alignToGPS =
ui_->checkBox_alignPosesWithGPS->isEnabled() &&
ui_->checkBox_alignPosesWithGPS->isChecked();
6990 if(refPoses.size() ==
graph.size() &&
length >= 100.0f)
6995 UINFO(
"KITTI t_err = %f %%", t_err);
6996 UINFO(
"KITTI r_err = %f deg/m", r_err);
6999 float translational_rmse = 0.0f;
7000 float translational_mean = 0.0f;
7001 float translational_median = 0.0f;
7002 float translational_std = 0.0f;
7003 float translational_min = 0.0f;
7004 float translational_max = 0.0f;
7005 float rotational_rmse = 0.0f;
7006 float rotational_mean = 0.0f;
7007 float rotational_median = 0.0f;
7008 float rotational_std = 0.0f;
7009 float rotational_min = 0.0f;
7010 float rotational_max = 0.0f;
7017 translational_median,
7030 ui_->label_rmse->setNum(translational_rmse);
7031 UINFO(
"translational_rmse=%f", translational_rmse);
7032 UINFO(
"translational_mean=%f", translational_mean);
7033 UINFO(
"translational_median=%f", translational_median);
7034 UINFO(
"translational_std=%f", translational_std);
7035 UINFO(
"translational_min=%f", translational_min);
7036 UINFO(
"translational_max=%f", translational_max);
7038 UINFO(
"rotational_rmse=%f", rotational_rmse);
7039 UINFO(
"rotational_mean=%f", rotational_mean);
7040 UINFO(
"rotational_median=%f", rotational_median);
7041 UINFO(
"rotational_std=%f", rotational_std);
7042 UINFO(
"rotational_min=%f", rotational_min);
7043 UINFO(
"rotational_max=%f", rotational_max);
7046 (
ui_->checkBox_alignPosesWithGroundTruth->isEnabled() &&
ui_->checkBox_alignPosesWithGroundTruth->isChecked())) &&
7051 iter->second = gtToMap *
iter->second;
7056 std::map<int, rtabmap::Transform> graphFiltered;
7057 if(
ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() &&
7058 ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() &&
7065 graphFiltered =
graph;
7067 if(
ui_->groupBox_posefiltering->isChecked())
7070 ui_->doubleSpinBox_posefilteringRadius->value(),
7071 ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
7074 #ifdef RTABMAP_OCTOMAP
7078 if(
ui_->dockWidget_graphView->isVisible() ||
ui_->dockWidget_occupancyGridView->isVisible())
7081 UINFO(
"Update local maps list...");
7082 std::vector<int> ids =
uKeys(graphFiltered);
7083 for(
unsigned int i=0;
i<ids.size(); ++
i)
7090 else if(ids.at(
i)>0)
7094 cv::Mat ground, obstacles,
empty;
7095 if(
data.gridCellSize()>0.0f)
7097 data.uncompressData(0, 0, 0, 0, &ground, &obstacles, &
empty);
7099 localMaps_.
add(ids.at(
i), ground, obstacles,
empty,
data.gridCellSize()>0.0f?
data.gridCellSize():Parameters::defaultGridCellSize(),
data.gridViewPoint());
7100 if(!ground.empty() || !obstacles.empty())
7106 UINFO(
"Update local maps list... done (%d local maps, graph size=%d)", (
int)combinedLocalMaps.
size(), (
int)
graph.size());
7110 float cellSize = Parameters::defaultGridCellSize();
7116 if(
ui_->checkBox_wmState->isEnabled() &&
7117 ui_->checkBox_wmState->isChecked() &&
7120 bool allNodesAreInWM =
true;
7121 std::map<int, float> colors;
7126 colors.insert(std::make_pair(
iter->first, 1));
7130 allNodesAreInWM =
false;
7133 if(!allNodesAreInWM)
7135 ui_->graphViewer->updatePosterior(colors, 1, 1);
7138 QGraphicsRectItem * rectScaleItem = 0;
7139 ui_->graphViewer->clearMap();
7141 if(
graph.size() && combinedLocalMaps.
size() &&
7142 (
ui_->graphViewer->isGridMapVisible() ||
ui_->dockWidget_occupancyGridView->isVisible()))
7147 #ifdef RTABMAP_OCTOMAP
7148 if(
ui_->checkBox_octomap->isChecked())
7155 if((
ui_->dockWidget_graphView->isVisible() &&
ui_->graphViewer->isGridMapVisible()) ||
7156 (
ui_->dockWidget_occupancyGridView->isVisible() &&
ui_->checkBox_grid_2d->isChecked()))
7158 bool eroded = Parameters::defaultGridGlobalEroded();
7163 #ifdef RTABMAP_OCTOMAP
7164 if(
ui_->checkBox_octomap->isChecked())
7176 grid.
update(graphFiltered);
7177 if(
ui_->checkBox_grid_showProbMap->isChecked())
7183 map = grid.
getMap(xMin, yMin);
7187 ui_->label_timeGrid->setNum(
double(
time.elapsed())/1000.0);
7192 if(
ui_->dockWidget_graphView->isVisible() &&
ui_->graphViewer->isGridMapVisible())
7194 ui_->graphViewer->updateMap(map8U, cellSize, xMin, yMin);
7196 if(
ui_->dockWidget_occupancyGridView->isVisible() &&
ui_->checkBox_grid_2d->isChecked())
7205 int xLast = map.cols;
7206 int yLast = map.rows;
7207 bool firstSet =
false;
7208 bool lastSet =
false;
7209 for(
int x=0;
x<map.cols && (!firstSet || !lastSet); ++
x)
7211 for(
int y=0;
y<map.rows; ++
y)
7214 if(!firstSet && map.at<
signed char>(
y,
x) != -1)
7220 int opp = map.cols-(
x+1);
7221 if(!lastSet && map.at<
signed char>(
y, opp) != -1)
7230 for(
int y=0;
y<map.rows && (!firstSet || !lastSet); ++
y)
7232 for(
int x=0;
x<map.cols; ++
x)
7235 if(!firstSet && map.at<
signed char>(
y,
x) != -1)
7241 int opp = map.rows-(
y+1);
7242 if(!lastSet && map.at<
signed char>(map.rows-(
y+1),
x) != -1)
7250 if( (xLast > xFirst && yLast > yFirst) &&
7252 xLast < map.cols-50 ||
7254 yLast < map.rows-50))
7256 rectScaleItem =
ui_->graphViewer->scene()->addRect(
7261 rectScaleItem->setTransform(QTransform::fromScale(cellSize*100.0
f, -cellSize*100.0
f),
true);
7262 rectScaleItem->setRotation(90);
7263 rectScaleItem->setPos(-yMin*100.0
f, -xMin*100.0
f);
7269 if(
ui_->dockWidget_occupancyGridView->isVisible() &&
ui_->checkBox_grid_grid->isChecked())
7271 #ifdef RTABMAP_OCTOMAP
7272 if(
ui_->checkBox_octomap->isChecked())
7279 CloudMap cloudMap(&combinedLocalMaps, parameters);
7281 cloudMap.
update(graphFiltered);
7283 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & groundCells = cloudMap.
getMapGround();
7284 const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacleCells = cloudMap.
getMapObstacles();
7285 const pcl::PointCloud<pcl::PointXYZ>::Ptr & emptyCells = cloudMap.
getMapEmptyCells();
7288 if(groundCells->size())
7293 QColor(
ui_->lineEdit_groundColor->text()));
7296 if(obstacleCells->size())
7301 QColor(
ui_->lineEdit_obstacleColor->text()));
7304 if(
ui_->checkBox_grid_empty->isChecked() && emptyCells->size())
7309 QColor(
ui_->lineEdit_emptyColor->text()));
7317 #ifdef RTABMAP_GRIDMAP
7319 if(
ui_->dockWidget_occupancyGridView->isVisible() &&
7320 ui_->checkBox_grid_elevation->checkState() != Qt::Unchecked)
7322 GridMap gridMap(&combinedLocalMaps, parameters);
7324 gridMap.
update(graphFiltered);
7325 if(
ui_->checkBox_grid_elevation->checkState() == Qt::PartiallyChecked)
7340 ui_->graphViewer->fitInView(
ui_->graphViewer->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
7341 if(rectScaleItem != 0)
7343 ui_->graphViewer->fitInView(rectScaleItem, Qt::KeepAspectRatio);
7344 ui_->graphViewer->scene()->removeItem(rectScaleItem);
7345 delete rectScaleItem;
7348 ui_->graphViewer->update();
7349 ui_->label_iterations->setNum(
value);
7355 std::map<int, rtabmap::Transform>::const_iterator jterA =
graph.find(
iter->first);
7356 std::map<int, rtabmap::Transform>::const_iterator jterB =
graph.find(
iter->second.to());
7357 if(jterA !=
graph.end() && jterB !=
graph.end())
7364 Eigen::Vector3f
vA,
vB;
7380 if(
ui_->horizontalSlider_rotation->isEnabled())
7382 float theta =
float(
ui_->horizontalSlider_rotation->value())*
M_PI/1800.0f;
7383 ui_->graphViewer->setWorldMapRotation(theta);
7384 ui_->label_rotation->setText(QString::number(
float(-
ui_->horizontalSlider_rotation->value())/10.0f,
'f', 1) +
" deg");
7388 ui_->graphViewer->setWorldMapRotation(0);
7394 ui_->label_loopClosures->clear();
7395 ui_->label_poses->clear();
7396 ui_->label_rmse->clear();
7398 if(sender() ==
ui_->checkBox_alignPosesWithGPS &&
ui_->checkBox_alignPosesWithGPS->isChecked())
7400 ui_->checkBox_alignPosesWithGroundTruth->setChecked(
false);
7402 else if(sender() ==
ui_->checkBox_alignPosesWithGroundTruth &&
ui_->checkBox_alignPosesWithGroundTruth->isChecked())
7404 ui_->checkBox_alignPosesWithGPS->setChecked(
false);
7409 int fromId =
ui_->spinBox_optimizationsFrom->value();
7412 QMessageBox::warning(
this, tr(
""), tr(
"Graph optimization from id (%1) for which node is not linked to graph.\n Minimum=%2, Maximum=%3")
7422 std::map<int, rtabmap::Transform> poses =
odomPoses_;
7423 if(
ui_->checkBox_wmState->isEnabled() &&
7424 ui_->checkBox_wmState->isChecked() &&
7427 std::map<int, rtabmap::Transform> wmPoses;
7428 std::vector<int> & wmState =
wmStates_.at(fromId);
7429 for(
unsigned int i=0;
i<wmState.size(); ++
i)
7431 std::map<int, rtabmap::Transform>::iterator
iter = poses.find(wmState[
i]);
7432 if(
iter!=poses.end())
7434 wmPoses.insert(*
iter);
7437 if(!wmPoses.empty())
7443 UWARN(
"Empty WM poses!? Ignoring WM state... (root id=%d, wmState=%d)", fromId, wmState.size());
7450 int currentMapId =
mapIds_.at(fromId);
7451 for(std::map<int, rtabmap::Transform>::iterator
iter=poses.begin();
iter!=poses.end();)
7455 poses.erase(
iter++);
7464 ui_->menuExport_poses->setEnabled(
true);
7465 std::multimap<int, rtabmap::Link> links =
links_;
7470 int currentMapId =
mapIds_.at(fromId);
7471 for(std::multimap<int, rtabmap::Link>::iterator
iter=links.begin();
iter!=links.end();)
7476 links.erase(
iter++);
7486 if(
ui_->checkBox_ignorePoseCorrection->isChecked())
7488 for(std::multimap<int, Link>::iterator
iter=links.begin();
iter!=links.end(); ++
iter)
7499 iter->second.from(),
7501 iter->second.type(),
7509 double markerPriorsLinearVariance = Parameters::defaultMarkerPriorsVarianceLinear();
7510 double markerPriorsAngularVariance = Parameters::defaultMarkerPriorsVarianceAngular();
7511 std::map<int, Transform> markerPriors;
7513 Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), markerPriorsLinearVariance);
7514 UASSERT(markerPriorsLinearVariance>0.0
f);
7515 Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceAngular(), markerPriorsAngularVariance);
7516 UASSERT(markerPriorsAngularVariance>0.0
f);
7517 std::string markerPriorsStr;
7520 std::list<std::string> strList =
uSplit(markerPriorsStr,
'|');
7521 for(std::list<std::string>::iterator
iter=strList.begin();
iter!=strList.end(); ++
iter)
7523 std::string markerStr = *
iter;
7524 while(!markerStr.empty() && !
uIsDigit(markerStr[0]))
7526 markerStr.erase(markerStr.begin());
7528 if(!markerStr.empty())
7533 if(!
prior.isNull() &&
id>0)
7535 markerPriors.insert(std::make_pair(-
id,
prior));
7536 UDEBUG(
"Added landmark prior %d: %s",
id,
prior.prettyPrint().c_str());
7540 UERROR(
"Failed to parse element \"%s\" in parameter %s", markerStr.c_str(), Parameters::kMarkerPriors().c_str());
7543 else if(!
iter->empty())
7545 UERROR(
"Failed to parse parameter %s, value=\"%s\"", Parameters::kMarkerPriors().
c_str(),
iter->c_str());
7552 int totalNeighbor = 0;
7553 int totalNeighborMerged = 0;
7554 int totalGlobal = 0;
7555 int totalLocalTime = 0;
7556 int totalLocalSpace = 0;
7558 int totalPriors = 0;
7559 int totalLandmarks = 0;
7560 int totalGravity = 0;
7561 std::multimap<int, int> uniqueLinks;
7562 for(std::multimap<int, rtabmap::Link>::iterator
iter=links.begin();
iter!=links.end();)
7564 bool isUnique =
iter->second.from() ==
iter->second.to();
7567 uniqueLinks.insert(std::make_pair(
iter->second.from(),
iter->second.to()));
7579 ++totalNeighborMerged;
7583 if(
ui_->checkBox_ignoreGlobalLoop->isChecked())
7585 links.erase(
iter++);
7593 if(
ui_->checkBox_ignoreLocalLoopSpace->isChecked())
7595 links.erase(
iter++);
7603 if(
ui_->checkBox_ignoreLocalLoopTime->isChecked())
7605 links.erase(
iter++);
7613 if(
ui_->checkBox_ignoreUserLoop->isChecked())
7615 links.erase(
iter++);
7623 if(
ui_->checkBox_ignoreLandmarks->isChecked())
7625 links.erase(
iter++);
7629 if(poses.find(
iter->second.from()) != poses.end() && poses.find(
iter->second.to()) == poses.end())
7631 poses.insert(std::make_pair(
iter->second.to(), poses.at(
iter->second.from())*
iter->second.transform()));
7637 int markerId =
iter->second.to();
7638 if(markerPriors.find(markerId) != markerPriors.end())
7640 cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64FC1);
7641 infMatrix(cv::Range(0,3), cv::Range(0,3)) /= markerPriorsLinearVariance;
7642 infMatrix(cv::Range(3,6), cv::Range(3,6)) /= markerPriorsAngularVariance;
7643 links.insert(std::make_pair(markerId,
Link(markerId, markerId,
Link::kPosePrior, markerPriors.at(markerId), infMatrix)));
7644 UDEBUG(
"Added prior %d : %s (variance: lin=%f ang=%f)", markerId, markerPriors.at(markerId).prettyPrint().c_str(),
7645 markerPriorsLinearVariance, markerPriorsAngularVariance);
7664 ui_->label_loopClosures->setText(tr(
"(%1, %2, %3, %4, %5, %6, %7, %8, %9)")
7666 .
arg(totalNeighborMerged)
7668 .
arg(totalLocalSpace)
7669 .
arg(totalLocalTime)
7672 .
arg(totalLandmarks)
7673 .
arg(totalGravity));
7676 if(
ui_->checkBox_ignoreIntermediateNodes->isVisible() &&
7677 ui_->checkBox_ignoreIntermediateNodes->isEnabled() &&
7678 ui_->checkBox_ignoreIntermediateNodes->isChecked())
7680 for(std::multimap<int, Link>::iterator
iter=links.begin();
iter!=links.end(); ++
iter)
7688 std::multimap<int, Link>::iterator uter = links.find(link.
to());
7689 while(uter != links.end() &&
7690 uter->first==link.
to() &&
7691 uter->second.from()>uter->second.to())
7695 if(uter != links.end())
7697 poses.erase(link.
to());
7698 link = link.
merge(uter->second, uter->second.type());
7699 links.erase(uter->first);
7707 iter->second = link;
7712 bool applyRotation = sender() ==
ui_->pushButton_applyRotation;
7715 float xMin, yMin, cellSize;
7719 QMessageBox::StandardButton r = QMessageBox::question(
this,
7720 tr(
"Rotate Optimized Graph"),
7721 tr(
"There is a 2D occupancy grid or mesh already saved in "
7722 "database. Applying rotation will clear them (they can be "
7723 "regenerated later from File menu options). "
7724 "Do you want to continue?"),
7725 QMessageBox::Cancel | QMessageBox::Yes,
7726 QMessageBox::Cancel);
7727 if(r != QMessageBox::Yes)
7729 applyRotation =
false;
7734 std::map<int, Transform> optPoses;
7737 ui_->comboBox_optimizationFlavor->currentIndex() == 2)
7740 if(optPoses.empty())
7742 ui_->comboBox_optimizationFlavor->setCurrentIndex(0);
7743 QMessageBox::warning(
this, tr(
"Optimization Flavor"),
7744 tr(
"There is no local optimized graph in the database, "
7745 "falling back to global iterative optimization."));
7750 ui_->comboBox_optimizationFlavor->currentIndex() != 2)
7752 if(
ui_->horizontalSlider_rotation->value()!=0 && applyRotation)
7754 float theta =
float(-
ui_->horizontalSlider_rotation->value())*
M_PI/1800.0f;
7756 poses.at(fromId) = rotT * poses.at(fromId);
7763 std::map<int, rtabmap::Transform> posesOut;
7764 std::multimap<int, rtabmap::Link> linksOut;
7765 UINFO(
"Get connected graph from %d (%d poses, %d links)", fromId, (
int)poses.size(), (
int)links.size());
7772 UINFO(
"Connected graph of %d poses and %d links", (
int)posesOut.size(), (
int)linksOut.size());
7775 std::map<int, rtabmap::Transform> finalPoses = optimizer->
optimize(fromId, posesOut, linksOut,
ui_->comboBox_optimizationFlavor->currentIndex()==0?&
graphes_:0);
7776 ui_->label_timeOptimization->setNum(
double(
time.elapsed())/1000.0);
7778 if(posesOut.size() && finalPoses.empty())
7780 UWARN(
"Optimization failed... (poses=%d, links=%d).", (
int)posesOut.size(), (
int)linksOut.size());
7783 QMessageBox::warning(
this, tr(
"Graph optimization error!"), tr(
"Graph optimization has failed. See the terminal for potential errors. "
7784 "Give it a try with %1=0 and %2=true.").
arg(Parameters::kOptimizerStrategy().
c_str()).
arg(Parameters::kOptimizerVarianceIgnored().
c_str()));
7788 QMessageBox::warning(
this, tr(
"Graph optimization error!"), tr(
"Graph optimization has failed. See the terminal for potential errors."));
7791 ui_->label_poses->setNum((
int)finalPoses.size());
7795 if(applyRotation && !finalPoses.empty())
7797 ui_->comboBox_optimizationFlavor->setCurrentIndex(2);
7800 if(lastLocalizationPose.
isNull())
7803 lastLocalizationPose = finalPoses.rbegin()->second;
7807 float xMin, yMin, cellSize;
7813 QMessageBox::StandardButton r = QMessageBox::question(
this,
7814 tr(
"Rotate Optimized Graph"),
7815 tr(
"Optimized graph has been rotated and saved back to database. "
7816 "Note that 2D occupancy grid and mesh have been cleared (if set). "
7817 "Do you want to regenerate the 2D occupancy grid now "
7818 "(can be done later from File menu)?"),
7819 QMessageBox::Ignore | QMessageBox::Yes,
7821 ui_->actionEdit_optimized_2D_map->setEnabled(
false);
7822 ui_->actionExport_saved_2D_map->setEnabled(
false);
7823 ui_->actionImport_2D_map->setEnabled(
false);
7824 ui_->actionView_optimized_mesh->setEnabled(
false);
7825 ui_->actionExport_optimized_mesh->setEnabled(
false);
7826 if(r == QMessageBox::Yes)
7835 if(
ui_->comboBox_optimizationFlavor->currentIndex() == 2)
7840 ui_->label_timeOptimization->setNum(0);
7841 ui_->label_poses->setNum((
int)optPoses.size());
7844 ui_->horizontalSlider_rotation->setEnabled(
false);
7845 ui_->pushButton_applyRotation->setEnabled(
false);
7846 ui_->spinBox_optimizationsFrom->setEnabled(
false);
7847 ui_->checkBox_spanAllMaps->setEnabled(
false);
7848 ui_->checkBox_wmState->setEnabled(
false);
7849 ui_->checkBox_alignPosesWithGPS->setEnabled(
false);
7850 ui_->checkBox_alignPosesWithGroundTruth->setEnabled(
false);
7851 ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(
false);
7852 ui_->checkBox_ignoreIntermediateNodes->setEnabled(
false);
7857 ui_->pushButton_applyRotation->setEnabled(
true);
7858 ui_->horizontalSlider_rotation->setEnabled(
true);
7859 ui_->spinBox_optimizationsFrom->setEnabled(
true);
7860 ui_->checkBox_spanAllMaps->setEnabled(
true);
7861 ui_->checkBox_wmState->setEnabled(
true);
7862 ui_->checkBox_alignPosesWithGPS->setEnabled(
ui_->checkBox_alignPosesWithGPS->isVisible());
7863 ui_->checkBox_alignPosesWithGroundTruth->setEnabled(
ui_->checkBox_alignPosesWithGroundTruth->isVisible());
7864 ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(
ui_->checkBox_alignScansCloudsWithGroundTruth->isVisible());
7865 ui_->checkBox_ignoreIntermediateNodes->setEnabled(
true);
7871 if(
ui_->doubleSpinBox_optimizationScale->value()!=-1.0)
7876 for(std::map<int, Transform>::iterator jter=
iter->begin(); jter!=
iter->end(); ++jter)
7878 jter->second = jter->second.clone();
7879 jter->second.x() *=
ui_->doubleSpinBox_optimizationScale->value();
7880 jter->second.y() *=
ui_->doubleSpinBox_optimizationScale->value();
7881 jter->second.z() *=
ui_->doubleSpinBox_optimizationScale->value();
7886 ui_->horizontalSlider_iterations->setMaximum((
int)
graphes_.size()-1);
7887 ui_->horizontalSlider_iterations->setValue((
int)
graphes_.size()-1);
7888 ui_->horizontalSlider_iterations->setEnabled(
true);
7889 ui_->spinBox_optimizationsFrom->setEnabled(
true);
7894 ui_->horizontalSlider_iterations->setEnabled(
false);
7895 ui_->spinBox_optimizationsFrom->setEnabled(
false);
7901 if(sender() ==
ui_->checkBox_grid_2d && !
ui_->checkBox_grid_2d->isChecked())
7909 ui_->comboBox_octomap_rendering_type->setVisible(
ui_->checkBox_octomap->isChecked());
7910 ui_->spinBox_grid_depth->setVisible(
ui_->checkBox_octomap->isChecked());
7911 ui_->checkBox_grid_empty->setVisible(!
ui_->checkBox_octomap->isChecked() ||
ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7912 ui_->checkBox_grid_frontiers->setVisible(
ui_->checkBox_octomap->isChecked() &&
ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7913 ui_->label_octomap_cubes->setVisible(
ui_->checkBox_octomap->isChecked());
7914 ui_->label_octomap_depth->setVisible(
ui_->checkBox_octomap->isChecked());
7915 ui_->label_octomap_empty->setVisible(!
ui_->checkBox_octomap->isChecked() ||
ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7916 ui_->label_octomap_frontiers->setVisible(
ui_->checkBox_octomap->isChecked() &&
ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7925 #ifdef RTABMAP_OCTOMAP
7926 ui_->comboBox_octomap_rendering_type->setVisible(
ui_->checkBox_octomap->isChecked());
7927 ui_->spinBox_grid_depth->setVisible(
ui_->checkBox_octomap->isChecked());
7928 ui_->checkBox_grid_empty->setVisible(!
ui_->checkBox_octomap->isChecked() ||
ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7929 ui_->checkBox_grid_frontiers->setVisible(
ui_->checkBox_octomap->isChecked() &&
ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7930 ui_->label_octomap_cubes->setVisible(
ui_->checkBox_octomap->isChecked());
7931 ui_->label_octomap_depth->setVisible(
ui_->checkBox_octomap->isChecked());
7932 ui_->label_octomap_empty->setVisible(!
ui_->checkBox_octomap->isChecked() ||
ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7933 ui_->label_octomap_frontiers->setVisible(
ui_->checkBox_octomap->isChecked() &&
ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7935 if(
ui_->checkBox_octomap->isChecked())
7947 if(
ui_->comboBox_octomap_rendering_type->currentIndex()>0)
7953 pcl::IndicesPtr obstacles(
new std::vector<int>);
7954 pcl::IndicesPtr
empty(
new std::vector<int>);
7955 pcl::IndicesPtr ground(
new std::vector<int>);
7956 pcl::IndicesPtr frontiers(
new std::vector<int>);
7957 std::vector<double> prob;
7959 ui_->spinBox_grid_depth->value(),
7969 pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(
new pcl::PointCloud<pcl::PointXYZRGB>);
7970 pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
7974 pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(
new pcl::PointCloud<pcl::PointXYZRGB>);
7975 pcl::copyPointCloud(*cloud, *ground, *groundCloud);
7981 pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(
new pcl::PointCloud<pcl::PointXYZ>);
7982 pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
7986 pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(
new pcl::PointCloud<pcl::PointXYZ>);
7987 pcl::copyPointCloud(*cloud, *ground, *groundCloud);
7992 if(
ui_->checkBox_grid_empty->isChecked())
7994 if(prob.size()==cloud->size())
7996 float occThr = Parameters::defaultGridGlobalOccupancyThr();
7997 Parameters::parse(
ui_->parameters_toolbox->getParameters(), Parameters::kGridGlobalOccupancyThr(), occThr);
7998 pcl::PointCloud<pcl::PointXYZRGB>::Ptr emptyCloud(
new pcl::PointCloud<pcl::PointXYZRGB>);
7999 emptyCloud->resize(
empty->size());
8000 for(
unsigned int i=0;
i<
empty->size(); ++
i)
8002 emptyCloud->points.at(
i).x = cloud->points.at(
empty->at(
i)).x;
8003 emptyCloud->points.at(
i).y = cloud->points.at(
empty->at(
i)).y;
8004 emptyCloud->points.at(
i).z = cloud->points.at(
empty->at(
i)).z;
8005 QColor hsv = QColor::fromHsv(
int(prob.at(
empty->at(
i))/occThr*240.0), 255, 255, 255);
8006 QRgb color = hsv.rgb();
8007 emptyCloud->points.at(
i).r = qRed(color);
8008 emptyCloud->points.at(
i).g = qGreen(color);
8009 emptyCloud->points.at(
i).b = qBlue(color);
8017 pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCloud(
new pcl::PointCloud<pcl::PointXYZ>);
8018 pcl::copyPointCloud(*cloud, *
empty, *emptyCloud);
8025 if(
ui_->checkBox_grid_frontiers->isChecked())
8027 pcl::PointCloud<pcl::PointXYZ>::Ptr frontiersCloud(
new pcl::PointCloud<pcl::PointXYZ>);
8028 pcl::copyPointCloud(*cloud, *frontiers, *frontiersCloud);
8036 if(
ui_->dockWidget_view3d->isVisible() &&
ui_->checkBox_showGrid->isChecked())
8050 link = findIter->second;
8057 link = findIter->second;
8062 if(findIter !=
links_.end())
8064 link = findIter->second;
8078 int from =
ids_.at(
ui_->horizontalSlider_A->value());
8079 int to =
ids_.at(
ui_->horizontalSlider_B->value());
8085 UDEBUG(
"%d -> %d", from, to);
8086 bool switchedIds =
false;
8089 UWARN(
"Cannot refine link to same node");
8097 UERROR(
"Not found link! (%d->%d)", from, to);
8101 UDEBUG(
"%d -> %d (type=%d)", currentLink.
from(), currentLink.
to(), currentLink.
type());
8103 if(
ui_->checkBox_showOptimized->isChecked() &&
8106 (
int)
graphes_.size()-1 ==
ui_->horizontalSlider_iterations->maximum())
8111 std::map<int, rtabmap::Transform>::iterator iterFrom =
graph.find(currentLink.
from());
8112 std::map<int, rtabmap::Transform>::iterator iterTo =
graph.find(currentLink.
to());
8113 if(iterFrom !=
graph.end() && iterTo !=
graph.end())
8120 else if(
ui_->checkBox_ignorePoseCorrection->isChecked() &&
8143 UERROR(
"Signature %d not found!", currentLink.
from());
8153 std::map<int, rtabmap::Transform> scanPoses;
8157 userData.type() == CV_8SC1 &&
8158 userData.rows == 1 &&
8159 userData.cols >= 8 &&
8160 userData.at<
char>(userData.cols-1) == 0 &&
8161 memcmp(userData.data,
"SCANS:", 6) == 0 &&
8162 currentLink.
from() > currentLink.
to())
8164 std::string scansStr = (
const char *)userData.data;
8165 UINFO(
"Detected \"%s\" in links's user data", scansStr.c_str());
8166 if(!scansStr.empty())
8168 std::list<std::string> strs =
uSplit(scansStr,
':');
8169 if(strs.size() == 2)
8171 std::list<std::string> strIds =
uSplit(strs.rbegin()->c_str(),
';');
8172 for(std::list<std::string>::iterator
iter=strIds.begin();
iter!=strIds.end(); ++
iter)
8174 int id = atoi(
iter->c_str());
8181 UERROR(
"Not found %d node!",
id);
8187 if(scanPoses.size()>1)
8193 std::map<int, rtabmap::Transform> posesOut;
8194 std::multimap<int, rtabmap::Link> linksOut;
8202 if(scanPoses.size() != posesOut.size())
8204 UWARN(
"Scan poses input and output are different! %d vs %d", (
int)scanPoses.size(), (
int)posesOut.size());
8205 UWARN(
"Input poses: ");
8206 for(std::map<int, Transform>::iterator
iter=scanPoses.begin();
iter!=scanPoses.end(); ++
iter)
8210 UWARN(
"Input links: ");
8212 for(std::multimap<int, Link>::iterator
iter=modifiedLinks.begin();
iter!=modifiedLinks.end(); ++
iter)
8218 scanPoses = optimizer->
optimize(currentLink.
to(), posesOut, linksOut);
8221 std::map<int, Transform> filteredScanPoses = scanPoses;
8222 float proximityFilteringRadius = 0.0f;
8223 Parameters::parse(parameters, Parameters::kRGBDProximityPathFilteringRadius(), proximityFilteringRadius);
8224 if(scanPoses.size() > 2 && proximityFilteringRadius > 0.0f)
8229 filteredScanPoses.insert(*scanPoses.find(currentLink.
to()));
8236 int maxPoints = fromScan.
size();
8239 UWARN(
"From scan %d is empty!", fromS->
id());
8241 pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(
new pcl::PointCloud<pcl::PointXYZ>);
8242 pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(
new pcl::PointCloud<pcl::PointNormal>);
8243 pcl::PointCloud<pcl::PointXYZI>::Ptr assembledToIClouds(
new pcl::PointCloud<pcl::PointXYZI>);
8244 pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembledToNormalIClouds(
new pcl::PointCloud<pcl::PointXYZINormal>);
8245 pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledToRGBClouds(
new pcl::PointCloud<pcl::PointXYZRGB>);
8246 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledToNormalRGBClouds(
new pcl::PointCloud<pcl::PointXYZRGBNormal>);
8247 for(std::map<int, Transform>::const_iterator
iter = filteredScanPoses.begin();
iter!=filteredScanPoses.end(); ++
iter)
8249 if(
iter->first != currentLink.
from())
8253 if(!
data.laserScanCompressed().isEmpty())
8256 data.uncompressData(0, 0, &scan);
8299 if(scan.
size() > maxPoints)
8301 maxPoints = scan.
size();
8306 UWARN(
"scan format of %d is not the same than from scan %d: %d vs %d",
data.id(), fromS->
id(), scan.
format(), fromScan.
format());
8311 UWARN(
"Laser scan not found for signature %d",
iter->first);
8317 if(assembledToNormalClouds->size())
8321 else if(assembledToClouds->size())
8325 else if(assembledToNormalIClouds->size())
8329 else if(assembledToIClouds->size())
8333 else if(assembledToNormalRGBClouds->size())
8338 else if(assembledToRGBClouds->size())
8345 UWARN(
"Assembled scan is empty!");
8361 info.covariance*=100.0;
8369 UERROR(
"Signature %d not found!", currentLink.
to());
8374 bool reextractVisualFeatures =
uStr2Bool(parameters.at(Parameters::kRGBDLoopClosureReextractFeatures()));
8378 reextractVisualFeatures ||
8391 if(reextractVisualFeatures)
8394 fromS->
sensorData().
setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
8401 if(
ui_->checkBox_icp_from_depth->isChecked())
8404 cv::Mat tmpA, tmpB, tmpC, tmpD;
8409 ui_->spinBox_icp_decimation->value()==0?1:
ui_->spinBox_icp_decimation->value(),
8410 ui_->doubleSpinBox_icp_maxDepth->value(),
8411 ui_->doubleSpinBox_icp_minDepth->value(),
8413 ui_->parameters_toolbox->getParameters());
8416 ui_->spinBox_icp_decimation->value()==0?1:
ui_->spinBox_icp_decimation->value(),
8417 ui_->doubleSpinBox_icp_maxDepth->value(),
8418 ui_->doubleSpinBox_icp_minDepth->value(),
8420 ui_->parameters_toolbox->getParameters());
8422 if(cloudFrom->empty() && cloudTo->empty())
8424 std::string
msg =
"Option to generate scan from depth is checked (GUI Parameters->Refine), but "
8425 "resulting clouds from depth are empty. Transformation estimation will likely "
8426 "fails. Uncheck the parameter to use laser scans.";
8430 QMessageBox::warning(
this,
8432 tr(
"%1").
arg(
msg.c_str()));
8437 UWARN(
"There are laser scans in data, but generate laser scan from "
8438 "depth image option is activated (GUI Parameters->Refine). "
8439 "Ignoring saved laser scans...");
8442 int maxLaserScans = cloudFrom->size();
8456 cv::Mat tmpA, tmpB, tmpC, tmpD;
8461 UINFO(
"Uncompress time: %f s",
timer.ticks());
8463 if(fromS->
id() < toS->
id())
8475 UINFO(
"(%d ->%d) Registration time: %f s", currentLink.
from(), currentLink.
to(),
timer.ticks());
8481 if(
info.covariance.at<
double>(0,0)<=0.0)
8483 info.covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001;
8497 if(
iter->second.to() == currentLink.
to() &&
8498 iter->second.type() == currentLink.
type())
8500 iter->second = newLink;
8517 if(!silent &&
ui_->dockWidget_constraints->isVisible())
8519 if(toS && fromS->
id() > 0 && toS->
id() > 0)
8522 std::multimap<int, cv::KeyPoint> keypointsFrom;
8523 std::multimap<int, cv::KeyPoint> keypointsTo;
8528 keypointsFrom.insert(keypointsFrom.end(), std::make_pair(
iter->first, fromS->
getWordsKpts()[
iter->second]));
8535 keypointsTo.insert(keypointsTo.end(), std::make_pair(
iter->first, toS->
getWordsKpts()[
iter->second]));
8561 if(toS && fromS->
id() > 0 && toS->
id() > 0)
8564 std::multimap<int, cv::KeyPoint> keypointsFrom;
8565 std::multimap<int, cv::KeyPoint> keypointsTo;
8570 keypointsFrom.insert(keypointsFrom.end(), std::make_pair(
iter->first, fromS->
getWordsKpts()[
iter->second]));
8577 keypointsTo.insert(keypointsTo.end(), std::make_pair(
iter->first, toS->
getWordsKpts()[
iter->second]));
8593 QMessageBox::warning(
this,
8595 tr(
"Cannot find a transformation between nodes %1 and %2: %3").
arg(currentLink.
from()).
arg(currentLink.
to()).arg(
info.rejectedMsg.c_str()));
8603 int from =
ids_.at(
ui_->horizontalSlider_A->value());
8604 int to =
ids_.at(
ui_->horizontalSlider_B->value());
8610 bool switchedIds =
false;
8613 UWARN(
"Cannot add link to same node");
8623 std::list<Signature*> signatures;
8638 bool loopCovLimited = Parameters::defaultRGBDLoopCovLimited();
8639 Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), loopCovLimited);
8649 ids.push_back(from);
8652 if(signatures.size() != 2)
8654 for(std::list<Signature*>::iterator
iter=signatures.begin();
iter!=signatures.end(); ++
iter)
8660 fromS = *signatures.begin();
8661 toS = *signatures.rbegin();
8663 bool reextractVisualFeatures =
uStr2Bool(parameters.at(Parameters::kRGBDLoopClosureReextractFeatures()));
8666 reextractVisualFeatures ||
8674 if(reextractVisualFeatures)
8677 fromS->
sensorData().
setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
8686 ui_->spinBox_icp_decimation->value()==0?1:
ui_->spinBox_icp_decimation->value(),
8687 ui_->doubleSpinBox_icp_maxDepth->value(),
8688 ui_->doubleSpinBox_icp_minDepth->value(),
8690 ui_->parameters_toolbox->getParameters());
8693 ui_->spinBox_icp_decimation->value()==0?1:
ui_->spinBox_icp_decimation->value(),
8694 ui_->doubleSpinBox_icp_maxDepth->value(),
8695 ui_->doubleSpinBox_icp_minDepth->value(),
8697 ui_->parameters_toolbox->getParameters());
8699 if(cloudFrom->empty() && cloudTo->empty())
8701 std::string
msg =
"Option to generate scan from depth is checked (GUI Parameters->Refine), but "
8702 "resulting clouds from depth are empty. Transformation estimation will likely "
8703 "fails. Uncheck the parameter to use laser scans.";
8707 QMessageBox::warning(
this,
8709 tr(
"%1").
arg(
msg.c_str()));
8714 UWARN(
"There are laser scans in data, but generate laser scan from "
8715 "depth image option is activated (GUI Parameters->Refine). Ignoring saved laser scans...");
8718 int maxLaserScans = cloudFrom->size();
8723 else if(!reextractVisualFeatures && fromS->
getWords().empty() && toS->
getWords().empty())
8725 std::string
msg =
uFormat(
"\"%s\" is false and signatures (%d and %d) don't have words, "
8726 "registration will not be possible. Set \"%s\" to true.",
8727 Parameters::kRGBDLoopClosureReextractFeatures().
c_str(),
8730 Parameters::kRGBDLoopClosureReextractFeatures().c_str());
8734 QMessageBox::warning(
this,
8736 tr(
"%1").
arg(
msg.c_str()));
8741 bool guessFromGraphRejected =
false;
8747 std::map<int, Transform> optimizedPoses =
graphes_.back();
8748 if(optimizedPoses.size() > 0)
8750 std::map<int, Transform>::iterator fromIter = optimizedPoses.find(from);
8751 std::map<int, Transform>::iterator toIter = optimizedPoses.find(to);
8752 if(fromIter != optimizedPoses.end() &&
8753 toIter != optimizedPoses.end())
8757 if(QMessageBox::question(
this,
8758 tr(
"Add constraint from optimized graph"),
8759 tr(
"Registration is done without vision (see %1 parameter), "
8760 "do you want to use a guess from the optimized graph?"
8762 "\n\nOtherwise, if "
8763 "the database has images, it is recommended to use %2=2 instead so that "
8764 "the guess can be found visually.")
8765 .
arg(Parameters::kRegStrategy().
c_str()).
arg(Parameters::kRegStrategy().
c_str()),
8766 QMessageBox::Yes | QMessageBox::No,
8767 QMessageBox::Yes) == QMessageBox::Yes)
8769 guess = fromIter->second.
inverse() * toIter->second;
8773 guessFromGraphRejected =
true;
8776 else if(silentlyUseOptimizedGraphAsGuess)
8778 guess = fromIter->second.
inverse() * toIter->second;
8783 if(guess.
isNull() && !silent && !guessFromGraphRejected)
8785 if(QMessageBox::question(
this,
8786 tr(
"Add constraint without guess"),
8787 tr(
"Registration is done without vision (see %1 parameter) and we cannot (or we don't want to) find relative "
8788 "transformation between the nodes with the current graph. Do you want to use an identity "
8789 "transform for ICP guess? "
8791 "\n\nOtherwise, if the database has images, it is recommended to use %2=2 "
8792 "instead so that the guess can be found visually.")
8793 .
arg(Parameters::kRegStrategy().
c_str()).
arg(Parameters::kRegStrategy().
c_str()),
8794 QMessageBox::Yes | QMessageBox::Abort,
8795 QMessageBox::Abort) == QMessageBox::Yes)
8801 guessFromGraphRejected =
true;
8823 cv::Mat information =
info.covariance.inv();
8824 if(odomMaxInf.size() == 6 && information.cols==6 && information.rows==6)
8826 for(
int i=0;
i<6; ++
i)
8828 if(information.at<
double>(
i,
i) > odomMaxInf[
i])
8830 information.at<
double>(
i,
i) = odomMaxInf[
i];
8837 else if(!silent && !guessFromGraphRejected)
8839 QMessageBox::StandardButton button = QMessageBox::warning(
this,
8841 tr(
"Cannot find a transformation between nodes %1 and %2: %3\n\nDo you want to add it manually?").
arg(from).
arg(to).
arg(
info.rejectedMsg.c_str()),
8842 QMessageBox::Yes | QMessageBox::No,
8844 if(button == QMessageBox::Yes)
8856 bool updateConstraints = newLink.
isValid();
8857 float maxOptimizationError =
uStr2Float(
ui_->parameters_toolbox->getParameters().at(Parameters::kRGBDOptimizeMaxError()));
8859 maxOptimizationError > 0.0f &&
8860 uStr2Int(
ui_->parameters_toolbox->getParameters().at(Parameters::kOptimizerIterations())) > 0.0f)
8862 int fromId = newLink.
from();
8864 linksIn.insert(std::make_pair(newLink.
from(), newLink));
8865 const Link * maxLinearLink = 0;
8866 const Link * maxAngularLink = 0;
8867 float maxLinearErrorRatio = 0.0f;
8868 float maxAngularErrorRatio = 0.0f;
8870 std::map<int, Transform> poses;
8871 std::multimap<int, Link> links;
8879 const std::map<int, Transform> & optimizedPoses =
graphes_.back();
8880 for(std::map<int, Transform>::iterator
iter = poses.begin();
iter!=poses.end(); ++
iter)
8882 if(optimizedPoses.find(
iter->first) != optimizedPoses.end())
8884 iter->second = optimizedPoses.at(
iter->first);
8888 UASSERT(poses.find(fromId) != poses.end());
8889 UASSERT_MSG(poses.find(newLink.
from()) != poses.end(),
uFormat(
"id=%d poses=%d links=%d", newLink.
from(), (
int)poses.size(), (
int)links.size()).c_str());
8890 UASSERT_MSG(poses.find(newLink.
to()) != poses.end(),
uFormat(
"id=%d poses=%d links=%d", newLink.
to(), (
int)poses.size(), (
int)links.size()).c_str());
8892 std::map<int, Transform> posesIn = poses;
8893 poses = optimizer->
optimize(fromId, posesIn, links);
8894 if(posesIn.size() && poses.empty())
8896 UWARN(
"Optimization failed... (poses=%d, links=%d).", (
int)posesIn.size(), (
int)links.size());
8901 float maxLinearError = 0.0f;
8902 float maxAngularError = 0.0f;
8906 maxLinearErrorRatio,
8907 maxAngularErrorRatio,
8914 UINFO(
"Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->
from(), maxLinearLink->
to(), maxLinearLink->
transVariance(), maxLinearError/
sqrt(maxLinearLink->
transVariance()));
8915 if(maxLinearErrorRatio > maxOptimizationError)
8917 msg =
uFormat(
"Rejecting edge %d->%d because "
8918 "graph error is too large (abs=%f m) after optimization (ratio %f for edge %d->%d, stddev=%f m). "
8923 maxLinearErrorRatio,
8924 maxLinearLink->
from(),
8925 maxLinearLink->
to(),
8927 Parameters::kRGBDOptimizeMaxError().c_str(),
8928 maxOptimizationError);
8933 UINFO(
"Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0
f/CV_PI, maxAngularLink->
from(), maxAngularLink->
to(), maxAngularLink->
rotVariance(), maxAngularError/
sqrt(maxAngularLink->
rotVariance()));
8934 if(maxAngularErrorRatio > maxOptimizationError)
8936 msg =
uFormat(
"Rejecting edge %d->%d because "
8937 "graph error is too large (abs=%f deg) after optimization (ratio %f for edge %d->%d, stddev=%f deg). "
8941 maxAngularError*180.0f/CV_PI,
8942 maxAngularErrorRatio,
8943 maxAngularLink->
from(),
8944 maxAngularLink->
to(),
8946 Parameters::kRGBDOptimizeMaxError().c_str(),
8947 maxOptimizationError);
8953 msg =
uFormat(
"Rejecting edge %d->%d because graph optimization has failed!",
8962 QMessageBox::warning(
this,
8964 tr(
"%1").
arg(
msg.c_str()));
8967 updateConstraints =
false;
8970 if(updateConstraints && silent && !
graphes_.empty() &&
graphes_.back().size() == poses.size())
8976 if(updateConstraints)
8985 if(newLink.
from() < newLink.
to())
8997 if((updateConstraints && newLink.
from() > newLink.
to()) || (!updateConstraints && switchedIds))
9004 if(updateConstraints)
9011 std::multimap<int, cv::KeyPoint> keypointsFrom;
9012 std::multimap<int, cv::KeyPoint> keypointsTo;
9017 keypointsFrom.insert(keypointsFrom.end(), std::make_pair(
iter->first, fromS->
getWordsKpts()[
iter->second]));
9024 keypointsTo.insert(keypointsTo.end(), std::make_pair(
iter->first, toS->
getWordsKpts()[
iter->second]));
9031 else if(updateConstraints)
9038 for(std::list<Signature*>::iterator
iter=signatures.begin();
iter!=signatures.end(); ++
iter)
9043 return updateConstraints;
9048 int from =
ids_.at(
ui_->horizontalSlider_A->value());
9049 int to =
ids_.at(
ui_->horizontalSlider_B->value());
9052 int position =
ui_->horizontalSlider_loops->value();
9067 UWARN(
"Cannot reset link to same node");
9084 int priorId = sender() ==
ui_->toolButton_remove_priorA?
ids_.at(
ui_->horizontalSlider_A->value()):
9085 sender() ==
ui_->toolButton_remove_priorB?
ids_.at(
ui_->horizontalSlider_B->value()):0;
9087 int from = priorId>0?priorId:
ids_.at(
ui_->horizontalSlider_A->value());
9088 int to = priorId>0?priorId:
ids_.at(
ui_->horizontalSlider_B->value());
9091 int position =
ui_->horizontalSlider_loops->value();
9104 if(priorId==0 && from == to)
9106 UWARN(
"Cannot reject link to same node");
9110 bool removed =
false;
9113 std::multimap<int, Link>::iterator
iter;
9119 QMessageBox::StandardButton button = QMessageBox::warning(
this, tr(
"Reject link"),
9120 tr(
"Removing the neighbor link %1->%2 will split the graph. Do you want to continue?").
arg(from).
arg(to),
9121 QMessageBox::Yes | QMessageBox::No, QMessageBox::No);
9122 if(button != QMessageBox::Yes)
9152 bool priorsIgnored = Parameters::defaultOptimizerPriorsIgnored();
9153 Parameters::parse(
ui_->parameters_toolbox->getParameters(), Parameters::kOptimizerPriorsIgnored(), priorsIgnored);
9154 int indexA =
ui_->horizontalSlider_A->value();
9155 int indexB =
ui_->horizontalSlider_B->value();
9160 if(
ui_->horizontalSlider_A->value() != indexA)
9161 ui_->horizontalSlider_A->setValue(indexA);
9164 if(
ui_->horizontalSlider_B->value() != indexB)
9165 ui_->horizontalSlider_B->setValue(indexB);
9173 const std::multimap<int, rtabmap::Link> & edgeConstraints)
9177 std::multimap<int, rtabmap::Link> links;
9178 for(std::multimap<int, rtabmap::Link>::const_iterator
iter=edgeConstraints.begin();
9179 iter!=edgeConstraints.end();
9182 std::multimap<int, rtabmap::Link>::iterator findIter;
9187 UDEBUG(
"Removed link (%d->%d, %d)",
iter->second.from(),
iter->second.to(),
iter->second.type());
9195 if(
iter->second.from() == findIter->second.to() &&
9196 iter->second.from() !=
iter->second.to())
9198 links.insert(std::make_pair(
iter->second.from(), findIter->second.inverse()));
9202 links.insert(*findIter);
9204 UDEBUG(
"Updated link (%d->%d, %d)",
iter->second.from(),
iter->second.to(),
iter->second.type());
9208 links.insert(*
iter);
9212 for(std::multimap<int, rtabmap::Link>::const_iterator
iter=
linksAdded_.begin();
9219 links.insert(*findIter);
9220 links.insert(std::make_pair(findIter->second.to(), findIter->second.inverse()));
9221 UDEBUG(
"Added refined link (%d->%d, %d)", findIter->second.from(), findIter->second.to(), findIter->second.type());
9225 UDEBUG(
"Added link (%d->%d, %d)",
iter->second.from(),
iter->second.to(),
iter->second.type());
9226 links.insert(*
iter);
9227 links.insert(std::make_pair(
iter->second.to(),
iter->second.inverse()));
9235 UDEBUG(
"%d %d", from, to);
9238 int position =
ui_->horizontalSlider_neighbors->value();
9239 std::multimap<int, Link> linksSortedByChildren;
9240 for(std::multimap<int, rtabmap::Link>::iterator
iter = links.begin();
iter!=links.end(); ++
iter)
9242 if(
iter->second.from() <
iter->second.to())
9244 linksSortedByChildren.insert(*
iter);
9248 for(std::multimap<int, rtabmap::Link>::iterator
iter = linksSortedByChildren.begin();
iter!=linksSortedByChildren.end(); ++
iter)
9250 if(!
iter->second.transform().isNull())
9255 if((
iter->second.from() == from &&
iter->second.to() == to) ||
9256 (
iter->second.to() == from &&
iter->second.from() == to))
9265 UERROR(
"Transform null for link from %d to %d",
iter->first,
iter->second.to());
9276 ui_->horizontalSlider_neighbors->setMinimum(0);
9278 ui_->horizontalSlider_neighbors->setEnabled(
true);
9279 if(
position !=
ui_->horizontalSlider_neighbors->value())
9281 ui_->horizontalSlider_neighbors->setValue(
position);
9290 ui_->horizontalSlider_neighbors->setEnabled(
false);
9299 UDEBUG(
"%d %d", from, to);
9302 int position =
ui_->horizontalSlider_loops->value();
9303 std::multimap<int, Link> linksSortedByParents;
9304 for(std::multimap<int, rtabmap::Link>::iterator
iter = links.begin();
iter!=links.end(); ++
iter)
9306 if(
iter->second.to() >
iter->second.from() &&
iter->second.from() < 0)
9308 linksSortedByParents.insert(std::make_pair(
iter->second.to(),
iter->second.inverse()));
9310 else if(
iter->second.to() <
iter->second.from())
9312 linksSortedByParents.insert(*
iter);
9315 for(std::multimap<int, rtabmap::Link>::iterator
iter = linksSortedByParents.begin();
iter!=linksSortedByParents.end(); ++
iter)
9317 if(!
iter->second.transform().isNull())
9322 if((
iter->second.from() == from &&
iter->second.to() == to) ||
9323 (
iter->second.to() == from &&
iter->second.from() == to))
9332 UERROR(
"Transform null for link from %d to %d",
iter->first,
iter->second.to());
9343 ui_->horizontalSlider_loops->setMinimum(0);
9344 ui_->horizontalSlider_loops->setMaximum(
loopLinks_.size()-1);
9345 ui_->horizontalSlider_loops->setEnabled(
true);
9346 if(
position !=
ui_->horizontalSlider_loops->value())
9357 ui_->horizontalSlider_loops->setEnabled(
false);
9368 for(QStringList::const_iterator
iter=parametersChanged.constBegin();
9372 QString group =
iter->split(
'/').first();