00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "rtabmap/gui/DatabaseViewer.h"
00029 #include "rtabmap/gui/CloudViewer.h"
00030 #include "ui_DatabaseViewer.h"
00031 #include <QMessageBox>
00032 #include <QFileDialog>
00033 #include <QInputDialog>
00034 #include <QDesktopWidget>
00035 #include <QColorDialog>
00036 #include <QGraphicsLineItem>
00037 #include <QtGui/QCloseEvent>
00038 #include <QGraphicsOpacityEffect>
00039 #include <QtCore/QBuffer>
00040 #include <QtCore/QTextStream>
00041 #include <QtCore/QDateTime>
00042 #include <QtCore/QSettings>
00043 #include <rtabmap/utilite/ULogger.h>
00044 #include <rtabmap/utilite/UDirectory.h>
00045 #include <rtabmap/utilite/UConversion.h>
00046 #include <opencv2/core/core_c.h>
00047 #include <opencv2/imgproc/types_c.h>
00048 #include <opencv2/highgui/highgui.hpp>
00049 #include <rtabmap/utilite/UTimer.h>
00050 #include <rtabmap/utilite/UFile.h>
00051 #include "rtabmap/utilite/UPlot.h"
00052 #include "rtabmap/core/DBDriver.h"
00053 #include "rtabmap/gui/KeypointItem.h"
00054 #include "rtabmap/gui/CloudViewer.h"
00055 #include "rtabmap/utilite/UCv2Qt.h"
00056 #include "rtabmap/core/util3d.h"
00057 #include "rtabmap/core/util3d_transforms.h"
00058 #include "rtabmap/core/util3d_filtering.h"
00059 #include "rtabmap/core/util3d_surface.h"
00060 #include "rtabmap/core/util3d_registration.h"
00061 #include "rtabmap/core/util3d_mapping.h"
00062 #include "rtabmap/core/util2d.h"
00063 #include "rtabmap/core/Signature.h"
00064 #include "rtabmap/core/Memory.h"
00065 #include "rtabmap/core/Features2d.h"
00066 #include "rtabmap/core/Compression.h"
00067 #include "rtabmap/core/Graph.h"
00068 #include "rtabmap/core/Stereo.h"
00069 #include "rtabmap/core/Optimizer.h"
00070 #include "rtabmap/core/RegistrationVis.h"
00071 #include "rtabmap/core/RegistrationIcp.h"
00072 #include "rtabmap/core/OccupancyGrid.h"
00073 #include "rtabmap/core/GeodeticCoords.h"
00074 #include "rtabmap/core/Recovery.h"
00075 #include "rtabmap/gui/DataRecorder.h"
00076 #include "rtabmap/gui/ExportCloudsDialog.h"
00077 #include "rtabmap/gui/EditDepthArea.h"
00078 #include "rtabmap/core/SensorData.h"
00079 #include "rtabmap/core/GainCompensator.h"
00080 #include "rtabmap/gui/ExportDialog.h"
00081 #include "rtabmap/gui/ProgressDialog.h"
00082 #include "rtabmap/gui/ParametersToolBox.h"
00083 #include "rtabmap/gui/RecoveryState.h"
00084 #include <pcl/io/pcd_io.h>
00085 #include <pcl/io/ply_io.h>
00086 #include <pcl/io/obj_io.h>
00087 #include <pcl/filters/voxel_grid.h>
00088 #include <pcl/common/transforms.h>
00089 #include <pcl/common/common.h>
00090
00091 #ifdef RTABMAP_OCTOMAP
00092 #include "rtabmap/core/OctoMap.h"
00093 #endif
00094
00095 namespace rtabmap {
00096
00097 DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
00098 QMainWindow(parent),
00099 dbDriver_(0),
00100 octomap_(0),
00101 exportDialog_(new ExportCloudsDialog(this)),
00102 editDepthDialog_(new QDialog(this)),
00103 savedMaximized_(false),
00104 firstCall_(true),
00105 iniFilePath_(ini),
00106 useLastOptimizedGraphAsGuess_(false)
00107 {
00108 pathDatabase_ = QDir::homePath()+"/Documents/RTAB-Map";
00109
00110 if(!UDirectory::exists(pathDatabase_.toStdString()))
00111 {
00112 pathDatabase_ = QDir::homePath();
00113 }
00114
00115 ui_ = new Ui_DatabaseViewer();
00116 ui_->setupUi(this);
00117 ui_->buttonBox->setVisible(false);
00118 connect(ui_->buttonBox->button(QDialogButtonBox::Close), SIGNAL(clicked()), this, SLOT(close()));
00119
00120 ui_->comboBox_logger_level->setVisible(parent==0);
00121 ui_->label_logger_level->setVisible(parent==0);
00122 connect(ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(int)), this, SLOT(updateLoggerLevel()));
00123 connect(ui_->actionVertical_Layout, SIGNAL(toggled(bool)), this, SLOT(setupMainLayout(bool)));
00124
00125 editDepthDialog_->resize(640, 480);
00126 QVBoxLayout * vLayout = new QVBoxLayout(editDepthDialog_);
00127 editDepthArea_ = new EditDepthArea(editDepthDialog_);
00128 vLayout->setContentsMargins(0,0,0,0);
00129 vLayout->setSpacing(0);
00130 vLayout->addWidget(editDepthArea_, 1);
00131 QDialogButtonBox * buttonBox = new QDialogButtonBox(QDialogButtonBox::Save | QDialogButtonBox::Cancel | QDialogButtonBox::Reset, Qt::Horizontal, editDepthDialog_);
00132 vLayout->addWidget(buttonBox);
00133 connect(buttonBox, SIGNAL(accepted()), editDepthDialog_, SLOT(accept()));
00134 connect(buttonBox, SIGNAL(rejected()), editDepthDialog_, SLOT(reject()));
00135 connect(buttonBox->button(QDialogButtonBox::Reset), SIGNAL(clicked()), editDepthArea_, SLOT(resetChanges()));
00136 editDepthDialog_->setLayout(vLayout);
00137 editDepthDialog_->setWindowTitle(tr("Edit Depth Image"));
00138
00139 QString title("RTAB-Map Database Viewer[*]");
00140 this->setWindowTitle(title);
00141
00142 ui_->dockWidget_constraints->setVisible(false);
00143 ui_->dockWidget_graphView->setVisible(false);
00144 ui_->dockWidget_occupancyGridView->setVisible(false);
00145 ui_->dockWidget_guiparameters->setVisible(false);
00146 ui_->dockWidget_coreparameters->setVisible(false);
00147 ui_->dockWidget_info->setVisible(false);
00148 ui_->dockWidget_stereoView->setVisible(false);
00149 ui_->dockWidget_view3d->setVisible(false);
00150 ui_->dockWidget_statistics->setVisible(false);
00151
00152
00153 constraintsViewer_ = new CloudViewer(ui_->dockWidgetContents);
00154 cloudViewer_ = new CloudViewer(ui_->dockWidgetContents_3dviews);
00155 stereoViewer_ = new CloudViewer(ui_->dockWidgetContents_stereo);
00156 occupancyGridViewer_ = new CloudViewer(ui_->dockWidgetContents_occupancyGrid);
00157 constraintsViewer_->setObjectName("constraintsViewer");
00158 cloudViewer_->setObjectName("cloudViewerA");
00159 stereoViewer_->setObjectName("stereoViewer");
00160 occupancyGridViewer_->setObjectName("occupancyGridView");
00161 ui_->layout_constraintsViewer->addWidget(constraintsViewer_);
00162 ui_->horizontalLayout_3dviews->addWidget(cloudViewer_, 1);
00163 ui_->horizontalLayout_stereo->addWidget(stereoViewer_, 1);
00164 ui_->layout_occupancyGridView->addWidget(occupancyGridViewer_, 1);
00165
00166 constraintsViewer_->setCameraLockZ(false);
00167 constraintsViewer_->setCameraFree();
00168 occupancyGridViewer_->setCameraFree();
00169
00170 ui_->graphicsView_stereo->setAlpha(255);
00171
00172 #ifndef RTABMAP_OCTOMAP
00173 ui_->checkBox_octomap->setEnabled(false);
00174 ui_->checkBox_octomap->setChecked(false);
00175 #endif
00176
00177 ParametersMap parameters;
00178 uInsert(parameters, Parameters::getDefaultParameters("SURF"));
00179 uInsert(parameters, Parameters::getDefaultParameters("SIFT"));
00180 uInsert(parameters, Parameters::getDefaultParameters("BRIEF"));
00181 uInsert(parameters, Parameters::getDefaultParameters("FAST"));
00182 uInsert(parameters, Parameters::getDefaultParameters("GFTT"));
00183 uInsert(parameters, Parameters::getDefaultParameters("ORB"));
00184 uInsert(parameters, Parameters::getDefaultParameters("FREAK"));
00185 uInsert(parameters, Parameters::getDefaultParameters("BRISK"));
00186 uInsert(parameters, Parameters::getDefaultParameters("Optimizer"));
00187 uInsert(parameters, Parameters::getDefaultParameters("g2o"));
00188 uInsert(parameters, Parameters::getDefaultParameters("GTSAM"));
00189 uInsert(parameters, Parameters::getDefaultParameters("Reg"));
00190 uInsert(parameters, Parameters::getDefaultParameters("Vis"));
00191 uInsert(parameters, Parameters::getDefaultParameters("Icp"));
00192 uInsert(parameters, Parameters::getDefaultParameters("Stereo"));
00193 uInsert(parameters, Parameters::getDefaultParameters("StereoBM"));
00194 uInsert(parameters, Parameters::getDefaultParameters("Grid"));
00195 uInsert(parameters, Parameters::getDefaultParameters("GridGlobal"));
00196 parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDOptimizeMaxError()));
00197 parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDLoopClosureReextractFeatures()));
00198 ui_->parameters_toolbox->setupUi(parameters);
00199 exportDialog_->setObjectName("ExportCloudsDialog");
00200 restoreDefaultSettings();
00201 this->readSettings();
00202
00203 setupMainLayout(ui_->actionVertical_Layout->isChecked());
00204 ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked());
00205 ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
00206 ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
00207 ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
00208 ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
00209 ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
00210
00211 ui_->menuView->addAction(ui_->dockWidget_constraints->toggleViewAction());
00212 ui_->menuView->addAction(ui_->dockWidget_graphView->toggleViewAction());
00213 ui_->menuView->addAction(ui_->dockWidget_occupancyGridView->toggleViewAction());
00214 ui_->menuView->addAction(ui_->dockWidget_stereoView->toggleViewAction());
00215 ui_->menuView->addAction(ui_->dockWidget_view3d->toggleViewAction());
00216 ui_->menuView->addAction(ui_->dockWidget_guiparameters->toggleViewAction());
00217 ui_->menuView->addAction(ui_->dockWidget_coreparameters->toggleViewAction());
00218 ui_->menuView->addAction(ui_->dockWidget_info->toggleViewAction());
00219 ui_->menuView->addAction(ui_->dockWidget_statistics->toggleViewAction());
00220 connect(ui_->dockWidget_graphView->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView()));
00221 connect(ui_->dockWidget_occupancyGridView->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView()));
00222 connect(ui_->dockWidget_statistics->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateStatistics()));
00223
00224
00225 connect(ui_->parameters_toolbox, SIGNAL(parametersChanged(const QStringList &)), this, SLOT(notifyParametersChanged(const QStringList &)));
00226
00227 connect(ui_->actionQuit, SIGNAL(triggered()), this, SLOT(close()));
00228
00229 ui_->actionOpen_database->setEnabled(true);
00230 ui_->actionClose_database->setEnabled(false);
00231
00232
00233 ui_->actionSave_config->setShortcut(QKeySequence::Save);
00234 connect(ui_->actionSave_config, SIGNAL(triggered()), this, SLOT(writeSettings()));
00235 connect(ui_->actionOpen_database, SIGNAL(triggered()), this, SLOT(openDatabase()));
00236 connect(ui_->actionClose_database, SIGNAL(triggered()), this, SLOT(closeDatabase()));
00237 connect(ui_->actionDatabase_recovery, SIGNAL(triggered()), this, SLOT(recoverDatabase()));
00238 connect(ui_->actionExport, SIGNAL(triggered()), this, SLOT(exportDatabase()));
00239 connect(ui_->actionExtract_images, SIGNAL(triggered()), this, SLOT(extractImages()));
00240 connect(ui_->actionEdit_depth_image, SIGNAL(triggered()), this, SLOT(editDepthImage()));
00241 connect(ui_->actionGenerate_graph_dot, SIGNAL(triggered()), this, SLOT(generateGraph()));
00242 connect(ui_->actionGenerate_local_graph_dot, SIGNAL(triggered()), this, SLOT(generateLocalGraph()));
00243 connect(ui_->actionRaw_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRaw()));
00244 connect(ui_->actionRGBD_SLAM_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAM()));
00245 connect(ui_->actionKITTI_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesKITTI()));
00246 connect(ui_->actionTORO_graph, SIGNAL(triggered()), this , SLOT(exportPosesTORO()));
00247 connect(ui_->actionG2o_g2o, SIGNAL(triggered()), this , SLOT(exportPosesG2O()));
00248 connect(ui_->actionPoses_KML, SIGNAL(triggered()), this , SLOT(exportPosesKML()));
00249 connect(ui_->actionGPS_TXT, SIGNAL(triggered()), this , SLOT(exportGPS_TXT()));
00250 connect(ui_->actionGPS_KML, SIGNAL(triggered()), this , SLOT(exportGPS_KML()));
00251 connect(ui_->actionExport_saved_2D_map, SIGNAL(triggered()), this , SLOT(exportSaved2DMap()));
00252 connect(ui_->actionImport_2D_map, SIGNAL(triggered()), this , SLOT(import2DMap()));
00253 connect(ui_->actionView_optimized_mesh, SIGNAL(triggered()), this , SLOT(viewOptimizedMesh()));
00254 connect(ui_->actionExport_optimized_mesh, SIGNAL(triggered()), this , SLOT(exportOptimizedMesh()));
00255 connect(ui_->actionUpdate_optimized_mesh, SIGNAL(triggered()), this , SLOT(updateOptimizedMesh()));
00256 connect(ui_->actionView_3D_map, SIGNAL(triggered()), this, SLOT(view3DMap()));
00257 connect(ui_->actionGenerate_3D_map_pcd, SIGNAL(triggered()), this, SLOT(generate3DMap()));
00258 connect(ui_->actionDetect_more_loop_closures, SIGNAL(triggered()), this, SLOT(detectMoreLoopClosures()));
00259 connect(ui_->actionRefine_all_neighbor_links, SIGNAL(triggered()), this, SLOT(refineAllNeighborLinks()));
00260 connect(ui_->actionRefine_all_loop_closure_links, SIGNAL(triggered()), this, SLOT(refineAllLoopClosureLinks()));
00261 connect(ui_->actionRegenerate_local_grid_maps, SIGNAL(triggered()), this, SLOT(regenerateLocalMaps()));
00262 connect(ui_->actionRegenerate_local_grid_maps_selected, SIGNAL(triggered()), this, SLOT(regenerateCurrentLocalMaps()));
00263 connect(ui_->actionReset_all_changes, SIGNAL(triggered()), this, SLOT(resetAllChanges()));
00264 connect(ui_->actionRestore_default_GUI_settings, SIGNAL(triggered()), this, SLOT(restoreDefaultSettings()));
00265
00266
00267 connect(ui_->pushButton_refine, SIGNAL(clicked()), this, SLOT(refineConstraint()));
00268 connect(ui_->pushButton_add, SIGNAL(clicked()), this, SLOT(addConstraint()));
00269 connect(ui_->pushButton_reset, SIGNAL(clicked()), this, SLOT(resetConstraint()));
00270 connect(ui_->pushButton_reject, SIGNAL(clicked()), this, SLOT(rejectConstraint()));
00271 ui_->pushButton_refine->setEnabled(false);
00272 ui_->pushButton_add->setEnabled(false);
00273 ui_->pushButton_reset->setEnabled(false);
00274 ui_->pushButton_reject->setEnabled(false);
00275
00276 ui_->menuEdit->setEnabled(false);
00277 ui_->actionGenerate_3D_map_pcd->setEnabled(false);
00278 ui_->actionExport->setEnabled(false);
00279 ui_->actionExtract_images->setEnabled(false);
00280 ui_->menuExport_poses->setEnabled(false);
00281 ui_->menuExport_GPS->setEnabled(false);
00282 ui_->actionPoses_KML->setEnabled(false);
00283 ui_->actionExport_saved_2D_map->setEnabled(false);
00284 ui_->actionView_optimized_mesh->setEnabled(false);
00285 ui_->actionExport_optimized_mesh->setEnabled(false);
00286 ui_->actionUpdate_optimized_mesh->setEnabled(false);
00287
00288 ui_->horizontalSlider_A->setTracking(false);
00289 ui_->horizontalSlider_B->setTracking(false);
00290 ui_->horizontalSlider_A->setEnabled(false);
00291 ui_->horizontalSlider_B->setEnabled(false);
00292 connect(ui_->horizontalSlider_A, SIGNAL(valueChanged(int)), this, SLOT(sliderAValueChanged(int)));
00293 connect(ui_->horizontalSlider_B, SIGNAL(valueChanged(int)), this, SLOT(sliderBValueChanged(int)));
00294 connect(ui_->horizontalSlider_A, SIGNAL(sliderMoved(int)), this, SLOT(sliderAMoved(int)));
00295 connect(ui_->horizontalSlider_B, SIGNAL(sliderMoved(int)), this, SLOT(sliderBMoved(int)));
00296
00297 connect(ui_->spinBox_mesh_angleTolerance, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
00298 connect(ui_->spinBox_mesh_minClusterSize, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
00299 connect(ui_->spinBox_mesh_fillDepthHoles, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
00300 connect(ui_->spinBox_mesh_depthError, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
00301 connect(ui_->checkBox_mesh_quad, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
00302 connect(ui_->spinBox_mesh_triangleSize, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
00303 connect(ui_->checkBox_showWords, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
00304 connect(ui_->checkBox_showCloud, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
00305 connect(ui_->checkBox_showMesh, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
00306 connect(ui_->checkBox_showScan, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
00307 connect(ui_->checkBox_showMap, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
00308 connect(ui_->checkBox_showGrid, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
00309 connect(ui_->checkBox_odomFrame_3dview, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
00310
00311 ui_->horizontalSlider_neighbors->setTracking(false);
00312 ui_->horizontalSlider_loops->setTracking(false);
00313 ui_->horizontalSlider_neighbors->setEnabled(false);
00314 ui_->horizontalSlider_loops->setEnabled(false);
00315 connect(ui_->horizontalSlider_neighbors, SIGNAL(valueChanged(int)), this, SLOT(sliderNeighborValueChanged(int)));
00316 connect(ui_->horizontalSlider_loops, SIGNAL(valueChanged(int)), this, SLOT(sliderLoopValueChanged(int)));
00317 connect(ui_->horizontalSlider_neighbors, SIGNAL(sliderMoved(int)), this, SLOT(sliderNeighborValueChanged(int)));
00318 connect(ui_->horizontalSlider_loops, SIGNAL(sliderMoved(int)), this, SLOT(sliderLoopValueChanged(int)));
00319 connect(ui_->checkBox_showOptimized, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
00320 connect(ui_->checkBox_show3Dclouds, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
00321 connect(ui_->checkBox_show2DScans, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
00322 connect(ui_->checkBox_show3DWords, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
00323 connect(ui_->checkBox_odomFrame, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
00324 ui_->checkBox_showOptimized->setEnabled(false);
00325
00326 ui_->horizontalSlider_iterations->setTracking(false);
00327 ui_->horizontalSlider_iterations->setEnabled(false);
00328 ui_->spinBox_optimizationsFrom->setEnabled(false);
00329 connect(ui_->horizontalSlider_iterations, SIGNAL(valueChanged(int)), this, SLOT(sliderIterationsValueChanged(int)));
00330 connect(ui_->horizontalSlider_iterations, SIGNAL(sliderMoved(int)), this, SLOT(sliderIterationsValueChanged(int)));
00331 connect(ui_->spinBox_optimizationsFrom, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
00332 connect(ui_->checkBox_iterativeOptimization, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
00333 connect(ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
00334 connect(ui_->checkBox_wmState, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
00335 connect(ui_->graphViewer, SIGNAL(mapShownRequested()), this, SLOT(updateGraphView()));
00336 connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
00337 connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
00338 connect(ui_->checkBox_ignoreGlobalLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
00339 connect(ui_->checkBox_ignoreLocalLoopSpace, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
00340 connect(ui_->checkBox_ignoreLocalLoopTime, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
00341 connect(ui_->checkBox_ignoreUserLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
00342 connect(ui_->spinBox_optimizationDepth, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
00343 connect(ui_->doubleSpinBox_optimizationScale, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
00344 connect(ui_->checkBox_octomap, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
00345 connect(ui_->checkBox_grid_2d, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
00346 connect(ui_->comboBox_octomap_rendering_type, SIGNAL(currentIndexChanged(int)), this, SLOT(updateOctomapView()));
00347 connect(ui_->spinBox_grid_depth, SIGNAL(valueChanged(int)), this, SLOT(updateOctomapView()));
00348 connect(ui_->checkBox_grid_empty, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
00349 connect(ui_->doubleSpinBox_gainCompensationRadius, SIGNAL(valueChanged(double)), this, SLOT(updateConstraintView()));
00350 connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(updateConstraintView()));
00351 connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(update3dView()));
00352 connect(ui_->spinBox_decimation, SIGNAL(valueChanged(int)), this, SLOT(updateConstraintView()));
00353 connect(ui_->spinBox_decimation, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
00354 connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(updateGraphView()));
00355 connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
00356 connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
00357
00358 ui_->label_stereo_inliers_name->setStyleSheet("QLabel {color : blue; }");
00359 ui_->label_stereo_flowOutliers_name->setStyleSheet("QLabel {color : red; }");
00360 ui_->label_stereo_slopeOutliers_name->setStyleSheet("QLabel {color : yellow; }");
00361 ui_->label_stereo_disparityOutliers_name->setStyleSheet("QLabel {color : magenta; }");
00362
00363
00364
00365 connect(ui_->graphViewer, SIGNAL(configChanged()), this, SLOT(configModified()));
00366
00367
00368 connect(ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
00369 connect(ui_->actionVertical_Layout, SIGNAL(toggled(bool)), this, SLOT(configModified()));
00370 connect(ui_->checkBox_alignPosesWithGroundTruth, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
00371 connect(ui_->checkBox_alignScansCloudsWithGroundTruth, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
00372 connect(ui_->checkBox_ignoreIntermediateNodes, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
00373 connect(ui_->checkBox_ignoreIntermediateNodes, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
00374 connect(ui_->checkBox_timeStats, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
00375 connect(ui_->checkBox_timeStats, SIGNAL(stateChanged(int)), this, SLOT(updateStatistics()));
00376
00377 connect(ui_->doubleSpinBox_gainCompensationRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00378 connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00379 connect(ui_->spinBox_decimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
00380 connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(configModified()));
00381 connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00382 connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00383
00384 connect(ui_->spinBox_icp_decimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
00385 connect(ui_->doubleSpinBox_icp_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00386 connect(ui_->doubleSpinBox_icp_minDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00387 connect(ui_->checkBox_icp_from_depth, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
00388
00389 connect(ui_->doubleSpinBox_detectMore_radius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00390 connect(ui_->doubleSpinBox_detectMore_angle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00391 connect(ui_->spinBox_detectMore_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
00392
00393 connect(ui_->lineEdit_obstacleColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified()));
00394 connect(ui_->lineEdit_groundColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified()));
00395 connect(ui_->lineEdit_emptyColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified()));
00396 connect(ui_->lineEdit_obstacleColor, SIGNAL(textChanged(const QString &)), this, SLOT(updateGrid()));
00397 connect(ui_->lineEdit_groundColor, SIGNAL(textChanged(const QString &)), this, SLOT(updateGrid()));
00398 connect(ui_->lineEdit_emptyColor, SIGNAL(textChanged(const QString &)), this, SLOT(updateGrid()));
00399 connect(ui_->toolButton_obstacleColor, SIGNAL(clicked(bool)), this, SLOT(selectObstacleColor()));
00400 connect(ui_->toolButton_groundColor, SIGNAL(clicked(bool)), this, SLOT(selectGroundColor()));
00401 connect(ui_->toolButton_emptyColor, SIGNAL(clicked(bool)), this, SLOT(selectEmptyColor()));
00402
00403 connect(exportDialog_, SIGNAL(configChanged()), this, SLOT(configModified()));
00404
00405
00406 QList<QDockWidget*> dockWidgets = this->findChildren<QDockWidget*>();
00407 for(int i=0; i<dockWidgets.size(); ++i)
00408 {
00409 connect(dockWidgets[i], SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(configModified()));
00410 connect(dockWidgets[i]->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configModified()));
00411 }
00412 ui_->dockWidget_constraints->installEventFilter(this);
00413 ui_->dockWidget_graphView->installEventFilter(this);
00414 ui_->dockWidget_occupancyGridView->installEventFilter(this);
00415 ui_->dockWidget_stereoView->installEventFilter(this);
00416 ui_->dockWidget_view3d->installEventFilter(this);
00417 ui_->dockWidget_guiparameters->installEventFilter(this);
00418 ui_->dockWidget_coreparameters->installEventFilter(this);
00419 ui_->dockWidget_info->installEventFilter(this);
00420 ui_->dockWidget_statistics->installEventFilter(this);
00421 }
00422
00423 DatabaseViewer::~DatabaseViewer()
00424 {
00425 delete ui_;
00426 delete dbDriver_;
00427 #ifdef RTABMAP_OCTOMAP
00428 delete octomap_;
00429 #endif
00430 }
00431
00432 void DatabaseViewer::setupMainLayout(bool vertical)
00433 {
00434 if(vertical)
00435 {
00436 qobject_cast<QHBoxLayout *>(ui_->horizontalLayout_imageViews->layout())->setDirection(QBoxLayout::TopToBottom);
00437 }
00438 else if(!vertical)
00439 {
00440 qobject_cast<QHBoxLayout *>(ui_->horizontalLayout_imageViews->layout())->setDirection(QBoxLayout::LeftToRight);
00441 }
00442 if(ids_.size())
00443 {
00444 sliderAValueChanged(ui_->horizontalSlider_A->value());
00445 }
00446 }
00447
00448 void DatabaseViewer::showCloseButton(bool visible)
00449 {
00450 ui_->buttonBox->setVisible(visible);
00451 }
00452
00453 void DatabaseViewer::configModified()
00454 {
00455 this->setWindowModified(true);
00456 }
00457
00458 QString DatabaseViewer::getIniFilePath() const
00459 {
00460 if(!iniFilePath_.isEmpty())
00461 {
00462 return iniFilePath_;
00463 }
00464 QString privatePath = QDir::homePath() + "/.rtabmap";
00465 if(!QDir(privatePath).exists())
00466 {
00467 QDir::home().mkdir(".rtabmap");
00468 }
00469 return privatePath + "/rtabmap.ini";
00470 }
00471
00472 void DatabaseViewer::readSettings()
00473 {
00474 QString path = getIniFilePath();
00475 QSettings settings(path, QSettings::IniFormat);
00476 settings.beginGroup("DatabaseViewer");
00477
00478
00479 QByteArray bytes;
00480 bytes = settings.value("geometry", QByteArray()).toByteArray();
00481 if(!bytes.isEmpty())
00482 {
00483 this->restoreGeometry(bytes);
00484 }
00485 bytes = settings.value("state", QByteArray()).toByteArray();
00486 if(!bytes.isEmpty())
00487 {
00488 this->restoreState(bytes);
00489 }
00490 savedMaximized_ = settings.value("maximized", false).toBool();
00491
00492 ui_->comboBox_logger_level->setCurrentIndex(settings.value("loggerLevel", ui_->comboBox_logger_level->currentIndex()).toInt());
00493 ui_->actionVertical_Layout->setChecked(settings.value("verticalLayout", ui_->actionVertical_Layout->isChecked()).toBool());
00494 ui_->checkBox_ignoreIntermediateNodes->setChecked(settings.value("ignoreIntermediateNodes", ui_->checkBox_ignoreIntermediateNodes->isChecked()).toBool());
00495 ui_->checkBox_timeStats->setChecked(settings.value("timeStats", ui_->checkBox_timeStats->isChecked()).toBool());
00496
00497
00498 ui_->graphViewer->loadSettings(settings, "GraphView");
00499
00500 settings.beginGroup("optimization");
00501 ui_->doubleSpinBox_gainCompensationRadius->setValue(settings.value("gainCompensationRadius", ui_->doubleSpinBox_gainCompensationRadius->value()).toDouble());
00502 ui_->doubleSpinBox_voxelSize->setValue(settings.value("voxelSize", ui_->doubleSpinBox_voxelSize->value()).toDouble());
00503 ui_->spinBox_decimation->setValue(settings.value("decimation", ui_->spinBox_decimation->value()).toInt());
00504 settings.endGroup();
00505
00506 settings.beginGroup("grid");
00507 ui_->groupBox_posefiltering->setChecked(settings.value("poseFiltering", ui_->groupBox_posefiltering->isChecked()).toBool());
00508 ui_->doubleSpinBox_posefilteringRadius->setValue(settings.value("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value()).toDouble());
00509 ui_->doubleSpinBox_posefilteringAngle->setValue(settings.value("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value()).toDouble());
00510 ui_->lineEdit_obstacleColor->setText(settings.value("colorObstacle", ui_->lineEdit_obstacleColor->text()).toString());
00511 ui_->lineEdit_groundColor->setText(settings.value("colorGround", ui_->lineEdit_groundColor->text()).toString());
00512 ui_->lineEdit_emptyColor->setText(settings.value("colorEmpty", ui_->lineEdit_emptyColor->text()).toString());
00513 settings.endGroup();
00514
00515 settings.beginGroup("mesh");
00516 ui_->checkBox_mesh_quad->setChecked(settings.value("quad", ui_->checkBox_mesh_quad->isChecked()).toBool());
00517 ui_->spinBox_mesh_angleTolerance->setValue(settings.value("angleTolerance", ui_->spinBox_mesh_angleTolerance->value()).toInt());
00518 ui_->spinBox_mesh_minClusterSize->setValue(settings.value("minClusterSize", ui_->spinBox_mesh_minClusterSize->value()).toInt());
00519 ui_->spinBox_mesh_fillDepthHoles->setValue(settings.value("fillDepthHolesSize", ui_->spinBox_mesh_fillDepthHoles->value()).toInt());
00520 ui_->spinBox_mesh_depthError->setValue(settings.value("fillDepthHolesError", ui_->spinBox_mesh_depthError->value()).toInt());
00521 ui_->spinBox_mesh_triangleSize->setValue(settings.value("triangleSize", ui_->spinBox_mesh_triangleSize->value()).toInt());
00522 settings.endGroup();
00523
00524
00525
00526
00527
00528
00529 settings.beginGroup("icp");
00530 ui_->spinBox_icp_decimation->setValue(settings.value("decimation", ui_->spinBox_icp_decimation->value()).toInt());
00531 ui_->doubleSpinBox_icp_maxDepth->setValue(settings.value("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value()).toDouble());
00532 ui_->doubleSpinBox_icp_minDepth->setValue(settings.value("minDepth", ui_->doubleSpinBox_icp_minDepth->value()).toDouble());
00533 ui_->checkBox_icp_from_depth->setChecked(settings.value("icpFromDepth", ui_->checkBox_icp_from_depth->isChecked()).toBool());
00534 settings.endGroup();
00535
00536 settings.endGroup();
00537
00538
00539 settings.beginGroup("Gui");
00540 exportDialog_->loadSettings(settings, exportDialog_->objectName());
00541 settings.beginGroup("PostProcessingDialog");
00542 ui_->doubleSpinBox_detectMore_radius->setValue(settings.value("cluster_radius", ui_->doubleSpinBox_detectMore_radius->value()).toDouble());
00543 ui_->doubleSpinBox_detectMore_angle->setValue(settings.value("cluster_angle", ui_->doubleSpinBox_detectMore_angle->value()).toDouble());
00544 ui_->spinBox_detectMore_iterations->setValue(settings.value("iterations", ui_->spinBox_detectMore_iterations->value()).toInt());
00545 settings.endGroup();
00546 settings.endGroup();
00547
00548
00549 ParametersMap parameters;
00550 Parameters::readINI(path.toStdString(), parameters);
00551 for(ParametersMap::iterator iter = parameters.begin(); iter!= parameters.end(); ++iter)
00552 {
00553 ui_->parameters_toolbox->updateParameter(iter->first, iter->second);
00554 }
00555 }
00556
00557 void DatabaseViewer::writeSettings()
00558 {
00559 QString path = getIniFilePath();
00560 QSettings settings(path, QSettings::IniFormat);
00561 settings.beginGroup("DatabaseViewer");
00562
00563
00564 if(!this->isMaximized())
00565 {
00566 settings.setValue("geometry", this->saveGeometry());
00567 }
00568 settings.setValue("state", this->saveState());
00569 settings.setValue("maximized", this->isMaximized());
00570 savedMaximized_ = this->isMaximized();
00571
00572 settings.setValue("loggerLevel", ui_->comboBox_logger_level->currentIndex());
00573 settings.setValue("verticalLayout", ui_->actionVertical_Layout->isChecked());
00574 settings.setValue("ignoreIntermediateNodes", ui_->checkBox_ignoreIntermediateNodes->isChecked());
00575 settings.setValue("timeStats", ui_->checkBox_timeStats->isChecked());
00576
00577
00578 ui_->graphViewer->saveSettings(settings, "GraphView");
00579
00580
00581 settings.beginGroup("optimization");
00582 settings.setValue("gainCompensationRadius", ui_->doubleSpinBox_gainCompensationRadius->value());
00583 settings.setValue("voxelSize", ui_->doubleSpinBox_voxelSize->value());
00584 settings.setValue("decimation", ui_->spinBox_decimation->value());
00585 settings.endGroup();
00586
00587
00588 settings.beginGroup("grid");
00589 settings.setValue("poseFiltering", ui_->groupBox_posefiltering->isChecked());
00590 settings.setValue("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value());
00591 settings.setValue("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value());
00592 settings.setValue("colorObstacle", ui_->lineEdit_obstacleColor->text());
00593 settings.setValue("colorGround", ui_->lineEdit_groundColor->text());
00594 settings.setValue("colorEmpty", ui_->lineEdit_emptyColor->text());
00595 settings.endGroup();
00596
00597 settings.beginGroup("mesh");
00598 settings.setValue("quad", ui_->checkBox_mesh_quad->isChecked());
00599 settings.setValue("angleTolerance", ui_->spinBox_mesh_angleTolerance->value());
00600 settings.setValue("minClusterSize", ui_->spinBox_mesh_minClusterSize->value());
00601 settings.setValue("fillDepthHolesSize", ui_->spinBox_mesh_fillDepthHoles->value());
00602 settings.setValue("fillDepthHolesError", ui_->spinBox_mesh_depthError->value());
00603 settings.setValue("triangleSize", ui_->spinBox_mesh_triangleSize->value());
00604 settings.endGroup();
00605
00606
00607
00608
00609
00610
00611 settings.beginGroup("icp");
00612 settings.setValue("decimation", ui_->spinBox_icp_decimation->value());
00613 settings.setValue("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value());
00614 settings.setValue("minDepth", ui_->doubleSpinBox_icp_minDepth->value());
00615 settings.setValue("icpFromDepth", ui_->checkBox_icp_from_depth->isChecked());
00616 settings.endGroup();
00617
00618 settings.endGroup();
00619
00620
00621 settings.beginGroup("Gui");
00622 exportDialog_->saveSettings(settings, exportDialog_->objectName());
00623 settings.beginGroup("PostProcessingDialog");
00624 settings.setValue("cluster_radius", ui_->doubleSpinBox_detectMore_radius->value());
00625 settings.setValue("cluster_angle", ui_->doubleSpinBox_detectMore_angle->value());
00626 settings.setValue("iterations", ui_->spinBox_detectMore_iterations->value());
00627 settings.endGroup();
00628 settings.endGroup();
00629
00630 ParametersMap parameters = ui_->parameters_toolbox->getParameters();
00631 for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end();)
00632 {
00633 if(!ui_->parameters_toolbox->getParameterWidget(iter->first.c_str()))
00634 {
00635 parameters.erase(iter++);
00636 }
00637 else
00638 {
00639 ++iter;
00640 }
00641 }
00642 Parameters::writeINI(path.toStdString(), parameters);
00643
00644 this->setWindowModified(false);
00645 }
00646
00647 void DatabaseViewer::restoreDefaultSettings()
00648 {
00649
00650 ui_->comboBox_logger_level->setCurrentIndex(1);
00651 ui_->checkBox_alignPosesWithGroundTruth->setChecked(true);
00652 ui_->checkBox_alignScansCloudsWithGroundTruth->setChecked(false);
00653 ui_->checkBox_ignoreIntermediateNodes->setChecked(false);
00654 ui_->checkBox_timeStats->setChecked(true);
00655
00656 ui_->checkBox_iterativeOptimization->setChecked(true);
00657 ui_->checkBox_spanAllMaps->setChecked(true);
00658 ui_->checkBox_wmState->setChecked(false);
00659 ui_->checkBox_ignorePoseCorrection->setChecked(false);
00660 ui_->checkBox_ignoreGlobalLoop->setChecked(false);
00661 ui_->checkBox_ignoreLocalLoopSpace->setChecked(false);
00662 ui_->checkBox_ignoreLocalLoopTime->setChecked(false);
00663 ui_->checkBox_ignoreUserLoop->setChecked(false);
00664 ui_->spinBox_optimizationDepth->setValue(0);
00665 ui_->doubleSpinBox_optimizationScale->setValue(1.0);
00666 ui_->doubleSpinBox_gainCompensationRadius->setValue(0.0);
00667 ui_->doubleSpinBox_voxelSize->setValue(0.0);
00668 ui_->spinBox_decimation->setValue(1);
00669
00670 ui_->groupBox_posefiltering->setChecked(false);
00671 ui_->doubleSpinBox_posefilteringRadius->setValue(0.1);
00672 ui_->doubleSpinBox_posefilteringAngle->setValue(30);
00673 ui_->checkBox_grid_empty->setChecked(true);
00674 ui_->checkBox_octomap->setChecked(false);
00675 ui_->lineEdit_obstacleColor->setText(QColor(Qt::red).name());
00676 ui_->lineEdit_groundColor->setText(QColor(Qt::green).name());
00677 ui_->lineEdit_emptyColor->setText(QColor(Qt::yellow).name());
00678
00679 ui_->checkBox_mesh_quad->setChecked(true);
00680 ui_->spinBox_mesh_angleTolerance->setValue(15);
00681 ui_->spinBox_mesh_minClusterSize->setValue(0);
00682 ui_->spinBox_mesh_fillDepthHoles->setValue(false);
00683 ui_->spinBox_mesh_depthError->setValue(10);
00684 ui_->spinBox_mesh_triangleSize->setValue(2);
00685
00686 ui_->spinBox_icp_decimation->setValue(1);
00687 ui_->doubleSpinBox_icp_maxDepth->setValue(0.0);
00688 ui_->doubleSpinBox_icp_minDepth->setValue(0.0);
00689 ui_->checkBox_icp_from_depth->setChecked(false);
00690
00691 ui_->doubleSpinBox_detectMore_radius->setValue(1.0);
00692 ui_->doubleSpinBox_detectMore_angle->setValue(30.0);
00693 ui_->spinBox_detectMore_iterations->setValue(5);
00694 }
00695
00696 void DatabaseViewer::openDatabase()
00697 {
00698 QString path = QFileDialog::getOpenFileName(this, tr("Select file"), pathDatabase_, tr("Databases (*.db)"));
00699 if(!path.isEmpty())
00700 {
00701 openDatabase(path);
00702 }
00703 }
00704
00705 bool DatabaseViewer::openDatabase(const QString & path)
00706 {
00707 UDEBUG("Open database \"%s\"", path.toStdString().c_str());
00708 if(QFile::exists(path))
00709 {
00710 if(QFileInfo(path).isFile())
00711 {
00712 std::string driverType = "sqlite3";
00713
00714 dbDriver_ = DBDriver::create();
00715
00716 if(!dbDriver_->openConnection(path.toStdString()))
00717 {
00718 ui_->actionClose_database->setEnabled(false);
00719 ui_->actionOpen_database->setEnabled(true);
00720 delete dbDriver_;
00721 dbDriver_ = 0;
00722 QMessageBox::warning(this, "Database error", tr("Can't open database \"%1\"").arg(path));
00723 }
00724 else
00725 {
00726 ui_->actionClose_database->setEnabled(true);
00727 ui_->actionOpen_database->setEnabled(false);
00728
00729 pathDatabase_ = UDirectory::getDir(path.toStdString()).c_str();
00730 if(pathDatabase_.isEmpty() || pathDatabase_.compare(".") == 0)
00731 {
00732 pathDatabase_ = QDir::currentPath();
00733 }
00734 databaseFileName_ = UFile::getName(path.toStdString());
00735 ui_->graphViewer->setWorkingDirectory(pathDatabase_);
00736
00737
00738 ParametersMap parameters = dbDriver_->getLastParameters();
00739
00740 if(parameters.size())
00741 {
00742 const ParametersMap & currentParameters = ui_->parameters_toolbox->getParameters();
00743 ParametersMap differentParameters;
00744 for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
00745 {
00746 ParametersMap::const_iterator jter = currentParameters.find(iter->first);
00747 if(jter!=currentParameters.end() &&
00748 ui_->parameters_toolbox->getParameterWidget(QString(iter->first.c_str())) != 0 &&
00749 iter->second.compare(jter->second) != 0 &&
00750 iter->first.compare(Parameters::kRtabmapWorkingDirectory()) != 0)
00751 {
00752 bool different = true;
00753 if(Parameters::getType(iter->first).compare("double") ==0 ||
00754 Parameters::getType(iter->first).compare("float") == 0)
00755 {
00756 if(uStr2Double(iter->second) == uStr2Double(jter->second))
00757 {
00758 different = false;
00759 }
00760 }
00761 if(different)
00762 {
00763 differentParameters.insert(*iter);
00764 QString msg = tr("Parameter \"%1\": database=\"%2\" Preferences=\"%3\"")
00765 .arg(iter->first.c_str())
00766 .arg(iter->second.c_str())
00767 .arg(jter->second.c_str());
00768 UWARN(msg.toStdString().c_str());
00769 }
00770 }
00771 }
00772
00773 if(differentParameters.size())
00774 {
00775 int r = QMessageBox::question(this,
00776 tr("Update parameters..."),
00777 tr("The database is using %1 different parameter(s) than "
00778 "those currently set in Core parameters panel. Do you want "
00779 "to use database's parameters?").arg(differentParameters.size()),
00780 QMessageBox::Yes | QMessageBox::No,
00781 QMessageBox::Yes);
00782 if(r == QMessageBox::Yes)
00783 {
00784 QStringList str;
00785 for(rtabmap::ParametersMap::const_iterator iter = differentParameters.begin(); iter!=differentParameters.end(); ++iter)
00786 {
00787 ui_->parameters_toolbox->updateParameter(iter->first, iter->second);
00788 str.push_back(iter->first.c_str());
00789 }
00790 notifyParametersChanged(str);
00791 }
00792 }
00793 }
00794
00795 updateIds();
00796 return true;
00797 }
00798 }
00799 else
00800 {
00801 pathDatabase_ = path;
00802 if(pathDatabase_.isEmpty() || pathDatabase_.compare(".") == 0)
00803 {
00804 pathDatabase_ = QDir::currentPath();
00805 }
00806 ui_->graphViewer->setWorkingDirectory(pathDatabase_);
00807 }
00808 }
00809 else
00810 {
00811 QMessageBox::warning(this, "Database error", tr("Database \"%1\" does not exist.").arg(path));
00812 }
00813 return false;
00814 }
00815
00816 bool DatabaseViewer::closeDatabase()
00817 {
00818 if(dbDriver_)
00819 {
00820 if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size())
00821 {
00822 QMessageBox::StandardButton button = QMessageBox::question(this,
00823 tr("Links modified"),
00824 tr("Some links are modified (%1 added, %2 refined, %3 removed), do you want to save them?")
00825 .arg(linksAdded_.size()).arg(linksRefined_.size()).arg(linksRemoved_.size()),
00826 QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
00827 QMessageBox::Cancel);
00828
00829 if(button == QMessageBox::Yes)
00830 {
00831
00832 for(std::multimap<int, rtabmap::Link>::iterator iter=linksAdded_.begin(); iter!=linksAdded_.end(); ++iter)
00833 {
00834 std::multimap<int, rtabmap::Link>::iterator refinedIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
00835 if(refinedIter != linksRefined_.end())
00836 {
00837 dbDriver_->addLink(refinedIter->second);
00838 dbDriver_->addLink(refinedIter->second.inverse());
00839 }
00840 else
00841 {
00842 dbDriver_->addLink(iter->second);
00843 dbDriver_->addLink(iter->second.inverse());
00844 }
00845 }
00846
00847
00848 for(std::multimap<int, rtabmap::Link>::iterator iter=linksRefined_.begin(); iter!=linksRefined_.end(); ++iter)
00849 {
00850 if(!containsLink(linksAdded_, iter->second.from(), iter->second.to()))
00851 {
00852 dbDriver_->updateLink(iter->second);
00853 dbDriver_->updateLink(iter->second.inverse());
00854 }
00855 }
00856
00857
00858 for(std::multimap<int, rtabmap::Link>::iterator iter=linksRemoved_.begin(); iter!=linksRemoved_.end(); ++iter)
00859 {
00860 dbDriver_->removeLink(iter->second.to(), iter->second.from());
00861 dbDriver_->removeLink(iter->second.from(), iter->second.to());
00862 }
00863 linksAdded_.clear();
00864 linksRefined_.clear();
00865 linksRemoved_.clear();
00866 }
00867
00868 if(button != QMessageBox::Yes && button != QMessageBox::No)
00869 {
00870 return false;
00871 }
00872 }
00873
00874 if( generatedLocalMaps_.size() &&
00875 uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.11.10") >= 0)
00876 {
00877 QMessageBox::StandardButton button = QMessageBox::question(this,
00878 tr("Local occupancy grid maps modified"),
00879 tr("%1 occupancy grid maps are modified, do you want to "
00880 "save them? This will overwrite occupancy grids saved in the database.")
00881 .arg(generatedLocalMaps_.size()),
00882 QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
00883 QMessageBox::Cancel);
00884
00885 if(button == QMessageBox::Yes)
00886 {
00887
00888 UASSERT(generatedLocalMaps_.size() == generatedLocalMapsInfo_.size());
00889 std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat > >::iterator mapIter = generatedLocalMaps_.begin();
00890 std::map<int, std::pair<float, cv::Point3f> >::iterator infoIter = generatedLocalMapsInfo_.begin();
00891 for(; mapIter!=generatedLocalMaps_.end(); ++mapIter, ++infoIter)
00892 {
00893 UASSERT(mapIter->first == infoIter->first);
00894 dbDriver_->updateOccupancyGrid(
00895 mapIter->first,
00896 mapIter->second.first.first,
00897 mapIter->second.first.second,
00898 mapIter->second.second,
00899 infoIter->second.first,
00900 infoIter->second.second);
00901 }
00902 generatedLocalMaps_.clear();
00903 generatedLocalMapsInfo_.clear();
00904 localMaps_.clear();
00905 localMapsInfo_.clear();
00906 }
00907
00908 if(button != QMessageBox::Yes && button != QMessageBox::No)
00909 {
00910 return false;
00911 }
00912 }
00913
00914 delete dbDriver_;
00915 dbDriver_ = 0;
00916 ids_.clear();
00917 idToIndex_.clear();
00918 neighborLinks_.clear();
00919 loopLinks_.clear();
00920 graphes_.clear();
00921 graphLinks_.clear();
00922 odomPoses_.clear();
00923 groundTruthPoses_.clear();
00924 gpsPoses_.clear();
00925 gpsValues_.clear();
00926 mapIds_.clear();
00927 weights_.clear();
00928 wmStates_.clear();
00929 links_.clear();
00930 linksAdded_.clear();
00931 linksRefined_.clear();
00932 linksRemoved_.clear();
00933 localMaps_.clear();
00934 localMapsInfo_.clear();
00935 generatedLocalMaps_.clear();
00936 generatedLocalMapsInfo_.clear();
00937 ui_->graphViewer->clearAll();
00938 occupancyGridViewer_->clear();
00939 ui_->menuEdit->setEnabled(false);
00940 ui_->actionGenerate_3D_map_pcd->setEnabled(false);
00941 ui_->actionExport->setEnabled(false);
00942 ui_->actionExtract_images->setEnabled(false);
00943 ui_->menuExport_poses->setEnabled(false);
00944 ui_->menuExport_GPS->setEnabled(false);
00945 ui_->actionPoses_KML->setEnabled(false);
00946 ui_->actionExport_saved_2D_map->setEnabled(false);
00947 ui_->actionImport_2D_map->setEnabled(false);
00948 ui_->actionView_optimized_mesh->setEnabled(false);
00949 ui_->actionExport_optimized_mesh->setEnabled(false);
00950 ui_->actionUpdate_optimized_mesh->setEnabled(false);
00951 ui_->checkBox_showOptimized->setEnabled(false);
00952 ui_->toolBox_statistics->clear();
00953 databaseFileName_.clear();
00954 ui_->checkBox_alignPosesWithGroundTruth->setVisible(false);
00955 ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(false);
00956 ui_->doubleSpinBox_optimizationScale->setVisible(false);
00957 ui_->label_scale_title->setVisible(false);
00958 ui_->label_rmse->setVisible(false);
00959 ui_->label_rmse_title->setVisible(false);
00960 ui_->checkBox_ignoreIntermediateNodes->setVisible(false);
00961 ui_->label_ignoreINtermediateNdoes->setVisible(false);
00962 ui_->label_alignPosesWithGroundTruth->setVisible(false);
00963 ui_->label_alignScansCloudsWithGroundTruth->setVisible(false);
00964 ui_->label_optimizeFrom->setText(tr("Root"));
00965 ui_->textEdit_info->clear();
00966
00967 ui_->pushButton_refine->setEnabled(false);
00968 ui_->pushButton_add->setEnabled(false);
00969 ui_->pushButton_reset->setEnabled(false);
00970 ui_->pushButton_reject->setEnabled(false);
00971
00972 ui_->horizontalSlider_loops->setEnabled(false);
00973 ui_->horizontalSlider_loops->setMaximum(0);
00974 ui_->horizontalSlider_iterations->setEnabled(false);
00975 ui_->horizontalSlider_iterations->setMaximum(0);
00976 ui_->horizontalSlider_neighbors->setEnabled(false);
00977 ui_->horizontalSlider_neighbors->setMaximum(0);
00978 ui_->label_constraint->clear();
00979 ui_->label_constraint_opt->clear();
00980 ui_->label_variance->clear();
00981 ui_->lineEdit_covariance->clear();
00982
00983 ui_->horizontalSlider_A->setEnabled(false);
00984 ui_->horizontalSlider_A->setMaximum(0);
00985 ui_->horizontalSlider_B->setEnabled(false);
00986 ui_->horizontalSlider_B->setMaximum(0);
00987 ui_->label_idA->setText("NaN");
00988 ui_->label_idB->setText("NaN");
00989 sliderAValueChanged(0);
00990 sliderBValueChanged(0);
00991
00992 constraintsViewer_->clear();
00993 constraintsViewer_->update();
00994
00995 cloudViewer_->clear();
00996 cloudViewer_->update();
00997
00998 occupancyGridViewer_->clear();
00999 occupancyGridViewer_->update();
01000
01001 ui_->graphViewer->clearAll();
01002 ui_->label_loopClosures->clear();
01003 ui_->label_timeOptimization->clear();
01004 ui_->label_pathLength->clear();
01005 ui_->label_poses->clear();
01006 ui_->label_rmse->clear();
01007 ui_->spinBox_optimizationsFrom->setEnabled(false);
01008
01009 ui_->graphicsView_A->clear();
01010 ui_->graphicsView_B->clear();
01011
01012 ui_->graphicsView_stereo->clear();
01013 stereoViewer_->clear();
01014 stereoViewer_->update();
01015
01016 ui_->toolBox_statistics->clear();
01017
01018 useLastOptimizedGraphAsGuess_ = false;
01019 lastOptimizedGraph_.clear();
01020 }
01021
01022 ui_->actionClose_database->setEnabled(dbDriver_ != 0);
01023 ui_->actionOpen_database->setEnabled(dbDriver_ == 0);
01024
01025 return dbDriver_ == 0;
01026 }
01027
01028
01029 void DatabaseViewer::recoverDatabase()
01030 {
01031 QString path = QFileDialog::getOpenFileName(this, tr("Select file"), pathDatabase_, tr("Databases (*.db)"));
01032 if(!path.isEmpty())
01033 {
01034 if(path.compare(pathDatabase_+QDir::separator()+databaseFileName_.c_str()) == 0)
01035 {
01036 QMessageBox::information(this, "Database recovery", tr("The selected database is already opened, close it first."));
01037 return;
01038 }
01039 std::string errorMsg;
01040 rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
01041 progressDialog->setAttribute(Qt::WA_DeleteOnClose);
01042 progressDialog->setMaximumSteps(100);
01043 progressDialog->show();
01044 progressDialog->setCancelButtonVisible(true);
01045 RecoveryState state(progressDialog);
01046 if(databaseRecovery(path.toStdString(), false, &errorMsg, &state))
01047 {
01048 QMessageBox::information(this, "Database recovery", tr("Database \"%1\" recovered! Try opening it again.").arg(path));
01049 }
01050 else
01051 {
01052 QMessageBox::warning(this, "Database recovery", tr("Database recovery failed: \"%1\".").arg(errorMsg.c_str()));
01053 }
01054 progressDialog->setValue(progressDialog->maximumSteps());
01055 }
01056 }
01057
01058 void DatabaseViewer::closeEvent(QCloseEvent* event)
01059 {
01060
01061 bool save = false;
01062 if(this->isWindowModified())
01063 {
01064 QMessageBox::Button b=QMessageBox::question(this,
01065 tr("Database Viewer"),
01066 tr("There are unsaved changed settings. Save them?"),
01067 QMessageBox::Save | QMessageBox::Cancel | QMessageBox::Discard);
01068 if(b == QMessageBox::Save)
01069 {
01070 save = true;
01071 }
01072 else if(b != QMessageBox::Discard)
01073 {
01074 event->ignore();
01075 return;
01076 }
01077 }
01078
01079 if(save)
01080 {
01081 writeSettings();
01082 }
01083
01084 event->accept();
01085
01086 if(!closeDatabase())
01087 {
01088 event->ignore();
01089 }
01090
01091 if(event->isAccepted())
01092 {
01093 ui_->toolBox_statistics->closeFigures();
01094 if(dbDriver_)
01095 {
01096 delete dbDriver_;
01097 dbDriver_ = 0;
01098 }
01099 }
01100 }
01101
01102 void DatabaseViewer::showEvent(QShowEvent* anEvent)
01103 {
01104 this->setWindowModified(false);
01105
01106 if((ui_->graphViewer->isVisible() || ui_->dockWidget_occupancyGridView->isVisible()) && graphes_.size() && localMaps_.size()==0)
01107 {
01108 sliderIterationsValueChanged((int)graphes_.size()-1);
01109 }
01110 }
01111
01112 void DatabaseViewer::moveEvent(QMoveEvent* anEvent)
01113 {
01114 if(this->isVisible())
01115 {
01116
01117 if(!firstCall_)
01118 {
01119 this->configModified();
01120 }
01121 firstCall_ = false;
01122 }
01123 }
01124
01125 void DatabaseViewer::resizeEvent(QResizeEvent* anEvent)
01126 {
01127 if(this->isVisible())
01128 {
01129 this->configModified();
01130 }
01131 }
01132
01133 void DatabaseViewer::keyPressEvent(QKeyEvent *event)
01134 {
01135
01136 if((event->modifiers() & Qt::ControlModifier) && event->key() == Qt::Key_S)
01137 {
01138 this->writeSettings();
01139 }
01140 }
01141
01142 bool DatabaseViewer::eventFilter(QObject *obj, QEvent *event)
01143 {
01144 if (event->type() == QEvent::Resize && qobject_cast<QDockWidget*>(obj))
01145 {
01146 this->setWindowModified(true);
01147 }
01148 return QWidget::eventFilter(obj, event);
01149 }
01150
01151
01152 void DatabaseViewer::exportDatabase()
01153 {
01154 if(!dbDriver_ || ids_.size() == 0)
01155 {
01156 return;
01157 }
01158
01159 rtabmap::ExportDialog dialog;
01160
01161 if(dialog.exec())
01162 {
01163 if(!dialog.outputPath().isEmpty())
01164 {
01165 int framesIgnored = dialog.framesIgnored();
01166 double frameRate = dialog.targetFramerate();
01167 int sessionExported = dialog.sessionExported();
01168 QString path = dialog.outputPath();
01169 rtabmap::DataRecorder recorder;
01170 QList<int> ids;
01171
01172 double previousStamp = 0;
01173 std::vector<double> delays(ids_.size());
01174 int oi=0;
01175 std::map<int, Transform> poses;
01176 std::map<int, double> stamps;
01177 std::map<int, Transform> groundTruths;
01178 std::map<int, GPS> gpsValues;
01179 for(int i=0; i<ids_.size(); i+=1+framesIgnored)
01180 {
01181 Transform odomPose, groundTruth;
01182 int weight = -1;
01183 int mapId = -1;
01184 std::string label;
01185 double stamp = 0;
01186 std::vector<float> velocity;
01187 GPS gps;
01188 if(dbDriver_->getNodeInfo(ids_[i], odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps))
01189 {
01190 if(frameRate == 0 ||
01191 previousStamp == 0 ||
01192 stamp == 0 ||
01193 stamp - previousStamp >= 1.0/frameRate)
01194 {
01195 if(sessionExported < 0 || sessionExported == mapId)
01196 {
01197 ids.push_back(ids_[i]);
01198
01199 if(previousStamp && stamp)
01200 {
01201 delays[oi++] = stamp - previousStamp;
01202 }
01203 previousStamp = stamp;
01204
01205 poses.insert(std::make_pair(ids_[i], odomPose));
01206 stamps.insert(std::make_pair(ids_[i], stamp));
01207 groundTruths.insert(std::make_pair(ids_[i], groundTruth));
01208 if(gps.stamp() > 0.0)
01209 {
01210 gpsValues.insert(std::make_pair(ids_[i], gps));
01211 }
01212 }
01213 }
01214 if(sessionExported >= 0 && mapId > sessionExported)
01215 {
01216 break;
01217 }
01218 }
01219 }
01220 delays.resize(oi);
01221
01222 if(recorder.init(path, false))
01223 {
01224 rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
01225 progressDialog->setAttribute(Qt::WA_DeleteOnClose);
01226 progressDialog->setMaximumSteps(ids.size());
01227 progressDialog->show();
01228 progressDialog->setCancelButtonVisible(true);
01229 UINFO("Decompress: rgb=%d depth=%d scan=%d userData=%d",
01230 dialog.isRgbExported()?1:0,
01231 dialog.isDepthExported()?1:0,
01232 dialog.isDepth2dExported()?1:0,
01233 dialog.isUserDataExported()?1:0);
01234
01235 for(int i=0; i<ids.size() && !progressDialog->isCanceled(); ++i)
01236 {
01237 int id = ids.at(i);
01238
01239 SensorData data;
01240 dbDriver_->getNodeData(id, data);
01241 cv::Mat depth, rgb, userData;
01242 LaserScan scan;
01243 data.uncompressDataConst(
01244 !dialog.isRgbExported()?0:&rgb,
01245 !dialog.isDepthExported()?0:&depth,
01246 !dialog.isDepth2dExported()?0:&scan,
01247 !dialog.isUserDataExported()?0:&userData);
01248 cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
01249 if(dialog.isOdomExported())
01250 {
01251 std::map<int, Link> links;
01252 dbDriver_->loadLinks(id, links, Link::kNeighbor);
01253 if(links.size() && links.begin()->first < id)
01254 {
01255 covariance = links.begin()->second.infMatrix().inv();
01256 }
01257 }
01258
01259 rtabmap::SensorData sensorData;
01260 if(data.cameraModels().size())
01261 {
01262 sensorData = rtabmap::SensorData(
01263 scan,
01264 rgb,
01265 depth,
01266 data.cameraModels(),
01267 id,
01268 stamps.at(id),
01269 userData);
01270 }
01271 else
01272 {
01273 sensorData = rtabmap::SensorData(
01274 scan,
01275 rgb,
01276 depth,
01277 data.stereoCameraModel(),
01278 id,
01279 stamps.at(id),
01280 userData);
01281 }
01282 if(groundTruths.find(id)!=groundTruths.end())
01283 {
01284 sensorData.setGroundTruth(groundTruths.at(id));
01285 }
01286 if(gpsValues.find(id)!=gpsValues.end())
01287 {
01288 sensorData.setGPS(gpsValues.at(id));
01289 }
01290
01291 recorder.addData(sensorData, dialog.isOdomExported()?poses.at(id):Transform(), covariance);
01292
01293 progressDialog->appendText(tr("Exported node %1").arg(id));
01294 progressDialog->incrementStep();
01295 QApplication::processEvents();
01296 }
01297 progressDialog->setValue(progressDialog->maximumSteps());
01298 if(delays.size())
01299 {
01300 progressDialog->appendText(tr("Average frame rate=%1 Hz (Min=%2, Max=%3)")
01301 .arg(1.0/uMean(delays)).arg(1.0/uMax(delays)).arg(1.0/uMin(delays)));
01302 }
01303 progressDialog->appendText(tr("Export finished to \"%1\"!").arg(path));
01304 }
01305 else
01306 {
01307 UERROR("DataRecorder init failed?!");
01308 }
01309 }
01310 else
01311 {
01312 QMessageBox::warning(this, tr("Cannot export database"), tr("An output path must be set!"));
01313 }
01314 }
01315 }
01316
01317 void DatabaseViewer::extractImages()
01318 {
01319 if(!dbDriver_ || ids_.size() == 0)
01320 {
01321 return;
01322 }
01323
01324 QStringList formats;
01325 formats.push_back("jpg");
01326 formats.push_back("png");
01327 bool ok;
01328 QString ext = QInputDialog::getItem(this, tr("Which RGB format?"), tr("Format:"), formats, 0, false, &ok);
01329 if(!ok)
01330 {
01331 return;
01332 }
01333
01334 QString path = QFileDialog::getExistingDirectory(this, tr("Select directory where to save images..."), QDir::homePath());
01335 if(!path.isEmpty())
01336 {
01337 if(ids_.size())
01338 {
01339 int id = ids_.at(0);
01340 SensorData data;
01341 dbDriver_->getNodeData(id, data);
01342 data.uncompressData();
01343 if(!data.imageRaw().empty() && !data.rightRaw().empty())
01344 {
01345 QDir dir;
01346 dir.mkdir(QString("%1/left").arg(path));
01347 dir.mkdir(QString("%1/right").arg(path));
01348 if(databaseFileName_.empty())
01349 {
01350 UERROR("Cannot save calibration file, database name is empty!");
01351 }
01352 else if(data.stereoCameraModel().isValidForProjection())
01353 {
01354 std::string cameraName = uSplit(databaseFileName_, '.').front();
01355 StereoCameraModel model(
01356 cameraName,
01357 data.imageRaw().size(),
01358 data.stereoCameraModel().left().K(),
01359 data.stereoCameraModel().left().D(),
01360 data.stereoCameraModel().left().R(),
01361 data.stereoCameraModel().left().P(),
01362 data.rightRaw().size(),
01363 data.stereoCameraModel().right().K(),
01364 data.stereoCameraModel().right().D(),
01365 data.stereoCameraModel().right().R(),
01366 data.stereoCameraModel().right().P(),
01367 data.stereoCameraModel().R(),
01368 data.stereoCameraModel().T(),
01369 data.stereoCameraModel().E(),
01370 data.stereoCameraModel().F(),
01371 data.stereoCameraModel().left().localTransform());
01372 if(model.save(path.toStdString()))
01373 {
01374 UINFO("Saved stereo calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
01375 }
01376 else
01377 {
01378 UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
01379 }
01380 }
01381 }
01382 else if(!data.imageRaw().empty())
01383 {
01384 if(!data.depthRaw().empty())
01385 {
01386 QDir dir;
01387 dir.mkdir(QString("%1/rgb").arg(path));
01388 dir.mkdir(QString("%1/depth").arg(path));
01389 }
01390
01391 if(databaseFileName_.empty())
01392 {
01393 UERROR("Cannot save calibration file, database name is empty!");
01394 }
01395 else if(data.cameraModels().size() > 1)
01396 {
01397 UERROR("Only one camera calibration can be saved at this time (%d detected)", (int)data.cameraModels().size());
01398 }
01399 else if(data.cameraModels().size() == 1 && data.cameraModels().front().isValidForProjection())
01400 {
01401 std::string cameraName = uSplit(databaseFileName_, '.').front();
01402 CameraModel model(cameraName,
01403 data.imageRaw().size(),
01404 data.cameraModels().front().K(),
01405 data.cameraModels().front().D(),
01406 data.cameraModels().front().R(),
01407 data.cameraModels().front().P(),
01408 data.cameraModels().front().localTransform());
01409 if(model.save(path.toStdString()))
01410 {
01411 UINFO("Saved calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
01412 }
01413 else
01414 {
01415 UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
01416 }
01417 }
01418 }
01419 }
01420
01421 int imagesExported = 0;
01422 for(int i=0; i<ids_.size(); ++i)
01423 {
01424 int id = ids_.at(i);
01425 SensorData data;
01426 dbDriver_->getNodeData(id, data);
01427 data.uncompressData();
01428 if(!data.imageRaw().empty() && !data.rightRaw().empty())
01429 {
01430 cv::imwrite(QString("%1/left/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
01431 cv::imwrite(QString("%1/right/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.rightRaw());
01432 UINFO(QString("Saved left/%1.%2 and right/%1.%2").arg(id).arg(ext).toStdString().c_str());
01433 ++imagesExported;
01434 }
01435 else if(!data.imageRaw().empty() && !data.depthRaw().empty())
01436 {
01437 cv::imwrite(QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
01438 cv::imwrite(QString("%1/depth/%2.png").arg(path).arg(id).toStdString(), data.depthRaw().type()==CV_32FC1?util2d::cvtDepthFromFloat(data.depthRaw()):data.depthRaw());
01439 UINFO(QString("Saved rgb/%1.%2 and depth/%1.png").arg(id).arg(ext).toStdString().c_str());
01440 ++imagesExported;
01441 }
01442 else if(!data.imageRaw().empty())
01443 {
01444 cv::imwrite(QString("%1/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
01445 UINFO(QString("Saved %1.%2").arg(id).arg(ext).toStdString().c_str());
01446 ++imagesExported;
01447 }
01448 }
01449 QMessageBox::information(this, tr("Exporting"), tr("%1 images exported!").arg(imagesExported));
01450 }
01451 }
01452
01453 void DatabaseViewer::updateIds()
01454 {
01455 if(!dbDriver_)
01456 {
01457 return;
01458 }
01459
01460 UINFO("Loading all IDs...");
01461 std::set<int> ids;
01462 dbDriver_->getAllNodeIds(ids);
01463 ids_ = QList<int>::fromStdList(std::list<int>(ids.begin(), ids.end()));
01464 idToIndex_.clear();
01465 mapIds_.clear();
01466 weights_.clear();
01467 wmStates_.clear();
01468 odomPoses_.clear();
01469 groundTruthPoses_.clear();
01470 gpsPoses_.clear();
01471 gpsValues_.clear();
01472 ui_->checkBox_wmState->setVisible(false);
01473 ui_->checkBox_alignPosesWithGroundTruth->setVisible(false);
01474 ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(false);
01475 ui_->doubleSpinBox_optimizationScale->setVisible(false);
01476 ui_->label_scale_title->setVisible(false);
01477 ui_->label_rmse->setVisible(false);
01478 ui_->label_rmse_title->setVisible(false);
01479 ui_->checkBox_ignoreIntermediateNodes->setVisible(false);
01480 ui_->label_ignoreINtermediateNdoes->setVisible(false);
01481 ui_->label_alignPosesWithGroundTruth->setVisible(false);
01482 ui_->label_alignScansCloudsWithGroundTruth->setVisible(false);
01483 ui_->menuEdit->setEnabled(true);
01484 ui_->actionGenerate_3D_map_pcd->setEnabled(true);
01485 ui_->actionExport->setEnabled(true);
01486 ui_->actionExtract_images->setEnabled(true);
01487 ui_->menuExport_poses->setEnabled(false);
01488 ui_->menuExport_GPS->setEnabled(false);
01489 ui_->actionPoses_KML->setEnabled(false);
01490 ui_->actionExport_saved_2D_map->setEnabled(false);
01491 ui_->actionImport_2D_map->setEnabled(false);
01492 ui_->actionView_optimized_mesh->setEnabled(false);
01493 ui_->actionExport_optimized_mesh->setEnabled(false);
01494 ui_->actionUpdate_optimized_mesh->setEnabled(uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.13.0") >= 0);
01495 links_.clear();
01496 linksAdded_.clear();
01497 linksRefined_.clear();
01498 linksRemoved_.clear();
01499 ui_->toolBox_statistics->clear();
01500 ui_->label_optimizeFrom->setText(tr("Root"));
01501 std::multimap<int, Link> links;
01502 dbDriver_->getAllLinks(links, true);
01503 UDEBUG("%d total links loaded", (int)links.size());
01504
01505 double totalOdom = 0.0;
01506 Transform previousPose;
01507 int sessions = ids_.size()?1:0;
01508 double totalTime = 0.0;
01509 double previousStamp = 0.0;
01510 std::set<int> idsWithoutBad;
01511 dbDriver_->getAllNodeIds(idsWithoutBad, false, true);
01512 int badcountInLTM = 0;
01513 int badCountInGraph = 0;
01514 bool hasReducedGraph = false;
01515 std::map<int, std::vector<int> > wmStates = dbDriver_->getAllStatisticsWmStates();
01516 for(int i=0; i<ids_.size(); ++i)
01517 {
01518 idToIndex_.insert(ids_[i], i);
01519
01520 Transform p, g;
01521 int w;
01522 std::string l;
01523 double s;
01524 int mapId;
01525 std::vector<float> v;
01526 GPS gps;
01527 dbDriver_->getNodeInfo(ids_[i], p, mapId, w, l, s, g, v, gps);
01528 mapIds_.insert(std::make_pair(ids_[i], mapId));
01529 weights_.insert(std::make_pair(ids_[i], w));
01530 if(wmStates.find(ids_[i]) != wmStates.end())
01531 {
01532 wmStates_.insert(std::make_pair(ids_[i], wmStates.at(ids_[i])));
01533 ui_->checkBox_wmState->setVisible(true);
01534 }
01535 if(w < 0)
01536 {
01537 ui_->checkBox_ignoreIntermediateNodes->setVisible(true);
01538 ui_->label_ignoreINtermediateNdoes->setVisible(true);
01539 }
01540 if(i>0)
01541 {
01542 if(mapIds_.at(ids_[i-1]) == mapId)
01543 {
01544 if(!p.isNull() && !previousPose.isNull())
01545 {
01546 totalOdom += p.getDistance(previousPose);
01547 }
01548
01549 if(previousStamp > 0.0 && s > 0.0)
01550 {
01551 totalTime += s-previousStamp;
01552 }
01553 }
01554 else
01555 {
01556 ++sessions;
01557 }
01558 }
01559 previousStamp=s;
01560 previousPose=p;
01561
01562
01563 bool addPose = links.find(ids_[i]) == links.end();
01564 for(std::multimap<int, Link>::iterator jter=links.find(ids_[i]); jter!=links.end() && jter->first == ids_[i]; ++jter)
01565 {
01566 if(jter->second.type() == Link::kNeighborMerged)
01567 {
01568 hasReducedGraph = true;
01569 }
01570
01571 std::multimap<int, Link>::iterator invertedLinkIter = graph::findLink(links, jter->second.to(), jter->second.from(), false);
01572 if( jter->second.isValid() &&
01573 ids.find(jter->second.from()) != ids.end() &&
01574 ids.find(jter->second.to()) != ids.end() &&
01575 graph::findLink(links_, jter->second.from(), jter->second.to()) == links_.end() &&
01576 graph::findLink(links, jter->second.from(), jter->second.to(), false) != links.end() &&
01577 invertedLinkIter != links.end())
01578 {
01579
01580 if(jter->second.userDataCompressed().cols == 0 &&
01581 invertedLinkIter->second.userDataCompressed().cols != 0)
01582 {
01583 links_.insert(std::make_pair(invertedLinkIter->second.from(), invertedLinkIter->second));
01584 }
01585 else
01586 {
01587 links_.insert(std::make_pair(ids_[i], jter->second));
01588 }
01589 addPose = true;
01590 }
01591 else if(graph::findLink(links_, jter->second.from(), jter->second.to()) != links_.end())
01592 {
01593 addPose = true;
01594 }
01595 }
01596 if(addPose)
01597 {
01598 odomPoses_.insert(std::make_pair(ids_[i], p));
01599 if(!g.isNull())
01600 {
01601 groundTruthPoses_.insert(std::make_pair(ids_[i], g));
01602 }
01603 if(gps.stamp() > 0.0)
01604 {
01605 gpsValues_.insert(std::make_pair(ids_[i], gps));
01606
01607 cv::Point3f p(0.0f,0.0f,0.0f);
01608 if(!gpsPoses_.empty())
01609 {
01610 GeodeticCoords coords = gps.toGeodeticCoords();
01611 GPS originGPS = gpsValues_.begin()->second;
01612 p = coords.toENU_WGS84(originGPS.toGeodeticCoords());
01613 }
01614 Transform pose(p.x, p.y, p.z, 0.0f, 0.0f, (float)((-(gps.bearing()-90))*180.0/M_PI));
01615 gpsPoses_.insert(std::make_pair(ids_[i], pose));
01616 }
01617 }
01618
01619 if(idsWithoutBad.find(ids_[i]) == idsWithoutBad.end())
01620 {
01621 ++badcountInLTM;
01622 if(addPose)
01623 {
01624 ++badCountInGraph;
01625 }
01626 }
01627 }
01628
01629 if(!groundTruthPoses_.empty() || !gpsPoses_.empty())
01630 {
01631 ui_->checkBox_alignPosesWithGroundTruth->setVisible(true);
01632 ui_->doubleSpinBox_optimizationScale->setVisible(true);
01633 ui_->label_scale_title->setVisible(true);
01634 ui_->label_rmse->setVisible(true);
01635 ui_->label_rmse_title->setVisible(true);
01636 ui_->label_alignPosesWithGroundTruth->setVisible(true);
01637
01638 if(!groundTruthPoses_.empty())
01639 {
01640 ui_->label_alignPosesWithGroundTruth->setText(tr("Align poses with ground truth"));
01641 ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(true);
01642 ui_->label_alignScansCloudsWithGroundTruth->setVisible(true);
01643 }
01644 else
01645 {
01646 ui_->label_alignPosesWithGroundTruth->setText(tr("Align poses with GPS"));
01647 }
01648 }
01649 if(!gpsValues_.empty())
01650 {
01651 ui_->menuExport_GPS->setEnabled(true);
01652 ui_->actionPoses_KML->setEnabled(groundTruthPoses_.empty());
01653 }
01654
01655 float xMin, yMin, cellSize;
01656 bool hasMap = !dbDriver_->load2DMap(xMin, yMin, cellSize).empty();
01657 ui_->actionExport_saved_2D_map->setEnabled(hasMap);
01658 ui_->actionImport_2D_map->setEnabled(hasMap);
01659
01660 if(!dbDriver_->loadOptimizedMesh().empty())
01661 {
01662 ui_->actionView_optimized_mesh->setEnabled(true);
01663 ui_->actionExport_optimized_mesh->setEnabled(true);
01664 }
01665
01666 UINFO("Loaded %d ids, %d poses and %d links", (int)ids_.size(), (int)odomPoses_.size(), (int)links_.size());
01667
01668 if(ids_.size() && ui_->toolBox_statistics->isVisible())
01669 {
01670 UINFO("Update statistics...");
01671 updateStatistics();
01672 }
01673
01674 UINFO("Update database info...");
01675 ui_->textEdit_info->clear();
01676 ui_->textEdit_info->append(tr("Path:\t\t%1").arg(dbDriver_->getUrl().c_str()));
01677 ui_->textEdit_info->append(tr("Version:\t\t%1").arg(dbDriver_->getDatabaseVersion().c_str()));
01678 ui_->textEdit_info->append(tr("Sessions:\t\t%1").arg(sessions));
01679 if(hasReducedGraph)
01680 {
01681 ui_->textEdit_info->append(tr("Total odometry length:\t%1 m (approx. as graph has been reduced)").arg(totalOdom));
01682 }
01683 else
01684 {
01685 ui_->textEdit_info->append(tr("Total odometry length:\t%1 m").arg(totalOdom));
01686 }
01687 ui_->textEdit_info->append(tr("Total time:\t\t%1").arg(QDateTime::fromMSecsSinceEpoch(totalTime*1000).toUTC().toString("hh:mm:ss.zzz")));
01688 ui_->textEdit_info->append(tr("LTM:\t\t%1 nodes and %2 words").arg(ids.size()).arg(dbDriver_->getTotalDictionarySize()));
01689 ui_->textEdit_info->append(tr("WM:\t\t%1 nodes and %2 words").arg(dbDriver_->getLastNodesSize()).arg(dbDriver_->getLastDictionarySize()));
01690 ui_->textEdit_info->append(tr("Global graph:\t%1 poses and %2 links").arg(odomPoses_.size()).arg(links_.size()));
01691 ui_->textEdit_info->append(tr("Ground truth:\t%1 poses").arg(groundTruthPoses_.size()));
01692 ui_->textEdit_info->append(tr("GPS:\t%1 poses").arg(gpsValues_.size()));
01693 ui_->textEdit_info->append("");
01694 long total = 0;
01695 long dbSize = UFile::length(dbDriver_->getUrl());
01696 long mem = dbSize;
01697 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"));
01698 mem = dbDriver_->getNodesMemoryUsed();
01699 total+=mem;
01700 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"));
01701 mem = dbDriver_->getLinksMemoryUsed();
01702 total+=mem;
01703 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"));
01704 mem = dbDriver_->getImagesMemoryUsed();
01705 total+=mem;
01706 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"));
01707 mem = dbDriver_->getDepthImagesMemoryUsed();
01708 total+=mem;
01709 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"));
01710 mem = dbDriver_->getCalibrationsMemoryUsed();
01711 total+=mem;
01712 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"));
01713 mem = dbDriver_->getGridsMemoryUsed();
01714 total+=mem;
01715 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"));
01716 mem = dbDriver_->getLaserScansMemoryUsed();
01717 total+=mem;
01718 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"));
01719 mem = dbDriver_->getUserDataMemoryUsed();
01720 total+=mem;
01721 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"));
01722 mem = dbDriver_->getWordsMemoryUsed();
01723 total+=mem;
01724 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"));
01725 mem = dbDriver_->getFeaturesMemoryUsed();
01726 total+=mem;
01727 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"));
01728 mem = dbDriver_->getStatisticsMemoryUsed();
01729 total+=mem;
01730 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"));
01731 mem = dbSize - total;
01732 ui_->textEdit_info->append(tr("Other (indexing):\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"));
01733 ui_->textEdit_info->append("");
01734 ui_->textEdit_info->append(tr("%1 bad signatures in LTM").arg(badcountInLTM));
01735 ui_->textEdit_info->append(tr("%1 bad signatures in the global graph").arg(badCountInGraph));
01736 ui_->textEdit_info->append("");
01737 ParametersMap parameters = dbDriver_->getLastParameters();
01738 QFontMetrics metrics(ui_->textEdit_info->font());
01739 int tabW = ui_->textEdit_info->tabStopWidth();
01740 for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
01741 {
01742 int strW = metrics.width(QString(iter->first.c_str()) + "=");
01743 ui_->textEdit_info->append(tr("%1=%2%3")
01744 .arg(iter->first.c_str())
01745 .arg(strW < tabW?"\t\t\t\t":strW < tabW*2?"\t\t\t":strW < tabW*3?"\t\t":"\t")
01746 .arg(iter->second.c_str()));
01747 }
01748
01749
01750 ui_->textEdit_info->moveCursor(QTextCursor::Start) ;
01751 ui_->textEdit_info->ensureCursorVisible() ;
01752
01753 if(ids.size())
01754 {
01755 if(odomPoses_.size())
01756 {
01757 bool nullPoses = odomPoses_.begin()->second.isNull();
01758 for(std::map<int,Transform>::iterator iter=odomPoses_.begin(); iter!=odomPoses_.end(); ++iter)
01759 {
01760 if((!iter->second.isNull() && nullPoses) ||
01761 (iter->second.isNull() && !nullPoses))
01762 {
01763 if(iter->second.isNull())
01764 {
01765 UWARN("Pose %d is null!", iter->first);
01766 }
01767 UWARN("Mixed valid and null poses! Ignoring graph...");
01768 odomPoses_.clear();
01769 links_.clear();
01770 break;
01771 }
01772 }
01773 if(nullPoses)
01774 {
01775 odomPoses_.clear();
01776 links_.clear();
01777 }
01778
01779 if(odomPoses_.size())
01780 {
01781 ui_->spinBox_optimizationsFrom->setRange(odomPoses_.begin()->first, odomPoses_.rbegin()->first);
01782 ui_->spinBox_optimizationsFrom->setValue(odomPoses_.begin()->first);
01783 ui_->label_optimizeFrom->setText(tr("Root [%1, %2]").arg(odomPoses_.begin()->first).arg(odomPoses_.rbegin()->first));
01784 }
01785 }
01786 }
01787
01788 ui_->menuExport_poses->setEnabled(false);
01789 graphes_.clear();
01790 graphLinks_.clear();
01791 neighborLinks_.clear();
01792 loopLinks_.clear();
01793 for(std::multimap<int, rtabmap::Link>::iterator iter = links_.begin(); iter!=links_.end(); ++iter)
01794 {
01795 if(!iter->second.transform().isNull())
01796 {
01797 if(iter->second.type() == rtabmap::Link::kNeighbor ||
01798 iter->second.type() == rtabmap::Link::kNeighborMerged)
01799 {
01800 neighborLinks_.append(iter->second);
01801 }
01802 else if(iter->second.type()!=rtabmap::Link::kPosePrior)
01803 {
01804 loopLinks_.append(iter->second);
01805 }
01806 }
01807 else
01808 {
01809 UERROR("Transform null for link from %d to %d", iter->first, iter->second.to());
01810 }
01811 }
01812
01813 if(ids_.size())
01814 {
01815 ui_->horizontalSlider_A->setMinimum(0);
01816 ui_->horizontalSlider_B->setMinimum(0);
01817 ui_->horizontalSlider_A->setMaximum(ids_.size()-1);
01818 ui_->horizontalSlider_B->setMaximum(ids_.size()-1);
01819 ui_->horizontalSlider_A->setEnabled(true);
01820 ui_->horizontalSlider_B->setEnabled(true);
01821 ui_->horizontalSlider_A->setSliderPosition(0);
01822 ui_->horizontalSlider_B->setSliderPosition(0);
01823 sliderAValueChanged(0);
01824 sliderBValueChanged(0);
01825 }
01826 else
01827 {
01828 ui_->horizontalSlider_A->setEnabled(false);
01829 ui_->horizontalSlider_B->setEnabled(false);
01830 ui_->label_idA->setText("NaN");
01831 ui_->label_idB->setText("NaN");
01832 }
01833
01834 if(neighborLinks_.size())
01835 {
01836 ui_->horizontalSlider_neighbors->setMinimum(0);
01837 ui_->horizontalSlider_neighbors->setMaximum(neighborLinks_.size()-1);
01838 ui_->horizontalSlider_neighbors->setEnabled(true);
01839 ui_->horizontalSlider_neighbors->setSliderPosition(0);
01840 }
01841 else
01842 {
01843 ui_->horizontalSlider_neighbors->setEnabled(false);
01844 }
01845
01846 if(ids_.size())
01847 {
01848 updateLoopClosuresSlider();
01849 if(ui_->graphViewer->isVisible() || ui_->dockWidget_occupancyGridView->isVisible())
01850 {
01851 updateGraphView();
01852 }
01853 }
01854 }
01855
01856 void DatabaseViewer::updateStatistics()
01857 {
01858 UDEBUG("");
01859 if(dbDriver_)
01860 {
01861 ui_->toolBox_statistics->clear();
01862 double firstStamp = 0.0;
01863 std::map<int, std::pair<std::map<std::string, float>, double> > allStats = dbDriver_->getAllStatistics();
01864
01865 std::map<std::string, std::pair<std::vector<float>, std::vector<float> > > allData;
01866 std::map<std::string, int > allDataOi;
01867
01868 for(int i=0; i<ids_.size(); ++i)
01869 {
01870 double stamp=0.0;
01871 std::map<std::string, float> statistics;
01872 if(allStats.find(ids_[i]) != allStats.end())
01873 {
01874 statistics = allStats.at(ids_[i]).first;
01875 stamp = allStats.at(ids_[i]).second;
01876 }
01877 if(firstStamp==0.0)
01878 {
01879 firstStamp = stamp;
01880 }
01881 for(std::map<std::string, float>::iterator iter=statistics.begin(); iter!=statistics.end(); ++iter)
01882 {
01883 if(allData.find(iter->first) == allData.end())
01884 {
01885
01886 allData.insert(std::make_pair(iter->first, std::make_pair(std::vector<float>(ids_.size(), 0.0f), std::vector<float>(ids_.size(), 0.0f) )));
01887 allDataOi.insert(std::make_pair(iter->first, 0));
01888 }
01889
01890 int & oi = allDataOi.at(iter->first);
01891 allData.at(iter->first).first[oi] = ui_->checkBox_timeStats->isChecked()?float(stamp-firstStamp):ids_[i];
01892 allData.at(iter->first).second[oi] = iter->second;
01893 ++oi;
01894 }
01895 }
01896
01897 for(std::map<std::string, std::pair<std::vector<float>, std::vector<float> > >::iterator iter=allData.begin(); iter!=allData.end(); ++iter)
01898 {
01899 int oi = allDataOi.at(iter->first);
01900 iter->second.first.resize(oi);
01901 iter->second.second.resize(oi);
01902 ui_->toolBox_statistics->updateStat(iter->first.c_str(), iter->second.first, iter->second.second, true);
01903 }
01904 }
01905 UDEBUG("");
01906 }
01907
01908 void DatabaseViewer::selectObstacleColor()
01909 {
01910 QColor c = QColorDialog::getColor(ui_->lineEdit_obstacleColor->text(), this);
01911 if(c.isValid())
01912 {
01913 ui_->lineEdit_obstacleColor->setText(c.name());
01914 }
01915 }
01916 void DatabaseViewer::selectGroundColor()
01917 {
01918 QColor c = QColorDialog::getColor(ui_->lineEdit_groundColor->text(), this);
01919 if(c.isValid())
01920 {
01921 ui_->lineEdit_groundColor->setText(c.name());
01922 }
01923 }
01924 void DatabaseViewer::selectEmptyColor()
01925 {
01926 QColor c = QColorDialog::getColor(ui_->lineEdit_emptyColor->text(), this);
01927 if(c.isValid())
01928 {
01929 ui_->lineEdit_emptyColor->setText(c.name());
01930 }
01931 }
01932 void DatabaseViewer::editDepthImage()
01933 {
01934 if(dbDriver_ && ids_.size())
01935 {
01936 int id = ids_.at(ui_->horizontalSlider_A->value());
01937 SensorData data;
01938 dbDriver_->getNodeData(id, data, true, false, false, false);
01939 data.uncompressData();
01940 if(!data.depthRaw().empty())
01941 {
01942 editDepthArea_->setImage(data.depthRaw(), data.imageRaw());
01943 if(editDepthDialog_->exec() == QDialog::Accepted && editDepthArea_->isModified())
01944 {
01945 cv::Mat depth = editDepthArea_->getModifiedImage();
01946 UASSERT(data.depthRaw().type() == depth.type());
01947 UASSERT(data.depthRaw().cols == depth.cols);
01948 UASSERT(data.depthRaw().rows == depth.rows);
01949 dbDriver_->updateDepthImage(id, depth);
01950 this->update3dView();
01951 }
01952 }
01953 }
01954 }
01955
01956 void DatabaseViewer::exportPosesRaw()
01957 {
01958 exportPoses(0);
01959 }
01960 void DatabaseViewer::exportPosesRGBDSLAM()
01961 {
01962 exportPoses(1);
01963 }
01964 void DatabaseViewer::exportPosesKITTI()
01965 {
01966 exportPoses(2);
01967 }
01968 void DatabaseViewer::exportPosesTORO()
01969 {
01970 exportPoses(3);
01971 }
01972 void DatabaseViewer::exportPosesG2O()
01973 {
01974 exportPoses(4);
01975 }
01976 void DatabaseViewer::exportPosesKML()
01977 {
01978 exportPoses(5);
01979 }
01980
01981 void DatabaseViewer::exportPoses(int format)
01982 {
01983 if(graphes_.empty())
01984 {
01985 this->updateGraphView();
01986 if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
01987 {
01988 QMessageBox::warning(this, tr("Cannot export poses"), tr("No graph in database?!"));
01989 return;
01990 }
01991 }
01992
01993 if(format == 5)
01994 {
01995 if(gpsValues_.empty() || gpsPoses_.empty())
01996 {
01997 QMessageBox::warning(this, tr("Cannot export poses"), tr("No GPS in database?!"));
01998 }
01999 else
02000 {
02001 std::map<int, rtabmap::Transform> graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
02002
02003
02004 pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
02005 cloud1.resize(graph.size());
02006 cloud2.resize(graph.size());
02007 int oi = 0;
02008 int idFirst = 0;
02009 for(std::map<int, Transform>::const_iterator iter=gpsPoses_.begin(); iter!=gpsPoses_.end(); ++iter)
02010 {
02011 std::map<int, Transform>::iterator iter2 = graph.find(iter->first);
02012 if(iter2!=graph.end())
02013 {
02014 if(oi==0)
02015 {
02016 idFirst = iter->first;
02017 }
02018 cloud1[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
02019 cloud2[oi++] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z());
02020 }
02021 }
02022
02023 Transform t = Transform::getIdentity();
02024 if(oi>5)
02025 {
02026 cloud1.resize(oi);
02027 cloud2.resize(oi);
02028
02029 t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
02030 }
02031 else if(idFirst)
02032 {
02033 t = gpsPoses_.at(idFirst) * graph.at(idFirst).inverse();
02034 }
02035
02036 std::map<int, GPS> values;
02037 GeodeticCoords origin = gpsValues_.begin()->second.toGeodeticCoords();
02038 for(std::map<int, Transform>::iterator iter=graph.begin(); iter!=graph.end(); ++iter)
02039 {
02040 iter->second = t * iter->second;
02041
02042 GeodeticCoords coord;
02043 coord.fromENU_WGS84(cv::Point3d(iter->second.x(), iter->second.y(), iter->second.z()), origin);
02044 double bearing = -(iter->second.theta()*180.0/M_PI-90.0);
02045 if(bearing < 0)
02046 {
02047 bearing += 360;
02048 }
02049
02050 Transform p, g;
02051 int w;
02052 std::string l;
02053 double stamp=0.0;
02054 int mapId;
02055 std::vector<float> v;
02056 GPS gps;
02057 dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps);
02058 values.insert(std::make_pair(iter->first, GPS(stamp, coord.longitude(), coord.latitude(), coord.altitude(), 0, 0)));
02059 }
02060
02061 QString output = pathDatabase_ + QDir::separator() + "poses.kml";
02062 QString path = QFileDialog::getSaveFileName(
02063 this,
02064 tr("Save File"),
02065 output,
02066 tr("Google Earth file (*.kml)"));
02067
02068 if(!path.isEmpty())
02069 {
02070 bool saved = graph::exportGPS(path.toStdString(), values, ui_->graphViewer->getNodeColor().rgba());
02071
02072 if(saved)
02073 {
02074 QMessageBox::information(this,
02075 tr("Export poses..."),
02076 tr("GPS coordinates saved to \"%1\".")
02077 .arg(path));
02078 }
02079 else
02080 {
02081 QMessageBox::information(this,
02082 tr("Export poses..."),
02083 tr("Failed to save GPS coordinates to \"%1\"!")
02084 .arg(path));
02085 }
02086 }
02087 }
02088 return;
02089 }
02090
02091 std::map<int, Transform> optimizedPoses;
02092 if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty())
02093 {
02094 optimizedPoses = groundTruthPoses_;
02095 }
02096 else
02097 {
02098 optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
02099
02100 if(ui_->checkBox_alignPosesWithGroundTruth->isChecked())
02101 {
02102 std::map<int, Transform> refPoses = groundTruthPoses_;
02103 if(refPoses.empty())
02104 {
02105 refPoses = gpsPoses_;
02106 }
02107
02108
02109 if(refPoses.size())
02110 {
02111 float translational_rmse = 0.0f;
02112 float translational_mean = 0.0f;
02113 float translational_median = 0.0f;
02114 float translational_std = 0.0f;
02115 float translational_min = 0.0f;
02116 float translational_max = 0.0f;
02117 float rotational_rmse = 0.0f;
02118 float rotational_mean = 0.0f;
02119 float rotational_median = 0.0f;
02120 float rotational_std = 0.0f;
02121 float rotational_min = 0.0f;
02122 float rotational_max = 0.0f;
02123
02124 Transform gtToMap = graph::calcRMSE(
02125 refPoses,
02126 optimizedPoses,
02127 translational_rmse,
02128 translational_mean,
02129 translational_median,
02130 translational_std,
02131 translational_min,
02132 translational_max,
02133 rotational_rmse,
02134 rotational_mean,
02135 rotational_median,
02136 rotational_std,
02137 rotational_min,
02138 rotational_max);
02139
02140 if(ui_->checkBox_alignPosesWithGroundTruth->isChecked() && !gtToMap.isIdentity())
02141 {
02142 for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
02143 {
02144 iter->second = gtToMap * iter->second;
02145 }
02146 }
02147 }
02148 }
02149 }
02150
02151 if(optimizedPoses.size())
02152 {
02153 std::map<int, Transform> localTransforms;
02154 QStringList items;
02155 items.push_back("Robot");
02156 items.push_back("Camera");
02157 items.push_back("Scan");
02158 bool ok;
02159 QString item = QInputDialog::getItem(this, tr("Export Poses"), tr("Frame: "), items, 0, false, &ok);
02160 if(!ok || item.isEmpty())
02161 {
02162 return;
02163 }
02164 if(item.compare("Robot") != 0)
02165 {
02166 bool cameraFrame = item.compare("Camera") == 0;
02167 for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
02168 {
02169 Transform localTransform;
02170 if(cameraFrame)
02171 {
02172 std::vector<CameraModel> models;
02173 StereoCameraModel stereoModel;
02174 if(dbDriver_->getCalibration(iter->first, models, stereoModel))
02175 {
02176 if((models.size() == 1 &&
02177 !models.at(0).localTransform().isNull()))
02178 {
02179 localTransform = models.at(0).localTransform();
02180 }
02181 else if(!stereoModel.localTransform().isNull())
02182 {
02183 localTransform = stereoModel.localTransform();
02184 }
02185 else if(models.size()>1)
02186 {
02187 UWARN("Multi-camera is not supported (node %d)", iter->first);
02188 }
02189 else
02190 {
02191 UWARN("Calibration not valid for node %d", iter->first);
02192 }
02193 }
02194 else
02195 {
02196 UWARN("Missing calibration for node %d", iter->first);
02197 }
02198 }
02199 else
02200 {
02201 LaserScan info;
02202 if(dbDriver_->getLaserScanInfo(iter->first, info))
02203 {
02204 localTransform = info.localTransform();
02205 }
02206 else
02207 {
02208 UWARN("Missing scan info for node %d", iter->first);
02209 }
02210
02211 }
02212 if(!localTransform.isNull())
02213 {
02214 localTransforms.insert(std::make_pair(iter->first, localTransform));
02215 }
02216 }
02217 if(localTransforms.empty())
02218 {
02219 QMessageBox::warning(this,
02220 tr("Export Poses"),
02221 tr("Could not find any \"%1\" frame, exporting in Robot frame instead.").arg(item));
02222 }
02223 }
02224
02225 std::map<int, Transform> poses;
02226 std::multimap<int, Link> links;
02227 if(localTransforms.empty())
02228 {
02229 poses = optimizedPoses;
02230 links = graphLinks_;
02231 }
02232 else
02233 {
02234
02235 for(std::map<int, Transform>::iterator iter=localTransforms.begin(); iter!=localTransforms.end(); ++iter)
02236 {
02237 poses.insert(std::make_pair(iter->first, optimizedPoses.at(iter->first) * iter->second));
02238 }
02239 for(std::multimap<int, Link>::iterator iter=graphLinks_.begin(); iter!=graphLinks_.end(); ++iter)
02240 {
02241 if(uContains(poses, iter->second.from()) && uContains(poses, iter->second.to()))
02242 {
02243 std::multimap<int, Link>::iterator inserted = links.insert(*iter);
02244 int from = iter->second.from();
02245 int to = iter->second.to();
02246 inserted->second.setTransform(localTransforms.at(from).inverse()*iter->second.transform()*localTransforms.at(to));
02247 }
02248 }
02249 }
02250
02251 std::map<int, double> stamps;
02252 if(format == 1)
02253 {
02254 for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
02255 {
02256 Transform p, g;
02257 int w;
02258 std::string l;
02259 double stamp=0.0;
02260 int mapId;
02261 std::vector<float> v;
02262 GPS gps;
02263 if(dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps))
02264 {
02265 stamps.insert(std::make_pair(iter->first, stamp));
02266 }
02267 }
02268 if(stamps.size()!=poses.size())
02269 {
02270 QMessageBox::warning(this, tr("Export poses..."), tr("Poses (%1) and stamps (%2) have not the same size! Cannot export in RGB-D SLAM format.")
02271 .arg(poses.size()).arg(stamps.size()));
02272 return;
02273 }
02274 }
02275
02276 QString output = pathDatabase_ + QDir::separator() + (format==3?"toro.graph":format==4?"poses.g2o":"poses.txt");
02277
02278 QString path = QFileDialog::getSaveFileName(
02279 this,
02280 tr("Save File"),
02281 output,
02282 format == 3?tr("TORO file (*.graph)"):format==4?tr("g2o file (*.g2o)"):tr("Text file (*.txt)"));
02283
02284 if(!path.isEmpty())
02285 {
02286 if(QFileInfo(path).suffix() == "")
02287 {
02288 if(format == 3)
02289 {
02290 path += ".graph";
02291 }
02292 else if(format==4)
02293 {
02294 path += ".g2o";
02295 }
02296 else
02297 {
02298 path += ".txt";
02299 }
02300 }
02301
02302 bool saved = graph::exportPoses(path.toStdString(), format, poses, links, stamps);
02303
02304 if(saved)
02305 {
02306 QMessageBox::information(this,
02307 tr("Export poses..."),
02308 tr("%1 saved to \"%2\".")
02309 .arg(format == 3?"TORO graph":format == 4?"g2o graph":"Poses")
02310 .arg(path));
02311 }
02312 else
02313 {
02314 QMessageBox::information(this,
02315 tr("Export poses..."),
02316 tr("Failed to save %1 to \"%2\"!")
02317 .arg(format == 3?"TORO graph":format == 4?"g2o graph":"poses")
02318 .arg(path));
02319 }
02320 }
02321 }
02322 }
02323
02324 void DatabaseViewer::exportGPS_TXT()
02325 {
02326 exportGPS(0);
02327 }
02328 void DatabaseViewer::exportGPS_KML()
02329 {
02330 exportGPS(1);
02331 }
02332
02333 void DatabaseViewer::exportGPS(int format)
02334 {
02335 if(!gpsValues_.empty())
02336 {
02337 QString output = pathDatabase_ + QDir::separator() + (format==0?"gps.txt":"gps.kml");
02338 QString path = QFileDialog::getSaveFileName(
02339 this,
02340 tr("Save File"),
02341 output,
02342 format==0?tr("Raw format (*.txt)"):tr("Google Earth file (*.kml)"));
02343
02344 if(!path.isEmpty())
02345 {
02346 bool saved = graph::exportGPS(path.toStdString(), gpsValues_, ui_->graphViewer->getGPSColor().rgba());
02347
02348 if(saved)
02349 {
02350 QMessageBox::information(this,
02351 tr("Export poses..."),
02352 tr("GPS coordinates saved to \"%1\".")
02353 .arg(path));
02354 }
02355 else
02356 {
02357 QMessageBox::information(this,
02358 tr("Export poses..."),
02359 tr("Failed to save GPS coordinates to \"%1\"!")
02360 .arg(path));
02361 }
02362 }
02363 }
02364 }
02365
02366 void DatabaseViewer::exportSaved2DMap()
02367 {
02368 if(!dbDriver_)
02369 {
02370 QMessageBox::warning(this, tr("Cannot export 2D map"), tr("A database must must loaded first...\nUse File->Open database."));
02371 return;
02372 }
02373
02374 float xMin, yMin, cellSize;
02375 cv::Mat map = dbDriver_->load2DMap(xMin, yMin, cellSize);
02376 if(map.empty())
02377 {
02378 QMessageBox::warning(this, tr("Cannot export 2D map"), tr("The database doesn't contain a saved 2D map."));
02379 }
02380 else
02381 {
02382 cv::Mat map8U = rtabmap::util3d::convertMap2Image8U(map, true);
02383 QString name = QFileInfo(databaseFileName_.c_str()).baseName();
02384 QString path = QFileDialog::getSaveFileName(
02385 this,
02386 tr("Save File"),
02387 pathDatabase_+"/" + name + ".pgm",
02388 tr("Map (*.pgm)"));
02389
02390 if(!path.isEmpty())
02391 {
02392 if(QFileInfo(path).suffix() == "")
02393 {
02394 path += ".pgm";
02395 }
02396 cv::imwrite(path.toStdString(), map8U);
02397 QMessageBox::information(this, tr("Export 2D map"), tr("Exported %1!").arg(path));
02398 }
02399 }
02400 }
02401
02402 void DatabaseViewer::import2DMap()
02403 {
02404 if(!dbDriver_)
02405 {
02406 QMessageBox::warning(this, tr("Cannot import 2D map"), tr("A database must must loaded first...\nUse File->Open database."));
02407 return;
02408 }
02409
02410 float xMin, yMin, cellSize;
02411 cv::Mat mapOrg = dbDriver_->load2DMap(xMin, yMin, cellSize);
02412 if(mapOrg.empty())
02413 {
02414 QMessageBox::warning(this, tr("Cannot import 2D map"), tr("The database doesn't contain a saved 2D map."));
02415 }
02416 else
02417 {
02418 QString path = QFileDialog::getOpenFileName(
02419 this,
02420 tr("Open File"),
02421 pathDatabase_,
02422 tr("Map (*.pgm)"));
02423 if(!path.isEmpty())
02424 {
02425 cv::Mat map8U = cv::imread(path.toStdString(), cv::IMREAD_UNCHANGED);
02426 cv::Mat map = rtabmap::util3d::convertImage8U2Map(map8U, true);
02427
02428 if(mapOrg.cols == map.cols && mapOrg.rows == map8U.rows)
02429 {
02430 dbDriver_->save2DMap(map, xMin, yMin, cellSize);
02431 QMessageBox::information(this, tr("Import 2D map"), tr("Imported %1!").arg(path));
02432 }
02433 else
02434 {
02435 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));
02436 }
02437 }
02438 }
02439 }
02440
02441 void DatabaseViewer::viewOptimizedMesh()
02442 {
02443 if(!dbDriver_)
02444 {
02445 QMessageBox::warning(this, tr("Cannot view optimized mesh"), tr("A database must must loaded first...\nUse File->Open database."));
02446 return;
02447 }
02448
02449 std::vector<std::vector<std::vector<unsigned int> > > polygons;
02450 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
02451 std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
02452 #else
02453 std::vector<std::vector<Eigen::Vector2f> > texCoords;
02454 #endif
02455 cv::Mat textures;
02456 cv::Mat cloudMat = dbDriver_->loadOptimizedMesh(&polygons, &texCoords, &textures);
02457 if(cloudMat.empty())
02458 {
02459 QMessageBox::warning(this, tr("Cannot view optimized mesh"), tr("The database doesn't contain a saved optimized mesh."));
02460 }
02461 else
02462 {
02463 CloudViewer * viewer = new CloudViewer(this);
02464 viewer->setWindowFlags(Qt::Window);
02465 viewer->setAttribute(Qt::WA_DeleteOnClose);
02466 viewer->buildPickingLocator(true);
02467 if(!textures.empty())
02468 {
02469 pcl::TextureMeshPtr mesh = util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
02470 util3d::fixTextureMeshForVisualization(*mesh);
02471 viewer->setWindowTitle("Optimized Textured Mesh");
02472 viewer->setPolygonPicking(true);
02473 viewer->addCloudTextureMesh("mesh", mesh, textures);
02474 }
02475 else if(polygons.size() == 1)
02476 {
02477 pcl::PolygonMeshPtr mesh = util3d::assemblePolygonMesh(cloudMat, polygons.at(0));
02478 viewer->setWindowTitle("Optimized Mesh");
02479 viewer->setPolygonPicking(true);
02480 viewer->addCloudMesh("mesh", mesh);
02481 }
02482 else
02483 {
02484 LaserScan scan = LaserScan::backwardCompatibility(cloudMat);
02485 pcl::PCLPointCloud2::Ptr cloud = util3d::laserScanToPointCloud2(scan);
02486 viewer->setWindowTitle("Optimized Point Cloud");
02487 viewer->addCloud("mesh", cloud, Transform::getIdentity(), scan.hasRGB(), scan.hasNormals(), scan.hasIntensity());
02488 }
02489 viewer->show();
02490 }
02491 }
02492
02493 void DatabaseViewer::exportOptimizedMesh()
02494 {
02495 if(!dbDriver_)
02496 {
02497 QMessageBox::warning(this, tr("Cannot export optimized mesh"), tr("A database must must loaded first...\nUse File->Open database."));
02498 return;
02499 }
02500
02501 std::vector<std::vector<std::vector<unsigned int> > > polygons;
02502 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
02503 std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
02504 #else
02505 std::vector<std::vector<Eigen::Vector2f> > texCoords;
02506 #endif
02507 cv::Mat textures;
02508 cv::Mat cloudMat = dbDriver_->loadOptimizedMesh(&polygons, &texCoords, &textures);
02509 if(cloudMat.empty())
02510 {
02511 QMessageBox::warning(this, tr("Cannot export optimized mesh"), tr("The database doesn't contain a saved optimized mesh."));
02512 }
02513 else
02514 {
02515 QString name = QFileInfo(databaseFileName_.c_str()).baseName();
02516
02517 if(!textures.empty())
02518 {
02519 pcl::TextureMeshPtr mesh = util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures);
02520 QString path = QFileDialog::getSaveFileName(
02521 this,
02522 tr("Save File"),
02523 pathDatabase_+"/" + name + ".obj",
02524 tr("Mesh (*.obj)"));
02525
02526 if(!path.isEmpty())
02527 {
02528 if(QFileInfo(path).suffix() == "")
02529 {
02530 path += ".obj";
02531 }
02532 QString baseName = QFileInfo(path).baseName();
02533 if(mesh->tex_materials.size() == 1)
02534 {
02535 mesh->tex_materials.at(0).tex_file = baseName.toStdString() + ".png";
02536 cv::imwrite((QFileInfo(path).absoluteDir().absolutePath()+QDir::separator()+baseName).toStdString() + ".png", textures);
02537 }
02538 else
02539 {
02540 for(unsigned int i=0; i<mesh->tex_materials.size(); ++i)
02541 {
02542 mesh->tex_materials.at(i).tex_file = (baseName+QDir::separator()+QString::number(i)+".png").toStdString();
02543 UASSERT((i+1)*textures.rows <= (unsigned int)textures.cols);
02544 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)));
02545 }
02546 }
02547 pcl::io::saveOBJFile(path.toStdString(), *mesh);
02548
02549 QMessageBox::information(this, tr("Export Textured Mesh"), tr("Exported %1!").arg(path));
02550 }
02551 }
02552 else if(polygons.size() == 1)
02553 {
02554 pcl::PolygonMeshPtr mesh = util3d::assemblePolygonMesh(cloudMat, polygons.at(0));
02555 QString path = QFileDialog::getSaveFileName(
02556 this,
02557 tr("Save File"),
02558 pathDatabase_+"/" + name + ".ply",
02559 tr("Mesh (*.ply)"));
02560
02561 if(!path.isEmpty())
02562 {
02563 if(QFileInfo(path).suffix() == "")
02564 {
02565 path += ".ply";
02566 }
02567 pcl::io::savePLYFileBinary(path.toStdString(), *mesh);
02568 QMessageBox::information(this, tr("Export Mesh"), tr("Exported %1!").arg(path));
02569 }
02570 }
02571 else
02572 {
02573 QString path = QFileDialog::getSaveFileName(
02574 this,
02575 tr("Save File"),
02576 pathDatabase_+"/" + name + ".ply",
02577 tr("Point cloud data (*.ply *.pcd)"));
02578
02579 if(!path.isEmpty())
02580 {
02581 if(QFileInfo(path).suffix() == "")
02582 {
02583 path += ".ply";
02584 }
02585 bool success = false;
02586 pcl::PCLPointCloud2::Ptr cloud = util3d::laserScanToPointCloud2(LaserScan::backwardCompatibility(cloudMat));
02587 if(QFileInfo(path).suffix() == "pcd")
02588 {
02589 success = pcl::io::savePCDFile(path.toStdString(), *cloud) == 0;
02590 }
02591 else
02592 {
02593 success = pcl::io::savePLYFile(path.toStdString(), *cloud) == 0;
02594 }
02595 if(success)
02596 {
02597 QMessageBox::information(this, tr("Export Point Cloud"), tr("Exported %1!").arg(path));
02598 }
02599 else
02600 {
02601 QMessageBox::critical(this, tr("Export Point Cloud"), tr("Failed exporting %1!").arg(path));
02602 }
02603 }
02604 }
02605 }
02606 }
02607
02608 void DatabaseViewer::updateOptimizedMesh()
02609 {
02610 if(!ids_.size() || !dbDriver_)
02611 {
02612 QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
02613 return;
02614 }
02615
02616 if(graphes_.empty())
02617 {
02618 this->updateGraphView();
02619 if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
02620 {
02621 QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
02622 return;
02623 }
02624 }
02625
02626 std::map<int, Transform> optimizedPoses;
02627 if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty())
02628 {
02629 optimizedPoses = groundTruthPoses_;
02630 }
02631 else
02632 {
02633 optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
02634 }
02635 if(ui_->groupBox_posefiltering->isChecked())
02636 {
02637 optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
02638 ui_->doubleSpinBox_posefilteringRadius->value(),
02639 ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
02640 }
02641 if(optimizedPoses.size() > 0)
02642 {
02643 exportDialog_->setDBDriver(dbDriver_);
02644 exportDialog_->forceAssembling(true);
02645 exportDialog_->setOkButton();
02646
02647 std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> clouds;
02648 std::map<int, pcl::PolygonMesh::Ptr> meshes;
02649 std::map<int, pcl::TextureMesh::Ptr> textureMeshes;
02650 std::vector<std::map<int, pcl::PointXY> > textureVertexToPixels;
02651
02652 if(exportDialog_->getExportedClouds(
02653 optimizedPoses,
02654 updateLinksWithModifications(links_),
02655 mapIds_,
02656 QMap<int, Signature>(),
02657 std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
02658 std::map<int, LaserScan>(),
02659 pathDatabase_,
02660 ui_->parameters_toolbox->getParameters(),
02661 clouds,
02662 meshes,
02663 textureMeshes,
02664 textureVertexToPixels))
02665 {
02666 if(textureMeshes.size())
02667 {
02668 dbDriver_->saveOptimizedPoses(optimizedPoses, Transform());
02669
02670 cv::Mat globalTextures;
02671 pcl::TextureMeshPtr textureMesh = textureMeshes.at(0);
02672 if(textureMesh->tex_materials.size()>1)
02673 {
02674 globalTextures = util3d::mergeTextures(
02675 *textureMesh,
02676 std::map<int, cv::Mat>(),
02677 std::map<int, std::vector<CameraModel> >(),
02678 0,
02679 dbDriver_,
02680 exportDialog_->getTextureSize(),
02681 exportDialog_->getMaxTextures(),
02682 textureVertexToPixels,
02683 exportDialog_->isGainCompensation(),
02684 exportDialog_->getGainBeta(),
02685 exportDialog_->isGainRGB(),
02686 exportDialog_->isBlending(),
02687 exportDialog_->getBlendingDecimation(),
02688 exportDialog_->getTextureBrightnessConstrastRatioLow(),
02689 exportDialog_->getTextureBrightnessConstrastRatioHigh(),
02690 exportDialog_->isExposeFusion());
02691 }
02692 dbDriver_->saveOptimizedMesh(
02693 util3d::laserScanFromPointCloud(textureMesh->cloud, false).data(),
02694 util3d::convertPolygonsFromPCL(textureMesh->tex_polygons),
02695 textureMesh->tex_coordinates,
02696 globalTextures);
02697 QMessageBox::information(this, tr("Update Optimized Textured Mesh"), tr("Updated!"));
02698 ui_->actionView_optimized_mesh->setEnabled(true);
02699 ui_->actionExport_optimized_mesh->setEnabled(true);
02700 this->viewOptimizedMesh();
02701 }
02702 else if(meshes.size())
02703 {
02704 dbDriver_->saveOptimizedPoses(optimizedPoses, Transform());
02705 std::vector<std::vector<std::vector<unsigned int> > > polygons(1);
02706 polygons.at(0) = util3d::convertPolygonsFromPCL(meshes.at(0)->polygons);
02707 dbDriver_->saveOptimizedMesh(util3d::laserScanFromPointCloud(meshes.at(0)->cloud, false).data(), polygons);
02708 QMessageBox::information(this, tr("Update Optimized Mesh"), tr("Updated!"));
02709 ui_->actionView_optimized_mesh->setEnabled(true);
02710 ui_->actionExport_optimized_mesh->setEnabled(true);
02711 this->viewOptimizedMesh();
02712 }
02713 else if(clouds.size())
02714 {
02715 dbDriver_->saveOptimizedPoses(optimizedPoses, Transform());
02716 dbDriver_->saveOptimizedMesh(util3d::laserScanFromPointCloud(*clouds.at(0)));
02717 QMessageBox::information(this, tr("Update Optimized PointCloud"), tr("Updated!"));
02718 ui_->actionView_optimized_mesh->setEnabled(true);
02719 ui_->actionExport_optimized_mesh->setEnabled(true);
02720 this->viewOptimizedMesh();
02721 }
02722 else
02723 {
02724 QMessageBox::critical(this, tr("Update Optimized Mesh"), tr("Nothing to save!"));
02725 }
02726 }
02727 exportDialog_->setProgressDialogToMax();
02728 }
02729 else
02730 {
02731 QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
02732 }
02733 }
02734
02735 void DatabaseViewer::generateGraph()
02736 {
02737 if(!dbDriver_)
02738 {
02739 QMessageBox::warning(this, tr("Cannot generate a graph"), tr("A database must must loaded first...\nUse File->Open database."));
02740 return;
02741 }
02742
02743 QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/Graph.dot", tr("Graphiz file (*.dot)"));
02744 if(!path.isEmpty())
02745 {
02746 dbDriver_->generateGraph(path.toStdString());
02747 }
02748 }
02749
02750 void DatabaseViewer::generateLocalGraph()
02751 {
02752 if(!ids_.size() || !dbDriver_)
02753 {
02754 QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
02755 return;
02756 }
02757 bool ok = false;
02758 int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID"), ids_.first(), ids_.first(), ids_.last(), 1, &ok);
02759
02760 if(ok)
02761 {
02762 int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin"), 4, 1, 100, 1, &ok);
02763 if(ok)
02764 {
02765 QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/Graph" + QString::number(id) + ".dot", tr("Graphiz file (*.dot)"));
02766 if(!path.isEmpty() && id>0)
02767 {
02768 std::map<int, int> ids;
02769 std::list<int> curentMarginList;
02770 std::set<int> currentMargin;
02771 std::set<int> nextMargin;
02772 nextMargin.insert(id);
02773 int m = 0;
02774 while((margin == 0 || m < margin) && nextMargin.size())
02775 {
02776 curentMarginList = std::list<int>(nextMargin.rbegin(), nextMargin.rend());
02777 nextMargin.clear();
02778
02779 for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
02780 {
02781 if(ids.find(*jter) == ids.end())
02782 {
02783 std::map<int, Link> links;
02784 ids.insert(std::pair<int, int>(*jter, m));
02785
02786 UTimer timer;
02787 dbDriver_->loadLinks(*jter, links);
02788
02789
02790 for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
02791 {
02792 if( !uContains(ids, iter->first))
02793 {
02794 UASSERT(iter->second.type() != Link::kUndef);
02795 if(iter->second.type() == Link::kNeighbor ||
02796 iter->second.type() == Link::kNeighborMerged)
02797 {
02798 nextMargin.insert(iter->first);
02799 }
02800 else
02801 {
02802
02803 if(currentMargin.insert(iter->first).second)
02804 {
02805 curentMarginList.push_back(iter->first);
02806 }
02807 }
02808 }
02809 }
02810 }
02811 }
02812 ++m;
02813 }
02814
02815 if(ids.size() > 0)
02816 {
02817 ids.insert(std::pair<int,int>(id, 0));
02818 std::set<int> idsSet;
02819 for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
02820 {
02821 idsSet.insert(idsSet.end(), iter->first);
02822 UINFO("Node %d", iter->first);
02823 }
02824 UINFO("idsSet=%d", idsSet.size());
02825 dbDriver_->generateGraph(path.toStdString(), idsSet);
02826 }
02827 else
02828 {
02829 QMessageBox::critical(this, tr("Error"), tr("No neighbors found for signature %1.").arg(id));
02830 }
02831 }
02832 }
02833 }
02834 }
02835
02836 void DatabaseViewer::regenerateLocalMaps()
02837 {
02838 OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
02839
02840 generatedLocalMaps_.clear();
02841 generatedLocalMapsInfo_.clear();
02842
02843 rtabmap::ProgressDialog progressDialog(this);
02844 progressDialog.setMaximumSteps(ids_.size());
02845 progressDialog.show();
02846 progressDialog.setCancelButtonVisible(true);
02847
02848 UPlot * plot = new UPlot(this);
02849 plot->setWindowFlags(Qt::Window);
02850 plot->setWindowTitle("Local Occupancy Grid Generation Time (ms)");
02851 plot->setAttribute(Qt::WA_DeleteOnClose);
02852 UPlotCurve * decompressionCurve = plot->addCurve("Decompression");
02853 UPlotCurve * gridCreationCurve = plot->addCurve("Grid Creation");
02854 plot->show();
02855
02856 UPlot * plotCells = new UPlot(this);
02857 plotCells->setWindowFlags(Qt::Window);
02858 plotCells->setWindowTitle("Occupancy Cells");
02859 plotCells->setAttribute(Qt::WA_DeleteOnClose);
02860 UPlotCurve * totalCurve = plotCells->addCurve("Total");
02861 UPlotCurve * emptyCurve = plotCells->addCurve("Empty");
02862 UPlotCurve * obstaclesCurve = plotCells->addCurve("Obstacles");
02863 UPlotCurve * groundCurve = plotCells->addCurve("Ground");
02864 plotCells->show();
02865
02866 double decompressionTime = 0;
02867 double gridCreationTime = 0;
02868
02869 for(int i =0; i<ids_.size() && !progressDialog.isCanceled(); ++i)
02870 {
02871 UTimer timer;
02872 SensorData data;
02873 dbDriver_->getNodeData(ids_.at(i), data);
02874 data.uncompressData();
02875 decompressionTime = timer.ticks()*1000.0;
02876
02877 int mapId, weight;
02878 Transform odomPose, groundTruth;
02879 std::string label;
02880 double stamp;
02881 QString msg;
02882 std::vector<float> velocity;
02883 GPS gps;
02884 if(dbDriver_->getNodeInfo(data.id(), odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps))
02885 {
02886 Signature s = data;
02887 s.setPose(odomPose);
02888 cv::Mat ground, obstacles, empty;
02889 cv::Point3f viewpoint;
02890 timer.ticks();
02891
02892 if(ui_->checkBox_grid_regenerateFromSavedGrid->isChecked() && s.sensorData().gridCellSize() > 0.0f)
02893 {
02894 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridObstacleCellsRaw()));
02895 *cloud+=*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridGroundCellsRaw()));
02896
02897 if(cloud->size())
02898 {
02899
02900 if(s.sensorData().cameraModels().size())
02901 {
02902
02903 float sum = 0;
02904 for(unsigned int i=0; i<s.sensorData().cameraModels().size(); ++i)
02905 {
02906 const Transform & t = s.sensorData().cameraModels()[i].localTransform();
02907 if(!t.isNull())
02908 {
02909 viewpoint.x += t.x();
02910 viewpoint.y += t.y();
02911 viewpoint.z += t.z();
02912 sum += 1.0f;
02913 }
02914 }
02915 if(sum > 0.0f)
02916 {
02917 viewpoint.x /= sum;
02918 viewpoint.y /= sum;
02919 viewpoint.z /= sum;
02920 }
02921 }
02922 else
02923 {
02924 const Transform & t = s.sensorData().stereoCameraModel().localTransform();
02925 viewpoint = cv::Point3f(t.x(), t.y(), t.z());
02926 }
02927
02928 grid.createLocalMap(LaserScan::backwardCompatibility(util3d::laserScanFromPointCloud(*cloud)), s.getPose(), ground, obstacles, empty, viewpoint);
02929 }
02930 }
02931 else
02932 {
02933 grid.createLocalMap(s, ground, obstacles, empty, viewpoint);
02934 }
02935
02936 gridCreationTime = timer.ticks()*1000.0;
02937 uInsert(generatedLocalMaps_, std::make_pair(data.id(), std::make_pair(std::make_pair(ground, obstacles), empty)));
02938 uInsert(generatedLocalMapsInfo_, std::make_pair(data.id(), std::make_pair(grid.getCellSize(), viewpoint)));
02939 msg = QString("Generated local occupancy grid map %1/%2").arg(i+1).arg((int)ids_.size());
02940
02941 totalCurve->addValue(ids_.at(i), obstacles.cols+ground.cols+empty.cols);
02942 emptyCurve->addValue(ids_.at(i), empty.cols);
02943 obstaclesCurve->addValue(ids_.at(i), obstacles.cols);
02944 groundCurve->addValue(ids_.at(i), ground.cols);
02945 }
02946
02947 progressDialog.appendText(msg);
02948 progressDialog.incrementStep();
02949
02950 decompressionCurve->addValue(ids_.at(i), decompressionTime);
02951 gridCreationCurve->addValue(ids_.at(i), gridCreationTime);
02952
02953 if(ids_.size() < 50 || (i+1) % 25 == 0)
02954 {
02955 QApplication::processEvents();
02956 }
02957 }
02958 progressDialog.setValue(progressDialog.maximumSteps());
02959
02960 if(graphes_.size())
02961 {
02962 update3dView();
02963 sliderIterationsValueChanged((int)graphes_.size()-1);
02964 }
02965 else
02966 {
02967 updateGrid();
02968 }
02969 }
02970
02971 void DatabaseViewer::regenerateCurrentLocalMaps()
02972 {
02973 UTimer time;
02974 OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
02975
02976 if(ids_.size() == 0)
02977 {
02978 UWARN("ids_ is empty!");
02979 return;
02980 }
02981
02982 QSet<int> idsSet;
02983 idsSet.insert(ids_.at(ui_->horizontalSlider_A->value()));
02984 idsSet.insert(ids_.at(ui_->horizontalSlider_B->value()));
02985 QList<int> ids = idsSet.toList();
02986
02987 rtabmap::ProgressDialog progressDialog(this);
02988 progressDialog.setMaximumSteps(ids.size());
02989 progressDialog.show();
02990
02991 for(int i =0; i<ids.size(); ++i)
02992 {
02993 generatedLocalMaps_.erase(ids.at(i));
02994 generatedLocalMapsInfo_.erase(ids.at(i));
02995
02996 SensorData data;
02997 dbDriver_->getNodeData(ids.at(i), data);
02998 data.uncompressData();
02999
03000 int mapId, weight;
03001 Transform odomPose, groundTruth;
03002 std::string label;
03003 double stamp;
03004 QString msg;
03005 std::vector<float> velocity;
03006 GPS gps;
03007 if(dbDriver_->getNodeInfo(data.id(), odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps))
03008 {
03009 Signature s = data;
03010 s.setPose(odomPose);
03011 cv::Mat ground, obstacles, empty;
03012 cv::Point3f viewpoint;
03013
03014 if(ui_->checkBox_grid_regenerateFromSavedGrid->isChecked() && s.sensorData().gridCellSize() > 0.0f)
03015 {
03016 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridObstacleCellsRaw()));
03017 *cloud+=*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridGroundCellsRaw()));
03018
03019 if(cloud->size())
03020 {
03021
03022 if(s.sensorData().cameraModels().size())
03023 {
03024
03025 float sum = 0;
03026 for(unsigned int i=0; i<s.sensorData().cameraModels().size(); ++i)
03027 {
03028 const Transform & t = s.sensorData().cameraModels()[i].localTransform();
03029 if(!t.isNull())
03030 {
03031 viewpoint.x += t.x();
03032 viewpoint.y += t.y();
03033 viewpoint.z += t.z();
03034 sum += 1.0f;
03035 }
03036 }
03037 if(sum > 0.0f)
03038 {
03039 viewpoint.x /= sum;
03040 viewpoint.y /= sum;
03041 viewpoint.z /= sum;
03042 }
03043 }
03044 else
03045 {
03046 const Transform & t = s.sensorData().stereoCameraModel().localTransform();
03047 viewpoint = cv::Point3f(t.x(), t.y(), t.z());
03048 }
03049
03050 grid.createLocalMap(LaserScan::backwardCompatibility(util3d::laserScanFromPointCloud(*cloud)), s.getPose(), ground, obstacles, empty, viewpoint);
03051 }
03052 }
03053 else
03054 {
03055 grid.createLocalMap(s, ground, obstacles, empty, viewpoint);
03056 }
03057
03058
03059 uInsert(generatedLocalMaps_, std::make_pair(data.id(), std::make_pair(std::make_pair(ground, obstacles),empty)));
03060 uInsert(generatedLocalMapsInfo_, std::make_pair(data.id(), std::make_pair(grid.getCellSize(), viewpoint)));
03061 msg = QString("Generated local occupancy grid map %1/%2 (%3s)").arg(i+1).arg((int)ids.size()).arg(time.ticks());
03062 }
03063
03064 progressDialog.appendText(msg);
03065 progressDialog.incrementStep();
03066 QApplication::processEvents();
03067 }
03068 progressDialog.setValue(progressDialog.maximumSteps());
03069
03070 if(graphes_.size())
03071 {
03072 update3dView();
03073 sliderIterationsValueChanged((int)graphes_.size()-1);
03074 }
03075 else
03076 {
03077 updateGrid();
03078 }
03079 }
03080
03081 void DatabaseViewer::view3DMap()
03082 {
03083 if(!ids_.size() || !dbDriver_)
03084 {
03085 QMessageBox::warning(this, tr("Cannot view 3D map"), tr("The database is empty..."));
03086 return;
03087 }
03088
03089 if(graphes_.empty())
03090 {
03091 this->updateGraphView();
03092 if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
03093 {
03094 QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
03095 return;
03096 }
03097 }
03098
03099 std::map<int, Transform> optimizedPoses;
03100 if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty())
03101 {
03102 optimizedPoses = groundTruthPoses_;
03103 }
03104 else
03105 {
03106 optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
03107 }
03108 if(ui_->groupBox_posefiltering->isChecked())
03109 {
03110 optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
03111 ui_->doubleSpinBox_posefilteringRadius->value(),
03112 ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
03113 }
03114 if(optimizedPoses.size() > 0)
03115 {
03116 exportDialog_->setDBDriver(dbDriver_);
03117 exportDialog_->viewClouds(optimizedPoses,
03118 updateLinksWithModifications(links_),
03119 mapIds_,
03120 QMap<int, Signature>(),
03121 std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
03122 std::map<int, LaserScan>(),
03123 pathDatabase_,
03124 ui_->parameters_toolbox->getParameters());
03125 }
03126 else
03127 {
03128 QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
03129 }
03130 }
03131
03132 void DatabaseViewer::generate3DMap()
03133 {
03134 if(!ids_.size() || !dbDriver_)
03135 {
03136 QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
03137 return;
03138 }
03139
03140 if(graphes_.empty())
03141 {
03142 this->updateGraphView();
03143 if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
03144 {
03145 QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
03146 return;
03147 }
03148 }
03149
03150 std::map<int, Transform> optimizedPoses;
03151 if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty())
03152 {
03153 optimizedPoses = groundTruthPoses_;
03154 }
03155 else
03156 {
03157 optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
03158 }
03159 if(ui_->groupBox_posefiltering->isChecked())
03160 {
03161 optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
03162 ui_->doubleSpinBox_posefilteringRadius->value(),
03163 ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
03164 }
03165 if(optimizedPoses.size() > 0)
03166 {
03167 exportDialog_->setDBDriver(dbDriver_);
03168 exportDialog_->exportClouds(optimizedPoses,
03169 updateLinksWithModifications(links_),
03170 mapIds_,
03171 QMap<int, Signature>(),
03172 std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
03173 std::map<int, LaserScan>(),
03174 pathDatabase_,
03175 ui_->parameters_toolbox->getParameters());
03176 }
03177 else
03178 {
03179 QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
03180 }
03181 }
03182
03183 void DatabaseViewer::detectMoreLoopClosures()
03184 {
03185 if(graphes_.empty())
03186 {
03187 this->updateGraphView();
03188 if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
03189 {
03190 QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
03191 return;
03192 }
03193 }
03194
03195 const std::map<int, Transform> & optimizedPoses = graphes_.back();
03196
03197 rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
03198 progressDialog->setAttribute(Qt::WA_DeleteOnClose);
03199 progressDialog->setMaximumSteps(1);
03200 progressDialog->setCancelButtonVisible(true);
03201 progressDialog->setMinimumWidth(800);
03202 progressDialog->show();
03203
03204 int iterations = ui_->spinBox_detectMore_iterations->value();
03205 UASSERT(iterations > 0);
03206 int added = 0;
03207 std::multimap<int, int> checkedLoopClosures;
03208 std::pair<int, int> lastAdded(0,0);
03209 for(int n=0; n<iterations; ++n)
03210 {
03211 UINFO("iteration %d/%d", n+1, iterations);
03212 std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(
03213 optimizedPoses,
03214 ui_->doubleSpinBox_detectMore_radius->value(),
03215 ui_->doubleSpinBox_detectMore_angle->value()*CV_PI/180.0);
03216
03217 progressDialog->setMaximumSteps(progressDialog->maximumSteps()+(int)clusters.size());
03218 progressDialog->appendText(tr("Looking for more loop closures, clusters found %1 clusters.").arg(clusters.size()));
03219 QApplication::processEvents();
03220 if(progressDialog->isCanceled())
03221 {
03222 break;
03223 }
03224
03225 std::set<int> addedLinks;
03226 int i=0;
03227 for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end() && !progressDialog->isCanceled(); ++iter, ++i)
03228 {
03229 int from = iter->first;
03230 int to = iter->second;
03231 if(from < to)
03232 {
03233 from = iter->second;
03234 to = iter->first;
03235 }
03236
03237
03238 if(rtabmap::graph::findLink(checkedLoopClosures, from, to) == checkedLoopClosures.end())
03239 {
03240 if(!findActiveLink(from, to).isValid() && !containsLink(linksRemoved_, from, to) &&
03241 addedLinks.find(from) == addedLinks.end() && addedLinks.find(to) == addedLinks.end())
03242 {
03243 checkedLoopClosures.insert(std::make_pair(from, to));
03244 if(addConstraint(from, to, true))
03245 {
03246 UINFO("Added new loop closure between %d and %d.", from, to);
03247 ++added;
03248 addedLinks.insert(from);
03249 addedLinks.insert(to);
03250 lastAdded.first = from;
03251 lastAdded.second = to;
03252
03253 progressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
03254 QApplication::processEvents();
03255 }
03256 }
03257 }
03258 progressDialog->incrementStep();
03259 if(i%100)
03260 {
03261 QApplication::processEvents();
03262 }
03263 }
03264 UINFO("Iteration %d/%d: added %d loop closures.", n+1, iterations, (int)addedLinks.size()/2);
03265 progressDialog->appendText(tr("Iteration %1/%2: Detected %3 loop closures!").arg(n+1).arg(iterations).arg(addedLinks.size()/2));
03266 if(addedLinks.size() == 0)
03267 {
03268 break;
03269 }
03270 }
03271
03272 if(added)
03273 {
03274 this->updateGraphView();
03275 this->updateLoopClosuresSlider(lastAdded.first, lastAdded.second);
03276 }
03277 UINFO("Total added %d loop closures.", added);
03278
03279 progressDialog->appendText(tr("Total new loop closures detected=%1").arg(added));
03280 progressDialog->setValue(progressDialog->maximumSteps());
03281 }
03282
03283 void DatabaseViewer::refineAllNeighborLinks()
03284 {
03285 if(neighborLinks_.size())
03286 {
03287 rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
03288 progressDialog->setAttribute(Qt::WA_DeleteOnClose);
03289 progressDialog->setMaximumSteps(neighborLinks_.size());
03290 progressDialog->setCancelButtonVisible(true);
03291 progressDialog->setMinimumWidth(800);
03292 progressDialog->show();
03293
03294 for(int i=0; i<neighborLinks_.size(); ++i)
03295 {
03296 int from = neighborLinks_[i].from();
03297 int to = neighborLinks_[i].to();
03298 this->refineConstraint(neighborLinks_[i].from(), neighborLinks_[i].to(), true);
03299
03300 progressDialog->appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(neighborLinks_.size()));
03301 progressDialog->incrementStep();
03302 QApplication::processEvents();
03303 if(progressDialog->isCanceled())
03304 {
03305 break;
03306 }
03307 }
03308 this->updateGraphView();
03309
03310 progressDialog->setValue(progressDialog->maximumSteps());
03311 progressDialog->appendText("Refining links finished!");
03312 }
03313 }
03314
03315 void DatabaseViewer::refineAllLoopClosureLinks()
03316 {
03317 if(loopLinks_.size())
03318 {
03319 rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
03320 progressDialog->setAttribute(Qt::WA_DeleteOnClose);
03321 progressDialog->setMaximumSteps(loopLinks_.size());
03322 progressDialog->setCancelButtonVisible(true);
03323 progressDialog->setMinimumWidth(800);
03324 progressDialog->show();
03325
03326 for(int i=0; i<loopLinks_.size(); ++i)
03327 {
03328 int from = loopLinks_[i].from();
03329 int to = loopLinks_[i].to();
03330 this->refineConstraint(loopLinks_[i].from(), loopLinks_[i].to(), true);
03331
03332 progressDialog->appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(loopLinks_.size()));
03333 progressDialog->incrementStep();
03334 QApplication::processEvents();
03335 if(progressDialog->isCanceled())
03336 {
03337 break;
03338 }
03339 }
03340 this->updateGraphView();
03341
03342 progressDialog->setValue(progressDialog->maximumSteps());
03343 progressDialog->appendText("Refining links finished!");
03344 }
03345 }
03346
03347 void DatabaseViewer::resetAllChanges()
03348 {
03349 linksAdded_.clear();
03350 linksRefined_.clear();
03351 linksRemoved_.clear();
03352 generatedLocalMaps_.clear();
03353 generatedLocalMapsInfo_.clear();
03354 updateLoopClosuresSlider();
03355 this->updateGraphView();
03356 }
03357
03358 void DatabaseViewer::sliderAValueChanged(int value)
03359 {
03360 this->update(value,
03361 ui_->label_indexA,
03362 ui_->label_parentsA,
03363 ui_->label_childrenA,
03364 ui_->label_weightA,
03365 ui_->label_labelA,
03366 ui_->label_stampA,
03367 ui_->graphicsView_A,
03368 ui_->label_idA,
03369 ui_->label_mapA,
03370 ui_->label_poseA,
03371 ui_->label_velA,
03372 ui_->label_calibA,
03373 ui_->label_gpsA,
03374 true);
03375 }
03376
03377 void DatabaseViewer::sliderBValueChanged(int value)
03378 {
03379 this->update(value,
03380 ui_->label_indexB,
03381 ui_->label_parentsB,
03382 ui_->label_childrenB,
03383 ui_->label_weightB,
03384 ui_->label_labelB,
03385 ui_->label_stampB,
03386 ui_->graphicsView_B,
03387 ui_->label_idB,
03388 ui_->label_mapB,
03389 ui_->label_poseB,
03390 ui_->label_velB,
03391 ui_->label_calibB,
03392 ui_->label_gpsB,
03393 true);
03394 }
03395
03396 void DatabaseViewer::update(int value,
03397 QLabel * labelIndex,
03398 QLabel * labelParents,
03399 QLabel * labelChildren,
03400 QLabel * weight,
03401 QLabel * label,
03402 QLabel * stamp,
03403 rtabmap::ImageView * view,
03404 QLabel * labelId,
03405 QLabel * labelMapId,
03406 QLabel * labelPose,
03407 QLabel * labelVelocity,
03408 QLabel * labelCalib,
03409 QLabel * labelGps,
03410 bool updateConstraintView)
03411 {
03412 UTimer timer;
03413 labelIndex->setText(QString::number(value));
03414 labelParents->clear();
03415 labelChildren->clear();
03416 weight->clear();
03417 label->clear();
03418 labelMapId->clear();
03419 labelPose->clear();
03420 labelVelocity->clear();
03421 stamp->clear();
03422 labelCalib->clear();
03423 labelGps->clear();
03424 QRectF rect;
03425 if(value >= 0 && value < ids_.size())
03426 {
03427 view->clear();
03428 int id = ids_.at(value);
03429 int mapId = -1;
03430 labelId->setText(QString::number(id));
03431 if(id>0)
03432 {
03433
03434 QImage img;
03435 cv::Mat imgDepth;
03436 if(dbDriver_)
03437 {
03438 SensorData data;
03439 dbDriver_->getNodeData(id, data);
03440 data.uncompressData();
03441 if(!data.imageRaw().empty())
03442 {
03443 img = uCvMat2QImage(ui_->label_indexB==labelIndex?data.imageRaw():data.imageRaw());
03444 }
03445 if(!data.depthOrRightRaw().empty())
03446 {
03447 cv::Mat depth =data.depthOrRightRaw();
03448 if(!data.depthRaw().empty())
03449 {
03450 if(ui_->spinBox_mesh_fillDepthHoles->value() > 0)
03451 {
03452 depth = util2d::fillDepthHoles(depth, ui_->spinBox_mesh_fillDepthHoles->value(), float(ui_->spinBox_mesh_depthError->value())/100.0f);
03453 }
03454 }
03455 imgDepth = depth;
03456 }
03457
03458 std::list<int> ids;
03459 ids.push_back(id);
03460 std::list<Signature*> signatures;
03461 dbDriver_->loadSignatures(ids, signatures);
03462
03463 if(signatures.size() && signatures.front()!=0 && signatures.front()->getWords().size())
03464 {
03465 view->setFeatures(signatures.front()->getWords(), data.depthOrRightRaw().type() == CV_8UC1?cv::Mat():data.depthOrRightRaw(), Qt::yellow);
03466 }
03467
03468 Transform odomPose, g;
03469 int w;
03470 std::string l;
03471 double s;
03472 std::vector<float> v;
03473 GPS gps;
03474 dbDriver_->getNodeInfo(id, odomPose, mapId, w, l, s, g, v, gps);
03475
03476 weight->setNum(w);
03477 label->setText(l.c_str());
03478 float x,y,z,roll,pitch,yaw;
03479 odomPose.getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw);
03480 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));
03481 if(s!=0.0)
03482 {
03483 stamp->setText(QString::number(s, 'f'));
03484 stamp->setToolTip(QDateTime::fromMSecsSinceEpoch(s*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
03485 }
03486 if(v.size()==6)
03487 {
03488 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]));
03489 }
03490 if(gps.stamp()>0.0)
03491 {
03492 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()));
03493 labelGps->setToolTip(QDateTime::fromMSecsSinceEpoch(gps.stamp()*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
03494 }
03495 if(data.cameraModels().size() || data.stereoCameraModel().isValidForProjection())
03496 {
03497 if(data.cameraModels().size())
03498 {
03499 if(!data.depthRaw().empty() && data.depthRaw().cols!=data.imageRaw().cols && data.imageRaw().cols)
03500 {
03501 labelCalib->setText(tr("%1 %2x%3 [%8x%9] fx=%4 fy=%5 cx=%6 cy=%7 T=%10")
03502 .arg(data.cameraModels().size())
03503 .arg(data.cameraModels()[0].imageWidth()>0?data.cameraModels()[0].imageWidth():data.imageRaw().cols/data.cameraModels().size())
03504 .arg(data.cameraModels()[0].imageHeight()>0?data.cameraModels()[0].imageHeight():data.imageRaw().rows)
03505 .arg(data.cameraModels()[0].fx())
03506 .arg(data.cameraModels()[0].fy())
03507 .arg(data.cameraModels()[0].cx())
03508 .arg(data.cameraModels()[0].cy())
03509 .arg(data.depthRaw().cols/data.cameraModels().size())
03510 .arg(data.depthRaw().rows)
03511 .arg(data.cameraModels()[0].localTransform().prettyPrint().c_str()));
03512 }
03513 else
03514 {
03515 labelCalib->setText(tr("%1 %2x%3 fx=%4 fy=%5 cx=%6 cy=%7 T=%8")
03516 .arg(data.cameraModels().size())
03517 .arg(data.cameraModels()[0].imageWidth()>0?data.cameraModels()[0].imageWidth():data.imageRaw().cols/data.cameraModels().size())
03518 .arg(data.cameraModels()[0].imageHeight()>0?data.cameraModels()[0].imageHeight():data.imageRaw().rows)
03519 .arg(data.cameraModels()[0].fx())
03520 .arg(data.cameraModels()[0].fy())
03521 .arg(data.cameraModels()[0].cx())
03522 .arg(data.cameraModels()[0].cy())
03523 .arg(data.cameraModels()[0].localTransform().prettyPrint().c_str()));
03524 }
03525 }
03526 else
03527 {
03528
03529 labelCalib->setText(tr("%1x%2 fx=%3 fy=%4 cx=%5 cy=%6 baseline=%7m T=%8")
03530 .arg(data.stereoCameraModel().left().imageWidth()>0?data.stereoCameraModel().left().imageWidth():data.imageRaw().cols)
03531 .arg(data.stereoCameraModel().left().imageHeight()>0?data.stereoCameraModel().left().imageHeight():data.imageRaw().rows)
03532 .arg(data.stereoCameraModel().left().fx())
03533 .arg(data.stereoCameraModel().left().fy())
03534 .arg(data.stereoCameraModel().left().cx())
03535 .arg(data.stereoCameraModel().left().cy())
03536 .arg(data.stereoCameraModel().baseline())
03537 .arg(data.stereoCameraModel().localTransform().prettyPrint().c_str()));
03538 }
03539
03540 }
03541 else
03542 {
03543 labelCalib->setText("NA");
03544 }
03545
03546
03547 if(!data.depthOrRightRaw().empty() && data.depthOrRightRaw().type() == CV_8UC1)
03548 {
03549 this->updateStereo(&data);
03550 }
03551 else
03552 {
03553 stereoViewer_->clear();
03554 ui_->graphicsView_stereo->clear();
03555 }
03556
03557
03558 if(cloudViewer_->isVisible())
03559 {
03560 Transform pose = Transform::getIdentity();
03561 if(signatures.size() && ui_->checkBox_odomFrame_3dview->isChecked())
03562 {
03563 float x, y, z, roll, pitch, yaw;
03564 (*signatures.begin())->getPose().getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
03565 pose = Transform(0,0,z,roll,pitch,0);
03566 }
03567
03568 cloudViewer_->removeAllFrustums();
03569 cloudViewer_->removeCloud("mesh");
03570 cloudViewer_->removeCloud("cloud");
03571 cloudViewer_->removeCloud("scan");
03572 cloudViewer_->removeCloud("map");
03573 cloudViewer_->removeCloud("ground");
03574 cloudViewer_->removeCloud("obstacles");
03575 cloudViewer_->removeCloud("empty_cells");
03576 cloudViewer_->removeCloud("words");
03577 cloudViewer_->removeOctomap();
03578 if(ui_->checkBox_showCloud->isChecked() || ui_->checkBox_showMesh->isChecked())
03579 {
03580 if(!data.depthOrRightRaw().empty())
03581 {
03582 if(!data.imageRaw().empty())
03583 {
03584 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
03585 pcl::IndicesPtr indices(new std::vector<int>);
03586 if(!data.depthRaw().empty() && data.cameraModels().size()==1)
03587 {
03588 cv::Mat depth = data.depthRaw();
03589 if(ui_->spinBox_mesh_fillDepthHoles->value() > 0)
03590 {
03591 depth = util2d::fillDepthHoles(depth, ui_->spinBox_mesh_fillDepthHoles->value(), float(ui_->spinBox_mesh_depthError->value())/100.0f);
03592 }
03593 cloud = util3d::cloudFromDepthRGB(
03594 data.imageRaw(),
03595 depth,
03596 data.cameraModels()[0],
03597 ui_->spinBox_decimation->value(),0,0,indices.get());
03598 if(indices->size())
03599 {
03600 cloud = util3d::transformPointCloud(cloud, data.cameraModels()[0].localTransform());
03601 }
03602
03603 }
03604 else
03605 {
03606 cloud = util3d::cloudRGBFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, indices.get(), ui_->parameters_toolbox->getParameters());
03607 }
03608 if(indices->size())
03609 {
03610 if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
03611 {
03612 cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value());
03613 }
03614
03615 if(ui_->checkBox_showMesh->isChecked() && !cloud->is_dense)
03616 {
03617 Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
03618 if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
03619 {
03620 viewpoint[0] = data.cameraModels()[0].localTransform().x();
03621 viewpoint[1] = data.cameraModels()[0].localTransform().y();
03622 viewpoint[2] = data.cameraModels()[0].localTransform().z();
03623 }
03624 else if(!data.stereoCameraModel().localTransform().isNull())
03625 {
03626 viewpoint[0] = data.stereoCameraModel().localTransform().x();
03627 viewpoint[1] = data.stereoCameraModel().localTransform().y();
03628 viewpoint[2] = data.stereoCameraModel().localTransform().z();
03629 }
03630 std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
03631 cloud,
03632 float(ui_->spinBox_mesh_angleTolerance->value())*M_PI/180.0f,
03633 ui_->checkBox_mesh_quad->isChecked(),
03634 ui_->spinBox_mesh_triangleSize->value(),
03635 viewpoint);
03636
03637 if(ui_->spinBox_mesh_minClusterSize->value())
03638 {
03639
03640 std::vector<std::set<int> > neighbors;
03641 std::vector<std::set<int> > vertexToPolygons;
03642 util3d::createPolygonIndexes(polygons,
03643 cloud->size(),
03644 neighbors,
03645 vertexToPolygons);
03646 std::list<std::list<int> > clusters = util3d::clusterPolygons(
03647 neighbors,
03648 ui_->spinBox_mesh_minClusterSize->value());
03649 std::vector<pcl::Vertices> filteredPolygons(polygons.size());
03650 int oi=0;
03651 for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
03652 {
03653 for(std::list<int>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
03654 {
03655 filteredPolygons[oi++] = polygons.at(*jter);
03656 }
03657 }
03658 filteredPolygons.resize(oi);
03659 polygons = filteredPolygons;
03660 }
03661
03662 cloudViewer_->addCloudMesh("mesh", cloud, polygons, pose);
03663 }
03664 if(ui_->checkBox_showCloud->isChecked())
03665 {
03666 cloudViewer_->addCloud("cloud", cloud, pose);
03667 }
03668 }
03669 }
03670 else if(ui_->checkBox_showCloud->isChecked())
03671 {
03672 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
03673 pcl::IndicesPtr indices(new std::vector<int>);
03674 cloud = util3d::cloudFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, indices.get(), ui_->parameters_toolbox->getParameters());
03675 if(indices->size())
03676 {
03677 if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
03678 {
03679 cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value());
03680 }
03681
03682 cloudViewer_->addCloud("cloud", cloud, pose);
03683 }
03684 }
03685 }
03686 }
03687
03688
03689 if(cloudViewer_->isFrustumShown())
03690 {
03691 if(data.cameraModels().size())
03692 {
03693 cloudViewer_->updateCameraFrustums(pose, data.cameraModels());
03694 }
03695 else
03696 {
03697 cloudViewer_->updateCameraFrustum(pose, data.stereoCameraModel());
03698 }
03699 }
03700
03701
03702 if(ui_->checkBox_showWords->isChecked() && signatures.size())
03703 {
03704 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
03705 cloud->resize((*signatures.begin())->getWords3().size());
03706 int i=0;
03707 for(std::multimap<int, cv::Point3f>::const_iterator iter=(*signatures.begin())->getWords3().begin();
03708 iter!=(*signatures.begin())->getWords3().end();
03709 ++iter)
03710 {
03711 cloud->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
03712 }
03713
03714 if(cloud->size())
03715 {
03716 cloud = rtabmap::util3d::removeNaNFromPointCloud(cloud);
03717 }
03718
03719 if(cloud->size())
03720 {
03721 cloudViewer_->addCloud("words", cloud, pose, Qt::red);
03722 }
03723 }
03724
03725
03726 if(ui_->checkBox_showScan->isChecked() && data.laserScanRaw().size())
03727 {
03728 if(data.laserScanRaw().hasRGB() && data.laserScanRaw().hasNormals())
03729 {
03730 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr scan = util3d::laserScanToPointCloudRGBNormal(data.laserScanRaw(), data.laserScanRaw().localTransform());
03731 cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
03732 }
03733 else if(data.laserScanRaw().hasIntensity() && data.laserScanRaw().hasNormals())
03734 {
03735 pcl::PointCloud<pcl::PointXYZINormal>::Ptr scan = util3d::laserScanToPointCloudINormal(data.laserScanRaw(), data.laserScanRaw().localTransform());
03736 cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
03737 }
03738 else if(data.laserScanRaw().hasNormals())
03739 {
03740 pcl::PointCloud<pcl::PointNormal>::Ptr scan = util3d::laserScanToPointCloudNormal(data.laserScanRaw(), data.laserScanRaw().localTransform());
03741 cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
03742 }
03743 else if(data.laserScanRaw().hasRGB())
03744 {
03745 pcl::PointCloud<pcl::PointXYZRGB>::Ptr scan = util3d::laserScanToPointCloudRGB(data.laserScanRaw(), data.laserScanRaw().localTransform());
03746 cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
03747 }
03748 else
03749 {
03750 pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(data.laserScanRaw(), data.laserScanRaw().localTransform());
03751 cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
03752 }
03753 }
03754
03755
03756 if(ui_->checkBox_showMap->isChecked() || ui_->checkBox_showGrid->isChecked())
03757 {
03758 std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > localMaps;
03759 std::map<int, std::pair<float, cv::Point3f> > localMapsInfo;
03760 if(generatedLocalMaps_.find(data.id()) != generatedLocalMaps_.end())
03761 {
03762 localMaps.insert(*generatedLocalMaps_.find(data.id()));
03763 localMapsInfo.insert(*generatedLocalMapsInfo_.find(data.id()));
03764 }
03765 else if(!data.gridGroundCellsRaw().empty() || !data.gridObstacleCellsRaw().empty())
03766 {
03767 localMaps.insert(std::make_pair(data.id(), std::make_pair(std::make_pair(data.gridGroundCellsRaw(), data.gridObstacleCellsRaw()), data.gridEmptyCellsRaw())));
03768 localMapsInfo.insert(std::make_pair(data.id(), std::make_pair(data.gridCellSize(), data.gridViewPoint())));
03769 }
03770 if(!localMaps.empty())
03771 {
03772 std::map<int, Transform> poses;
03773 poses.insert(std::make_pair(data.id(), pose));
03774
03775 #ifdef RTABMAP_OCTOMAP
03776 OctoMap * octomap = 0;
03777 if(ui_->checkBox_octomap->isChecked() &&
03778 (!localMaps.begin()->second.first.first.empty() || !localMaps.begin()->second.first.second.empty()) &&
03779 (localMaps.begin()->second.first.first.empty() || localMaps.begin()->second.first.first.channels() > 2) &&
03780 (localMaps.begin()->second.first.second.empty() || localMaps.begin()->second.first.second.channels() > 2) &&
03781 (localMaps.begin()->second.second.empty() || localMaps.begin()->second.second.channels() > 2) &&
03782 localMapsInfo.begin()->second.first > 0.0f)
03783 {
03784
03785 octomap = new OctoMap(localMapsInfo.begin()->second.first);
03786 octomap->addToCache(data.id(), localMaps.begin()->second.first.first, localMaps.begin()->second.first.second, localMaps.begin()->second.second, localMapsInfo.begin()->second.second);
03787 octomap->update(poses);
03788 }
03789 #endif
03790
03791 if(ui_->checkBox_showMap->isChecked())
03792 {
03793 float xMin=0.0f, yMin=0.0f;
03794 cv::Mat map8S;
03795 ParametersMap parameters = ui_->parameters_toolbox->getParameters();
03796 float gridCellSize = Parameters::defaultGridCellSize();
03797 Parameters::parse(parameters, Parameters::kGridCellSize(), gridCellSize);
03798 #ifdef RTABMAP_OCTOMAP
03799 if(octomap)
03800 {
03801 map8S = octomap->createProjectionMap(xMin, yMin, gridCellSize, 0);
03802 }
03803 else
03804 #endif
03805 {
03806 OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
03807 grid.setCellSize(gridCellSize);
03808 grid.addToCache(data.id(), localMaps.begin()->second.first.first, localMaps.begin()->second.first.second, localMaps.begin()->second.second);
03809 grid.update(poses);
03810 map8S = grid.getMap(xMin, yMin);
03811 }
03812 if(!map8S.empty())
03813 {
03814
03815 cloudViewer_->addOccupancyGridMap(util3d::convertMap2Image8U(map8S), gridCellSize, xMin, yMin, 1);
03816 }
03817 }
03818
03819 if(ui_->checkBox_showGrid->isChecked())
03820 {
03821 #ifdef RTABMAP_OCTOMAP
03822 if(octomap)
03823 {
03824 if(ui_->comboBox_octomap_rendering_type->currentIndex()== 0)
03825 {
03826 pcl::IndicesPtr obstacles(new std::vector<int>);
03827 pcl::IndicesPtr empty(new std::vector<int>);
03828 pcl::IndicesPtr ground(new std::vector<int>);
03829 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(ui_->spinBox_grid_depth->value(), obstacles.get(), empty.get(), ground.get());
03830 if(octomap->hasColor())
03831 {
03832 pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
03833 pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
03834 cloudViewer_->addCloud("obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
03835 cloudViewer_->setCloudPointSize("obstacles", 5);
03836
03837 pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
03838 pcl::copyPointCloud(*cloud, *ground, *groundCloud);
03839 cloudViewer_->addCloud("ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
03840 cloudViewer_->setCloudPointSize("ground", 5);
03841 }
03842 else
03843 {
03844 pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
03845 pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
03846 cloudViewer_->addCloud("obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
03847 cloudViewer_->setCloudPointSize("obstacles", 5);
03848
03849 pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
03850 pcl::copyPointCloud(*cloud, *ground, *groundCloud);
03851 cloudViewer_->addCloud("ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
03852 cloudViewer_->setCloudPointSize("ground", 5);
03853 }
03854
03855 if(ui_->checkBox_grid_empty->isChecked())
03856 {
03857 pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZ>);
03858 pcl::copyPointCloud(*cloud, *empty, *emptyCloud);
03859 cloudViewer_->addCloud("empty_cells", emptyCloud, Transform::getIdentity(), QColor(ui_->lineEdit_emptyColor->text()));
03860 cloudViewer_->setCloudOpacity("empty_cells", 0.5);
03861 cloudViewer_->setCloudPointSize("empty_cells", 5);
03862 }
03863 }
03864 else
03865 {
03866 cloudViewer_->addOctomap(octomap, ui_->spinBox_grid_depth->value(), ui_->comboBox_octomap_rendering_type->currentIndex()>1);
03867 }
03868 }
03869 else
03870 #endif
03871 {
03872
03873 LaserScan scan = LaserScan::backwardCompatibility(localMaps.begin()->second.first.first);
03874 if(scan.hasRGB())
03875 {
03876 cloudViewer_->addCloud("ground", util3d::laserScanToPointCloudRGB(scan), pose, QColor(ui_->lineEdit_groundColor->text()));
03877 }
03878 else
03879 {
03880 cloudViewer_->addCloud("ground", util3d::laserScanToPointCloud(scan), pose, QColor(ui_->lineEdit_groundColor->text()));
03881 }
03882 scan = LaserScan::backwardCompatibility(localMaps.begin()->second.first.second);
03883 if(scan.hasRGB())
03884 {
03885 cloudViewer_->addCloud("obstacles", util3d::laserScanToPointCloudRGB(scan), pose, QColor(ui_->lineEdit_obstacleColor->text()));
03886 }
03887 else
03888 {
03889 cloudViewer_->addCloud("obstacles", util3d::laserScanToPointCloud(scan), pose, QColor(ui_->lineEdit_obstacleColor->text()));
03890 }
03891
03892 cloudViewer_->setCloudPointSize("ground", 5);
03893 cloudViewer_->setCloudPointSize("obstacles", 5);
03894
03895 if(ui_->checkBox_grid_empty->isChecked())
03896 {
03897 cloudViewer_->addCloud("empty_cells",
03898 util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(localMaps.begin()->second.second)),
03899 pose,
03900 QColor(ui_->lineEdit_emptyColor->text()));
03901 cloudViewer_->setCloudPointSize("empty_cells", 5);
03902 cloudViewer_->setCloudOpacity("empty_cells", 0.5);
03903 }
03904 }
03905 }
03906 #ifdef RTABMAP_OCTOMAP
03907 if(octomap)
03908 {
03909 delete octomap;
03910 }
03911 #endif
03912 }
03913 }
03914 cloudViewer_->update();
03915 }
03916
03917 if(signatures.size())
03918 {
03919 UASSERT(signatures.front() != 0 && signatures.size() == 1);
03920 delete signatures.front();
03921 signatures.clear();
03922 }
03923 }
03924
03925 if(!img.isNull())
03926 {
03927 view->setImage(img);
03928 rect = img.rect();
03929 }
03930 else
03931 {
03932 ULOGGER_DEBUG("Image is empty");
03933 }
03934
03935 if(!imgDepth.empty())
03936 {
03937 view->setImageDepth(imgDepth);
03938 if(img.isNull())
03939 {
03940 rect.setWidth(imgDepth.cols);
03941 rect.setHeight(imgDepth.rows);
03942 }
03943 }
03944 else
03945 {
03946 ULOGGER_DEBUG("Image depth is empty");
03947 }
03948
03949
03950 std::map<int, rtabmap::Link> links;
03951 dbDriver_->loadLinks(id, links);
03952 if(links.size())
03953 {
03954 QString strParents, strChildren;
03955 for(std::map<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
03956 {
03957 if(iter->second.type() != Link::kNeighbor &&
03958 iter->second.type() != Link::kNeighborMerged)
03959 {
03960 if(iter->first < id)
03961 {
03962 strChildren.append(QString("%1 ").arg(iter->first));
03963 }
03964 else
03965 {
03966 strParents.append(QString("%1 ").arg(iter->first));
03967 }
03968 }
03969 }
03970 labelParents->setText(strParents);
03971 labelChildren->setText(strChildren);
03972 }
03973 }
03974
03975 if(mapId>=0)
03976 {
03977 labelMapId->setText(QString::number(mapId));
03978 }
03979 }
03980 else if(value != 0)
03981 {
03982 ULOGGER_ERROR("Slider index out of range ?");
03983 }
03984
03985 updateConstraintButtons();
03986 updateWordsMatching();
03987
03988 if(updateConstraintView && ui_->dockWidget_constraints->isVisible())
03989 {
03990
03991 int from = ids_.at(ui_->horizontalSlider_A->value());
03992 int to = ids_.at(ui_->horizontalSlider_B->value());
03993 bool set = false;
03994 for(int i=0; i<loopLinks_.size() || i<neighborLinks_.size(); ++i)
03995 {
03996 if(i < loopLinks_.size())
03997 {
03998 if((loopLinks_[i].from() == from && loopLinks_[i].to() == to) ||
03999 (loopLinks_[i].from() == to && loopLinks_[i].to() == from))
04000 {
04001 if(i != ui_->horizontalSlider_loops->value())
04002 {
04003 ui_->horizontalSlider_loops->blockSignals(true);
04004 ui_->horizontalSlider_loops->setValue(i);
04005 ui_->horizontalSlider_loops->blockSignals(false);
04006 this->updateConstraintView(loopLinks_[i].from() == from?loopLinks_.at(i):loopLinks_.at(i).inverse(), false);
04007 }
04008 ui_->horizontalSlider_neighbors->blockSignals(true);
04009 ui_->horizontalSlider_neighbors->setValue(0);
04010 ui_->horizontalSlider_neighbors->blockSignals(false);
04011 set = true;
04012 break;
04013 }
04014 }
04015 if(i < neighborLinks_.size())
04016 {
04017 if((neighborLinks_[i].from() == from && neighborLinks_[i].to() == to) ||
04018 (neighborLinks_[i].from() == to && neighborLinks_[i].to() == from))
04019 {
04020 if(i != ui_->horizontalSlider_neighbors->value())
04021 {
04022 ui_->horizontalSlider_neighbors->blockSignals(true);
04023 ui_->horizontalSlider_neighbors->setValue(i);
04024 ui_->horizontalSlider_neighbors->blockSignals(false);
04025 this->updateConstraintView(neighborLinks_[i].from() == from?neighborLinks_.at(i):neighborLinks_.at(i).inverse(), false);
04026 }
04027 ui_->horizontalSlider_loops->blockSignals(true);
04028 ui_->horizontalSlider_loops->setValue(0);
04029 ui_->horizontalSlider_loops->blockSignals(false);
04030 set = true;
04031 break;
04032 }
04033 }
04034 }
04035 if(!set)
04036 {
04037 ui_->horizontalSlider_loops->blockSignals(true);
04038 ui_->horizontalSlider_neighbors->blockSignals(true);
04039 ui_->horizontalSlider_loops->setValue(0);
04040 ui_->horizontalSlider_neighbors->setValue(0);
04041 ui_->horizontalSlider_loops->blockSignals(false);
04042 ui_->horizontalSlider_neighbors->blockSignals(false);
04043
04044 constraintsViewer_->removeAllClouds();
04045
04046
04047 if(graphes_.size())
04048 {
04049 std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
04050 if(optimizedPoses.size() > 0)
04051 {
04052 std::map<int, Transform>::iterator fromIter = optimizedPoses.find(from);
04053 std::map<int, Transform>::iterator toIter = optimizedPoses.find(to);
04054 if(fromIter != optimizedPoses.end() &&
04055 toIter != optimizedPoses.end())
04056 {
04057 Link link(from, to, Link::kUndef, fromIter->second.inverse() * toIter->second);
04058 this->updateConstraintView(link, false);
04059 }
04060 }
04061 }
04062
04063 constraintsViewer_->update();
04064
04065 }
04066 }
04067
04068 if(rect.isValid())
04069 {
04070 view->setSceneRect(rect);
04071 }
04072 }
04073
04074 void DatabaseViewer::updateLoggerLevel()
04075 {
04076 if(this->parent() == 0)
04077 {
04078 ULogger::setLevel((ULogger::Level)ui_->comboBox_logger_level->currentIndex());
04079 }
04080 }
04081
04082 void DatabaseViewer::updateStereo()
04083 {
04084 if(ui_->horizontalSlider_A->maximum())
04085 {
04086 int id = ids_.at(ui_->horizontalSlider_A->value());
04087 SensorData data;
04088 dbDriver_->getNodeData(id, data);
04089 data.uncompressData();
04090 updateStereo(&data);
04091 }
04092 }
04093
04094 void DatabaseViewer::updateStereo(const SensorData * data)
04095 {
04096 if(data &&
04097 ui_->dockWidget_stereoView->isVisible() &&
04098 !data->imageRaw().empty() &&
04099 !data->depthOrRightRaw().empty() &&
04100 data->depthOrRightRaw().type() == CV_8UC1 &&
04101 data->stereoCameraModel().isValidForProjection())
04102 {
04103 cv::Mat leftMono;
04104 if(data->imageRaw().channels() == 3)
04105 {
04106 cv::cvtColor(data->imageRaw(), leftMono, CV_BGR2GRAY);
04107 }
04108 else
04109 {
04110 leftMono = data->imageRaw();
04111 }
04112
04113 UTimer timer;
04114 ParametersMap parameters = ui_->parameters_toolbox->getParameters();
04115 Stereo * stereo = Stereo::create(parameters);
04116
04117
04118 std::vector<cv::KeyPoint> kpts;
04119 uInsert(parameters, ParametersPair(Parameters::kKpMaxFeatures(), parameters.at(Parameters::kVisMaxFeatures())));
04120 uInsert(parameters, ParametersPair(Parameters::kKpMinDepth(), parameters.at(Parameters::kVisMinDepth())));
04121 uInsert(parameters, ParametersPair(Parameters::kKpMaxDepth(), parameters.at(Parameters::kVisMaxDepth())));
04122 uInsert(parameters, ParametersPair(Parameters::kKpDetectorStrategy(), parameters.at(Parameters::kVisFeatureType())));
04123 uInsert(parameters, ParametersPair(Parameters::kKpRoiRatios(), parameters.at(Parameters::kVisRoiRatios())));
04124 uInsert(parameters, ParametersPair(Parameters::kKpSubPixEps(), parameters.at(Parameters::kVisSubPixEps())));
04125 uInsert(parameters, ParametersPair(Parameters::kKpSubPixIterations(), parameters.at(Parameters::kVisSubPixIterations())));
04126 uInsert(parameters, ParametersPair(Parameters::kKpSubPixWinSize(), parameters.at(Parameters::kVisSubPixWinSize())));
04127 Feature2D * kptDetector = Feature2D::create(parameters);
04128 kpts = kptDetector->generateKeypoints(leftMono);
04129 delete kptDetector;
04130
04131 float timeKpt = timer.ticks();
04132
04133 std::vector<cv::Point2f> leftCorners;
04134 cv::KeyPoint::convert(kpts, leftCorners);
04135
04136
04137 std::vector<unsigned char> status;
04138 std::vector<cv::Point2f> rightCorners;
04139
04140 rightCorners = stereo->computeCorrespondences(
04141 leftMono,
04142 data->rightRaw(),
04143 leftCorners,
04144 status);
04145 delete stereo;
04146
04147 float timeStereo = timer.ticks();
04148
04149 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
04150 cloud->resize(kpts.size());
04151 float bad_point = std::numeric_limits<float>::quiet_NaN ();
04152 UASSERT(status.size() == kpts.size());
04153 int oi = 0;
04154 int inliers = 0;
04155 int flowOutliers= 0;
04156 int slopeOutliers= 0;
04157 int negativeDisparityOutliers = 0;
04158 for(unsigned int i=0; i<status.size(); ++i)
04159 {
04160 cv::Point3f pt(bad_point, bad_point, bad_point);
04161 if(status[i])
04162 {
04163 float disparity = leftCorners[i].x - rightCorners[i].x;
04164 if(disparity > 0.0f)
04165 {
04166 cv::Point3f tmpPt = util3d::projectDisparityTo3D(
04167 leftCorners[i],
04168 disparity,
04169 data->stereoCameraModel());
04170
04171 if(util3d::isFinite(tmpPt))
04172 {
04173 pt = util3d::transformPoint(tmpPt, data->stereoCameraModel().left().localTransform());
04174 status[i] = 100;
04175 ++inliers;
04176 cloud->at(oi++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
04177 }
04178 }
04179 else
04180 {
04181 status[i] = 102;
04182 ++negativeDisparityOutliers;
04183 }
04184 }
04185 else
04186 {
04187 ++flowOutliers;
04188 }
04189 }
04190 cloud->resize(oi);
04191
04192 UINFO("correspondences = %d/%d (%f) (time kpt=%fs stereo=%fs)",
04193 (int)cloud->size(), (int)leftCorners.size(), float(cloud->size())/float(leftCorners.size()), timeKpt, timeStereo);
04194
04195 stereoViewer_->updateCameraTargetPosition(Transform::getIdentity());
04196 stereoViewer_->addCloud("stereo", cloud);
04197 stereoViewer_->update();
04198
04199 ui_->label_stereo_inliers->setNum(inliers);
04200 ui_->label_stereo_flowOutliers->setNum(flowOutliers);
04201 ui_->label_stereo_slopeOutliers->setNum(slopeOutliers);
04202 ui_->label_stereo_disparityOutliers->setNum(negativeDisparityOutliers);
04203
04204 std::vector<cv::KeyPoint> rightKpts;
04205 cv::KeyPoint::convert(rightCorners, rightKpts);
04206 std::vector<cv::DMatch> good_matches(kpts.size());
04207 for(unsigned int i=0; i<good_matches.size(); ++i)
04208 {
04209 good_matches[i].trainIdx = i;
04210 good_matches[i].queryIdx = i;
04211 }
04212
04213
04214
04215
04216
04217
04218
04219
04220
04221
04222 ui_->graphicsView_stereo->clear();
04223 ui_->graphicsView_stereo->setLinesShown(true);
04224 ui_->graphicsView_stereo->setFeaturesShown(false);
04225 ui_->graphicsView_stereo->setImageDepthShown(true);
04226
04227 ui_->graphicsView_stereo->setImage(uCvMat2QImage(data->imageRaw()));
04228 ui_->graphicsView_stereo->setImageDepth(data->depthOrRightRaw());
04229
04230
04231 for(unsigned int i=0; i<kpts.size(); ++i)
04232 {
04233 if(rightKpts[i].pt.x > 0 && rightKpts[i].pt.y > 0)
04234 {
04235 QColor c = Qt::green;
04236 if(status[i] == 0)
04237 {
04238 c = Qt::red;
04239 }
04240 else if(status[i] == 100)
04241 {
04242 c = Qt::blue;
04243 }
04244 else if(status[i] == 101)
04245 {
04246 c = Qt::yellow;
04247 }
04248 else if(status[i] == 102)
04249 {
04250 c = Qt::magenta;
04251 }
04252 else if(status[i] == 110)
04253 {
04254 c = Qt::cyan;
04255 }
04256 ui_->graphicsView_stereo->addLine(
04257 kpts[i].pt.x,
04258 kpts[i].pt.y,
04259 rightKpts[i].pt.x,
04260 rightKpts[i].pt.y,
04261 c,
04262 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));
04263 }
04264 }
04265 ui_->graphicsView_stereo->update();
04266 }
04267 }
04268
04269 void DatabaseViewer::updateWordsMatching()
04270 {
04271 int from = ids_.at(ui_->horizontalSlider_A->value());
04272 int to = ids_.at(ui_->horizontalSlider_B->value());
04273 if(from && to)
04274 {
04275 int alpha = 70;
04276 ui_->graphicsView_A->clearLines();
04277 ui_->graphicsView_A->setFeaturesColor(QColor(255, 255, 0, alpha));
04278 ui_->graphicsView_B->clearLines();
04279 ui_->graphicsView_B->setFeaturesColor(QColor(255, 255, 0, alpha));
04280
04281 const QMultiMap<int, KeypointItem*> & wordsA = ui_->graphicsView_A->getFeatures();
04282 const QMultiMap<int, KeypointItem*> & wordsB = ui_->graphicsView_B->getFeatures();
04283 if(wordsA.size() && wordsB.size())
04284 {
04285 QList<int> ids = wordsA.uniqueKeys();
04286 for(int i=0; i<ids.size(); ++i)
04287 {
04288 if(ids[i] > 0 && wordsA.count(ids[i]) == 1 && wordsB.count(ids[i]) == 1)
04289 {
04290
04291 ui_->graphicsView_A->setFeatureColor(ids[i], Qt::magenta);
04292 ui_->graphicsView_B->setFeatureColor(ids[i], Qt::magenta);
04293
04294
04295
04296 float scaleX = ui_->graphicsView_A->viewScale();
04297 float deltaX = 0;
04298 float deltaY = 0;
04299
04300 if(ui_->actionVertical_Layout->isChecked())
04301 {
04302 deltaY = ui_->graphicsView_A->height()/scaleX;
04303 }
04304 else
04305 {
04306 deltaX = ui_->graphicsView_A->width()/scaleX;
04307 }
04308
04309 const KeypointItem * kptA = wordsA.value(ids[i]);
04310 const KeypointItem * kptB = wordsB.value(ids[i]);
04311 ui_->graphicsView_A->addLine(
04312 kptA->rect().x()+kptA->rect().width()/2,
04313 kptA->rect().y()+kptA->rect().height()/2,
04314 kptB->rect().x()+kptB->rect().width()/2+deltaX,
04315 kptB->rect().y()+kptB->rect().height()/2+deltaY,
04316 Qt::cyan);
04317
04318 ui_->graphicsView_B->addLine(
04319 kptA->rect().x()+kptA->rect().width()/2-deltaX,
04320 kptA->rect().y()+kptA->rect().height()/2-deltaY,
04321 kptB->rect().x()+kptB->rect().width()/2,
04322 kptB->rect().y()+kptB->rect().height()/2,
04323 Qt::cyan);
04324 }
04325 }
04326 ui_->graphicsView_A->update();
04327 ui_->graphicsView_B->update();
04328 }
04329 }
04330 }
04331
04332 void DatabaseViewer::sliderAMoved(int value)
04333 {
04334 ui_->label_indexA->setText(QString::number(value));
04335 if(value>=0 && value < ids_.size())
04336 {
04337 ui_->label_idA->setText(QString::number(ids_.at(value)));
04338 }
04339 else
04340 {
04341 ULOGGER_ERROR("Slider index out of range ?");
04342 }
04343 }
04344
04345 void DatabaseViewer::sliderBMoved(int value)
04346 {
04347 ui_->label_indexB->setText(QString::number(value));
04348 if(value>=0 && value < ids_.size())
04349 {
04350 ui_->label_idB->setText(QString::number(ids_.at(value)));
04351 }
04352 else
04353 {
04354 ULOGGER_ERROR("Slider index out of range ?");
04355 }
04356 }
04357
04358 void DatabaseViewer::update3dView()
04359 {
04360 if(ui_->dockWidget_view3d->isVisible())
04361 {
04362 sliderAValueChanged(ui_->horizontalSlider_A->value());
04363 sliderBValueChanged(ui_->horizontalSlider_B->value());
04364 }
04365 }
04366
04367 void DatabaseViewer::sliderNeighborValueChanged(int value)
04368 {
04369 if(value < neighborLinks_.size())
04370 {
04371 this->updateConstraintView(neighborLinks_.at(value));
04372 }
04373 }
04374
04375 void DatabaseViewer::sliderLoopValueChanged(int value)
04376 {
04377 if(value < loopLinks_.size())
04378 {
04379 this->updateConstraintView(loopLinks_.at(value));
04380 }
04381 }
04382
04383
04384 void DatabaseViewer::updateConstraintView()
04385 {
04386 if(ids_.size())
04387 {
04388 Link link = this->findActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value()));
04389 if(link.isValid())
04390 {
04391 if(link.type() == Link::kNeighbor ||
04392 link.type() == Link::kNeighborMerged)
04393 {
04394 this->updateConstraintView(neighborLinks_.at(ui_->horizontalSlider_neighbors->value()), false);
04395 }
04396 else
04397 {
04398 this->updateConstraintView(loopLinks_.at(ui_->horizontalSlider_loops->value()), false);
04399 }
04400 }
04401 }
04402 }
04403
04404 void DatabaseViewer::updateConstraintView(
04405 const rtabmap::Link & linkIn,
04406 bool updateImageSliders,
04407 const Signature & signatureFrom,
04408 const Signature & signatureTo)
04409 {
04410 std::multimap<int, Link>::iterator iterLink = rtabmap::graph::findLink(linksRefined_, linkIn.from(), linkIn.to());
04411 rtabmap::Link link = linkIn;
04412
04413 if(iterLink != linksRefined_.end())
04414 {
04415 link = iterLink->second;
04416 }
04417 else if(ui_->checkBox_ignorePoseCorrection->isChecked())
04418 {
04419 if(link.type() == Link::kNeighbor ||
04420 link.type() == Link::kNeighborMerged)
04421 {
04422 Transform poseFrom = uValue(odomPoses_, link.from(), Transform());
04423 Transform poseTo = uValue(odomPoses_, link.to(), Transform());
04424 if(!poseFrom.isNull() && !poseTo.isNull())
04425 {
04426
04427
04428 link = Link(link.from(),
04429 link.to(),
04430 link.type(),
04431 poseFrom.inverse() * poseTo);
04432 }
04433 }
04434 }
04435 rtabmap::Transform t = link.transform();
04436
04437 ui_->label_constraint->clear();
04438 ui_->label_constraint_opt->clear();
04439 ui_->checkBox_showOptimized->setEnabled(false);
04440 UASSERT(!t.isNull() && dbDriver_);
04441
04442 ui_->label_type->setText(tr("%1 (%2)")
04443 .arg(link.type())
04444 .arg(link.type()==Link::kNeighbor?"Neighbor":
04445 link.type()==Link::kNeighborMerged?"Merged neighbor":
04446 link.type()==Link::kGlobalClosure?"Loop closure":
04447 link.type()==Link::kLocalSpaceClosure?"Space proximity link":
04448 link.type()==Link::kLocalTimeClosure?"Time proximity link":
04449 link.type()==Link::kUserClosure?"User link":
04450 link.type()==Link::kVirtualClosure?"Virtual link":"Undefined"));
04451 ui_->label_variance->setText(QString("%1, %2")
04452 .arg(sqrt(link.transVariance()))
04453 .arg(sqrt(link.rotVariance())));
04454 std::stringstream out;
04455 out << link.infMatrix().inv();
04456 ui_->lineEdit_covariance->setText(out.str().c_str());
04457 ui_->label_constraint->setText(QString("%1").arg(t.prettyPrint().c_str()).replace(" ", "\n"));
04458 if(graphes_.size() &&
04459 (int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
04460 {
04461 std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
04462
04463 std::map<int, rtabmap::Transform>::iterator iterFrom = graph.find(link.from());
04464 std::map<int, rtabmap::Transform>::iterator iterTo = graph.find(link.to());
04465 if(iterFrom != graph.end() && iterTo != graph.end())
04466 {
04467 ui_->checkBox_showOptimized->setEnabled(true);
04468 Transform topt = iterFrom->second.inverse()*iterTo->second;
04469 float diff = topt.getDistance(t);
04470 Transform v1 = t.rotation()*Transform(1,0,0,0,0,0);
04471 Transform v2 = topt.rotation()*Transform(1,0,0,0,0,0);
04472 float a = pcl::getAngle3D(Eigen::Vector4f(v1.x(), v1.y(), v1.z(), 0), Eigen::Vector4f(v2.x(), v2.y(), v2.z(), 0));
04473 a = (a *180.0f) / CV_PI;
04474 ui_->label_constraint_opt->setText(QString("%1\n(error=%2% a=%3)").arg(QString(topt.prettyPrint().c_str()).replace(" ", "\n")).arg((diff/t.getNorm())*100.0f).arg(a));
04475
04476 if(ui_->checkBox_showOptimized->isChecked())
04477 {
04478 t = topt;
04479 }
04480 }
04481 }
04482
04483 if(updateImageSliders)
04484 {
04485 ui_->horizontalSlider_A->blockSignals(true);
04486 ui_->horizontalSlider_B->blockSignals(true);
04487
04488 ui_->horizontalSlider_A->setValue(idToIndex_.value(link.from()));
04489 ui_->horizontalSlider_B->setValue(idToIndex_.value(link.to()));
04490 ui_->horizontalSlider_A->blockSignals(false);
04491 ui_->horizontalSlider_B->blockSignals(false);
04492 this->update(idToIndex_.value(link.from()),
04493 ui_->label_indexA,
04494 ui_->label_parentsA,
04495 ui_->label_childrenA,
04496 ui_->label_weightA,
04497 ui_->label_labelA,
04498 ui_->label_stampA,
04499 ui_->graphicsView_A,
04500 ui_->label_idA,
04501 ui_->label_mapA,
04502 ui_->label_poseA,
04503 ui_->label_velA,
04504 ui_->label_calibA,
04505 ui_->label_gpsA,
04506 false);
04507 this->update(idToIndex_.value(link.to()),
04508 ui_->label_indexB,
04509 ui_->label_parentsB,
04510 ui_->label_childrenB,
04511 ui_->label_weightB,
04512 ui_->label_labelB,
04513 ui_->label_stampB,
04514 ui_->graphicsView_B,
04515 ui_->label_idB,
04516 ui_->label_mapB,
04517 ui_->label_poseB,
04518 ui_->label_velB,
04519 ui_->label_calibB,
04520 ui_->label_gpsB,
04521 false);
04522 }
04523
04524 if(constraintsViewer_->isVisible())
04525 {
04526 SensorData dataFrom, dataTo;
04527
04528 if(signatureFrom.id()>0)
04529 {
04530 dataFrom = signatureFrom.sensorData();
04531 }
04532 else
04533 {
04534 dbDriver_->getNodeData(link.from(), dataFrom);
04535 }
04536 dataFrom.uncompressData();
04537 UASSERT(dataFrom.imageRaw().empty() || dataFrom.imageRaw().type()==CV_8UC3 || dataFrom.imageRaw().type() == CV_8UC1);
04538 UASSERT(dataFrom.depthOrRightRaw().empty() || dataFrom.depthOrRightRaw().type()==CV_8UC1 || dataFrom.depthOrRightRaw().type() == CV_16UC1 || dataFrom.depthOrRightRaw().type() == CV_32FC1);
04539
04540 if(signatureTo.id()>0)
04541 {
04542 dataTo = signatureTo.sensorData();
04543 }
04544 else
04545 {
04546 dbDriver_->getNodeData(link.to(), dataTo);
04547 }
04548 dataTo.uncompressData();
04549 UASSERT(dataTo.imageRaw().empty() || dataTo.imageRaw().type()==CV_8UC3 || dataTo.imageRaw().type() == CV_8UC1);
04550 UASSERT(dataTo.depthOrRightRaw().empty() || dataTo.depthOrRightRaw().type()==CV_8UC1 || dataTo.depthOrRightRaw().type() == CV_16UC1 || dataTo.depthOrRightRaw().type() == CV_32FC1);
04551
04552
04553 Transform pose = Transform::getIdentity();
04554 if(ui_->checkBox_odomFrame->isChecked())
04555 {
04556 int m,w;
04557 std::string l;
04558 double s;
04559 Transform p,g;
04560 std::vector<float> v;
04561 GPS gps;
04562 dbDriver_->getNodeInfo(link.from(), p, m, w, l, s, g, v, gps);
04563 if(!p.isNull())
04564 {
04565
04566 float x, y, z, roll, pitch, yaw;
04567 p.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
04568 pose = Transform(0,0,z,roll,pitch,0);
04569 }
04570 }
04571
04572 constraintsViewer_->removeCloud("cloud0");
04573 constraintsViewer_->removeCloud("cloud1");
04574
04575 if(ui_->checkBox_show3Dclouds->isChecked())
04576 {
04577 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom, cloudTo;
04578 pcl::IndicesPtr indicesFrom(new std::vector<int>);
04579 pcl::IndicesPtr indicesTo(new std::vector<int>);
04580 if(!dataFrom.imageRaw().empty() && !dataFrom.depthOrRightRaw().empty())
04581 {
04582 cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, ui_->spinBox_decimation->value(), 0, 0, indicesFrom.get(), ui_->parameters_toolbox->getParameters());
04583 }
04584 if(!dataTo.imageRaw().empty() && !dataTo.depthOrRightRaw().empty())
04585 {
04586 cloudTo=util3d::cloudRGBFromSensorData(dataTo, ui_->spinBox_decimation->value(), 0, 0, indicesTo.get(), ui_->parameters_toolbox->getParameters());
04587 }
04588
04589 if(cloudTo.get() && indicesTo->size())
04590 {
04591 cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t);
04592 }
04593
04594
04595 if(ui_->doubleSpinBox_gainCompensationRadius->value()>0.0 &&
04596 cloudFrom.get() && indicesFrom->size() &&
04597 cloudTo.get() && indicesTo->size())
04598 {
04599 UTimer t;
04600 GainCompensator compensator(ui_->doubleSpinBox_gainCompensationRadius->value());
04601 compensator.feed(cloudFrom, indicesFrom, cloudTo, indicesTo, Transform::getIdentity());
04602 compensator.apply(0, cloudFrom, indicesFrom);
04603 compensator.apply(1, cloudTo, indicesTo);
04604 UINFO("Gain compensation time = %fs", t.ticks());
04605 }
04606
04607 if(cloudFrom.get() && indicesFrom->size())
04608 {
04609 if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
04610 {
04611 cloudFrom = util3d::voxelize(cloudFrom, indicesFrom, ui_->doubleSpinBox_voxelSize->value());
04612 }
04613 constraintsViewer_->addCloud("cloud0", cloudFrom, pose, Qt::red);
04614 }
04615 if(cloudTo.get() && indicesTo->size())
04616 {
04617 if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
04618 {
04619 cloudTo = util3d::voxelize(cloudTo, indicesTo, ui_->doubleSpinBox_voxelSize->value());
04620 }
04621 constraintsViewer_->addCloud("cloud1", cloudTo, pose, Qt::cyan);
04622 }
04623 }
04624
04625 constraintsViewer_->removeCloud("words0");
04626 constraintsViewer_->removeCloud("words1");
04627 if(ui_->checkBox_show3DWords->isChecked())
04628 {
04629 std::list<int> ids;
04630 ids.push_back(link.from());
04631 ids.push_back(link.to());
04632 std::list<Signature*> signatures;
04633 dbDriver_->loadSignatures(ids, signatures);
04634 if(signatures.size() == 2)
04635 {
04636 const Signature * sFrom = signatureFrom.id()>0?&signatureFrom:signatures.front();
04637 const Signature * sTo = signatureTo.id()>0?&signatureTo:signatures.back();
04638 UASSERT(sFrom && sTo);
04639 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom(new pcl::PointCloud<pcl::PointXYZ>);
04640 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo(new pcl::PointCloud<pcl::PointXYZ>);
04641 cloudFrom->resize(sFrom->getWords3().size());
04642 cloudTo->resize(sTo->getWords3().size());
04643 int i=0;
04644 for(std::multimap<int, cv::Point3f>::const_iterator iter=sFrom->getWords3().begin();
04645 iter!=sFrom->getWords3().end();
04646 ++iter)
04647 {
04648 cloudFrom->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
04649 }
04650 i=0;
04651 for(std::multimap<int, cv::Point3f>::const_iterator iter=sTo->getWords3().begin();
04652 iter!=sTo->getWords3().end();
04653 ++iter)
04654 {
04655 cloudTo->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
04656 }
04657
04658 if(cloudFrom->size())
04659 {
04660 cloudFrom = rtabmap::util3d::removeNaNFromPointCloud(cloudFrom);
04661 }
04662 if(cloudTo->size())
04663 {
04664 cloudTo = rtabmap::util3d::removeNaNFromPointCloud(cloudTo);
04665 if(cloudTo->size())
04666 {
04667 cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t);
04668 }
04669 }
04670
04671 if(cloudFrom->size())
04672 {
04673 constraintsViewer_->addCloud("words0", cloudFrom, pose, Qt::red);
04674 }
04675 else
04676 {
04677 UWARN("Empty 3D words for node %d", link.from());
04678 constraintsViewer_->removeCloud("words0");
04679 }
04680 if(cloudTo->size())
04681 {
04682 constraintsViewer_->addCloud("words1", cloudTo, pose, Qt::cyan);
04683 }
04684 else
04685 {
04686 UWARN("Empty 3D words for node %d", link.to());
04687 constraintsViewer_->removeCloud("words1");
04688 }
04689 }
04690 else
04691 {
04692 UERROR("Not found signature %d or %d in RAM", link.from(), link.to());
04693 constraintsViewer_->removeCloud("words0");
04694 constraintsViewer_->removeCloud("words1");
04695 }
04696
04697 for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
04698 {
04699 delete *iter;
04700 }
04701 }
04702
04703 constraintsViewer_->removeCloud("scan2");
04704 constraintsViewer_->removeCloud("scan2normals");
04705 constraintsViewer_->removeGraph("scan2graph");
04706 constraintsViewer_->removeCloud("scan0");
04707 constraintsViewer_->removeCloud("scan1");
04708 if(ui_->checkBox_show2DScans->isChecked())
04709 {
04710
04711 if(link.type() == Link::kLocalSpaceClosure &&
04712 !link.userDataCompressed().empty())
04713 {
04714 std::vector<int> ids;
04715 cv::Mat userData = link.uncompressUserDataConst();
04716 if(userData.type() == CV_8SC1 &&
04717 userData.rows == 1 &&
04718 userData.cols >= 8 &&
04719 userData.at<char>(userData.cols-1) == 0 &&
04720 memcmp(userData.data, "SCANS:", 6) == 0)
04721 {
04722 std::string scansStr = (const char *)userData.data;
04723 UINFO("Detected \"%s\" in links's user data", scansStr.c_str());
04724 if(!scansStr.empty())
04725 {
04726 std::list<std::string> strs = uSplit(scansStr, ':');
04727 if(strs.size() == 2)
04728 {
04729 std::list<std::string> strIds = uSplit(strs.rbegin()->c_str(), ';');
04730 for(std::list<std::string>::iterator iter=strIds.begin(); iter!=strIds.end(); ++iter)
04731 {
04732 ids.push_back(atoi(iter->c_str()));
04733 if(ids.back() == link.from())
04734 {
04735 ids.pop_back();
04736 }
04737 }
04738 }
04739 }
04740 }
04741 if(ids.size())
04742 {
04743
04744
04745
04746 std::map<int, rtabmap::Transform> poses;
04747 for(unsigned int i=0; i<ids.size(); ++i)
04748 {
04749 if(uContains(odomPoses_, ids[i]))
04750 {
04751 poses.insert(*odomPoses_.find(ids[i]));
04752 }
04753 else
04754 {
04755 UERROR("Not found %d node!", ids[i]);
04756 }
04757 }
04758 if(poses.size())
04759 {
04760 Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
04761
04762 UASSERT(uContains(poses, link.to()));
04763 std::map<int, rtabmap::Transform> posesOut;
04764 std::multimap<int, rtabmap::Link> linksOut;
04765 optimizer->getConnectedGraph(
04766 link.to(),
04767 poses,
04768 updateLinksWithModifications(links_),
04769 posesOut,
04770 linksOut);
04771
04772 if(poses.size() != posesOut.size())
04773 {
04774 UWARN("Scan poses input and output are different! %d vs %d", (int)poses.size(), (int)posesOut.size());
04775 UWARN("Input poses: ");
04776 for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
04777 {
04778 UWARN(" %d", iter->first);
04779 }
04780 UWARN("Input links: ");
04781 std::multimap<int, Link> modifiedLinks = updateLinksWithModifications(links_);
04782 for(std::multimap<int, Link>::iterator iter=modifiedLinks.begin(); iter!=modifiedLinks.end(); ++iter)
04783 {
04784 UWARN(" %d->%d", iter->second.from(), iter->second.to());
04785 }
04786 }
04787
04788 QTime time;
04789 time.start();
04790 std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(link.to(), posesOut, linksOut);
04791 delete optimizer;
04792
04793
04794 Transform u = t * finalPoses.at(link.to()).inverse();
04795 pcl::PointCloud<pcl::PointXYZ>::Ptr assembledScans(new pcl::PointCloud<pcl::PointXYZ>);
04796 pcl::PointCloud<pcl::PointNormal>::Ptr assembledNormalScans(new pcl::PointCloud<pcl::PointNormal>);
04797 pcl::PointCloud<pcl::PointXYZ>::Ptr graph(new pcl::PointCloud<pcl::PointXYZ>);
04798 for(std::map<int, Transform>::iterator iter=finalPoses.begin(); iter!=finalPoses.end(); ++iter)
04799 {
04800 iter->second = u * iter->second;
04801 if(iter->first != link.to())
04802 {
04803
04804 SensorData data;
04805 dbDriver_->getNodeData(iter->first, data);
04806 LaserScan scan;
04807 data.uncompressDataConst(0, 0, &scan, 0);
04808 if(!scan.isEmpty())
04809 {
04810 if(scan.hasNormals())
04811 {
04812 *assembledNormalScans += *util3d::laserScanToPointCloudNormal(scan, iter->second*scan.localTransform());
04813 }
04814 else
04815 {
04816 *assembledScans += *util3d::laserScanToPointCloud(scan, iter->second*scan.localTransform());
04817 }
04818 }
04819 }
04820 graph->push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
04821 }
04822
04823 if(assembledNormalScans->size())
04824 {
04825 constraintsViewer_->addCloud("scan2normals", assembledNormalScans, pose, Qt::cyan);
04826 constraintsViewer_->setCloudColorIndex("scan2normals", 2);
04827 }
04828 if(assembledScans->size())
04829 {
04830 constraintsViewer_->addCloud("scan2", assembledScans, pose, Qt::cyan);
04831 constraintsViewer_->setCloudColorIndex("scan2", 2);
04832 }
04833 if(graph->size())
04834 {
04835 constraintsViewer_->addOrUpdateGraph("scan2graph", graph, Qt::cyan);
04836 }
04837 }
04838 }
04839 }
04840
04841
04842 constraintsViewer_->removeCloud("scan0");
04843 constraintsViewer_->removeCloud("scan1");
04844 if(!dataFrom.laserScanRaw().isEmpty())
04845 {
04846 if(dataFrom.laserScanRaw().hasNormals())
04847 {
04848 pcl::PointCloud<pcl::PointNormal>::Ptr scan;
04849 scan = rtabmap::util3d::laserScanToPointCloudNormal(dataFrom.laserScanRaw(), dataFrom.laserScanRaw().localTransform());
04850 constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
04851 constraintsViewer_->setCloudColorIndex("scan0", 2);
04852 }
04853 else
04854 {
04855 pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
04856 scan = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw(), dataFrom.laserScanRaw().localTransform());
04857 constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
04858 constraintsViewer_->setCloudColorIndex("scan0", 2);
04859 }
04860 }
04861 if(!dataTo.laserScanRaw().isEmpty())
04862 {
04863 if(dataTo.laserScanRaw().hasNormals())
04864 {
04865 pcl::PointCloud<pcl::PointNormal>::Ptr scan;
04866 scan = rtabmap::util3d::laserScanToPointCloudNormal(dataTo.laserScanRaw(), t*dataTo.laserScanRaw().localTransform());
04867 constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
04868 constraintsViewer_->setCloudColorIndex("scan1", 2);
04869 }
04870 else
04871 {
04872 pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
04873 scan = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw(), t*dataTo.laserScanRaw().localTransform());
04874 constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
04875 constraintsViewer_->setCloudColorIndex("scan1", 2);
04876 }
04877 }
04878 }
04879
04880
04881 constraintsViewer_->addOrUpdateCoordinate("from_coordinate", pose, 0.2);
04882 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
04883 constraintsViewer_->addOrUpdateCoordinate("to_coordinate", pose*t, 0.2);
04884 constraintsViewer_->removeCoordinate("to_coordinate_gt");
04885 if(uContains(groundTruthPoses_, link.from()) && uContains(groundTruthPoses_, link.to()))
04886 {
04887 constraintsViewer_->addOrUpdateCoordinate("to_coordinate_gt",
04888 pose*(groundTruthPoses_.at(link.from()).inverse()*groundTruthPoses_.at(link.to())), 0.1);
04889 }
04890 #endif
04891
04892 constraintsViewer_->clearTrajectory();
04893
04894 constraintsViewer_->update();
04895 }
04896
04897
04898 updateConstraintButtons();
04899 }
04900
04901 void DatabaseViewer::updateConstraintButtons()
04902 {
04903 ui_->pushButton_refine->setEnabled(false);
04904 ui_->pushButton_reset->setEnabled(false);
04905 ui_->pushButton_add->setEnabled(false);
04906 ui_->pushButton_reject->setEnabled(false);
04907
04908 int from = ids_.at(ui_->horizontalSlider_A->value());
04909 int to = ids_.at(ui_->horizontalSlider_B->value());
04910 if(from!=to && from && to && odomPoses_.find(from) != odomPoses_.end() && odomPoses_.find(to) != odomPoses_.end())
04911 {
04912 if((!containsLink(links_, from ,to) && !containsLink(linksAdded_, from ,to)) ||
04913 containsLink(linksRemoved_, from ,to))
04914 {
04915 ui_->pushButton_add->setEnabled(true);
04916 }
04917 }
04918
04919 Link currentLink = findActiveLink(from ,to);
04920
04921 if(currentLink.isValid() &&
04922 ((currentLink.from() == from && currentLink.to() == to) || (currentLink.from() == to && currentLink.to() == from)))
04923 {
04924 if(!containsLink(linksRemoved_, from ,to))
04925 {
04926 ui_->pushButton_reject->setEnabled(
04927 currentLink.type() != Link::kNeighbor &&
04928 currentLink.type() != Link::kNeighborMerged);
04929 }
04930
04931
04932 bool modified = false;
04933 std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, currentLink.from(), currentLink.to());
04934 if(iter != linksRefined_.end())
04935 {
04936 currentLink = iter->second;
04937 ui_->pushButton_reset->setEnabled(true);
04938 modified = true;
04939 }
04940 if(!modified)
04941 {
04942 ui_->pushButton_reset->setEnabled(false);
04943 }
04944 ui_->pushButton_refine->setEnabled(true);
04945 }
04946 }
04947
04948 void DatabaseViewer::sliderIterationsValueChanged(int value)
04949 {
04950 if(dbDriver_ && value >=0 && value < (int)graphes_.size())
04951 {
04952 std::map<int, rtabmap::Transform> graph = uValueAt(graphes_, value);
04953
04954 std::map<int, Transform> refPoses = groundTruthPoses_;
04955 if(refPoses.empty())
04956 {
04957 refPoses = gpsPoses_;
04958 }
04959
04960
04961 if(refPoses.size())
04962 {
04963
04964 float length = graph::computePathLength(graph);
04965 if(refPoses.size() == graph.size() && length >= 100.0f)
04966 {
04967 float t_err = 0.0f;
04968 float r_err = 0.0f;
04969 graph::calcKittiSequenceErrors(uValues(refPoses), uValues(graph), t_err, r_err);
04970 UINFO("KITTI t_err = %f %%", t_err);
04971 UINFO("KITTI r_err = %f deg/m", r_err);
04972 }
04973
04974 float translational_rmse = 0.0f;
04975 float translational_mean = 0.0f;
04976 float translational_median = 0.0f;
04977 float translational_std = 0.0f;
04978 float translational_min = 0.0f;
04979 float translational_max = 0.0f;
04980 float rotational_rmse = 0.0f;
04981 float rotational_mean = 0.0f;
04982 float rotational_median = 0.0f;
04983 float rotational_std = 0.0f;
04984 float rotational_min = 0.0f;
04985 float rotational_max = 0.0f;
04986
04987 Transform gtToMap = graph::calcRMSE(
04988 refPoses,
04989 graph,
04990 translational_rmse,
04991 translational_mean,
04992 translational_median,
04993 translational_std,
04994 translational_min,
04995 translational_max,
04996 rotational_rmse,
04997 rotational_mean,
04998 rotational_median,
04999 rotational_std,
05000 rotational_min,
05001 rotational_max);
05002
05003
05004 ui_->label_rmse->setNum(translational_rmse);
05005 UINFO("translational_rmse=%f", translational_rmse);
05006 UINFO("translational_mean=%f", translational_mean);
05007 UINFO("translational_median=%f", translational_median);
05008 UINFO("translational_std=%f", translational_std);
05009 UINFO("translational_min=%f", translational_min);
05010 UINFO("translational_max=%f", translational_max);
05011
05012 UINFO("rotational_rmse=%f", rotational_rmse);
05013 UINFO("rotational_mean=%f", rotational_mean);
05014 UINFO("rotational_median=%f", rotational_median);
05015 UINFO("rotational_std=%f", rotational_std);
05016 UINFO("rotational_min=%f", rotational_min);
05017 UINFO("rotational_max=%f", rotational_max);
05018
05019 if(ui_->checkBox_alignPosesWithGroundTruth->isChecked() && !gtToMap.isIdentity())
05020 {
05021 for(std::map<int, Transform>::iterator iter=graph.begin(); iter!=graph.end(); ++iter)
05022 {
05023 iter->second = gtToMap * iter->second;
05024 }
05025 }
05026 }
05027
05028 std::map<int, rtabmap::Transform> graphFiltered;
05029 if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty())
05030 {
05031 graphFiltered = groundTruthPoses_;
05032 }
05033 else
05034 {
05035 graphFiltered = graph;
05036 }
05037 if(ui_->groupBox_posefiltering->isChecked())
05038 {
05039 graphFiltered = graph::radiusPosesFiltering(graph,
05040 ui_->doubleSpinBox_posefilteringRadius->value(),
05041 ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
05042 }
05043 std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > localMaps;
05044 std::map<int, std::pair<float, cv::Point3f> > localMapsInfo;
05045 #ifdef RTABMAP_OCTOMAP
05046 if(octomap_)
05047 {
05048 delete octomap_;
05049 octomap_ = 0;
05050 }
05051 #endif
05052 if(ui_->dockWidget_graphView->isVisible() || ui_->dockWidget_occupancyGridView->isVisible())
05053 {
05054
05055 UINFO("Update local maps list...");
05056 std::vector<int> ids = uKeys(graphFiltered);
05057 for(unsigned int i=0; i<ids.size(); ++i)
05058 {
05059 if(generatedLocalMaps_.find(ids[i]) != generatedLocalMaps_.end())
05060 {
05061 localMaps.insert(*generatedLocalMaps_.find(ids[i]));
05062 localMapsInfo.insert(*generatedLocalMapsInfo_.find(ids[i]));
05063 }
05064 else if(localMaps_.find(ids[i]) != localMaps_.end())
05065 {
05066 if(!localMaps_.find(ids[i])->second.first.first.empty() || !localMaps_.find(ids[i])->second.first.second.empty())
05067 {
05068 localMaps.insert(*localMaps_.find(ids.at(i)));
05069 localMapsInfo.insert(*localMapsInfo_.find(ids[i]));
05070 }
05071 }
05072 else
05073 {
05074 SensorData data;
05075 dbDriver_->getNodeData(ids.at(i), data);
05076 cv::Mat ground, obstacles, empty;
05077 data.uncompressData(0, 0, 0, 0, &ground, &obstacles, &empty);
05078 localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(std::make_pair(ground, obstacles), empty)));
05079 localMapsInfo_.insert(std::make_pair(ids.at(i), std::make_pair(data.gridCellSize(), data.gridViewPoint())));
05080 if(!ground.empty() || !obstacles.empty())
05081 {
05082 localMaps.insert(std::make_pair(ids.at(i), std::make_pair(std::make_pair(ground, obstacles), empty)));
05083 localMapsInfo.insert(std::make_pair(ids.at(i), std::make_pair(data.gridCellSize(), data.gridViewPoint())));
05084 }
05085 }
05086 }
05087
05088 for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end();)
05089 {
05090 if(graphFiltered.find(iter->first) == graphFiltered.end())
05091 {
05092 localMapsInfo_.erase(iter->first);
05093 localMaps_.erase(iter++);
05094 }
05095 else
05096 {
05097 ++iter;
05098 }
05099 }
05100 UINFO("Update local maps list... done (%d local maps, graph size=%d)", (int)localMaps.size(), (int)graph.size());
05101 }
05102
05103 ParametersMap parameters = ui_->parameters_toolbox->getParameters();
05104 float cellSize = Parameters::defaultGridCellSize();
05105 Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize);
05106
05107 ui_->graphViewer->updateGTGraph(groundTruthPoses_);
05108 ui_->graphViewer->updateGPSGraph(gpsPoses_, gpsValues_);
05109 ui_->graphViewer->updateGraph(graph, graphLinks_, mapIds_);
05110 ui_->graphViewer->clearMap();
05111 occupancyGridViewer_->clear();
05112 if(graph.size() && localMaps.size() &&
05113 (ui_->graphViewer->isGridMapVisible() || ui_->dockWidget_occupancyGridView->isVisible()))
05114 {
05115 QTime time;
05116 time.start();
05117
05118 #ifdef RTABMAP_OCTOMAP
05119 if(ui_->checkBox_octomap->isChecked())
05120 {
05121 octomap_ = new OctoMap(parameters);
05122 bool updateAborted = false;
05123 for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter)
05124 {
05125 if(iter->second.first.first.channels() == 2 || iter->second.first.second.channels() == 2)
05126 {
05127 QMessageBox::warning(this, tr(""),
05128 tr("Some local occupancy grids are 2D, but OctoMap requires 3D local "
05129 "occupancy grids. Uncheck OctoMap under GUI parameters or generate "
05130 "3D local occupancy grids (\"Grid/3D\" core parameter)."));
05131 updateAborted = true;
05132 break;
05133 }
05134 octomap_->addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second, localMapsInfo.at(iter->first).second);
05135 }
05136 if(!updateAborted)
05137 {
05138 octomap_->update(graphFiltered);
05139 }
05140 }
05141 #endif
05142
05143
05144 if((ui_->dockWidget_graphView->isVisible() && ui_->graphViewer->isGridMapVisible()) ||
05145 (ui_->dockWidget_occupancyGridView->isVisible() && ui_->checkBox_grid_2d->isChecked()))
05146 {
05147 bool eroded = Parameters::defaultGridGlobalEroded();
05148 Parameters::parse(parameters, Parameters::kGridGlobalEroded(), eroded);
05149 float xMin, yMin;
05150 cv::Mat map;
05151
05152 #ifdef RTABMAP_OCTOMAP
05153 if(ui_->checkBox_octomap->isChecked())
05154 {
05155 map = octomap_->createProjectionMap(xMin, yMin, cellSize, 0, ui_->spinBox_grid_depth->value());
05156 }
05157 else
05158 #endif
05159 {
05160 if(eroded)
05161 {
05162 uInsert(parameters, ParametersPair(Parameters::kGridGlobalEroded(), "true"));
05163 }
05164 OccupancyGrid grid(parameters);
05165 grid.setCellSize(cellSize);
05166 for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter)
05167 {
05168 grid.addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second);
05169 }
05170 grid.update(graphFiltered);
05171 map = grid.getMap(xMin, yMin);
05172 }
05173
05174 ui_->label_timeGrid->setNum(double(time.elapsed())/1000.0);
05175
05176 if(!map.empty())
05177 {
05178 cv::Mat map8U = rtabmap::util3d::convertMap2Image8U(map);
05179 if(ui_->dockWidget_graphView->isVisible() && ui_->graphViewer->isGridMapVisible())
05180 {
05181 ui_->graphViewer->updateMap(map8U, cellSize, xMin, yMin);
05182 }
05183 if(ui_->dockWidget_occupancyGridView->isVisible() && ui_->checkBox_grid_2d->isChecked())
05184 {
05185 occupancyGridViewer_->addOccupancyGridMap(map8U, cellSize, xMin, yMin, 1.0f);
05186 occupancyGridViewer_->update();
05187 }
05188 }
05189 }
05190
05191
05192 if(ui_->dockWidget_occupancyGridView->isVisible())
05193 {
05194 #ifdef RTABMAP_OCTOMAP
05195 if(ui_->checkBox_octomap->isChecked())
05196 {
05197 updateOctomapView();
05198 }
05199 else
05200 #endif
05201 {
05202 pcl::PointCloud<pcl::PointXYZ>::Ptr groundXYZ(new pcl::PointCloud<pcl::PointXYZ>);
05203 pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesXYZ(new pcl::PointCloud<pcl::PointXYZ>);
05204 pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCellsXYZ(new pcl::PointCloud<pcl::PointXYZ>);
05205 pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
05206 pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
05207 pcl::PointCloud<pcl::PointXYZRGB>::Ptr emptyCellsRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
05208
05209 for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter)
05210 {
05211 Transform pose = graphFiltered.at(iter->first);
05212 float x,y,z,roll,pitch,yaw;
05213 pose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
05214 Transform pose2d(x,y, 0, 0, 0, yaw);
05215 if(!iter->second.first.first.empty())
05216 {
05217 if(iter->second.first.first.channels() == 4)
05218 {
05219 *groundRGB += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(iter->second.first.first), pose);
05220 }
05221 else
05222 {
05223 *groundXYZ += *util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(iter->second.first.first), iter->second.first.first.channels()==2?pose2d:pose);
05224 }
05225 }
05226 if(!iter->second.first.second.empty())
05227 {
05228 if(iter->second.first.second.channels() == 4)
05229 {
05230 *obstaclesRGB += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(iter->second.first.second), pose);
05231 }
05232 else
05233 {
05234 *obstaclesXYZ += *util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(iter->second.first.second), iter->second.first.second.channels()==2?pose2d:pose);
05235 }
05236 }
05237 if(ui_->checkBox_grid_empty->isChecked())
05238 {
05239 if(!iter->second.second.empty())
05240 {
05241 if(iter->second.second.channels() == 4)
05242 {
05243 *emptyCellsRGB += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(iter->second.second), pose);
05244 }
05245 else
05246 {
05247 *emptyCellsXYZ += *util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(iter->second.second), iter->second.second.channels()==2?pose2d:pose);
05248 }
05249 }
05250 }
05251 }
05252
05253 if(groundRGB->size())
05254 {
05255 groundRGB = util3d::voxelize(groundRGB, cellSize);
05256 occupancyGridViewer_->addCloud("groundRGB",
05257 groundRGB,
05258 Transform::getIdentity(),
05259 QColor(ui_->lineEdit_groundColor->text()));
05260 occupancyGridViewer_->setCloudPointSize("groundRGB", 5);
05261 }
05262 if(groundXYZ->size())
05263 {
05264 groundXYZ = util3d::voxelize(groundXYZ, cellSize);
05265 occupancyGridViewer_->addCloud("groundXYZ",
05266 groundXYZ,
05267 Transform::getIdentity(),
05268 QColor(ui_->lineEdit_groundColor->text()));
05269 occupancyGridViewer_->setCloudPointSize("groundXYZ", 5);
05270 }
05271 if(obstaclesRGB->size())
05272 {
05273 obstaclesRGB = util3d::voxelize(obstaclesRGB, cellSize);
05274 occupancyGridViewer_->addCloud("obstaclesRGB",
05275 obstaclesRGB,
05276 Transform::getIdentity(),
05277 QColor(ui_->lineEdit_obstacleColor->text()));
05278 occupancyGridViewer_->setCloudPointSize("obstaclesRGB", 5);
05279 }
05280 if(obstaclesXYZ->size())
05281 {
05282 obstaclesXYZ = util3d::voxelize(obstaclesXYZ, cellSize);
05283 occupancyGridViewer_->addCloud("obstaclesXYZ",
05284 obstaclesXYZ,
05285 Transform::getIdentity(),
05286 QColor(ui_->lineEdit_obstacleColor->text()));
05287 occupancyGridViewer_->setCloudPointSize("obstaclesXYZ", 5);
05288 }
05289 if(emptyCellsRGB->size())
05290 {
05291 emptyCellsRGB = util3d::voxelize(emptyCellsRGB, cellSize);
05292 occupancyGridViewer_->addCloud("emptyCellsRGB",
05293 emptyCellsRGB,
05294 Transform::getIdentity(),
05295 QColor(ui_->lineEdit_emptyColor->text()));
05296 occupancyGridViewer_->setCloudPointSize("emptyCellsRGB", 5);
05297 occupancyGridViewer_->setCloudOpacity("emptyCellsRGB", 0.5);
05298 }
05299 if(emptyCellsXYZ->size())
05300 {
05301 emptyCellsXYZ = util3d::voxelize(emptyCellsXYZ, cellSize);
05302 occupancyGridViewer_->addCloud("emptyCellsXYZ",
05303 emptyCellsXYZ,
05304 Transform::getIdentity(),
05305 QColor(ui_->lineEdit_emptyColor->text()));
05306 occupancyGridViewer_->setCloudPointSize("emptyCellsXYZ", 5);
05307 occupancyGridViewer_->setCloudOpacity("emptyCellsXYZ", 0.5);
05308 }
05309 occupancyGridViewer_->update();
05310 }
05311 }
05312 }
05313 ui_->graphViewer->fitInView(ui_->graphViewer->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
05314 ui_->graphViewer->update();
05315 ui_->label_iterations->setNum(value);
05316
05317
05318 float length = 0.0f;
05319 for(std::multimap<int, rtabmap::Link>::const_iterator iter=graphLinks_.begin(); iter!=graphLinks_.end(); ++iter)
05320 {
05321 std::map<int, rtabmap::Transform>::const_iterator jterA = graph.find(iter->first);
05322 std::map<int, rtabmap::Transform>::const_iterator jterB = graph.find(iter->second.to());
05323 if(jterA != graph.end() && jterB != graph.end())
05324 {
05325 const rtabmap::Transform & poseA = jterA->second;
05326 const rtabmap::Transform & poseB = jterB->second;
05327 if(iter->second.type() == rtabmap::Link::kNeighbor ||
05328 iter->second.type() == rtabmap::Link::kNeighborMerged)
05329 {
05330 Eigen::Vector3f vA, vB;
05331 float x,y,z;
05332 poseA.getTranslation(x,y,z);
05333 vA[0] = x; vA[1] = y; vA[2] = z;
05334 poseB.getTranslation(x,y,z);
05335 vB[0] = x; vB[1] = y; vB[2] = z;
05336 length += (vB - vA).norm();
05337 }
05338 }
05339 }
05340 ui_->label_pathLength->setNum(length);
05341 }
05342 }
05343 void DatabaseViewer::updateGraphView()
05344 {
05345 ui_->label_loopClosures->clear();
05346 ui_->label_poses->clear();
05347 ui_->label_rmse->clear();
05348
05349 if(odomPoses_.size())
05350 {
05351 int fromId = ui_->spinBox_optimizationsFrom->value();
05352 if(!uContains(odomPoses_, fromId))
05353 {
05354 QMessageBox::warning(this, tr(""), tr("Graph optimization from id (%1) for which node is not linked to graph.\n Minimum=%2, Maximum=%3")
05355 .arg(fromId)
05356 .arg(odomPoses_.begin()->first)
05357 .arg(odomPoses_.rbegin()->first));
05358 return;
05359 }
05360
05361 std::map<int, Transform> optimizedGraphGuess;
05362 if(graphes_.size() && useLastOptimizedGraphAsGuess_)
05363 {
05364 optimizedGraphGuess = lastOptimizedGraph_;
05365 }
05366
05367 graphes_.clear();
05368 graphLinks_.clear();
05369
05370 std::map<int, rtabmap::Transform> poses = odomPoses_;
05371 if(ui_->checkBox_wmState->isChecked() && uContains(wmStates_, fromId))
05372 {
05373 std::map<int, rtabmap::Transform> wmPoses;
05374 std::vector<int> & wmState = wmStates_.at(fromId);
05375 for(unsigned int i=0; i<wmState.size(); ++i)
05376 {
05377 std::map<int, rtabmap::Transform>::iterator iter = poses.find(wmState[i]);
05378 if(iter!=poses.end())
05379 {
05380 wmPoses.insert(*iter);
05381 }
05382 }
05383 if(!wmPoses.empty())
05384 {
05385 poses = wmPoses;
05386 }
05387 else
05388 {
05389 UWARN("Empty WM poses!? Ignoring WM state... (root id=%d, wmState=%d)", fromId, wmState.size());
05390 }
05391 }
05392
05393
05394 if(!ui_->checkBox_spanAllMaps->isChecked() && uContains(mapIds_, fromId) && mapIds_.at(fromId) >= 0)
05395 {
05396 int currentMapId = mapIds_.at(fromId);
05397 for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end();)
05398 {
05399 if(!uContains(mapIds_, iter->first) ||
05400 mapIds_.at(iter->first) != currentMapId)
05401 {
05402 poses.erase(iter++);
05403 }
05404 else
05405 {
05406 ++iter;
05407 }
05408 }
05409 }
05410
05411 ui_->menuExport_poses->setEnabled(true);
05412 std::multimap<int, rtabmap::Link> links = links_;
05413 loopLinks_.clear();
05414
05415
05416 if(!ui_->checkBox_spanAllMaps->isChecked() && uContains(mapIds_, fromId) && mapIds_.at(fromId) >= 0)
05417 {
05418 int currentMapId = mapIds_.at(fromId);
05419 for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end();)
05420 {
05421 if(!uContains(mapIds_, iter->second.from()) ||
05422 !uContains(mapIds_, iter->second.to()) ||
05423 mapIds_.at(iter->second.from()) != currentMapId ||
05424 mapIds_.at(iter->second.to()) != currentMapId)
05425 {
05426 links.erase(iter++);
05427 }
05428 else
05429 {
05430 ++iter;
05431 }
05432 }
05433 }
05434
05435 links = updateLinksWithModifications(links);
05436 if(ui_->checkBox_ignorePoseCorrection->isChecked())
05437 {
05438 for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
05439 {
05440 if(iter->second.type() == Link::kNeighbor ||
05441 iter->second.type() == Link::kNeighborMerged)
05442 {
05443 Transform poseFrom = uValue(poses, iter->second.from(), Transform());
05444 Transform poseTo = uValue(poses, iter->second.to(), Transform());
05445 if(!poseFrom.isNull() && !poseTo.isNull())
05446 {
05447
05448 iter->second = Link(
05449 iter->second.from(),
05450 iter->second.to(),
05451 iter->second.type(),
05452 poseFrom.inverse() * poseTo);
05453 }
05454 }
05455 }
05456 }
05457
05458
05459 int totalNeighbor = 0;
05460 int totalNeighborMerged = 0;
05461 int totalGlobal = 0;
05462 int totalLocalTime = 0;
05463 int totalLocalSpace = 0;
05464 int totalUser = 0;
05465 int totalPriors = 0;
05466 for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end();)
05467 {
05468 if(iter->second.type() == Link::kNeighbor)
05469 {
05470 ++totalNeighbor;
05471 }
05472 else if(iter->second.type() == Link::kNeighborMerged)
05473 {
05474 ++totalNeighborMerged;
05475 }
05476 else if(iter->second.type() == Link::kGlobalClosure)
05477 {
05478 if(ui_->checkBox_ignoreGlobalLoop->isChecked())
05479 {
05480 links.erase(iter++);
05481 continue;
05482 }
05483 loopLinks_.push_back(iter->second);
05484 ++totalGlobal;
05485 }
05486 else if(iter->second.type() == Link::kLocalSpaceClosure)
05487 {
05488 if(ui_->checkBox_ignoreLocalLoopSpace->isChecked())
05489 {
05490 links.erase(iter++);
05491 continue;
05492 }
05493 loopLinks_.push_back(iter->second);
05494 ++totalLocalSpace;
05495 }
05496 else if(iter->second.type() == Link::kLocalTimeClosure)
05497 {
05498 if(ui_->checkBox_ignoreLocalLoopTime->isChecked())
05499 {
05500 links.erase(iter++);
05501 continue;
05502 }
05503 loopLinks_.push_back(iter->second);
05504 ++totalLocalTime;
05505 }
05506 else if(iter->second.type() == Link::kUserClosure)
05507 {
05508 if(ui_->checkBox_ignoreUserLoop->isChecked())
05509 {
05510 links.erase(iter++);
05511 continue;
05512 }
05513 loopLinks_.push_back(iter->second);
05514 ++totalUser;
05515 }
05516 else if(iter->second.type() == Link::kPosePrior)
05517 {
05518 ++totalPriors;
05519 }
05520 else
05521 {
05522 loopLinks_.push_back(iter->second);
05523 }
05524 ++iter;
05525 }
05526 updateLoopClosuresSlider();
05527
05528 ui_->label_loopClosures->setText(tr("(%1, %2, %3, %4, %5, %6, %7)")
05529 .arg(totalNeighbor)
05530 .arg(totalNeighborMerged)
05531 .arg(totalGlobal)
05532 .arg(totalLocalSpace)
05533 .arg(totalLocalTime)
05534 .arg(totalUser)
05535 .arg(totalPriors));
05536
05537
05538 if(ui_->checkBox_ignoreIntermediateNodes->isVisible() &&
05539 ui_->checkBox_ignoreIntermediateNodes->isChecked())
05540 {
05541 for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
05542 {
05543 if(iter->second.type() == Link::kNeighbor ||
05544 iter->second.type() == Link::kNeighborMerged)
05545 {
05546 Link link = iter->second;
05547 while(uContains(weights_, link.to()) && weights_.at(link.to()) < 0)
05548 {
05549 std::multimap<int, Link>::iterator uter = links.find(link.to());
05550 if(uter != links.end())
05551 {
05552 UASSERT(links.count(link.to()) == 1);
05553 poses.erase(link.to());
05554 link = link.merge(uter->second, uter->second.type());
05555 links.erase(uter);
05556 }
05557 else
05558 {
05559 break;
05560 }
05561 }
05562
05563 iter->second = link;
05564 }
05565 }
05566 }
05567
05568 graphes_.push_back(poses);
05569
05570 Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
05571
05572 std::map<int, rtabmap::Transform> posesOut;
05573 std::multimap<int, rtabmap::Link> linksOut;
05574 UINFO("Get connected graph from %d (%d poses, %d links)", fromId, (int)poses.size(), (int)links.size());
05575 optimizer->getConnectedGraph(
05576 fromId,
05577 poses,
05578 links,
05579 posesOut,
05580 linksOut,
05581 ui_->spinBox_optimizationDepth->value());
05582 if(optimizedGraphGuess.size() == posesOut.size())
05583 {
05584 bool identical=true;
05585 for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
05586 {
05587 if(!uContains(optimizedGraphGuess, iter->first))
05588 {
05589 identical = false;
05590 break;
05591 }
05592 }
05593 if(identical)
05594 {
05595 posesOut = optimizedGraphGuess;
05596 }
05597 }
05598 UINFO("Connected graph of %d poses and %d links", (int)posesOut.size(), (int)linksOut.size());
05599 QTime time;
05600 time.start();
05601 std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(fromId, posesOut, linksOut, ui_->checkBox_iterativeOptimization->isChecked()?&graphes_:0);
05602 ui_->label_timeOptimization->setNum(double(time.elapsed())/1000.0);
05603 graphes_.push_back(finalPoses);
05604 graphLinks_ = linksOut;
05605 ui_->label_poses->setNum((int)finalPoses.size());
05606 if(posesOut.size() && finalPoses.empty())
05607 {
05608 UWARN("Optimization failed, trying incremental optimization instead... this may take a while (poses=%d, links=%d).", (int)posesOut.size(), (int)linksOut.size());
05609 finalPoses = optimizer->optimizeIncremental(fromId, posesOut, linksOut, &graphes_);
05610
05611 if(finalPoses.empty())
05612 {
05613 UWARN("Incremental optimization also failed.");
05614 if(!optimizer->isCovarianceIgnored() || optimizer->type() != Optimizer::kTypeTORO)
05615 {
05616 QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors. "
05617 "Give it a try with %1=0 and %2=true.").arg(Parameters::kOptimizerStrategy().c_str()).arg(Parameters::kOptimizerVarianceIgnored().c_str()));
05618 }
05619 else
05620 {
05621 QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors."));
05622 }
05623 }
05624 else
05625 {
05626 UWARN("Incremental optimization succeeded!");
05627 QMessageBox::information(this, tr("Incremental optimization succeeded!"), tr("Graph optimization has failed but "
05628 "incremental optimization succeeded. Next optimizations will use the current "
05629 "best optimized poses as first guess instead of odometry poses."));
05630 useLastOptimizedGraphAsGuess_ = true;
05631 lastOptimizedGraph_ = finalPoses;
05632
05633 }
05634 }
05635 delete optimizer;
05636 }
05637 if(graphes_.size())
05638 {
05639 if(ui_->doubleSpinBox_optimizationScale->value()!=-1.0)
05640 {
05641
05642 for(std::list<std::map<int, Transform> >::iterator iter=graphes_.begin(); iter!=graphes_.end(); ++iter)
05643 {
05644 for(std::map<int, Transform>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
05645 {
05646 jter->second = jter->second.clone();
05647 jter->second.x() *= ui_->doubleSpinBox_optimizationScale->value();
05648 jter->second.y() *= ui_->doubleSpinBox_optimizationScale->value();
05649 jter->second.z() *= ui_->doubleSpinBox_optimizationScale->value();
05650 }
05651 }
05652 }
05653
05654 ui_->horizontalSlider_iterations->setMaximum((int)graphes_.size()-1);
05655 ui_->horizontalSlider_iterations->setValue((int)graphes_.size()-1);
05656 ui_->horizontalSlider_iterations->setEnabled(true);
05657 ui_->spinBox_optimizationsFrom->setEnabled(true);
05658 sliderIterationsValueChanged((int)graphes_.size()-1);
05659 }
05660 else
05661 {
05662 ui_->horizontalSlider_iterations->setEnabled(false);
05663 ui_->spinBox_optimizationsFrom->setEnabled(false);
05664 }
05665 }
05666
05667 void DatabaseViewer::updateGrid()
05668 {
05669 if(sender() == ui_->checkBox_grid_2d && !ui_->checkBox_grid_2d->isChecked())
05670 {
05671
05672 occupancyGridViewer_->removeOccupancyGridMap();
05673 occupancyGridViewer_->update();
05674 }
05675 else
05676 {
05677 ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked());
05678 ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
05679 ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
05680 ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
05681 ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
05682 ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
05683
05684 update3dView();
05685 updateGraphView();
05686 }
05687 }
05688
05689 void DatabaseViewer::updateOctomapView()
05690 {
05691 #ifdef RTABMAP_OCTOMAP
05692 ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked());
05693 ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
05694 ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
05695 ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
05696 ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
05697 ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
05698
05699 if(ui_->checkBox_octomap->isChecked())
05700 {
05701 if(octomap_)
05702 {
05703 occupancyGridViewer_->removeOctomap();
05704 occupancyGridViewer_->removeCloud("octomap_obstacles");
05705 occupancyGridViewer_->removeCloud("octomap_empty");
05706 if(ui_->comboBox_octomap_rendering_type->currentIndex()>0)
05707 {
05708 occupancyGridViewer_->addOctomap(octomap_, ui_->spinBox_grid_depth->value(), ui_->comboBox_octomap_rendering_type->currentIndex()>1);
05709 }
05710 else
05711 {
05712 pcl::IndicesPtr obstacles(new std::vector<int>);
05713 pcl::IndicesPtr empty(new std::vector<int>);
05714 pcl::IndicesPtr ground(new std::vector<int>);
05715 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(ui_->spinBox_grid_depth->value(), obstacles.get(), empty.get(), ground.get());
05716
05717 if(octomap_->hasColor())
05718 {
05719 pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
05720 pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
05721 occupancyGridViewer_->addCloud("octomap_obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
05722 occupancyGridViewer_->setCloudPointSize("octomap_obstacles", 5);
05723
05724 pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
05725 pcl::copyPointCloud(*cloud, *ground, *groundCloud);
05726 occupancyGridViewer_->addCloud("octomap_ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
05727 occupancyGridViewer_->setCloudPointSize("octomap_ground", 5);
05728 }
05729 else
05730 {
05731 pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
05732 pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
05733 occupancyGridViewer_->addCloud("octomap_obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
05734 occupancyGridViewer_->setCloudPointSize("octomap_obstacles", 5);
05735
05736 pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
05737 pcl::copyPointCloud(*cloud, *ground, *groundCloud);
05738 occupancyGridViewer_->addCloud("octomap_ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
05739 occupancyGridViewer_->setCloudPointSize("octomap_ground", 5);
05740 }
05741
05742 if(ui_->checkBox_grid_empty->isChecked())
05743 {
05744 pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZ>);
05745 pcl::copyPointCloud(*cloud, *empty, *emptyCloud);
05746 occupancyGridViewer_->addCloud("octomap_empty", emptyCloud, Transform::getIdentity(), QColor(ui_->lineEdit_emptyColor->text()));
05747 occupancyGridViewer_->setCloudOpacity("octomap_empty", 0.5);
05748 occupancyGridViewer_->setCloudPointSize("octomap_empty", 5);
05749 }
05750 }
05751 occupancyGridViewer_->update();
05752 }
05753 if(ui_->dockWidget_view3d->isVisible() && ui_->checkBox_showGrid->isChecked())
05754 {
05755 this->update3dView();
05756 }
05757 }
05758 #endif
05759 }
05760
05761 Link DatabaseViewer::findActiveLink(int from, int to)
05762 {
05763 Link link;
05764 std::multimap<int, Link>::iterator findIter = rtabmap::graph::findLink(linksRefined_, from ,to);
05765 if(findIter != linksRefined_.end())
05766 {
05767 link = findIter->second;
05768 }
05769 else
05770 {
05771 findIter = rtabmap::graph::findLink(linksAdded_, from ,to);
05772 if(findIter != linksAdded_.end())
05773 {
05774 link = findIter->second;
05775 }
05776 else if(!containsLink(linksRemoved_, from ,to))
05777 {
05778 findIter = rtabmap::graph::findLink(links_, from ,to);
05779 if(findIter != links_.end())
05780 {
05781 link = findIter->second;
05782 }
05783 }
05784 }
05785 return link;
05786 }
05787
05788 bool DatabaseViewer::containsLink(std::multimap<int, Link> & links, int from, int to)
05789 {
05790 return rtabmap::graph::findLink(links, from, to) != links.end();
05791 }
05792
05793 void DatabaseViewer::refineConstraint()
05794 {
05795 int from = ids_.at(ui_->horizontalSlider_A->value());
05796 int to = ids_.at(ui_->horizontalSlider_B->value());
05797 refineConstraint(from, to, false);
05798 }
05799
05800 void DatabaseViewer::refineConstraint(int from, int to, bool silent)
05801 {
05802 if(from == to)
05803 {
05804 UWARN("Cannot refine link to same node");
05805 return;
05806 }
05807
05808 Link currentLink = findActiveLink(from, to);
05809 if(!currentLink.isValid())
05810 {
05811 UERROR("Not found link! (%d->%d)", from, to);
05812 return;
05813 }
05814 UDEBUG("%d -> %d (type=%d)", from ,to, currentLink.type());
05815 Transform t = currentLink.transform();
05816 if(ui_->checkBox_showOptimized->isChecked() &&
05817 (currentLink.type() == Link::kNeighbor || currentLink.type() == Link::kNeighborMerged) &&
05818 graphes_.size() &&
05819 (int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
05820 {
05821 std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
05822 if(currentLink.type() == Link::kNeighbor || currentLink.type() == Link::kNeighborMerged)
05823 {
05824 std::map<int, rtabmap::Transform>::iterator iterFrom = graph.find(currentLink.from());
05825 std::map<int, rtabmap::Transform>::iterator iterTo = graph.find(currentLink.to());
05826 if(iterFrom != graph.end() && iterTo != graph.end())
05827 {
05828 Transform topt = iterFrom->second.inverse()*iterTo->second;
05829 t = topt;
05830 }
05831 }
05832 }
05833 else if(ui_->checkBox_ignorePoseCorrection->isChecked() &&
05834 graph::findLink(linksRefined_, from, to) == linksRefined_.end())
05835 {
05836 if(currentLink.type() == Link::kNeighbor ||
05837 currentLink.type() == Link::kNeighborMerged)
05838 {
05839 Transform poseFrom = uValue(odomPoses_, currentLink.from(), Transform());
05840 Transform poseTo = uValue(odomPoses_, currentLink.to(), Transform());
05841 if(!poseFrom.isNull() && !poseTo.isNull())
05842 {
05843 t = poseFrom.inverse() * poseTo;
05844 }
05845 }
05846 }
05847
05848 Transform transform;
05849 RegistrationInfo info;
05850 Signature fromS;
05851 Signature toS;
05852
05853 SensorData dataFrom;
05854 dbDriver_->getNodeData(currentLink.from(), dataFrom);
05855
05856 ParametersMap parameters = ui_->parameters_toolbox->getParameters();
05857
05858 UTimer timer;
05859
05860
05861 cv::Mat userData = currentLink.uncompressUserDataConst();
05862 std::map<int, rtabmap::Transform> scanPoses;
05863
05864 if(currentLink.type() == Link::kLocalSpaceClosure &&
05865 !currentLink.userDataCompressed().empty() &&
05866 userData.type() == CV_8SC1 &&
05867 userData.rows == 1 &&
05868 userData.cols >= 8 &&
05869 userData.at<char>(userData.cols-1) == 0 &&
05870 memcmp(userData.data, "SCANS:", 6) == 0 &&
05871 currentLink.from() > currentLink.to())
05872 {
05873 std::string scansStr = (const char *)userData.data;
05874 UINFO("Detected \"%s\" in links's user data", scansStr.c_str());
05875 if(!scansStr.empty())
05876 {
05877 std::list<std::string> strs = uSplit(scansStr, ':');
05878 if(strs.size() == 2)
05879 {
05880 std::list<std::string> strIds = uSplit(strs.rbegin()->c_str(), ';');
05881 for(std::list<std::string>::iterator iter=strIds.begin(); iter!=strIds.end(); ++iter)
05882 {
05883 int id = atoi(iter->c_str());
05884 if(uContains(odomPoses_, id))
05885 {
05886 scanPoses.insert(*odomPoses_.find(id));
05887 }
05888 else
05889 {
05890 UERROR("Not found %d node!", id);
05891 }
05892 }
05893 }
05894 }
05895 }
05896 if(scanPoses.size())
05897 {
05898
05899 Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
05900
05901 UASSERT(uContains(scanPoses, currentLink.to()));
05902 std::map<int, rtabmap::Transform> posesOut;
05903 std::multimap<int, rtabmap::Link> linksOut;
05904 optimizer->getConnectedGraph(
05905 currentLink.to(),
05906 scanPoses,
05907 updateLinksWithModifications(links_),
05908 posesOut,
05909 linksOut);
05910
05911 if(scanPoses.size() != posesOut.size())
05912 {
05913 UWARN("Scan poses input and output are different! %d vs %d", (int)scanPoses.size(), (int)posesOut.size());
05914 UWARN("Input poses: ");
05915 for(std::map<int, Transform>::iterator iter=scanPoses.begin(); iter!=scanPoses.end(); ++iter)
05916 {
05917 UWARN(" %d", iter->first);
05918 }
05919 UWARN("Input links: ");
05920 std::multimap<int, Link> modifiedLinks = updateLinksWithModifications(links_);
05921 for(std::multimap<int, Link>::iterator iter=modifiedLinks.begin(); iter!=modifiedLinks.end(); ++iter)
05922 {
05923 UWARN(" %d->%d", iter->second.from(), iter->second.to());
05924 }
05925 }
05926
05927 scanPoses = optimizer->optimize(currentLink.to(), posesOut, linksOut);
05928 delete optimizer;
05929
05930 std::map<int, Transform> filteredScanPoses = scanPoses;
05931 float proximityFilteringRadius = 0.0f;
05932 Parameters::parse(parameters, Parameters::kRGBDProximityPathFilteringRadius(), proximityFilteringRadius);
05933 if(scanPoses.size() > 2 && proximityFilteringRadius > 0.0f)
05934 {
05935
05936 filteredScanPoses = graph::radiusPosesFiltering(scanPoses, proximityFilteringRadius, 0, true);
05937
05938 filteredScanPoses.insert(*scanPoses.find(currentLink.to()));
05939 }
05940
05941 Transform toPoseInv = filteredScanPoses.at(currentLink.to()).inverse();
05942 LaserScan fromScan;
05943 dataFrom.uncompressData(0,0,&fromScan);
05944 int maxPoints = fromScan.size();
05945 pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
05946 pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(new pcl::PointCloud<pcl::PointNormal>);
05947 for(std::map<int, Transform>::const_iterator iter = filteredScanPoses.begin(); iter!=filteredScanPoses.end(); ++iter)
05948 {
05949 if(iter->first != currentLink.from())
05950 {
05951 SensorData data;
05952 dbDriver_->getNodeData(iter->first, data);
05953 if(!data.laserScanCompressed().isEmpty())
05954 {
05955 LaserScan scan;
05956 data.uncompressData(0, 0, &scan);
05957 if(!scan.isEmpty() && fromScan.format() == scan.format())
05958 {
05959 if(scan.hasNormals())
05960 {
05961 *assembledToNormalClouds += *util3d::laserScanToPointCloudNormal(scan, toPoseInv * iter->second * scan.localTransform());
05962 }
05963 else
05964 {
05965 *assembledToClouds += *util3d::laserScanToPointCloud(scan, toPoseInv * iter->second * scan.localTransform());
05966 }
05967
05968 if(scan.size() > maxPoints)
05969 {
05970 maxPoints = scan.size();
05971 }
05972 }
05973 }
05974 else
05975 {
05976 UWARN("Laser scan not found for signature %d", iter->first);
05977 }
05978 }
05979 }
05980
05981 cv::Mat assembledScan;
05982 if(assembledToNormalClouds->size())
05983 {
05984 assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToNormalClouds):util3d::laserScanFromPointCloud(*assembledToNormalClouds);
05985 }
05986 else if(assembledToClouds->size())
05987 {
05988 assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToClouds):util3d::laserScanFromPointCloud(*assembledToClouds);
05989 }
05990 SensorData assembledData;
05991
05992 assembledData.setLaserScanRaw(LaserScan(
05993 assembledScan,
05994 fromScan.maxPoints()?fromScan.maxPoints():maxPoints,
05995 fromScan.maxRange(),
05996 fromScan.format(),
05997 fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
05998
05999 RegistrationIcp registrationIcp(parameters);
06000 transform = registrationIcp.computeTransformation(dataFrom, assembledData, currentLink.transform(), &info);
06001 if(!transform.isNull())
06002 {
06003
06004 info.covariance*=100.0;
06005 }
06006 }
06007 else
06008 {
06009 SensorData dataTo;
06010 dbDriver_->getNodeData(currentLink.to(), dataTo);
06011 Registration * registration = Registration::create(parameters);
06012 if(registration->isScanRequired())
06013 {
06014 if(ui_->checkBox_icp_from_depth->isChecked())
06015 {
06016
06017 cv::Mat tmpA, tmpB, tmpC, tmpD;
06018 dataFrom.uncompressData(&tmpA, &tmpB, 0);
06019 dataTo.uncompressData(&tmpC, &tmpD, 0);
06020 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom = util3d::cloudFromSensorData(
06021 dataFrom,
06022 ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
06023 ui_->doubleSpinBox_icp_maxDepth->value(),
06024 ui_->doubleSpinBox_icp_minDepth->value(),
06025 0,
06026 ui_->parameters_toolbox->getParameters());
06027 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo = util3d::cloudFromSensorData(
06028 dataTo,
06029 ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
06030 ui_->doubleSpinBox_icp_maxDepth->value(),
06031 ui_->doubleSpinBox_icp_minDepth->value(),
06032 0,
06033 ui_->parameters_toolbox->getParameters());
06034 int maxLaserScans = cloudFrom->size();
06035 dataFrom.setLaserScanRaw(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0, LaserScan::kXYZ));
06036 dataTo.setLaserScanRaw(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0, LaserScan::kXYZ));
06037
06038 if(!dataFrom.laserScanCompressed().isEmpty() || !dataTo.laserScanCompressed().isEmpty())
06039 {
06040 UWARN("There are laser scans in data, but generate laser scan from "
06041 "depth image option is activated. Ignoring saved laser scans...");
06042 }
06043 }
06044 else
06045 {
06046 LaserScan tmpA, tmpB;
06047 dataFrom.uncompressData(0, 0, &tmpA);
06048 dataTo.uncompressData(0, 0, &tmpB);
06049 }
06050 }
06051
06052 if(registration->isImageRequired())
06053 {
06054 cv::Mat tmpA, tmpB, tmpC, tmpD;
06055 dataFrom.uncompressData(&tmpA, &tmpB, 0);
06056 dataTo.uncompressData(&tmpC, &tmpD, 0);
06057 }
06058
06059 UINFO("Uncompress time: %f s", timer.ticks());
06060
06061 fromS = Signature(dataFrom);
06062 toS = Signature(dataTo);
06063 transform = registration->computeTransformationMod(fromS, toS, t, &info);
06064 delete registration;
06065 }
06066 UINFO("(%d ->%d) Registration time: %f s", from, to, timer.ticks());
06067
06068 if(!transform.isNull())
06069 {
06070 if(!transform.isIdentity())
06071 {
06072 if(info.covariance.at<double>(0,0)<=0.0)
06073 {
06074 info.covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001;
06075 }
06076 }
06077 Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform, info.covariance.inv(), currentLink.userDataCompressed());
06078
06079 bool updated = false;
06080 std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
06081 while(iter != linksRefined_.end() && iter->first == currentLink.from())
06082 {
06083 if(iter->second.to() == currentLink.to() &&
06084 iter->second.type() == currentLink.type())
06085 {
06086 iter->second = newLink;
06087 updated = true;
06088 break;
06089 }
06090 ++iter;
06091 }
06092 if(!updated)
06093 {
06094 linksRefined_.insert(std::make_pair(newLink.from(), newLink));
06095
06096 if(!silent)
06097 {
06098 this->updateGraphView();
06099 }
06100 }
06101
06102 if(!silent && ui_->dockWidget_constraints->isVisible())
06103 {
06104 if(fromS.id() > 0 && toS.id() > 0)
06105 {
06106 this->updateConstraintView(newLink, true, fromS, toS);
06107
06108 ui_->graphicsView_A->setFeatures(fromS.getWords(), fromS.sensorData().depthRaw());
06109 ui_->graphicsView_B->setFeatures(toS.getWords(), toS.sensorData().depthRaw());
06110 updateWordsMatching();
06111 }
06112 else
06113 {
06114 this->updateConstraintView();
06115 }
06116 }
06117 }
06118
06119 else if(!silent)
06120 {
06121 QMessageBox::warning(this,
06122 tr("Refine link"),
06123 tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(info.rejectedMsg.c_str()));
06124 }
06125 }
06126
06127 void DatabaseViewer::addConstraint()
06128 {
06129 int from = ids_.at(ui_->horizontalSlider_A->value());
06130 int to = ids_.at(ui_->horizontalSlider_B->value());
06131 addConstraint(from, to, false);
06132 }
06133
06134 bool DatabaseViewer::addConstraint(int from, int to, bool silent)
06135 {
06136 bool switchedIds = false;
06137 if(from == to)
06138 {
06139 UWARN("Cannot add link to same node");
06140 return false;
06141 }
06142 else if(from < to)
06143 {
06144 switchedIds = true;
06145 int tmp = from;
06146 from = to;
06147 to = tmp;
06148 }
06149
06150 Link newLink;
06151 if(!containsLink(linksAdded_, from, to) &&
06152 !containsLink(links_, from, to))
06153 {
06154 UASSERT(!containsLink(linksRemoved_, from, to));
06155 UASSERT(!containsLink(linksRefined_, from, to));
06156
06157 ParametersMap parameters = ui_->parameters_toolbox->getParameters();
06158 Registration * reg = Registration::create(parameters);
06159
06160 Transform t;
06161 RegistrationInfo info;
06162
06163 std::list<int> ids;
06164 ids.push_back(from);
06165 ids.push_back(to);
06166 std::list<Signature*> signatures;
06167 dbDriver_->loadSignatures(ids, signatures);
06168 if(signatures.size() != 2)
06169 {
06170 for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
06171 {
06172 delete *iter;
06173 return false;
06174 }
06175 }
06176 Signature * fromS = *signatures.begin();
06177 Signature * toS = *signatures.rbegin();
06178
06179 bool reextractVisualFeatures = uStr2Bool(parameters.at(Parameters::kRGBDLoopClosureReextractFeatures()));
06180 if(reg->isScanRequired() ||
06181 reg->isUserDataRequired() ||
06182 reextractVisualFeatures)
06183 {
06184
06185 dbDriver_->getNodeData(from, fromS->sensorData(), reextractVisualFeatures, reg->isScanRequired(), reg->isUserDataRequired(), false);
06186 fromS->sensorData().uncompressData();
06187 dbDriver_->getNodeData(to, toS->sensorData());
06188 toS->sensorData().uncompressData();
06189 if(reextractVisualFeatures)
06190 {
06191 fromS->setWords(std::multimap<int, cv::KeyPoint>());
06192 fromS->setWords3(std::multimap<int, cv::Point3f>());
06193 fromS->setWordsDescriptors(std::multimap<int, cv::Mat>());
06194 fromS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
06195 toS->setWords(std::multimap<int, cv::KeyPoint>());
06196 toS->setWords3(std::multimap<int, cv::Point3f>());
06197 toS->setWordsDescriptors(std::multimap<int, cv::Mat>());
06198 toS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
06199 }
06200 }
06201 else if(!reextractVisualFeatures && fromS->getWords().empty() && toS->getWords().empty())
06202 {
06203 UWARN("\"%s\" is false and signatures (%d and %d) don't have words, "
06204 "registration will not be possible. Set \"%s\" to true.",
06205 Parameters::kRGBDLoopClosureReextractFeatures().c_str(),
06206 fromS->id(),
06207 toS->id(),
06208 Parameters::kRGBDLoopClosureReextractFeatures().c_str());
06209 }
06210
06211 t = reg->computeTransformationMod(*fromS, *toS, Transform(), &info);
06212 delete reg;
06213 UDEBUG("");
06214
06215 if(!silent)
06216 {
06217 if(switchedIds)
06218 {
06219 ui_->graphicsView_A->setFeatures(toS->getWords(), toS->sensorData().depthRaw());
06220 ui_->graphicsView_B->setFeatures(fromS->getWords(), fromS->sensorData().depthRaw());
06221 }
06222 else
06223 {
06224 ui_->graphicsView_A->setFeatures(fromS->getWords(), fromS->sensorData().depthRaw());
06225 ui_->graphicsView_B->setFeatures(toS->getWords(), toS->sensorData().depthRaw());
06226 }
06227 updateWordsMatching();
06228 }
06229
06230 if(!t.isNull())
06231 {
06232 if(!t.isIdentity())
06233 {
06234
06235 info.covariance *= t.getNorm();
06236 if(info.covariance.at<double>(0,0)<=0.0)
06237 {
06238 info.covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001;
06239 }
06240 }
06241 newLink = Link(from, to, Link::kUserClosure, t, info.covariance.inv());
06242 }
06243 else if(!silent)
06244 {
06245 QMessageBox::warning(this,
06246 tr("Add link"),
06247 tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(info.rejectedMsg.c_str()));
06248 }
06249
06250 for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
06251 {
06252 delete *iter;
06253 }
06254 }
06255 else if(containsLink(linksRemoved_, from, to))
06256 {
06257 newLink = rtabmap::graph::findLink(linksRemoved_, from, to)->second;
06258 }
06259
06260 bool updateConstraints = newLink.isValid();
06261 float maxOptimizationError = uStr2Float(ui_->parameters_toolbox->getParameters().at(Parameters::kRGBDOptimizeMaxError()));
06262 if(newLink.isValid() &&
06263 maxOptimizationError > 0.0f &&
06264 uStr2Int(ui_->parameters_toolbox->getParameters().at(Parameters::kOptimizerIterations())) > 0.0f)
06265 {
06266 int fromId = newLink.from();
06267 int mapId = mapIds_.at(newLink.from());
06268
06269 for(std::map<int, int>::iterator iter=mapIds_.begin(); iter!=mapIds_.end(); ++iter)
06270 {
06271 if(iter->second == mapId)
06272 {
06273 fromId = iter->first;
06274 break;
06275 }
06276 }
06277 std::multimap<int, Link> linksIn = updateLinksWithModifications(links_);
06278 linksIn.insert(std::make_pair(newLink.from(), newLink));
06279 const Link * maxLinearLink = 0;
06280 const Link * maxAngularLink = 0;
06281 float maxLinearErrorRatio = 0.0f;
06282 float maxAngularErrorRatio = 0.0f;
06283 Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
06284 std::map<int, Transform> poses;
06285 std::multimap<int, Link> links;
06286 UASSERT(odomPoses_.find(fromId) != odomPoses_.end());
06287 UASSERT_MSG(odomPoses_.find(newLink.from()) != odomPoses_.end(), uFormat("id=%d poses=%d links=%d", newLink.from(), (int)poses.size(), (int)links.size()).c_str());
06288 UASSERT_MSG(odomPoses_.find(newLink.to()) != odomPoses_.end(), uFormat("id=%d poses=%d links=%d", newLink.to(), (int)poses.size(), (int)links.size()).c_str());
06289 optimizer->getConnectedGraph(fromId, odomPoses_, linksIn, poses, links);
06290 UASSERT(poses.find(fromId) != poses.end());
06291 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());
06292 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());
06293 UASSERT(graph::findLink(links, newLink.from(), newLink.to()) != links.end());
06294 poses = optimizer->optimize(fromId, poses, links);
06295 std::string msg;
06296 if(poses.size())
06297 {
06298 float maxLinearError = 0.0f;
06299 float maxAngularError = 0.0f;
06300 for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
06301 {
06302
06303 if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
06304 {
06305 Transform t1 = uValue(poses, iter->second.from(), Transform());
06306 Transform t2 = uValue(poses, iter->second.to(), Transform());
06307 Transform t = t1.inverse()*t2;
06308 float linearError = uMax3(
06309 fabs(iter->second.transform().x() - t.x()),
06310 fabs(iter->second.transform().y() - t.y()),
06311 fabs(iter->second.transform().z() - t.z()));
06312 float opt_roll,opt__pitch,opt__yaw;
06313 float link_roll,link_pitch,link_yaw;
06314 t.getEulerAngles(opt_roll, opt__pitch, opt__yaw);
06315 iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
06316 float angularError = uMax3(
06317 fabs(opt_roll - link_roll),
06318 fabs(opt__pitch - link_pitch),
06319 fabs(opt__yaw - link_yaw));
06320 float stddevLinear = sqrt(iter->second.transVariance());
06321 float linearErrorRatio = linearError/stddevLinear;
06322 if(linearErrorRatio > maxLinearErrorRatio)
06323 {
06324 maxLinearError = linearError;
06325 maxLinearErrorRatio = linearErrorRatio;
06326 maxLinearLink = &iter->second;
06327 }
06328 float stddevAngular = sqrt(iter->second.rotVariance());
06329 float angularErrorRatio = angularError/stddevAngular;
06330 if(angularErrorRatio > maxAngularErrorRatio)
06331 {
06332 maxAngularError = angularError;
06333 maxAngularErrorRatio = angularErrorRatio;
06334 maxAngularLink = &iter->second;
06335 }
06336 }
06337 }
06338 if(maxLinearLink)
06339 {
06340 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()));
06341 if(maxLinearErrorRatio > maxOptimizationError)
06342 {
06343 msg = uFormat("Rejecting edge %d->%d because "
06344 "graph error is too large after optimization (ratio %f for edge %d->%d, stddev=%f). "
06345 "\"%s\" is %f.",
06346 newLink.from(),
06347 newLink.to(),
06348 maxLinearErrorRatio,
06349 maxLinearLink->from(),
06350 maxLinearLink->to(),
06351 sqrt(maxLinearLink->transVariance()),
06352 Parameters::kRGBDOptimizeMaxError().c_str(),
06353 maxOptimizationError);
06354 }
06355 }
06356 if(maxAngularLink)
06357 {
06358 UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0f/CV_PI, maxAngularLink->from(), maxAngularLink->to(), maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance()));
06359 if(maxAngularErrorRatio > maxOptimizationError)
06360 {
06361 msg = uFormat("Rejecting edge %d->%d because "
06362 "graph error is too large after optimization (ratio %f for edge %d->%d, stddev=%f). "
06363 "\"%s\" is %f.",
06364 newLink.from(),
06365 newLink.to(),
06366 maxAngularErrorRatio,
06367 maxAngularLink->from(),
06368 maxAngularLink->to(),
06369 sqrt(maxAngularLink->rotVariance()),
06370 Parameters::kRGBDOptimizeMaxError().c_str(),
06371 maxOptimizationError);
06372 }
06373 }
06374 }
06375 else
06376 {
06377 msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
06378 newLink.from(),
06379 newLink.to());
06380 }
06381 if(!msg.empty())
06382 {
06383 UWARN("%s", msg.c_str());
06384 if(!silent)
06385 {
06386 QMessageBox::warning(this,
06387 tr("Add link"),
06388 tr("%1").arg(msg.c_str()));
06389 }
06390
06391 updateConstraints = false;
06392 }
06393 }
06394
06395 if(updateConstraints)
06396 {
06397 if(containsLink(linksRemoved_, from, to))
06398 {
06399
06400 linksRemoved_.erase(rtabmap::graph::findLink(linksRemoved_, from, to));
06401 }
06402 else
06403 {
06404 linksAdded_.insert(std::make_pair(newLink.from(), newLink));
06405 }
06406 if(!silent)
06407 {
06408 updateLoopClosuresSlider(from, to);
06409 this->updateGraphView();
06410 }
06411 }
06412
06413 return updateConstraints;
06414 }
06415
06416 void DatabaseViewer::resetConstraint()
06417 {
06418 int from = ids_.at(ui_->horizontalSlider_A->value());
06419 int to = ids_.at(ui_->horizontalSlider_B->value());
06420 if(from < to)
06421 {
06422 int tmp = to;
06423 to = from;
06424 from = tmp;
06425 }
06426
06427 if(from == to)
06428 {
06429 UWARN("Cannot reset link to same node");
06430 return;
06431 }
06432
06433
06434 std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, from, to);
06435 if(iter != linksRefined_.end())
06436 {
06437 linksRefined_.erase(iter);
06438 this->updateGraphView();
06439 }
06440
06441 iter = rtabmap::graph::findLink(links_, from, to);
06442 if(iter != links_.end())
06443 {
06444 this->updateConstraintView(iter->second);
06445 }
06446 iter = rtabmap::graph::findLink(linksAdded_, from, to);
06447 if(iter != linksAdded_.end())
06448 {
06449 this->updateConstraintView(iter->second);
06450 }
06451 }
06452
06453 void DatabaseViewer::rejectConstraint()
06454 {
06455 int from = ids_.at(ui_->horizontalSlider_A->value());
06456 int to = ids_.at(ui_->horizontalSlider_B->value());
06457 if(from < to)
06458 {
06459 int tmp = to;
06460 to = from;
06461 from = tmp;
06462 }
06463
06464 if(from == to)
06465 {
06466 UWARN("Cannot reject link to same node");
06467 return;
06468 }
06469
06470 bool removed = false;
06471
06472
06473 std::multimap<int, Link>::iterator iter;
06474 iter = rtabmap::graph::findLink(links_, from, to);
06475 if(iter != links_.end())
06476 {
06477 if(iter->second.type() == Link::kNeighbor || iter->second.type() == Link::kNeighborMerged)
06478 {
06479 UWARN("Cannot reject neighbor links (%d->%d)", from, to);
06480 return;
06481 }
06482 linksRemoved_.insert(*iter);
06483 removed = true;
06484 }
06485
06486
06487 iter = rtabmap::graph::findLink(linksRefined_, from, to);
06488 if(iter != linksRefined_.end())
06489 {
06490 linksRefined_.erase(iter);
06491 removed = true;
06492 }
06493 iter = rtabmap::graph::findLink(linksAdded_, from, to);
06494 if(iter != linksAdded_.end())
06495 {
06496 linksAdded_.erase(iter);
06497 removed = true;
06498 }
06499 if(removed)
06500 {
06501 this->updateGraphView();
06502 }
06503 updateLoopClosuresSlider();
06504 }
06505
06506 std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
06507 const std::multimap<int, rtabmap::Link> & edgeConstraints)
06508 {
06509 std::multimap<int, rtabmap::Link> links;
06510 for(std::multimap<int, rtabmap::Link>::const_iterator iter=edgeConstraints.begin();
06511 iter!=edgeConstraints.end();
06512 ++iter)
06513 {
06514 std::multimap<int, rtabmap::Link>::iterator findIter;
06515
06516 findIter = rtabmap::graph::findLink(linksRemoved_, iter->second.from(), iter->second.to());
06517 if(findIter != linksRemoved_.end())
06518 {
06519 if(!(iter->second.from() == findIter->second.from() &&
06520 iter->second.to() == findIter->second.to() &&
06521 iter->second.type() == findIter->second.type()))
06522 {
06523 UWARN("Links (%d->%d,%d) and (%d->%d,%d) are not equal!?",
06524 iter->second.from(), iter->second.to(), iter->second.type(),
06525 findIter->second.from(), findIter->second.to(), findIter->second.type());
06526 }
06527 else
06528 {
06529
06530 continue;
06531 }
06532 }
06533
06534 findIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
06535 if(findIter!=linksRefined_.end())
06536 {
06537 if(iter->second.from() == findIter->second.from() &&
06538 iter->second.to() == findIter->second.to() &&
06539 iter->second.type() == findIter->second.type())
06540 {
06541 links.insert(*findIter);
06542
06543 continue;
06544 }
06545 else
06546 {
06547 UWARN("Links (%d->%d,%d) and (%d->%d,%d) are not equal!?",
06548 iter->second.from(), iter->second.to(), iter->second.type(),
06549 findIter->second.from(), findIter->second.to(), findIter->second.type());
06550 }
06551 }
06552
06553 links.insert(*iter);
06554 }
06555
06556
06557 for(std::multimap<int, rtabmap::Link>::const_iterator iter=linksAdded_.begin();
06558 iter!=linksAdded_.end();
06559 ++iter)
06560 {
06561
06562 links.insert(*iter);
06563 }
06564
06565 return links;
06566 }
06567
06568 void DatabaseViewer::updateLoopClosuresSlider(int from, int to)
06569 {
06570 int size = loopLinks_.size();
06571 loopLinks_.clear();
06572 std::multimap<int, Link> links = updateLinksWithModifications(links_);
06573 int position = ui_->horizontalSlider_loops->value();
06574 std::multimap<int, Link> linksSortedByParents;
06575 for(std::multimap<int, rtabmap::Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
06576 {
06577 if(iter->second.to() > iter->second.from())
06578 {
06579 linksSortedByParents.insert(std::make_pair(iter->second.to(), iter->second.inverse()));
06580 }
06581 else
06582 {
06583 linksSortedByParents.insert(*iter);
06584 }
06585 }
06586 for(std::multimap<int, rtabmap::Link>::iterator iter = linksSortedByParents.begin(); iter!=linksSortedByParents.end(); ++iter)
06587 {
06588 if(!iter->second.transform().isNull())
06589 {
06590 if(iter->second.type() != rtabmap::Link::kNeighbor &&
06591 iter->second.type() != rtabmap::Link::kNeighborMerged)
06592 {
06593 if((iter->second.from() == from && iter->second.to() == to) ||
06594 (iter->second.to() == from && iter->second.from() == to))
06595 {
06596 position = loopLinks_.size();
06597 }
06598 loopLinks_.append(iter->second);
06599 }
06600 }
06601 else
06602 {
06603 UERROR("Transform null for link from %d to %d", iter->first, iter->second.to());
06604 }
06605 }
06606
06607 if(loopLinks_.size())
06608 {
06609 if(loopLinks_.size() == 1)
06610 {
06611
06612 loopLinks_.push_back(loopLinks_.front());
06613 }
06614 ui_->horizontalSlider_loops->setMinimum(0);
06615 ui_->horizontalSlider_loops->setMaximum(loopLinks_.size()-1);
06616 ui_->horizontalSlider_loops->setEnabled(true);
06617 if(position != ui_->horizontalSlider_loops->value())
06618 {
06619 ui_->horizontalSlider_loops->setValue(position);
06620 }
06621 else if(size != loopLinks_.size())
06622 {
06623 this->updateConstraintView(loopLinks_.at(position));
06624 }
06625 }
06626 else
06627 {
06628 ui_->horizontalSlider_loops->setEnabled(false);
06629 constraintsViewer_->removeAllClouds();
06630 constraintsViewer_->update();
06631 updateConstraintButtons();
06632 }
06633 }
06634
06635 void DatabaseViewer::notifyParametersChanged(const QStringList & parametersChanged)
06636 {
06637 bool updateStereo = false;
06638 bool updateGraphView = false;
06639 for(QStringList::const_iterator iter=parametersChanged.constBegin();
06640 iter!=parametersChanged.constEnd() && (!updateStereo || !updateGraphView);
06641 ++iter)
06642 {
06643 QString group = iter->split('/').first();
06644 if(!updateStereo && group == "Stereo")
06645 {
06646 updateStereo = true;
06647 continue;
06648 }
06649 if(!updateGraphView && group == "Optimize")
06650 {
06651 updateGraphView = true;
06652 continue;
06653 }
06654 }
06655
06656 if(updateStereo)
06657 {
06658 this->updateStereo();
06659 }
06660 if(updateGraphView)
06661 {
06662 this->updateGraphView();
06663 }
06664 this->configModified();
06665 }
06666
06667 }