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/core/Memory.h"
00048 #include "rtabmap/core/DBDriver.h"
00049 #include "rtabmap/gui/KeypointItem.h"
00050 #include "rtabmap/gui/UCv2Qt.h"
00051 #include "rtabmap/core/util3d.h"
00052 #include "rtabmap/core/Signature.h"
00053 #include "rtabmap/core/Features2d.h"
00054 #include "rtabmap/core/Compression.h"
00055 #include "rtabmap/core/Graph.h"
00056 #include "rtabmap/gui/DataRecorder.h"
00057 #include "rtabmap/core/SensorData.h"
00058 #include "ExportDialog.h"
00059 #include "DetailedProgressDialog.h"
00060
00061 #include <pcl/io/pcd_io.h>
00062 #include <pcl/filters/voxel_grid.h>
00063 #include <pcl/common/transforms.h>
00064
00065 namespace rtabmap {
00066
00067 DatabaseViewer::DatabaseViewer(QWidget * parent) :
00068 QMainWindow(parent),
00069 memory_(0),
00070 savedMaximized_(false),
00071 firstCall_(true)
00072 {
00073 pathDatabase_ = QDir::homePath()+"/Documents/RTAB-Map";
00074
00075 if(!UDirectory::exists(pathDatabase_.toStdString()))
00076 {
00077 pathDatabase_ = QDir::homePath();
00078 }
00079
00080 ui_ = new Ui_DatabaseViewer();
00081 ui_->setupUi(this);
00082 ui_->buttonBox->setVisible(false);
00083 connect(ui_->buttonBox->button(QDialogButtonBox::Close), SIGNAL(clicked()), this, SLOT(close()));
00084
00085 QString title("RTAB-Map Database Viewer[*]");
00086 this->setWindowTitle(title);
00087
00088 ui_->dockWidget_constraints->setVisible(false);
00089 ui_->dockWidget_graphView->setVisible(false);
00090 ui_->dockWidget_parameters->setVisible(false);
00091 ui_->dockWidget_stereoView->setVisible(false);
00092 ui_->dockWidget_view3d->setVisible(false);
00093
00094 ui_->constraintsViewer->setCameraLockZ(false);
00095 ui_->constraintsViewer->setCameraFree();
00096
00097 this->readSettings();
00098
00099 if(RTABMAP_NONFREE == 0)
00100 {
00101 ui_->comboBox_featureType->setItemData(0, 0, Qt::UserRole - 1);
00102 ui_->comboBox_featureType->setItemData(1, 0, Qt::UserRole - 1);
00103
00104 if(ui_->comboBox_featureType->currentIndex() <= 1)
00105 {
00106 UWARN("SURF/SIFT not available, setting feature default to FAST/BRIEF.");
00107 ui_->comboBox_featureType->setCurrentIndex(4);
00108 ui_->comboBox_nnType->setCurrentIndex(3);
00109 }
00110 }
00111 if(!graph::G2OOptimizer::available())
00112 {
00113 ui_->comboBox_graphOptimizer->setItemData(1, 0, Qt::UserRole - 1);
00114 if(ui_->comboBox_graphOptimizer->currentIndex() == 1)
00115 {
00116 UWARN("g2o is not available, setting optimization default to TORO.");
00117 ui_->comboBox_graphOptimizer->setCurrentIndex(0);
00118 }
00119 }
00120
00121 ui_->menuView->addAction(ui_->dockWidget_constraints->toggleViewAction());
00122 ui_->menuView->addAction(ui_->dockWidget_graphView->toggleViewAction());
00123 ui_->menuView->addAction(ui_->dockWidget_stereoView->toggleViewAction());
00124 ui_->menuView->addAction(ui_->dockWidget_view3d->toggleViewAction());
00125 ui_->menuView->addAction(ui_->dockWidget_parameters->toggleViewAction());
00126 connect(ui_->dockWidget_graphView->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView()));
00127
00128 connect(ui_->actionQuit, SIGNAL(triggered()), this, SLOT(close()));
00129
00130
00131 ui_->actionSave_config->setShortcut(QKeySequence::Save);
00132 connect(ui_->actionSave_config, SIGNAL(triggered()), this, SLOT(writeSettings()));
00133 connect(ui_->actionOpen_database, SIGNAL(triggered()), this, SLOT(openDatabase()));
00134 connect(ui_->actionExport, SIGNAL(triggered()), this, SLOT(exportDatabase()));
00135 connect(ui_->actionExtract_images, SIGNAL(triggered()), this, SLOT(extractImages()));
00136 connect(ui_->actionGenerate_graph_dot, SIGNAL(triggered()), this, SLOT(generateGraph()));
00137 connect(ui_->actionGenerate_local_graph_dot, SIGNAL(triggered()), this, SLOT(generateLocalGraph()));
00138 connect(ui_->actionGenerate_TORO_graph_graph, SIGNAL(triggered()), this, SLOT(generateTOROGraph()));
00139 connect(ui_->actionView_3D_map, SIGNAL(triggered()), this, SLOT(view3DMap()));
00140 connect(ui_->actionGenerate_3D_map_pcd, SIGNAL(triggered()), this, SLOT(generate3DMap()));
00141 connect(ui_->actionDetect_more_loop_closures, SIGNAL(triggered()), this, SLOT(detectMoreLoopClosures()));
00142 connect(ui_->actionRefine_all_neighbor_links, SIGNAL(triggered()), this, SLOT(refineAllNeighborLinks()));
00143 connect(ui_->actionRefine_all_loop_closure_links, SIGNAL(triggered()), this, SLOT(refineAllLoopClosureLinks()));
00144 connect(ui_->actionVisual_Refine_all_neighbor_links, SIGNAL(triggered()), this, SLOT(refineVisuallyAllNeighborLinks()));
00145 connect(ui_->actionVisual_Refine_all_loop_closure_links, SIGNAL(triggered()), this, SLOT(refineVisuallyAllLoopClosureLinks()));
00146 connect(ui_->actionReset_all_changes, SIGNAL(triggered()), this, SLOT(resetAllChanges()));
00147
00148
00149 connect(ui_->pushButton_refine, SIGNAL(clicked()), this, SLOT(refineConstraint()));
00150 connect(ui_->pushButton_refineVisually, SIGNAL(clicked()), this, SLOT(refineConstraintVisually()));
00151 connect(ui_->pushButton_add, SIGNAL(clicked()), this, SLOT(addConstraint()));
00152 connect(ui_->pushButton_reset, SIGNAL(clicked()), this, SLOT(resetConstraint()));
00153 connect(ui_->pushButton_reject, SIGNAL(clicked()), this, SLOT(rejectConstraint()));
00154 ui_->pushButton_refine->setEnabled(false);
00155 ui_->pushButton_refineVisually->setEnabled(false);
00156 ui_->pushButton_add->setEnabled(false);
00157 ui_->pushButton_reset->setEnabled(false);
00158 ui_->pushButton_reject->setEnabled(false);
00159
00160 ui_->actionGenerate_TORO_graph_graph->setEnabled(false);
00161
00162 ui_->horizontalSlider_A->setTracking(false);
00163 ui_->horizontalSlider_B->setTracking(false);
00164 ui_->horizontalSlider_A->setEnabled(false);
00165 ui_->horizontalSlider_B->setEnabled(false);
00166 connect(ui_->horizontalSlider_A, SIGNAL(valueChanged(int)), this, SLOT(sliderAValueChanged(int)));
00167 connect(ui_->horizontalSlider_B, SIGNAL(valueChanged(int)), this, SLOT(sliderBValueChanged(int)));
00168 connect(ui_->horizontalSlider_A, SIGNAL(sliderMoved(int)), this, SLOT(sliderAMoved(int)));
00169 connect(ui_->horizontalSlider_B, SIGNAL(sliderMoved(int)), this, SLOT(sliderBMoved(int)));
00170
00171 ui_->horizontalSlider_neighbors->setTracking(false);
00172 ui_->horizontalSlider_loops->setTracking(false);
00173 ui_->horizontalSlider_neighbors->setEnabled(false);
00174 ui_->horizontalSlider_loops->setEnabled(false);
00175 connect(ui_->horizontalSlider_neighbors, SIGNAL(valueChanged(int)), this, SLOT(sliderNeighborValueChanged(int)));
00176 connect(ui_->horizontalSlider_loops, SIGNAL(valueChanged(int)), this, SLOT(sliderLoopValueChanged(int)));
00177 connect(ui_->horizontalSlider_neighbors, SIGNAL(sliderMoved(int)), this, SLOT(sliderNeighborValueChanged(int)));
00178 connect(ui_->horizontalSlider_loops, SIGNAL(sliderMoved(int)), this, SLOT(sliderLoopValueChanged(int)));
00179 connect(ui_->checkBox_showOptimized, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
00180 connect(ui_->checkBox_show3DWords, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
00181 ui_->checkBox_showOptimized->setEnabled(false);
00182
00183 ui_->horizontalSlider_iterations->setTracking(false);
00184 ui_->horizontalSlider_iterations->setEnabled(false);
00185 ui_->spinBox_optimizationsFrom->setEnabled(false);
00186 connect(ui_->horizontalSlider_iterations, SIGNAL(valueChanged(int)), this, SLOT(sliderIterationsValueChanged(int)));
00187 connect(ui_->horizontalSlider_iterations, SIGNAL(sliderMoved(int)), this, SLOT(sliderIterationsValueChanged(int)));
00188 connect(ui_->spinBox_iterations, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
00189 connect(ui_->spinBox_optimizationsFrom, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
00190 connect(ui_->checkBox_ignoreCovariance, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
00191 connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
00192 connect(ui_->comboBox_graphOptimizer, SIGNAL(currentIndexChanged(int)), this, SLOT(updateGraphView()));
00193 connect(ui_->checkBox_2dslam, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
00194 connect(ui_->spinBox_optimizationDepth, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
00195 connect(ui_->checkBox_gridErode, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
00196 connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(updateGraphView()));
00197 connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
00198 connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
00199
00200 connect(ui_->groupBox_gridFromProjection, SIGNAL(clicked(bool)), this, SLOT(updateGrid()));
00201 connect(ui_->doubleSpinBox_gridCellSize, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
00202 connect(ui_->spinBox_projDecimation, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
00203 connect(ui_->doubleSpinBox_projMaxDepth, SIGNAL(editingFinished()), this, SLOT(updateGrid()));
00204
00205
00206
00207 connect(ui_->graphViewer, SIGNAL(configChanged()), this, SLOT(configModified()));
00208
00209
00210
00211 connect(ui_->spinBox_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
00212 connect(ui_->checkBox_ignoreCovariance, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
00213 connect(ui_->comboBox_graphOptimizer, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
00214 connect(ui_->checkBox_2dslam, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
00215 connect(ui_->spinBox_optimizationDepth, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
00216 connect(ui_->checkBox_gridErode, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
00217 connect(ui_->groupBox_gridFromProjection, SIGNAL(clicked(bool)), this, SLOT(configModified()));
00218 connect(ui_->doubleSpinBox_gridCellSize, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00219 connect(ui_->spinBox_projDecimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
00220 connect(ui_->doubleSpinBox_projMaxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00221 connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(configModified()));
00222 connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00223 connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00224
00225 connect(ui_->spinBox_icp_decimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
00226 connect(ui_->doubleSpinBox_icp_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00227 connect(ui_->doubleSpinBox_icp_voxel, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00228 connect(ui_->doubleSpinBox_icp_maxCorrespDistance, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00229 connect(ui_->spinBox_icp_iteration, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
00230 connect(ui_->checkBox_icp_p2plane, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
00231 connect(ui_->spinBox_icp_normalKSearch, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
00232 connect(ui_->checkBox_icp_2d, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
00233 connect(ui_->doubleSpinBox_icp_minCorrespondenceRatio, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00234
00235 connect(ui_->groupBox_visual_recomputeFeatures, SIGNAL(clicked(bool)), this, SLOT(configModified()));
00236 connect(ui_->comboBox_featureType, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
00237 connect(ui_->comboBox_nnType, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
00238 connect(ui_->checkBox_visual_2d, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
00239 connect(ui_->doubleSpinBox_visual_nndr, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00240 connect(ui_->spinBox_visual_minCorrespondences, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
00241 connect(ui_->doubleSpinBox_visual_maxCorrespDistance, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00242 connect(ui_->spinBox_visual_iteration, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
00243 connect(ui_->doubleSpinBox_visual_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00244 connect(ui_->doubleSpinBox_detectMore_radius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00245 connect(ui_->doubleSpinBox_detectMore_angle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
00246 connect(ui_->spinBox_detectMore_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
00247
00248 QList<QDockWidget*> dockWidgets = this->findChildren<QDockWidget*>();
00249 for(int i=0; i<dockWidgets.size(); ++i)
00250 {
00251 connect(dockWidgets[i], SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(configModified()));
00252 connect(dockWidgets[i]->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configModified()));
00253 }
00254 ui_->dockWidget_constraints->installEventFilter(this);
00255 ui_->dockWidget_graphView->installEventFilter(this);
00256 ui_->dockWidget_stereoView->installEventFilter(this);
00257 ui_->dockWidget_view3d->installEventFilter(this);
00258 ui_->dockWidget_parameters->installEventFilter(this);
00259 }
00260
00261 DatabaseViewer::~DatabaseViewer()
00262 {
00263 delete ui_;
00264 if(memory_)
00265 {
00266 delete memory_;
00267 }
00268 }
00269
00270 void DatabaseViewer::showCloseButton(bool visible)
00271 {
00272 ui_->buttonBox->setVisible(visible);
00273 }
00274
00275 void DatabaseViewer::configModified()
00276 {
00277 this->setWindowModified(true);
00278 }
00279
00280 QString DatabaseViewer::getIniFilePath() const
00281 {
00282 QString privatePath = QDir::homePath() + "/.rtabmap";
00283 if(!QDir(privatePath).exists())
00284 {
00285 QDir::home().mkdir(".rtabmap");
00286 }
00287 return privatePath + "/dbviewer.ini";
00288 }
00289
00290 void DatabaseViewer::readSettings()
00291 {
00292 QString path = getIniFilePath();
00293 QSettings settings(path, QSettings::IniFormat);
00294 settings.beginGroup("DatabaseViewer");
00295
00296
00297 QByteArray bytes;
00298 bytes = settings.value("geometry", QByteArray()).toByteArray();
00299 if(!bytes.isEmpty())
00300 {
00301 this->restoreGeometry(bytes);
00302 }
00303 bytes = settings.value("state", QByteArray()).toByteArray();
00304 if(!bytes.isEmpty())
00305 {
00306 this->restoreState(bytes);
00307 }
00308 savedMaximized_ = settings.value("maximized", false).toBool();
00309
00310
00311 ui_->graphViewer->loadSettings(settings, "GraphView");
00312
00313 settings.beginGroup("optimization");
00314 ui_->spinBox_iterations->setValue(settings.value("iterations", ui_->spinBox_iterations->value()).toInt());
00315 ui_->checkBox_ignoreCovariance->setChecked(settings.value("ignoreCovariance", ui_->checkBox_ignoreCovariance->isChecked()).toBool());
00316 ui_->checkBox_ignorePoseCorrection->setChecked(settings.value("ignorePoseCorrection", ui_->checkBox_ignorePoseCorrection->isChecked()).toBool());
00317 ui_->comboBox_graphOptimizer->setCurrentIndex(settings.value("strategy", ui_->comboBox_graphOptimizer->currentIndex()).toInt());
00318 ui_->checkBox_2dslam->setChecked(settings.value("slam2d", ui_->checkBox_2dslam->isChecked()).toBool());
00319 ui_->spinBox_optimizationDepth->setValue(settings.value("depth", ui_->spinBox_optimizationDepth->value()).toInt());
00320 ui_->checkBox_gridErode->setChecked(settings.value("erode", ui_->checkBox_gridErode->isChecked()).toBool());
00321 settings.endGroup();
00322
00323 settings.beginGroup("grid");
00324 ui_->groupBox_gridFromProjection->setChecked(settings.value("gridFromProj", ui_->groupBox_gridFromProjection->isChecked()).toBool());
00325 ui_->doubleSpinBox_gridCellSize->setValue(settings.value("gridCellSize", ui_->doubleSpinBox_gridCellSize->value()).toDouble());
00326 ui_->spinBox_projDecimation->setValue(settings.value("projDecimation", ui_->spinBox_projDecimation->value()).toInt());
00327 ui_->doubleSpinBox_projMaxDepth->setValue(settings.value("projMaxDepth", ui_->doubleSpinBox_projMaxDepth->value()).toDouble());
00328 ui_->groupBox_posefiltering->setChecked(settings.value("poseFiltering", ui_->groupBox_posefiltering->isChecked()).toBool());
00329 ui_->doubleSpinBox_posefilteringRadius->setValue(settings.value("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value()).toDouble());
00330 ui_->doubleSpinBox_posefilteringAngle->setValue(settings.value("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value()).toDouble());
00331 settings.endGroup();
00332
00333
00334
00335
00336
00337
00338 settings.beginGroup("icp");
00339 ui_->spinBox_icp_decimation->setValue(settings.value("decimation", ui_->spinBox_icp_decimation->value()).toInt());
00340 ui_->doubleSpinBox_icp_maxDepth->setValue(settings.value("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value()).toDouble());
00341 ui_->doubleSpinBox_icp_voxel->setValue(settings.value("voxel", ui_->doubleSpinBox_icp_voxel->value()).toDouble());
00342 ui_->doubleSpinBox_icp_maxCorrespDistance->setValue(settings.value("maxCorrDist", ui_->doubleSpinBox_icp_maxCorrespDistance->value()).toDouble());
00343 ui_->spinBox_icp_iteration->setValue(settings.value("iterations", ui_->spinBox_icp_iteration->value()).toInt());
00344 ui_->checkBox_icp_p2plane->setChecked(settings.value("point2place", ui_->checkBox_icp_p2plane->isChecked()).toBool());
00345 ui_->spinBox_icp_normalKSearch->setValue(settings.value("normalKSearch", ui_->spinBox_icp_normalKSearch->value()).toInt());
00346 ui_->checkBox_icp_2d->setChecked(settings.value("icp2d", ui_->checkBox_icp_2d->isChecked()).toBool());
00347 ui_->doubleSpinBox_icp_minCorrespondenceRatio->setValue(settings.value("icpMinRatio", ui_->doubleSpinBox_icp_minCorrespondenceRatio->value()).toDouble());
00348 settings.endGroup();
00349
00350
00351 settings.beginGroup("visual");
00352 ui_->groupBox_visual_recomputeFeatures->setChecked(settings.value("reextract", ui_->groupBox_visual_recomputeFeatures->isChecked()).toBool());
00353 ui_->comboBox_featureType->setCurrentIndex(settings.value("featureType", ui_->comboBox_featureType->currentIndex()).toInt());
00354 ui_->comboBox_nnType->setCurrentIndex(settings.value("nnType", ui_->comboBox_nnType->currentIndex()).toInt());
00355 ui_->checkBox_visual_2d->setChecked(settings.value("force2d", ui_->checkBox_visual_2d->isChecked()).toBool());
00356 ui_->doubleSpinBox_visual_nndr->setValue(settings.value("nndr", ui_->doubleSpinBox_visual_nndr->value()).toDouble());
00357 ui_->spinBox_visual_minCorrespondences->setValue(settings.value("minCorr", ui_->spinBox_visual_minCorrespondences->value()).toInt());
00358 ui_->doubleSpinBox_visual_maxCorrespDistance->setValue(settings.value("maxCorrDist", ui_->doubleSpinBox_visual_maxCorrespDistance->value()).toDouble());
00359 ui_->spinBox_visual_iteration->setValue(settings.value("iterations", ui_->spinBox_visual_iteration->value()).toDouble());
00360 ui_->doubleSpinBox_visual_maxDepth->setValue(settings.value("maxDepth", ui_->doubleSpinBox_visual_maxDepth->value()).toDouble());
00361 ui_->doubleSpinBox_detectMore_radius->setValue(settings.value("detectMoreRadius", ui_->doubleSpinBox_detectMore_radius->value()).toDouble());
00362 ui_->doubleSpinBox_detectMore_angle->setValue(settings.value("detectMoreAngle", ui_->doubleSpinBox_detectMore_angle->value()).toDouble());
00363 ui_->spinBox_detectMore_iterations->setValue(settings.value("detectMoreIterations", ui_->spinBox_detectMore_iterations->value()).toInt());
00364 settings.endGroup();
00365
00366 settings.endGroup();
00367 }
00368
00369 void DatabaseViewer::writeSettings()
00370 {
00371 QString path = getIniFilePath();
00372 QSettings settings(path, QSettings::IniFormat);
00373 settings.beginGroup("DatabaseViewer");
00374
00375
00376 if(!this->isMaximized())
00377 {
00378 settings.setValue("geometry", this->saveGeometry());
00379 }
00380 settings.setValue("state", this->saveState());
00381 settings.setValue("maximized", this->isMaximized());
00382 savedMaximized_ = this->isMaximized();
00383
00384
00385 ui_->graphViewer->saveSettings(settings, "GraphView");
00386
00387
00388 settings.beginGroup("optimization");
00389 settings.setValue("iterations", ui_->spinBox_iterations->value());
00390 settings.setValue("ignoreCovariance", ui_->checkBox_ignoreCovariance->isChecked());
00391 settings.setValue("ignorePoseCorrection", ui_->checkBox_ignorePoseCorrection->isChecked());
00392 settings.setValue("strategy", ui_->comboBox_graphOptimizer->currentIndex());
00393 settings.setValue("slam2d", ui_->checkBox_2dslam->isChecked());
00394 settings.setValue("depth", ui_->spinBox_optimizationDepth->value());
00395 settings.setValue("erode", ui_->checkBox_gridErode->isChecked());
00396 settings.endGroup();
00397
00398
00399 settings.beginGroup("grid");
00400 settings.setValue("gridFromProj", ui_->groupBox_gridFromProjection->isChecked());
00401 settings.setValue("gridCellSize", ui_->doubleSpinBox_gridCellSize->value());
00402 settings.setValue("projDecimation", ui_->spinBox_projDecimation->value());
00403 settings.setValue("projMaxDepth", ui_->doubleSpinBox_projMaxDepth->value());
00404 settings.setValue("poseFiltering", ui_->groupBox_posefiltering->isChecked());
00405 settings.setValue("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value());
00406 settings.setValue("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value());
00407 settings.endGroup();
00408
00409
00410
00411
00412
00413
00414 settings.beginGroup("icp");
00415 settings.setValue("decimation", ui_->spinBox_icp_decimation->value());
00416 settings.setValue("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value());
00417 settings.setValue("voxel", ui_->doubleSpinBox_icp_voxel->value());
00418 settings.setValue("maxCorrDist", ui_->doubleSpinBox_icp_maxCorrespDistance->value());
00419 settings.setValue("iterations", ui_->spinBox_icp_iteration->value());
00420 settings.setValue("point2place", ui_->checkBox_icp_p2plane->isChecked());
00421 settings.setValue("normalKSearch", ui_->spinBox_icp_normalKSearch->value());
00422 settings.setValue("icp2d", ui_->checkBox_icp_2d->isChecked());
00423 settings.setValue("icpMinRatio", ui_->doubleSpinBox_icp_minCorrespondenceRatio->value());
00424 settings.endGroup();
00425
00426
00427 settings.beginGroup("visual");
00428 settings.setValue("reextract", ui_->groupBox_visual_recomputeFeatures->isChecked());
00429 settings.setValue("featureType", ui_->comboBox_featureType->currentIndex());
00430 settings.setValue("nnType", ui_->comboBox_nnType->currentIndex());
00431 settings.setValue("force2d", ui_->checkBox_visual_2d->isChecked());
00432 settings.setValue("nndr", ui_->doubleSpinBox_visual_nndr->value());
00433 settings.setValue("minCorr", ui_->spinBox_visual_minCorrespondences->value());
00434 settings.setValue("maxCorrDist", ui_->doubleSpinBox_visual_maxCorrespDistance->value());
00435 settings.setValue("iterations", ui_->spinBox_visual_iteration->value());
00436 settings.setValue("maxDepth", ui_->doubleSpinBox_visual_maxDepth->value());
00437 settings.setValue("detectMoreRadius", ui_->doubleSpinBox_detectMore_radius->value());
00438 settings.setValue("detectMoreAngle", ui_->doubleSpinBox_detectMore_angle->value());
00439 settings.setValue("detectMoreIterations", ui_->spinBox_detectMore_iterations->value());
00440 settings.endGroup();
00441
00442 settings.endGroup();
00443
00444 this->setWindowModified(false);
00445 }
00446
00447 void DatabaseViewer::openDatabase()
00448 {
00449 QString path = QFileDialog::getOpenFileName(this, tr("Select file"), pathDatabase_, tr("Databases (*.db)"));
00450 if(!path.isEmpty())
00451 {
00452 openDatabase(path);
00453 }
00454 }
00455
00456 bool DatabaseViewer::openDatabase(const QString & path)
00457 {
00458 UDEBUG("Open database \"%s\"", path.toStdString().c_str());
00459 if(QFile::exists(path))
00460 {
00461 if(memory_)
00462 {
00463 delete memory_;
00464 memory_ = 0;
00465 ids_.clear();
00466 idToIndex_.clear();
00467 neighborLinks_.clear();
00468 loopLinks_.clear();
00469 graphes_.clear();
00470 poses_.clear();
00471 links_.clear();
00472 linksAdded_.clear();
00473 linksRefined_.clear();
00474 linksRemoved_.clear();
00475 localMaps_.clear();
00476 ui_->actionGenerate_TORO_graph_graph->setEnabled(false);
00477 ui_->checkBox_showOptimized->setEnabled(false);
00478 }
00479
00480 std::string driverType = "sqlite3";
00481 rtabmap::ParametersMap parameters;
00482 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), "false"));
00483 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "false"));
00484 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemInitWMWithAllNodes(), "true"));
00485
00486 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpNNStrategy(), "3"));
00487
00488 memory_ = new rtabmap::Memory();
00489
00490 if(!memory_->init(path.toStdString(), false, parameters))
00491 {
00492 QMessageBox::warning(this, "Database error", tr("Can't open database \"%1\"").arg(path));
00493 }
00494 else
00495 {
00496 pathDatabase_ = UDirectory::getDir(path.toStdString()).c_str();
00497 updateIds();
00498 return true;
00499 }
00500 }
00501 else
00502 {
00503 QMessageBox::warning(this, "Database error", tr("Database \"%1\" does not exist.").arg(path));
00504 }
00505 return false;
00506 }
00507
00508 void DatabaseViewer::closeEvent(QCloseEvent* event)
00509 {
00510
00511 bool save = false;
00512 if(this->isWindowModified())
00513 {
00514 QMessageBox::Button b=QMessageBox::question(this,
00515 tr("Database Viewer"),
00516 tr("There are unsaved changed settings. Save them?"),
00517 QMessageBox::Save | QMessageBox::Cancel | QMessageBox::Discard);
00518 if(b == QMessageBox::Save)
00519 {
00520 save = true;
00521 }
00522 else if(b != QMessageBox::Discard)
00523 {
00524 event->ignore();
00525 return;
00526 }
00527 }
00528
00529 if(save)
00530 {
00531 writeSettings();
00532 }
00533
00534 if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size())
00535 {
00536 QMessageBox::StandardButton button = QMessageBox::question(this,
00537 tr("Links modified"),
00538 tr("Some links are modified (%1 added, %2 refined, %3 removed), do you want to save them?")
00539 .arg(linksAdded_.size()).arg(linksRefined_.size()).arg(linksRemoved_.size()),
00540 QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
00541 QMessageBox::Cancel);
00542
00543 if(button == QMessageBox::Yes)
00544 {
00545
00546 for(std::multimap<int, rtabmap::Link>::iterator iter=linksAdded_.begin(); iter!=linksAdded_.end(); ++iter)
00547 {
00548 std::multimap<int, rtabmap::Link>::iterator refinedIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
00549 if(refinedIter != linksRefined_.end())
00550 {
00551 memory_->addLink(
00552 refinedIter->second.to(),
00553 refinedIter->second.from(),
00554 refinedIter->second.transform(),
00555 refinedIter->second.type(),
00556 refinedIter->second.rotVariance(),
00557 refinedIter->second.transVariance());
00558 }
00559 else
00560 {
00561 memory_->addLink(
00562 iter->second.to(),
00563 iter->second.from(),
00564 iter->second.transform(),
00565 iter->second.type(),
00566 iter->second.rotVariance(),
00567 iter->second.transVariance());
00568 }
00569 }
00570
00571
00572 for(std::multimap<int, rtabmap::Link>::iterator iter=linksRefined_.begin(); iter!=linksRefined_.end(); ++iter)
00573 {
00574 if(!containsLink(linksAdded_, iter->second.from(), iter->second.to()))
00575 {
00576 memory_->updateLink(
00577 iter->second.from(),
00578 iter->second.to(),
00579 iter->second.transform(),
00580 iter->second.rotVariance(),
00581 iter->second.transVariance());
00582 }
00583 }
00584
00585
00586 for(std::multimap<int, rtabmap::Link>::iterator iter=linksRemoved_.begin(); iter!=linksRemoved_.end(); ++iter)
00587 {
00588 memory_->removeLink(iter->second.to(), iter->second.from());
00589 }
00590 }
00591
00592 if(button == QMessageBox::Yes || button == QMessageBox::No)
00593 {
00594 event->accept();
00595 }
00596 else
00597 {
00598 event->ignore();
00599 }
00600 }
00601 else
00602 {
00603 event->accept();
00604 }
00605
00606 if(event->isAccepted())
00607 {
00608 if(memory_)
00609 {
00610 delete memory_;
00611 memory_ = 0;
00612 }
00613 }
00614 }
00615
00616 void DatabaseViewer::showEvent(QShowEvent* anEvent)
00617 {
00618 this->setWindowModified(false);
00619
00620 if(ui_->graphViewer->isVisible() && graphes_.size() && localMaps_.size()==0)
00621 {
00622 sliderIterationsValueChanged((int)graphes_.size()-1);
00623 }
00624 }
00625
00626 void DatabaseViewer::moveEvent(QMoveEvent* anEvent)
00627 {
00628 if(this->isVisible())
00629 {
00630
00631 if(!firstCall_)
00632 {
00633 this->configModified();
00634 }
00635 firstCall_ = false;
00636 }
00637 }
00638
00639 void DatabaseViewer::resizeEvent(QResizeEvent* anEvent)
00640 {
00641 if(this->isVisible())
00642 {
00643 this->configModified();
00644 }
00645 }
00646
00647 bool DatabaseViewer::eventFilter(QObject *obj, QEvent *event)
00648 {
00649 if (event->type() == QEvent::Resize && qobject_cast<QDockWidget*>(obj))
00650 {
00651 this->setWindowModified(true);
00652 }
00653 return QWidget::eventFilter(obj, event);
00654 }
00655
00656
00657 void DatabaseViewer::exportDatabase()
00658 {
00659 if(!memory_ || ids_.size() == 0)
00660 {
00661 return;
00662 }
00663
00664 rtabmap::ExportDialog dialog;
00665
00666 if(dialog.exec())
00667 {
00668 if(!dialog.outputPath().isEmpty())
00669 {
00670 int framesIgnored = dialog.framesIgnored();
00671 double frameRate = dialog.targetFramerate();
00672 int sessionExported = dialog.sessionExported();
00673 QString path = dialog.outputPath();
00674 rtabmap::DataRecorder recorder;
00675 QList<int> ids;
00676
00677 double previousStamp = 0;
00678 std::vector<double> delays(ids_.size());
00679 int oi=0;
00680 for(int i=0; i<ids_.size(); i+=1+framesIgnored)
00681 {
00682 Transform odomPose;
00683 int weight = -1;
00684 int mapId = -1;
00685 std::string label;
00686 double stamp = 0;
00687 std::vector<unsigned char> userData;
00688 if(memory_->getNodeInfo(ids_[i], odomPose, mapId, weight, label, stamp, userData, true))
00689 {
00690 if(frameRate == 0 ||
00691 previousStamp == 0 ||
00692 stamp == 0 ||
00693 stamp - previousStamp >= 1.0/frameRate)
00694 {
00695 if(sessionExported < 0 || sessionExported == mapId)
00696 {
00697 ids.push_back(ids_[i]);
00698
00699 if(previousStamp && stamp)
00700 {
00701 delays[oi++] = stamp - previousStamp;
00702 }
00703 previousStamp = stamp;
00704 }
00705 }
00706 if(sessionExported >= 0 && mapId > sessionExported)
00707 {
00708 break;
00709 }
00710 }
00711 }
00712 delays.resize(oi);
00713
00714 if(recorder.init(path, false))
00715 {
00716 rtabmap::DetailedProgressDialog * progressDialog = new rtabmap::DetailedProgressDialog(this);
00717 progressDialog->setAttribute(Qt::WA_DeleteOnClose);
00718 progressDialog->setMaximumSteps(ids.size());
00719 progressDialog->show();
00720
00721 for(int i=0; i<ids.size(); ++i)
00722 {
00723 int id = ids.at(i);
00724
00725 Signature data = memory_->getSignatureData(id, true);
00726 float rotVariance = 1.0f;
00727 float transVariance = 1.0f;
00728 if(dialog.isOdomExported())
00729 {
00730 data.getPoseVariance(rotVariance, transVariance);
00731 }
00732 rtabmap::SensorData sensorData(
00733 dialog.isDepth2dExported()?data.getLaserScanRaw():cv::Mat(),
00734 dialog.isDepth2dExported()?data.getLaserScanMaxPts():0,
00735 dialog.isRgbExported()?data.getImageRaw():cv::Mat(),
00736 dialog.isDepthExported()?data.getDepthRaw():cv::Mat(),
00737 dialog.isRgbExported() || dialog.isDepthExported()?data.getFx():0,
00738 dialog.isRgbExported() || dialog.isDepthExported()?data.getFy():0,
00739 dialog.isRgbExported() || dialog.isDepthExported()?data.getCx():0,
00740 dialog.isRgbExported() || dialog.isDepthExported()?data.getCy():0,
00741 dialog.isRgbExported() || dialog.isDepthExported()?data.getLocalTransform():Transform::getIdentity(),
00742 dialog.isOdomExported()?data.getPose():Transform(),
00743 rotVariance,
00744 transVariance,
00745 data.id(),
00746 data.getStamp(),
00747 dialog.isUserDataExported()?data.getUserData():std::vector<unsigned char>());
00748
00749 recorder.addData(sensorData);
00750
00751 progressDialog->appendText(tr("Exported node %1").arg(id));
00752 progressDialog->incrementStep();
00753 QApplication::processEvents();
00754 }
00755 progressDialog->setValue(progressDialog->maximumSteps());
00756 if(delays.size())
00757 {
00758 progressDialog->appendText(tr("Average frame rate=%1 Hz (Min=%2, Max=%3)")
00759 .arg(1.0/uMean(delays)).arg(1.0/uMax(delays)).arg(1.0/uMin(delays)));
00760 }
00761 progressDialog->appendText(tr("Export finished to \"%1\"!").arg(path));
00762 }
00763 else
00764 {
00765 UERROR("DataRecorder init failed?!");
00766 }
00767 }
00768 else
00769 {
00770 QMessageBox::warning(this, tr("Cannot export database"), tr("An output path must be set!"));
00771 }
00772 }
00773 }
00774
00775 void DatabaseViewer::extractImages()
00776 {
00777 if(!memory_ || ids_.size() == 0)
00778 {
00779 return;
00780 }
00781
00782 QString path = QFileDialog::getExistingDirectory(this, tr("Select directory where to save images..."), QDir::homePath());
00783 if(!path.isNull())
00784 {
00785 for(int i=0; i<ids_.size(); i+=1)
00786 {
00787 int id = ids_.at(i);
00788 cv::Mat compressedRgb = memory_->getImageCompressed(id);
00789 if(!compressedRgb.empty())
00790 {
00791 cv::Mat imageMat = rtabmap::uncompressImage(compressedRgb);
00792 cv::imwrite(QString("%1/%2.png").arg(path).arg(id).toStdString(), imageMat);
00793 UINFO(QString("Saved %1/%2.png").arg(path).arg(id).toStdString().c_str());
00794 }
00795 }
00796 }
00797 }
00798
00799 void DatabaseViewer::updateIds()
00800 {
00801 if(!memory_)
00802 {
00803 return;
00804 }
00805
00806 std::set<int> ids = memory_->getAllSignatureIds();
00807 ids_ = QList<int>::fromStdList(std::list<int>(ids.begin(), ids.end()));
00808 idToIndex_.clear();
00809 for(int i=0; i<ids_.size(); ++i)
00810 {
00811 idToIndex_.insert(ids_[i], i);
00812 }
00813
00814 poses_.clear();
00815 links_.clear();
00816 linksAdded_.clear();
00817 linksRefined_.clear();
00818 linksRemoved_.clear();
00819 ui_->label_optimizeFrom->setText(tr("Optimize from"));
00820 if(memory_->getLastWorkingSignature())
00821 {
00822
00823
00824 memory_->getMetricConstraints(ids, poses_, links_, true);
00825
00826 if(poses_.size())
00827 {
00828 bool nullPoses = poses_.begin()->second.isNull();
00829 for(std::map<int,Transform>::iterator iter=poses_.begin(); iter!=poses_.end(); ++iter)
00830 {
00831 if((!iter->second.isNull() && nullPoses) ||
00832 (iter->second.isNull() && !nullPoses))
00833 {
00834 if(iter->second.isNull())
00835 {
00836 UWARN("Pose %d is null!", iter->first);
00837 }
00838 UWARN("Mixed valid and null poses! Ignoring graph...");
00839 poses_.clear();
00840 links_.clear();
00841 break;
00842 }
00843 }
00844 if(nullPoses)
00845 {
00846 poses_.clear();
00847 links_.clear();
00848 }
00849
00850 int first = *ids.begin();
00851 ui_->spinBox_optimizationsFrom->setRange(first, memory_->getLastWorkingSignature()->id());
00852 ui_->spinBox_optimizationsFrom->setValue(memory_->getLastWorkingSignature()->id());
00853 ui_->label_optimizeFrom->setText(tr("Optimize from [%1, %2]").arg(first).arg(memory_->getLastWorkingSignature()->id()));
00854 }
00855 }
00856
00857 ui_->actionGenerate_TORO_graph_graph->setEnabled(false);
00858 graphes_.clear();
00859 neighborLinks_.clear();
00860 loopLinks_.clear();
00861 for(std::multimap<int, rtabmap::Link>::iterator iter = links_.begin(); iter!=links_.end(); ++iter)
00862 {
00863 if(!iter->second.transform().isNull())
00864 {
00865 if(iter->second.type() == rtabmap::Link::kNeighbor)
00866 {
00867 neighborLinks_.append(iter->second);
00868 }
00869 else
00870 {
00871 loopLinks_.append(iter->second);
00872 }
00873 }
00874 else
00875 {
00876 UERROR("Transform null for link from %d to %d", iter->first, iter->second.to());
00877 }
00878 }
00879
00880 UINFO("Loaded %d ids", ids_.size());
00881
00882 if(ids_.size())
00883 {
00884 ui_->horizontalSlider_A->setMinimum(0);
00885 ui_->horizontalSlider_B->setMinimum(0);
00886 ui_->horizontalSlider_A->setMaximum(ids_.size()-1);
00887 ui_->horizontalSlider_B->setMaximum(ids_.size()-1);
00888 ui_->horizontalSlider_A->setEnabled(true);
00889 ui_->horizontalSlider_B->setEnabled(true);
00890 ui_->horizontalSlider_A->setSliderPosition(0);
00891 ui_->horizontalSlider_B->setSliderPosition(0);
00892 sliderAValueChanged(0);
00893 sliderBValueChanged(0);
00894 }
00895 else
00896 {
00897 ui_->horizontalSlider_A->setEnabled(false);
00898 ui_->horizontalSlider_B->setEnabled(false);
00899 ui_->label_idA->setText("NaN");
00900 ui_->label_idB->setText("NaN");
00901 }
00902
00903 if(neighborLinks_.size())
00904 {
00905 ui_->horizontalSlider_neighbors->setMinimum(0);
00906 ui_->horizontalSlider_neighbors->setMaximum(neighborLinks_.size()-1);
00907 ui_->horizontalSlider_neighbors->setEnabled(true);
00908 ui_->horizontalSlider_neighbors->setSliderPosition(0);
00909 }
00910 else
00911 {
00912 ui_->horizontalSlider_neighbors->setEnabled(false);
00913 }
00914
00915 if(ids_.size())
00916 {
00917 updateLoopClosuresSlider();
00918 updateGraphView();
00919 }
00920 }
00921
00922 void DatabaseViewer::generateGraph()
00923 {
00924 if(!memory_)
00925 {
00926 QMessageBox::warning(this, tr("Cannot generate a graph"), tr("A database must must loaded first...\nUse File->Open database."));
00927 return;
00928 }
00929
00930 QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/Graph.dot", tr("Graphiz file (*.dot)"));
00931 if(!path.isEmpty())
00932 {
00933 memory_->generateGraph(path.toStdString());
00934 }
00935 }
00936
00937 void DatabaseViewer::generateLocalGraph()
00938 {
00939 if(!ids_.size() || !memory_)
00940 {
00941 QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
00942 return;
00943 }
00944 bool ok = false;
00945 int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID"), ids_.first(), ids_.first(), ids_.last(), 1, &ok);
00946
00947 if(ok)
00948 {
00949 int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin"), 4, 1, 100, 1, &ok);
00950 if(ok)
00951 {
00952 QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/Graph" + QString::number(id) + ".dot", tr("Graphiz file (*.dot)"));
00953 if(!path.isEmpty())
00954 {
00955 std::map<int, int> ids = memory_->getNeighborsId(id, margin, -1, false);
00956
00957 if(ids.size() > 0)
00958 {
00959 ids.insert(std::pair<int,int>(id, 0));
00960 std::set<int> idsSet;
00961 for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
00962 {
00963 idsSet.insert(idsSet.end(), iter->first);
00964 UINFO("Node %d", iter->first);
00965 }
00966 UINFO("idsSet=%d", idsSet.size());
00967 memory_->generateGraph(path.toStdString(), idsSet);
00968 }
00969 else
00970 {
00971 QMessageBox::critical(this, tr("Error"), tr("No neighbors found for signature %1.").arg(id));
00972 }
00973 }
00974 }
00975 }
00976 }
00977
00978 void DatabaseViewer::generateTOROGraph()
00979 {
00980 std::multimap<int, Link> links = updateLinksWithModifications(links_);
00981 if(!graphes_.size() || !links.size())
00982 {
00983 QMessageBox::warning(this, tr("Cannot generate a TORO graph"), tr("No poses or no links..."));
00984 return;
00985 }
00986 bool ok = false;
00987 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);
00988
00989 if(ok)
00990 {
00991 QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/constraints" + QString::number(id) + ".graph", tr("TORO file (*.graph)"));
00992 if(!path.isEmpty())
00993 {
00994 graph::TOROOptimizer::saveGraph(path.toStdString(), uValueAt(graphes_, id), links);
00995 }
00996 }
00997 }
00998
00999 void DatabaseViewer::view3DMap()
01000 {
01001 if(!ids_.size() || !memory_)
01002 {
01003 QMessageBox::warning(this, tr("Cannot view 3D map"), tr("The database is empty..."));
01004 return;
01005 }
01006 if(graphes_.empty())
01007 {
01008 this->updateGraphView();
01009 if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
01010 {
01011 QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
01012 return;
01013 }
01014 }
01015 bool ok = false;
01016 QStringList items;
01017 items.append("1");
01018 items.append("2");
01019 items.append("4");
01020 items.append("8");
01021 items.append("16");
01022 QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation"), items, 2, false, &ok);
01023 if(ok)
01024 {
01025 int decimation = item.toInt();
01026 double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok);
01027 if(ok)
01028 {
01029 std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
01030 if(ui_->groupBox_posefiltering->isChecked())
01031 {
01032 optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
01033 ui_->doubleSpinBox_posefilteringRadius->value(),
01034 ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
01035 }
01036 if(optimizedPoses.size() > 0)
01037 {
01038 rtabmap::DetailedProgressDialog progressDialog(this);
01039 progressDialog.setMaximumSteps((int)optimizedPoses.size());
01040 progressDialog.show();
01041
01042
01043 QDialog * window = new QDialog(this, Qt::Window);
01044 window->setModal(this->isModal());
01045 window->setWindowTitle(tr("3D Map"));
01046 window->setMinimumWidth(800);
01047 window->setMinimumHeight(600);
01048
01049 rtabmap::CloudViewer * viewer = new rtabmap::CloudViewer(window);
01050
01051 QVBoxLayout *layout = new QVBoxLayout();
01052 layout->addWidget(viewer);
01053 viewer->setCameraLockZ(false);
01054 window->setLayout(layout);
01055 connect(window, SIGNAL(finished(int)), viewer, SLOT(clear()));
01056
01057 window->show();
01058
01059 for(std::map<int, Transform>::const_iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
01060 {
01061 rtabmap::Transform pose = iter->second;
01062 if(!pose.isNull())
01063 {
01064 Signature data = memory_->getSignatureData(iter->first, true);
01065 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
01066 UASSERT(data.getImageRaw().empty() || data.getImageRaw().type()==CV_8UC3 || data.getImageRaw().type() == CV_8UC1);
01067 UASSERT(data.getDepthRaw().empty() || data.getDepthRaw().type()==CV_8UC1 || data.getDepthRaw().type() == CV_16UC1 || data.getDepthRaw().type() == CV_32FC1);
01068 if(data.getDepthRaw().type() == CV_8UC1)
01069 {
01070 cv::Mat leftImg;
01071 if(data.getImageRaw().channels() == 3)
01072 {
01073 cv::cvtColor(data.getImageRaw(), leftImg, CV_BGR2GRAY);
01074 }
01075 else
01076 {
01077 leftImg = data.getImageRaw();
01078 }
01079 cloud = rtabmap::util3d::cloudFromDisparityRGB(
01080 data.getImageRaw(),
01081 util3d::disparityFromStereoImages(leftImg, data.getDepthRaw()),
01082 data.getCx(), data.getCy(),
01083 data.getFx(), data.getFy(),
01084 decimation);
01085 }
01086 else
01087 {
01088 cloud = rtabmap::util3d::cloudFromDepthRGB(
01089 data.getImageRaw(),
01090 data.getDepthRaw(),
01091 data.getCx(), data.getCy(),
01092 data.getFx(), data.getFy(),
01093 decimation);
01094 }
01095
01096 if(maxDepth)
01097 {
01098 cloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, maxDepth);
01099 }
01100
01101 cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, data.getLocalTransform());
01102
01103 QColor color = Qt::red;
01104 int mapId, weight;
01105 Transform odomPose;
01106 std::string label;
01107 double stamp;
01108 std::vector<unsigned char> userData;
01109 if(memory_->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, userData, true))
01110 {
01111 color = (Qt::GlobalColor)(mapId % 12 + 7 );
01112 }
01113
01114 viewer->addCloud(uFormat("cloud%d", iter->first), cloud, pose, color);
01115
01116 UINFO("Generated %d (%d points)", iter->first, cloud->size());
01117 progressDialog.appendText(QString("Generated %1 (%2 points)").arg(iter->first).arg(cloud->size()));
01118 progressDialog.incrementStep();
01119 QApplication::processEvents();
01120 }
01121 }
01122 progressDialog.setValue(progressDialog.maximumSteps());
01123 }
01124 else
01125 {
01126 QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
01127 }
01128 }
01129 }
01130 }
01131
01132 void DatabaseViewer::generate3DMap()
01133 {
01134 if(!ids_.size() || !memory_)
01135 {
01136 QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
01137 return;
01138 }
01139 bool ok = false;
01140 QStringList items;
01141 items.append("1");
01142 items.append("2");
01143 items.append("4");
01144 items.append("8");
01145 items.append("16");
01146 QString item = QInputDialog::getItem(this, tr("Decimation?"), tr("Image decimation"), items, 2, false, &ok);
01147 if(ok)
01148 {
01149 int decimation = item.toInt();
01150 double maxDepth = QInputDialog::getDouble(this, tr("Camera depth?"), tr("Maximum depth (m, 0=no max):"), 4.0, 0, 10, 2, &ok);
01151 if(ok)
01152 {
01153 QString path = QFileDialog::getExistingDirectory(this, tr("Save directory"), pathDatabase_);
01154 if(!path.isEmpty())
01155 {
01156 std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
01157 if(ui_->groupBox_posefiltering->isChecked())
01158 {
01159 optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
01160 ui_->doubleSpinBox_posefilteringRadius->value(),
01161 ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
01162 }
01163 if(optimizedPoses.size() > 0)
01164 {
01165 rtabmap::DetailedProgressDialog progressDialog;
01166 progressDialog.setMaximumSteps((int)optimizedPoses.size());
01167 progressDialog.show();
01168
01169 for(std::map<int, Transform>::const_iterator iter = optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
01170 {
01171 const rtabmap::Transform & pose = iter->second;
01172 if(!pose.isNull())
01173 {
01174 Signature data = memory_->getSignatureData(iter->first, true);
01175 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
01176 UASSERT(data.getImageRaw().empty() || data.getImageRaw().type()==CV_8UC3 || data.getImageRaw().type() == CV_8UC1);
01177 UASSERT(data.getDepthRaw().empty() || data.getDepthRaw().type()==CV_8UC1 || data.getDepthRaw().type() == CV_16UC1 || data.getDepthRaw().type() == CV_32FC1);
01178 if(data.getDepthRaw().type() == CV_8UC1)
01179 {
01180 cv::Mat leftImg;
01181 if(data.getImageRaw().channels() == 3)
01182 {
01183 cv::cvtColor(data.getImageRaw(), leftImg, CV_BGR2GRAY);
01184 }
01185 else
01186 {
01187 leftImg = data.getImageRaw();
01188 }
01189 cloud = rtabmap::util3d::cloudFromDisparityRGB(
01190 data.getImageRaw(),
01191 util3d::disparityFromStereoImages(leftImg, data.getDepthRaw()),
01192 data.getCx(), data.getCy(),
01193 data.getFx(), data.getFy(),
01194 decimation);
01195 }
01196 else
01197 {
01198 cloud = rtabmap::util3d::cloudFromDepthRGB(
01199 data.getImageRaw(),
01200 data.getDepthRaw(),
01201 data.getCx(), data.getCy(),
01202 data.getFx(), data.getFy(),
01203 decimation);
01204 }
01205
01206 if(maxDepth)
01207 {
01208 cloud = rtabmap::util3d::passThrough<pcl::PointXYZRGB>(cloud, "z", 0, maxDepth);
01209 }
01210
01211 cloud = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloud, pose*data.getLocalTransform());
01212 std::string name = uFormat("%s/node%d.pcd", path.toStdString().c_str(), iter->first);
01213 pcl::io::savePCDFile(name, *cloud);
01214 UINFO("Saved %s (%d points)", name.c_str(), cloud->size());
01215 progressDialog.appendText(QString("Saved %1 (%2 points)").arg(name.c_str()).arg(cloud->size()));
01216 progressDialog.incrementStep();
01217 QApplication::processEvents();
01218 }
01219 }
01220 progressDialog.setValue(progressDialog.maximumSteps());
01221
01222 QMessageBox::information(this, tr("Finished"), tr("%1 clouds generated to %2.").arg(optimizedPoses.size()).arg(path));
01223 }
01224 else
01225 {
01226 QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
01227 }
01228 }
01229 }
01230 }
01231 }
01232
01233 void DatabaseViewer::detectMoreLoopClosures()
01234 {
01235 const std::map<int, Transform> & optimizedPoses = graphes_.back();
01236
01237 int iterations = ui_->spinBox_detectMore_iterations->value();
01238 UASSERT(iterations > 0);
01239 int added = 0;
01240 for(int n=0; n<iterations; ++n)
01241 {
01242 UINFO("iteration %d/%d", n+1, iterations);
01243 std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(
01244 optimizedPoses,
01245 ui_->doubleSpinBox_detectMore_radius->value(),
01246 ui_->doubleSpinBox_detectMore_angle->value()*CV_PI/180.0);
01247 std::set<int> addedLinks;
01248 for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end(); ++iter)
01249 {
01250 int from = iter->first;
01251 int to = iter->second;
01252 if(from < to)
01253 {
01254 from = iter->second;
01255 to = iter->first;
01256 }
01257 if(!findActiveLink(from, to).isValid() && !containsLink(linksRemoved_, from, to) &&
01258 addedLinks.find(from) == addedLinks.end() && addedLinks.find(to) == addedLinks.end())
01259 {
01260 if(addConstraint(from, to, true, false))
01261 {
01262 UINFO("Added new loop closure between %d and %d.", from, to);
01263 ++added;
01264 addedLinks.insert(from);
01265 addedLinks.insert(to);
01266 }
01267 }
01268 }
01269 UINFO("Iteration %d/%d: added %d loop closures.", n+1, iterations, (int)addedLinks.size()/2);
01270 if(addedLinks.size() == 0)
01271 {
01272 break;
01273 }
01274 }
01275 if(added)
01276 {
01277 this->updateGraphView();
01278 }
01279 UINFO("Total added %d loop closures.", added);
01280 }
01281
01282 void DatabaseViewer::refineAllNeighborLinks()
01283 {
01284 if(neighborLinks_.size())
01285 {
01286 rtabmap::DetailedProgressDialog progressDialog(this);
01287 progressDialog.setMaximumSteps(neighborLinks_.size());
01288 progressDialog.show();
01289
01290 for(int i=0; i<neighborLinks_.size(); ++i)
01291 {
01292 int from = neighborLinks_[i].from();
01293 int to = neighborLinks_[i].to();
01294 this->refineConstraint(neighborLinks_[i].from(), neighborLinks_[i].to(), true, false);
01295
01296 progressDialog.appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(neighborLinks_.size()));
01297 progressDialog.incrementStep();
01298 QApplication::processEvents();
01299 }
01300 this->updateGraphView();
01301
01302 progressDialog.setValue(progressDialog.maximumSteps());
01303 progressDialog.appendText("Refining links finished!");
01304 }
01305 }
01306
01307 void DatabaseViewer::refineAllLoopClosureLinks()
01308 {
01309 if(loopLinks_.size())
01310 {
01311 rtabmap::DetailedProgressDialog progressDialog(this);
01312 progressDialog.setMaximumSteps(loopLinks_.size());
01313 progressDialog.show();
01314
01315 for(int i=0; i<loopLinks_.size(); ++i)
01316 {
01317 int from = loopLinks_[i].from();
01318 int to = loopLinks_[i].to();
01319 this->refineConstraint(loopLinks_[i].from(), loopLinks_[i].to(), true, false);
01320
01321 progressDialog.appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(loopLinks_.size()));
01322 progressDialog.incrementStep();
01323 QApplication::processEvents();
01324 }
01325 this->updateGraphView();
01326
01327 progressDialog.setValue(progressDialog.maximumSteps());
01328 progressDialog.appendText("Refining links finished!");
01329 }
01330 }
01331
01332 void DatabaseViewer::refineVisuallyAllNeighborLinks()
01333 {
01334 if(neighborLinks_.size())
01335 {
01336 rtabmap::DetailedProgressDialog progressDialog(this);
01337 progressDialog.setMaximumSteps(neighborLinks_.size());
01338 progressDialog.show();
01339
01340 for(int i=0; i<neighborLinks_.size(); ++i)
01341 {
01342 int from = neighborLinks_[i].from();
01343 int to = neighborLinks_[i].to();
01344 this->refineConstraintVisually(neighborLinks_[i].from(), neighborLinks_[i].to(), true, false);
01345
01346 progressDialog.appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(neighborLinks_.size()));
01347 progressDialog.incrementStep();
01348 QApplication::processEvents();
01349 }
01350 this->updateGraphView();
01351
01352 progressDialog.setValue(progressDialog.maximumSteps());
01353 progressDialog.appendText("Refining links finished!");
01354 }
01355 }
01356
01357 void DatabaseViewer::refineVisuallyAllLoopClosureLinks()
01358 {
01359 if(loopLinks_.size())
01360 {
01361 rtabmap::DetailedProgressDialog progressDialog(this);
01362 progressDialog.setMaximumSteps(loopLinks_.size());
01363 progressDialog.show();
01364
01365 for(int i=0; i<loopLinks_.size(); ++i)
01366 {
01367 int from = loopLinks_[i].from();
01368 int to = loopLinks_[i].to();
01369 this->refineConstraintVisually(loopLinks_[i].from(), loopLinks_[i].to(), true, false);
01370
01371 progressDialog.appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(loopLinks_.size()));
01372 progressDialog.incrementStep();
01373 QApplication::processEvents();
01374 }
01375 this->updateGraphView();
01376
01377 progressDialog.setValue(progressDialog.maximumSteps());
01378 progressDialog.appendText("Refining links finished!");
01379 }
01380 }
01381
01382 void DatabaseViewer::resetAllChanges()
01383 {
01384 linksAdded_.clear();
01385 linksRefined_.clear();
01386 linksRemoved_.clear();
01387 updateLoopClosuresSlider();
01388 this->updateGraphView();
01389 }
01390
01391 void DatabaseViewer::sliderAValueChanged(int value)
01392 {
01393 this->update(value,
01394 ui_->label_indexA,
01395 ui_->label_parentsA,
01396 ui_->label_childrenA,
01397 ui_->label_weightA,
01398 ui_->label_labelA,
01399 ui_->label_stampA,
01400 ui_->graphicsView_A,
01401 ui_->widget_cloudA,
01402 ui_->label_idA,
01403 ui_->label_mapA,
01404 true);
01405 }
01406
01407 void DatabaseViewer::sliderBValueChanged(int value)
01408 {
01409 this->update(value,
01410 ui_->label_indexB,
01411 ui_->label_parentsB,
01412 ui_->label_childrenB,
01413 ui_->label_weightB,
01414 ui_->label_labelB,
01415 ui_->label_stampB,
01416 ui_->graphicsView_B,
01417 ui_->widget_cloudB,
01418 ui_->label_idB,
01419 ui_->label_mapB,
01420 true);
01421 }
01422
01423 void DatabaseViewer::update(int value,
01424 QLabel * labelIndex,
01425 QLabel * labelParents,
01426 QLabel * labelChildren,
01427 QLabel * weight,
01428 QLabel * label,
01429 QLabel * stamp,
01430 rtabmap::ImageView * view,
01431 rtabmap::CloudViewer * view3D,
01432 QLabel * labelId,
01433 QLabel * labelMapId,
01434 bool updateConstraintView)
01435 {
01436 UTimer timer;
01437 labelIndex->setText(QString::number(value));
01438 labelParents->clear();
01439 labelChildren->clear();
01440 weight->clear();
01441 label->clear();
01442 labelMapId->clear();
01443 stamp->clear();
01444 QRectF rect;
01445 if(value >= 0 && value < ids_.size())
01446 {
01447 view->clear();
01448 int id = ids_.at(value);
01449 int mapId = -1;
01450 labelId->setText(QString::number(id));
01451 if(id>0)
01452 {
01453
01454 QImage img;
01455 QImage imgDepth;
01456 if(memory_)
01457 {
01458 Signature data = memory_->getSignatureData(id, true);
01459 if(!data.getImageRaw().empty())
01460 {
01461 img = uCvMat2QImage(data.getImageRaw());
01462 }
01463 if(!data.getDepthRaw().empty())
01464 {
01465 imgDepth = uCvMat2QImage(data.getDepthRaw());
01466 }
01467
01468 if(data.getWords().size())
01469 {
01470 view->setFeatures(data.getWords(), data.getDepthRaw().type() == CV_8UC1?cv::Mat():data.getDepthRaw(), Qt::yellow);
01471 }
01472
01473 Transform odomPose;
01474 int w;
01475 std::string l;
01476 double s;
01477 std::vector<unsigned char> d;
01478 memory_->getNodeInfo(id, odomPose, mapId, w, l, s, d, true);
01479
01480 weight->setNum(data.getWeight());
01481 label->setText(data.getLabel().c_str());
01482 if(data.getStamp()!=0.0)
01483 {
01484 stamp->setText(QDateTime::fromMSecsSinceEpoch(data.getStamp()*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
01485 }
01486
01487
01488 if(!data.getDepthRaw().empty() && data.getDepthRaw().type() == CV_8UC1)
01489 {
01490 this->updateStereo(&data);
01491 }
01492
01493
01494 if(view3D->isVisible() && !data.getDepthRaw().empty())
01495 {
01496 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
01497 if(data.getDepthRaw().type() == CV_8UC1)
01498 {
01499 cloud = util3d::cloudFromStereoImages(
01500 data.getImageRaw(),
01501 data.getDepthRaw(),
01502 data.getCx(), data.getCy(),
01503 data.getFx(), data.getFy(),
01504 1);
01505 }
01506 else
01507 {
01508 cloud = util3d::cloudFromDepthRGB(
01509 data.getImageRaw(),
01510 data.getDepthRaw(),
01511 data.getCx(), data.getCy(),
01512 data.getFx(), data.getFy(),
01513 1);
01514 }
01515 view3D->addOrUpdateCloud("0", cloud, data.getLocalTransform());
01516
01517
01518 pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(data.getLaserScanRaw());
01519 view3D->addOrUpdateCloud("1", scan);
01520
01521 view3D->update();
01522 }
01523 }
01524
01525 if(!imgDepth.isNull())
01526 {
01527 view->setImageDepth(imgDepth);
01528 rect = imgDepth.rect();
01529 }
01530 else
01531 {
01532 ULOGGER_DEBUG("Image depth is empty");
01533 }
01534 if(!img.isNull())
01535 {
01536 view->setImage(img);
01537 rect = img.rect();
01538 }
01539 else
01540 {
01541 ULOGGER_DEBUG("Image is empty");
01542 }
01543
01544
01545 std::map<int, rtabmap::Link> loopClosures;
01546 loopClosures = memory_->getLoopClosureLinks(id, true);
01547 if(loopClosures.size())
01548 {
01549 QString strParents, strChildren;
01550 for(std::map<int, rtabmap::Link>::iterator iter=loopClosures.begin(); iter!=loopClosures.end(); ++iter)
01551 {
01552 if(iter->first < id)
01553 {
01554 strChildren.append(QString("%1 ").arg(iter->first));
01555 }
01556 else
01557 {
01558 strParents.append(QString("%1 ").arg(iter->first));
01559 }
01560 }
01561 labelParents->setText(strParents);
01562 labelChildren->setText(strChildren);
01563 }
01564 }
01565
01566 if(mapId>=0)
01567 {
01568 labelMapId->setText(QString::number(mapId));
01569 }
01570 }
01571 else
01572 {
01573 ULOGGER_ERROR("Slider index out of range ?");
01574 }
01575
01576 updateConstraintButtons();
01577 updateWordsMatching();
01578
01579 if(updateConstraintView)
01580 {
01581
01582 int from = ids_.at(ui_->horizontalSlider_A->value());
01583 int to = ids_.at(ui_->horizontalSlider_B->value());
01584 bool set = false;
01585 for(int i=0; i<loopLinks_.size() || i<neighborLinks_.size(); ++i)
01586 {
01587 if(i < loopLinks_.size())
01588 {
01589 if((loopLinks_[i].from() == from && loopLinks_[i].to() == to) ||
01590 (loopLinks_[i].from() == to && loopLinks_[i].to() == from))
01591 {
01592 if(i != ui_->horizontalSlider_loops->value())
01593 {
01594 ui_->horizontalSlider_loops->blockSignals(true);
01595 ui_->horizontalSlider_loops->setValue(i);
01596 ui_->horizontalSlider_loops->blockSignals(false);
01597 this->updateConstraintView(loopLinks_.at(i), false);
01598 }
01599 ui_->horizontalSlider_neighbors->blockSignals(true);
01600 ui_->horizontalSlider_neighbors->setValue(0);
01601 ui_->horizontalSlider_neighbors->blockSignals(false);
01602 set = true;
01603 break;
01604 }
01605 }
01606 if(i < neighborLinks_.size())
01607 {
01608 if((neighborLinks_[i].from() == from && neighborLinks_[i].to() == to) ||
01609 (neighborLinks_[i].from() == to && neighborLinks_[i].to() == from))
01610 {
01611 if(i != ui_->horizontalSlider_neighbors->value())
01612 {
01613 ui_->horizontalSlider_neighbors->blockSignals(true);
01614 ui_->horizontalSlider_neighbors->setValue(i);
01615 ui_->horizontalSlider_neighbors->blockSignals(false);
01616 this->updateConstraintView(neighborLinks_.at(i), false);
01617 }
01618 ui_->horizontalSlider_loops->blockSignals(true);
01619 ui_->horizontalSlider_loops->setValue(0);
01620 ui_->horizontalSlider_loops->blockSignals(false);
01621 set = true;
01622 break;
01623 }
01624 }
01625 }
01626 if(!set)
01627 {
01628 ui_->horizontalSlider_loops->blockSignals(true);
01629 ui_->horizontalSlider_neighbors->blockSignals(true);
01630 ui_->horizontalSlider_loops->setValue(0);
01631 ui_->horizontalSlider_neighbors->setValue(0);
01632 ui_->constraintsViewer->removeAllClouds();
01633 ui_->constraintsViewer->update();
01634 ui_->horizontalSlider_loops->blockSignals(false);
01635 ui_->horizontalSlider_neighbors->blockSignals(false);
01636 }
01637 }
01638
01639 if(rect.isValid())
01640 {
01641 view->setSceneRect(rect);
01642 }
01643 }
01644
01645 void DatabaseViewer::updateStereo(const Signature * data)
01646 {
01647 if(data && ui_->dockWidget_stereoView->isVisible() && !data->getImageRaw().empty() && !data->getDepthRaw().empty() && data->getDepthRaw().type() == CV_8UC1)
01648 {
01649 cv::Mat leftMono;
01650 if(data->getImageRaw().channels() == 3)
01651 {
01652 cv::cvtColor(data->getImageRaw(), leftMono, CV_BGR2GRAY);
01653 }
01654 else
01655 {
01656 leftMono = data->getImageRaw();
01657 }
01658
01659 UTimer timer;
01660
01661
01662 std::vector<cv::KeyPoint> kpts;
01663 cv::Rect roi = Feature2D::computeRoi(leftMono, "0.03 0.03 0.04 0.04");
01664 ParametersMap parameters;
01665 parameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "1000"));
01666 parameters.insert(ParametersPair(Parameters::kGFTTMinDistance(), "5"));
01667 Feature2D::Type type = Feature2D::kFeatureGfttBrief;
01668 Feature2D * kptDetector = Feature2D::create(type, parameters);
01669 kpts = kptDetector->generateKeypoints(leftMono, roi);
01670 delete kptDetector;
01671
01672 float timeKpt = timer.ticks();
01673
01674 std::vector<cv::Point2f> leftCorners;
01675 cv::KeyPoint::convert(kpts, leftCorners);
01676
01677
01678 std::vector<unsigned char> status;
01679 std::vector<float> err;
01680 std::vector<cv::Point2f> rightCorners;
01681 cv::calcOpticalFlowPyrLK(
01682 leftMono,
01683 data->getDepthRaw(),
01684 leftCorners,
01685 rightCorners,
01686 status,
01687 err,
01688 cv::Size(Parameters::defaultStereoWinSize(), Parameters::defaultStereoWinSize()), Parameters::defaultStereoMaxLevel(),
01689 cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, Parameters::defaultStereoIterations(), Parameters::defaultStereoEps()));
01690
01691 float timeFlow = timer.ticks();
01692
01693 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
01694 cloud->resize(kpts.size());
01695 float bad_point = std::numeric_limits<float>::quiet_NaN ();
01696 UASSERT(status.size() == kpts.size());
01697 int oi = 0;
01698 for(unsigned int i=0; i<status.size(); ++i)
01699 {
01700 pcl::PointXYZ pt(bad_point, bad_point, bad_point);
01701 if(status[i])
01702 {
01703 float disparity = leftCorners[i].x - rightCorners[i].x;
01704 if(disparity > 0.0f)
01705 {
01706 if(fabs((leftCorners[i].y-rightCorners[i].y) / (leftCorners[i].x-rightCorners[i].x)) < Parameters::defaultStereoMaxSlope())
01707 {
01708 pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
01709 leftCorners[i],
01710 disparity,
01711 data->getCx(), data->getCy(), data->getFx(), data->getFy());
01712
01713 if(pcl::isFinite(tmpPt))
01714 {
01715 pt = pcl::transformPoint(tmpPt, data->getLocalTransform().toEigen3f());
01716 if(fabs(pt.x) > 2 || fabs(pt.y) > 2 || fabs(pt.z) > 2)
01717 {
01718 status[i] = 100;
01719 }
01720 cloud->at(oi++) = pt;
01721 }
01722 }
01723 else
01724 {
01725 status[i] = 101;
01726 }
01727 }
01728 else
01729 {
01730 status[i] = 102;
01731 }
01732 }
01733 }
01734 cloud->resize(oi);
01735
01736 UINFO("correspondences = %d/%d (%f) (time kpt=%fs flow=%fs)",
01737 (int)cloud->size(), (int)leftCorners.size(), float(cloud->size())/float(leftCorners.size()), timeKpt, timeFlow);
01738
01739 ui_->stereoViewer->updateCameraTargetPosition(Transform::getIdentity());
01740 ui_->stereoViewer->addOrUpdateCloud("stereo", cloud);
01741 ui_->stereoViewer->update();
01742
01743 std::vector<cv::KeyPoint> rightKpts;
01744 cv::KeyPoint::convert(rightCorners, rightKpts);
01745 std::vector<cv::DMatch> good_matches(kpts.size());
01746 for(unsigned int i=0; i<good_matches.size(); ++i)
01747 {
01748 good_matches[i].trainIdx = i;
01749 good_matches[i].queryIdx = i;
01750 }
01751
01752
01753
01754
01755
01756
01757
01758
01759
01760
01761 ui_->graphicsView_stereo->clear();
01762 ui_->graphicsView_stereo->setLinesShown(true);
01763 ui_->graphicsView_stereo->setFeaturesShown(false);
01764 ui_->graphicsView_stereo->setImageDepthShown(true);
01765
01766 ui_->graphicsView_stereo->setImage(uCvMat2QImage(data->getImageRaw()));
01767 ui_->graphicsView_stereo->setImageDepth(uCvMat2QImage(data->getDepthRaw()));
01768
01769
01770 for(unsigned int i=0; i<kpts.size(); ++i)
01771 {
01772 QColor c = Qt::green;
01773 if(status[i] == 0)
01774 {
01775 c = Qt::red;
01776 }
01777 else if(status[i] == 100)
01778 {
01779 c = Qt::blue;
01780 }
01781 else if(status[i] == 101)
01782 {
01783 c = Qt::yellow;
01784 }
01785 else if(status[i] == 102)
01786 {
01787 c = Qt::magenta;
01788 }
01789 ui_->graphicsView_stereo->addLine(
01790 kpts[i].pt.x,
01791 kpts[i].pt.y,
01792 rightKpts[i].pt.x,
01793 rightKpts[i].pt.y,
01794 c);
01795 }
01796 ui_->graphicsView_stereo->update();
01797 }
01798 }
01799
01800 void DatabaseViewer::updateWordsMatching()
01801 {
01802 int from = ids_.at(ui_->horizontalSlider_A->value());
01803 int to = ids_.at(ui_->horizontalSlider_B->value());
01804 if(from && to)
01805 {
01806 int alpha = 70;
01807 ui_->graphicsView_A->clearLines();
01808 ui_->graphicsView_A->setFeaturesColor(QColor(255, 255, 0, alpha));
01809 ui_->graphicsView_B->clearLines();
01810 ui_->graphicsView_B->setFeaturesColor(QColor(255, 255, 0, alpha));
01811
01812 const QMultiMap<int, KeypointItem*> & wordsA = ui_->graphicsView_A->getFeatures();
01813 const QMultiMap<int, KeypointItem*> & wordsB = ui_->graphicsView_B->getFeatures();
01814 if(wordsA.size() && wordsB.size())
01815 {
01816 QList<int> ids = wordsA.uniqueKeys();
01817 for(int i=0; i<ids.size(); ++i)
01818 {
01819 if(wordsA.count(ids[i]) == 1 && wordsB.count(ids[i]) == 1)
01820 {
01821
01822 ui_->graphicsView_A->setFeatureColor(ids[i], Qt::magenta);
01823 ui_->graphicsView_B->setFeatureColor(ids[i], Qt::magenta);
01824
01825
01826
01827 float scaleX = ui_->graphicsView_A->viewScale();
01828 float deltaX = ui_->graphicsView_A->width()/scaleX;
01829 float deltaY = 0;
01830
01831 const KeypointItem * kptA = wordsA.value(ids[i]);
01832 const KeypointItem * kptB = wordsB.value(ids[i]);
01833 ui_->graphicsView_A->addLine(
01834 kptA->rect().x()+kptA->rect().width()/2,
01835 kptA->rect().y()+kptA->rect().height()/2,
01836 kptB->rect().x()+kptB->rect().width()/2+deltaX,
01837 kptB->rect().y()+kptB->rect().height()/2+deltaY,
01838 Qt::cyan);
01839
01840 ui_->graphicsView_B->addLine(
01841 kptA->rect().x()+kptA->rect().width()/2-deltaX,
01842 kptA->rect().y()+kptA->rect().height()/2-deltaY,
01843 kptB->rect().x()+kptB->rect().width()/2,
01844 kptB->rect().y()+kptB->rect().height()/2,
01845 Qt::cyan);
01846 }
01847 }
01848 ui_->graphicsView_A->update();
01849 ui_->graphicsView_B->update();
01850 }
01851 }
01852 }
01853
01854 void DatabaseViewer::sliderAMoved(int value)
01855 {
01856 ui_->label_indexA->setText(QString::number(value));
01857 if(value>=0 && value < ids_.size())
01858 {
01859 ui_->label_idA->setText(QString::number(ids_.at(value)));
01860 }
01861 else
01862 {
01863 ULOGGER_ERROR("Slider index out of range ?");
01864 }
01865 }
01866
01867 void DatabaseViewer::sliderBMoved(int value)
01868 {
01869 ui_->label_indexB->setText(QString::number(value));
01870 if(value>=0 && value < ids_.size())
01871 {
01872 ui_->label_idB->setText(QString::number(ids_.at(value)));
01873 }
01874 else
01875 {
01876 ULOGGER_ERROR("Slider index out of range ?");
01877 }
01878 }
01879
01880 void DatabaseViewer::sliderNeighborValueChanged(int value)
01881 {
01882 this->updateConstraintView(neighborLinks_.at(value));
01883 }
01884
01885 void DatabaseViewer::sliderLoopValueChanged(int value)
01886 {
01887 this->updateConstraintView(loopLinks_.at(value));
01888 }
01889
01890
01891 void DatabaseViewer::updateConstraintView()
01892 {
01893 if(ids_.size())
01894 {
01895 Link link = this->findActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value()));
01896 if(link.isValid())
01897 {
01898 if(link.type() == Link::kNeighbor)
01899 {
01900 this->updateConstraintView(neighborLinks_.at(ui_->horizontalSlider_neighbors->value()), false);
01901 }
01902 else
01903 {
01904 this->updateConstraintView(loopLinks_.at(ui_->horizontalSlider_loops->value()), false);
01905 }
01906 }
01907 }
01908 }
01909
01910 void DatabaseViewer::updateConstraintView(
01911 const rtabmap::Link & linkIn,
01912 bool updateImageSliders,
01913 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudFrom,
01914 const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloudTo,
01915 const pcl::PointCloud<pcl::PointXYZ>::Ptr & scanFrom,
01916 const pcl::PointCloud<pcl::PointXYZ>::Ptr & scanTo)
01917 {
01918 std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, linkIn.from(), linkIn.to());
01919 rtabmap::Link link = linkIn;
01920 if(iter != linksRefined_.end())
01921 {
01922 link = iter->second;
01923 }
01924 rtabmap::Transform t = link.transform();
01925
01926 ui_->label_constraint->clear();
01927 ui_->label_constraint_opt->clear();
01928 ui_->checkBox_showOptimized->setEnabled(false);
01929 UASSERT(!t.isNull() && memory_);
01930
01931 ui_->label_type->setNum(link.type());
01932 ui_->label_variance->setText(QString("%1, %2").arg(sqrt(link.rotVariance())).arg(sqrt(link.transVariance())));
01933 ui_->label_constraint->setText(QString("%1").arg(t.prettyPrint().c_str()).replace(" ", "\n"));
01934 if(link.type() == Link::kNeighbor &&
01935 graphes_.size() &&
01936 (int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
01937 {
01938 std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
01939 if(link.type() == Link::kNeighbor)
01940 {
01941 std::map<int, rtabmap::Transform>::iterator iterFrom = graph.find(link.from());
01942 std::map<int, rtabmap::Transform>::iterator iterTo = graph.find(link.to());
01943 if(iterFrom != graph.end() && iterTo != graph.end())
01944 {
01945 ui_->checkBox_showOptimized->setEnabled(true);
01946 Transform topt = iterFrom->second.inverse()*iterTo->second;
01947 float diff = topt.getDistance(t);
01948 Transform v1 = t.rotation()*Transform(1,0,0,0,0,0);
01949 Transform v2 = topt.rotation()*Transform(1,0,0,0,0,0);
01950 float a = pcl::getAngle3D(Eigen::Vector4f(v1.x(), v1.y(), v1.z(), 0), Eigen::Vector4f(v2.x(), v2.y(), v2.z(), 0));
01951 a = (a *180.0f) / CV_PI;
01952 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));
01953
01954 if(ui_->checkBox_showOptimized->isChecked())
01955 {
01956 t = topt;
01957 }
01958 }
01959 }
01960 }
01961
01962 if(updateImageSliders)
01963 {
01964 ui_->horizontalSlider_A->blockSignals(true);
01965 ui_->horizontalSlider_B->blockSignals(true);
01966
01967 ui_->horizontalSlider_A->setValue(idToIndex_.value(link.from()));
01968 ui_->horizontalSlider_B->setValue(idToIndex_.value(link.to()));
01969 ui_->horizontalSlider_A->blockSignals(false);
01970 ui_->horizontalSlider_B->blockSignals(false);
01971 this->update(idToIndex_.value(link.from()),
01972 ui_->label_indexA,
01973 ui_->label_parentsA,
01974 ui_->label_childrenA,
01975 ui_->label_weightA,
01976 ui_->label_labelA,
01977 ui_->label_stampA,
01978 ui_->graphicsView_A,
01979 ui_->widget_cloudA,
01980 ui_->label_idA,
01981 ui_->label_mapA,
01982 false);
01983 this->update(idToIndex_.value(link.to()),
01984 ui_->label_indexB,
01985 ui_->label_parentsB,
01986 ui_->label_childrenB,
01987 ui_->label_weightB,
01988 ui_->label_labelB,
01989 ui_->label_stampB,
01990 ui_->graphicsView_B,
01991 ui_->widget_cloudB,
01992 ui_->label_idB,
01993 ui_->label_mapB,
01994 false);
01995 }
01996
01997 if(ui_->constraintsViewer->isVisible())
01998 {
01999 Signature dataFrom, dataTo;
02000
02001 dataFrom = memory_->getSignatureData(link.from(), true);
02002 UASSERT(dataFrom.getImageRaw().empty() || dataFrom.getImageRaw().type()==CV_8UC3 || dataFrom.getImageRaw().type() == CV_8UC1);
02003 UASSERT(dataFrom.getDepthRaw().empty() || dataFrom.getDepthRaw().type()==CV_8UC1 || dataFrom.getDepthRaw().type() == CV_16UC1 || dataFrom.getDepthRaw().type() == CV_32FC1);
02004
02005 dataTo = memory_->getSignatureData(link.to(), true);
02006 UASSERT(dataTo.getImageRaw().empty() || dataTo.getImageRaw().type()==CV_8UC3 || dataTo.getImageRaw().type() == CV_8UC1);
02007 UASSERT(dataTo.getDepthRaw().empty() || dataTo.getDepthRaw().type()==CV_8UC1 || dataTo.getDepthRaw().type() == CV_16UC1 || dataTo.getDepthRaw().type() == CV_32FC1);
02008
02009
02010 if(cloudFrom->size() == 0 && cloudTo->size() == 0)
02011 {
02012
02013 if(!ui_->checkBox_show3DWords->isChecked())
02014 {
02015 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom;
02016 if(dataFrom.getDepthRaw().type() == CV_8UC1)
02017 {
02018 cloudFrom = rtabmap::util3d::cloudFromStereoImages(
02019 dataFrom.getImageRaw(),
02020 dataFrom.getDepthRaw(),
02021 dataFrom.getCx(), dataFrom.getCy(),
02022 dataFrom.getFx(), dataFrom.getFy(),
02023 1);
02024 }
02025 else
02026 {
02027 cloudFrom = rtabmap::util3d::cloudFromDepthRGB(
02028 dataFrom.getImageRaw(),
02029 dataFrom.getDepthRaw(),
02030 dataFrom.getCx(), dataFrom.getCy(),
02031 dataFrom.getFx(), dataFrom.getFy(),
02032 1);
02033 }
02034
02035 cloudFrom = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudFrom);
02036 cloudFrom = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudFrom, dataFrom.getLocalTransform());
02037
02038 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudTo;
02039 if(dataTo.getDepthRaw().type() == CV_8UC1)
02040 {
02041 cloudTo = rtabmap::util3d::cloudFromStereoImages(
02042 dataTo.getImageRaw(),
02043 dataTo.getDepthRaw(),
02044 dataTo.getCx(), dataTo.getCy(),
02045 dataTo.getFx(), dataTo.getFy(),
02046 1);
02047 }
02048 else
02049 {
02050 cloudTo = rtabmap::util3d::cloudFromDepthRGB(
02051 dataTo.getImageRaw(),
02052 dataTo.getDepthRaw(),
02053 dataTo.getCx(), dataTo.getCy(),
02054 dataTo.getFx(), dataTo.getFy(),
02055 1);
02056 }
02057
02058 cloudTo = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZRGB>(cloudTo);
02059 cloudTo = rtabmap::util3d::transformPointCloud<pcl::PointXYZRGB>(cloudTo, t*dataTo.getLocalTransform());
02060
02061 if(cloudFrom->size())
02062 {
02063 ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom, Transform::getIdentity(), Qt::red);
02064 }
02065 if(cloudTo->size())
02066 {
02067 ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo, Transform::getIdentity(), Qt::cyan);
02068 }
02069 }
02070 else
02071 {
02072 const Signature * sFrom = memory_->getSignature(link.from());
02073 const Signature * sTo = memory_->getSignature(link.to());
02074 if(sFrom && sTo)
02075 {
02076 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom(new pcl::PointCloud<pcl::PointXYZ>);
02077 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo(new pcl::PointCloud<pcl::PointXYZ>);
02078 cloudFrom->resize(sFrom->getWords3().size());
02079 cloudTo->resize(sTo->getWords3().size());
02080 int i=0;
02081 for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=sFrom->getWords3().begin();
02082 iter!=sFrom->getWords3().end();
02083 ++iter)
02084 {
02085 cloudFrom->at(i++) = iter->second;
02086 }
02087 i=0;
02088 for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=sTo->getWords3().begin();
02089 iter!=sTo->getWords3().end();
02090 ++iter)
02091 {
02092 cloudTo->at(i++) = iter->second;
02093 }
02094
02095 if(cloudFrom->size())
02096 {
02097 cloudFrom = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZ>(cloudFrom);
02098 }
02099 if(cloudTo->size())
02100 {
02101 cloudTo = rtabmap::util3d::removeNaNFromPointCloud<pcl::PointXYZ>(cloudTo);
02102 if(cloudTo->size())
02103 {
02104 cloudTo = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(cloudTo, t);
02105 }
02106 }
02107
02108 if(cloudFrom->size())
02109 {
02110 ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom, Transform::getIdentity(), Qt::red);
02111 }
02112 else
02113 {
02114 UWARN("Empty 3D words for node %d", link.from());
02115 }
02116 if(cloudTo->size())
02117 {
02118 ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo, Transform::getIdentity(), Qt::cyan);
02119 }
02120 else
02121 {
02122 UWARN("Empty 3D words for node %d", link.to());
02123 }
02124 }
02125 else
02126 {
02127 UERROR("Not found signature %d or %d in RAM", link.from(), link.to());
02128 }
02129 }
02130 }
02131 else
02132 {
02133 if(cloudFrom->size())
02134 {
02135 ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom, Transform::getIdentity(), Qt::red);
02136 }
02137 if(cloudTo->size())
02138 {
02139 ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo, Transform::getIdentity(), Qt::cyan);
02140 }
02141 }
02142
02143 if(scanFrom->size() == 0 && scanTo->size() == 0)
02144 {
02145
02146 pcl::PointCloud<pcl::PointXYZ>::Ptr scanA, scanB;
02147 scanA = rtabmap::util3d::laserScanToPointCloud(dataFrom.getLaserScanRaw());
02148 scanB = rtabmap::util3d::laserScanToPointCloud(dataTo.getLaserScanRaw());
02149 scanB = rtabmap::util3d::transformPointCloud<pcl::PointXYZ>(scanB, t);
02150 if(scanA->size())
02151 {
02152 ui_->constraintsViewer->addOrUpdateCloud("scan0", scanA, Transform::getIdentity(), Qt::yellow);
02153 }
02154 else
02155 {
02156 ui_->constraintsViewer->removeCloud("scan0");
02157 }
02158 if(scanB->size())
02159 {
02160 ui_->constraintsViewer->addOrUpdateCloud("scan1", scanB, Transform::getIdentity(), Qt::magenta);
02161 }
02162 else
02163 {
02164 ui_->constraintsViewer->removeCloud("scan1");
02165 }
02166 }
02167 else
02168 {
02169 if(scanFrom->size())
02170 {
02171 ui_->constraintsViewer->addOrUpdateCloud("scan0", scanFrom, Transform::getIdentity(), Qt::yellow);
02172 }
02173 else
02174 {
02175 ui_->constraintsViewer->removeCloud("scan0");
02176 }
02177 if(scanTo->size())
02178 {
02179 ui_->constraintsViewer->addOrUpdateCloud("scan1", scanTo, Transform::getIdentity(), Qt::magenta);
02180 }
02181 else
02182 {
02183 ui_->constraintsViewer->removeCloud("scan1");
02184 }
02185 }
02186
02187
02188 ui_->constraintsViewer->updateCameraTargetPosition(t);
02189 ui_->constraintsViewer->clearTrajectory();
02190
02191 ui_->constraintsViewer->update();
02192 }
02193
02194
02195 updateConstraintButtons();
02196 }
02197
02198 void DatabaseViewer::updateConstraintButtons()
02199 {
02200 ui_->pushButton_refine->setEnabled(false);
02201 ui_->pushButton_refineVisually->setEnabled(false);
02202 ui_->pushButton_reset->setEnabled(false);
02203 ui_->pushButton_add->setEnabled(false);
02204 ui_->pushButton_reject->setEnabled(false);
02205
02206 int from = ids_.at(ui_->horizontalSlider_A->value());
02207 int to = ids_.at(ui_->horizontalSlider_B->value());
02208 if(from!=to && from && to)
02209 {
02210 if((!containsLink(links_, from ,to) && !containsLink(linksAdded_, from ,to)) ||
02211 containsLink(linksRemoved_, from ,to))
02212 {
02213 ui_->pushButton_add->setEnabled(true);
02214 }
02215 }
02216
02217 Link currentLink = findActiveLink(from ,to);
02218
02219 if(currentLink.isValid() &&
02220 ((currentLink.from() == from && currentLink.to() == to) || (currentLink.from() == to && currentLink.to() == from)))
02221 {
02222 if(!containsLink(linksRemoved_, from ,to))
02223 {
02224 ui_->pushButton_reject->setEnabled(currentLink.type() != Link::kNeighbor);
02225 }
02226
02227
02228 bool modified = false;
02229 std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, currentLink.from(), currentLink.to());
02230 if(iter != linksRefined_.end())
02231 {
02232 currentLink = iter->second;
02233 ui_->pushButton_reset->setEnabled(true);
02234 modified = true;
02235 }
02236 if(!modified)
02237 {
02238 ui_->pushButton_reset->setEnabled(false);
02239 }
02240 ui_->pushButton_refine->setEnabled(true);
02241 ui_->pushButton_refineVisually->setEnabled(true);
02242 }
02243 }
02244
02245 void DatabaseViewer::sliderIterationsValueChanged(int value)
02246 {
02247 if(memory_ && value >=0 && value < (int)graphes_.size())
02248 {
02249 if(ui_->dockWidget_graphView->isVisible() && localMaps_.size() == 0)
02250 {
02251
02252 UINFO("Update local maps list...");
02253
02254 for(int i=0; i<ids_.size(); ++i)
02255 {
02256 UTimer time;
02257 bool added = false;
02258 if(ui_->groupBox_gridFromProjection->isChecked())
02259 {
02260 Signature data = memory_->getSignatureData(ids_.at(i), true);
02261 if(!data.getDepthRaw().empty())
02262 {
02263 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
02264 if(data.getDepthRaw().type() == CV_8UC1)
02265 {
02266 cloud = rtabmap::util3d::cloudFromDisparity(
02267 util3d::disparityFromStereoImages(data.getImageRaw(), data.getDepthRaw()),
02268 data.getCx(),
02269 data.getCy(),
02270 data.getFx(),
02271 data.getFy(),
02272 ui_->spinBox_projDecimation->value());
02273 }
02274 else
02275 {
02276 cloud = util3d::cloudFromDepth(
02277 data.getDepthRaw(),
02278 data.getCx(),
02279 data.getCy(),
02280 data.getFx(),
02281 data.getFy(),
02282 ui_->spinBox_projDecimation->value());
02283 }
02284 if(cloud->size())
02285 {
02286 cloud = util3d::passThrough<pcl::PointXYZ>(cloud, "z", 0, ui_->doubleSpinBox_projMaxDepth->value());
02287 }
02288
02289 if(cloud->size())
02290 {
02291 cloud = util3d::voxelize<pcl::PointXYZ>(cloud, ui_->doubleSpinBox_gridCellSize->value());
02292 cloud = util3d::transformPointCloud<pcl::PointXYZ>(cloud, data.getLocalTransform());
02293
02294 UTimer timer;
02295 float cellSize = ui_->doubleSpinBox_gridCellSize->value();
02296 float groundNormalMaxAngle = M_PI_4;
02297 int minClusterSize = 20;
02298 cv::Mat ground, obstacles;
02299 util3d::occupancy2DFromCloud3D<pcl::PointXYZ>(
02300 cloud,
02301 ground, obstacles,
02302 cellSize,
02303 groundNormalMaxAngle,
02304 minClusterSize);
02305 if(!ground.empty() || !obstacles.empty())
02306 {
02307 localMaps_.insert(std::make_pair(ids_.at(i), std::make_pair(ground, obstacles)));
02308 added = true;
02309 }
02310 }
02311 }
02312 }
02313 else
02314 {
02315 Signature data = memory_->getSignatureData(ids_.at(i), false);
02316 if(!data.getLaserScanCompressed().empty())
02317 {
02318 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
02319 cv::Mat laserScan;
02320 data.uncompressDataConst(0, 0, &laserScan);
02321 cv::Mat ground, obstacles;
02322 util3d::occupancy2DFromLaserScan(laserScan, ground, obstacles, ui_->doubleSpinBox_gridCellSize->value());
02323 localMaps_.insert(std::make_pair(ids_.at(i), std::make_pair(ground, obstacles)));
02324 added = true;
02325 }
02326 }
02327 if(added)
02328 {
02329 UINFO("Processed grid map %d/%d (%fs)", i+1, (int)ids_.size(), time.ticks());
02330 }
02331 }
02332 UINFO("Update local maps list... done");
02333 }
02334 std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, value);
02335 std::multimap<int, Link> links = updateLinksWithModifications(links_);
02336 ui_->graphViewer->updateGraph(graph, links);
02337 if(graph.size() && localMaps_.size() && ui_->graphViewer->isGridMapVisible())
02338 {
02339 float xMin, yMin;
02340 float cell = ui_->doubleSpinBox_gridCellSize->value();
02341 cv::Mat map;
02342 QTime time;
02343 time.start();
02344 if(ui_->groupBox_posefiltering->isChecked())
02345 {
02346 std::map<int, rtabmap::Transform> graphFiltered = graph::radiusPosesFiltering(graph,
02347 ui_->doubleSpinBox_posefilteringRadius->value(),
02348 ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
02349 map = rtabmap::util3d::create2DMapFromOccupancyLocalMaps(graphFiltered, localMaps_, cell, xMin, yMin, 0, ui_->checkBox_gridErode->isChecked());
02350 }
02351 else
02352 {
02353 map = rtabmap::util3d::create2DMapFromOccupancyLocalMaps(graph, localMaps_, cell, xMin, yMin, 0, ui_->checkBox_gridErode->isChecked());
02354 }
02355 if(!map.empty())
02356 {
02357 ui_->graphViewer->updateMap(rtabmap::util3d::convertMap2Image8U(map), cell, xMin, yMin);
02358 }
02359 ui_->label_timeGrid->setNum(double(time.elapsed())/1000.0);
02360 }
02361 ui_->graphViewer->update();
02362 ui_->label_iterations->setNum(value);
02363
02364
02365 float length = 0.0f;
02366 for(std::multimap<int, rtabmap::Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
02367 {
02368 std::map<int, rtabmap::Transform>::const_iterator jterA = graph.find(iter->first);
02369 std::map<int, rtabmap::Transform>::const_iterator jterB = graph.find(iter->second.to());
02370 if(jterA != graph.end() && jterB != graph.end())
02371 {
02372 const rtabmap::Transform & poseA = jterA->second;
02373 const rtabmap::Transform & poseB = jterB->second;
02374 if(iter->second.type() == rtabmap::Link::kNeighbor)
02375 {
02376 Eigen::Vector3f vA, vB;
02377 poseA.getTranslation(vA[0], vA[1], vA[2]);
02378 poseB.getTranslation(vB[0], vB[1], vB[2]);
02379 length += (vB - vA).norm();
02380 }
02381 }
02382 }
02383 ui_->label_pathLength->setNum(length);
02384 }
02385 }
02386 void DatabaseViewer::updateGraphView()
02387 {
02388 if(poses_.size())
02389 {
02390 if(!uContains(poses_, ui_->spinBox_optimizationsFrom->value()))
02391 {
02392 QMessageBox::warning(this, tr(""), tr("Graph optimization from id (%1) for which node is not linked to graph.\n Minimum=%2, Maximum=%3")
02393 .arg(ui_->spinBox_optimizationsFrom->value())
02394 .arg(poses_.begin()->first)
02395 .arg(poses_.rbegin()->first));
02396 return;
02397 }
02398
02399 graphes_.clear();
02400
02401 std::map<int, rtabmap::Transform> finalPoses;
02402 graphes_.push_back(poses_);
02403 ui_->actionGenerate_TORO_graph_graph->setEnabled(true);
02404 std::multimap<int, rtabmap::Link> links;
02405 if(ui_->checkBox_ignorePoseCorrection->isChecked())
02406 {
02407 std::multimap<int, Link> tmp = links_;
02408 for(std::multimap<int, Link>::iterator iter=tmp.begin(); iter!=tmp.end(); ++iter)
02409 {
02410 if(iter->second.type() == Link::kNeighbor)
02411 {
02412 Transform poseFrom = uValue(poses_, iter->second.from(), Transform());
02413 Transform poseTo = uValue(poses_, iter->second.to(), Transform());
02414 if(!poseFrom.isNull() && !poseTo.isNull())
02415 {
02416 iter->second.setTransform(poseFrom.inverse() * poseTo);
02417
02418 }
02419 }
02420 }
02421 links = updateLinksWithModifications(tmp);
02422 }
02423 else
02424 {
02425 links = updateLinksWithModifications(links_);
02426 }
02427 graph::Optimizer * optimizer = 0;
02428 if(ui_->comboBox_graphOptimizer->currentIndex() == graph::Optimizer::kTypeG2O)
02429 {
02430 optimizer = new graph::G2OOptimizer(ui_->spinBox_iterations->value(), ui_->checkBox_2dslam->isChecked(), ui_->checkBox_ignoreCovariance->isChecked());
02431 }
02432 else
02433 {
02434 optimizer = new graph::TOROOptimizer(ui_->spinBox_iterations->value(), ui_->checkBox_2dslam->isChecked(), ui_->checkBox_ignoreCovariance->isChecked());
02435 }
02436 int fromId = ui_->spinBox_optimizationsFrom->value();
02437 std::map<int, rtabmap::Transform> posesOut;
02438 std::multimap<int, rtabmap::Link> linksOut;
02439 optimizer->getConnectedGraph(
02440 fromId,
02441 poses_,
02442 links,
02443 posesOut,
02444 linksOut,
02445 ui_->spinBox_optimizationDepth->value());
02446
02447 QTime time;
02448 time.start();
02449 finalPoses = optimizer->optimize(fromId, posesOut, linksOut, &graphes_);
02450 ui_->label_timeOptimization->setNum(double(time.elapsed())/1000.0);
02451 graphes_.push_back(finalPoses);
02452 delete optimizer;
02453 }
02454 if(graphes_.size())
02455 {
02456 ui_->horizontalSlider_iterations->setMaximum((int)graphes_.size()-1);
02457 ui_->horizontalSlider_iterations->setValue((int)graphes_.size()-1);
02458 ui_->horizontalSlider_iterations->setEnabled(true);
02459 ui_->spinBox_optimizationsFrom->setEnabled(true);
02460 sliderIterationsValueChanged((int)graphes_.size()-1);
02461 }
02462 else
02463 {
02464 ui_->horizontalSlider_iterations->setEnabled(false);
02465 ui_->spinBox_optimizationsFrom->setEnabled(false);
02466 }
02467 }
02468
02469 void DatabaseViewer::updateGrid()
02470 {
02471 if((sender() != ui_->spinBox_projDecimation && sender() != ui_->doubleSpinBox_projMaxDepth) ||
02472 (sender() == ui_->spinBox_projDecimation && ui_->groupBox_gridFromProjection->isChecked()) ||
02473 (sender() == ui_->doubleSpinBox_projMaxDepth && ui_->groupBox_gridFromProjection->isChecked()))
02474 {
02475 localMaps_.clear();
02476 updateGraphView();
02477 }
02478 }
02479
02480 Link DatabaseViewer::findActiveLink(int from, int to)
02481 {
02482 Link link;
02483 std::multimap<int, Link>::iterator findIter = rtabmap::graph::findLink(linksRefined_, from ,to);
02484 if(findIter != linksRefined_.end())
02485 {
02486 link = findIter->second;
02487 }
02488 else
02489 {
02490 findIter = rtabmap::graph::findLink(linksAdded_, from ,to);
02491 if(findIter != linksAdded_.end())
02492 {
02493 link = findIter->second;
02494 }
02495 else if(!containsLink(linksRemoved_, from ,to))
02496 {
02497 findIter = rtabmap::graph::findLink(links_, from ,to);
02498 if(findIter != links_.end())
02499 {
02500 link = findIter->second;
02501 }
02502 }
02503 }
02504 return link;
02505 }
02506
02507 bool DatabaseViewer::containsLink(std::multimap<int, Link> & links, int from, int to)
02508 {
02509 return rtabmap::graph::findLink(links, from, to) != links.end();
02510 }
02511
02512 void DatabaseViewer::refineConstraint()
02513 {
02514 int from = ids_.at(ui_->horizontalSlider_A->value());
02515 int to = ids_.at(ui_->horizontalSlider_B->value());
02516 refineConstraint(from, to, false, true);
02517 }
02518
02519 void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool updateGraph)
02520 {
02521 if(from == to)
02522 {
02523 UWARN("Cannot refine link to same node");
02524 return;
02525 }
02526
02527 Link currentLink = findActiveLink(from, to);
02528 if(!currentLink.isValid())
02529 {
02530 UERROR("Not found link! (%d->%d)", from, to);
02531 return;
02532 }
02533 Transform t = currentLink.transform();
02534 if(ui_->checkBox_showOptimized->isChecked() &&
02535 currentLink.type() == Link::kNeighbor &&
02536 graphes_.size() &&
02537 (int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
02538 {
02539 std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
02540 if(currentLink.type() == Link::kNeighbor)
02541 {
02542 std::map<int, rtabmap::Transform>::iterator iterFrom = graph.find(currentLink.from());
02543 std::map<int, rtabmap::Transform>::iterator iterTo = graph.find(currentLink.to());
02544 if(iterFrom != graph.end() && iterTo != graph.end())
02545 {
02546 Transform topt = iterFrom->second.inverse()*iterTo->second;
02547 t = topt;
02548 }
02549 }
02550 }
02551
02552
02553 bool hasConverged = false;
02554 double variance = -1.0;
02555 int correspondences = 0;
02556 Transform transform;
02557
02558 Signature dataFrom, dataTo;
02559 dataFrom = memory_->getSignatureData(currentLink.from(), false);
02560 dataTo = memory_->getSignatureData(currentLink.to(), false);
02561
02562 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZ>);
02563 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB(new pcl::PointCloud<pcl::PointXYZ>);
02564 pcl::PointCloud<pcl::PointXYZ>::Ptr scanA(new pcl::PointCloud<pcl::PointXYZ>);
02565 pcl::PointCloud<pcl::PointXYZ>::Ptr scanB(new pcl::PointCloud<pcl::PointXYZ>);
02566 float correspondenceRatio = 0.0f;
02567 if(ui_->checkBox_icp_2d->isChecked())
02568 {
02569
02570 cv::Mat oldLaserScan = rtabmap::uncompressData(dataFrom.getLaserScanCompressed());
02571 cv::Mat newLaserScan = rtabmap::uncompressData(dataTo.getLaserScanCompressed());
02572
02573 if(!oldLaserScan.empty() && !newLaserScan.empty())
02574 {
02575
02576 scanA = util3d::cvMat2Cloud(oldLaserScan);
02577 scanB = util3d::cvMat2Cloud(newLaserScan, t);
02578
02579
02580 if(ui_->doubleSpinBox_icp_voxel->value() > 0.0f)
02581 {
02582 scanA = util3d::voxelize<pcl::PointXYZ>(scanA, ui_->doubleSpinBox_icp_voxel->value());
02583 scanB = util3d::voxelize<pcl::PointXYZ>(scanB, ui_->doubleSpinBox_icp_voxel->value());
02584 }
02585
02586 if(scanB->size() && scanA->size())
02587 {
02588 transform = util3d::icp2D(scanB,
02589 scanA,
02590 ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
02591 ui_->spinBox_icp_iteration->value(),
02592 &hasConverged,
02593 &variance,
02594 &correspondences);
02595
02596 if(!transform.isNull())
02597 {
02598 if(dataTo.getLaserScanMaxPts())
02599 {
02600 correspondenceRatio = float(correspondences)/float(dataTo.getLaserScanMaxPts());
02601 }
02602 else if(ui_->doubleSpinBox_icp_minCorrespondenceRatio->value())
02603 {
02604 UWARN("Laser scan max pts not set, but correspondence ratio is set!");
02605 }
02606 }
02607 }
02608 }
02609 }
02610 else
02611 {
02612
02613 cv::Mat depthA = rtabmap::uncompressImage(dataFrom.getDepthCompressed());
02614 cv::Mat depthB = rtabmap::uncompressImage(dataTo.getDepthCompressed());
02615
02616 if(depthA.type() == CV_8UC1)
02617 {
02618 cv::Mat leftMono;
02619 cv::Mat left = rtabmap::uncompressImage(dataFrom.getImageCompressed());
02620 if(left.channels() > 1)
02621 {
02622 cv::cvtColor(left, leftMono, CV_BGR2GRAY);
02623 }
02624 else
02625 {
02626 leftMono = left;
02627 }
02628 cloudA = util3d::cloudFromDisparity(util3d::disparityFromStereoImages(leftMono, depthA), dataFrom.getCx(), dataFrom.getCy(), dataFrom.getFx(), dataFrom.getFy(), ui_->spinBox_icp_decimation->value());
02629 if(ui_->doubleSpinBox_icp_maxDepth->value() > 0)
02630 {
02631 cloudA = util3d::passThrough<pcl::PointXYZ>(cloudA, "z", 0, ui_->doubleSpinBox_icp_maxDepth->value());
02632 }
02633 if(ui_->doubleSpinBox_icp_voxel->value() > 0)
02634 {
02635 cloudA = util3d::voxelize<pcl::PointXYZ>(cloudA, ui_->doubleSpinBox_icp_voxel->value());
02636 }
02637 cloudA = util3d::transformPointCloud<pcl::PointXYZ>(cloudA, dataFrom.getLocalTransform());
02638 }
02639 else
02640 {
02641 cloudA = util3d::getICPReadyCloud(depthA,
02642 dataFrom.getFx(), dataFrom.getFy(), dataFrom.getCx(), dataFrom.getCy(),
02643 ui_->spinBox_icp_decimation->value(),
02644 ui_->doubleSpinBox_icp_maxDepth->value(),
02645 ui_->doubleSpinBox_icp_voxel->value(),
02646 0,
02647 dataFrom.getLocalTransform());
02648 }
02649 if(depthB.type() == CV_8UC1)
02650 {
02651 cv::Mat leftMono;
02652 cv::Mat left = rtabmap::uncompressImage(dataTo.getImageCompressed());
02653 if(left.channels() > 1)
02654 {
02655 cv::cvtColor(left, leftMono, CV_BGR2GRAY);
02656 }
02657 else
02658 {
02659 leftMono = left;
02660 }
02661 cloudB = util3d::cloudFromDisparity(util3d::disparityFromStereoImages(leftMono, depthB), dataTo.getCx(), dataTo.getCy(), dataTo.getFx(), dataTo.getFy(), ui_->spinBox_icp_decimation->value());
02662 if(ui_->doubleSpinBox_icp_maxDepth->value() > 0)
02663 {
02664 cloudB = util3d::passThrough<pcl::PointXYZ>(cloudB, "z", 0, ui_->doubleSpinBox_icp_maxDepth->value());
02665 }
02666 if(ui_->doubleSpinBox_icp_voxel->value() > 0)
02667 {
02668 cloudB = util3d::voxelize<pcl::PointXYZ>(cloudB, ui_->doubleSpinBox_icp_voxel->value());
02669 }
02670 cloudB = util3d::transformPointCloud<pcl::PointXYZ>(cloudB, t * dataTo.getLocalTransform());
02671 }
02672 else
02673 {
02674 cloudB = util3d::getICPReadyCloud(depthB,
02675 dataTo.getFx(), dataTo.getFy(), dataTo.getCx(), dataTo.getCy(),
02676 ui_->spinBox_icp_decimation->value(),
02677 ui_->doubleSpinBox_icp_maxDepth->value(),
02678 ui_->doubleSpinBox_icp_voxel->value(),
02679 0,
02680 t * dataTo.getLocalTransform());
02681 }
02682
02683 if(ui_->checkBox_icp_p2plane->isChecked())
02684 {
02685 pcl::PointCloud<pcl::PointNormal>::Ptr cloudANormals = util3d::computeNormals(cloudA, ui_->spinBox_icp_normalKSearch->value());
02686 pcl::PointCloud<pcl::PointNormal>::Ptr cloudBNormals = util3d::computeNormals(cloudB, ui_->spinBox_icp_normalKSearch->value());
02687
02688 cloudANormals = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(cloudANormals);
02689 if(cloudA->size() != cloudANormals->size())
02690 {
02691 UWARN("removed nan normals...");
02692 }
02693
02694 cloudBNormals = util3d::removeNaNNormalsFromPointCloud<pcl::PointNormal>(cloudBNormals);
02695 if(cloudB->size() != cloudBNormals->size())
02696 {
02697 UWARN("removed nan normals...");
02698 }
02699
02700 transform = util3d::icpPointToPlane(cloudBNormals,
02701 cloudANormals,
02702 ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
02703 ui_->spinBox_icp_iteration->value(),
02704 &hasConverged,
02705 &variance,
02706 &correspondences);
02707 }
02708 else
02709 {
02710 transform = util3d::icp(cloudB,
02711 cloudA,
02712 ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
02713 ui_->spinBox_icp_iteration->value(),
02714 &hasConverged,
02715 &variance,
02716 &correspondences);
02717
02718 correspondenceRatio = float(correspondences)/float(depthB.total());
02719 }
02720 }
02721
02722 if(hasConverged && !transform.isNull())
02723 {
02724 if(correspondenceRatio < ui_->doubleSpinBox_icp_minCorrespondenceRatio->value())
02725 {
02726 if(!silent)
02727 {
02728 QMessageBox::warning(this,
02729 tr("Refine link"),
02730 tr("Cannot find a transformation between nodes %1 and %2, correspondence ratio too low (%3).")
02731 .arg(from).arg(to).arg(correspondenceRatio));
02732 }
02733 }
02734 else
02735 {
02736
02737 Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform*t, variance, variance);
02738
02739 bool updated = false;
02740 std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
02741 while(iter != linksRefined_.end() && iter->first == currentLink.from())
02742 {
02743 if(iter->second.to() == currentLink.to() &&
02744 iter->second.type() == currentLink.type())
02745 {
02746 iter->second = newLink;
02747 updated = true;
02748 break;
02749 }
02750 ++iter;
02751 }
02752 if(!updated)
02753 {
02754 linksRefined_.insert(std::make_pair<int, Link>(newLink.from(), newLink));
02755
02756 if(updateGraph)
02757 {
02758 this->updateGraphView();
02759 }
02760 }
02761
02762 if(ui_->dockWidget_constraints->isVisible())
02763 {
02764 cloudB = util3d::transformPointCloud<pcl::PointXYZ>(cloudB, transform);
02765 scanB = util3d::transformPointCloud<pcl::PointXYZ>(scanB, transform);
02766 this->updateConstraintView(newLink, true, cloudA, cloudB, scanA, scanB);
02767 }
02768 }
02769 }
02770 else if(!silent)
02771 {
02772 QMessageBox::warning(this,
02773 tr("Refine link"),
02774 tr("Cannot find a transformation between nodes %1 and %2").arg(from).arg(to));
02775 }
02776 }
02777
02778 void DatabaseViewer::refineConstraintVisually()
02779 {
02780 int from = ids_.at(ui_->horizontalSlider_A->value());
02781 int to = ids_.at(ui_->horizontalSlider_B->value());
02782 refineConstraintVisually(from, to, false, true);
02783 }
02784
02785 void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, bool updateGraph)
02786 {
02787 if(from == to)
02788 {
02789 UWARN("Cannot refine link to same node");
02790 return;
02791 }
02792
02793 Link currentLink = findActiveLink(from, to);
02794 if(!currentLink.isValid())
02795 {
02796 UERROR("Not found link! (%d->%d)", from, to);
02797 return;
02798 }
02799
02800 Transform t;
02801 std::string rejectedMsg;
02802 double variance = -1.0;
02803 int inliers = -1;
02804 if(ui_->groupBox_visual_recomputeFeatures->isChecked())
02805 {
02806
02807 ParametersMap parameters;
02808 parameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(ui_->comboBox_featureType->currentIndex())));
02809 parameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(ui_->comboBox_nnType->currentIndex())));
02810 parameters.insert(ParametersPair(Parameters::kLccBowInlierDistance(), uNumber2Str(ui_->doubleSpinBox_visual_maxCorrespDistance->value())));
02811 parameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(ui_->doubleSpinBox_visual_maxDepth->value())));
02812 parameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(ui_->doubleSpinBox_visual_nndr->value())));
02813 parameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value())));
02814 parameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value())));
02815 parameters.insert(ParametersPair(Parameters::kMemGenerateIds(), "false"));
02816 parameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0"));
02817 parameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "0"));
02818 Memory tmpMemory(parameters);
02819
02820
02821 SensorData dataFrom = memory_->getSignatureData(from, true).toSensorData();
02822 SensorData dataTo = memory_->getSignatureData(to, true).toSensorData();
02823
02824 if(from > to)
02825 {
02826 tmpMemory.update(dataTo);
02827 tmpMemory.update(dataFrom);
02828 }
02829 else
02830 {
02831 tmpMemory.update(dataFrom);
02832 tmpMemory.update(dataTo);
02833 }
02834
02835
02836 t = tmpMemory.computeVisualTransform(to, from, &rejectedMsg, &inliers, &variance);
02837 }
02838 else
02839 {
02840 ParametersMap parameters;
02841 parameters.insert(ParametersPair(Parameters::kLccBowInlierDistance(), uNumber2Str(ui_->doubleSpinBox_visual_maxCorrespDistance->value())));
02842 parameters.insert(ParametersPair(Parameters::kLccBowMaxDepth(), uNumber2Str(ui_->doubleSpinBox_visual_maxDepth->value())));
02843 parameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value())));
02844 parameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value())));
02845 memory_->parseParameters(parameters);
02846 t = memory_->computeVisualTransform(to, from, &rejectedMsg, &inliers, &variance);
02847 }
02848
02849 if(!t.isNull())
02850 {
02851 if(ui_->checkBox_visual_2d->isChecked())
02852 {
02853
02854 float x,y,z,r,p,yaw;
02855 t.getTranslationAndEulerAngles(x,y,z, r,p,yaw);
02856 t = Transform::fromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
02857 }
02858
02859 Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), t, variance, variance);
02860
02861 bool updated = false;
02862 std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
02863 while(iter != linksRefined_.end() && iter->first == currentLink.from())
02864 {
02865 if(iter->second.to() == currentLink.to() &&
02866 iter->second.type() == currentLink.type())
02867 {
02868 iter->second = newLink;
02869 updated = true;
02870 break;
02871 }
02872 ++iter;
02873 }
02874 if(!updated)
02875 {
02876 linksRefined_.insert(std::make_pair<int, Link>(newLink.from(), newLink));
02877
02878 if(updateGraph)
02879 {
02880 this->updateGraphView();
02881 }
02882 }
02883 if(ui_->dockWidget_constraints->isVisible())
02884 {
02885 this->updateConstraintView(newLink);
02886 }
02887 }
02888 else if(!silent)
02889 {
02890 QMessageBox::warning(this,
02891 tr("Add link"),
02892 tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(rejectedMsg.c_str()));
02893 }
02894 }
02895
02896 void DatabaseViewer::addConstraint()
02897 {
02898 int from = ids_.at(ui_->horizontalSlider_A->value());
02899 int to = ids_.at(ui_->horizontalSlider_B->value());
02900 addConstraint(from, to, false, true);
02901 }
02902
02903 bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool updateGraph)
02904 {
02905 if(from == to)
02906 {
02907 UWARN("Cannot add link to same node");
02908 return false;
02909 }
02910
02911 bool updateSlider = false;
02912 if(!containsLink(linksAdded_, from, to) &&
02913 !containsLink(links_, from, to))
02914 {
02915 UASSERT(!containsLink(linksRemoved_, from, to));
02916 UASSERT(!containsLink(linksRefined_, from, to));
02917
02918 Transform t;
02919 std::string rejectedMsg;
02920 double variance = -1.0;
02921 int inliers = -1;
02922 if(ui_->groupBox_visual_recomputeFeatures->isChecked())
02923 {
02924
02925 ParametersMap parameters;
02926 parameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(ui_->comboBox_featureType->currentIndex())));
02927 parameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(ui_->comboBox_nnType->currentIndex())));
02928 parameters.insert(ParametersPair(Parameters::kLccBowInlierDistance(), uNumber2Str(ui_->doubleSpinBox_visual_maxCorrespDistance->value())));
02929 parameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(ui_->doubleSpinBox_visual_maxDepth->value())));
02930 parameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(ui_->doubleSpinBox_visual_nndr->value())));
02931 parameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value())));
02932 parameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value())));
02933 parameters.insert(ParametersPair(Parameters::kMemGenerateIds(), "false"));
02934 parameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0"));
02935 parameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "0"));
02936 Memory tmpMemory(parameters);
02937
02938
02939 SensorData dataFrom = memory_->getSignatureData(from, true).toSensorData();
02940 SensorData dataTo = memory_->getSignatureData(to, true).toSensorData();
02941
02942 if(from > to)
02943 {
02944 tmpMemory.update(dataTo);
02945 tmpMemory.update(dataFrom);
02946 }
02947 else
02948 {
02949 tmpMemory.update(dataFrom);
02950 tmpMemory.update(dataTo);
02951 }
02952
02953
02954 t = tmpMemory.computeVisualTransform(to, from, &rejectedMsg, &inliers, &variance);
02955
02956 if(!silent)
02957 {
02958 ui_->graphicsView_A->setFeatures(tmpMemory.getSignature(from)->getWords(), dataFrom.depth());
02959 ui_->graphicsView_B->setFeatures(tmpMemory.getSignature(to)->getWords(), dataTo.depth());
02960 updateWordsMatching();
02961 }
02962 }
02963 else
02964 {
02965 ParametersMap parameters;
02966 parameters.insert(ParametersPair(Parameters::kLccBowInlierDistance(), uNumber2Str(ui_->doubleSpinBox_visual_maxCorrespDistance->value())));
02967 parameters.insert(ParametersPair(Parameters::kLccBowMaxDepth(), uNumber2Str(ui_->doubleSpinBox_visual_maxDepth->value())));
02968 parameters.insert(ParametersPair(Parameters::kLccBowIterations(), uNumber2Str(ui_->spinBox_visual_iteration->value())));
02969 parameters.insert(ParametersPair(Parameters::kLccBowMinInliers(), uNumber2Str(ui_->spinBox_visual_minCorrespondences->value())));
02970 memory_->parseParameters(parameters);
02971 t = memory_->computeVisualTransform(to, from, &rejectedMsg, &inliers, &variance);
02972 }
02973
02974 if(!t.isNull())
02975 {
02976 if(ui_->checkBox_visual_2d->isChecked())
02977 {
02978
02979 float x,y,z,r,p,yaw;
02980 t.getTranslationAndEulerAngles(x,y,z, r,p,yaw);
02981 t = Transform::fromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
02982 }
02983
02984
02985 if(from>to)
02986 {
02987 linksAdded_.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, variance, variance)));
02988 }
02989 else
02990 {
02991 linksAdded_.insert(std::make_pair(to, Link(to, from, Link::kUserClosure, t.inverse(), variance, variance)));
02992 }
02993 updateSlider = true;
02994 }
02995 else if(!silent)
02996 {
02997 QMessageBox::warning(this,
02998 tr("Add link"),
02999 tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(rejectedMsg.c_str()));
03000 }
03001 }
03002 else if(containsLink(linksRemoved_, from, to))
03003 {
03004
03005 linksRemoved_.erase(rtabmap::graph::findLink(linksRemoved_, from, to));
03006 updateSlider = true;
03007 }
03008
03009 if(updateSlider)
03010 {
03011 updateLoopClosuresSlider(from, to);
03012 if(updateGraph)
03013 {
03014 this->updateGraphView();
03015 }
03016 }
03017 return updateSlider;
03018 }
03019
03020 void DatabaseViewer::resetConstraint()
03021 {
03022 int from = ids_.at(ui_->horizontalSlider_A->value());
03023 int to = ids_.at(ui_->horizontalSlider_B->value());
03024 if(from < to)
03025 {
03026 int tmp = to;
03027 to = from;
03028 from = tmp;
03029 }
03030
03031 if(from == to)
03032 {
03033 UWARN("Cannot reset link to same node");
03034 return;
03035 }
03036
03037
03038 std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, from, to);
03039 if(iter != linksRefined_.end())
03040 {
03041 linksRefined_.erase(iter);
03042 this->updateGraphView();
03043 }
03044
03045 iter = rtabmap::graph::findLink(links_, from, to);
03046 if(iter != links_.end())
03047 {
03048 this->updateConstraintView(iter->second);
03049 }
03050 iter = rtabmap::graph::findLink(linksAdded_, from, to);
03051 if(iter != linksAdded_.end())
03052 {
03053 this->updateConstraintView(iter->second);
03054 }
03055 }
03056
03057 void DatabaseViewer::rejectConstraint()
03058 {
03059 int from = ids_.at(ui_->horizontalSlider_A->value());
03060 int to = ids_.at(ui_->horizontalSlider_B->value());
03061 if(from < to)
03062 {
03063 int tmp = to;
03064 to = from;
03065 from = tmp;
03066 }
03067
03068 if(from == to)
03069 {
03070 UWARN("Cannot reject link to same node");
03071 return;
03072 }
03073
03074 bool removed = false;
03075
03076
03077 std::multimap<int, Link>::iterator iter;
03078 iter = rtabmap::graph::findLink(links_, from, to);
03079 if(iter != links_.end())
03080 {
03081 if(iter->second.type() == Link::kNeighbor)
03082 {
03083 UWARN("Cannot reject neighbor links (%d->%d)", from, to);
03084 return;
03085 }
03086 linksRemoved_.insert(*iter);
03087 removed = true;
03088 }
03089
03090
03091 iter = rtabmap::graph::findLink(linksRefined_, from, to);
03092 if(iter != linksRefined_.end())
03093 {
03094 linksRefined_.erase(iter);
03095 removed = true;
03096 }
03097 iter = rtabmap::graph::findLink(linksAdded_, from, to);
03098 if(iter != linksAdded_.end())
03099 {
03100 linksAdded_.erase(iter);
03101 removed = true;
03102 }
03103 if(removed)
03104 {
03105 this->updateGraphView();
03106 }
03107 updateLoopClosuresSlider();
03108 }
03109
03110 std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
03111 const std::multimap<int, rtabmap::Link> & edgeConstraints)
03112 {
03113 std::multimap<int, rtabmap::Link> links;
03114 for(std::multimap<int, rtabmap::Link>::const_iterator iter=edgeConstraints.begin();
03115 iter!=edgeConstraints.end();
03116 ++iter)
03117 {
03118 std::multimap<int, rtabmap::Link>::iterator findIter;
03119
03120 findIter = rtabmap::graph::findLink(linksRemoved_, iter->second.from(), iter->second.to());
03121 if(findIter != linksRemoved_.end())
03122 {
03123 if(!(iter->second.from() == findIter->second.from() &&
03124 iter->second.to() == findIter->second.to() &&
03125 iter->second.type() == findIter->second.type()))
03126 {
03127 UWARN("Links (%d->%d,%d) and (%d->%d,%d) are not equal!?",
03128 iter->second.from(), iter->second.to(), iter->second.type(),
03129 findIter->second.from(), findIter->second.to(), findIter->second.type());
03130 }
03131 else
03132 {
03133
03134 continue;
03135 }
03136 }
03137
03138 findIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
03139 if(findIter!=linksRefined_.end())
03140 {
03141 if(iter->second.from() == findIter->second.from() &&
03142 iter->second.to() == findIter->second.to() &&
03143 iter->second.type() == findIter->second.type())
03144 {
03145 links.insert(*findIter);
03146
03147 continue;
03148 }
03149 else
03150 {
03151 UWARN("Links (%d->%d,%d) and (%d->%d,%d) are not equal!?",
03152 iter->second.from(), iter->second.to(), iter->second.type(),
03153 findIter->second.from(), findIter->second.to(), findIter->second.type());
03154 }
03155 }
03156
03157 links.insert(*iter);
03158 }
03159
03160
03161 for(std::multimap<int, rtabmap::Link>::const_iterator iter=linksAdded_.begin();
03162 iter!=linksAdded_.end();
03163 ++iter)
03164 {
03165
03166 links.insert(*iter);
03167 }
03168
03169 return links;
03170 }
03171
03172 void DatabaseViewer::updateLoopClosuresSlider(int from, int to)
03173 {
03174 int size = loopLinks_.size();
03175 loopLinks_.clear();
03176 std::multimap<int, Link> links = updateLinksWithModifications(links_);
03177 int position = ui_->horizontalSlider_loops->value();
03178 for(std::multimap<int, rtabmap::Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
03179 {
03180 if(!iter->second.transform().isNull())
03181 {
03182 if(iter->second.type() != rtabmap::Link::kNeighbor)
03183 {
03184 if((iter->second.from() == from && iter->second.to() == to) ||
03185 (iter->second.to() == from && iter->second.from() == to))
03186 {
03187 position = loopLinks_.size();
03188 }
03189 loopLinks_.append(iter->second);
03190 }
03191 }
03192 else
03193 {
03194 UERROR("Transform null for link from %d to %d", iter->first, iter->second.to());
03195 }
03196 }
03197
03198 if(loopLinks_.size())
03199 {
03200 if(loopLinks_.size() == 1)
03201 {
03202
03203 loopLinks_.push_back(loopLinks_.front());
03204 }
03205 ui_->horizontalSlider_loops->setMinimum(0);
03206 ui_->horizontalSlider_loops->setMaximum(loopLinks_.size()-1);
03207 ui_->horizontalSlider_loops->setEnabled(true);
03208 if(position != ui_->horizontalSlider_loops->value())
03209 {
03210 ui_->horizontalSlider_loops->setValue(position);
03211 }
03212 else if(size != loopLinks_.size())
03213 {
03214 this->updateConstraintView(loopLinks_.at(position));
03215 }
03216 }
03217 else
03218 {
03219 ui_->horizontalSlider_loops->setEnabled(false);
03220 ui_->constraintsViewer->removeAllClouds();
03221 ui_->constraintsViewer->update();
03222 updateConstraintButtons();
03223 }
03224 }
03225
03226 }