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 <QGraphicsLineItem>
00035 #include <QtGui/QCloseEvent>
00036 #include <QGraphicsOpacityEffect>
00037 #include <QtCore/QBuffer>
00038 #include <QtCore/QTextStream>
00039 #include <QtCore/QDateTime>
00040 #include <QtCore/QSettings>
00041 #include <rtabmap/utilite/ULogger.h>
00042 #include <rtabmap/utilite/UDirectory.h>
00043 #include <rtabmap/utilite/UConversion.h>
00044 #include <opencv2/core/core_c.h>
00045 #include <opencv2/highgui/highgui.hpp>
00046 #include <rtabmap/utilite/UTimer.h>
00047 #include <rtabmap/utilite/UFile.h>
00048 #include "rtabmap/core/DBDriver.h"
00049 #include "rtabmap/gui/KeypointItem.h"
00050 #include "rtabmap/gui/CloudViewer.h"
00051 #include "rtabmap/utilite/UCv2Qt.h"
00052 #include "rtabmap/core/util3d.h"
00053 #include "rtabmap/core/util3d_transforms.h"
00054 #include "rtabmap/core/util3d_filtering.h"
00055 #include "rtabmap/core/util3d_surface.h"
00056 #include "rtabmap/core/util3d_registration.h"
00057 #include "rtabmap/core/util3d_mapping.h"
00058 #include "rtabmap/core/util2d.h"
00059 #include "rtabmap/core/Signature.h"
00060 #include "rtabmap/core/Memory.h"
00061 #include "rtabmap/core/Features2d.h"
00062 #include "rtabmap/core/Compression.h"
00063 #include "rtabmap/core/Graph.h"
00064 #include "rtabmap/core/Stereo.h"
00065 #include "rtabmap/core/Optimizer.h"
00066 #include "rtabmap/core/RegistrationVis.h"
00067 #include "rtabmap/core/RegistrationIcp.h"
00068 #include "rtabmap/gui/DataRecorder.h"
00069 #include "rtabmap/core/SensorData.h"
00070 #include "ExportDialog.h"
00071 #include "rtabmap/gui/ProgressDialog.h"
00072 #include "ParametersToolBox.h"
00073
00074 #include <pcl/io/pcd_io.h>
00075 #include <pcl/io/ply_io.h>
00076 #include <pcl/filters/voxel_grid.h>
00077 #include <pcl/common/transforms.h>
00078 #include <pcl/common/common.h>
00079
00080 namespace rtabmap {
00081
00082 DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
00083 QMainWindow(parent),
00084 dbDriver_(0),
00085 savedMaximized_(false),
00086 firstCall_(true),
00087 iniFilePath_(ini)
00088 {
00089 pathDatabase_ = QDir::homePath()+"/Documents/RTAB-Map";
00090
00091 if(!UDirectory::exists(pathDatabase_.toStdString()))
00092 {
00093 pathDatabase_ = QDir::homePath();
00094 }
00095
00096 ui_ = new Ui_DatabaseViewer();
00097 ui_->setupUi(this);
00098 ui_->buttonBox->setVisible(false);
00099 connect(ui_->buttonBox->button(QDialogButtonBox::Close), SIGNAL(clicked()), this, SLOT(close()));
00100
00101 ui_->comboBox_logger_level->setVisible(parent==0);
00102 ui_->label_logger_level->setVisible(parent==0);
00103 connect(ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(int)), this, SLOT(updateLoggerLevel()));
00104 connect(ui_->checkBox_verticalLayout, SIGNAL(stateChanged(int)), this, SLOT(setupMainLayout(int)));
00105
00106 QString title("RTAB-Map Database Viewer[*]");
00107 this->setWindowTitle(title);
00108
00109 ui_->dockWidget_constraints->setVisible(false);
00110 ui_->dockWidget_graphView->setVisible(false);
00111 ui_->dockWidget_guiparameters->setVisible(false);
00112 ui_->dockWidget_coreparameters->setVisible(false);
00113 ui_->dockWidget_info->setVisible(false);
00114 ui_->dockWidget_stereoView->setVisible(false);
00115 ui_->dockWidget_view3d->setVisible(false);
00116
00117
00118 constraintsViewer_ = new CloudViewer(ui_->dockWidgetContents);
00119 cloudViewerA_ = new CloudViewer(ui_->dockWidgetContents_3dviews);
00120 cloudViewerB_ = new CloudViewer(ui_->dockWidgetContents_3dviews);
00121 stereoViewer_ = new CloudViewer(ui_->dockWidgetContents_stereo);
00122 constraintsViewer_->setObjectName("constraintsViewer");
00123 cloudViewerA_->setObjectName("cloudViewerA");
00124 cloudViewerB_->setObjectName("cloudViewerB");
00125 stereoViewer_->setObjectName("stereoViewer");
00126 ui_->layout_constraintsViewer->addWidget(constraintsViewer_);
00127 ui_->horizontalLayout_3dviews->addWidget(cloudViewerA_, 1);
00128 ui_->horizontalLayout_3dviews->addWidget(cloudViewerB_, 1);
00129 ui_->horizontalLayout_stereo->addWidget(stereoViewer_, 1);
00130
00131 constraintsViewer_->setCameraLockZ(false);
00132 constraintsViewer_->setCameraFree();
00133
00134 ui_->graphicsView_stereo->setAlpha(255);
00135
00136 QSet<QString> ignoredGroups;
00137 ignoredGroups.insert("Rtabmap");
00138 ignoredGroups.insert("Mem");
00139 ignoredGroups.insert("Kp");
00140 ignoredGroups.insert("Odom");
00141 ignoredGroups.insert("OdomBow");
00142 ignoredGroups.insert("OdomFlow");
00143 ignoredGroups.insert("OdomMono");
00144 ignoredGroups.insert("VhEp");
00145 ignoredGroups.insert("StereoBM");
00146 ignoredGroups.insert("RGBD");
00147 ignoredGroups.insert("DbSqlite3");
00148 ignoredGroups.insert("Bayes");
00149 ui_->parameters_toolbox->setupUi(ignoredGroups);
00150
00151 this->readSettings();
00152
00153 ui_->menuView->addAction(ui_->dockWidget_constraints->toggleViewAction());
00154 ui_->menuView->addAction(ui_->dockWidget_graphView->toggleViewAction());
00155 ui_->menuView->addAction(ui_->dockWidget_stereoView->toggleViewAction());
00156 ui_->menuView->addAction(ui_->dockWidget_view3d->toggleViewAction());
00157 ui_->menuView->addAction(ui_->dockWidget_guiparameters->toggleViewAction());
00158 ui_->menuView->addAction(ui_->dockWidget_coreparameters->toggleViewAction());
00159 ui_->menuView->addAction(ui_->dockWidget_info->toggleViewAction());
00160 connect(ui_->dockWidget_graphView->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView()));
00161
00162 connect(ui_->parameters_toolbox, SIGNAL(parametersChanged(const QStringList &)), this, SLOT(notifyParametersChanged(const QStringList &)));
00163
00164 connect(ui_->actionQuit, SIGNAL(triggered()), this, SLOT(close()));
00165
00166
00167 ui_->actionSave_config->setShortcut(QKeySequence::Save);
00168 connect(ui_->actionSave_config, SIGNAL(triggered()), this, SLOT(writeSettings()));
00169 connect(ui_->actionOpen_database, SIGNAL(triggered()), this, SLOT(openDatabase()));
00170 connect(ui_->actionExport, SIGNAL(triggered()), this, SLOT(exportDatabase()));
00171 connect(ui_->actionExtract_images, SIGNAL(triggered()), this, SLOT(extractImages()));
00172 connect(ui_->actionGenerate_graph_dot, SIGNAL(triggered()), this, SLOT(generateGraph()));
00173 connect(ui_->actionGenerate_local_graph_dot, SIGNAL(triggered()), this, SLOT(generateLocalGraph()));
00174 connect(ui_->actionGenerate_TORO_graph_graph, SIGNAL(triggered()), this, SLOT(generateTOROGraph()));
00175 connect(ui_->actionGenerate_g2o_graph_g2o, SIGNAL(triggered()), this, SLOT(generateG2OGraph()));
00176 ui_->actionGenerate_g2o_graph_g2o->setEnabled(Optimizer::isAvailable(Optimizer::kTypeG2O));
00177 connect(ui_->actionView_3D_map, SIGNAL(triggered()), this, SLOT(view3DMap()));
00178 connect(ui_->actionView_3D_laser_scans, SIGNAL(triggered()), this, SLOT(view3DLaserScans()));
00179 connect(ui_->actionGenerate_3D_map_pcd, SIGNAL(triggered()), this, SLOT(generate3DMap()));
00180 connect(ui_->actionExport_3D_laser_scans_ply_pcd, SIGNAL(triggered()), this, SLOT(generate3DLaserScans()));
00181 connect(ui_->actionDetect_more_loop_closures, SIGNAL(triggered()), this, SLOT(detectMoreLoopClosures()));
00182 connect(ui_->actionRefine_all_neighbor_links, SIGNAL(triggered()), this, SLOT(refineAllNeighborLinks()));
00183 connect(ui_->actionRefine_all_loop_closure_links, SIGNAL(triggered()), this, SLOT(refineAllLoopClosureLinks()));
00184 connect(ui_->actionVisual_Refine_all_neighbor_links, SIGNAL(triggered()), this, SLOT(refineVisuallyAllNeighborLinks()));
00185 connect(ui_->actionVisual_Refine_all_loop_closure_links, SIGNAL(triggered()), this, SLOT(refineVisuallyAllLoopClosureLinks()));
00186 connect(ui_->actionReset_all_changes, SIGNAL(triggered()), this, SLOT(resetAllChanges()));
00187
00188
00189 connect(ui_->pushButton_refine, SIGNAL(clicked()), this, SLOT(refineConstraint()));
00190 connect(ui_->pushButton_refineVisually, SIGNAL(clicked()), this, SLOT(refineConstraintVisually()));
00191 connect(ui_->pushButton_add, SIGNAL(clicked()), this, SLOT(addConstraint()));
00192 connect(ui_->pushButton_reset, SIGNAL(clicked()), this, SLOT(resetConstraint()));
00193 connect(ui_->pushButton_reject, SIGNAL(clicked()), this, SLOT(rejectConstraint()));
00194 ui_->pushButton_refine->setEnabled(false);
00195 ui_->pushButton_refineVisually->setEnabled(false);
00196 ui_->pushButton_add->setEnabled(false);
00197 ui_->pushButton_reset->setEnabled(false);
00198 ui_->pushButton_reject->setEnabled(false);
00199
00200 ui_->actionGenerate_TORO_graph_graph->setEnabled(false);
00201 ui_->actionGenerate_g2o_graph_g2o->setEnabled(false);
00202
00203 ui_->horizontalSlider_A->setTracking(false);
00204 ui_->horizontalSlider_B->setTracking(false);
00205 ui_->horizontalSlider_A->setEnabled(false);
00206 ui_->horizontalSlider_B->setEnabled(false);
00207 connect(ui_->horizontalSlider_A, SIGNAL(valueChanged(int)), this, SLOT(sliderAValueChanged(int)));
00208 connect(ui_->horizontalSlider_B, SIGNAL(valueChanged(int)), this, SLOT(sliderBValueChanged(int)));
00209 connect(ui_->horizontalSlider_A, SIGNAL(sliderMoved(int)), this, SLOT(sliderAMoved(int)));
00210 connect(ui_->horizontalSlider_B, SIGNAL(sliderMoved(int)), this, SLOT(sliderBMoved(int)));
00211
00212 connect(ui_->spinBox_mesh_angleTolerance, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
00213 connect(ui_->spinBox_mesh_fillDepthHoles, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
00214 connect(ui_->spinBox_mesh_depthError, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
00215 connect(ui_->checkBox_mesh_quad, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
00216 connect(ui_->spinBox_mesh_triangleSize, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
00217 connect(ui_->checkBox_showMesh, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
00218
00219 ui_->horizontalSlider_neighbors->setTracking(false);
00220 ui_->horizontalSlider_loops->setTracking(false);
00221 ui_->horizontalSlider_neighbors->setEnabled(false);
00222 ui_->horizontalSlider_loops->setEnabled(false);
00223 connect(ui_->horizontalSlider_neighbors, SIGNAL(valueChanged(int)), this, SLOT(sliderNeighborValueChanged(int)));
00224 connect(ui_->horizontalSlider_loops, SIGNAL(valueChanged(int)), this, SLOT(sliderLoopValueChanged(int)));
00225 connect(ui_->horizontalSlider_neighbors, SIGNAL(sliderMoved(int)), this, SLOT(sliderNeighborValueChanged(int)));
00226 connect(ui_->horizontalSlider_loops, SIGNAL(sliderMoved(int)), this, SLOT(sliderLoopValueChanged(int)));
00227 connect(ui_->checkBox_showOptimized, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
00228 connect(ui_->checkBox_show3Dclouds, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
00229 connect(ui_->checkBox_show2DScans, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
00230 connect(ui_->checkBox_show3DWords, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
00231 ui_->checkBox_showOptimized->setEnabled(false);
00232
00233 ui_->horizontalSlider_iterations->setTracking(false);
00234 ui_->horizontalSlider_iterations->setEnabled(false);
00235 ui_->spinBox_optimizationsFrom->setEnabled(false);
00236 connect(ui_->horizontalSlider_iterations, SIGNAL(valueChanged(int)), this, SLOT(sliderIterationsValueChanged(int)));
00237 connect(ui_->horizontalSlider_iterations, SIGNAL(sliderMoved(int)), this, SLOT(sliderIterationsValueChanged(int)));
00238 connect(ui_->spinBox_optimizationsFrom, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
00239 connect(ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
00240 connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
00241 connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
00242 connect(ui_->checkBox_ignoreGlobalLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
00243 connect(ui_->checkBox_ignoreLocalLoopSpace, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
00244 connect(ui_->checkBox_ignoreLocalLoopTime, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
00245 connect(ui_->checkBox_ignoreUserLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
00246 connect(ui_->spinBox_optimizationDepth, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
00247 connect(ui_->checkBox_gridErode, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
00248 connect(ui_->checkBox_gridFillUnkownSpace, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
00249 connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(updateGraphView()));
00250 connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
00251 connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
00252
00253 connect(ui_->groupBox_gridFromProjection, SIGNAL(clicked(bool)), this, SLOT(updateGrid()));
00254 connect(ui_->doubleSpinBox_gridCellSize, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
00255 connect(ui_->spinBox_projDecimation, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
00256 connect(ui_->doubleSpinBox_projMaxDepth, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
00257 connect(ui_->doubleSpinBox_projMinDepth, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
00258 connect(ui_->doubleSpinBox_projMaxAngle, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
00259 connect(ui_->spinBox_projClusterSize, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
00260
00261 ui_->label_stereo_inliers_name->setStyleSheet("QLabel {color : blue; }");
00262 ui_->label_stereo_flowOutliers_name->setStyleSheet("QLabel {color : red; }");
00263 ui_->label_stereo_slopeOutliers_name->setStyleSheet("QLabel {color : yellow; }");
00264 ui_->label_stereo_disparityOutliers_name->setStyleSheet("QLabel {color : magenta; }");
00265
00266
00267
00268 connect(ui_->graphViewer, SIGNAL(configChanged()), this, SLOT(configModified()));
00269
00270
00271 connect(ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
00272 connect(ui_->checkBox_verticalLayout, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
00273
00274 connect(ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
00275 connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
00276 connect(ui_->checkBox_ignoreGlobalLoop, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
00277 connect(ui_->checkBox_ignoreLocalLoopSpace, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
00278 connect(ui_->checkBox_ignoreLocalLoopTime, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
00279 connect(ui_->checkBox_ignoreUserLoop, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
00280 connect(ui_->spinBox_optimizationDepth, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
00281 connect(ui_->checkBox_gridErode, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
00282 connect(ui_->checkBox_gridFillUnkownSpace, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
00283 connect(ui_->groupBox_gridFromProjection, SIGNAL(clicked(bool)), this, SLOT(configModified()));
00284 connect(ui_->doubleSpinBox_gridCellSize, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00285 connect(ui_->spinBox_projDecimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
00286 connect(ui_->doubleSpinBox_projMaxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00287 connect(ui_->doubleSpinBox_projMinDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00288 connect(ui_->doubleSpinBox_projMaxAngle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00289 connect(ui_->spinBox_projClusterSize, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
00290 connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(configModified()));
00291 connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00292 connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00293
00294 connect(ui_->spinBox_icp_decimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
00295 connect(ui_->doubleSpinBox_icp_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00296 connect(ui_->doubleSpinBox_icp_minDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00297 connect(ui_->checkBox_icp_laserScan, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
00298
00299 connect(ui_->doubleSpinBox_detectMore_radius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00300 connect(ui_->doubleSpinBox_detectMore_angle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00301 connect(ui_->spinBox_detectMore_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
00302
00303
00304 QList<QDockWidget*> dockWidgets = this->findChildren<QDockWidget*>();
00305 for(int i=0; i<dockWidgets.size(); ++i)
00306 {
00307 connect(dockWidgets[i], SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(configModified()));
00308 connect(dockWidgets[i]->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configModified()));
00309 }
00310 ui_->dockWidget_constraints->installEventFilter(this);
00311 ui_->dockWidget_graphView->installEventFilter(this);
00312 ui_->dockWidget_stereoView->installEventFilter(this);
00313 ui_->dockWidget_view3d->installEventFilter(this);
00314 ui_->dockWidget_guiparameters->installEventFilter(this);
00315 ui_->dockWidget_coreparameters->installEventFilter(this);
00316 ui_->dockWidget_info->installEventFilter(this);
00317 }
00318
00319 DatabaseViewer::~DatabaseViewer()
00320 {
00321 delete ui_;
00322 if(dbDriver_)
00323 {
00324 delete dbDriver_;
00325 }
00326 }
00327
00328 void DatabaseViewer::setupMainLayout(int vertical)
00329 {
00330 if(vertical)
00331 {
00332 qobject_cast<QHBoxLayout *>(ui_->horizontalLayout_imageViews->layout())->setDirection(QBoxLayout::TopToBottom);
00333 qobject_cast<QHBoxLayout *>(ui_->horizontalLayout_3dviews->layout())->setDirection(QBoxLayout::TopToBottom);
00334 }
00335 else if(!vertical)
00336 {
00337 qobject_cast<QHBoxLayout *>(ui_->horizontalLayout_imageViews->layout())->setDirection(QBoxLayout::LeftToRight);
00338 qobject_cast<QHBoxLayout *>(ui_->horizontalLayout_3dviews->layout())->setDirection(QBoxLayout::LeftToRight);
00339 }
00340 }
00341
00342 void DatabaseViewer::showCloseButton(bool visible)
00343 {
00344 ui_->buttonBox->setVisible(visible);
00345 }
00346
00347 void DatabaseViewer::configModified()
00348 {
00349 this->setWindowModified(true);
00350 }
00351
00352 QString DatabaseViewer::getIniFilePath() const
00353 {
00354 if(!iniFilePath_.isEmpty())
00355 {
00356 return iniFilePath_;
00357 }
00358 QString privatePath = QDir::homePath() + "/.rtabmap";
00359 if(!QDir(privatePath).exists())
00360 {
00361 QDir::home().mkdir(".rtabmap");
00362 }
00363 return privatePath + "/rtabmap.ini";
00364 }
00365
00366 void DatabaseViewer::readSettings()
00367 {
00368 QString path = getIniFilePath();
00369 QSettings settings(path, QSettings::IniFormat);
00370 settings.beginGroup("DatabaseViewer");
00371
00372
00373 QByteArray bytes;
00374 bytes = settings.value("geometry", QByteArray()).toByteArray();
00375 if(!bytes.isEmpty())
00376 {
00377 this->restoreGeometry(bytes);
00378 }
00379 bytes = settings.value("state", QByteArray()).toByteArray();
00380 if(!bytes.isEmpty())
00381 {
00382 this->restoreState(bytes);
00383 }
00384 savedMaximized_ = settings.value("maximized", false).toBool();
00385
00386 ui_->comboBox_logger_level->setCurrentIndex(settings.value("loggerLevel", ui_->comboBox_logger_level->currentIndex()).toInt());
00387 ui_->checkBox_verticalLayout->setChecked(settings.value("verticalLayout", ui_->checkBox_verticalLayout->isChecked()).toBool());
00388
00389
00390 ui_->graphViewer->loadSettings(settings, "GraphView");
00391
00392 settings.beginGroup("optimization");
00393 ui_->checkBox_spanAllMaps->setChecked(settings.value("spanToAllMaps", ui_->checkBox_spanAllMaps->isChecked()).toBool());
00394 ui_->checkBox_ignorePoseCorrection->setChecked(settings.value("ignorePoseCorrection", ui_->checkBox_ignorePoseCorrection->isChecked()).toBool());
00395 ui_->checkBox_ignoreGlobalLoop->setChecked(settings.value("ignoreGlobalLoop", ui_->checkBox_ignoreGlobalLoop->isChecked()).toBool());
00396 ui_->checkBox_ignoreLocalLoopSpace->setChecked(settings.value("ignoreLocalLoopSpace", ui_->checkBox_ignoreLocalLoopSpace->isChecked()).toBool());
00397 ui_->checkBox_ignoreLocalLoopTime->setChecked(settings.value("ignoreLocalLoopTime", ui_->checkBox_ignoreLocalLoopTime->isChecked()).toBool());
00398 ui_->checkBox_ignoreUserLoop->setChecked(settings.value("ignoreUserLoop", ui_->checkBox_ignoreUserLoop->isChecked()).toBool());
00399 ui_->spinBox_optimizationDepth->setValue(settings.value("depth", ui_->spinBox_optimizationDepth->value()).toInt());
00400 ui_->checkBox_gridErode->setChecked(settings.value("erode", ui_->checkBox_gridErode->isChecked()).toBool());
00401 ui_->checkBox_gridFillUnkownSpace->setChecked(settings.value("unknownSpaceFilled", ui_->checkBox_gridFillUnkownSpace->isChecked()).toBool());
00402 settings.endGroup();
00403
00404 settings.beginGroup("grid");
00405 ui_->groupBox_gridFromProjection->setChecked(settings.value("gridFromProj", ui_->groupBox_gridFromProjection->isChecked()).toBool());
00406 ui_->doubleSpinBox_gridCellSize->setValue(settings.value("gridCellSize", ui_->doubleSpinBox_gridCellSize->value()).toDouble());
00407 ui_->spinBox_projDecimation->setValue(settings.value("projDecimation", ui_->spinBox_projDecimation->value()).toInt());
00408 ui_->doubleSpinBox_projMaxDepth->setValue(settings.value("projMaxDepth", ui_->doubleSpinBox_projMaxDepth->value()).toDouble());
00409 ui_->doubleSpinBox_projMinDepth->setValue(settings.value("projMinDepth", ui_->doubleSpinBox_projMinDepth->value()).toDouble());
00410 ui_->doubleSpinBox_projMaxAngle->setValue(settings.value("projMaxAngle", ui_->doubleSpinBox_projMaxAngle->value()).toDouble());
00411 ui_->spinBox_projClusterSize->setValue(settings.value("projClusterSize", ui_->spinBox_projClusterSize->value()).toInt());
00412 ui_->groupBox_posefiltering->setChecked(settings.value("poseFiltering", ui_->groupBox_posefiltering->isChecked()).toBool());
00413 ui_->doubleSpinBox_posefilteringRadius->setValue(settings.value("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value()).toDouble());
00414 ui_->doubleSpinBox_posefilteringAngle->setValue(settings.value("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value()).toDouble());
00415 settings.endGroup();
00416
00417 settings.beginGroup("mesh");
00418 ui_->checkBox_mesh_quad->setChecked(settings.value("quad", ui_->checkBox_mesh_quad->isChecked()).toBool());
00419 ui_->spinBox_mesh_angleTolerance->setValue(settings.value("angleTolerance", ui_->spinBox_mesh_angleTolerance->value()).toInt());
00420 ui_->spinBox_mesh_fillDepthHoles->setValue(settings.value("fillDepthHolesSize", ui_->spinBox_mesh_fillDepthHoles->value()).toInt());
00421 ui_->spinBox_mesh_depthError->setValue(settings.value("fillDepthHolesError", ui_->spinBox_mesh_depthError->value()).toInt());
00422 ui_->spinBox_mesh_triangleSize->setValue(settings.value("triangleSize", ui_->spinBox_mesh_triangleSize->value()).toInt());
00423 settings.endGroup();
00424
00425
00426
00427
00428
00429
00430 settings.beginGroup("icp");
00431 ui_->spinBox_icp_decimation->setValue(settings.value("decimation", ui_->spinBox_icp_decimation->value()).toInt());
00432 ui_->doubleSpinBox_icp_maxDepth->setValue(settings.value("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value()).toDouble());
00433 ui_->doubleSpinBox_icp_minDepth->setValue(settings.value("minDepth", ui_->doubleSpinBox_icp_minDepth->value()).toDouble());
00434 ui_->checkBox_icp_laserScan->setChecked(settings.value("icpLaserScan", ui_->checkBox_icp_laserScan->isChecked()).toBool());
00435 settings.endGroup();
00436
00437 settings.beginGroup("visual");
00438 ui_->doubleSpinBox_detectMore_radius->setValue(settings.value("detectMoreRadius", ui_->doubleSpinBox_detectMore_radius->value()).toDouble());
00439 ui_->doubleSpinBox_detectMore_angle->setValue(settings.value("detectMoreAngle", ui_->doubleSpinBox_detectMore_angle->value()).toDouble());
00440 ui_->spinBox_detectMore_iterations->setValue(settings.value("detectMoreIterations", ui_->spinBox_detectMore_iterations->value()).toInt());
00441 settings.endGroup();
00442
00443 settings.endGroup();
00444
00445 ParametersMap parameters;
00446 Parameters::readINI(path.toStdString(), parameters);
00447 for(ParametersMap::iterator iter = parameters.begin(); iter!= parameters.end(); ++iter)
00448 {
00449 ui_->parameters_toolbox->updateParameter(iter->first, iter->second);
00450 }
00451 }
00452
00453 void DatabaseViewer::writeSettings()
00454 {
00455 QString path = getIniFilePath();
00456 QSettings settings(path, QSettings::IniFormat);
00457 settings.beginGroup("DatabaseViewer");
00458
00459
00460 if(!this->isMaximized())
00461 {
00462 settings.setValue("geometry", this->saveGeometry());
00463 }
00464 settings.setValue("state", this->saveState());
00465 settings.setValue("maximized", this->isMaximized());
00466 savedMaximized_ = this->isMaximized();
00467
00468 settings.setValue("loggerLevel", ui_->comboBox_logger_level->currentIndex());
00469 settings.setValue("verticalLayout", ui_->checkBox_verticalLayout->isChecked());
00470
00471
00472 ui_->graphViewer->saveSettings(settings, "GraphView");
00473
00474
00475 settings.beginGroup("optimization");
00476
00477 settings.setValue("spanToAllMaps", ui_->checkBox_spanAllMaps->isChecked());
00478
00479 settings.setValue("ignorePoseCorrection", ui_->checkBox_ignorePoseCorrection->isChecked());
00480 settings.setValue("ignoreGlobalLoop", ui_->checkBox_ignoreGlobalLoop->isChecked());
00481 settings.setValue("ignoreLocalLoopSpace", ui_->checkBox_ignoreLocalLoopSpace->isChecked());
00482 settings.setValue("ignoreLocalLoopTime", ui_->checkBox_ignoreLocalLoopTime->isChecked());
00483 settings.setValue("ignoreUserLoop", ui_->checkBox_ignoreUserLoop->isChecked());
00484
00485
00486 settings.setValue("depth", ui_->spinBox_optimizationDepth->value());
00487 settings.setValue("erode", ui_->checkBox_gridErode->isChecked());
00488 settings.setValue("unknownSpaceFilled", ui_->checkBox_gridFillUnkownSpace->isChecked());
00489 settings.endGroup();
00490
00491
00492 settings.beginGroup("grid");
00493 settings.setValue("gridFromProj", ui_->groupBox_gridFromProjection->isChecked());
00494 settings.setValue("gridCellSize", ui_->doubleSpinBox_gridCellSize->value());
00495 settings.setValue("projDecimation", ui_->spinBox_projDecimation->value());
00496 settings.setValue("projMaxDepth", ui_->doubleSpinBox_projMaxDepth->value());
00497 settings.setValue("projMinDepth", ui_->doubleSpinBox_projMinDepth->value());
00498 settings.setValue("projMaxAngle", ui_->doubleSpinBox_projMaxAngle->value());
00499 settings.setValue("projClusterSize", ui_->spinBox_projClusterSize->value());
00500 settings.setValue("poseFiltering", ui_->groupBox_posefiltering->isChecked());
00501 settings.setValue("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value());
00502 settings.setValue("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value());
00503 settings.endGroup();
00504
00505 settings.beginGroup("mesh");
00506 settings.setValue("quad", ui_->checkBox_mesh_quad->isChecked());
00507 settings.setValue("angleTolerance", ui_->spinBox_mesh_angleTolerance->value());
00508 settings.setValue("fillDepthHolesSize", ui_->spinBox_mesh_fillDepthHoles->value());
00509 settings.setValue("fillDepthHolesError", ui_->spinBox_mesh_depthError->value());
00510 settings.setValue("triangleSize", ui_->spinBox_mesh_triangleSize->value());
00511 settings.endGroup();
00512
00513
00514
00515
00516
00517
00518 settings.beginGroup("icp");
00519 settings.setValue("decimation", ui_->spinBox_icp_decimation->value());
00520 settings.setValue("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value());
00521 settings.setValue("minDepth", ui_->doubleSpinBox_icp_minDepth->value());
00522 settings.setValue("icpLaserScan", ui_->checkBox_icp_laserScan->isChecked());
00523 settings.endGroup();
00524
00525
00526 settings.beginGroup("visual");
00527 settings.setValue("detectMoreRadius", ui_->doubleSpinBox_detectMore_radius->value());
00528 settings.setValue("detectMoreAngle", ui_->doubleSpinBox_detectMore_angle->value());
00529 settings.setValue("detectMoreIterations", ui_->spinBox_detectMore_iterations->value());
00530 settings.endGroup();
00531
00532 settings.endGroup();
00533
00534 ParametersMap parameters = ui_->parameters_toolbox->getParameters();
00535 for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end();)
00536 {
00537 if(!ui_->parameters_toolbox->getParameterWidget(iter->first.c_str()))
00538 {
00539 parameters.erase(iter++);
00540 }
00541 else
00542 {
00543 ++iter;
00544 }
00545 }
00546 Parameters::writeINI(path.toStdString(), parameters);
00547
00548 this->setWindowModified(false);
00549 }
00550
00551 void DatabaseViewer::openDatabase()
00552 {
00553 QString path = QFileDialog::getOpenFileName(this, tr("Select file"), pathDatabase_, tr("Databases (*.db)"));
00554 if(!path.isEmpty())
00555 {
00556 openDatabase(path);
00557 }
00558 }
00559
00560 bool DatabaseViewer::openDatabase(const QString & path)
00561 {
00562 UDEBUG("Open database \"%s\"", path.toStdString().c_str());
00563 if(QFile::exists(path))
00564 {
00565 if(dbDriver_)
00566 {
00567 delete dbDriver_;
00568 dbDriver_ = 0;
00569 ids_.clear();
00570 idToIndex_.clear();
00571 neighborLinks_.clear();
00572 loopLinks_.clear();
00573 graphes_.clear();
00574 graphLinks_.clear();
00575 poses_.clear();
00576 groundTruthPoses_.clear();
00577 mapIds_.clear();
00578 links_.clear();
00579 linksAdded_.clear();
00580 linksRefined_.clear();
00581 linksRemoved_.clear();
00582 localMaps_.clear();
00583 ui_->graphViewer->clearAll();
00584 ui_->actionGenerate_TORO_graph_graph->setEnabled(false);
00585 ui_->actionGenerate_g2o_graph_g2o->setEnabled(false);
00586 ui_->checkBox_showOptimized->setEnabled(false);
00587 databaseFileName_.clear();
00588 }
00589
00590 std::string driverType = "sqlite3";
00591
00592 dbDriver_ = DBDriver::create();
00593
00594 if(!dbDriver_->openConnection(path.toStdString()))
00595 {
00596 QMessageBox::warning(this, "Database error", tr("Can't open database \"%1\"").arg(path));
00597 }
00598 else
00599 {
00600 pathDatabase_ = UDirectory::getDir(path.toStdString()).c_str();
00601 databaseFileName_ = UFile::getName(path.toStdString());
00602
00603
00604 ParametersMap parameters = dbDriver_->getLastParameters();
00605
00606 if(parameters.size())
00607 {
00608 const ParametersMap & currentParameters = ui_->parameters_toolbox->getParameters();
00609 ParametersMap differentParameters;
00610 for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
00611 {
00612 ParametersMap::const_iterator jter = currentParameters.find(iter->first);
00613 if(jter!=currentParameters.end() &&
00614 ui_->parameters_toolbox->getParameterWidget(QString(iter->first.c_str())) != 0 &&
00615 iter->second.compare(jter->second) != 0 &&
00616 iter->first.compare(Parameters::kRtabmapWorkingDirectory()) != 0)
00617 {
00618 differentParameters.insert(*iter);
00619 QString msg = tr("Parameter \"%1\": database=\"%2\" Preferences=\"%3\"")
00620 .arg(iter->first.c_str())
00621 .arg(iter->second.c_str())
00622 .arg(jter->second.c_str());
00623 UWARN(msg.toStdString().c_str());
00624 }
00625 }
00626
00627 if(differentParameters.size())
00628 {
00629 int r = QMessageBox::question(this,
00630 tr("Update parameters..."),
00631 tr("The database is using %1 different parameter(s) than "
00632 "those currently set in Core parameters panel. Do you want "
00633 "to use database's parameters?").arg(differentParameters.size()),
00634 QMessageBox::Yes | QMessageBox::No,
00635 QMessageBox::Yes);
00636 if(r == QMessageBox::Yes)
00637 {
00638 for(rtabmap::ParametersMap::const_iterator iter = differentParameters.begin(); iter!=differentParameters.end(); ++iter)
00639 {
00640 ui_->parameters_toolbox->updateParameter(iter->first, iter->second);
00641 }
00642 }
00643 }
00644 }
00645
00646 updateIds();
00647 return true;
00648 }
00649 }
00650 else
00651 {
00652 QMessageBox::warning(this, "Database error", tr("Database \"%1\" does not exist.").arg(path));
00653 }
00654 return false;
00655 }
00656
00657 void DatabaseViewer::closeEvent(QCloseEvent* event)
00658 {
00659
00660 bool save = false;
00661 if(this->isWindowModified())
00662 {
00663 QMessageBox::Button b=QMessageBox::question(this,
00664 tr("Database Viewer"),
00665 tr("There are unsaved changed settings. Save them?"),
00666 QMessageBox::Save | QMessageBox::Cancel | QMessageBox::Discard);
00667 if(b == QMessageBox::Save)
00668 {
00669 save = true;
00670 }
00671 else if(b != QMessageBox::Discard)
00672 {
00673 event->ignore();
00674 return;
00675 }
00676 }
00677
00678 if(save)
00679 {
00680 writeSettings();
00681 }
00682
00683 if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size())
00684 {
00685 QMessageBox::StandardButton button = QMessageBox::question(this,
00686 tr("Links modified"),
00687 tr("Some links are modified (%1 added, %2 refined, %3 removed), do you want to save them?")
00688 .arg(linksAdded_.size()).arg(linksRefined_.size()).arg(linksRemoved_.size()),
00689 QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
00690 QMessageBox::Cancel);
00691
00692 if(button == QMessageBox::Yes)
00693 {
00694
00695 for(std::multimap<int, rtabmap::Link>::iterator iter=linksAdded_.begin(); iter!=linksAdded_.end(); ++iter)
00696 {
00697 std::multimap<int, rtabmap::Link>::iterator refinedIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
00698 if(refinedIter != linksRefined_.end())
00699 {
00700 dbDriver_->addLink(refinedIter->second);
00701 dbDriver_->addLink(refinedIter->second.inverse());
00702 }
00703 else
00704 {
00705 dbDriver_->addLink(iter->second);
00706 dbDriver_->addLink(iter->second.inverse());
00707 }
00708 }
00709
00710
00711 for(std::multimap<int, rtabmap::Link>::iterator iter=linksRefined_.begin(); iter!=linksRefined_.end(); ++iter)
00712 {
00713 if(!containsLink(linksAdded_, iter->second.from(), iter->second.to()))
00714 {
00715 dbDriver_->updateLink(iter->second);
00716 dbDriver_->updateLink(iter->second.inverse());
00717 }
00718 }
00719
00720
00721 for(std::multimap<int, rtabmap::Link>::iterator iter=linksRemoved_.begin(); iter!=linksRemoved_.end(); ++iter)
00722 {
00723 dbDriver_->removeLink(iter->second.to(), iter->second.from());
00724 dbDriver_->removeLink(iter->second.from(), iter->second.to());
00725 }
00726 }
00727
00728 if(button == QMessageBox::Yes || button == QMessageBox::No)
00729 {
00730 event->accept();
00731 }
00732 else
00733 {
00734 event->ignore();
00735 }
00736 }
00737 else
00738 {
00739 event->accept();
00740 }
00741
00742 if(event->isAccepted())
00743 {
00744 if(dbDriver_)
00745 {
00746 delete dbDriver_;
00747 dbDriver_ = 0;
00748 }
00749 }
00750 }
00751
00752 void DatabaseViewer::showEvent(QShowEvent* anEvent)
00753 {
00754 this->setWindowModified(false);
00755
00756 if(ui_->graphViewer->isVisible() && graphes_.size() && localMaps_.size()==0)
00757 {
00758 sliderIterationsValueChanged((int)graphes_.size()-1);
00759 }
00760 }
00761
00762 void DatabaseViewer::moveEvent(QMoveEvent* anEvent)
00763 {
00764 if(this->isVisible())
00765 {
00766
00767 if(!firstCall_)
00768 {
00769 this->configModified();
00770 }
00771 firstCall_ = false;
00772 }
00773 }
00774
00775 void DatabaseViewer::resizeEvent(QResizeEvent* anEvent)
00776 {
00777 if(this->isVisible())
00778 {
00779 this->configModified();
00780 }
00781 }
00782
00783 bool DatabaseViewer::eventFilter(QObject *obj, QEvent *event)
00784 {
00785 if (event->type() == QEvent::Resize && qobject_cast<QDockWidget*>(obj))
00786 {
00787 this->setWindowModified(true);
00788 }
00789 return QWidget::eventFilter(obj, event);
00790 }
00791
00792
00793 void DatabaseViewer::exportDatabase()
00794 {
00795 if(!dbDriver_ || ids_.size() == 0)
00796 {
00797 return;
00798 }
00799
00800 rtabmap::ExportDialog dialog;
00801
00802 if(dialog.exec())
00803 {
00804 if(!dialog.outputPath().isEmpty())
00805 {
00806 int framesIgnored = dialog.framesIgnored();
00807 double frameRate = dialog.targetFramerate();
00808 int sessionExported = dialog.sessionExported();
00809 QString path = dialog.outputPath();
00810 rtabmap::DataRecorder recorder;
00811 QList<int> ids;
00812
00813 double previousStamp = 0;
00814 std::vector<double> delays(ids_.size());
00815 int oi=0;
00816 std::map<int, Transform> poses;
00817 std::map<int, double> stamps;
00818 std::map<int, Transform> groundTruths;
00819 for(int i=0; i<ids_.size(); i+=1+framesIgnored)
00820 {
00821 Transform odomPose, groundTruth;
00822 int weight = -1;
00823 int mapId = -1;
00824 std::string label;
00825 double stamp = 0;
00826 if(dbDriver_->getNodeInfo(ids_[i], odomPose, mapId, weight, label, stamp, groundTruth))
00827 {
00828 if(frameRate == 0 ||
00829 previousStamp == 0 ||
00830 stamp == 0 ||
00831 stamp - previousStamp >= 1.0/frameRate)
00832 {
00833 if(sessionExported < 0 || sessionExported == mapId)
00834 {
00835 ids.push_back(ids_[i]);
00836
00837 if(previousStamp && stamp)
00838 {
00839 delays[oi++] = stamp - previousStamp;
00840 }
00841 previousStamp = stamp;
00842
00843 poses.insert(std::make_pair(ids_[i], odomPose));
00844 stamps.insert(std::make_pair(ids_[i], stamp));
00845 groundTruths.insert(std::make_pair(ids_[i], groundTruth));
00846 }
00847 }
00848 if(sessionExported >= 0 && mapId > sessionExported)
00849 {
00850 break;
00851 }
00852 }
00853 }
00854 delays.resize(oi);
00855
00856 if(recorder.init(path, false))
00857 {
00858 rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
00859 progressDialog->setAttribute(Qt::WA_DeleteOnClose);
00860 progressDialog->setMaximumSteps(ids.size());
00861 progressDialog->show();
00862 UINFO("Decompress: rgb=%d depth=%d scan=%d userData=%d",
00863 dialog.isRgbExported()?1:0,
00864 dialog.isDepthExported()?1:0,
00865 dialog.isDepth2dExported()?1:0,
00866 dialog.isUserDataExported()?1:0);
00867
00868 for(int i=0; i<ids.size(); ++i)
00869 {
00870 int id = ids.at(i);
00871
00872 SensorData data;
00873 dbDriver_->getNodeData(id, data);
00874 cv::Mat depth, rgb, scan, userData;
00875 data.uncompressDataConst(
00876 !dialog.isRgbExported()?0:&rgb,
00877 !dialog.isDepthExported()?0:&depth,
00878 !dialog.isDepth2dExported()?0:&scan,
00879 !dialog.isUserDataExported()?0:&userData);
00880 cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
00881 if(dialog.isOdomExported())
00882 {
00883 std::map<int, Link> links;
00884 dbDriver_->loadLinks(id, links, Link::kNeighbor);
00885 if(links.size() && links.begin()->first < id)
00886 {
00887 covariance = links.begin()->second.infMatrix().inv();
00888 }
00889 }
00890
00891 rtabmap::SensorData sensorData;
00892 if(data.cameraModels().size())
00893 {
00894 sensorData = rtabmap::SensorData(
00895 scan,
00896 dialog.isDepth2dExported()?data.laserScanMaxPts():0,
00897 dialog.isDepth2dExported()?data.laserScanMaxRange():0,
00898 rgb,
00899 depth,
00900 data.cameraModels(),
00901 id,
00902 stamps.at(id),
00903 userData);
00904 }
00905 else
00906 {
00907 sensorData = rtabmap::SensorData(
00908 scan,
00909 dialog.isDepth2dExported()?data.laserScanMaxPts():0,
00910 dialog.isDepth2dExported()?data.laserScanMaxRange():0,
00911 rgb,
00912 depth,
00913 data.stereoCameraModel(),
00914 id,
00915 stamps.at(id),
00916 userData);
00917 }
00918 sensorData.setGroundTruth(groundTruths.at(id));
00919
00920 recorder.addData(sensorData, dialog.isOdomExported()?poses.at(id):Transform(), covariance);
00921
00922 progressDialog->appendText(tr("Exported node %1").arg(id));
00923 progressDialog->incrementStep();
00924 QApplication::processEvents();
00925 }
00926 progressDialog->setValue(progressDialog->maximumSteps());
00927 if(delays.size())
00928 {
00929 progressDialog->appendText(tr("Average frame rate=%1 Hz (Min=%2, Max=%3)")
00930 .arg(1.0/uMean(delays)).arg(1.0/uMax(delays)).arg(1.0/uMin(delays)));
00931 }
00932 progressDialog->appendText(tr("Export finished to \"%1\"!").arg(path));
00933 }
00934 else
00935 {
00936 UERROR("DataRecorder init failed?!");
00937 }
00938 }
00939 else
00940 {
00941 QMessageBox::warning(this, tr("Cannot export database"), tr("An output path must be set!"));
00942 }
00943 }
00944 }
00945
00946 void DatabaseViewer::extractImages()
00947 {
00948 if(!dbDriver_ || ids_.size() == 0)
00949 {
00950 return;
00951 }
00952
00953 QStringList formats;
00954 formats.push_back("jpg");
00955 formats.push_back("png");
00956 bool ok;
00957 QString ext = QInputDialog::getItem(this, tr("Which RGB format?"), tr("Format:"), formats, 0, false, &ok);
00958 if(!ok)
00959 {
00960 return;
00961 }
00962
00963 QString path = QFileDialog::getExistingDirectory(this, tr("Select directory where to save images..."), QDir::homePath());
00964 if(!path.isNull())
00965 {
00966 if(ids_.size())
00967 {
00968 int id = ids_.at(0);
00969 SensorData data;
00970 dbDriver_->getNodeData(id, data);
00971 data.uncompressData();
00972 if(!data.imageRaw().empty() && !data.rightRaw().empty())
00973 {
00974 QDir dir;
00975 dir.mkdir(QString("%1/left").arg(path));
00976 dir.mkdir(QString("%1/right").arg(path));
00977 if(databaseFileName_.empty())
00978 {
00979 UERROR("Cannot save calibration file, database name is empty!");
00980 }
00981 else if(data.stereoCameraModel().isValidForProjection())
00982 {
00983 std::string cameraName = uSplit(databaseFileName_, '.').front();
00984 StereoCameraModel model(
00985 cameraName,
00986 data.imageRaw().size(),
00987 data.stereoCameraModel().left().K(),
00988 data.stereoCameraModel().left().D(),
00989 data.stereoCameraModel().left().R(),
00990 data.stereoCameraModel().left().P(),
00991 data.rightRaw().size(),
00992 data.stereoCameraModel().right().K(),
00993 data.stereoCameraModel().right().D(),
00994 data.stereoCameraModel().right().R(),
00995 data.stereoCameraModel().right().P(),
00996 data.stereoCameraModel().R(),
00997 data.stereoCameraModel().T(),
00998 data.stereoCameraModel().E(),
00999 data.stereoCameraModel().F(),
01000 data.stereoCameraModel().left().localTransform());
01001 if(model.save(path.toStdString()))
01002 {
01003 UINFO("Saved stereo calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
01004 }
01005 else
01006 {
01007 UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
01008 }
01009 }
01010 }
01011 else if(!data.imageRaw().empty())
01012 {
01013 if(!data.depthRaw().empty())
01014 {
01015 QDir dir;
01016 dir.mkdir(QString("%1/rgb").arg(path));
01017 dir.mkdir(QString("%1/depth").arg(path));
01018 }
01019
01020 if(databaseFileName_.empty())
01021 {
01022 UERROR("Cannot save calibration file, database name is empty!");
01023 }
01024 else if(data.cameraModels().size() > 1)
01025 {
01026 UERROR("Only one camera calibration can be saved at this time (%d detected)", (int)data.cameraModels().size());
01027 }
01028 else if(data.cameraModels().size() == 1 && data.cameraModels().front().isValidForProjection())
01029 {
01030 std::string cameraName = uSplit(databaseFileName_, '.').front();
01031 CameraModel model(cameraName,
01032 data.imageRaw().size(),
01033 data.cameraModels().front().K(),
01034 data.cameraModels().front().D(),
01035 data.cameraModels().front().R(),
01036 data.cameraModels().front().P(),
01037 data.cameraModels().front().localTransform());
01038 if(model.save(path.toStdString()))
01039 {
01040 UINFO("Saved calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
01041 }
01042 else
01043 {
01044 UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
01045 }
01046 }
01047 }
01048 }
01049
01050 for(int i=0; i<ids_.size(); ++i)
01051 {
01052 int id = ids_.at(i);
01053 SensorData data;
01054 dbDriver_->getNodeData(id, data);
01055 data.uncompressData();
01056 if(!data.imageRaw().empty() && !data.rightRaw().empty())
01057 {
01058 cv::imwrite(QString("%1/left/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
01059 cv::imwrite(QString("%1/right/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.rightRaw());
01060 UINFO(QString("Saved left/%1.%2 and right/%1.%2").arg(id).arg(ext).toStdString().c_str());
01061 }
01062 else if(!data.imageRaw().empty() && !data.depthRaw().empty())
01063 {
01064 cv::imwrite(QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
01065 cv::imwrite(QString("%1/depth/%2.png").arg(path).arg(id).toStdString(), data.depthRaw());
01066 UINFO(QString("Saved rgb/%1.%2 and depth/%1.png").arg(id).arg(ext).toStdString().c_str());
01067 }
01068 else if(!data.imageRaw().empty())
01069 {
01070 cv::imwrite(QString("%1/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
01071 UINFO(QString("Saved %1.%2").arg(id).arg(ext).toStdString().c_str());
01072 }
01073 }
01074 }
01075 }
01076
01077 void DatabaseViewer::updateIds()
01078 {
01079 if(!dbDriver_)
01080 {
01081 return;
01082 }
01083
01084 UINFO("Loading all IDs...");
01085 std::set<int> ids;
01086 dbDriver_->getAllNodeIds(ids);
01087 ids_ = QList<int>::fromStdList(std::list<int>(ids.begin(), ids.end()));
01088 idToIndex_.clear();
01089 mapIds_.clear();
01090 poses_.clear();
01091 groundTruthPoses_.clear();
01092 links_.clear();
01093 linksAdded_.clear();
01094 linksRefined_.clear();
01095 linksRemoved_.clear();
01096 ui_->label_optimizeFrom->setText(tr("Optimize from"));
01097 std::multimap<int, Link> links;
01098 dbDriver_->getAllLinks(links, true);
01099 UDEBUG("%d total links loaded", (int)links.size());
01100 double totalOdom = 0.0;
01101 Transform previousPose;
01102 int sessions = ids_.size()?1:0;
01103 double totalTime = 0.0;
01104 double previousStamp = 0.0;
01105 std::set<int> idsWithoutBad;
01106 dbDriver_->getAllNodeIds(idsWithoutBad, false, true);
01107 int badcountInLTM = 0;
01108 int badCountInGraph = 0;
01109 for(int i=0; i<ids_.size(); ++i)
01110 {
01111 idToIndex_.insert(ids_[i], i);
01112
01113 Transform p, g;
01114 int w;
01115 std::string l;
01116 double s;
01117 int mapId;
01118 dbDriver_->getNodeInfo(ids_[i], p, mapId, w, l, s, g);
01119 mapIds_.insert(std::make_pair(ids_[i], mapId));
01120
01121 if(i>0)
01122 {
01123 if(mapIds_.at(ids_[i-1]) == mapId)
01124 {
01125 if(!p.isNull() && !previousPose.isNull())
01126 {
01127 totalOdom += p.getDistance(previousPose);
01128 }
01129
01130 if(previousStamp > 0.0 && s > 0.0)
01131 {
01132 totalTime += s-previousStamp;
01133 }
01134 }
01135 else
01136 {
01137 ++sessions;
01138 }
01139 }
01140 previousStamp=s;
01141 previousPose=p;
01142
01143
01144 bool addPose = false;
01145 for(std::multimap<int, Link>::iterator jter=links.find(ids_[i]); jter!=links.end() && jter->first == ids_[i]; ++jter)
01146 {
01147 std::multimap<int, Link>::iterator invertedLinkIter = graph::findLink(links, jter->second.to(), jter->second.from(), false);
01148 if( jter->second.isValid() &&
01149 ids.find(jter->second.from()) != ids.end() &&
01150 ids.find(jter->second.to()) != ids.end() &&
01151 graph::findLink(links_, jter->second.from(), jter->second.to()) == links_.end() &&
01152 graph::findLink(links, jter->second.from(), jter->second.to(), false) != links.end() &&
01153 invertedLinkIter != links.end())
01154 {
01155
01156 if(jter->second.userDataCompressed().cols == 0 &&
01157 invertedLinkIter->second.userDataCompressed().cols != 0)
01158 {
01159 links_.insert(std::make_pair(invertedLinkIter->second.from(), invertedLinkIter->second));
01160 }
01161 else
01162 {
01163 links_.insert(std::make_pair(ids_[i], jter->second));
01164 }
01165 addPose = true;
01166 }
01167 else if(graph::findLink(links_, jter->second.from(), jter->second.to()) != links_.end())
01168 {
01169 addPose = true;
01170 }
01171 }
01172 if(addPose)
01173 {
01174 poses_.insert(std::make_pair(ids_[i], p));
01175 if(!g.isNull())
01176 {
01177 groundTruthPoses_.insert(std::make_pair(ids_[i], g));
01178 }
01179 }
01180
01181 if(idsWithoutBad.find(ids_[i]) == idsWithoutBad.end())
01182 {
01183 ++badcountInLTM;
01184 if(addPose)
01185 {
01186 ++badCountInGraph;
01187 }
01188 }
01189 }
01190 UINFO("Loaded %d ids, %d poses and %d links", (int)ids_.size(), (int)poses_.size(), (int)links_.size());
01191
01192 UINFO("Update database info...");
01193 ui_->textEdit_info->clear();
01194 ui_->textEdit_info->append(tr("Version:\t\t%1").arg(dbDriver_->getDatabaseVersion().c_str()));
01195 ui_->textEdit_info->append(tr("Sessions:\t\t%1").arg(sessions));
01196 ui_->textEdit_info->append(tr("Total odometry length:\t%1 m").arg(totalOdom));
01197 ui_->textEdit_info->append(tr("Total time:\t\t%1").arg(QDateTime::fromMSecsSinceEpoch(totalTime*1000).toUTC().toString("hh:mm:ss.zzz")));
01198 ui_->textEdit_info->append(tr("LTM:\t\t%1 nodes and %2 words").arg(ids.size()).arg(dbDriver_->getTotalDictionarySize()));
01199 ui_->textEdit_info->append(tr("WM:\t\t%1 nodes and %2 words").arg(dbDriver_->getLastNodesSize()).arg(dbDriver_->getLastDictionarySize()));
01200 ui_->textEdit_info->append(tr("Global graph:\t%1 poses and %2 links").arg(poses_.size()).arg(links_.size()));
01201 ui_->textEdit_info->append(tr("Ground truth:\t%1 poses").arg(groundTruthPoses_.size()));
01202 ui_->textEdit_info->append("");
01203 ui_->textEdit_info->append(tr("Database size:\t%1 MB").arg(dbDriver_->getMemoryUsed()/1000000));
01204 ui_->textEdit_info->append(tr("Images size:\t%1 MB").arg(dbDriver_->getImagesMemoryUsed()/1000000));
01205 ui_->textEdit_info->append(tr("Depths size:\t%1 MB").arg(dbDriver_->getDepthImagesMemoryUsed()/1000000));
01206 ui_->textEdit_info->append(tr("Scans size:\t\t%1 MB").arg(dbDriver_->getLaserScansMemoryUsed()/1000000));
01207 ui_->textEdit_info->append(tr("User data size:\t%1 bytes").arg(dbDriver_->getUserDataMemoryUsed()));
01208 ui_->textEdit_info->append(tr("Dictionary size:\t%1 bytes").arg(dbDriver_->getWordsMemoryUsed()));
01209 ui_->textEdit_info->append("");
01210 ui_->textEdit_info->append(tr("%1 bad signatures in LTM").arg(badcountInLTM));
01211 ui_->textEdit_info->append(tr("%1 bad signatures in the global graph").arg(badCountInGraph));
01212
01213 if(ids.size())
01214 {
01215 if(poses_.size())
01216 {
01217 bool nullPoses = poses_.begin()->second.isNull();
01218 for(std::map<int,Transform>::iterator iter=poses_.begin(); iter!=poses_.end(); ++iter)
01219 {
01220 if((!iter->second.isNull() && nullPoses) ||
01221 (iter->second.isNull() && !nullPoses))
01222 {
01223 if(iter->second.isNull())
01224 {
01225 UWARN("Pose %d is null!", iter->first);
01226 }
01227 UWARN("Mixed valid and null poses! Ignoring graph...");
01228 poses_.clear();
01229 links_.clear();
01230 break;
01231 }
01232 }
01233 if(nullPoses)
01234 {
01235 poses_.clear();
01236 links_.clear();
01237 }
01238
01239 if(poses_.size())
01240 {
01241 ui_->spinBox_optimizationsFrom->setRange(poses_.begin()->first, poses_.rbegin()->first);
01242 ui_->spinBox_optimizationsFrom->setValue(poses_.begin()->first);
01243 ui_->label_optimizeFrom->setText(tr("Optimize from [%1, %2]").arg(poses_.begin()->first).arg(poses_.rbegin()->first));
01244 }
01245 }
01246 }
01247
01248 ui_->actionGenerate_TORO_graph_graph->setEnabled(false);
01249 ui_->actionGenerate_g2o_graph_g2o->setEnabled(false);
01250 graphes_.clear();
01251 graphLinks_.clear();
01252 neighborLinks_.clear();
01253 loopLinks_.clear();
01254 for(std::multimap<int, rtabmap::Link>::iterator iter = links_.begin(); iter!=links_.end(); ++iter)
01255 {
01256 if(!iter->second.transform().isNull())
01257 {
01258 if(iter->second.type() == rtabmap::Link::kNeighbor ||
01259 iter->second.type() == rtabmap::Link::kNeighborMerged)
01260 {
01261 neighborLinks_.append(iter->second);
01262 }
01263 else
01264 {
01265 loopLinks_.append(iter->second);
01266 }
01267 }
01268 else
01269 {
01270 UERROR("Transform null for link from %d to %d", iter->first, iter->second.to());
01271 }
01272 }
01273
01274 if(ids_.size())
01275 {
01276 ui_->horizontalSlider_A->setMinimum(0);
01277 ui_->horizontalSlider_B->setMinimum(0);
01278 ui_->horizontalSlider_A->setMaximum(ids_.size()-1);
01279 ui_->horizontalSlider_B->setMaximum(ids_.size()-1);
01280 ui_->horizontalSlider_A->setEnabled(true);
01281 ui_->horizontalSlider_B->setEnabled(true);
01282 ui_->horizontalSlider_A->setSliderPosition(0);
01283 ui_->horizontalSlider_B->setSliderPosition(0);
01284 sliderAValueChanged(0);
01285 sliderBValueChanged(0);
01286 }
01287 else
01288 {
01289 ui_->horizontalSlider_A->setEnabled(false);
01290 ui_->horizontalSlider_B->setEnabled(false);
01291 ui_->label_idA->setText("NaN");
01292 ui_->label_idB->setText("NaN");
01293 }
01294
01295 if(neighborLinks_.size())
01296 {
01297 ui_->horizontalSlider_neighbors->setMinimum(0);
01298 ui_->horizontalSlider_neighbors->setMaximum(neighborLinks_.size()-1);
01299 ui_->horizontalSlider_neighbors->setEnabled(true);
01300 ui_->horizontalSlider_neighbors->setSliderPosition(0);
01301 }
01302 else
01303 {
01304 ui_->horizontalSlider_neighbors->setEnabled(false);
01305 }
01306
01307 if(ids_.size())
01308 {
01309 updateLoopClosuresSlider();
01310 if(ui_->graphViewer->isVisible())
01311 {
01312 updateGraphView();
01313 }
01314 }
01315 }
01316
01317 void DatabaseViewer::generateGraph()
01318 {
01319 if(!dbDriver_)
01320 {
01321 QMessageBox::warning(this, tr("Cannot generate a graph"), tr("A database must must loaded first...\nUse File->Open database."));
01322 return;
01323 }
01324
01325 QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/Graph.dot", tr("Graphiz file (*.dot)"));
01326 if(!path.isEmpty())
01327 {
01328 dbDriver_->generateGraph(path.toStdString());
01329 }
01330 }
01331
01332 void DatabaseViewer::generateLocalGraph()
01333 {
01334 if(!ids_.size() || !dbDriver_)
01335 {
01336 QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
01337 return;
01338 }
01339 bool ok = false;
01340 int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID"), ids_.first(), ids_.first(), ids_.last(), 1, &ok);
01341
01342 if(ok)
01343 {
01344 int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin"), 4, 1, 100, 1, &ok);
01345 if(ok)
01346 {
01347 QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/Graph" + QString::number(id) + ".dot", tr("Graphiz file (*.dot)"));
01348 if(!path.isEmpty() && id>0)
01349 {
01350 std::map<int, int> ids;
01351 std::list<int> curentMarginList;
01352 std::set<int> currentMargin;
01353 std::set<int> nextMargin;
01354 nextMargin.insert(id);
01355 int m = 0;
01356 while((margin == 0 || m < margin) && nextMargin.size())
01357 {
01358 curentMarginList = std::list<int>(nextMargin.rbegin(), nextMargin.rend());
01359 nextMargin.clear();
01360
01361 for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
01362 {
01363 if(ids.find(*jter) == ids.end())
01364 {
01365 std::map<int, Link> links;
01366 ids.insert(std::pair<int, int>(*jter, m));
01367
01368 UTimer timer;
01369 dbDriver_->loadLinks(*jter, links);
01370
01371
01372 for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
01373 {
01374 if( !uContains(ids, iter->first))
01375 {
01376 UASSERT(iter->second.type() != Link::kUndef);
01377 if(iter->second.type() == Link::kNeighbor ||
01378 iter->second.type() == Link::kNeighborMerged)
01379 {
01380 nextMargin.insert(iter->first);
01381 }
01382 else
01383 {
01384
01385 if(currentMargin.insert(iter->first).second)
01386 {
01387 curentMarginList.push_back(iter->first);
01388 }
01389 }
01390 }
01391 }
01392 }
01393 }
01394 ++m;
01395 }
01396
01397 if(ids.size() > 0)
01398 {
01399 ids.insert(std::pair<int,int>(id, 0));
01400 std::set<int> idsSet;
01401 for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
01402 {
01403 idsSet.insert(idsSet.end(), iter->first);
01404 UINFO("Node %d", iter->first);
01405 }
01406 UINFO("idsSet=%d", idsSet.size());
01407 dbDriver_->generateGraph(path.toStdString(), idsSet);
01408 }
01409 else
01410 {
01411 QMessageBox::critical(this, tr("Error"), tr("No neighbors found for signature %1.").arg(id));
01412 }
01413 }
01414 }
01415 }
01416 }
01417
01418 void DatabaseViewer::generateTOROGraph()
01419 {
01420 if(graphes_.empty())
01421 {
01422 this->updateGraphView();
01423 if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
01424 {
01425 QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
01426 return;
01427 }
01428 }
01429
01430 if(!graphes_.size() || !graphLinks_.size())
01431 {
01432 QMessageBox::warning(this, tr("Cannot generate a TORO graph"), tr("No poses or no links..."));
01433 return;
01434 }
01435 bool ok = false;
01436 int id = QInputDialog::getInt(this, tr("Which iteration?"), tr("Iteration (0 -> %1)").arg((int)graphes_.size()-1), (int)graphes_.size()-1, 0, (int)graphes_.size()-1, 1, &ok);
01437
01438 if(ok)
01439 {
01440 QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/constraints" + QString::number(id) + ".graph", tr("TORO file (*.graph)"));
01441 if(!path.isEmpty())
01442 {
01443 bool varianceIgnored = uStr2Bool(ui_->parameters_toolbox->getParameters().at(Parameters::kOptimizerVarianceIgnored()));
01444 if(varianceIgnored)
01445 {
01446 std::multimap<int, rtabmap::Link> links = graphLinks_;
01447 for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
01448 {
01449 iter->second.setInfMatrix(cv::Mat::eye(6,6,CV_64FC1));
01450 }
01451 graph::exportPoses(path.toStdString(), 3, uValueAt(graphes_, id), links);
01452 }
01453 else
01454 {
01455 graph::exportPoses(path.toStdString(), 3, uValueAt(graphes_, id), graphLinks_);
01456 }
01457 }
01458 }
01459 }
01460
01461 void DatabaseViewer::generateG2OGraph()
01462 {
01463 if(graphes_.empty())
01464 {
01465 this->updateGraphView();
01466 if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
01467 {
01468 QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
01469 return;
01470 }
01471 }
01472
01473 if(!graphes_.size() || !graphLinks_.size())
01474 {
01475 QMessageBox::warning(this, tr("Cannot generate a g2o graph"), tr("No poses or no links..."));
01476 return;
01477 }
01478 bool ok = false;
01479 int id = QInputDialog::getInt(this, tr("Which iteration?"), tr("Iteration (0 -> %1)").arg((int)graphes_.size()-1), (int)graphes_.size()-1, 0, (int)graphes_.size()-1, 1, &ok);
01480
01481 if(ok)
01482 {
01483 QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/constraints" + QString::number(id) + ".g2o", tr("g2o file (*.g2o)"));
01484 if(!path.isEmpty())
01485 {
01486 bool varianceIgnored = uStr2Bool(ui_->parameters_toolbox->getParameters().at(Parameters::kOptimizerVarianceIgnored()));
01487 bool robust = uStr2Bool(ui_->parameters_toolbox->getParameters().at(Parameters::kOptimizerRobust()));
01488 if(varianceIgnored)
01489 {
01490 std::multimap<int, rtabmap::Link> links = graphLinks_;
01491 for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
01492 {
01493 iter->second.setInfMatrix(cv::Mat::eye(6,6,CV_64FC1));
01494 }
01495 graph::exportPoses(path.toStdString(), 4, uValueAt(graphes_, id), links, std::map<int, double>(), robust);
01496 }
01497 else
01498 {
01499 graph::exportPoses(path.toStdString(), 4, uValueAt(graphes_, id), graphLinks_, std::map<int, double>(), robust);
01500 }
01501 }
01502 }
01503 }
01504
01505 void DatabaseViewer::view3DMap()
01506 {
01507 if(!ids_.size() || !dbDriver_)
01508 {
01509 QMessageBox::warning(this, tr("Cannot view 3D map"), tr("The database is empty..."));
01510 return;
01511 }
01512
01513 if(graphes_.empty())
01514 {
01515 this->updateGraphView();
01516 if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
01517 {
01518 QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
01519 return;
01520 }
01521 }
01522 bool ok = false;
01523 QStringList items;
01524 items.append("1");
01525 items.append("2");
01526 items.append("4");
01527 items.append("8");
01528 items.append("16");
01529 QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation"), items, 2, false, &ok);
01530 if(ok)
01531 {
01532 int decimation = item.toInt();
01533 double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 100, 2, &ok);
01534 if(ok)
01535 {
01536 std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
01537 if(ui_->groupBox_posefiltering->isChecked())
01538 {
01539 optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
01540 ui_->doubleSpinBox_posefilteringRadius->value(),
01541 ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
01542 }
01543 if(optimizedPoses.size() > 0)
01544 {
01545 rtabmap::ProgressDialog progressDialog(this);
01546 progressDialog.setMaximumSteps((int)optimizedPoses.size());
01547 progressDialog.show();
01548
01549
01550 QDialog * window = new QDialog(this, Qt::Window);
01551 window->setModal(this->isModal());
01552 window->setWindowTitle(tr("3D Map"));
01553 window->setMinimumWidth(800);
01554 window->setMinimumHeight(600);
01555
01556 rtabmap::CloudViewer * viewer = new rtabmap::CloudViewer(window);
01557
01558 QVBoxLayout *layout = new QVBoxLayout();
01559 layout->addWidget(viewer);
01560 viewer->setCameraLockZ(false);
01561 window->setLayout(layout);
01562 connect(window, SIGNAL(finished(int)), viewer, SLOT(clear()));
01563
01564 window->show();
01565
01566 for(std::map<int, Transform>::const_iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
01567 {
01568 rtabmap::Transform pose = iter->second;
01569 if(!pose.isNull())
01570 {
01571 SensorData data;
01572 dbDriver_->getNodeData(iter->first, data);
01573 data.uncompressData();
01574 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
01575 UASSERT(data.imageRaw().empty() || data.imageRaw().type()==CV_8UC3 || data.imageRaw().type() == CV_8UC1);
01576 UASSERT(data.depthOrRightRaw().empty() || data.depthOrRightRaw().type()==CV_8UC1 || data.depthOrRightRaw().type() == CV_16UC1 || data.depthOrRightRaw().type() == CV_32FC1);
01577 cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth, 0, 0, ui_->parameters_toolbox->getParameters());
01578
01579 if(cloud->size())
01580 {
01581 QColor color = Qt::red;
01582 int mapId, weight;
01583 Transform odomPose, groundTruth;
01584 std::string label;
01585 double stamp;
01586 if(dbDriver_->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, groundTruth))
01587 {
01588 color = (Qt::GlobalColor)(mapId % 12 + 7 );
01589 }
01590
01591 viewer->addCloud(uFormat("cloud%d", iter->first), cloud, pose, color);
01592
01593 UINFO("Generated %d (%d points)", iter->first, cloud->size());
01594 progressDialog.appendText(QString("Generated %1 (%2 points)").arg(iter->first).arg(cloud->size()));
01595 }
01596 else
01597 {
01598 UINFO("Empty cloud %d", iter->first);
01599 progressDialog.appendText(QString("Empty cloud %1").arg(iter->first));
01600 }
01601 progressDialog.incrementStep();
01602 QApplication::processEvents();
01603 }
01604 }
01605 progressDialog.setValue(progressDialog.maximumSteps());
01606 }
01607 else
01608 {
01609 QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
01610 }
01611 }
01612 }
01613 }
01614
01615 void DatabaseViewer::view3DLaserScans()
01616 {
01617 if(!ids_.size() || !dbDriver_)
01618 {
01619 QMessageBox::warning(this, tr("Cannot view 3D laser scans"), tr("The database is empty..."));
01620 return;
01621 }
01622
01623 if(graphes_.empty())
01624 {
01625 this->updateGraphView();
01626 if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
01627 {
01628 QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
01629 return;
01630 }
01631 }
01632 bool ok = false;
01633 int downsamplingStepSize = QInputDialog::getInt(this, tr("Downsampling?"), tr("Downsample step size (1 = no filtering)"), 1, 1, 99999, 1, &ok);
01634 if(ok)
01635 {
01636 std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
01637 if(ui_->groupBox_posefiltering->isChecked())
01638 {
01639 optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
01640 ui_->doubleSpinBox_posefilteringRadius->value(),
01641 ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
01642 }
01643 if(optimizedPoses.size() > 0)
01644 {
01645 rtabmap::ProgressDialog progressDialog(this);
01646 progressDialog.setMaximumSteps((int)optimizedPoses.size());
01647 progressDialog.show();
01648
01649
01650 QDialog * window = new QDialog(this, Qt::Window);
01651 window->setModal(this->isModal());
01652 window->setWindowTitle(tr("3D Laser Scans"));
01653 window->setMinimumWidth(800);
01654 window->setMinimumHeight(600);
01655
01656 rtabmap::CloudViewer * viewer = new rtabmap::CloudViewer(window);
01657
01658 QVBoxLayout *layout = new QVBoxLayout();
01659 layout->addWidget(viewer);
01660 viewer->setCameraLockZ(false);
01661 window->setLayout(layout);
01662 connect(window, SIGNAL(finished(int)), viewer, SLOT(clear()));
01663
01664 window->show();
01665
01666 for(std::map<int, Transform>::const_iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
01667 {
01668 rtabmap::Transform pose = iter->second;
01669 if(!pose.isNull())
01670 {
01671 SensorData data;
01672 dbDriver_->getNodeData(iter->first, data);
01673 cv::Mat scan;
01674 data.uncompressDataConst(0, 0, &scan);
01675 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
01676 UASSERT(scan.empty() || scan.type()==CV_32FC2 || scan.type() == CV_32FC3);
01677
01678 if(downsamplingStepSize>1)
01679 {
01680 scan = util3d::downsample(scan, downsamplingStepSize);
01681 }
01682 cloud = util3d::laserScanToPointCloud(scan);
01683
01684 if(cloud->size())
01685 {
01686 QColor color = Qt::red;
01687 int mapId, weight;
01688 Transform odomPose, groundTruth;
01689 std::string label;
01690 double stamp;
01691 if(dbDriver_->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, groundTruth))
01692 {
01693 color = (Qt::GlobalColor)(mapId % 12 + 7 );
01694 }
01695
01696 int normalK = uStr2Int(ui_->parameters_toolbox->getParameters().at(Parameters::kIcpPointToPlaneNormalNeighbors()));
01697 pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, normalK);
01698 pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
01699 pcl::concatenateFields(*cloud, *normals, *cloudNormals);
01700
01701 viewer->addCloud(uFormat("cloud%d", iter->first), cloudNormals, pose, color);
01702
01703 UINFO("Generated %d (%d points)", iter->first, cloud->size());
01704 progressDialog.appendText(QString("Generated %1 (%2 points)").arg(iter->first).arg(cloud->size()));
01705 }
01706 else
01707 {
01708 UINFO("Empty cloud %d", iter->first);
01709 progressDialog.appendText(QString("Empty cloud %1").arg(iter->first));
01710 }
01711 progressDialog.incrementStep();
01712 QApplication::processEvents();
01713 }
01714 }
01715 progressDialog.setValue(progressDialog.maximumSteps());
01716 }
01717 else
01718 {
01719 QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
01720 }
01721 }
01722 }
01723
01724 void DatabaseViewer::generate3DMap()
01725 {
01726 if(!ids_.size() || !dbDriver_)
01727 {
01728 QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
01729 return;
01730 }
01731
01732 if(graphes_.empty())
01733 {
01734 this->updateGraphView();
01735 if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
01736 {
01737 QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
01738 return;
01739 }
01740 }
01741
01742 bool ok = false;
01743 QStringList items;
01744 items.append("1");
01745 items.append("2");
01746 items.append("4");
01747 items.append("8");
01748 items.append("16");
01749 QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation"), items, 2, false, &ok);
01750 if(ok)
01751 {
01752 int decimation = item.toInt();
01753 double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 100, 2, &ok);
01754 if(ok)
01755 {
01756 QMessageBox::StandardButton b = QMessageBox::question(
01757 this,
01758 tr("Assembling?"),
01759 tr("Do you want to assemble all the point clouds (creating only one file with a density of 1pt/cm)?"),
01760 QMessageBox::Yes|QMessageBox::No,
01761 QMessageBox::Yes);
01762
01763 bool assemble = b == QMessageBox::Yes;
01764 QString path;
01765 if(assemble)
01766 {
01767 path = QFileDialog::getSaveFileName(this, tr("Save point cloud"),
01768 pathDatabase_+QDir::separator()+"cloud.ply",
01769 tr("Point Cloud (*.ply *.pcd)"));
01770 }
01771 else
01772 {
01773 path = QFileDialog::getExistingDirectory(this, tr("Save directory"), pathDatabase_);
01774 }
01775 if(!path.isEmpty())
01776 {
01777 std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
01778 if(ui_->groupBox_posefiltering->isChecked())
01779 {
01780 optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
01781 ui_->doubleSpinBox_posefilteringRadius->value(),
01782 ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
01783 }
01784 if(optimizedPoses.size() > 0)
01785 {
01786 rtabmap::ProgressDialog progressDialog;
01787 progressDialog.setMaximumSteps((int)optimizedPoses.size());
01788 progressDialog.show();
01789
01790 pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
01791 for(std::map<int, Transform>::const_iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
01792 {
01793 const rtabmap::Transform & pose = iter->second;
01794 if(!pose.isNull())
01795 {
01796 SensorData data;
01797 dbDriver_->getNodeData(iter->first, data);
01798 data.uncompressData();
01799 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
01800 UASSERT(data.imageRaw().empty() || data.imageRaw().type()==CV_8UC3 || data.imageRaw().type() == CV_8UC1);
01801 UASSERT(data.depthOrRightRaw().empty() || data.depthOrRightRaw().type()==CV_8UC1 || data.depthOrRightRaw().type() == CV_16UC1 || data.depthOrRightRaw().type() == CV_32FC1);
01802 pcl::IndicesPtr validIndices(new std::vector<int>);
01803 cloud = util3d::cloudRGBFromSensorData(data, decimation, maxDepth, 0, validIndices.get(), ui_->parameters_toolbox->getParameters());
01804
01805 if(assemble)
01806 {
01807 if(cloud->size())
01808 {
01809 cloud = util3d::voxelize(cloud, validIndices, 0.01);
01810
01811 if(cloud->size())
01812 {
01813 cloud = rtabmap::util3d::transformPointCloud(cloud, pose);
01814 if(assembledCloud->size() == 0)
01815 {
01816 *assembledCloud = *cloud;
01817 }
01818 else
01819 {
01820 *assembledCloud += *cloud;
01821 }
01822 }
01823 }
01824 UINFO("Created cloud %d (%d points)", iter->first, (int)cloud->size());
01825 progressDialog.appendText(QString("Created cloud %1 (%2 points)").arg(iter->first).arg(cloud->size()));
01826 }
01827 else
01828 {
01829 std::string name = uFormat("%s/node%d.pcd", path.toStdString().c_str(), iter->first);
01830 if(cloud->size())
01831 {
01832 cloud = rtabmap::util3d::transformPointCloud(cloud, pose);
01833 pcl::io::savePCDFile(name, *cloud);
01834 UINFO("Saved %s (%d points)", name.c_str(), cloud->size());
01835 progressDialog.appendText(QString("Saved %1 (%2 points)").arg(name.c_str()).arg(cloud->size()));
01836 }
01837 else
01838 {
01839 UINFO("Ignored empty cloud %s", name.c_str());
01840 progressDialog.appendText(QString("Ignored empty cloud %1").arg(name.c_str()));
01841 }
01842 }
01843 progressDialog.incrementStep();
01844 QApplication::processEvents();
01845 }
01846 }
01847
01848 if(assemble && assembledCloud->size())
01849 {
01850
01851 progressDialog.appendText(QString("Voxelize assembled cloud (%1 points)").arg(assembledCloud->size()));
01852 QApplication::processEvents();
01853 assembledCloud = util3d::voxelize(assembledCloud, 0.01);
01854 if(QFileInfo(path).suffix() == "ply")
01855 {
01856 pcl::io::savePLYFile(path.toStdString(), *assembledCloud);
01857 }
01858 else
01859 {
01860 pcl::io::savePCDFile(path.toStdString(), *assembledCloud);
01861 }
01862 progressDialog.appendText(QString("Saved %1 (%2 points)").arg(path).arg(assembledCloud->size()));
01863 QApplication::processEvents();
01864 }
01865 QMessageBox::information(this, tr("Finished"), tr("%1 clouds generated to %2.").arg(optimizedPoses.size()).arg(path));
01866
01867 progressDialog.setValue(progressDialog.maximumSteps());
01868 }
01869 else
01870 {
01871 QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
01872 }
01873 }
01874 }
01875 }
01876 }
01877
01878 void DatabaseViewer::generate3DLaserScans()
01879 {
01880 if(!ids_.size() || !dbDriver_)
01881 {
01882 QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
01883 return;
01884 }
01885 bool ok = false;
01886 int downsamplingStepSize = QInputDialog::getInt(this, tr("Downsampling?"), tr("Downsample step size (1 = no filtering)"), 1, 1, 99999, 1, &ok);
01887 if(ok)
01888 {
01889 QString path = QFileDialog::getSaveFileName(this, tr("Save point cloud"),
01890 pathDatabase_+QDir::separator()+"cloud.ply",
01891 tr("Point Cloud (*.ply *.pcd)"));
01892 if(!path.isEmpty())
01893 {
01894 std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
01895 if(ui_->groupBox_posefiltering->isChecked())
01896 {
01897 optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
01898 ui_->doubleSpinBox_posefilteringRadius->value(),
01899 ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
01900 }
01901 if(optimizedPoses.size() > 0)
01902 {
01903 rtabmap::ProgressDialog progressDialog;
01904 progressDialog.setMaximumSteps((int)optimizedPoses.size());
01905 progressDialog.show();
01906
01907 pcl::PointCloud<pcl::PointXYZ>::Ptr assembledCloud(new pcl::PointCloud<pcl::PointXYZ>);
01908 for(std::map<int, Transform>::const_iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
01909 {
01910 const rtabmap::Transform & pose = iter->second;
01911 if(!pose.isNull())
01912 {
01913 SensorData data;
01914 dbDriver_->getNodeData(iter->first, data);
01915 cv::Mat scan;
01916 data.uncompressDataConst(0, 0, &scan);
01917 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
01918 UASSERT(scan.empty() || scan.type()==CV_32FC2 || scan.type() == CV_32FC3);
01919
01920 if(downsamplingStepSize > 1)
01921 {
01922 scan = util3d::downsample(scan, downsamplingStepSize);
01923 }
01924 cloud = util3d::laserScanToPointCloud(scan);
01925
01926 if(cloud->size())
01927 {
01928 cloud = rtabmap::util3d::transformPointCloud(cloud, pose);
01929 if(assembledCloud->size() == 0)
01930 {
01931 *assembledCloud = *cloud;
01932 }
01933 else
01934 {
01935 *assembledCloud += *cloud;
01936 }
01937 }
01938 UINFO("Created cloud %d (%d points)", iter->first, (int)cloud->size());
01939 progressDialog.appendText(QString("Created cloud %1 (%2 points)").arg(iter->first).arg(cloud->size()));
01940
01941 progressDialog.incrementStep();
01942 QApplication::processEvents();
01943 }
01944 }
01945
01946 if(assembledCloud->size())
01947 {
01948
01949 progressDialog.appendText(QString("Voxelize assembled cloud (%1 points)").arg(assembledCloud->size()));
01950 QApplication::processEvents();
01951 assembledCloud = util3d::voxelize(assembledCloud, 0.01);
01952 if(QFileInfo(path).suffix() == "ply")
01953 {
01954 pcl::io::savePLYFile(path.toStdString(), *assembledCloud);
01955 }
01956 else
01957 {
01958 pcl::io::savePCDFile(path.toStdString(), *assembledCloud);
01959 }
01960 progressDialog.appendText(QString("Saved %1 (%2 points)").arg(path).arg(assembledCloud->size()));
01961 QApplication::processEvents();
01962 }
01963 QMessageBox::information(this, tr("Finished"), tr("%1 clouds generated to %2.").arg(optimizedPoses.size()).arg(path));
01964
01965 progressDialog.setValue(progressDialog.maximumSteps());
01966 }
01967 else
01968 {
01969 QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
01970 }
01971 }
01972 }
01973 }
01974
01975 void DatabaseViewer::detectMoreLoopClosures()
01976 {
01977 if(graphes_.empty())
01978 {
01979 this->updateGraphView();
01980 if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
01981 {
01982 QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
01983 return;
01984 }
01985 }
01986
01987 const std::map<int, Transform> & optimizedPoses = graphes_.back();
01988
01989 int iterations = ui_->spinBox_detectMore_iterations->value();
01990 UASSERT(iterations > 0);
01991 int added = 0;
01992 for(int n=0; n<iterations; ++n)
01993 {
01994 UINFO("iteration %d/%d", n+1, iterations);
01995 std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(
01996 optimizedPoses,
01997 ui_->doubleSpinBox_detectMore_radius->value(),
01998 ui_->doubleSpinBox_detectMore_angle->value()*CV_PI/180.0);
01999 std::set<int> addedLinks;
02000 for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end(); ++iter)
02001 {
02002 int from = iter->first;
02003 int to = iter->second;
02004 if(from < to)
02005 {
02006 from = iter->second;
02007 to = iter->first;
02008 }
02009 if(!findActiveLink(from, to).isValid() && !containsLink(linksRemoved_, from, to) &&
02010 addedLinks.find(from) == addedLinks.end() && addedLinks.find(to) == addedLinks.end())
02011 {
02012 if(addConstraint(from, to, true, false))
02013 {
02014 UINFO("Added new loop closure between %d and %d.", from, to);
02015 ++added;
02016 addedLinks.insert(from);
02017 addedLinks.insert(to);
02018 }
02019 }
02020 }
02021 UINFO("Iteration %d/%d: added %d loop closures.", n+1, iterations, (int)addedLinks.size()/2);
02022 if(addedLinks.size() == 0)
02023 {
02024 break;
02025 }
02026 }
02027 if(added)
02028 {
02029 this->updateGraphView();
02030 }
02031 UINFO("Total added %d loop closures.", added);
02032 }
02033
02034 void DatabaseViewer::refineAllNeighborLinks()
02035 {
02036 if(neighborLinks_.size())
02037 {
02038 rtabmap::ProgressDialog progressDialog(this);
02039 progressDialog.setMaximumSteps(neighborLinks_.size());
02040 progressDialog.show();
02041
02042 for(int i=0; i<neighborLinks_.size(); ++i)
02043 {
02044 int from = neighborLinks_[i].from();
02045 int to = neighborLinks_[i].to();
02046 this->refineConstraint(neighborLinks_[i].from(), neighborLinks_[i].to(), true, false);
02047
02048 progressDialog.appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(neighborLinks_.size()));
02049 progressDialog.incrementStep();
02050 QApplication::processEvents();
02051 }
02052 this->updateGraphView();
02053
02054 progressDialog.setValue(progressDialog.maximumSteps());
02055 progressDialog.appendText("Refining links finished!");
02056 }
02057 }
02058
02059 void DatabaseViewer::refineAllLoopClosureLinks()
02060 {
02061 if(loopLinks_.size())
02062 {
02063 rtabmap::ProgressDialog progressDialog(this);
02064 progressDialog.setMaximumSteps(loopLinks_.size());
02065 progressDialog.show();
02066
02067 for(int i=0; i<loopLinks_.size(); ++i)
02068 {
02069 int from = loopLinks_[i].from();
02070 int to = loopLinks_[i].to();
02071 this->refineConstraint(loopLinks_[i].from(), loopLinks_[i].to(), true, false);
02072
02073 progressDialog.appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(loopLinks_.size()));
02074 progressDialog.incrementStep();
02075 QApplication::processEvents();
02076 }
02077 this->updateGraphView();
02078
02079 progressDialog.setValue(progressDialog.maximumSteps());
02080 progressDialog.appendText("Refining links finished!");
02081 }
02082 }
02083
02084 void DatabaseViewer::refineVisuallyAllNeighborLinks()
02085 {
02086 if(neighborLinks_.size())
02087 {
02088 rtabmap::ProgressDialog progressDialog(this);
02089 progressDialog.setMaximumSteps(neighborLinks_.size());
02090 progressDialog.show();
02091
02092 for(int i=0; i<neighborLinks_.size(); ++i)
02093 {
02094 int from = neighborLinks_[i].from();
02095 int to = neighborLinks_[i].to();
02096 this->refineConstraintVisually(neighborLinks_[i].from(), neighborLinks_[i].to(), true, false);
02097
02098 progressDialog.appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(neighborLinks_.size()));
02099 progressDialog.incrementStep();
02100 QApplication::processEvents();
02101 }
02102 this->updateGraphView();
02103
02104 progressDialog.setValue(progressDialog.maximumSteps());
02105 progressDialog.appendText("Refining links finished!");
02106 }
02107 }
02108
02109 void DatabaseViewer::refineVisuallyAllLoopClosureLinks()
02110 {
02111 if(loopLinks_.size())
02112 {
02113 rtabmap::ProgressDialog progressDialog(this);
02114 progressDialog.setMaximumSteps(loopLinks_.size());
02115 progressDialog.show();
02116
02117 for(int i=0; i<loopLinks_.size(); ++i)
02118 {
02119 int from = loopLinks_[i].from();
02120 int to = loopLinks_[i].to();
02121 this->refineConstraintVisually(loopLinks_[i].from(), loopLinks_[i].to(), true, false);
02122
02123 progressDialog.appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(loopLinks_.size()));
02124 progressDialog.incrementStep();
02125 QApplication::processEvents();
02126 }
02127 this->updateGraphView();
02128
02129 progressDialog.setValue(progressDialog.maximumSteps());
02130 progressDialog.appendText("Refining links finished!");
02131 }
02132 }
02133
02134 void DatabaseViewer::resetAllChanges()
02135 {
02136 linksAdded_.clear();
02137 linksRefined_.clear();
02138 linksRemoved_.clear();
02139 updateLoopClosuresSlider();
02140 this->updateGraphView();
02141 }
02142
02143 void DatabaseViewer::sliderAValueChanged(int value)
02144 {
02145 this->update(value,
02146 ui_->label_indexA,
02147 ui_->label_parentsA,
02148 ui_->label_childrenA,
02149 ui_->label_weightA,
02150 ui_->label_labelA,
02151 ui_->label_stampA,
02152 ui_->graphicsView_A,
02153 cloudViewerA_,
02154 ui_->label_idA,
02155 ui_->label_mapA,
02156 ui_->label_poseA,
02157 ui_->label_calibA,
02158 true);
02159 }
02160
02161 void DatabaseViewer::sliderBValueChanged(int value)
02162 {
02163 this->update(value,
02164 ui_->label_indexB,
02165 ui_->label_parentsB,
02166 ui_->label_childrenB,
02167 ui_->label_weightB,
02168 ui_->label_labelB,
02169 ui_->label_stampB,
02170 ui_->graphicsView_B,
02171 cloudViewerB_,
02172 ui_->label_idB,
02173 ui_->label_mapB,
02174 ui_->label_poseB,
02175 ui_->label_calibB,
02176 true);
02177 }
02178
02179 void DatabaseViewer::update(int value,
02180 QLabel * labelIndex,
02181 QLabel * labelParents,
02182 QLabel * labelChildren,
02183 QLabel * weight,
02184 QLabel * label,
02185 QLabel * stamp,
02186 rtabmap::ImageView * view,
02187 rtabmap::CloudViewer * view3D,
02188 QLabel * labelId,
02189 QLabel * labelMapId,
02190 QLabel * labelPose,
02191 QLabel * labelCalib,
02192 bool updateConstraintView)
02193 {
02194 UTimer timer;
02195 labelIndex->setText(QString::number(value));
02196 labelParents->clear();
02197 labelChildren->clear();
02198 weight->clear();
02199 label->clear();
02200 labelMapId->clear();
02201 labelPose->clear();
02202 stamp->clear();
02203 labelCalib->clear();
02204 QRectF rect;
02205 if(value >= 0 && value < ids_.size())
02206 {
02207 view->clear();
02208 int id = ids_.at(value);
02209 int mapId = -1;
02210 labelId->setText(QString::number(id));
02211 if(id>0)
02212 {
02213
02214 QImage img;
02215 QImage imgDepth;
02216 if(dbDriver_)
02217 {
02218 SensorData data;
02219 dbDriver_->getNodeData(id, data);
02220 data.uncompressData();
02221 if(!data.imageRaw().empty())
02222 {
02223 img = uCvMat2QImage(ui_->label_indexB==labelIndex?data.imageRaw():data.imageRaw());
02224 }
02225 if(!data.depthOrRightRaw().empty())
02226 {
02227 cv::Mat depth =data.depthOrRightRaw();
02228 if(!data.depthRaw().empty())
02229 {
02230 if(ui_->spinBox_mesh_fillDepthHoles->value() > 0)
02231 {
02232 depth = util2d::fillDepthHoles(depth, ui_->spinBox_mesh_fillDepthHoles->value(), float(ui_->spinBox_mesh_depthError->value())/100.0f);
02233 }
02234 }
02235 imgDepth = uCvMat2QImage(depth);
02236 }
02237
02238 std::list<int> ids;
02239 ids.push_back(id);
02240 std::list<Signature*> signatures;
02241 dbDriver_->loadSignatures(ids, signatures);
02242
02243 if(signatures.size() && signatures.front()!=0 && signatures.front()->getWords().size())
02244 {
02245 view->setFeatures(signatures.front()->getWords(), data.depthOrRightRaw().type() == CV_8UC1?cv::Mat():data.depthOrRightRaw(), Qt::yellow);
02246 delete signatures.front();
02247 signatures.clear();
02248 }
02249
02250 Transform odomPose, g;
02251 int w;
02252 std::string l;
02253 double s;
02254 dbDriver_->getNodeInfo(id, odomPose, mapId, w, l, s, g);
02255
02256 weight->setNum(w);
02257 label->setText(l.c_str());
02258 float x,y,z,roll,pitch,yaw;
02259 odomPose.getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw);
02260 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));
02261 if(s!=0.0)
02262 {
02263 stamp->setText(QDateTime::fromMSecsSinceEpoch(s*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
02264 }
02265 if(data.cameraModels().size() || data.stereoCameraModel().isValidForProjection())
02266 {
02267 if(data.cameraModels().size())
02268 {
02269 if(!data.depthRaw().empty() && data.depthRaw().cols!=data.imageRaw().cols && data.imageRaw().cols)
02270 {
02271 labelCalib->setText(tr("%1 %2x%3 [%8x%9] fx=%4 fy=%5 cx=%6 cy=%7")
02272 .arg(data.cameraModels().size())
02273 .arg(data.cameraModels()[0].imageWidth()>0?data.cameraModels()[0].imageWidth():data.imageRaw().cols/data.cameraModels().size())
02274 .arg(data.cameraModels()[0].imageHeight()>0?data.cameraModels()[0].imageHeight():data.imageRaw().rows)
02275 .arg(data.cameraModels()[0].fx())
02276 .arg(data.cameraModels()[0].fy())
02277 .arg(data.cameraModels()[0].cx())
02278 .arg(data.cameraModels()[0].cy())
02279 .arg(data.depthRaw().cols/data.cameraModels().size())
02280 .arg(data.depthRaw().rows));
02281 }
02282 else
02283 {
02284 labelCalib->setText(tr("%1 %2x%3 fx=%4 fy=%5 cx=%6 cy=%7")
02285 .arg(data.cameraModels().size())
02286 .arg(data.cameraModels()[0].imageWidth()>0?data.cameraModels()[0].imageWidth():data.imageRaw().cols/data.cameraModels().size())
02287 .arg(data.cameraModels()[0].imageHeight()>0?data.cameraModels()[0].imageHeight():data.imageRaw().rows)
02288 .arg(data.cameraModels()[0].fx())
02289 .arg(data.cameraModels()[0].fy())
02290 .arg(data.cameraModels()[0].cx())
02291 .arg(data.cameraModels()[0].cy()));
02292 }
02293 }
02294 else
02295 {
02296
02297 labelCalib->setText(tr("%1x%2 fx=%3 fy=%4 cx=%5 cy=%6 baseline=%7m")
02298 .arg(data.stereoCameraModel().left().imageWidth()>0?data.stereoCameraModel().left().imageWidth():data.imageRaw().cols)
02299 .arg(data.stereoCameraModel().left().imageHeight()>0?data.stereoCameraModel().left().imageHeight():data.imageRaw().rows)
02300 .arg(data.stereoCameraModel().left().fx())
02301 .arg(data.stereoCameraModel().left().fy())
02302 .arg(data.stereoCameraModel().left().cx())
02303 .arg(data.stereoCameraModel().left().cy())
02304 .arg(data.stereoCameraModel().baseline()));
02305 }
02306
02307 }
02308 else
02309 {
02310 labelCalib->setText("NA");
02311 }
02312
02313
02314 if(!data.depthOrRightRaw().empty() && data.depthOrRightRaw().type() == CV_8UC1)
02315 {
02316 this->updateStereo(&data);
02317 }
02318 else
02319 {
02320 stereoViewer_->clear();
02321 ui_->graphicsView_stereo->clear();
02322 }
02323
02324
02325 if(view3D->isVisible() &&
02326 (!data.depthOrRightRaw().empty() ||
02327 !data.laserScanRaw().empty()))
02328 {
02329 if(!data.depthOrRightRaw().empty())
02330 {
02331 if(!data.imageRaw().empty())
02332 {
02333 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
02334 if(!data.depthRaw().empty() && data.cameraModels().size()==1)
02335 {
02336 cv::Mat depth = data.depthRaw();
02337 if(ui_->spinBox_mesh_fillDepthHoles->value() > 0)
02338 {
02339 depth = util2d::fillDepthHoles(depth, ui_->spinBox_mesh_fillDepthHoles->value(), float(ui_->spinBox_mesh_depthError->value())/100.0f);
02340 }
02341 cloud = util3d::cloudFromDepthRGB(
02342 data.imageRaw(),
02343 depth,
02344 data.cameraModels()[0]);
02345 }
02346 else
02347 {
02348 cloud = util3d::cloudRGBFromSensorData(data, 1, 0, 0, 0, ui_->parameters_toolbox->getParameters());
02349 }
02350 if(cloud->size())
02351 {
02352 if(ui_->checkBox_showMesh->isChecked() && !cloud->is_dense)
02353 {
02354 Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
02355 if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
02356 {
02357 viewpoint[0] = data.cameraModels()[0].localTransform().x();
02358 viewpoint[1] = data.cameraModels()[0].localTransform().y();
02359 viewpoint[2] = data.cameraModels()[0].localTransform().z();
02360 }
02361 else if(!data.stereoCameraModel().localTransform().isNull())
02362 {
02363 viewpoint[0] = data.stereoCameraModel().localTransform().x();
02364 viewpoint[1] = data.stereoCameraModel().localTransform().y();
02365 viewpoint[2] = data.stereoCameraModel().localTransform().z();
02366 }
02367 std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
02368 cloud,
02369 float(ui_->spinBox_mesh_angleTolerance->value())*M_PI/180.0f,
02370 ui_->checkBox_mesh_quad->isChecked(),
02371 ui_->spinBox_mesh_triangleSize->value(),
02372 viewpoint);
02373 view3D->removeCloud("0");
02374 view3D->addCloudMesh("0", cloud, polygons);
02375 }
02376 else
02377 {
02378 view3D->addCloud("0", cloud);
02379 }
02380 }
02381 }
02382 else
02383 {
02384 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
02385 cloud = util3d::cloudFromSensorData(data, 1, 0, 0, 0, ui_->parameters_toolbox->getParameters());
02386 if(cloud->size())
02387 {
02388 view3D->addCloud("0", cloud);
02389 }
02390 }
02391 }
02392
02393
02394 pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(data.laserScanRaw());
02395 if(scan->size())
02396 {
02397 view3D->addCloud("1", scan);
02398 }
02399
02400 view3D->update();
02401 }
02402 }
02403
02404 if(!img.isNull())
02405 {
02406 view->setImage(img);
02407 rect = img.rect();
02408 }
02409 else
02410 {
02411 ULOGGER_DEBUG("Image is empty");
02412 }
02413
02414 if(!imgDepth.isNull())
02415 {
02416 view->setImageDepth(imgDepth);
02417 if(!img.isNull())
02418 {
02419 rect = imgDepth.rect();
02420 }
02421 }
02422 else
02423 {
02424 ULOGGER_DEBUG("Image depth is empty");
02425 }
02426
02427
02428 std::map<int, rtabmap::Link> links;
02429 dbDriver_->loadLinks(id, links);
02430 if(links.size())
02431 {
02432 QString strParents, strChildren;
02433 for(std::map<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
02434 {
02435 if(iter->second.type() != Link::kNeighbor &&
02436 iter->second.type() != Link::kNeighborMerged)
02437 {
02438 if(iter->first < id)
02439 {
02440 strChildren.append(QString("%1 ").arg(iter->first));
02441 }
02442 else
02443 {
02444 strParents.append(QString("%1 ").arg(iter->first));
02445 }
02446 }
02447 }
02448 labelParents->setText(strParents);
02449 labelChildren->setText(strChildren);
02450 }
02451 }
02452
02453 if(mapId>=0)
02454 {
02455 labelMapId->setText(QString::number(mapId));
02456 }
02457 }
02458 else
02459 {
02460 ULOGGER_ERROR("Slider index out of range ?");
02461 }
02462
02463 updateConstraintButtons();
02464 updateWordsMatching();
02465
02466 if(updateConstraintView && ui_->dockWidget_constraints->isVisible())
02467 {
02468
02469 int from = ids_.at(ui_->horizontalSlider_A->value());
02470 int to = ids_.at(ui_->horizontalSlider_B->value());
02471 bool set = false;
02472 for(int i=0; i<loopLinks_.size() || i<neighborLinks_.size(); ++i)
02473 {
02474 if(i < loopLinks_.size())
02475 {
02476 if((loopLinks_[i].from() == from && loopLinks_[i].to() == to) ||
02477 (loopLinks_[i].from() == to && loopLinks_[i].to() == from))
02478 {
02479 if(i != ui_->horizontalSlider_loops->value())
02480 {
02481 ui_->horizontalSlider_loops->blockSignals(true);
02482 ui_->horizontalSlider_loops->setValue(i);
02483 ui_->horizontalSlider_loops->blockSignals(false);
02484 this->updateConstraintView(loopLinks_[i].from() == from?loopLinks_.at(i):loopLinks_.at(i).inverse(), false);
02485 }
02486 ui_->horizontalSlider_neighbors->blockSignals(true);
02487 ui_->horizontalSlider_neighbors->setValue(0);
02488 ui_->horizontalSlider_neighbors->blockSignals(false);
02489 set = true;
02490 break;
02491 }
02492 }
02493 if(i < neighborLinks_.size())
02494 {
02495 if((neighborLinks_[i].from() == from && neighborLinks_[i].to() == to) ||
02496 (neighborLinks_[i].from() == to && neighborLinks_[i].to() == from))
02497 {
02498 if(i != ui_->horizontalSlider_neighbors->value())
02499 {
02500 ui_->horizontalSlider_neighbors->blockSignals(true);
02501 ui_->horizontalSlider_neighbors->setValue(i);
02502 ui_->horizontalSlider_neighbors->blockSignals(false);
02503 this->updateConstraintView(neighborLinks_[i].from() == from?neighborLinks_.at(i):neighborLinks_.at(i).inverse(), false);
02504 }
02505 ui_->horizontalSlider_loops->blockSignals(true);
02506 ui_->horizontalSlider_loops->setValue(0);
02507 ui_->horizontalSlider_loops->blockSignals(false);
02508 set = true;
02509 break;
02510 }
02511 }
02512 }
02513 if(!set)
02514 {
02515 ui_->horizontalSlider_loops->blockSignals(true);
02516 ui_->horizontalSlider_neighbors->blockSignals(true);
02517 ui_->horizontalSlider_loops->setValue(0);
02518 ui_->horizontalSlider_neighbors->setValue(0);
02519 ui_->horizontalSlider_loops->blockSignals(false);
02520 ui_->horizontalSlider_neighbors->blockSignals(false);
02521
02522 constraintsViewer_->removeAllClouds();
02523
02524
02525 if(graphes_.size())
02526 {
02527 std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
02528 if(optimizedPoses.size() > 0)
02529 {
02530 std::map<int, Transform>::iterator fromIter = optimizedPoses.find(from);
02531 std::map<int, Transform>::iterator toIter = optimizedPoses.find(to);
02532 if(fromIter != optimizedPoses.end() &&
02533 toIter != optimizedPoses.end())
02534 {
02535 Link link(from, to, Link::kUndef, fromIter->second.inverse() * toIter->second);
02536 this->updateConstraintView(link, false);
02537 }
02538 }
02539 }
02540
02541 constraintsViewer_->update();
02542
02543 }
02544 }
02545
02546 if(rect.isValid())
02547 {
02548 view->setSceneRect(rect);
02549 }
02550 }
02551
02552 void DatabaseViewer::updateLoggerLevel()
02553 {
02554 if(this->parent() == 0)
02555 {
02556 ULogger::setLevel((ULogger::Level)ui_->comboBox_logger_level->currentIndex());
02557 }
02558 }
02559
02560 void DatabaseViewer::updateStereo()
02561 {
02562 if(ui_->horizontalSlider_A->maximum())
02563 {
02564 int id = ids_.at(ui_->horizontalSlider_A->value());
02565 SensorData data;
02566 dbDriver_->getNodeData(id, data);
02567 data.uncompressData();
02568 updateStereo(&data);
02569 }
02570 }
02571
02572 void DatabaseViewer::updateStereo(const SensorData * data)
02573 {
02574 if(data &&
02575 ui_->dockWidget_stereoView->isVisible() &&
02576 !data->imageRaw().empty() &&
02577 !data->depthOrRightRaw().empty() &&
02578 data->depthOrRightRaw().type() == CV_8UC1 &&
02579 data->stereoCameraModel().isValidForProjection())
02580 {
02581 cv::Mat leftMono;
02582 if(data->imageRaw().channels() == 3)
02583 {
02584 cv::cvtColor(data->imageRaw(), leftMono, CV_BGR2GRAY);
02585 }
02586 else
02587 {
02588 leftMono = data->imageRaw();
02589 }
02590
02591 UTimer timer;
02592 ParametersMap parameters = ui_->parameters_toolbox->getParameters();
02593 bool opticalFlow = uStr2Bool(parameters.at(Parameters::kStereoOpticalFlow()));
02594 Stereo * stereo = 0;
02595 if(opticalFlow)
02596 {
02597 stereo = new StereoOpticalFlow(parameters);
02598 }
02599 else
02600 {
02601 stereo = new Stereo(parameters);
02602 }
02603
02604
02605 std::vector<cv::KeyPoint> kpts;
02606 uInsert(parameters, ParametersPair(Parameters::kKpMaxFeatures(), parameters.at(Parameters::kVisMaxFeatures())));
02607 uInsert(parameters, ParametersPair(Parameters::kKpMinDepth(), parameters.at(Parameters::kVisMinDepth())));
02608 uInsert(parameters, ParametersPair(Parameters::kKpMaxDepth(), parameters.at(Parameters::kVisMaxDepth())));
02609 uInsert(parameters, ParametersPair(Parameters::kKpDetectorStrategy(), parameters.at(Parameters::kVisFeatureType())));
02610 uInsert(parameters, ParametersPair(Parameters::kKpRoiRatios(), parameters.at(Parameters::kVisRoiRatios())));
02611 uInsert(parameters, ParametersPair(Parameters::kKpSubPixEps(), parameters.at(Parameters::kVisSubPixEps())));
02612 uInsert(parameters, ParametersPair(Parameters::kKpSubPixIterations(), parameters.at(Parameters::kVisSubPixIterations())));
02613 uInsert(parameters, ParametersPair(Parameters::kKpSubPixWinSize(), parameters.at(Parameters::kVisSubPixWinSize())));
02614 Feature2D * kptDetector = Feature2D::create(parameters);
02615 kpts = kptDetector->generateKeypoints(leftMono);
02616 delete kptDetector;
02617
02618 float timeKpt = timer.ticks();
02619
02620 std::vector<cv::Point2f> leftCorners;
02621 cv::KeyPoint::convert(kpts, leftCorners);
02622
02623
02624 std::vector<unsigned char> status;
02625 std::vector<cv::Point2f> rightCorners;
02626
02627 rightCorners = stereo->computeCorrespondences(
02628 leftMono,
02629 data->rightRaw(),
02630 leftCorners,
02631 status);
02632 delete stereo;
02633
02634 float timeStereo = timer.ticks();
02635
02636 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
02637 cloud->resize(kpts.size());
02638 float bad_point = std::numeric_limits<float>::quiet_NaN ();
02639 UASSERT(status.size() == kpts.size());
02640 int oi = 0;
02641 int inliers = 0;
02642 int flowOutliers= 0;
02643 int slopeOutliers= 0;
02644 int negativeDisparityOutliers = 0;
02645 for(unsigned int i=0; i<status.size(); ++i)
02646 {
02647 cv::Point3f pt(bad_point, bad_point, bad_point);
02648 if(status[i])
02649 {
02650 float disparity = leftCorners[i].x - rightCorners[i].x;
02651 if(disparity > 0.0f)
02652 {
02653 cv::Point3f tmpPt = util3d::projectDisparityTo3D(
02654 leftCorners[i],
02655 disparity,
02656 data->stereoCameraModel());
02657
02658 if(util3d::isFinite(tmpPt))
02659 {
02660 pt = util3d::transformPoint(tmpPt, data->stereoCameraModel().left().localTransform());
02661 status[i] = 100;
02662 ++inliers;
02663 cloud->at(oi++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
02664 }
02665 }
02666 else
02667 {
02668 status[i] = 102;
02669 ++negativeDisparityOutliers;
02670 }
02671 }
02672 else
02673 {
02674 ++flowOutliers;
02675 }
02676 }
02677 cloud->resize(oi);
02678
02679 UINFO("correspondences = %d/%d (%f) (time kpt=%fs stereo=%fs)",
02680 (int)cloud->size(), (int)leftCorners.size(), float(cloud->size())/float(leftCorners.size()), timeKpt, timeStereo);
02681
02682 stereoViewer_->updateCameraTargetPosition(Transform::getIdentity());
02683 stereoViewer_->addCloud("stereo", cloud);
02684 stereoViewer_->update();
02685
02686 ui_->label_stereo_inliers->setNum(inliers);
02687 ui_->label_stereo_flowOutliers->setNum(flowOutliers);
02688 ui_->label_stereo_slopeOutliers->setNum(slopeOutliers);
02689 ui_->label_stereo_disparityOutliers->setNum(negativeDisparityOutliers);
02690
02691 std::vector<cv::KeyPoint> rightKpts;
02692 cv::KeyPoint::convert(rightCorners, rightKpts);
02693 std::vector<cv::DMatch> good_matches(kpts.size());
02694 for(unsigned int i=0; i<good_matches.size(); ++i)
02695 {
02696 good_matches[i].trainIdx = i;
02697 good_matches[i].queryIdx = i;
02698 }
02699
02700
02701
02702
02703
02704
02705
02706
02707
02708
02709 ui_->graphicsView_stereo->clear();
02710 ui_->graphicsView_stereo->setLinesShown(true);
02711 ui_->graphicsView_stereo->setFeaturesShown(false);
02712 ui_->graphicsView_stereo->setImageDepthShown(true);
02713
02714 ui_->graphicsView_stereo->setImage(uCvMat2QImage(data->imageRaw()));
02715 ui_->graphicsView_stereo->setImageDepth(uCvMat2QImage(data->depthOrRightRaw()));
02716
02717
02718 for(unsigned int i=0; i<kpts.size(); ++i)
02719 {
02720 if(rightKpts[i].pt.x > 0 && rightKpts[i].pt.y > 0)
02721 {
02722 QColor c = Qt::green;
02723 if(status[i] == 0)
02724 {
02725 c = Qt::red;
02726 }
02727 else if(status[i] == 100)
02728 {
02729 c = Qt::blue;
02730 }
02731 else if(status[i] == 101)
02732 {
02733 c = Qt::yellow;
02734 }
02735 else if(status[i] == 102)
02736 {
02737 c = Qt::magenta;
02738 }
02739 else if(status[i] == 110)
02740 {
02741 c = Qt::cyan;
02742 }
02743 ui_->graphicsView_stereo->addLine(
02744 kpts[i].pt.x,
02745 kpts[i].pt.y,
02746 rightKpts[i].pt.x,
02747 rightKpts[i].pt.y,
02748 c,
02749 QString("%1: (%2,%3) -> (%4,%5)").arg(i).arg(kpts[i].pt.x).arg(kpts[i].pt.y).arg(rightKpts[i].pt.x).arg(rightKpts[i].pt.y));
02750 }
02751 }
02752 ui_->graphicsView_stereo->update();
02753 }
02754 }
02755
02756 void DatabaseViewer::updateWordsMatching()
02757 {
02758 int from = ids_.at(ui_->horizontalSlider_A->value());
02759 int to = ids_.at(ui_->horizontalSlider_B->value());
02760 if(from && to)
02761 {
02762 int alpha = 70;
02763 ui_->graphicsView_A->clearLines();
02764 ui_->graphicsView_A->setFeaturesColor(QColor(255, 255, 0, alpha));
02765 ui_->graphicsView_B->clearLines();
02766 ui_->graphicsView_B->setFeaturesColor(QColor(255, 255, 0, alpha));
02767
02768 const QMultiMap<int, KeypointItem*> & wordsA = ui_->graphicsView_A->getFeatures();
02769 const QMultiMap<int, KeypointItem*> & wordsB = ui_->graphicsView_B->getFeatures();
02770 if(wordsA.size() && wordsB.size())
02771 {
02772 QList<int> ids = wordsA.uniqueKeys();
02773 for(int i=0; i<ids.size(); ++i)
02774 {
02775 if(wordsA.count(ids[i]) == 1 && wordsB.count(ids[i]) == 1)
02776 {
02777
02778 ui_->graphicsView_A->setFeatureColor(ids[i], Qt::magenta);
02779 ui_->graphicsView_B->setFeatureColor(ids[i], Qt::magenta);
02780
02781
02782
02783 float scaleX = ui_->graphicsView_A->viewScale();
02784 float deltaX = 0;
02785 float deltaY = 0;
02786
02787 if(ui_->checkBox_verticalLayout->isChecked())
02788 {
02789 deltaY = ui_->graphicsView_A->height()/scaleX;
02790 }
02791 else
02792 {
02793 deltaX = ui_->graphicsView_A->width()/scaleX;
02794 }
02795
02796 const KeypointItem * kptA = wordsA.value(ids[i]);
02797 const KeypointItem * kptB = wordsB.value(ids[i]);
02798 ui_->graphicsView_A->addLine(
02799 kptA->rect().x()+kptA->rect().width()/2,
02800 kptA->rect().y()+kptA->rect().height()/2,
02801 kptB->rect().x()+kptB->rect().width()/2+deltaX,
02802 kptB->rect().y()+kptB->rect().height()/2+deltaY,
02803 Qt::cyan);
02804
02805 ui_->graphicsView_B->addLine(
02806 kptA->rect().x()+kptA->rect().width()/2-deltaX,
02807 kptA->rect().y()+kptA->rect().height()/2-deltaY,
02808 kptB->rect().x()+kptB->rect().width()/2,
02809 kptB->rect().y()+kptB->rect().height()/2,
02810 Qt::cyan);
02811 }
02812 }
02813 ui_->graphicsView_A->update();
02814 ui_->graphicsView_B->update();
02815 }
02816 }
02817 }
02818
02819 void DatabaseViewer::sliderAMoved(int value)
02820 {
02821 ui_->label_indexA->setText(QString::number(value));
02822 if(value>=0 && value < ids_.size())
02823 {
02824 ui_->label_idA->setText(QString::number(ids_.at(value)));
02825 }
02826 else
02827 {
02828 ULOGGER_ERROR("Slider index out of range ?");
02829 }
02830 }
02831
02832 void DatabaseViewer::sliderBMoved(int value)
02833 {
02834 ui_->label_indexB->setText(QString::number(value));
02835 if(value>=0 && value < ids_.size())
02836 {
02837 ui_->label_idB->setText(QString::number(ids_.at(value)));
02838 }
02839 else
02840 {
02841 ULOGGER_ERROR("Slider index out of range ?");
02842 }
02843 }
02844
02845 void DatabaseViewer::update3dView()
02846 {
02847 if(ui_->dockWidget_view3d->isVisible())
02848 {
02849 sliderAValueChanged(ui_->horizontalSlider_A->value());
02850 sliderBValueChanged(ui_->horizontalSlider_B->value());
02851 }
02852 }
02853
02854 void DatabaseViewer::sliderNeighborValueChanged(int value)
02855 {
02856 this->updateConstraintView(neighborLinks_.at(value));
02857 }
02858
02859 void DatabaseViewer::sliderLoopValueChanged(int value)
02860 {
02861 this->updateConstraintView(loopLinks_.at(value));
02862 }
02863
02864
02865 void DatabaseViewer::updateConstraintView()
02866 {
02867 if(ids_.size())
02868 {
02869 Link link = this->findActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value()));
02870 if(link.isValid())
02871 {
02872 if(link.type() == Link::kNeighbor ||
02873 link.type() == Link::kNeighborMerged)
02874 {
02875 this->updateConstraintView(neighborLinks_.at(ui_->horizontalSlider_neighbors->value()), false);
02876 }
02877 else
02878 {
02879 this->updateConstraintView(loopLinks_.at(ui_->horizontalSlider_loops->value()), false);
02880 }
02881 }
02882 }
02883 }
02884
02885 void DatabaseViewer::updateConstraintView(
02886 const rtabmap::Link & linkIn,
02887 bool updateImageSliders,
02888 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudFrom,
02889 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudTo,
02890 const pcl::PointCloud<pcl::PointXYZ>::Ptr & scanFrom,
02891 const pcl::PointCloud<pcl::PointXYZ>::Ptr & scanTo)
02892 {
02893 std::multimap<int, Link>::iterator iterLink = rtabmap::graph::findLink(linksRefined_, linkIn.from(), linkIn.to());
02894 rtabmap::Link link = linkIn;
02895 if(iterLink != linksRefined_.end())
02896 {
02897 link = iterLink->second;
02898 }
02899 else if(ui_->checkBox_ignorePoseCorrection->isChecked())
02900 {
02901 if(link.type() == Link::kNeighbor ||
02902 link.type() == Link::kNeighborMerged)
02903 {
02904 Transform poseFrom = uValue(poses_, link.from(), Transform());
02905 Transform poseTo = uValue(poses_, link.to(), Transform());
02906 if(!poseFrom.isNull() && !poseTo.isNull())
02907 {
02908 link.setTransform(poseFrom.inverse() * poseTo);
02909 }
02910 }
02911 }
02912 rtabmap::Transform t = link.transform();
02913
02914 ui_->label_constraint->clear();
02915 ui_->label_constraint_opt->clear();
02916 ui_->checkBox_showOptimized->setEnabled(false);
02917 UASSERT(!t.isNull() && dbDriver_);
02918
02919 ui_->label_type->setText(tr("%1 (%2)")
02920 .arg(link.type())
02921 .arg(link.type()==Link::kNeighbor?"Neighbor":
02922 link.type()==Link::kNeighbor?"Merged neighbor":
02923 link.type()==Link::kGlobalClosure?"Loop closure":
02924 link.type()==Link::kLocalSpaceClosure?"Space proximity link":
02925 link.type()==Link::kLocalTimeClosure?"Time proximity link":
02926 link.type()==Link::kUserClosure?"User link":
02927 link.type()==Link::kVirtualClosure?"Virtual link":"Undefined"));
02928 ui_->label_variance->setText(QString("%1, %2")
02929 .arg(sqrt(link.rotVariance()))
02930 .arg(sqrt(link.transVariance())));
02931 ui_->label_constraint->setText(QString("%1").arg(t.prettyPrint().c_str()).replace(" ", "\n"));
02932 if((link.type() == Link::kNeighbor || link.type() == Link::kNeighborMerged) &&
02933 graphes_.size() &&
02934 (int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
02935 {
02936 std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
02937 if(link.type() == Link::kNeighbor || link.type() == Link::kNeighborMerged)
02938 {
02939 std::map<int, rtabmap::Transform>::iterator iterFrom = graph.find(link.from());
02940 std::map<int, rtabmap::Transform>::iterator iterTo = graph.find(link.to());
02941 if(iterFrom != graph.end() && iterTo != graph.end())
02942 {
02943 ui_->checkBox_showOptimized->setEnabled(true);
02944 Transform topt = iterFrom->second.inverse()*iterTo->second;
02945 float diff = topt.getDistance(t);
02946 Transform v1 = t.rotation()*Transform(1,0,0,0,0,0);
02947 Transform v2 = topt.rotation()*Transform(1,0,0,0,0,0);
02948 float a = pcl::getAngle3D(Eigen::Vector4f(v1.x(), v1.y(), v1.z(), 0), Eigen::Vector4f(v2.x(), v2.y(), v2.z(), 0));
02949 a = (a *180.0f) / CV_PI;
02950 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));
02951
02952 if(ui_->checkBox_showOptimized->isChecked())
02953 {
02954 t = topt;
02955 }
02956 }
02957 }
02958 }
02959
02960 if(updateImageSliders)
02961 {
02962 ui_->horizontalSlider_A->blockSignals(true);
02963 ui_->horizontalSlider_B->blockSignals(true);
02964
02965 ui_->horizontalSlider_A->setValue(idToIndex_.value(link.from()));
02966 ui_->horizontalSlider_B->setValue(idToIndex_.value(link.to()));
02967 ui_->horizontalSlider_A->blockSignals(false);
02968 ui_->horizontalSlider_B->blockSignals(false);
02969 this->update(idToIndex_.value(link.from()),
02970 ui_->label_indexA,
02971 ui_->label_parentsA,
02972 ui_->label_childrenA,
02973 ui_->label_weightA,
02974 ui_->label_labelA,
02975 ui_->label_stampA,
02976 ui_->graphicsView_A,
02977 cloudViewerA_,
02978 ui_->label_idA,
02979 ui_->label_mapA,
02980 ui_->label_poseA,
02981 ui_->label_calibA,
02982 false);
02983 this->update(idToIndex_.value(link.to()),
02984 ui_->label_indexB,
02985 ui_->label_parentsB,
02986 ui_->label_childrenB,
02987 ui_->label_weightB,
02988 ui_->label_labelB,
02989 ui_->label_stampB,
02990 ui_->graphicsView_B,
02991 cloudViewerB_,
02992 ui_->label_idB,
02993 ui_->label_mapB,
02994 ui_->label_poseB,
02995 ui_->label_calibB,
02996 false);
02997 }
02998
02999 if(constraintsViewer_->isVisible())
03000 {
03001 SensorData dataFrom, dataTo;
03002
03003 dbDriver_->getNodeData(link.from(), dataFrom);
03004 dataFrom.uncompressData();
03005 UASSERT(dataFrom.imageRaw().empty() || dataFrom.imageRaw().type()==CV_8UC3 || dataFrom.imageRaw().type() == CV_8UC1);
03006 UASSERT(dataFrom.depthOrRightRaw().empty() || dataFrom.depthOrRightRaw().type()==CV_8UC1 || dataFrom.depthOrRightRaw().type() == CV_16UC1 || dataFrom.depthOrRightRaw().type() == CV_32FC1);
03007
03008 dbDriver_->getNodeData(link.to(), dataTo);
03009 dataTo.uncompressData();
03010 UASSERT(dataTo.imageRaw().empty() || dataTo.imageRaw().type()==CV_8UC3 || dataTo.imageRaw().type() == CV_8UC1);
03011 UASSERT(dataTo.depthOrRightRaw().empty() || dataTo.depthOrRightRaw().type()==CV_8UC1 || dataTo.depthOrRightRaw().type() == CV_16UC1 || dataTo.depthOrRightRaw().type() == CV_32FC1);
03012
03013
03014 if(cloudFrom->size() == 0 && cloudTo->size() == 0)
03015 {
03016
03017 if(ui_->checkBox_show3Dclouds->isChecked())
03018 {
03019 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom, cloudTo;
03020 if(!dataFrom.imageRaw().empty() && !dataFrom.depthOrRightRaw().empty())
03021 {
03022 cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, 1, 0, 0, 0, ui_->parameters_toolbox->getParameters());
03023 }
03024 if(!dataTo.imageRaw().empty() && !dataTo.depthOrRightRaw().empty())
03025 {
03026 cloudTo=util3d::cloudRGBFromSensorData(dataTo, 1, 0, 0, 0, ui_->parameters_toolbox->getParameters());
03027 }
03028
03029 if(cloudFrom.get() && cloudFrom->size())
03030 {
03031 constraintsViewer_->addCloud("cloud0", cloudFrom, Transform::getIdentity(), Qt::red);
03032 }
03033 if(cloudTo.get() && cloudTo->size())
03034 {
03035 cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t);
03036 constraintsViewer_->addCloud("cloud1", cloudTo, Transform::getIdentity(), Qt::cyan);
03037 }
03038 }
03039 else
03040 {
03041 constraintsViewer_->removeCloud("cloud0");
03042 constraintsViewer_->removeCloud("cloud1");
03043 }
03044 if(ui_->checkBox_show3DWords->isChecked())
03045 {
03046 std::list<int> ids;
03047 ids.push_back(link.from());
03048 ids.push_back(link.to());
03049 std::list<Signature*> signatures;
03050 dbDriver_->loadSignatures(ids, signatures);
03051 if(signatures.size() == 2)
03052 {
03053 const Signature * sFrom = signatures.front();
03054 const Signature * sTo = signatures.back();
03055 UASSERT(sFrom && sTo);
03056 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom(new pcl::PointCloud<pcl::PointXYZ>);
03057 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo(new pcl::PointCloud<pcl::PointXYZ>);
03058 cloudFrom->resize(sFrom->getWords3().size());
03059 cloudTo->resize(sTo->getWords3().size());
03060 int i=0;
03061 for(std::multimap<int, cv::Point3f>::const_iterator iter=sFrom->getWords3().begin();
03062 iter!=sFrom->getWords3().end();
03063 ++iter)
03064 {
03065 cloudFrom->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
03066 }
03067 i=0;
03068 for(std::multimap<int, cv::Point3f>::const_iterator iter=sTo->getWords3().begin();
03069 iter!=sTo->getWords3().end();
03070 ++iter)
03071 {
03072 cloudTo->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
03073 }
03074
03075 if(cloudFrom->size())
03076 {
03077 cloudFrom = rtabmap::util3d::removeNaNFromPointCloud(cloudFrom);
03078 }
03079 if(cloudTo->size())
03080 {
03081 cloudTo = rtabmap::util3d::removeNaNFromPointCloud(cloudTo);
03082 if(cloudTo->size())
03083 {
03084 cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t);
03085 }
03086 }
03087
03088 if(cloudFrom->size())
03089 {
03090 constraintsViewer_->addCloud("words0", cloudFrom, Transform::getIdentity(), Qt::red);
03091 }
03092 else
03093 {
03094 UWARN("Empty 3D words for node %d", link.from());
03095 constraintsViewer_->removeCloud("words0");
03096 }
03097 if(cloudTo->size())
03098 {
03099 constraintsViewer_->addCloud("words1", cloudTo, Transform::getIdentity(), Qt::cyan);
03100 }
03101 else
03102 {
03103 UWARN("Empty 3D words for node %d", link.to());
03104 constraintsViewer_->removeCloud("words1");
03105 }
03106 }
03107 else
03108 {
03109 UERROR("Not found signature %d or %d in RAM", link.from(), link.to());
03110 constraintsViewer_->removeCloud("words0");
03111 constraintsViewer_->removeCloud("words1");
03112 }
03113
03114 for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
03115 {
03116 delete *iter;
03117 }
03118 }
03119 else
03120 {
03121 constraintsViewer_->removeCloud("words0");
03122 constraintsViewer_->removeCloud("words1");
03123 }
03124 }
03125 else
03126 {
03127 if(cloudFrom->size())
03128 {
03129 constraintsViewer_->addCloud("cloud0", cloudFrom, Transform::getIdentity(), Qt::red);
03130 }
03131 if(cloudTo->size())
03132 {
03133 constraintsViewer_->addCloud("cloud1", cloudTo, Transform::getIdentity(), Qt::cyan);
03134 }
03135 }
03136
03137 if(scanFrom->size() == 0 && scanTo->size() == 0)
03138 {
03139 if(ui_->checkBox_show2DScans->isChecked())
03140 {
03141
03142
03143 constraintsViewer_->removeCloud("scan2");
03144 constraintsViewer_->removeGraph("scan2graph");
03145 if(link.type() == Link::kLocalSpaceClosure &&
03146 !link.userDataCompressed().empty())
03147 {
03148 std::vector<int> ids;
03149 cv::Mat userData = link.uncompressUserDataConst();
03150 if(userData.type() == CV_8SC1 &&
03151 userData.rows == 1 &&
03152 userData.cols >= 8 &&
03153 userData.at<char>(userData.cols-1) == 0 &&
03154 memcmp(userData.data, "SCANS:", 6) == 0)
03155 {
03156 std::string scansStr = (const char *)userData.data;
03157 UINFO("Detected \"%s\" in links's user data", scansStr.c_str());
03158 if(!scansStr.empty())
03159 {
03160 std::list<std::string> strs = uSplit(scansStr, ':');
03161 if(strs.size() == 2)
03162 {
03163 std::list<std::string> strIds = uSplit(strs.rbegin()->c_str(), ';');
03164 for(std::list<std::string>::iterator iter=strIds.begin(); iter!=strIds.end(); ++iter)
03165 {
03166 ids.push_back(atoi(iter->c_str()));
03167 if(ids.back() == link.from())
03168 {
03169 ids.pop_back();
03170 }
03171 }
03172 }
03173 }
03174 }
03175 if(ids.size())
03176 {
03177
03178
03179
03180 std::map<int, rtabmap::Transform> poses;
03181 for(unsigned int i=0; i<ids.size(); ++i)
03182 {
03183 if(uContains(poses_, ids[i]))
03184 {
03185 poses.insert(*poses_.find(ids[i]));
03186 }
03187 else
03188 {
03189 UERROR("Not found %d node!", ids[i]);
03190 }
03191 }
03192 if(poses.size())
03193 {
03194 Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
03195
03196 UASSERT(uContains(poses, link.to()));
03197 std::map<int, rtabmap::Transform> posesOut;
03198 std::multimap<int, rtabmap::Link> linksOut;
03199 optimizer->getConnectedGraph(
03200 link.to(),
03201 poses,
03202 updateLinksWithModifications(links_),
03203 posesOut,
03204 linksOut);
03205
03206 if(poses.size() != posesOut.size())
03207 {
03208 UWARN("Scan poses input and output are different! %d vs %d", (int)poses.size(), (int)posesOut.size());
03209 UWARN("Input poses: ");
03210 for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
03211 {
03212 UWARN(" %d", iter->first);
03213 }
03214 UWARN("Input links: ");
03215 std::multimap<int, Link> modifiedLinks = updateLinksWithModifications(links_);
03216 for(std::multimap<int, Link>::iterator iter=modifiedLinks.begin(); iter!=modifiedLinks.end(); ++iter)
03217 {
03218 UWARN(" %d->%d", iter->second.from(), iter->second.to());
03219 }
03220 }
03221
03222 QTime time;
03223 time.start();
03224 std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(link.to(), posesOut, linksOut);
03225 delete optimizer;
03226
03227
03228 Transform u = t * finalPoses.at(link.to()).inverse();
03229 pcl::PointCloud<pcl::PointXYZ>::Ptr assembledScans(new pcl::PointCloud<pcl::PointXYZ>);
03230 pcl::PointCloud<pcl::PointXYZ>::Ptr graph(new pcl::PointCloud<pcl::PointXYZ>);
03231 for(std::map<int, Transform>::iterator iter=finalPoses.begin(); iter!=finalPoses.end(); ++iter)
03232 {
03233 iter->second = u * iter->second;
03234 if(iter->first != link.to())
03235 {
03236
03237 SensorData data;
03238 dbDriver_->getNodeData(iter->first, data);
03239 cv::Mat scan;
03240 data.uncompressDataConst(0, 0, &scan, 0);
03241 if(!scan.empty())
03242 {
03243 pcl::PointCloud<pcl::PointXYZ>::Ptr scanCloud = util3d::laserScanToPointCloud(scan);
03244 if(assembledScans->size() == 0)
03245 {
03246 assembledScans = util3d::transformPointCloud(scanCloud, iter->second);
03247 }
03248 else
03249 {
03250 *assembledScans += *util3d::transformPointCloud(scanCloud, iter->second);
03251 }
03252 }
03253 }
03254 graph->push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
03255 }
03256
03257 if(assembledScans->size())
03258 {
03259 constraintsViewer_->addCloud("scan2", assembledScans, Transform::getIdentity(), Qt::cyan);
03260 }
03261 if(graph->size())
03262 {
03263 constraintsViewer_->addOrUpdateGraph("scan2graph", graph, Qt::cyan);
03264 }
03265 }
03266 }
03267 }
03268
03269
03270 pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
03271 scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw());
03272 scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw());
03273 scanB = rtabmap::util3d::transformPointCloud(scanB, t);
03274 if(scanA->size())
03275 {
03276 constraintsViewer_->addCloud("scan0", scanA, Transform::getIdentity(), Qt::yellow);
03277 }
03278 else
03279 {
03280 constraintsViewer_->removeCloud("scan0");
03281 }
03282 if(scanB->size())
03283 {
03284 constraintsViewer_->addCloud("scan1", scanB, Transform::getIdentity(), Qt::magenta);
03285 }
03286 else
03287 {
03288 constraintsViewer_->removeCloud("scan1");
03289 }
03290 }
03291 else
03292 {
03293 constraintsViewer_->removeCloud("scan0");
03294 constraintsViewer_->removeCloud("scan1");
03295 constraintsViewer_->removeCloud("scan2");
03296 }
03297 }
03298 else
03299 {
03300 if(scanFrom->size())
03301 {
03302 constraintsViewer_->addCloud("scan0", scanFrom, Transform::getIdentity(), Qt::yellow);
03303 }
03304 else
03305 {
03306 constraintsViewer_->removeCloud("scan0");
03307 }
03308 if(scanTo->size())
03309 {
03310 constraintsViewer_->addCloud("scan1", scanTo, Transform::getIdentity(), Qt::magenta);
03311 }
03312 else
03313 {
03314 constraintsViewer_->removeCloud("scan1");
03315 }
03316 constraintsViewer_->removeCloud("scan2");
03317 }
03318
03319
03320
03321 constraintsViewer_->addOrUpdateCoordinate("from_coordinate", Transform::getIdentity(), 0.2);
03322 constraintsViewer_->addOrUpdateCoordinate("to_coordinate", t, 0.2);
03323 if(uContains(groundTruthPoses_, link.from()) && uContains(groundTruthPoses_, link.to()))
03324 {
03325 constraintsViewer_->addOrUpdateCoordinate("to_coordinate_gt",
03326 groundTruthPoses_.at(link.from()).inverse()*groundTruthPoses_.at(link.to()), 0.1);
03327 }
03328
03329 constraintsViewer_->clearTrajectory();
03330
03331 constraintsViewer_->update();
03332 }
03333
03334
03335 updateConstraintButtons();
03336 }
03337
03338 void DatabaseViewer::updateConstraintButtons()
03339 {
03340 ui_->pushButton_refine->setEnabled(false);
03341 ui_->pushButton_refineVisually->setEnabled(false);
03342 ui_->pushButton_reset->setEnabled(false);
03343 ui_->pushButton_add->setEnabled(false);
03344 ui_->pushButton_reject->setEnabled(false);
03345
03346 int from = ids_.at(ui_->horizontalSlider_A->value());
03347 int to = ids_.at(ui_->horizontalSlider_B->value());
03348 if(from!=to && from && to)
03349 {
03350 if((!containsLink(links_, from ,to) && !containsLink(linksAdded_, from ,to)) ||
03351 containsLink(linksRemoved_, from ,to))
03352 {
03353 ui_->pushButton_add->setEnabled(true);
03354 }
03355 }
03356
03357 Link currentLink = findActiveLink(from ,to);
03358
03359 if(currentLink.isValid() &&
03360 ((currentLink.from() == from && currentLink.to() == to) || (currentLink.from() == to && currentLink.to() == from)))
03361 {
03362 if(!containsLink(linksRemoved_, from ,to))
03363 {
03364 ui_->pushButton_reject->setEnabled(
03365 currentLink.type() != Link::kNeighbor &&
03366 currentLink.type() != Link::kNeighborMerged);
03367 }
03368
03369
03370 bool modified = false;
03371 std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, currentLink.from(), currentLink.to());
03372 if(iter != linksRefined_.end())
03373 {
03374 currentLink = iter->second;
03375 ui_->pushButton_reset->setEnabled(true);
03376 modified = true;
03377 }
03378 if(!modified)
03379 {
03380 ui_->pushButton_reset->setEnabled(false);
03381 }
03382 ui_->pushButton_refine->setEnabled(true);
03383 ui_->pushButton_refineVisually->setEnabled(true);
03384 }
03385 }
03386
03387 void DatabaseViewer::sliderIterationsValueChanged(int value)
03388 {
03389 if(dbDriver_ && value >=0 && value < (int)graphes_.size())
03390 {
03391 std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, value);
03392 std::map<int, rtabmap::Transform> graphFiltered = graph;
03393 if(ui_->groupBox_posefiltering->isChecked())
03394 {
03395 graphFiltered = graph::radiusPosesFiltering(graph,
03396 ui_->doubleSpinBox_posefilteringRadius->value(),
03397 ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
03398 }
03399 if(ui_->dockWidget_graphView->isVisible())
03400 {
03401
03402 UINFO("Update local maps list...");
03403 std::vector<int> ids = uKeys(graphFiltered);
03404 for(unsigned int i=0; i<ids.size(); ++i)
03405 {
03406 if(localMaps_.find(ids[i]) == localMaps_.end())
03407 {
03408 UTimer time;
03409 bool added = false;
03410 if(ui_->groupBox_gridFromProjection->isChecked())
03411 {
03412 SensorData data;
03413 dbDriver_->getNodeData(ids.at(i), data);
03414 data.uncompressData();
03415 if(!data.depthOrRightRaw().empty())
03416 {
03417 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
03418 pcl::IndicesPtr validIndices(new std::vector<int>);
03419 cloud = util3d::cloudFromSensorData(data,
03420 ui_->spinBox_projDecimation->value(),
03421 ui_->doubleSpinBox_projMaxDepth->value(),
03422 ui_->doubleSpinBox_projMinDepth->value(),
03423 validIndices.get(),
03424 ui_->parameters_toolbox->getParameters());
03425 UASSERT(ui_->doubleSpinBox_gridCellSize->value() > 0);
03426 cloud = util3d::voxelize(cloud, validIndices, ui_->doubleSpinBox_gridCellSize->value());
03427
03428
03429 float roll, pitch, yaw;
03430 graphFiltered.at(ids[i]).getEulerAngles(roll, pitch, yaw);
03431 cloud = util3d::transformPointCloud(cloud, Transform(0,0,0, roll, pitch, 0));
03432
03433 if(cloud->size())
03434 {
03435 UTimer timer;
03436 float cellSize = ui_->doubleSpinBox_gridCellSize->value();
03437 float groundNormalMaxAngle = ui_->doubleSpinBox_projMaxAngle->value();
03438 int minClusterSize = ui_->spinBox_projClusterSize->value();
03439 cv::Mat ground, obstacles;
03440
03441 util3d::occupancy2DFromCloud3D<pcl::PointXYZ>(
03442 cloud,
03443 ground, obstacles,
03444 cellSize,
03445 groundNormalMaxAngle,
03446 minClusterSize);
03447
03448 if(!ground.empty() || !obstacles.empty())
03449 {
03450 localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(ground, obstacles)));
03451 added = true;
03452 }
03453 }
03454 }
03455 }
03456 else
03457 {
03458 SensorData data;
03459 dbDriver_->getNodeData(ids.at(i), data);
03460 if(!data.laserScanCompressed().empty())
03461 {
03462 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
03463 cv::Mat laserScan;
03464 data.uncompressDataConst(0, 0, &laserScan);
03465 cv::Mat ground, obstacles;
03466 if(laserScan.type() == CV_32FC2)
03467 {
03468 util3d::occupancy2DFromLaserScan(
03469 laserScan,
03470 ground,
03471 obstacles,
03472 ui_->doubleSpinBox_gridCellSize->value(),
03473 ui_->checkBox_gridFillUnkownSpace->isChecked(),
03474 data.laserScanMaxRange());
03475 added = true;
03476 }
03477 localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(ground, obstacles)));
03478 }
03479 }
03480 if(added)
03481 {
03482 UINFO("Processed grid map %d/%d (%fs)", i+1, (int)ids.size(), time.ticks());
03483 }
03484 }
03485 }
03486
03487 for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end();)
03488 {
03489 if(graphFiltered.find(iter->first) == graphFiltered.end())
03490 {
03491 localMaps_.erase(iter++);
03492 }
03493 else
03494 {
03495 ++iter;
03496 }
03497 }
03498 UINFO("Update local maps list... done");
03499 }
03500
03501 ui_->graphViewer->updateGTGraph(groundTruthPoses_);
03502 ui_->graphViewer->updateGraph(graph, graphLinks_, mapIds_);
03503 if(graph.size() && localMaps_.size() && ui_->graphViewer->isGridMapVisible())
03504 {
03505 float xMin, yMin;
03506 float cell = ui_->doubleSpinBox_gridCellSize->value();
03507 cv::Mat map;
03508 QTime time;
03509 time.start();
03510 map = rtabmap::util3d::create2DMapFromOccupancyLocalMaps(graphFiltered, localMaps_, cell, xMin, yMin, 0, ui_->checkBox_gridErode->isChecked());
03511 if(!map.empty())
03512 {
03513 ui_->graphViewer->updateMap(rtabmap::util3d::convertMap2Image8U(map), cell, xMin, yMin);
03514 }
03515 ui_->label_timeGrid->setNum(double(time.elapsed())/1000.0);
03516 }
03517 ui_->graphViewer->update();
03518 ui_->label_iterations->setNum(value);
03519
03520
03521 float length = 0.0f;
03522 for(std::multimap<int, rtabmap::Link>::const_iterator iter=graphLinks_.begin(); iter!=graphLinks_.end(); ++iter)
03523 {
03524 std::map<int, rtabmap::Transform>::const_iterator jterA = graph.find(iter->first);
03525 std::map<int, rtabmap::Transform>::const_iterator jterB = graph.find(iter->second.to());
03526 if(jterA != graph.end() && jterB != graph.end())
03527 {
03528 const rtabmap::Transform & poseA = jterA->second;
03529 const rtabmap::Transform & poseB = jterB->second;
03530 if(iter->second.type() == rtabmap::Link::kNeighbor ||
03531 iter->second.type() == rtabmap::Link::kNeighborMerged)
03532 {
03533 Eigen::Vector3f vA, vB;
03534 float x,y,z;
03535 poseA.getTranslation(x,y,z);
03536 vA[0] = x; vA[1] = y; vA[2] = z;
03537 poseB.getTranslation(x,y,z);
03538 vB[0] = x; vB[1] = y; vB[2] = z;
03539 length += (vB - vA).norm();
03540 }
03541 }
03542 }
03543 ui_->label_pathLength->setNum(length);
03544 }
03545 }
03546 void DatabaseViewer::updateGraphView()
03547 {
03548 ui_->label_loopClosures->clear();
03549 ui_->label_poses->clear();
03550
03551 if(poses_.size())
03552 {
03553 int fromId = ui_->spinBox_optimizationsFrom->value();
03554 if(!uContains(poses_, fromId))
03555 {
03556 QMessageBox::warning(this, tr(""), tr("Graph optimization from id (%1) for which node is not linked to graph.\n Minimum=%2, Maximum=%3")
03557 .arg(fromId)
03558 .arg(poses_.begin()->first)
03559 .arg(poses_.rbegin()->first));
03560 return;
03561 }
03562
03563 graphes_.clear();
03564 graphLinks_.clear();
03565
03566 std::map<int, rtabmap::Transform> poses = poses_;
03567
03568
03569 if(!ui_->checkBox_spanAllMaps->isChecked() && uContains(mapIds_, fromId) && mapIds_.at(fromId) >= 0)
03570 {
03571 int currentMapId = mapIds_.at(fromId);
03572 for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end();)
03573 {
03574 if(!uContains(mapIds_, iter->first) ||
03575 mapIds_.at(iter->first) != currentMapId)
03576 {
03577 poses.erase(iter++);
03578 }
03579 else
03580 {
03581 ++iter;
03582 }
03583 }
03584 }
03585
03586 graphes_.push_back(poses);
03587
03588 ui_->actionGenerate_TORO_graph_graph->setEnabled(true);
03589 ui_->actionGenerate_g2o_graph_g2o->setEnabled(true);
03590 std::multimap<int, rtabmap::Link> links = links_;
03591
03592
03593 if(!ui_->checkBox_spanAllMaps->isChecked() && uContains(mapIds_, fromId) && mapIds_.at(fromId) >= 0)
03594 {
03595 int currentMapId = mapIds_.at(fromId);
03596 for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end();)
03597 {
03598 if(!uContains(mapIds_, iter->second.from()) ||
03599 !uContains(mapIds_, iter->second.to()) ||
03600 mapIds_.at(iter->second.from()) != currentMapId ||
03601 mapIds_.at(iter->second.to()) != currentMapId)
03602 {
03603 links.erase(iter++);
03604 }
03605 else
03606 {
03607 ++iter;
03608 }
03609 }
03610 }
03611
03612 if(ui_->checkBox_ignorePoseCorrection->isChecked())
03613 {
03614 std::multimap<int, Link> tmp = links;
03615 for(std::multimap<int, Link>::iterator iter=tmp.begin(); iter!=tmp.end(); ++iter)
03616 {
03617 if(iter->second.type() == Link::kNeighbor ||
03618 iter->second.type() == Link::kNeighborMerged)
03619 {
03620 Transform poseFrom = uValue(poses, iter->second.from(), Transform());
03621 Transform poseTo = uValue(poses, iter->second.to(), Transform());
03622 if(!poseFrom.isNull() && !poseTo.isNull())
03623 {
03624 iter->second.setTransform(poseFrom.inverse() * poseTo);
03625 iter->second.setVariance(1.0f, 1.0f);
03626 }
03627 }
03628 }
03629 links = updateLinksWithModifications(tmp);
03630 }
03631 else
03632 {
03633 links = updateLinksWithModifications(links);
03634 }
03635
03636
03637 int totalNeighbor = 0;
03638 int totalNeighborMerged = 0;
03639 int totalGlobal = 0;
03640 int totalLocalTime = 0;
03641 int totalLocalSpace = 0;
03642 int totalUser = 0;
03643 for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end();)
03644 {
03645 if(iter->second.type() == Link::kNeighbor)
03646 {
03647 ++totalNeighbor;
03648 }
03649 else if(iter->second.type() == Link::kNeighborMerged)
03650 {
03651 ++totalNeighborMerged;
03652 }
03653 else if(iter->second.type() == Link::kGlobalClosure)
03654 {
03655 if(ui_->checkBox_ignoreGlobalLoop->isChecked())
03656 {
03657 links.erase(iter++);
03658 continue;
03659 }
03660 ++totalGlobal;
03661 }
03662 else if(iter->second.type() == Link::kLocalSpaceClosure)
03663 {
03664 if(ui_->checkBox_ignoreLocalLoopSpace->isChecked())
03665 {
03666 links.erase(iter++);
03667 continue;
03668 }
03669 ++totalLocalSpace;
03670 }
03671 else if(iter->second.type() == Link::kLocalTimeClosure)
03672 {
03673 if(ui_->checkBox_ignoreLocalLoopTime->isChecked())
03674 {
03675 links.erase(iter++);
03676 continue;
03677 }
03678 ++totalLocalTime;
03679 }
03680 else if(iter->second.type() == Link::kUserClosure)
03681 {
03682 if(ui_->checkBox_ignoreUserLoop->isChecked())
03683 {
03684 links.erase(iter++);
03685 continue;
03686 }
03687 ++totalUser;
03688 }
03689 ++iter;
03690 }
03691 ui_->label_loopClosures->setText(tr("(%1, %2, %3, %4, %5, %6)")
03692 .arg(totalNeighbor)
03693 .arg(totalNeighborMerged)
03694 .arg(totalGlobal)
03695 .arg(totalLocalSpace)
03696 .arg(totalLocalTime)
03697 .arg(totalUser));
03698
03699 Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
03700
03701 std::map<int, rtabmap::Transform> posesOut;
03702 std::multimap<int, rtabmap::Link> linksOut;
03703 UINFO("Get connected graph from %d (%d poses, %d links)", fromId, (int)poses.size(), (int)links.size());
03704 optimizer->getConnectedGraph(
03705 fromId,
03706 poses,
03707 links,
03708 posesOut,
03709 linksOut,
03710 ui_->spinBox_optimizationDepth->value());
03711 UINFO("Connected graph of %d poses and %d links", (int)posesOut.size(), (int)linksOut.size());
03712 QTime time;
03713 time.start();
03714 std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(fromId, posesOut, linksOut, &graphes_);
03715 ui_->label_timeOptimization->setNum(double(time.elapsed())/1000.0);
03716 graphes_.push_back(finalPoses);
03717 graphLinks_ = linksOut;
03718 ui_->label_poses->setNum((int)finalPoses.size());
03719 delete optimizer;
03720 if(posesOut.size() && finalPoses.empty())
03721 {
03722 QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors."));
03723 }
03724
03725 if(uContains(groundTruthPoses_, fromId) && uContains(posesOut, fromId))
03726 {
03727
03728 Transform t = posesOut.at(fromId) * groundTruthPoses_.at(fromId).inverse();
03729 for(std::map<int, Transform>::iterator iter=groundTruthPoses_.begin(); iter!=groundTruthPoses_.end(); ++iter)
03730 {
03731 iter->second = t * iter->second;
03732 }
03733 }
03734 else if(groundTruthPoses_.size())
03735 {
03736 UWARN("Could not find ground truth for root node %d", fromId);
03737 }
03738 }
03739 if(graphes_.size())
03740 {
03741 ui_->horizontalSlider_iterations->setMaximum((int)graphes_.size()-1);
03742 ui_->horizontalSlider_iterations->setValue((int)graphes_.size()-1);
03743 ui_->horizontalSlider_iterations->setEnabled(true);
03744 ui_->spinBox_optimizationsFrom->setEnabled(true);
03745 sliderIterationsValueChanged((int)graphes_.size()-1);
03746 }
03747 else
03748 {
03749 ui_->horizontalSlider_iterations->setEnabled(false);
03750 ui_->spinBox_optimizationsFrom->setEnabled(false);
03751 }
03752 }
03753
03754 void DatabaseViewer::updateGrid()
03755 {
03756 if((sender() != ui_->spinBox_projDecimation && sender() != ui_->doubleSpinBox_projMaxDepth && sender() != ui_->doubleSpinBox_projMinDepth && sender()!=ui_->doubleSpinBox_projMaxAngle && sender()!=ui_->spinBox_projClusterSize) ||
03757 (sender() == ui_->spinBox_projDecimation && ui_->groupBox_gridFromProjection->isChecked()) ||
03758 (sender() == ui_->doubleSpinBox_projMaxDepth && ui_->groupBox_gridFromProjection->isChecked()) ||
03759 (sender() == ui_->doubleSpinBox_projMinDepth && ui_->groupBox_gridFromProjection->isChecked()) ||
03760 (sender() == ui_->doubleSpinBox_projMaxAngle && ui_->groupBox_gridFromProjection->isChecked()) ||
03761 (sender() == ui_->spinBox_projClusterSize && ui_->groupBox_gridFromProjection->isChecked()))
03762 {
03763 localMaps_.clear();
03764 updateGraphView();
03765 }
03766 }
03767
03768 Link DatabaseViewer::findActiveLink(int from, int to)
03769 {
03770 Link link;
03771 std::multimap<int, Link>::iterator findIter = rtabmap::graph::findLink(linksRefined_, from ,to);
03772 if(findIter != linksRefined_.end())
03773 {
03774 link = findIter->second;
03775 }
03776 else
03777 {
03778 findIter = rtabmap::graph::findLink(linksAdded_, from ,to);
03779 if(findIter != linksAdded_.end())
03780 {
03781 link = findIter->second;
03782 }
03783 else if(!containsLink(linksRemoved_, from ,to))
03784 {
03785 findIter = rtabmap::graph::findLink(links_, from ,to);
03786 if(findIter != links_.end())
03787 {
03788 link = findIter->second;
03789 }
03790 }
03791 }
03792 return link;
03793 }
03794
03795 bool DatabaseViewer::containsLink(std::multimap<int, Link> & links, int from, int to)
03796 {
03797 return rtabmap::graph::findLink(links, from, to) != links.end();
03798 }
03799
03800 void DatabaseViewer::refineConstraint()
03801 {
03802 int from = ids_.at(ui_->horizontalSlider_A->value());
03803 int to = ids_.at(ui_->horizontalSlider_B->value());
03804 refineConstraint(from, to, false, true);
03805 }
03806
03807 void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool updateGraph)
03808 {
03809 if(from == to)
03810 {
03811 UWARN("Cannot refine link to same node");
03812 return;
03813 }
03814
03815 Link currentLink = findActiveLink(from, to);
03816 if(!currentLink.isValid())
03817 {
03818 UERROR("Not found link! (%d->%d)", from, to);
03819 return;
03820 }
03821 Transform t = currentLink.transform();
03822 if(ui_->checkBox_showOptimized->isChecked() &&
03823 (currentLink.type() == Link::kNeighbor || currentLink.type() == Link::kNeighborMerged) &&
03824 graphes_.size() &&
03825 (int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
03826 {
03827 std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
03828 if(currentLink.type() == Link::kNeighbor || currentLink.type() == Link::kNeighborMerged)
03829 {
03830 std::map<int, rtabmap::Transform>::iterator iterFrom = graph.find(currentLink.from());
03831 std::map<int, rtabmap::Transform>::iterator iterTo = graph.find(currentLink.to());
03832 if(iterFrom != graph.end() && iterTo != graph.end())
03833 {
03834 Transform topt = iterFrom->second.inverse()*iterTo->second;
03835 t = topt;
03836 }
03837 }
03838 }
03839 else if(ui_->checkBox_ignorePoseCorrection->isChecked() &&
03840 graph::findLink(linksRefined_, from, to) == linksRefined_.end())
03841 {
03842 if(currentLink.type() == Link::kNeighbor ||
03843 currentLink.type() == Link::kNeighborMerged)
03844 {
03845 Transform poseFrom = uValue(poses_, currentLink.from(), Transform());
03846 Transform poseTo = uValue(poses_, currentLink.to(), Transform());
03847 if(!poseFrom.isNull() && !poseTo.isNull())
03848 {
03849 t = poseFrom.inverse() * poseTo;
03850 }
03851 }
03852 }
03853
03854 Transform transform;
03855 RegistrationInfo info;
03856
03857 SensorData dataFrom, dataTo;
03858 dbDriver_->getNodeData(currentLink.from(), dataFrom);
03859 dbDriver_->getNodeData(currentLink.to(), dataTo);
03860
03861 ParametersMap parameters = ui_->parameters_toolbox->getParameters();
03862
03863 UTimer timer;
03864 if(ui_->checkBox_icp_laserScan->isChecked())
03865 {
03866
03867 cv::Mat tmpA, tmpB, tmpC, tmpD;
03868 dataFrom.uncompressData(&tmpA, &tmpB, 0);
03869 dataTo.uncompressData(&tmpC, &tmpD, 0);
03870 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom = util3d::cloudFromSensorData(
03871 dataFrom,
03872 ui_->spinBox_icp_decimation->value(),
03873 ui_->doubleSpinBox_icp_maxDepth->value(),
03874 ui_->doubleSpinBox_icp_minDepth->value(),
03875 0,
03876 ui_->parameters_toolbox->getParameters());
03877 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo = util3d::cloudFromSensorData(
03878 dataTo,
03879 ui_->spinBox_icp_decimation->value(),
03880 ui_->doubleSpinBox_icp_maxDepth->value(),
03881 ui_->doubleSpinBox_icp_minDepth->value(),
03882 0,
03883 ui_->parameters_toolbox->getParameters());
03884 int maxLaserScans = cloudFrom->size();
03885 dataFrom.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0);
03886 dataTo.setLaserScanRaw(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0);
03887
03888 if(!dataFrom.laserScanCompressed().empty() || !dataTo.laserScanCompressed().empty())
03889 {
03890 UWARN("There are laser scans in data, but generate laser scan from "
03891 "depth image option is activated. Ignoring saved laser scans...");
03892 }
03893 }
03894 else
03895 {
03896 cv::Mat tmpA, tmpB;
03897 dataFrom.uncompressData(0, 0, &tmpA);
03898 dataTo.uncompressData(0, 0, &tmpB);
03899 }
03900 UINFO("Uncompress time: %f s", timer.ticks());
03901
03902 RegistrationIcp registration(parameters);
03903 transform = registration.computeTransformation(dataFrom, dataTo, t, &info);
03904 UINFO("Icp time: %f s", timer.ticks());
03905
03906 if(!transform.isNull())
03907 {
03908 Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform, info.variance, info.variance);
03909
03910 bool updated = false;
03911 std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
03912 while(iter != linksRefined_.end() && iter->first == currentLink.from())
03913 {
03914 if(iter->second.to() == currentLink.to() &&
03915 iter->second.type() == currentLink.type())
03916 {
03917 iter->second = newLink;
03918 updated = true;
03919 break;
03920 }
03921 ++iter;
03922 }
03923 if(!updated)
03924 {
03925 linksRefined_.insert(std::make_pair(newLink.from(), newLink));
03926
03927 if(updateGraph)
03928 {
03929 this->updateGraphView();
03930 }
03931 }
03932
03933 if(ui_->dockWidget_constraints->isVisible())
03934 {
03935 this->updateConstraintView(newLink, true);
03936 }
03937 }
03938
03939 else if(!silent)
03940 {
03941 QMessageBox::warning(this,
03942 tr("Refine link"),
03943 tr("Cannot find a transformation between nodes %1 and %2").arg(from).arg(to));
03944 }
03945 }
03946
03947 void DatabaseViewer::refineConstraintVisually()
03948 {
03949 int from = ids_.at(ui_->horizontalSlider_A->value());
03950 int to = ids_.at(ui_->horizontalSlider_B->value());
03951 refineConstraintVisually(from, to, false, true);
03952 }
03953
03954 void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, bool updateGraph)
03955 {
03956 UDEBUG("");
03957 if(from == to)
03958 {
03959 UWARN("Cannot refine link to same node");
03960 return;
03961 }
03962
03963 Link currentLink = findActiveLink(from, to);
03964 if(!currentLink.isValid())
03965 {
03966 UERROR("Not found link! (%d->%d)", from, to);
03967 return;
03968 }
03969
03970 ParametersMap parameters = ui_->parameters_toolbox->getParameters();
03971
03972 Transform t;
03973 RegistrationInfo info;
03974
03975
03976 SensorData dataFrom;
03977 dbDriver_->getNodeData(from, dataFrom);
03978 dataFrom.uncompressData();
03979 SensorData dataTo;
03980 dbDriver_->getNodeData(to, dataTo);
03981 dataTo.uncompressData();
03982
03983
03984 UDEBUG("");
03985 RegistrationVis reg(parameters);
03986 Signature fromS(dataFrom);
03987 Signature toS(dataTo);
03988 t = reg.computeTransformationMod(fromS, toS, currentLink.transform(), &info);
03989 UDEBUG("");
03990
03991 if(!silent)
03992 {
03993 ui_->graphicsView_A->setFeatures(fromS.getWords(), dataFrom.depthRaw());
03994 ui_->graphicsView_B->setFeatures(toS.getWords(), dataTo.depthRaw());
03995 updateWordsMatching();
03996 }
03997
03998 if(!t.isNull())
03999 {
04000 Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), t, info.variance, info.variance);
04001
04002 bool updated = false;
04003 std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
04004 while(iter != linksRefined_.end() && iter->first == currentLink.from())
04005 {
04006 if(iter->second.to() == currentLink.to() &&
04007 iter->second.type() == currentLink.type())
04008 {
04009 iter->second = newLink;
04010 updated = true;
04011 break;
04012 }
04013 ++iter;
04014 }
04015 if(!updated)
04016 {
04017 linksRefined_.insert(std::make_pair(newLink.from(), newLink));
04018
04019 if(updateGraph)
04020 {
04021 this->updateGraphView();
04022 }
04023 }
04024 if(ui_->dockWidget_constraints->isVisible())
04025 {
04026 this->updateConstraintView(newLink, false);
04027 }
04028 }
04029 else if(!silent)
04030 {
04031 QMessageBox::warning(this,
04032 tr("Add link"),
04033 tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(info.rejectedMsg.c_str()));
04034 }
04035 }
04036
04037 void DatabaseViewer::addConstraint()
04038 {
04039 int from = ids_.at(ui_->horizontalSlider_A->value());
04040 int to = ids_.at(ui_->horizontalSlider_B->value());
04041 addConstraint(from, to, false, true);
04042 }
04043
04044 bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGraph)
04045 {
04046 if(from == to)
04047 {
04048 UWARN("Cannot add link to same node");
04049 return false;
04050 }
04051
04052 bool updateSlider = false;
04053 if(!containsLink(linksAdded_, from, to) &&
04054 !containsLink(links_, from, to))
04055 {
04056 UASSERT(!containsLink(linksRemoved_, from, to));
04057 UASSERT(!containsLink(linksRefined_, from, to));
04058
04059 ParametersMap parameters = ui_->parameters_toolbox->getParameters();
04060
04061 Transform t;
04062 RegistrationInfo info;
04063
04064
04065 SensorData dataFrom;
04066 dbDriver_->getNodeData(from, dataFrom);
04067 dataFrom.uncompressData();
04068 SensorData dataTo;
04069 dbDriver_->getNodeData(to, dataTo);
04070 dataTo.uncompressData();
04071
04072
04073 UDEBUG("");
04074 RegistrationVis reg(parameters);
04075 Signature fromS(dataFrom);
04076 Signature toS(dataTo);
04077 t = reg.computeTransformationMod(fromS, toS, Transform::getIdentity(), &info);
04078 UDEBUG("");
04079
04080 if(!silent)
04081 {
04082 ui_->graphicsView_A->setFeatures(fromS.getWords(), dataFrom.depthRaw());
04083 ui_->graphicsView_B->setFeatures(toS.getWords(), dataTo.depthRaw());
04084 updateWordsMatching();
04085 }
04086
04087 if(!t.isNull())
04088 {
04089
04090 if(from>to)
04091 {
04092 linksAdded_.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, info.variance, info.variance)));
04093 }
04094 else
04095 {
04096 linksAdded_.insert(std::make_pair(to, Link(to, from, Link::kUserClosure, t.inverse(), info.variance, info.variance)));
04097 }
04098 updateSlider = true;
04099 }
04100 else if(!silent)
04101 {
04102 QMessageBox::warning(this,
04103 tr("Add link"),
04104 tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(info.rejectedMsg.c_str()));
04105 }
04106 }
04107 else if(containsLink(linksRemoved_, from, to))
04108 {
04109
04110 linksRemoved_.erase(rtabmap::graph::findLink(linksRemoved_, from, to));
04111 updateSlider = true;
04112 }
04113
04114 if(updateSlider)
04115 {
04116 updateLoopClosuresSlider(from, to);
04117 if(updateGraph)
04118 {
04119 this->updateGraphView();
04120 }
04121 }
04122 return updateSlider;
04123 }
04124
04125 void DatabaseViewer::resetConstraint()
04126 {
04127 int from = ids_.at(ui_->horizontalSlider_A->value());
04128 int to = ids_.at(ui_->horizontalSlider_B->value());
04129 if(from < to)
04130 {
04131 int tmp = to;
04132 to = from;
04133 from = tmp;
04134 }
04135
04136 if(from == to)
04137 {
04138 UWARN("Cannot reset link to same node");
04139 return;
04140 }
04141
04142
04143 std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, from, to);
04144 if(iter != linksRefined_.end())
04145 {
04146 linksRefined_.erase(iter);
04147 this->updateGraphView();
04148 }
04149
04150 iter = rtabmap::graph::findLink(links_, from, to);
04151 if(iter != links_.end())
04152 {
04153 this->updateConstraintView(iter->second);
04154 }
04155 iter = rtabmap::graph::findLink(linksAdded_, from, to);
04156 if(iter != linksAdded_.end())
04157 {
04158 this->updateConstraintView(iter->second);
04159 }
04160 }
04161
04162 void DatabaseViewer::rejectConstraint()
04163 {
04164 int from = ids_.at(ui_->horizontalSlider_A->value());
04165 int to = ids_.at(ui_->horizontalSlider_B->value());
04166 if(from < to)
04167 {
04168 int tmp = to;
04169 to = from;
04170 from = tmp;
04171 }
04172
04173 if(from == to)
04174 {
04175 UWARN("Cannot reject link to same node");
04176 return;
04177 }
04178
04179 bool removed = false;
04180
04181
04182 std::multimap<int, Link>::iterator iter;
04183 iter = rtabmap::graph::findLink(links_, from, to);
04184 if(iter != links_.end())
04185 {
04186 if(iter->second.type() == Link::kNeighbor || iter->second.type() == Link::kNeighborMerged)
04187 {
04188 UWARN("Cannot reject neighbor links (%d->%d)", from, to);
04189 return;
04190 }
04191 linksRemoved_.insert(*iter);
04192 removed = true;
04193 }
04194
04195
04196 iter = rtabmap::graph::findLink(linksRefined_, from, to);
04197 if(iter != linksRefined_.end())
04198 {
04199 linksRefined_.erase(iter);
04200 removed = true;
04201 }
04202 iter = rtabmap::graph::findLink(linksAdded_, from, to);
04203 if(iter != linksAdded_.end())
04204 {
04205 linksAdded_.erase(iter);
04206 removed = true;
04207 }
04208 if(removed)
04209 {
04210 this->updateGraphView();
04211 }
04212 updateLoopClosuresSlider();
04213 }
04214
04215 std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
04216 const std::multimap<int, rtabmap::Link> & edgeConstraints)
04217 {
04218 std::multimap<int, rtabmap::Link> links;
04219 for(std::multimap<int, rtabmap::Link>::const_iterator iter=edgeConstraints.begin();
04220 iter!=edgeConstraints.end();
04221 ++iter)
04222 {
04223 std::multimap<int, rtabmap::Link>::iterator findIter;
04224
04225 findIter = rtabmap::graph::findLink(linksRemoved_, iter->second.from(), iter->second.to());
04226 if(findIter != linksRemoved_.end())
04227 {
04228 if(!(iter->second.from() == findIter->second.from() &&
04229 iter->second.to() == findIter->second.to() &&
04230 iter->second.type() == findIter->second.type()))
04231 {
04232 UWARN("Links (%d->%d,%d) and (%d->%d,%d) are not equal!?",
04233 iter->second.from(), iter->second.to(), iter->second.type(),
04234 findIter->second.from(), findIter->second.to(), findIter->second.type());
04235 }
04236 else
04237 {
04238
04239 continue;
04240 }
04241 }
04242
04243 findIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
04244 if(findIter!=linksRefined_.end())
04245 {
04246 if(iter->second.from() == findIter->second.from() &&
04247 iter->second.to() == findIter->second.to() &&
04248 iter->second.type() == findIter->second.type())
04249 {
04250 links.insert(*findIter);
04251
04252 continue;
04253 }
04254 else
04255 {
04256 UWARN("Links (%d->%d,%d) and (%d->%d,%d) are not equal!?",
04257 iter->second.from(), iter->second.to(), iter->second.type(),
04258 findIter->second.from(), findIter->second.to(), findIter->second.type());
04259 }
04260 }
04261
04262 links.insert(*iter);
04263 }
04264
04265
04266 for(std::multimap<int, rtabmap::Link>::const_iterator iter=linksAdded_.begin();
04267 iter!=linksAdded_.end();
04268 ++iter)
04269 {
04270
04271 links.insert(*iter);
04272 }
04273
04274 return links;
04275 }
04276
04277 void DatabaseViewer::updateLoopClosuresSlider(int from, int to)
04278 {
04279 int size = loopLinks_.size();
04280 loopLinks_.clear();
04281 std::multimap<int, Link> links = updateLinksWithModifications(links_);
04282 int position = ui_->horizontalSlider_loops->value();
04283 for(std::multimap<int, rtabmap::Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
04284 {
04285 if(!iter->second.transform().isNull())
04286 {
04287 if(iter->second.type() != rtabmap::Link::kNeighbor &&
04288 iter->second.type() != rtabmap::Link::kNeighborMerged)
04289 {
04290 if((iter->second.from() == from && iter->second.to() == to) ||
04291 (iter->second.to() == from && iter->second.from() == to))
04292 {
04293 position = loopLinks_.size();
04294 }
04295 loopLinks_.append(iter->second);
04296 }
04297 }
04298 else
04299 {
04300 UERROR("Transform null for link from %d to %d", iter->first, iter->second.to());
04301 }
04302 }
04303
04304 if(loopLinks_.size())
04305 {
04306 if(loopLinks_.size() == 1)
04307 {
04308
04309 loopLinks_.push_back(loopLinks_.front());
04310 }
04311 ui_->horizontalSlider_loops->setMinimum(0);
04312 ui_->horizontalSlider_loops->setMaximum(loopLinks_.size()-1);
04313 ui_->horizontalSlider_loops->setEnabled(true);
04314 if(position != ui_->horizontalSlider_loops->value())
04315 {
04316 ui_->horizontalSlider_loops->setValue(position);
04317 }
04318 else if(size != loopLinks_.size())
04319 {
04320 this->updateConstraintView(loopLinks_.at(position));
04321 }
04322 }
04323 else
04324 {
04325 ui_->horizontalSlider_loops->setEnabled(false);
04326 constraintsViewer_->removeAllClouds();
04327 constraintsViewer_->update();
04328 updateConstraintButtons();
04329 }
04330 }
04331
04332 void DatabaseViewer::notifyParametersChanged(const QStringList & parametersChanged)
04333 {
04334 bool updateStereo = false;
04335 bool updateGraphView = false;
04336 for(QStringList::const_iterator iter=parametersChanged.constBegin();
04337 iter!=parametersChanged.constEnd() && (!updateStereo || !updateGraphView);
04338 ++iter)
04339 {
04340 QString group = iter->split('/').first();
04341 if(!updateStereo && group == "Stereo")
04342 {
04343 updateStereo = true;
04344 continue;
04345 }
04346 if(!updateGraphView && group == "Optimize")
04347 {
04348 updateGraphView = true;
04349 continue;
04350 }
04351 }
04352
04353 if(updateStereo)
04354 {
04355 this->updateStereo();
04356 }
04357 if(updateGraphView)
04358 {
04359 this->updateGraphView();
04360 }
04361 this->configModified();
04362 }
04363
04364 }