DatabaseViewer.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include "rtabmap/gui/DatabaseViewer.h"
29 #include "rtabmap/gui/CloudViewer.h"
30 #include "ui_DatabaseViewer.h"
31 #include <QMessageBox>
32 #include <QFileDialog>
33 #include <QInputDialog>
34 #include <QDesktopWidget>
35 #include <QColorDialog>
36 #include <QGraphicsLineItem>
37 #include <QtGui/QCloseEvent>
38 #include <QGraphicsOpacityEffect>
39 #include <QtCore/QBuffer>
40 #include <QtCore/QTextStream>
41 #include <QtCore/QDateTime>
42 #include <QtCore/QSettings>
43 #include <rtabmap/utilite/ULogger.h>
44 #include <rtabmap/utilite/UDirectory.h>
45 #include <rtabmap/utilite/UConversion.h>
46 #include <opencv2/core/core_c.h>
47 #include <opencv2/imgproc/types_c.h>
48 #include <opencv2/highgui/highgui.hpp>
49 #include <rtabmap/utilite/UTimer.h>
50 #include <rtabmap/utilite/UFile.h>
51 #include "rtabmap/utilite/UPlot.h"
52 #include "rtabmap/core/DBDriver.h"
53 #include "rtabmap/gui/KeypointItem.h"
54 #include "rtabmap/gui/CloudViewer.h"
55 #include "rtabmap/utilite/UCv2Qt.h"
56 #include "rtabmap/core/util3d.h"
57 #include "rtabmap/core/util3d_transforms.h"
58 #include "rtabmap/core/util3d_filtering.h"
59 #include "rtabmap/core/util3d_surface.h"
60 #include "rtabmap/core/util3d_registration.h"
61 #include "rtabmap/core/util3d_mapping.h"
62 #include "rtabmap/core/util2d.h"
63 #include "rtabmap/core/Signature.h"
64 #include "rtabmap/core/Memory.h"
65 #include "rtabmap/core/Features2d.h"
66 #include "rtabmap/core/Compression.h"
67 #include "rtabmap/core/Graph.h"
68 #include "rtabmap/core/Stereo.h"
69 #include "rtabmap/core/Optimizer.h"
70 #include "rtabmap/core/RegistrationVis.h"
71 #include "rtabmap/core/RegistrationIcp.h"
72 #include "rtabmap/core/OccupancyGrid.h"
73 #include "rtabmap/core/GeodeticCoords.h"
74 #include "rtabmap/core/Recovery.h"
75 #include "rtabmap/gui/DataRecorder.h"
76 #include "rtabmap/gui/ExportCloudsDialog.h"
77 #include "rtabmap/gui/EditDepthArea.h"
78 #include "rtabmap/core/SensorData.h"
79 #include "rtabmap/core/GainCompensator.h"
80 #include "rtabmap/gui/ExportDialog.h"
81 #include "rtabmap/gui/ProgressDialog.h"
82 #include "rtabmap/gui/ParametersToolBox.h"
83 #include "rtabmap/gui/RecoveryState.h"
84 #include <pcl/io/pcd_io.h>
85 #include <pcl/io/ply_io.h>
86 #include <pcl/io/obj_io.h>
87 #include <pcl/filters/voxel_grid.h>
88 #include <pcl/common/transforms.h>
89 #include <pcl/common/common.h>
90 
91 #ifdef RTABMAP_OCTOMAP
92 #include "rtabmap/core/OctoMap.h"
93 #endif
94 
95 namespace rtabmap {
96 
97 DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
98  QMainWindow(parent),
99  dbDriver_(0),
100  octomap_(0),
101  exportDialog_(new ExportCloudsDialog(this)),
102  editDepthDialog_(new QDialog(this)),
103  savedMaximized_(false),
104  firstCall_(true),
105  iniFilePath_(ini),
106  useLastOptimizedGraphAsGuess_(false)
107 {
108  pathDatabase_ = QDir::homePath()+"/Documents/RTAB-Map"; //use home directory by default
109 
110  if(!UDirectory::exists(pathDatabase_.toStdString()))
111  {
112  pathDatabase_ = QDir::homePath();
113  }
114 
115  ui_ = new Ui_DatabaseViewer();
116  ui_->setupUi(this);
117  ui_->buttonBox->setVisible(false);
118  connect(ui_->buttonBox->button(QDialogButtonBox::Close), SIGNAL(clicked()), this, SLOT(close()));
119 
120  ui_->comboBox_logger_level->setVisible(parent==0);
121  ui_->label_logger_level->setVisible(parent==0);
122  connect(ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(int)), this, SLOT(updateLoggerLevel()));
123  connect(ui_->actionVertical_Layout, SIGNAL(toggled(bool)), this, SLOT(setupMainLayout(bool)));
124 
125  editDepthDialog_->resize(640, 480);
126  QVBoxLayout * vLayout = new QVBoxLayout(editDepthDialog_);
127  editDepthArea_ = new EditDepthArea(editDepthDialog_);
128  vLayout->setContentsMargins(0,0,0,0);
129  vLayout->setSpacing(0);
130  vLayout->addWidget(editDepthArea_, 1);
131  QDialogButtonBox * buttonBox = new QDialogButtonBox(QDialogButtonBox::Save | QDialogButtonBox::Cancel | QDialogButtonBox::Reset, Qt::Horizontal, editDepthDialog_);
132  vLayout->addWidget(buttonBox);
133  connect(buttonBox, SIGNAL(accepted()), editDepthDialog_, SLOT(accept()));
134  connect(buttonBox, SIGNAL(rejected()), editDepthDialog_, SLOT(reject()));
135  connect(buttonBox->button(QDialogButtonBox::Reset), SIGNAL(clicked()), editDepthArea_, SLOT(resetChanges()));
136  editDepthDialog_->setLayout(vLayout);
137  editDepthDialog_->setWindowTitle(tr("Edit Depth Image"));
138 
139  QString title("RTAB-Map Database Viewer[*]");
140  this->setWindowTitle(title);
141 
142  ui_->dockWidget_constraints->setVisible(false);
143  ui_->dockWidget_graphView->setVisible(false);
144  ui_->dockWidget_occupancyGridView->setVisible(false);
145  ui_->dockWidget_guiparameters->setVisible(false);
146  ui_->dockWidget_coreparameters->setVisible(false);
147  ui_->dockWidget_info->setVisible(false);
148  ui_->dockWidget_stereoView->setVisible(false);
149  ui_->dockWidget_view3d->setVisible(false);
150  ui_->dockWidget_statistics->setVisible(false);
151 
152  // Create cloud viewers
153  constraintsViewer_ = new CloudViewer(ui_->dockWidgetContents);
154  cloudViewer_ = new CloudViewer(ui_->dockWidgetContents_3dviews);
155  stereoViewer_ = new CloudViewer(ui_->dockWidgetContents_stereo);
156  occupancyGridViewer_ = new CloudViewer(ui_->dockWidgetContents_occupancyGrid);
157  constraintsViewer_->setObjectName("constraintsViewer");
158  cloudViewer_->setObjectName("cloudViewerA");
159  stereoViewer_->setObjectName("stereoViewer");
160  occupancyGridViewer_->setObjectName("occupancyGridView");
161  ui_->layout_constraintsViewer->addWidget(constraintsViewer_);
162  ui_->horizontalLayout_3dviews->addWidget(cloudViewer_, 1);
163  ui_->horizontalLayout_stereo->addWidget(stereoViewer_, 1);
164  ui_->layout_occupancyGridView->addWidget(occupancyGridViewer_, 1);
165 
166  constraintsViewer_->setCameraLockZ(false);
167  constraintsViewer_->setCameraFree();
168  occupancyGridViewer_->setCameraFree();
169 
170  ui_->graphicsView_stereo->setAlpha(255);
171 
172 #ifndef RTABMAP_OCTOMAP
173  ui_->checkBox_octomap->setEnabled(false);
174  ui_->checkBox_octomap->setChecked(false);
175 #endif
176 
177  ParametersMap parameters;
178  uInsert(parameters, Parameters::getDefaultParameters("SURF"));
179  uInsert(parameters, Parameters::getDefaultParameters("SIFT"));
180  uInsert(parameters, Parameters::getDefaultParameters("BRIEF"));
181  uInsert(parameters, Parameters::getDefaultParameters("FAST"));
182  uInsert(parameters, Parameters::getDefaultParameters("GFTT"));
183  uInsert(parameters, Parameters::getDefaultParameters("ORB"));
184  uInsert(parameters, Parameters::getDefaultParameters("FREAK"));
185  uInsert(parameters, Parameters::getDefaultParameters("BRISK"));
186  uInsert(parameters, Parameters::getDefaultParameters("Optimizer"));
187  uInsert(parameters, Parameters::getDefaultParameters("g2o"));
188  uInsert(parameters, Parameters::getDefaultParameters("GTSAM"));
189  uInsert(parameters, Parameters::getDefaultParameters("Reg"));
190  uInsert(parameters, Parameters::getDefaultParameters("Vis"));
191  uInsert(parameters, Parameters::getDefaultParameters("Icp"));
192  uInsert(parameters, Parameters::getDefaultParameters("Stereo"));
193  uInsert(parameters, Parameters::getDefaultParameters("StereoBM"));
194  uInsert(parameters, Parameters::getDefaultParameters("Grid"));
195  uInsert(parameters, Parameters::getDefaultParameters("GridGlobal"));
196  parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDOptimizeMaxError()));
197  parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDLoopClosureReextractFeatures()));
198  ui_->parameters_toolbox->setupUi(parameters);
199  exportDialog_->setObjectName("ExportCloudsDialog");
200  restoreDefaultSettings();
201  this->readSettings();
202 
203  setupMainLayout(ui_->actionVertical_Layout->isChecked());
204  ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked());
205  ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
206  ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
207  ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
208  ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
209  ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
210 
211  ui_->menuView->addAction(ui_->dockWidget_constraints->toggleViewAction());
212  ui_->menuView->addAction(ui_->dockWidget_graphView->toggleViewAction());
213  ui_->menuView->addAction(ui_->dockWidget_occupancyGridView->toggleViewAction());
214  ui_->menuView->addAction(ui_->dockWidget_stereoView->toggleViewAction());
215  ui_->menuView->addAction(ui_->dockWidget_view3d->toggleViewAction());
216  ui_->menuView->addAction(ui_->dockWidget_guiparameters->toggleViewAction());
217  ui_->menuView->addAction(ui_->dockWidget_coreparameters->toggleViewAction());
218  ui_->menuView->addAction(ui_->dockWidget_info->toggleViewAction());
219  ui_->menuView->addAction(ui_->dockWidget_statistics->toggleViewAction());
220  connect(ui_->dockWidget_graphView->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView()));
221  connect(ui_->dockWidget_occupancyGridView->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView()));
222  connect(ui_->dockWidget_statistics->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateStatistics()));
223 
224 
225  connect(ui_->parameters_toolbox, SIGNAL(parametersChanged(const QStringList &)), this, SLOT(notifyParametersChanged(const QStringList &)));
226 
227  connect(ui_->actionQuit, SIGNAL(triggered()), this, SLOT(close()));
228 
229  ui_->actionOpen_database->setEnabled(true);
230  ui_->actionClose_database->setEnabled(false);
231 
232  // connect actions with custom slots
233  ui_->actionSave_config->setShortcut(QKeySequence::Save);
234  connect(ui_->actionSave_config, SIGNAL(triggered()), this, SLOT(writeSettings()));
235  connect(ui_->actionOpen_database, SIGNAL(triggered()), this, SLOT(openDatabase()));
236  connect(ui_->actionClose_database, SIGNAL(triggered()), this, SLOT(closeDatabase()));
237  connect(ui_->actionDatabase_recovery, SIGNAL(triggered()), this, SLOT(recoverDatabase()));
238  connect(ui_->actionExport, SIGNAL(triggered()), this, SLOT(exportDatabase()));
239  connect(ui_->actionExtract_images, SIGNAL(triggered()), this, SLOT(extractImages()));
240  connect(ui_->actionEdit_depth_image, SIGNAL(triggered()), this, SLOT(editDepthImage()));
241  connect(ui_->actionGenerate_graph_dot, SIGNAL(triggered()), this, SLOT(generateGraph()));
242  connect(ui_->actionGenerate_local_graph_dot, SIGNAL(triggered()), this, SLOT(generateLocalGraph()));
243  connect(ui_->actionRaw_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRaw()));
244  connect(ui_->actionRGBD_SLAM_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAM()));
245  connect(ui_->actionKITTI_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesKITTI()));
246  connect(ui_->actionTORO_graph, SIGNAL(triggered()), this , SLOT(exportPosesTORO()));
247  connect(ui_->actionG2o_g2o, SIGNAL(triggered()), this , SLOT(exportPosesG2O()));
248  connect(ui_->actionPoses_KML, SIGNAL(triggered()), this , SLOT(exportPosesKML()));
249  connect(ui_->actionGPS_TXT, SIGNAL(triggered()), this , SLOT(exportGPS_TXT()));
250  connect(ui_->actionGPS_KML, SIGNAL(triggered()), this , SLOT(exportGPS_KML()));
251  connect(ui_->actionExport_saved_2D_map, SIGNAL(triggered()), this , SLOT(exportSaved2DMap()));
252  connect(ui_->actionImport_2D_map, SIGNAL(triggered()), this , SLOT(import2DMap()));
253  connect(ui_->actionView_optimized_mesh, SIGNAL(triggered()), this , SLOT(viewOptimizedMesh()));
254  connect(ui_->actionExport_optimized_mesh, SIGNAL(triggered()), this , SLOT(exportOptimizedMesh()));
255  connect(ui_->actionUpdate_optimized_mesh, SIGNAL(triggered()), this , SLOT(updateOptimizedMesh()));
256  connect(ui_->actionView_3D_map, SIGNAL(triggered()), this, SLOT(view3DMap()));
257  connect(ui_->actionGenerate_3D_map_pcd, SIGNAL(triggered()), this, SLOT(generate3DMap()));
258  connect(ui_->actionDetect_more_loop_closures, SIGNAL(triggered()), this, SLOT(detectMoreLoopClosures()));
259  connect(ui_->actionRefine_all_neighbor_links, SIGNAL(triggered()), this, SLOT(refineAllNeighborLinks()));
260  connect(ui_->actionRefine_all_loop_closure_links, SIGNAL(triggered()), this, SLOT(refineAllLoopClosureLinks()));
261  connect(ui_->actionRegenerate_local_grid_maps, SIGNAL(triggered()), this, SLOT(regenerateLocalMaps()));
262  connect(ui_->actionRegenerate_local_grid_maps_selected, SIGNAL(triggered()), this, SLOT(regenerateCurrentLocalMaps()));
263  connect(ui_->actionReset_all_changes, SIGNAL(triggered()), this, SLOT(resetAllChanges()));
264  connect(ui_->actionRestore_default_GUI_settings, SIGNAL(triggered()), this, SLOT(restoreDefaultSettings()));
265 
266  //ICP buttons
267  connect(ui_->pushButton_refine, SIGNAL(clicked()), this, SLOT(refineConstraint()));
268  connect(ui_->pushButton_add, SIGNAL(clicked()), this, SLOT(addConstraint()));
269  connect(ui_->pushButton_reset, SIGNAL(clicked()), this, SLOT(resetConstraint()));
270  connect(ui_->pushButton_reject, SIGNAL(clicked()), this, SLOT(rejectConstraint()));
271  ui_->pushButton_refine->setEnabled(false);
272  ui_->pushButton_add->setEnabled(false);
273  ui_->pushButton_reset->setEnabled(false);
274  ui_->pushButton_reject->setEnabled(false);
275 
276  ui_->menuEdit->setEnabled(false);
277  ui_->actionGenerate_3D_map_pcd->setEnabled(false);
278  ui_->actionExport->setEnabled(false);
279  ui_->actionExtract_images->setEnabled(false);
280  ui_->menuExport_poses->setEnabled(false);
281  ui_->menuExport_GPS->setEnabled(false);
282  ui_->actionPoses_KML->setEnabled(false);
283  ui_->actionExport_saved_2D_map->setEnabled(false);
284  ui_->actionView_optimized_mesh->setEnabled(false);
285  ui_->actionExport_optimized_mesh->setEnabled(false);
286  ui_->actionUpdate_optimized_mesh->setEnabled(false);
287 
288  ui_->horizontalSlider_A->setTracking(false);
289  ui_->horizontalSlider_B->setTracking(false);
290  ui_->horizontalSlider_A->setEnabled(false);
291  ui_->horizontalSlider_B->setEnabled(false);
292  connect(ui_->horizontalSlider_A, SIGNAL(valueChanged(int)), this, SLOT(sliderAValueChanged(int)));
293  connect(ui_->horizontalSlider_B, SIGNAL(valueChanged(int)), this, SLOT(sliderBValueChanged(int)));
294  connect(ui_->horizontalSlider_A, SIGNAL(sliderMoved(int)), this, SLOT(sliderAMoved(int)));
295  connect(ui_->horizontalSlider_B, SIGNAL(sliderMoved(int)), this, SLOT(sliderBMoved(int)));
296 
297  connect(ui_->spinBox_mesh_angleTolerance, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
298  connect(ui_->spinBox_mesh_minClusterSize, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
299  connect(ui_->spinBox_mesh_fillDepthHoles, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
300  connect(ui_->spinBox_mesh_depthError, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
301  connect(ui_->checkBox_mesh_quad, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
302  connect(ui_->spinBox_mesh_triangleSize, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
303  connect(ui_->checkBox_showWords, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
304  connect(ui_->checkBox_showCloud, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
305  connect(ui_->checkBox_showMesh, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
306  connect(ui_->checkBox_showScan, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
307  connect(ui_->checkBox_showMap, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
308  connect(ui_->checkBox_showGrid, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
309  connect(ui_->checkBox_odomFrame_3dview, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
310 
311  ui_->horizontalSlider_neighbors->setTracking(false);
312  ui_->horizontalSlider_loops->setTracking(false);
313  ui_->horizontalSlider_neighbors->setEnabled(false);
314  ui_->horizontalSlider_loops->setEnabled(false);
315  connect(ui_->horizontalSlider_neighbors, SIGNAL(valueChanged(int)), this, SLOT(sliderNeighborValueChanged(int)));
316  connect(ui_->horizontalSlider_loops, SIGNAL(valueChanged(int)), this, SLOT(sliderLoopValueChanged(int)));
317  connect(ui_->horizontalSlider_neighbors, SIGNAL(sliderMoved(int)), this, SLOT(sliderNeighborValueChanged(int)));
318  connect(ui_->horizontalSlider_loops, SIGNAL(sliderMoved(int)), this, SLOT(sliderLoopValueChanged(int)));
319  connect(ui_->checkBox_showOptimized, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
320  connect(ui_->checkBox_show3Dclouds, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
321  connect(ui_->checkBox_show2DScans, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
322  connect(ui_->checkBox_show3DWords, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
323  connect(ui_->checkBox_odomFrame, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
324  ui_->checkBox_showOptimized->setEnabled(false);
325 
326  ui_->horizontalSlider_iterations->setTracking(false);
327  ui_->horizontalSlider_iterations->setEnabled(false);
328  ui_->spinBox_optimizationsFrom->setEnabled(false);
329  connect(ui_->horizontalSlider_iterations, SIGNAL(valueChanged(int)), this, SLOT(sliderIterationsValueChanged(int)));
330  connect(ui_->horizontalSlider_iterations, SIGNAL(sliderMoved(int)), this, SLOT(sliderIterationsValueChanged(int)));
331  connect(ui_->spinBox_optimizationsFrom, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
332  connect(ui_->checkBox_iterativeOptimization, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
333  connect(ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
334  connect(ui_->checkBox_wmState, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
335  connect(ui_->graphViewer, SIGNAL(mapShownRequested()), this, SLOT(updateGraphView()));
336  connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
337  connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
338  connect(ui_->checkBox_ignoreGlobalLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
339  connect(ui_->checkBox_ignoreLocalLoopSpace, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
340  connect(ui_->checkBox_ignoreLocalLoopTime, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
341  connect(ui_->checkBox_ignoreUserLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
342  connect(ui_->spinBox_optimizationDepth, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
343  connect(ui_->doubleSpinBox_optimizationScale, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
344  connect(ui_->checkBox_octomap, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
345  connect(ui_->checkBox_grid_2d, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
346  connect(ui_->comboBox_octomap_rendering_type, SIGNAL(currentIndexChanged(int)), this, SLOT(updateOctomapView()));
347  connect(ui_->spinBox_grid_depth, SIGNAL(valueChanged(int)), this, SLOT(updateOctomapView()));
348  connect(ui_->checkBox_grid_empty, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
349  connect(ui_->doubleSpinBox_gainCompensationRadius, SIGNAL(valueChanged(double)), this, SLOT(updateConstraintView()));
350  connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(updateConstraintView()));
351  connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(update3dView()));
352  connect(ui_->spinBox_decimation, SIGNAL(valueChanged(int)), this, SLOT(updateConstraintView()));
353  connect(ui_->spinBox_decimation, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
354  connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(updateGraphView()));
355  connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
356  connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
357 
358  ui_->label_stereo_inliers_name->setStyleSheet("QLabel {color : blue; }");
359  ui_->label_stereo_flowOutliers_name->setStyleSheet("QLabel {color : red; }");
360  ui_->label_stereo_slopeOutliers_name->setStyleSheet("QLabel {color : yellow; }");
361  ui_->label_stereo_disparityOutliers_name->setStyleSheet("QLabel {color : magenta; }");
362 
363 
364  // connect configuration changed
365  connect(ui_->graphViewer, SIGNAL(configChanged()), this, SLOT(configModified()));
366  //connect(ui_->graphicsView_A, SIGNAL(configChanged()), this, SLOT(configModified()));
367  //connect(ui_->graphicsView_B, SIGNAL(configChanged()), this, SLOT(configModified()));
368  connect(ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
369  connect(ui_->actionVertical_Layout, SIGNAL(toggled(bool)), this, SLOT(configModified()));
370  connect(ui_->checkBox_alignPosesWithGroundTruth, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
371  connect(ui_->checkBox_alignScansCloudsWithGroundTruth, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
372  connect(ui_->checkBox_ignoreIntermediateNodes, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
373  connect(ui_->checkBox_ignoreIntermediateNodes, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
374  connect(ui_->checkBox_timeStats, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
375  connect(ui_->checkBox_timeStats, SIGNAL(stateChanged(int)), this, SLOT(updateStatistics()));
376  // Graph view
377  connect(ui_->doubleSpinBox_gainCompensationRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
378  connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
379  connect(ui_->spinBox_decimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
380  connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(configModified()));
381  connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
382  connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
383 
384  connect(ui_->spinBox_icp_decimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
385  connect(ui_->doubleSpinBox_icp_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
386  connect(ui_->doubleSpinBox_icp_minDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
387  connect(ui_->checkBox_icp_from_depth, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
388 
389  connect(ui_->doubleSpinBox_detectMore_radius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
390  connect(ui_->doubleSpinBox_detectMore_angle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
391  connect(ui_->spinBox_detectMore_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
392 
393  connect(ui_->lineEdit_obstacleColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified()));
394  connect(ui_->lineEdit_groundColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified()));
395  connect(ui_->lineEdit_emptyColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified()));
396  connect(ui_->lineEdit_obstacleColor, SIGNAL(textChanged(const QString &)), this, SLOT(updateGrid()));
397  connect(ui_->lineEdit_groundColor, SIGNAL(textChanged(const QString &)), this, SLOT(updateGrid()));
398  connect(ui_->lineEdit_emptyColor, SIGNAL(textChanged(const QString &)), this, SLOT(updateGrid()));
399  connect(ui_->toolButton_obstacleColor, SIGNAL(clicked(bool)), this, SLOT(selectObstacleColor()));
400  connect(ui_->toolButton_groundColor, SIGNAL(clicked(bool)), this, SLOT(selectGroundColor()));
401  connect(ui_->toolButton_emptyColor, SIGNAL(clicked(bool)), this, SLOT(selectEmptyColor()));
402 
403  connect(exportDialog_, SIGNAL(configChanged()), this, SLOT(configModified()));
404 
405  // dockwidget
406  QList<QDockWidget*> dockWidgets = this->findChildren<QDockWidget*>();
407  for(int i=0; i<dockWidgets.size(); ++i)
408  {
409  connect(dockWidgets[i], SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(configModified()));
410  connect(dockWidgets[i]->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configModified()));
411  }
412  ui_->dockWidget_constraints->installEventFilter(this);
413  ui_->dockWidget_graphView->installEventFilter(this);
414  ui_->dockWidget_occupancyGridView->installEventFilter(this);
415  ui_->dockWidget_stereoView->installEventFilter(this);
416  ui_->dockWidget_view3d->installEventFilter(this);
417  ui_->dockWidget_guiparameters->installEventFilter(this);
418  ui_->dockWidget_coreparameters->installEventFilter(this);
419  ui_->dockWidget_info->installEventFilter(this);
420  ui_->dockWidget_statistics->installEventFilter(this);
421 }
422 
423 DatabaseViewer::~DatabaseViewer()
424 {
425  delete ui_;
426  delete dbDriver_;
427 #ifdef RTABMAP_OCTOMAP
428  delete octomap_;
429 #endif
430 }
431 
432 void DatabaseViewer::setupMainLayout(bool vertical)
433 {
434  if(vertical)
435  {
436  qobject_cast<QHBoxLayout *>(ui_->horizontalLayout_imageViews->layout())->setDirection(QBoxLayout::TopToBottom);
437  }
438  else if(!vertical)
439  {
440  qobject_cast<QHBoxLayout *>(ui_->horizontalLayout_imageViews->layout())->setDirection(QBoxLayout::LeftToRight);
441  }
442  if(ids_.size())
443  {
444  sliderAValueChanged(ui_->horizontalSlider_A->value()); // update matching lines
445  }
446 }
447 
448 void DatabaseViewer::showCloseButton(bool visible)
449 {
450  ui_->buttonBox->setVisible(visible);
451 }
452 
453 void DatabaseViewer::configModified()
454 {
455  this->setWindowModified(true);
456 }
457 
458 QString DatabaseViewer::getIniFilePath() const
459 {
460  if(!iniFilePath_.isEmpty())
461  {
462  return iniFilePath_;
463  }
464  QString privatePath = QDir::homePath() + "/.rtabmap";
465  if(!QDir(privatePath).exists())
466  {
467  QDir::home().mkdir(".rtabmap");
468  }
469  return privatePath + "/rtabmap.ini";
470 }
471 
472 void DatabaseViewer::readSettings()
473 {
474  QString path = getIniFilePath();
475  QSettings settings(path, QSettings::IniFormat);
476  settings.beginGroup("DatabaseViewer");
477 
478  //load window state / geometry
479  QByteArray bytes;
480  bytes = settings.value("geometry", QByteArray()).toByteArray();
481  if(!bytes.isEmpty())
482  {
483  this->restoreGeometry(bytes);
484  }
485  bytes = settings.value("state", QByteArray()).toByteArray();
486  if(!bytes.isEmpty())
487  {
488  this->restoreState(bytes);
489  }
490  savedMaximized_ = settings.value("maximized", false).toBool();
491 
492  ui_->comboBox_logger_level->setCurrentIndex(settings.value("loggerLevel", ui_->comboBox_logger_level->currentIndex()).toInt());
493  ui_->actionVertical_Layout->setChecked(settings.value("verticalLayout", ui_->actionVertical_Layout->isChecked()).toBool());
494  ui_->checkBox_ignoreIntermediateNodes->setChecked(settings.value("ignoreIntermediateNodes", ui_->checkBox_ignoreIntermediateNodes->isChecked()).toBool());
495  ui_->checkBox_timeStats->setChecked(settings.value("timeStats", ui_->checkBox_timeStats->isChecked()).toBool());
496 
497  // GraphViewer settings
498  ui_->graphViewer->loadSettings(settings, "GraphView");
499 
500  settings.beginGroup("optimization");
501  ui_->doubleSpinBox_gainCompensationRadius->setValue(settings.value("gainCompensationRadius", ui_->doubleSpinBox_gainCompensationRadius->value()).toDouble());
502  ui_->doubleSpinBox_voxelSize->setValue(settings.value("voxelSize", ui_->doubleSpinBox_voxelSize->value()).toDouble());
503  ui_->spinBox_decimation->setValue(settings.value("decimation", ui_->spinBox_decimation->value()).toInt());
504  settings.endGroup();
505 
506  settings.beginGroup("grid");
507  ui_->groupBox_posefiltering->setChecked(settings.value("poseFiltering", ui_->groupBox_posefiltering->isChecked()).toBool());
508  ui_->doubleSpinBox_posefilteringRadius->setValue(settings.value("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value()).toDouble());
509  ui_->doubleSpinBox_posefilteringAngle->setValue(settings.value("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value()).toDouble());
510  ui_->lineEdit_obstacleColor->setText(settings.value("colorObstacle", ui_->lineEdit_obstacleColor->text()).toString());
511  ui_->lineEdit_groundColor->setText(settings.value("colorGround", ui_->lineEdit_groundColor->text()).toString());
512  ui_->lineEdit_emptyColor->setText(settings.value("colorEmpty", ui_->lineEdit_emptyColor->text()).toString());
513  settings.endGroup();
514 
515  settings.beginGroup("mesh");
516  ui_->checkBox_mesh_quad->setChecked(settings.value("quad", ui_->checkBox_mesh_quad->isChecked()).toBool());
517  ui_->spinBox_mesh_angleTolerance->setValue(settings.value("angleTolerance", ui_->spinBox_mesh_angleTolerance->value()).toInt());
518  ui_->spinBox_mesh_minClusterSize->setValue(settings.value("minClusterSize", ui_->spinBox_mesh_minClusterSize->value()).toInt());
519  ui_->spinBox_mesh_fillDepthHoles->setValue(settings.value("fillDepthHolesSize", ui_->spinBox_mesh_fillDepthHoles->value()).toInt());
520  ui_->spinBox_mesh_depthError->setValue(settings.value("fillDepthHolesError", ui_->spinBox_mesh_depthError->value()).toInt());
521  ui_->spinBox_mesh_triangleSize->setValue(settings.value("triangleSize", ui_->spinBox_mesh_triangleSize->value()).toInt());
522  settings.endGroup();
523 
524  // ImageViews
525  //ui_->graphicsView_A->loadSettings(settings, "ImageViewA");
526  //ui_->graphicsView_B->loadSettings(settings, "ImageViewB");
527 
528  // ICP parameters
529  settings.beginGroup("icp");
530  ui_->spinBox_icp_decimation->setValue(settings.value("decimation", ui_->spinBox_icp_decimation->value()).toInt());
531  ui_->doubleSpinBox_icp_maxDepth->setValue(settings.value("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value()).toDouble());
532  ui_->doubleSpinBox_icp_minDepth->setValue(settings.value("minDepth", ui_->doubleSpinBox_icp_minDepth->value()).toDouble());
533  ui_->checkBox_icp_from_depth->setChecked(settings.value("icpFromDepth", ui_->checkBox_icp_from_depth->isChecked()).toBool());
534  settings.endGroup();
535 
536  settings.endGroup(); // DatabaseViewer
537 
538  // Use same parameters used by RTAB-Map
539  settings.beginGroup("Gui");
540  exportDialog_->loadSettings(settings, exportDialog_->objectName());
541  settings.beginGroup("PostProcessingDialog");
542  ui_->doubleSpinBox_detectMore_radius->setValue(settings.value("cluster_radius", ui_->doubleSpinBox_detectMore_radius->value()).toDouble());
543  ui_->doubleSpinBox_detectMore_angle->setValue(settings.value("cluster_angle", ui_->doubleSpinBox_detectMore_angle->value()).toDouble());
544  ui_->spinBox_detectMore_iterations->setValue(settings.value("iterations", ui_->spinBox_detectMore_iterations->value()).toInt());
545  settings.endGroup();
546  settings.endGroup();
547 
548 
549  ParametersMap parameters;
550  Parameters::readINI(path.toStdString(), parameters);
551  for(ParametersMap::iterator iter = parameters.begin(); iter!= parameters.end(); ++iter)
552  {
553  ui_->parameters_toolbox->updateParameter(iter->first, iter->second);
554  }
555 }
556 
557 void DatabaseViewer::writeSettings()
558 {
559  QString path = getIniFilePath();
560  QSettings settings(path, QSettings::IniFormat);
561  settings.beginGroup("DatabaseViewer");
562 
563  //save window state / geometry
564  if(!this->isMaximized())
565  {
566  settings.setValue("geometry", this->saveGeometry());
567  }
568  settings.setValue("state", this->saveState());
569  settings.setValue("maximized", this->isMaximized());
570  savedMaximized_ = this->isMaximized();
571 
572  settings.setValue("loggerLevel", ui_->comboBox_logger_level->currentIndex());
573  settings.setValue("verticalLayout", ui_->actionVertical_Layout->isChecked());
574  settings.setValue("ignoreIntermediateNodes", ui_->checkBox_ignoreIntermediateNodes->isChecked());
575  settings.setValue("timeStats", ui_->checkBox_timeStats->isChecked());
576 
577  // save GraphViewer settings
578  ui_->graphViewer->saveSettings(settings, "GraphView");
579 
580  // save optimization settings
581  settings.beginGroup("optimization");
582  settings.setValue("gainCompensationRadius", ui_->doubleSpinBox_gainCompensationRadius->value());
583  settings.setValue("voxelSize", ui_->doubleSpinBox_voxelSize->value());
584  settings.setValue("decimation", ui_->spinBox_decimation->value());
585  settings.endGroup();
586 
587  // save Grid settings
588  settings.beginGroup("grid");
589  settings.setValue("poseFiltering", ui_->groupBox_posefiltering->isChecked());
590  settings.setValue("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value());
591  settings.setValue("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value());
592  settings.setValue("colorObstacle", ui_->lineEdit_obstacleColor->text());
593  settings.setValue("colorGround", ui_->lineEdit_groundColor->text());
594  settings.setValue("colorEmpty", ui_->lineEdit_emptyColor->text());
595  settings.endGroup();
596 
597  settings.beginGroup("mesh");
598  settings.setValue("quad", ui_->checkBox_mesh_quad->isChecked());
599  settings.setValue("angleTolerance", ui_->spinBox_mesh_angleTolerance->value());
600  settings.setValue("minClusterSize", ui_->spinBox_mesh_minClusterSize->value());
601  settings.setValue("fillDepthHolesSize", ui_->spinBox_mesh_fillDepthHoles->value());
602  settings.setValue("fillDepthHolesError", ui_->spinBox_mesh_depthError->value());
603  settings.setValue("triangleSize", ui_->spinBox_mesh_triangleSize->value());
604  settings.endGroup();
605 
606  // ImageViews
607  //ui_->graphicsView_A->saveSettings(settings, "ImageViewA");
608  //ui_->graphicsView_B->saveSettings(settings, "ImageViewB");
609 
610  // save ICP parameters
611  settings.beginGroup("icp");
612  settings.setValue("decimation", ui_->spinBox_icp_decimation->value());
613  settings.setValue("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value());
614  settings.setValue("minDepth", ui_->doubleSpinBox_icp_minDepth->value());
615  settings.setValue("icpFromDepth", ui_->checkBox_icp_from_depth->isChecked());
616  settings.endGroup();
617 
618  settings.endGroup(); // DatabaseViewer
619 
620  // Use same parameters used by RTAB-Map
621  settings.beginGroup("Gui");
622  exportDialog_->saveSettings(settings, exportDialog_->objectName());
623  settings.beginGroup("PostProcessingDialog");
624  settings.setValue("cluster_radius", ui_->doubleSpinBox_detectMore_radius->value());
625  settings.setValue("cluster_angle", ui_->doubleSpinBox_detectMore_angle->value());
626  settings.setValue("iterations", ui_->spinBox_detectMore_iterations->value());
627  settings.endGroup();
628  settings.endGroup();
629 
630  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
631  for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end();)
632  {
633  if(!ui_->parameters_toolbox->getParameterWidget(iter->first.c_str()))
634  {
635  parameters.erase(iter++);
636  }
637  else
638  {
639  ++iter;
640  }
641  }
642  Parameters::writeINI(path.toStdString(), parameters);
643 
644  this->setWindowModified(false);
645 }
646 
647 void DatabaseViewer::restoreDefaultSettings()
648 {
649  // reset GUI parameters
650  ui_->comboBox_logger_level->setCurrentIndex(1);
651  ui_->checkBox_alignPosesWithGroundTruth->setChecked(true);
652  ui_->checkBox_alignScansCloudsWithGroundTruth->setChecked(false);
653  ui_->checkBox_ignoreIntermediateNodes->setChecked(false);
654  ui_->checkBox_timeStats->setChecked(true);
655 
656  ui_->checkBox_iterativeOptimization->setChecked(true);
657  ui_->checkBox_spanAllMaps->setChecked(true);
658  ui_->checkBox_wmState->setChecked(false);
659  ui_->checkBox_ignorePoseCorrection->setChecked(false);
660  ui_->checkBox_ignoreGlobalLoop->setChecked(false);
661  ui_->checkBox_ignoreLocalLoopSpace->setChecked(false);
662  ui_->checkBox_ignoreLocalLoopTime->setChecked(false);
663  ui_->checkBox_ignoreUserLoop->setChecked(false);
664  ui_->spinBox_optimizationDepth->setValue(0);
665  ui_->doubleSpinBox_optimizationScale->setValue(1.0);
666  ui_->doubleSpinBox_gainCompensationRadius->setValue(0.0);
667  ui_->doubleSpinBox_voxelSize->setValue(0.0);
668  ui_->spinBox_decimation->setValue(1);
669 
670  ui_->groupBox_posefiltering->setChecked(false);
671  ui_->doubleSpinBox_posefilteringRadius->setValue(0.1);
672  ui_->doubleSpinBox_posefilteringAngle->setValue(30);
673  ui_->checkBox_grid_empty->setChecked(true);
674  ui_->checkBox_octomap->setChecked(false);
675  ui_->lineEdit_obstacleColor->setText(QColor(Qt::red).name());
676  ui_->lineEdit_groundColor->setText(QColor(Qt::green).name());
677  ui_->lineEdit_emptyColor->setText(QColor(Qt::yellow).name());
678 
679  ui_->checkBox_mesh_quad->setChecked(true);
680  ui_->spinBox_mesh_angleTolerance->setValue(15);
681  ui_->spinBox_mesh_minClusterSize->setValue(0);
682  ui_->spinBox_mesh_fillDepthHoles->setValue(false);
683  ui_->spinBox_mesh_depthError->setValue(10);
684  ui_->spinBox_mesh_triangleSize->setValue(2);
685 
686  ui_->spinBox_icp_decimation->setValue(1);
687  ui_->doubleSpinBox_icp_maxDepth->setValue(0.0);
688  ui_->doubleSpinBox_icp_minDepth->setValue(0.0);
689  ui_->checkBox_icp_from_depth->setChecked(false);
690 
691  ui_->doubleSpinBox_detectMore_radius->setValue(1.0);
692  ui_->doubleSpinBox_detectMore_angle->setValue(30.0);
693  ui_->spinBox_detectMore_iterations->setValue(5);
694 }
695 
696 void DatabaseViewer::openDatabase()
697 {
698  QString path = QFileDialog::getOpenFileName(this, tr("Select file"), pathDatabase_, tr("Databases (*.db)"));
699  if(!path.isEmpty())
700  {
701  openDatabase(path);
702  }
703 }
704 
705 bool DatabaseViewer::openDatabase(const QString & path)
706 {
707  UDEBUG("Open database \"%s\"", path.toStdString().c_str());
708  if(QFile::exists(path))
709  {
710  if(QFileInfo(path).isFile())
711  {
712  std::string driverType = "sqlite3";
713 
714  dbDriver_ = DBDriver::create();
715 
716  if(!dbDriver_->openConnection(path.toStdString()))
717  {
718  ui_->actionClose_database->setEnabled(false);
719  ui_->actionOpen_database->setEnabled(true);
720  delete dbDriver_;
721  dbDriver_ = 0;
722  QMessageBox::warning(this, "Database error", tr("Can't open database \"%1\"").arg(path));
723  }
724  else
725  {
726  ui_->actionClose_database->setEnabled(true);
727  ui_->actionOpen_database->setEnabled(false);
728 
729  pathDatabase_ = UDirectory::getDir(path.toStdString()).c_str();
730  if(pathDatabase_.isEmpty() || pathDatabase_.compare(".") == 0)
731  {
732  pathDatabase_ = QDir::currentPath();
733  }
734  databaseFileName_ = UFile::getName(path.toStdString());
735  ui_->graphViewer->setWorkingDirectory(pathDatabase_);
736 
737  // look if there are saved parameters
738  ParametersMap parameters = dbDriver_->getLastParameters();
739 
740  if(parameters.size())
741  {
742  const ParametersMap & currentParameters = ui_->parameters_toolbox->getParameters();
743  ParametersMap differentParameters;
744  for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
745  {
746  ParametersMap::const_iterator jter = currentParameters.find(iter->first);
747  if(jter!=currentParameters.end() &&
748  ui_->parameters_toolbox->getParameterWidget(QString(iter->first.c_str())) != 0 &&
749  iter->second.compare(jter->second) != 0 &&
750  iter->first.compare(Parameters::kRtabmapWorkingDirectory()) != 0)
751  {
752  bool different = true;
753  if(Parameters::getType(iter->first).compare("double") ==0 ||
754  Parameters::getType(iter->first).compare("float") == 0)
755  {
756  if(uStr2Double(iter->second) == uStr2Double(jter->second))
757  {
758  different = false;
759  }
760  }
761  if(different)
762  {
763  differentParameters.insert(*iter);
764  QString msg = tr("Parameter \"%1\": database=\"%2\" Preferences=\"%3\"")
765  .arg(iter->first.c_str())
766  .arg(iter->second.c_str())
767  .arg(jter->second.c_str());
768  UWARN(msg.toStdString().c_str());
769  }
770  }
771  }
772 
773  if(differentParameters.size())
774  {
775  int r = QMessageBox::question(this,
776  tr("Update parameters..."),
777  tr("The database is using %1 different parameter(s) than "
778  "those currently set in Core parameters panel. Do you want "
779  "to use database's parameters?").arg(differentParameters.size()),
780  QMessageBox::Yes | QMessageBox::No,
781  QMessageBox::Yes);
782  if(r == QMessageBox::Yes)
783  {
784  QStringList str;
785  for(rtabmap::ParametersMap::const_iterator iter = differentParameters.begin(); iter!=differentParameters.end(); ++iter)
786  {
787  ui_->parameters_toolbox->updateParameter(iter->first, iter->second);
788  str.push_back(iter->first.c_str());
789  }
790  notifyParametersChanged(str);
791  }
792  }
793  }
794 
795  updateIds();
796  return true;
797  }
798  }
799  else // directory
800  {
801  pathDatabase_ = path;
802  if(pathDatabase_.isEmpty() || pathDatabase_.compare(".") == 0)
803  {
804  pathDatabase_ = QDir::currentPath();
805  }
806  ui_->graphViewer->setWorkingDirectory(pathDatabase_);
807  }
808  }
809  else
810  {
811  QMessageBox::warning(this, "Database error", tr("Database \"%1\" does not exist.").arg(path));
812  }
813  return false;
814 }
815 
816 bool DatabaseViewer::closeDatabase()
817 {
818  if(dbDriver_)
819  {
820  if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size())
821  {
822  QMessageBox::StandardButton button = QMessageBox::question(this,
823  tr("Links modified"),
824  tr("Some links are modified (%1 added, %2 refined, %3 removed), do you want to save them?")
825  .arg(linksAdded_.size()).arg(linksRefined_.size()).arg(linksRemoved_.size()),
826  QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
827  QMessageBox::Cancel);
828 
829  if(button == QMessageBox::Yes)
830  {
831  // Added links
832  for(std::multimap<int, rtabmap::Link>::iterator iter=linksAdded_.begin(); iter!=linksAdded_.end(); ++iter)
833  {
834  std::multimap<int, rtabmap::Link>::iterator refinedIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
835  if(refinedIter != linksRefined_.end())
836  {
837  dbDriver_->addLink(refinedIter->second);
838  dbDriver_->addLink(refinedIter->second.inverse());
839  }
840  else
841  {
842  dbDriver_->addLink(iter->second);
843  dbDriver_->addLink(iter->second.inverse());
844  }
845  }
846 
847  //Refined links
848  for(std::multimap<int, rtabmap::Link>::iterator iter=linksRefined_.begin(); iter!=linksRefined_.end(); ++iter)
849  {
850  if(!containsLink(linksAdded_, iter->second.from(), iter->second.to()))
851  {
852  dbDriver_->updateLink(iter->second);
853  dbDriver_->updateLink(iter->second.inverse());
854  }
855  }
856 
857  // Rejected links
858  for(std::multimap<int, rtabmap::Link>::iterator iter=linksRemoved_.begin(); iter!=linksRemoved_.end(); ++iter)
859  {
860  dbDriver_->removeLink(iter->second.to(), iter->second.from());
861  dbDriver_->removeLink(iter->second.from(), iter->second.to());
862  }
863  linksAdded_.clear();
864  linksRefined_.clear();
865  linksRemoved_.clear();
866  }
867 
868  if(button != QMessageBox::Yes && button != QMessageBox::No)
869  {
870  return false;
871  }
872  }
873 
874  if( generatedLocalMaps_.size() &&
875  uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.11.10") >= 0)
876  {
877  QMessageBox::StandardButton button = QMessageBox::question(this,
878  tr("Local occupancy grid maps modified"),
879  tr("%1 occupancy grid maps are modified, do you want to "
880  "save them? This will overwrite occupancy grids saved in the database.")
881  .arg(generatedLocalMaps_.size()),
882  QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
883  QMessageBox::Cancel);
884 
885  if(button == QMessageBox::Yes)
886  {
887  // Rejected links
888  UASSERT(generatedLocalMaps_.size() == generatedLocalMapsInfo_.size());
889  std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat > >::iterator mapIter = generatedLocalMaps_.begin();
890  std::map<int, std::pair<float, cv::Point3f> >::iterator infoIter = generatedLocalMapsInfo_.begin();
891  for(; mapIter!=generatedLocalMaps_.end(); ++mapIter, ++infoIter)
892  {
893  UASSERT(mapIter->first == infoIter->first);
894  dbDriver_->updateOccupancyGrid(
895  mapIter->first,
896  mapIter->second.first.first,
897  mapIter->second.first.second,
898  mapIter->second.second,
899  infoIter->second.first,
900  infoIter->second.second);
901  }
902  generatedLocalMaps_.clear();
903  generatedLocalMapsInfo_.clear();
904  localMaps_.clear();
905  localMapsInfo_.clear();
906  }
907 
908  if(button != QMessageBox::Yes && button != QMessageBox::No)
909  {
910  return false;
911  }
912  }
913 
914  delete dbDriver_;
915  dbDriver_ = 0;
916  ids_.clear();
917  idToIndex_.clear();
918  neighborLinks_.clear();
919  loopLinks_.clear();
920  graphes_.clear();
921  graphLinks_.clear();
922  odomPoses_.clear();
923  groundTruthPoses_.clear();
924  gpsPoses_.clear();
925  gpsValues_.clear();
926  mapIds_.clear();
927  weights_.clear();
928  wmStates_.clear();
929  links_.clear();
930  linksAdded_.clear();
931  linksRefined_.clear();
932  linksRemoved_.clear();
933  localMaps_.clear();
934  localMapsInfo_.clear();
935  generatedLocalMaps_.clear();
936  generatedLocalMapsInfo_.clear();
937  ui_->graphViewer->clearAll();
938  occupancyGridViewer_->clear();
939  ui_->menuEdit->setEnabled(false);
940  ui_->actionGenerate_3D_map_pcd->setEnabled(false);
941  ui_->actionExport->setEnabled(false);
942  ui_->actionExtract_images->setEnabled(false);
943  ui_->menuExport_poses->setEnabled(false);
944  ui_->menuExport_GPS->setEnabled(false);
945  ui_->actionPoses_KML->setEnabled(false);
946  ui_->actionExport_saved_2D_map->setEnabled(false);
947  ui_->actionImport_2D_map->setEnabled(false);
948  ui_->actionView_optimized_mesh->setEnabled(false);
949  ui_->actionExport_optimized_mesh->setEnabled(false);
950  ui_->actionUpdate_optimized_mesh->setEnabled(false);
951  ui_->checkBox_showOptimized->setEnabled(false);
952  ui_->toolBox_statistics->clear();
953  databaseFileName_.clear();
954  ui_->checkBox_alignPosesWithGroundTruth->setVisible(false);
955  ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(false);
956  ui_->doubleSpinBox_optimizationScale->setVisible(false);
957  ui_->label_scale_title->setVisible(false);
958  ui_->label_rmse->setVisible(false);
959  ui_->label_rmse_title->setVisible(false);
960  ui_->checkBox_ignoreIntermediateNodes->setVisible(false);
961  ui_->label_ignoreINtermediateNdoes->setVisible(false);
962  ui_->label_alignPosesWithGroundTruth->setVisible(false);
963  ui_->label_alignScansCloudsWithGroundTruth->setVisible(false);
964  ui_->label_optimizeFrom->setText(tr("Root"));
965  ui_->textEdit_info->clear();
966 
967  ui_->pushButton_refine->setEnabled(false);
968  ui_->pushButton_add->setEnabled(false);
969  ui_->pushButton_reset->setEnabled(false);
970  ui_->pushButton_reject->setEnabled(false);
971 
972  ui_->horizontalSlider_loops->setEnabled(false);
973  ui_->horizontalSlider_loops->setMaximum(0);
974  ui_->horizontalSlider_iterations->setEnabled(false);
975  ui_->horizontalSlider_iterations->setMaximum(0);
976  ui_->horizontalSlider_neighbors->setEnabled(false);
977  ui_->horizontalSlider_neighbors->setMaximum(0);
978  ui_->label_constraint->clear();
979  ui_->label_constraint_opt->clear();
980  ui_->label_variance->clear();
981  ui_->lineEdit_covariance->clear();
982 
983  ui_->horizontalSlider_A->setEnabled(false);
984  ui_->horizontalSlider_A->setMaximum(0);
985  ui_->horizontalSlider_B->setEnabled(false);
986  ui_->horizontalSlider_B->setMaximum(0);
987  ui_->label_idA->setText("NaN");
988  ui_->label_idB->setText("NaN");
989  sliderAValueChanged(0);
990  sliderBValueChanged(0);
991 
992  constraintsViewer_->clear();
993  constraintsViewer_->update();
994 
995  cloudViewer_->clear();
996  cloudViewer_->update();
997 
998  occupancyGridViewer_->clear();
999  occupancyGridViewer_->update();
1000 
1001  ui_->graphViewer->clearAll();
1002  ui_->label_loopClosures->clear();
1003  ui_->label_timeOptimization->clear();
1004  ui_->label_pathLength->clear();
1005  ui_->label_poses->clear();
1006  ui_->label_rmse->clear();
1007  ui_->spinBox_optimizationsFrom->setEnabled(false);
1008 
1009  ui_->graphicsView_A->clear();
1010  ui_->graphicsView_B->clear();
1011 
1012  ui_->graphicsView_stereo->clear();
1013  stereoViewer_->clear();
1014  stereoViewer_->update();
1015 
1016  ui_->toolBox_statistics->clear();
1017 
1018  useLastOptimizedGraphAsGuess_ = false;
1019  lastOptimizedGraph_.clear();
1020  }
1021 
1022  ui_->actionClose_database->setEnabled(dbDriver_ != 0);
1023  ui_->actionOpen_database->setEnabled(dbDriver_ == 0);
1024 
1025  return dbDriver_ == 0;
1026 }
1027 
1028 
1029 void DatabaseViewer::recoverDatabase()
1030 {
1031  QString path = QFileDialog::getOpenFileName(this, tr("Select file"), pathDatabase_, tr("Databases (*.db)"));
1032  if(!path.isEmpty())
1033  {
1034  if(path.compare(pathDatabase_+QDir::separator()+databaseFileName_.c_str()) == 0)
1035  {
1036  QMessageBox::information(this, "Database recovery", tr("The selected database is already opened, close it first."));
1037  return;
1038  }
1039  std::string errorMsg;
1040  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
1041  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
1042  progressDialog->setMaximumSteps(100);
1043  progressDialog->show();
1044  progressDialog->setCancelButtonVisible(true);
1045  RecoveryState state(progressDialog);
1046  if(databaseRecovery(path.toStdString(), false, &errorMsg, &state))
1047  {
1048  QMessageBox::information(this, "Database recovery", tr("Database \"%1\" recovered! Try opening it again.").arg(path));
1049  }
1050  else
1051  {
1052  QMessageBox::warning(this, "Database recovery", tr("Database recovery failed: \"%1\".").arg(errorMsg.c_str()));
1053  }
1054  progressDialog->setValue(progressDialog->maximumSteps());
1055  }
1056 }
1057 
1058 void DatabaseViewer::closeEvent(QCloseEvent* event)
1059 {
1060  //write settings before quit?
1061  bool save = false;
1062  if(this->isWindowModified())
1063  {
1064  QMessageBox::Button b=QMessageBox::question(this,
1065  tr("Database Viewer"),
1066  tr("There are unsaved changed settings. Save them?"),
1067  QMessageBox::Save | QMessageBox::Cancel | QMessageBox::Discard);
1068  if(b == QMessageBox::Save)
1069  {
1070  save = true;
1071  }
1072  else if(b != QMessageBox::Discard)
1073  {
1074  event->ignore();
1075  return;
1076  }
1077  }
1078 
1079  if(save)
1080  {
1081  writeSettings();
1082  }
1083 
1084  event->accept();
1085 
1086  if(!closeDatabase())
1087  {
1088  event->ignore();
1089  }
1090 
1091  if(event->isAccepted())
1092  {
1093  ui_->toolBox_statistics->closeFigures();
1094  if(dbDriver_)
1095  {
1096  delete dbDriver_;
1097  dbDriver_ = 0;
1098  }
1099  }
1100 }
1101 
1102 void DatabaseViewer::showEvent(QShowEvent* anEvent)
1103 {
1104  this->setWindowModified(false);
1105 
1106  if((ui_->graphViewer->isVisible() || ui_->dockWidget_occupancyGridView->isVisible()) && graphes_.size() && localMaps_.size()==0)
1107  {
1108  sliderIterationsValueChanged((int)graphes_.size()-1);
1109  }
1110 }
1111 
1112 void DatabaseViewer::moveEvent(QMoveEvent* anEvent)
1113 {
1114  if(this->isVisible())
1115  {
1116  // HACK, there is a move event when the window is shown the first time.
1117  if(!firstCall_)
1118  {
1119  this->configModified();
1120  }
1121  firstCall_ = false;
1122  }
1123 }
1124 
1125 void DatabaseViewer::resizeEvent(QResizeEvent* anEvent)
1126 {
1127  if(this->isVisible())
1128  {
1129  this->configModified();
1130  }
1131 }
1132 
1133 void DatabaseViewer::keyPressEvent(QKeyEvent *event)
1134 {
1135  //catch ctrl-s to save settings
1136  if((event->modifiers() & Qt::ControlModifier) && event->key() == Qt::Key_S)
1137  {
1138  this->writeSettings();
1139  }
1140 }
1141 
1142 bool DatabaseViewer::eventFilter(QObject *obj, QEvent *event)
1143 {
1144  if (event->type() == QEvent::Resize && qobject_cast<QDockWidget*>(obj))
1145  {
1146  this->setWindowModified(true);
1147  }
1148  return QWidget::eventFilter(obj, event);
1149 }
1150 
1151 
1152 void DatabaseViewer::exportDatabase()
1153 {
1154  if(!dbDriver_ || ids_.size() == 0)
1155  {
1156  return;
1157  }
1158 
1159  rtabmap::ExportDialog dialog;
1160 
1161  if(dialog.exec())
1162  {
1163  if(!dialog.outputPath().isEmpty())
1164  {
1165  int framesIgnored = dialog.framesIgnored();
1166  double frameRate = dialog.targetFramerate();
1167  int sessionExported = dialog.sessionExported();
1168  QString path = dialog.outputPath();
1169  rtabmap::DataRecorder recorder;
1170  QList<int> ids;
1171 
1172  double previousStamp = 0;
1173  std::vector<double> delays(ids_.size());
1174  int oi=0;
1175  std::map<int, Transform> poses;
1176  std::map<int, double> stamps;
1177  std::map<int, Transform> groundTruths;
1178  std::map<int, GPS> gpsValues;
1179  for(int i=0; i<ids_.size(); i+=1+framesIgnored)
1180  {
1181  Transform odomPose, groundTruth;
1182  int weight = -1;
1183  int mapId = -1;
1184  std::string label;
1185  double stamp = 0;
1186  std::vector<float> velocity;
1187  GPS gps;
1188  if(dbDriver_->getNodeInfo(ids_[i], odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps))
1189  {
1190  if(frameRate == 0 ||
1191  previousStamp == 0 ||
1192  stamp == 0 ||
1193  stamp - previousStamp >= 1.0/frameRate)
1194  {
1195  if(sessionExported < 0 || sessionExported == mapId)
1196  {
1197  ids.push_back(ids_[i]);
1198 
1199  if(previousStamp && stamp)
1200  {
1201  delays[oi++] = stamp - previousStamp;
1202  }
1203  previousStamp = stamp;
1204 
1205  poses.insert(std::make_pair(ids_[i], odomPose));
1206  stamps.insert(std::make_pair(ids_[i], stamp));
1207  groundTruths.insert(std::make_pair(ids_[i], groundTruth));
1208  if(gps.stamp() > 0.0)
1209  {
1210  gpsValues.insert(std::make_pair(ids_[i], gps));
1211  }
1212  }
1213  }
1214  if(sessionExported >= 0 && mapId > sessionExported)
1215  {
1216  break;
1217  }
1218  }
1219  }
1220  delays.resize(oi);
1221 
1222  if(recorder.init(path, false))
1223  {
1224  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
1225  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
1226  progressDialog->setMaximumSteps(ids.size());
1227  progressDialog->show();
1228  progressDialog->setCancelButtonVisible(true);
1229  UINFO("Decompress: rgb=%d depth=%d scan=%d userData=%d",
1230  dialog.isRgbExported()?1:0,
1231  dialog.isDepthExported()?1:0,
1232  dialog.isDepth2dExported()?1:0,
1233  dialog.isUserDataExported()?1:0);
1234 
1235  for(int i=0; i<ids.size() && !progressDialog->isCanceled(); ++i)
1236  {
1237  int id = ids.at(i);
1238 
1239  SensorData data;
1240  dbDriver_->getNodeData(id, data);
1241  cv::Mat depth, rgb, userData;
1242  LaserScan scan;
1243  data.uncompressDataConst(
1244  !dialog.isRgbExported()?0:&rgb,
1245  !dialog.isDepthExported()?0:&depth,
1246  !dialog.isDepth2dExported()?0:&scan,
1247  !dialog.isUserDataExported()?0:&userData);
1248  cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
1249  if(dialog.isOdomExported())
1250  {
1251  std::map<int, Link> links;
1252  dbDriver_->loadLinks(id, links, Link::kNeighbor);
1253  if(links.size() && links.begin()->first < id)
1254  {
1255  covariance = links.begin()->second.infMatrix().inv();
1256  }
1257  }
1258 
1259  rtabmap::SensorData sensorData;
1260  if(data.cameraModels().size())
1261  {
1262  sensorData = rtabmap::SensorData(
1263  scan,
1264  rgb,
1265  depth,
1266  data.cameraModels(),
1267  id,
1268  stamps.at(id),
1269  userData);
1270  }
1271  else
1272  {
1273  sensorData = rtabmap::SensorData(
1274  scan,
1275  rgb,
1276  depth,
1277  data.stereoCameraModel(),
1278  id,
1279  stamps.at(id),
1280  userData);
1281  }
1282  if(groundTruths.find(id)!=groundTruths.end())
1283  {
1284  sensorData.setGroundTruth(groundTruths.at(id));
1285  }
1286  if(gpsValues.find(id)!=gpsValues.end())
1287  {
1288  sensorData.setGPS(gpsValues.at(id));
1289  }
1290 
1291  recorder.addData(sensorData, dialog.isOdomExported()?poses.at(id):Transform(), covariance);
1292 
1293  progressDialog->appendText(tr("Exported node %1").arg(id));
1294  progressDialog->incrementStep();
1295  QApplication::processEvents();
1296  }
1297  progressDialog->setValue(progressDialog->maximumSteps());
1298  if(delays.size())
1299  {
1300  progressDialog->appendText(tr("Average frame rate=%1 Hz (Min=%2, Max=%3)")
1301  .arg(1.0/uMean(delays)).arg(1.0/uMax(delays)).arg(1.0/uMin(delays)));
1302  }
1303  progressDialog->appendText(tr("Export finished to \"%1\"!").arg(path));
1304  }
1305  else
1306  {
1307  UERROR("DataRecorder init failed?!");
1308  }
1309  }
1310  else
1311  {
1312  QMessageBox::warning(this, tr("Cannot export database"), tr("An output path must be set!"));
1313  }
1314  }
1315 }
1316 
1317 void DatabaseViewer::extractImages()
1318 {
1319  if(!dbDriver_ || ids_.size() == 0)
1320  {
1321  return;
1322  }
1323 
1324  QStringList formats;
1325  formats.push_back("jpg");
1326  formats.push_back("png");
1327  bool ok;
1328  QString ext = QInputDialog::getItem(this, tr("Which RGB format?"), tr("Format:"), formats, 0, false, &ok);
1329  if(!ok)
1330  {
1331  return;
1332  }
1333 
1334  QString path = QFileDialog::getExistingDirectory(this, tr("Select directory where to save images..."), QDir::homePath());
1335  if(!path.isEmpty())
1336  {
1337  if(ids_.size())
1338  {
1339  int id = ids_.at(0);
1340  SensorData data;
1341  dbDriver_->getNodeData(id, data);
1342  data.uncompressData();
1343  if(!data.imageRaw().empty() && !data.rightRaw().empty())
1344  {
1345  QDir dir;
1346  dir.mkdir(QString("%1/left").arg(path));
1347  dir.mkdir(QString("%1/right").arg(path));
1348  if(databaseFileName_.empty())
1349  {
1350  UERROR("Cannot save calibration file, database name is empty!");
1351  }
1352  else if(data.stereoCameraModel().isValidForProjection())
1353  {
1354  std::string cameraName = uSplit(databaseFileName_, '.').front();
1355  StereoCameraModel model(
1356  cameraName,
1357  data.imageRaw().size(),
1358  data.stereoCameraModel().left().K(),
1359  data.stereoCameraModel().left().D(),
1360  data.stereoCameraModel().left().R(),
1361  data.stereoCameraModel().left().P(),
1362  data.rightRaw().size(),
1363  data.stereoCameraModel().right().K(),
1364  data.stereoCameraModel().right().D(),
1365  data.stereoCameraModel().right().R(),
1366  data.stereoCameraModel().right().P(),
1367  data.stereoCameraModel().R(),
1368  data.stereoCameraModel().T(),
1369  data.stereoCameraModel().E(),
1370  data.stereoCameraModel().F(),
1371  data.stereoCameraModel().left().localTransform());
1372  if(model.save(path.toStdString()))
1373  {
1374  UINFO("Saved stereo calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
1375  }
1376  else
1377  {
1378  UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
1379  }
1380  }
1381  }
1382  else if(!data.imageRaw().empty())
1383  {
1384  if(!data.depthRaw().empty())
1385  {
1386  QDir dir;
1387  dir.mkdir(QString("%1/rgb").arg(path));
1388  dir.mkdir(QString("%1/depth").arg(path));
1389  }
1390 
1391  if(databaseFileName_.empty())
1392  {
1393  UERROR("Cannot save calibration file, database name is empty!");
1394  }
1395  else if(data.cameraModels().size() > 1)
1396  {
1397  UERROR("Only one camera calibration can be saved at this time (%d detected)", (int)data.cameraModels().size());
1398  }
1399  else if(data.cameraModels().size() == 1 && data.cameraModels().front().isValidForProjection())
1400  {
1401  std::string cameraName = uSplit(databaseFileName_, '.').front();
1402  CameraModel model(cameraName,
1403  data.imageRaw().size(),
1404  data.cameraModels().front().K(),
1405  data.cameraModels().front().D(),
1406  data.cameraModels().front().R(),
1407  data.cameraModels().front().P(),
1408  data.cameraModels().front().localTransform());
1409  if(model.save(path.toStdString()))
1410  {
1411  UINFO("Saved calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
1412  }
1413  else
1414  {
1415  UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName).c_str());
1416  }
1417  }
1418  }
1419  }
1420 
1421  int imagesExported = 0;
1422  for(int i=0; i<ids_.size(); ++i)
1423  {
1424  int id = ids_.at(i);
1425  SensorData data;
1426  dbDriver_->getNodeData(id, data);
1427  data.uncompressData();
1428  if(!data.imageRaw().empty() && !data.rightRaw().empty())
1429  {
1430  cv::imwrite(QString("%1/left/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
1431  cv::imwrite(QString("%1/right/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.rightRaw());
1432  UINFO(QString("Saved left/%1.%2 and right/%1.%2").arg(id).arg(ext).toStdString().c_str());
1433  ++imagesExported;
1434  }
1435  else if(!data.imageRaw().empty() && !data.depthRaw().empty())
1436  {
1437  cv::imwrite(QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
1438  cv::imwrite(QString("%1/depth/%2.png").arg(path).arg(id).toStdString(), data.depthRaw().type()==CV_32FC1?util2d::cvtDepthFromFloat(data.depthRaw()):data.depthRaw());
1439  UINFO(QString("Saved rgb/%1.%2 and depth/%1.png").arg(id).arg(ext).toStdString().c_str());
1440  ++imagesExported;
1441  }
1442  else if(!data.imageRaw().empty())
1443  {
1444  cv::imwrite(QString("%1/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw());
1445  UINFO(QString("Saved %1.%2").arg(id).arg(ext).toStdString().c_str());
1446  ++imagesExported;
1447  }
1448  }
1449  QMessageBox::information(this, tr("Exporting"), tr("%1 images exported!").arg(imagesExported));
1450  }
1451 }
1452 
1453 void DatabaseViewer::updateIds()
1454 {
1455  if(!dbDriver_)
1456  {
1457  return;
1458  }
1459 
1460  UINFO("Loading all IDs...");
1461  std::set<int> ids;
1462  dbDriver_->getAllNodeIds(ids);
1463  ids_ = QList<int>::fromStdList(std::list<int>(ids.begin(), ids.end()));
1464  idToIndex_.clear();
1465  mapIds_.clear();
1466  weights_.clear();
1467  wmStates_.clear();
1468  odomPoses_.clear();
1469  groundTruthPoses_.clear();
1470  gpsPoses_.clear();
1471  gpsValues_.clear();
1472  ui_->checkBox_wmState->setVisible(false);
1473  ui_->checkBox_alignPosesWithGroundTruth->setVisible(false);
1474  ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(false);
1475  ui_->doubleSpinBox_optimizationScale->setVisible(false);
1476  ui_->label_scale_title->setVisible(false);
1477  ui_->label_rmse->setVisible(false);
1478  ui_->label_rmse_title->setVisible(false);
1479  ui_->checkBox_ignoreIntermediateNodes->setVisible(false);
1480  ui_->label_ignoreINtermediateNdoes->setVisible(false);
1481  ui_->label_alignPosesWithGroundTruth->setVisible(false);
1482  ui_->label_alignScansCloudsWithGroundTruth->setVisible(false);
1483  ui_->menuEdit->setEnabled(true);
1484  ui_->actionGenerate_3D_map_pcd->setEnabled(true);
1485  ui_->actionExport->setEnabled(true);
1486  ui_->actionExtract_images->setEnabled(true);
1487  ui_->menuExport_poses->setEnabled(false);
1488  ui_->menuExport_GPS->setEnabled(false);
1489  ui_->actionPoses_KML->setEnabled(false);
1490  ui_->actionExport_saved_2D_map->setEnabled(false);
1491  ui_->actionImport_2D_map->setEnabled(false);
1492  ui_->actionView_optimized_mesh->setEnabled(false);
1493  ui_->actionExport_optimized_mesh->setEnabled(false);
1494  ui_->actionUpdate_optimized_mesh->setEnabled(uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.13.0") >= 0);
1495  links_.clear();
1496  linksAdded_.clear();
1497  linksRefined_.clear();
1498  linksRemoved_.clear();
1499  ui_->toolBox_statistics->clear();
1500  ui_->label_optimizeFrom->setText(tr("Root"));
1501  std::multimap<int, Link> links;
1502  dbDriver_->getAllLinks(links, true);
1503  UDEBUG("%d total links loaded", (int)links.size());
1504 
1505  double totalOdom = 0.0;
1506  Transform previousPose;
1507  int sessions = ids_.size()?1:0;
1508  double totalTime = 0.0;
1509  double previousStamp = 0.0;
1510  std::set<int> idsWithoutBad;
1511  dbDriver_->getAllNodeIds(idsWithoutBad, false, true);
1512  int badcountInLTM = 0;
1513  int badCountInGraph = 0;
1514  bool hasReducedGraph = false;
1515  std::map<int, std::vector<int> > wmStates = dbDriver_->getAllStatisticsWmStates();
1516  for(int i=0; i<ids_.size(); ++i)
1517  {
1518  idToIndex_.insert(ids_[i], i);
1519 
1520  Transform p, g;
1521  int w;
1522  std::string l;
1523  double s;
1524  int mapId;
1525  std::vector<float> v;
1526  GPS gps;
1527  dbDriver_->getNodeInfo(ids_[i], p, mapId, w, l, s, g, v, gps);
1528  mapIds_.insert(std::make_pair(ids_[i], mapId));
1529  weights_.insert(std::make_pair(ids_[i], w));
1530  if(wmStates.find(ids_[i]) != wmStates.end())
1531  {
1532  wmStates_.insert(std::make_pair(ids_[i], wmStates.at(ids_[i])));
1533  ui_->checkBox_wmState->setVisible(true);
1534  }
1535  if(w < 0)
1536  {
1537  ui_->checkBox_ignoreIntermediateNodes->setVisible(true);
1538  ui_->label_ignoreINtermediateNdoes->setVisible(true);
1539  }
1540  if(i>0)
1541  {
1542  if(mapIds_.at(ids_[i-1]) == mapId)
1543  {
1544  if(!p.isNull() && !previousPose.isNull())
1545  {
1546  totalOdom += p.getDistance(previousPose);
1547  }
1548 
1549  if(previousStamp > 0.0 && s > 0.0)
1550  {
1551  totalTime += s-previousStamp;
1552  }
1553  }
1554  else
1555  {
1556  ++sessions;
1557  }
1558  }
1559  previousStamp=s;
1560  previousPose=p;
1561 
1562  //links
1563  bool addPose = links.find(ids_[i]) == links.end();
1564  for(std::multimap<int, Link>::iterator jter=links.find(ids_[i]); jter!=links.end() && jter->first == ids_[i]; ++jter)
1565  {
1566  if(jter->second.type() == Link::kNeighborMerged)
1567  {
1568  hasReducedGraph = true;
1569  }
1570 
1571  std::multimap<int, Link>::iterator invertedLinkIter = graph::findLink(links, jter->second.to(), jter->second.from(), false);
1572  if( jter->second.isValid() && // null transform means a rehearsed location
1573  ids.find(jter->second.from()) != ids.end() &&
1574  ids.find(jter->second.to()) != ids.end() &&
1575  graph::findLink(links_, jter->second.from(), jter->second.to()) == links_.end() &&
1576  graph::findLink(links, jter->second.from(), jter->second.to(), false) != links.end() &&
1577  invertedLinkIter != links.end())
1578  {
1579  // check if user_data is set in opposite direction
1580  if(jter->second.userDataCompressed().cols == 0 &&
1581  invertedLinkIter->second.userDataCompressed().cols != 0)
1582  {
1583  links_.insert(std::make_pair(invertedLinkIter->second.from(), invertedLinkIter->second));
1584  }
1585  else
1586  {
1587  links_.insert(std::make_pair(ids_[i], jter->second));
1588  }
1589  addPose = true;
1590  }
1591  else if(graph::findLink(links_, jter->second.from(), jter->second.to()) != links_.end())
1592  {
1593  addPose = true;
1594  }
1595  }
1596  if(addPose)
1597  {
1598  odomPoses_.insert(std::make_pair(ids_[i], p));
1599  if(!g.isNull())
1600  {
1601  groundTruthPoses_.insert(std::make_pair(ids_[i], g));
1602  }
1603  if(gps.stamp() > 0.0)
1604  {
1605  gpsValues_.insert(std::make_pair(ids_[i], gps));
1606 
1607  cv::Point3f p(0.0f,0.0f,0.0f);
1608  if(!gpsPoses_.empty())
1609  {
1610  GeodeticCoords coords = gps.toGeodeticCoords();
1611  GPS originGPS = gpsValues_.begin()->second;
1612  p = coords.toENU_WGS84(originGPS.toGeodeticCoords());
1613  }
1614  Transform pose(p.x, p.y, p.z, 0.0f, 0.0f, (float)((-(gps.bearing()-90))*180.0/M_PI));
1615  gpsPoses_.insert(std::make_pair(ids_[i], pose));
1616  }
1617  }
1618 
1619  if(idsWithoutBad.find(ids_[i]) == idsWithoutBad.end())
1620  {
1621  ++badcountInLTM;
1622  if(addPose)
1623  {
1624  ++badCountInGraph;
1625  }
1626  }
1627  }
1628 
1629  if(!groundTruthPoses_.empty() || !gpsPoses_.empty())
1630  {
1631  ui_->checkBox_alignPosesWithGroundTruth->setVisible(true);
1632  ui_->doubleSpinBox_optimizationScale->setVisible(true);
1633  ui_->label_scale_title->setVisible(true);
1634  ui_->label_rmse->setVisible(true);
1635  ui_->label_rmse_title->setVisible(true);
1636  ui_->label_alignPosesWithGroundTruth->setVisible(true);
1637 
1638  if(!groundTruthPoses_.empty())
1639  {
1640  ui_->label_alignPosesWithGroundTruth->setText(tr("Align poses with ground truth"));
1641  ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(true);
1642  ui_->label_alignScansCloudsWithGroundTruth->setVisible(true);
1643  }
1644  else
1645  {
1646  ui_->label_alignPosesWithGroundTruth->setText(tr("Align poses with GPS"));
1647  }
1648  }
1649  if(!gpsValues_.empty())
1650  {
1651  ui_->menuExport_GPS->setEnabled(true);
1652  ui_->actionPoses_KML->setEnabled(groundTruthPoses_.empty());
1653  }
1654 
1655  float xMin, yMin, cellSize;
1656  bool hasMap = !dbDriver_->load2DMap(xMin, yMin, cellSize).empty();
1657  ui_->actionExport_saved_2D_map->setEnabled(hasMap);
1658  ui_->actionImport_2D_map->setEnabled(hasMap);
1659 
1660  if(!dbDriver_->loadOptimizedMesh().empty())
1661  {
1662  ui_->actionView_optimized_mesh->setEnabled(true);
1663  ui_->actionExport_optimized_mesh->setEnabled(true);
1664  }
1665 
1666  UINFO("Loaded %d ids, %d poses and %d links", (int)ids_.size(), (int)odomPoses_.size(), (int)links_.size());
1667 
1668  if(ids_.size() && ui_->toolBox_statistics->isVisible())
1669  {
1670  UINFO("Update statistics...");
1671  updateStatistics();
1672  }
1673 
1674  UINFO("Update database info...");
1675  ui_->textEdit_info->clear();
1676  ui_->textEdit_info->append(tr("Path:\t\t%1").arg(dbDriver_->getUrl().c_str()));
1677  ui_->textEdit_info->append(tr("Version:\t\t%1").arg(dbDriver_->getDatabaseVersion().c_str()));
1678  ui_->textEdit_info->append(tr("Sessions:\t\t%1").arg(sessions));
1679  if(hasReducedGraph)
1680  {
1681  ui_->textEdit_info->append(tr("Total odometry length:\t%1 m (approx. as graph has been reduced)").arg(totalOdom));
1682  }
1683  else
1684  {
1685  ui_->textEdit_info->append(tr("Total odometry length:\t%1 m").arg(totalOdom));
1686  }
1687  ui_->textEdit_info->append(tr("Total time:\t\t%1").arg(QDateTime::fromMSecsSinceEpoch(totalTime*1000).toUTC().toString("hh:mm:ss.zzz")));
1688  ui_->textEdit_info->append(tr("LTM:\t\t%1 nodes and %2 words").arg(ids.size()).arg(dbDriver_->getTotalDictionarySize()));
1689  ui_->textEdit_info->append(tr("WM:\t\t%1 nodes and %2 words").arg(dbDriver_->getLastNodesSize()).arg(dbDriver_->getLastDictionarySize()));
1690  ui_->textEdit_info->append(tr("Global graph:\t%1 poses and %2 links").arg(odomPoses_.size()).arg(links_.size()));
1691  ui_->textEdit_info->append(tr("Ground truth:\t%1 poses").arg(groundTruthPoses_.size()));
1692  ui_->textEdit_info->append(tr("GPS:\t%1 poses").arg(gpsValues_.size()));
1693  ui_->textEdit_info->append("");
1694  long total = 0;
1695  long dbSize = UFile::length(dbDriver_->getUrl());
1696  long mem = dbSize;
1697  ui_->textEdit_info->append(tr("Database size:\t%1 %2").arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).arg(mem>1000000?"MB":mem>1000?"KB":"Bytes"));
1698  mem = dbDriver_->getNodesMemoryUsed();
1699  total+=mem;
1700  ui_->textEdit_info->append(tr("Nodes size:\t\t%1 %2\t%3%").arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).arg(mem>1000000?"MB":mem>1000?"KB":"Bytes").arg(dbSize>0?QString::number(double(mem)/double(dbSize)*100.0, 'f', 2 ):"0"));
1701  mem = dbDriver_->getLinksMemoryUsed();
1702  total+=mem;
1703  ui_->textEdit_info->append(tr("Links size:\t\t%1 %2\t%3%").arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).arg(mem>1000000?"MB":mem>1000?"KB":"Bytes").arg(dbSize>0?QString::number(double(mem)/double(dbSize)*100.0, 'f', 2 ):"0"));
1704  mem = dbDriver_->getImagesMemoryUsed();
1705  total+=mem;
1706  ui_->textEdit_info->append(tr("RGB Images size:\t%1 %2\t%3%").arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).arg(mem>1000000?"MB":mem>1000?"KB":"Bytes").arg(dbSize>0?QString::number(double(mem)/double(dbSize)*100.0, 'f', 2 ):"0"));
1707  mem = dbDriver_->getDepthImagesMemoryUsed();
1708  total+=mem;
1709  ui_->textEdit_info->append(tr("Depth Images size:\t%1 %2\t%3%").arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).arg(mem>1000000?"MB":mem>1000?"KB":"Bytes").arg(dbSize>0?QString::number(double(mem)/double(dbSize)*100.0, 'f', 2 ):"0"));
1710  mem = dbDriver_->getCalibrationsMemoryUsed();
1711  total+=mem;
1712  ui_->textEdit_info->append(tr("Calibrations size:\t%1 %2\t%3%").arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).arg(mem>1000000?"MB":mem>1000?"KB":"Bytes").arg(dbSize>0?QString::number(double(mem)/double(dbSize)*100.0, 'f', 2 ):"0"));
1713  mem = dbDriver_->getGridsMemoryUsed();
1714  total+=mem;
1715  ui_->textEdit_info->append(tr("Grids size:\t\t%1 %2\t%3%").arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).arg(mem>1000000?"MB":mem>1000?"KB":"Bytes").arg(dbSize>0?QString::number(double(mem)/double(dbSize)*100.0, 'f', 2 ):"0"));
1716  mem = dbDriver_->getLaserScansMemoryUsed();
1717  total+=mem;
1718  ui_->textEdit_info->append(tr("Scans size:\t\t%1 %2\t%3%").arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).arg(mem>1000000?"MB":mem>1000?"KB":"Bytes").arg(dbSize>0?QString::number(double(mem)/double(dbSize)*100.0, 'f', 2 ):"0"));
1719  mem = dbDriver_->getUserDataMemoryUsed();
1720  total+=mem;
1721  ui_->textEdit_info->append(tr("User data size:\t%1 %2\t%3%").arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).arg(mem>1000000?"MB":mem>1000?"KB":"Bytes").arg(dbSize>0?QString::number(double(mem)/double(dbSize)*100.0, 'f', 2 ):"0"));
1722  mem = dbDriver_->getWordsMemoryUsed();
1723  total+=mem;
1724  ui_->textEdit_info->append(tr("Dictionary size:\t%1 %2\t%3%").arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).arg(mem>1000000?"MB":mem>1000?"KB":"Bytes").arg(dbSize>0?QString::number(double(mem)/double(dbSize)*100.0, 'f', 2 ):"0"));
1725  mem = dbDriver_->getFeaturesMemoryUsed();
1726  total+=mem;
1727  ui_->textEdit_info->append(tr("Features size:\t%1 %2\t%3%").arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).arg(mem>1000000?"MB":mem>1000?"KB":"Bytes").arg(dbSize>0?QString::number(double(mem)/double(dbSize)*100.0, 'f', 2 ):"0"));
1728  mem = dbDriver_->getStatisticsMemoryUsed();
1729  total+=mem;
1730  ui_->textEdit_info->append(tr("Statistics size:\t%1 %2\t%3%").arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).arg(mem>1000000?"MB":mem>1000?"KB":"Bytes").arg(dbSize>0?QString::number(double(mem)/double(dbSize)*100.0, 'f', 2 ):"0"));
1731  mem = dbSize - total;
1732  ui_->textEdit_info->append(tr("Other (indexing):\t%1 %2\t%3%").arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).arg(mem>1000000?"MB":mem>1000?"KB":"Bytes").arg(dbSize>0?QString::number(double(mem)/double(dbSize)*100.0, 'f', 2 ):"0"));
1733  ui_->textEdit_info->append("");
1734  ui_->textEdit_info->append(tr("%1 bad signatures in LTM").arg(badcountInLTM));
1735  ui_->textEdit_info->append(tr("%1 bad signatures in the global graph").arg(badCountInGraph));
1736  ui_->textEdit_info->append("");
1737  ParametersMap parameters = dbDriver_->getLastParameters();
1738  QFontMetrics metrics(ui_->textEdit_info->font());
1739  int tabW = ui_->textEdit_info->tabStopWidth();
1740  for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
1741  {
1742  int strW = metrics.width(QString(iter->first.c_str()) + "=");
1743  ui_->textEdit_info->append(tr("%1=%2%3")
1744  .arg(iter->first.c_str())
1745  .arg(strW < tabW?"\t\t\t\t":strW < tabW*2?"\t\t\t":strW < tabW*3?"\t\t":"\t")
1746  .arg(iter->second.c_str()));
1747  }
1748 
1749  // move back the cursor at the beginning
1750  ui_->textEdit_info->moveCursor(QTextCursor::Start) ;
1751  ui_->textEdit_info->ensureCursorVisible() ;
1752 
1753  if(ids.size())
1754  {
1755  if(odomPoses_.size())
1756  {
1757  bool nullPoses = odomPoses_.begin()->second.isNull();
1758  for(std::map<int,Transform>::iterator iter=odomPoses_.begin(); iter!=odomPoses_.end(); ++iter)
1759  {
1760  if((!iter->second.isNull() && nullPoses) ||
1761  (iter->second.isNull() && !nullPoses))
1762  {
1763  if(iter->second.isNull())
1764  {
1765  UWARN("Pose %d is null!", iter->first);
1766  }
1767  UWARN("Mixed valid and null poses! Ignoring graph...");
1768  odomPoses_.clear();
1769  links_.clear();
1770  break;
1771  }
1772  }
1773  if(nullPoses)
1774  {
1775  odomPoses_.clear();
1776  links_.clear();
1777  }
1778 
1779  if(odomPoses_.size())
1780  {
1781  ui_->spinBox_optimizationsFrom->setRange(odomPoses_.begin()->first, odomPoses_.rbegin()->first);
1782  ui_->spinBox_optimizationsFrom->setValue(odomPoses_.begin()->first);
1783  ui_->label_optimizeFrom->setText(tr("Root [%1, %2]").arg(odomPoses_.begin()->first).arg(odomPoses_.rbegin()->first));
1784  }
1785  }
1786  }
1787 
1788  ui_->menuExport_poses->setEnabled(false);
1789  graphes_.clear();
1790  graphLinks_.clear();
1791  neighborLinks_.clear();
1792  loopLinks_.clear();
1793  for(std::multimap<int, rtabmap::Link>::iterator iter = links_.begin(); iter!=links_.end(); ++iter)
1794  {
1795  if(!iter->second.transform().isNull())
1796  {
1797  if(iter->second.type() == rtabmap::Link::kNeighbor ||
1798  iter->second.type() == rtabmap::Link::kNeighborMerged)
1799  {
1800  neighborLinks_.append(iter->second);
1801  }
1802  else if(iter->second.type()!=rtabmap::Link::kPosePrior)
1803  {
1804  loopLinks_.append(iter->second);
1805  }
1806  }
1807  else
1808  {
1809  UERROR("Transform null for link from %d to %d", iter->first, iter->second.to());
1810  }
1811  }
1812 
1813  if(ids_.size())
1814  {
1815  ui_->horizontalSlider_A->setMinimum(0);
1816  ui_->horizontalSlider_B->setMinimum(0);
1817  ui_->horizontalSlider_A->setMaximum(ids_.size()-1);
1818  ui_->horizontalSlider_B->setMaximum(ids_.size()-1);
1819  ui_->horizontalSlider_A->setEnabled(true);
1820  ui_->horizontalSlider_B->setEnabled(true);
1821  ui_->horizontalSlider_A->setSliderPosition(0);
1822  ui_->horizontalSlider_B->setSliderPosition(0);
1823  sliderAValueChanged(0);
1824  sliderBValueChanged(0);
1825  }
1826  else
1827  {
1828  ui_->horizontalSlider_A->setEnabled(false);
1829  ui_->horizontalSlider_B->setEnabled(false);
1830  ui_->label_idA->setText("NaN");
1831  ui_->label_idB->setText("NaN");
1832  }
1833 
1834  if(neighborLinks_.size())
1835  {
1836  ui_->horizontalSlider_neighbors->setMinimum(0);
1837  ui_->horizontalSlider_neighbors->setMaximum(neighborLinks_.size()-1);
1838  ui_->horizontalSlider_neighbors->setEnabled(true);
1839  ui_->horizontalSlider_neighbors->setSliderPosition(0);
1840  }
1841  else
1842  {
1843  ui_->horizontalSlider_neighbors->setEnabled(false);
1844  }
1845 
1846  if(ids_.size())
1847  {
1848  updateLoopClosuresSlider();
1849  if(ui_->graphViewer->isVisible() || ui_->dockWidget_occupancyGridView->isVisible())
1850  {
1851  updateGraphView();
1852  }
1853  }
1854 }
1855 
1856 void DatabaseViewer::updateStatistics()
1857 {
1858  UDEBUG("");
1859  if(dbDriver_)
1860  {
1861  ui_->toolBox_statistics->clear();
1862  double firstStamp = 0.0;
1863  std::map<int, std::pair<std::map<std::string, float>, double> > allStats = dbDriver_->getAllStatistics();
1864 
1865  std::map<std::string, std::pair<std::vector<float>, std::vector<float> > > allData;
1866  std::map<std::string, int > allDataOi;
1867 
1868  for(int i=0; i<ids_.size(); ++i)
1869  {
1870  double stamp=0.0;
1871  std::map<std::string, float> statistics;
1872  if(allStats.find(ids_[i]) != allStats.end())
1873  {
1874  statistics = allStats.at(ids_[i]).first;
1875  stamp = allStats.at(ids_[i]).second;
1876  }
1877  if(firstStamp==0.0)
1878  {
1879  firstStamp = stamp;
1880  }
1881  for(std::map<std::string, float>::iterator iter=statistics.begin(); iter!=statistics.end(); ++iter)
1882  {
1883  if(allData.find(iter->first) == allData.end())
1884  {
1885  //initialize data vectors
1886  allData.insert(std::make_pair(iter->first, std::make_pair(std::vector<float>(ids_.size(), 0.0f), std::vector<float>(ids_.size(), 0.0f) )));
1887  allDataOi.insert(std::make_pair(iter->first, 0));
1888  }
1889 
1890  int & oi = allDataOi.at(iter->first);
1891  allData.at(iter->first).first[oi] = ui_->checkBox_timeStats->isChecked()?float(stamp-firstStamp):ids_[i];
1892  allData.at(iter->first).second[oi] = iter->second;
1893  ++oi;
1894  }
1895  }
1896 
1897  for(std::map<std::string, std::pair<std::vector<float>, std::vector<float> > >::iterator iter=allData.begin(); iter!=allData.end(); ++iter)
1898  {
1899  int oi = allDataOi.at(iter->first);
1900  iter->second.first.resize(oi);
1901  iter->second.second.resize(oi);
1902  ui_->toolBox_statistics->updateStat(iter->first.c_str(), iter->second.first, iter->second.second, true);
1903  }
1904  }
1905  UDEBUG("");
1906 }
1907 
1908 void DatabaseViewer::selectObstacleColor()
1909 {
1910  QColor c = QColorDialog::getColor(ui_->lineEdit_obstacleColor->text(), this);
1911  if(c.isValid())
1912  {
1913  ui_->lineEdit_obstacleColor->setText(c.name());
1914  }
1915 }
1916 void DatabaseViewer::selectGroundColor()
1917 {
1918  QColor c = QColorDialog::getColor(ui_->lineEdit_groundColor->text(), this);
1919  if(c.isValid())
1920  {
1921  ui_->lineEdit_groundColor->setText(c.name());
1922  }
1923 }
1924 void DatabaseViewer::selectEmptyColor()
1925 {
1926  QColor c = QColorDialog::getColor(ui_->lineEdit_emptyColor->text(), this);
1927  if(c.isValid())
1928  {
1929  ui_->lineEdit_emptyColor->setText(c.name());
1930  }
1931 }
1932 void DatabaseViewer::editDepthImage()
1933 {
1934  if(dbDriver_ && ids_.size())
1935  {
1936  int id = ids_.at(ui_->horizontalSlider_A->value());
1937  SensorData data;
1938  dbDriver_->getNodeData(id, data, true, false, false, false);
1939  data.uncompressData();
1940  if(!data.depthRaw().empty())
1941  {
1942  editDepthArea_->setImage(data.depthRaw(), data.imageRaw());
1943  if(editDepthDialog_->exec() == QDialog::Accepted && editDepthArea_->isModified())
1944  {
1945  cv::Mat depth = editDepthArea_->getModifiedImage();
1946  UASSERT(data.depthRaw().type() == depth.type());
1947  UASSERT(data.depthRaw().cols == depth.cols);
1948  UASSERT(data.depthRaw().rows == depth.rows);
1949  dbDriver_->updateDepthImage(id, depth);
1950  this->update3dView();
1951  }
1952  }
1953  }
1954 }
1955 
1956 void DatabaseViewer::exportPosesRaw()
1957 {
1958  exportPoses(0);
1959 }
1960 void DatabaseViewer::exportPosesRGBDSLAM()
1961 {
1962  exportPoses(1);
1963 }
1964 void DatabaseViewer::exportPosesKITTI()
1965 {
1966  exportPoses(2);
1967 }
1968 void DatabaseViewer::exportPosesTORO()
1969 {
1970  exportPoses(3);
1971 }
1972 void DatabaseViewer::exportPosesG2O()
1973 {
1974  exportPoses(4);
1975 }
1976 void DatabaseViewer::exportPosesKML()
1977 {
1978  exportPoses(5);
1979 }
1980 
1981 void DatabaseViewer::exportPoses(int format)
1982 {
1983  if(graphes_.empty())
1984  {
1985  this->updateGraphView();
1986  if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
1987  {
1988  QMessageBox::warning(this, tr("Cannot export poses"), tr("No graph in database?!"));
1989  return;
1990  }
1991  }
1992 
1993  if(format == 5)
1994  {
1995  if(gpsValues_.empty() || gpsPoses_.empty())
1996  {
1997  QMessageBox::warning(this, tr("Cannot export poses"), tr("No GPS in database?!"));
1998  }
1999  else
2000  {
2001  std::map<int, rtabmap::Transform> graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
2002 
2003  //align with ground truth for more meaningful results
2004  pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
2005  cloud1.resize(graph.size());
2006  cloud2.resize(graph.size());
2007  int oi = 0;
2008  int idFirst = 0;
2009  for(std::map<int, Transform>::const_iterator iter=gpsPoses_.begin(); iter!=gpsPoses_.end(); ++iter)
2010  {
2011  std::map<int, Transform>::iterator iter2 = graph.find(iter->first);
2012  if(iter2!=graph.end())
2013  {
2014  if(oi==0)
2015  {
2016  idFirst = iter->first;
2017  }
2018  cloud1[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
2019  cloud2[oi++] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z());
2020  }
2021  }
2022 
2023  Transform t = Transform::getIdentity();
2024  if(oi>5)
2025  {
2026  cloud1.resize(oi);
2027  cloud2.resize(oi);
2028 
2029  t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
2030  }
2031  else if(idFirst)
2032  {
2033  t = gpsPoses_.at(idFirst) * graph.at(idFirst).inverse();
2034  }
2035 
2036  std::map<int, GPS> values;
2037  GeodeticCoords origin = gpsValues_.begin()->second.toGeodeticCoords();
2038  for(std::map<int, Transform>::iterator iter=graph.begin(); iter!=graph.end(); ++iter)
2039  {
2040  iter->second = t * iter->second;
2041 
2042  GeodeticCoords coord;
2043  coord.fromENU_WGS84(cv::Point3d(iter->second.x(), iter->second.y(), iter->second.z()), origin);
2044  double bearing = -(iter->second.theta()*180.0/M_PI-90.0);
2045  if(bearing < 0)
2046  {
2047  bearing += 360;
2048  }
2049 
2050  Transform p, g;
2051  int w;
2052  std::string l;
2053  double stamp=0.0;
2054  int mapId;
2055  std::vector<float> v;
2056  GPS gps;
2057  dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps);
2058  values.insert(std::make_pair(iter->first, GPS(stamp, coord.longitude(), coord.latitude(), coord.altitude(), 0, 0)));
2059  }
2060 
2061  QString output = pathDatabase_ + QDir::separator() + "poses.kml";
2062  QString path = QFileDialog::getSaveFileName(
2063  this,
2064  tr("Save File"),
2065  output,
2066  tr("Google Earth file (*.kml)"));
2067 
2068  if(!path.isEmpty())
2069  {
2070  bool saved = graph::exportGPS(path.toStdString(), values, ui_->graphViewer->getNodeColor().rgba());
2071 
2072  if(saved)
2073  {
2074  QMessageBox::information(this,
2075  tr("Export poses..."),
2076  tr("GPS coordinates saved to \"%1\".")
2077  .arg(path));
2078  }
2079  else
2080  {
2081  QMessageBox::information(this,
2082  tr("Export poses..."),
2083  tr("Failed to save GPS coordinates to \"%1\"!")
2084  .arg(path));
2085  }
2086  }
2087  }
2088  return;
2089  }
2090 
2091  std::map<int, Transform> optimizedPoses;
2092  if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty())
2093  {
2094  optimizedPoses = groundTruthPoses_;
2095  }
2096  else
2097  {
2098  optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
2099 
2100  if(ui_->checkBox_alignPosesWithGroundTruth->isChecked())
2101  {
2102  std::map<int, Transform> refPoses = groundTruthPoses_;
2103  if(refPoses.empty())
2104  {
2105  refPoses = gpsPoses_;
2106  }
2107 
2108  // Log ground truth statistics (in TUM's RGBD-SLAM format)
2109  if(refPoses.size())
2110  {
2111  float translational_rmse = 0.0f;
2112  float translational_mean = 0.0f;
2113  float translational_median = 0.0f;
2114  float translational_std = 0.0f;
2115  float translational_min = 0.0f;
2116  float translational_max = 0.0f;
2117  float rotational_rmse = 0.0f;
2118  float rotational_mean = 0.0f;
2119  float rotational_median = 0.0f;
2120  float rotational_std = 0.0f;
2121  float rotational_min = 0.0f;
2122  float rotational_max = 0.0f;
2123 
2124  Transform gtToMap = graph::calcRMSE(
2125  refPoses,
2126  optimizedPoses,
2127  translational_rmse,
2128  translational_mean,
2129  translational_median,
2130  translational_std,
2131  translational_min,
2132  translational_max,
2133  rotational_rmse,
2134  rotational_mean,
2135  rotational_median,
2136  rotational_std,
2137  rotational_min,
2138  rotational_max);
2139 
2140  if(ui_->checkBox_alignPosesWithGroundTruth->isChecked() && !gtToMap.isIdentity())
2141  {
2142  for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
2143  {
2144  iter->second = gtToMap * iter->second;
2145  }
2146  }
2147  }
2148  }
2149  }
2150 
2151  if(optimizedPoses.size())
2152  {
2153  std::map<int, Transform> localTransforms;
2154  QStringList items;
2155  items.push_back("Robot");
2156  items.push_back("Camera");
2157  items.push_back("Scan");
2158  bool ok;
2159  QString item = QInputDialog::getItem(this, tr("Export Poses"), tr("Frame: "), items, 0, false, &ok);
2160  if(!ok || item.isEmpty())
2161  {
2162  return;
2163  }
2164  if(item.compare("Robot") != 0)
2165  {
2166  bool cameraFrame = item.compare("Camera") == 0;
2167  for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
2168  {
2169  Transform localTransform;
2170  if(cameraFrame)
2171  {
2172  std::vector<CameraModel> models;
2173  StereoCameraModel stereoModel;
2174  if(dbDriver_->getCalibration(iter->first, models, stereoModel))
2175  {
2176  if((models.size() == 1 &&
2177  !models.at(0).localTransform().isNull()))
2178  {
2179  localTransform = models.at(0).localTransform();
2180  }
2181  else if(!stereoModel.localTransform().isNull())
2182  {
2183  localTransform = stereoModel.localTransform();
2184  }
2185  else if(models.size()>1)
2186  {
2187  UWARN("Multi-camera is not supported (node %d)", iter->first);
2188  }
2189  else
2190  {
2191  UWARN("Calibration not valid for node %d", iter->first);
2192  }
2193  }
2194  else
2195  {
2196  UWARN("Missing calibration for node %d", iter->first);
2197  }
2198  }
2199  else
2200  {
2201  LaserScan info;
2202  if(dbDriver_->getLaserScanInfo(iter->first, info))
2203  {
2204  localTransform = info.localTransform();
2205  }
2206  else
2207  {
2208  UWARN("Missing scan info for node %d", iter->first);
2209  }
2210 
2211  }
2212  if(!localTransform.isNull())
2213  {
2214  localTransforms.insert(std::make_pair(iter->first, localTransform));
2215  }
2216  }
2217  if(localTransforms.empty())
2218  {
2219  QMessageBox::warning(this,
2220  tr("Export Poses"),
2221  tr("Could not find any \"%1\" frame, exporting in Robot frame instead.").arg(item));
2222  }
2223  }
2224 
2225  std::map<int, Transform> poses;
2226  std::multimap<int, Link> links;
2227  if(localTransforms.empty())
2228  {
2229  poses = optimizedPoses;
2230  links = graphLinks_;
2231  }
2232  else
2233  {
2234  //adjust poses and links
2235  for(std::map<int, Transform>::iterator iter=localTransforms.begin(); iter!=localTransforms.end(); ++iter)
2236  {
2237  poses.insert(std::make_pair(iter->first, optimizedPoses.at(iter->first) * iter->second));
2238  }
2239  for(std::multimap<int, Link>::iterator iter=graphLinks_.begin(); iter!=graphLinks_.end(); ++iter)
2240  {
2241  if(uContains(poses, iter->second.from()) && uContains(poses, iter->second.to()))
2242  {
2243  std::multimap<int, Link>::iterator inserted = links.insert(*iter);
2244  int from = iter->second.from();
2245  int to = iter->second.to();
2246  inserted->second.setTransform(localTransforms.at(from).inverse()*iter->second.transform()*localTransforms.at(to));
2247  }
2248  }
2249  }
2250 
2251  std::map<int, double> stamps;
2252  if(format == 1)
2253  {
2254  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
2255  {
2256  Transform p, g;
2257  int w;
2258  std::string l;
2259  double stamp=0.0;
2260  int mapId;
2261  std::vector<float> v;
2262  GPS gps;
2263  if(dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps))
2264  {
2265  stamps.insert(std::make_pair(iter->first, stamp));
2266  }
2267  }
2268  if(stamps.size()!=poses.size())
2269  {
2270  QMessageBox::warning(this, tr("Export poses..."), tr("Poses (%1) and stamps (%2) have not the same size! Cannot export in RGB-D SLAM format.")
2271  .arg(poses.size()).arg(stamps.size()));
2272  return;
2273  }
2274  }
2275 
2276  QString output = pathDatabase_ + QDir::separator() + (format==3?"toro.graph":format==4?"poses.g2o":"poses.txt");
2277 
2278  QString path = QFileDialog::getSaveFileName(
2279  this,
2280  tr("Save File"),
2281  output,
2282  format == 3?tr("TORO file (*.graph)"):format==4?tr("g2o file (*.g2o)"):tr("Text file (*.txt)"));
2283 
2284  if(!path.isEmpty())
2285  {
2286  if(QFileInfo(path).suffix() == "")
2287  {
2288  if(format == 3)
2289  {
2290  path += ".graph";
2291  }
2292  else if(format==4)
2293  {
2294  path += ".g2o";
2295  }
2296  else
2297  {
2298  path += ".txt";
2299  }
2300  }
2301 
2302  bool saved = graph::exportPoses(path.toStdString(), format, poses, links, stamps);
2303 
2304  if(saved)
2305  {
2306  QMessageBox::information(this,
2307  tr("Export poses..."),
2308  tr("%1 saved to \"%2\".")
2309  .arg(format == 3?"TORO graph":format == 4?"g2o graph":"Poses")
2310  .arg(path));
2311  }
2312  else
2313  {
2314  QMessageBox::information(this,
2315  tr("Export poses..."),
2316  tr("Failed to save %1 to \"%2\"!")
2317  .arg(format == 3?"TORO graph":format == 4?"g2o graph":"poses")
2318  .arg(path));
2319  }
2320  }
2321  }
2322 }
2323 
2324 void DatabaseViewer::exportGPS_TXT()
2325 {
2326  exportGPS(0);
2327 }
2328 void DatabaseViewer::exportGPS_KML()
2329 {
2330  exportGPS(1);
2331 }
2332 
2333 void DatabaseViewer::exportGPS(int format)
2334 {
2335  if(!gpsValues_.empty())
2336  {
2337  QString output = pathDatabase_ + QDir::separator() + (format==0?"gps.txt":"gps.kml");
2338  QString path = QFileDialog::getSaveFileName(
2339  this,
2340  tr("Save File"),
2341  output,
2342  format==0?tr("Raw format (*.txt)"):tr("Google Earth file (*.kml)"));
2343 
2344  if(!path.isEmpty())
2345  {
2346  bool saved = graph::exportGPS(path.toStdString(), gpsValues_, ui_->graphViewer->getGPSColor().rgba());
2347 
2348  if(saved)
2349  {
2350  QMessageBox::information(this,
2351  tr("Export poses..."),
2352  tr("GPS coordinates saved to \"%1\".")
2353  .arg(path));
2354  }
2355  else
2356  {
2357  QMessageBox::information(this,
2358  tr("Export poses..."),
2359  tr("Failed to save GPS coordinates to \"%1\"!")
2360  .arg(path));
2361  }
2362  }
2363  }
2364 }
2365 
2366 void DatabaseViewer::exportSaved2DMap()
2367 {
2368  if(!dbDriver_)
2369  {
2370  QMessageBox::warning(this, tr("Cannot export 2D map"), tr("A database must must loaded first...\nUse File->Open database."));
2371  return;
2372  }
2373 
2374  float xMin, yMin, cellSize;
2375  cv::Mat map = dbDriver_->load2DMap(xMin, yMin, cellSize);
2376  if(map.empty())
2377  {
2378  QMessageBox::warning(this, tr("Cannot export 2D map"), tr("The database doesn't contain a saved 2D map."));
2379  }
2380  else
2381  {
2382  cv::Mat map8U = rtabmap::util3d::convertMap2Image8U(map, true);
2383  QString name = QFileInfo(databaseFileName_.c_str()).baseName();
2384  QString path = QFileDialog::getSaveFileName(
2385  this,
2386  tr("Save File"),
2387  pathDatabase_+"/" + name + ".pgm",
2388  tr("Map (*.pgm)"));
2389 
2390  if(!path.isEmpty())
2391  {
2392  if(QFileInfo(path).suffix() == "")
2393  {
2394  path += ".pgm";
2395  }
2396  cv::imwrite(path.toStdString(), map8U);
2397  QMessageBox::information(this, tr("Export 2D map"), tr("Exported %1!").arg(path));
2398  }
2399  }
2400 }
2401 
2402 void DatabaseViewer::import2DMap()
2403 {
2404  if(!dbDriver_)
2405  {
2406  QMessageBox::warning(this, tr("Cannot import 2D map"), tr("A database must must loaded first...\nUse File->Open database."));
2407  return;
2408  }
2409 
2410  float xMin, yMin, cellSize;
2411  cv::Mat mapOrg = dbDriver_->load2DMap(xMin, yMin, cellSize);
2412  if(mapOrg.empty())
2413  {
2414  QMessageBox::warning(this, tr("Cannot import 2D map"), tr("The database doesn't contain a saved 2D map."));
2415  }
2416  else
2417  {
2418  QString path = QFileDialog::getOpenFileName(
2419  this,
2420  tr("Open File"),
2421  pathDatabase_,
2422  tr("Map (*.pgm)"));
2423  if(!path.isEmpty())
2424  {
2425  cv::Mat map8U = cv::imread(path.toStdString(), cv::IMREAD_UNCHANGED);
2426  cv::Mat map = rtabmap::util3d::convertImage8U2Map(map8U, true);
2427 
2428  if(mapOrg.cols == map.cols && mapOrg.rows == map8U.rows)
2429  {
2430  dbDriver_->save2DMap(map, xMin, yMin, cellSize);
2431  QMessageBox::information(this, tr("Import 2D map"), tr("Imported %1!").arg(path));
2432  }
2433  else
2434  {
2435  QMessageBox::warning(this, tr("Import 2D map"), tr("Cannot import %1 as its size doesn't match the current saved map. Import 2D Map action should only be used to modify the map saved in the database.").arg(path));
2436  }
2437  }
2438  }
2439 }
2440 
2441 void DatabaseViewer::viewOptimizedMesh()
2442 {
2443  if(!dbDriver_)
2444  {
2445  QMessageBox::warning(this, tr("Cannot view optimized mesh"), tr("A database must must loaded first...\nUse File->Open database."));
2446  return;
2447  }
2448 
2449  std::vector<std::vector<std::vector<unsigned int> > > polygons;
2450 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
2451  std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
2452 #else
2453  std::vector<std::vector<Eigen::Vector2f> > texCoords;
2454 #endif
2455  cv::Mat textures;
2456  cv::Mat cloudMat = dbDriver_->loadOptimizedMesh(&polygons, &texCoords, &textures);
2457  if(cloudMat.empty())
2458  {
2459  QMessageBox::warning(this, tr("Cannot view optimized mesh"), tr("The database doesn't contain a saved optimized mesh."));
2460  }
2461  else
2462  {
2463  CloudViewer * viewer = new CloudViewer(this);
2464  viewer->setWindowFlags(Qt::Window);
2465  viewer->setAttribute(Qt::WA_DeleteOnClose);
2466  viewer->buildPickingLocator(true);
2467  if(!textures.empty())
2468  {
2469  pcl::TextureMeshPtr mesh = util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
2470  util3d::fixTextureMeshForVisualization(*mesh);
2471  viewer->setWindowTitle("Optimized Textured Mesh");
2472  viewer->setPolygonPicking(true);
2473  viewer->addCloudTextureMesh("mesh", mesh, textures);
2474  }
2475  else if(polygons.size() == 1)
2476  {
2477  pcl::PolygonMeshPtr mesh = util3d::assemblePolygonMesh(cloudMat, polygons.at(0));
2478  viewer->setWindowTitle("Optimized Mesh");
2479  viewer->setPolygonPicking(true);
2480  viewer->addCloudMesh("mesh", mesh);
2481  }
2482  else
2483  {
2484  LaserScan scan = LaserScan::backwardCompatibility(cloudMat);
2485  pcl::PCLPointCloud2::Ptr cloud = util3d::laserScanToPointCloud2(scan);
2486  viewer->setWindowTitle("Optimized Point Cloud");
2487  viewer->addCloud("mesh", cloud, Transform::getIdentity(), scan.hasRGB(), scan.hasNormals(), scan.hasIntensity());
2488  }
2489  viewer->show();
2490  }
2491 }
2492 
2493 void DatabaseViewer::exportOptimizedMesh()
2494 {
2495  if(!dbDriver_)
2496  {
2497  QMessageBox::warning(this, tr("Cannot export optimized mesh"), tr("A database must must loaded first...\nUse File->Open database."));
2498  return;
2499  }
2500 
2501  std::vector<std::vector<std::vector<unsigned int> > > polygons;
2502 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
2503  std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
2504 #else
2505  std::vector<std::vector<Eigen::Vector2f> > texCoords;
2506 #endif
2507  cv::Mat textures;
2508  cv::Mat cloudMat = dbDriver_->loadOptimizedMesh(&polygons, &texCoords, &textures);
2509  if(cloudMat.empty())
2510  {
2511  QMessageBox::warning(this, tr("Cannot export optimized mesh"), tr("The database doesn't contain a saved optimized mesh."));
2512  }
2513  else
2514  {
2515  QString name = QFileInfo(databaseFileName_.c_str()).baseName();
2516 
2517  if(!textures.empty())
2518  {
2519  pcl::TextureMeshPtr mesh = util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures);
2520  QString path = QFileDialog::getSaveFileName(
2521  this,
2522  tr("Save File"),
2523  pathDatabase_+"/" + name + ".obj",
2524  tr("Mesh (*.obj)"));
2525 
2526  if(!path.isEmpty())
2527  {
2528  if(QFileInfo(path).suffix() == "")
2529  {
2530  path += ".obj";
2531  }
2532  QString baseName = QFileInfo(path).baseName();
2533  if(mesh->tex_materials.size() == 1)
2534  {
2535  mesh->tex_materials.at(0).tex_file = baseName.toStdString() + ".png";
2536  cv::imwrite((QFileInfo(path).absoluteDir().absolutePath()+QDir::separator()+baseName).toStdString() + ".png", textures);
2537  }
2538  else
2539  {
2540  for(unsigned int i=0; i<mesh->tex_materials.size(); ++i)
2541  {
2542  mesh->tex_materials.at(i).tex_file = (baseName+QDir::separator()+QString::number(i)+".png").toStdString();
2543  UASSERT((i+1)*textures.rows <= (unsigned int)textures.cols);
2544  cv::imwrite((QFileInfo(path).absoluteDir().absolutePath()+QDir::separator()+baseName+QDir::separator()+QString::number(i)+".png").toStdString(), textures(cv::Range::all(), cv::Range(i*textures.rows, (i+1)*textures.rows)));
2545  }
2546  }
2547  pcl::io::saveOBJFile(path.toStdString(), *mesh);
2548 
2549  QMessageBox::information(this, tr("Export Textured Mesh"), tr("Exported %1!").arg(path));
2550  }
2551  }
2552  else if(polygons.size() == 1)
2553  {
2554  pcl::PolygonMeshPtr mesh = util3d::assemblePolygonMesh(cloudMat, polygons.at(0));
2555  QString path = QFileDialog::getSaveFileName(
2556  this,
2557  tr("Save File"),
2558  pathDatabase_+"/" + name + ".ply",
2559  tr("Mesh (*.ply)"));
2560 
2561  if(!path.isEmpty())
2562  {
2563  if(QFileInfo(path).suffix() == "")
2564  {
2565  path += ".ply";
2566  }
2567  pcl::io::savePLYFileBinary(path.toStdString(), *mesh);
2568  QMessageBox::information(this, tr("Export Mesh"), tr("Exported %1!").arg(path));
2569  }
2570  }
2571  else
2572  {
2573  QString path = QFileDialog::getSaveFileName(
2574  this,
2575  tr("Save File"),
2576  pathDatabase_+"/" + name + ".ply",
2577  tr("Point cloud data (*.ply *.pcd)"));
2578 
2579  if(!path.isEmpty())
2580  {
2581  if(QFileInfo(path).suffix() == "")
2582  {
2583  path += ".ply";
2584  }
2585  bool success = false;
2586  pcl::PCLPointCloud2::Ptr cloud = util3d::laserScanToPointCloud2(LaserScan::backwardCompatibility(cloudMat));
2587  if(QFileInfo(path).suffix() == "pcd")
2588  {
2589  success = pcl::io::savePCDFile(path.toStdString(), *cloud) == 0;
2590  }
2591  else
2592  {
2593  success = pcl::io::savePLYFile(path.toStdString(), *cloud) == 0;
2594  }
2595  if(success)
2596  {
2597  QMessageBox::information(this, tr("Export Point Cloud"), tr("Exported %1!").arg(path));
2598  }
2599  else
2600  {
2601  QMessageBox::critical(this, tr("Export Point Cloud"), tr("Failed exporting %1!").arg(path));
2602  }
2603  }
2604  }
2605  }
2606 }
2607 
2608 void DatabaseViewer::updateOptimizedMesh()
2609 {
2610  if(!ids_.size() || !dbDriver_)
2611  {
2612  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
2613  return;
2614  }
2615 
2616  if(graphes_.empty())
2617  {
2618  this->updateGraphView();
2619  if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
2620  {
2621  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
2622  return;
2623  }
2624  }
2625 
2626  std::map<int, Transform> optimizedPoses;
2627  if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty())
2628  {
2629  optimizedPoses = groundTruthPoses_;
2630  }
2631  else
2632  {
2633  optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
2634  }
2635  if(ui_->groupBox_posefiltering->isChecked())
2636  {
2637  optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
2638  ui_->doubleSpinBox_posefilteringRadius->value(),
2639  ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
2640  }
2641  if(optimizedPoses.size() > 0)
2642  {
2643  exportDialog_->setDBDriver(dbDriver_);
2644  exportDialog_->forceAssembling(true);
2645  exportDialog_->setOkButton();
2646 
2647  std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> clouds;
2648  std::map<int, pcl::PolygonMesh::Ptr> meshes;
2649  std::map<int, pcl::TextureMesh::Ptr> textureMeshes;
2650  std::vector<std::map<int, pcl::PointXY> > textureVertexToPixels;
2651 
2652  if(exportDialog_->getExportedClouds(
2653  optimizedPoses,
2654  updateLinksWithModifications(links_),
2655  mapIds_,
2656  QMap<int, Signature>(),
2657  std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
2658  std::map<int, LaserScan>(),
2659  pathDatabase_,
2660  ui_->parameters_toolbox->getParameters(),
2661  clouds,
2662  meshes,
2663  textureMeshes,
2664  textureVertexToPixels))
2665  {
2666  if(textureMeshes.size())
2667  {
2668  dbDriver_->saveOptimizedPoses(optimizedPoses, Transform());
2669 
2670  cv::Mat globalTextures;
2671  pcl::TextureMeshPtr textureMesh = textureMeshes.at(0);
2672  if(textureMesh->tex_materials.size()>1)
2673  {
2674  globalTextures = util3d::mergeTextures(
2675  *textureMesh,
2676  std::map<int, cv::Mat>(),
2677  std::map<int, std::vector<CameraModel> >(),
2678  0,
2679  dbDriver_,
2680  exportDialog_->getTextureSize(),
2681  exportDialog_->getMaxTextures(),
2682  textureVertexToPixels,
2683  exportDialog_->isGainCompensation(),
2684  exportDialog_->getGainBeta(),
2685  exportDialog_->isGainRGB(),
2686  exportDialog_->isBlending(),
2687  exportDialog_->getBlendingDecimation(),
2688  exportDialog_->getTextureBrightnessConstrastRatioLow(),
2689  exportDialog_->getTextureBrightnessConstrastRatioHigh(),
2690  exportDialog_->isExposeFusion());
2691  }
2692  dbDriver_->saveOptimizedMesh(
2693  util3d::laserScanFromPointCloud(textureMesh->cloud, false).data(),
2694  util3d::convertPolygonsFromPCL(textureMesh->tex_polygons),
2695  textureMesh->tex_coordinates,
2696  globalTextures);
2697  QMessageBox::information(this, tr("Update Optimized Textured Mesh"), tr("Updated!"));
2698  ui_->actionView_optimized_mesh->setEnabled(true);
2699  ui_->actionExport_optimized_mesh->setEnabled(true);
2700  this->viewOptimizedMesh();
2701  }
2702  else if(meshes.size())
2703  {
2704  dbDriver_->saveOptimizedPoses(optimizedPoses, Transform());
2705  std::vector<std::vector<std::vector<unsigned int> > > polygons(1);
2706  polygons.at(0) = util3d::convertPolygonsFromPCL(meshes.at(0)->polygons);
2707  dbDriver_->saveOptimizedMesh(util3d::laserScanFromPointCloud(meshes.at(0)->cloud, false).data(), polygons);
2708  QMessageBox::information(this, tr("Update Optimized Mesh"), tr("Updated!"));
2709  ui_->actionView_optimized_mesh->setEnabled(true);
2710  ui_->actionExport_optimized_mesh->setEnabled(true);
2711  this->viewOptimizedMesh();
2712  }
2713  else if(clouds.size())
2714  {
2715  dbDriver_->saveOptimizedPoses(optimizedPoses, Transform());
2716  dbDriver_->saveOptimizedMesh(util3d::laserScanFromPointCloud(*clouds.at(0)));
2717  QMessageBox::information(this, tr("Update Optimized PointCloud"), tr("Updated!"));
2718  ui_->actionView_optimized_mesh->setEnabled(true);
2719  ui_->actionExport_optimized_mesh->setEnabled(true);
2720  this->viewOptimizedMesh();
2721  }
2722  else
2723  {
2724  QMessageBox::critical(this, tr("Update Optimized Mesh"), tr("Nothing to save!"));
2725  }
2726  }
2727  exportDialog_->setProgressDialogToMax();
2728  }
2729  else
2730  {
2731  QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
2732  }
2733 }
2734 
2735 void DatabaseViewer::generateGraph()
2736 {
2737  if(!dbDriver_)
2738  {
2739  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("A database must must loaded first...\nUse File->Open database."));
2740  return;
2741  }
2742 
2743  QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/Graph.dot", tr("Graphiz file (*.dot)"));
2744  if(!path.isEmpty())
2745  {
2746  dbDriver_->generateGraph(path.toStdString());
2747  }
2748 }
2749 
2750 void DatabaseViewer::generateLocalGraph()
2751 {
2752  if(!ids_.size() || !dbDriver_)
2753  {
2754  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
2755  return;
2756  }
2757  bool ok = false;
2758  int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID"), ids_.first(), ids_.first(), ids_.last(), 1, &ok);
2759 
2760  if(ok)
2761  {
2762  int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin"), 4, 1, 100, 1, &ok);
2763  if(ok)
2764  {
2765  QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/Graph" + QString::number(id) + ".dot", tr("Graphiz file (*.dot)"));
2766  if(!path.isEmpty() && id>0)
2767  {
2768  std::map<int, int> ids;
2769  std::list<int> curentMarginList;
2770  std::set<int> currentMargin;
2771  std::set<int> nextMargin;
2772  nextMargin.insert(id);
2773  int m = 0;
2774  while((margin == 0 || m < margin) && nextMargin.size())
2775  {
2776  curentMarginList = std::list<int>(nextMargin.rbegin(), nextMargin.rend());
2777  nextMargin.clear();
2778 
2779  for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
2780  {
2781  if(ids.find(*jter) == ids.end())
2782  {
2783  std::map<int, Link> links;
2784  ids.insert(std::pair<int, int>(*jter, m));
2785 
2786  UTimer timer;
2787  dbDriver_->loadLinks(*jter, links);
2788 
2789  // links
2790  for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
2791  {
2792  if( !uContains(ids, iter->first))
2793  {
2794  UASSERT(iter->second.type() != Link::kUndef);
2795  if(iter->second.type() == Link::kNeighbor ||
2796  iter->second.type() == Link::kNeighborMerged)
2797  {
2798  nextMargin.insert(iter->first);
2799  }
2800  else
2801  {
2802  // loop closures are on same margin
2803  if(currentMargin.insert(iter->first).second)
2804  {
2805  curentMarginList.push_back(iter->first);
2806  }
2807  }
2808  }
2809  }
2810  }
2811  }
2812  ++m;
2813  }
2814 
2815  if(ids.size() > 0)
2816  {
2817  ids.insert(std::pair<int,int>(id, 0));
2818  std::set<int> idsSet;
2819  for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
2820  {
2821  idsSet.insert(idsSet.end(), iter->first);
2822  UINFO("Node %d", iter->first);
2823  }
2824  UINFO("idsSet=%d", idsSet.size());
2825  dbDriver_->generateGraph(path.toStdString(), idsSet);
2826  }
2827  else
2828  {
2829  QMessageBox::critical(this, tr("Error"), tr("No neighbors found for signature %1.").arg(id));
2830  }
2831  }
2832  }
2833  }
2834 }
2835 
2836 void DatabaseViewer::regenerateLocalMaps()
2837 {
2838  OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
2839 
2840  generatedLocalMaps_.clear();
2841  generatedLocalMapsInfo_.clear();
2842 
2843  rtabmap::ProgressDialog progressDialog(this);
2844  progressDialog.setMaximumSteps(ids_.size());
2845  progressDialog.show();
2846  progressDialog.setCancelButtonVisible(true);
2847 
2848  UPlot * plot = new UPlot(this);
2849  plot->setWindowFlags(Qt::Window);
2850  plot->setWindowTitle("Local Occupancy Grid Generation Time (ms)");
2851  plot->setAttribute(Qt::WA_DeleteOnClose);
2852  UPlotCurve * decompressionCurve = plot->addCurve("Decompression");
2853  UPlotCurve * gridCreationCurve = plot->addCurve("Grid Creation");
2854  plot->show();
2855 
2856  UPlot * plotCells = new UPlot(this);
2857  plotCells->setWindowFlags(Qt::Window);
2858  plotCells->setWindowTitle("Occupancy Cells");
2859  plotCells->setAttribute(Qt::WA_DeleteOnClose);
2860  UPlotCurve * totalCurve = plotCells->addCurve("Total");
2861  UPlotCurve * emptyCurve = plotCells->addCurve("Empty");
2862  UPlotCurve * obstaclesCurve = plotCells->addCurve("Obstacles");
2863  UPlotCurve * groundCurve = plotCells->addCurve("Ground");
2864  plotCells->show();
2865 
2866  double decompressionTime = 0;
2867  double gridCreationTime = 0;
2868 
2869  for(int i =0; i<ids_.size() && !progressDialog.isCanceled(); ++i)
2870  {
2871  UTimer timer;
2872  SensorData data;
2873  dbDriver_->getNodeData(ids_.at(i), data);
2874  data.uncompressData();
2875  decompressionTime = timer.ticks()*1000.0;
2876 
2877  int mapId, weight;
2878  Transform odomPose, groundTruth;
2879  std::string label;
2880  double stamp;
2881  QString msg;
2882  std::vector<float> velocity;
2883  GPS gps;
2884  if(dbDriver_->getNodeInfo(data.id(), odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps))
2885  {
2886  Signature s = data;
2887  s.setPose(odomPose);
2888  cv::Mat ground, obstacles, empty;
2889  cv::Point3f viewpoint;
2890  timer.ticks();
2891 
2892  if(ui_->checkBox_grid_regenerateFromSavedGrid->isChecked() && s.sensorData().gridCellSize() > 0.0f)
2893  {
2894  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridObstacleCellsRaw()));
2895  *cloud+=*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridGroundCellsRaw()));
2896 
2897  if(cloud->size())
2898  {
2899  // update viewpoint
2900  if(s.sensorData().cameraModels().size())
2901  {
2902  // average of all local transforms
2903  float sum = 0;
2904  for(unsigned int i=0; i<s.sensorData().cameraModels().size(); ++i)
2905  {
2906  const Transform & t = s.sensorData().cameraModels()[i].localTransform();
2907  if(!t.isNull())
2908  {
2909  viewpoint.x += t.x();
2910  viewpoint.y += t.y();
2911  viewpoint.z += t.z();
2912  sum += 1.0f;
2913  }
2914  }
2915  if(sum > 0.0f)
2916  {
2917  viewpoint.x /= sum;
2918  viewpoint.y /= sum;
2919  viewpoint.z /= sum;
2920  }
2921  }
2922  else
2923  {
2924  const Transform & t = s.sensorData().stereoCameraModel().localTransform();
2925  viewpoint = cv::Point3f(t.x(), t.y(), t.z());
2926  }
2927 
2928  grid.createLocalMap(LaserScan::backwardCompatibility(util3d::laserScanFromPointCloud(*cloud)), s.getPose(), ground, obstacles, empty, viewpoint);
2929  }
2930  }
2931  else
2932  {
2933  grid.createLocalMap(s, ground, obstacles, empty, viewpoint);
2934  }
2935 
2936  gridCreationTime = timer.ticks()*1000.0;
2937  uInsert(generatedLocalMaps_, std::make_pair(data.id(), std::make_pair(std::make_pair(ground, obstacles), empty)));
2938  uInsert(generatedLocalMapsInfo_, std::make_pair(data.id(), std::make_pair(grid.getCellSize(), viewpoint)));
2939  msg = QString("Generated local occupancy grid map %1/%2").arg(i+1).arg((int)ids_.size());
2940 
2941  totalCurve->addValue(ids_.at(i), obstacles.cols+ground.cols+empty.cols);
2942  emptyCurve->addValue(ids_.at(i), empty.cols);
2943  obstaclesCurve->addValue(ids_.at(i), obstacles.cols);
2944  groundCurve->addValue(ids_.at(i), ground.cols);
2945  }
2946 
2947  progressDialog.appendText(msg);
2948  progressDialog.incrementStep();
2949 
2950  decompressionCurve->addValue(ids_.at(i), decompressionTime);
2951  gridCreationCurve->addValue(ids_.at(i), gridCreationTime);
2952 
2953  if(ids_.size() < 50 || (i+1) % 25 == 0)
2954  {
2955  QApplication::processEvents();
2956  }
2957  }
2958  progressDialog.setValue(progressDialog.maximumSteps());
2959 
2960  if(graphes_.size())
2961  {
2962  update3dView();
2963  sliderIterationsValueChanged((int)graphes_.size()-1);
2964  }
2965  else
2966  {
2967  updateGrid();
2968  }
2969 }
2970 
2971 void DatabaseViewer::regenerateCurrentLocalMaps()
2972 {
2973  UTimer time;
2974  OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
2975 
2976  if(ids_.size() == 0)
2977  {
2978  UWARN("ids_ is empty!");
2979  return;
2980  }
2981 
2982  QSet<int> idsSet;
2983  idsSet.insert(ids_.at(ui_->horizontalSlider_A->value()));
2984  idsSet.insert(ids_.at(ui_->horizontalSlider_B->value()));
2985  QList<int> ids = idsSet.toList();
2986 
2987  rtabmap::ProgressDialog progressDialog(this);
2988  progressDialog.setMaximumSteps(ids.size());
2989  progressDialog.show();
2990 
2991  for(int i =0; i<ids.size(); ++i)
2992  {
2993  generatedLocalMaps_.erase(ids.at(i));
2994  generatedLocalMapsInfo_.erase(ids.at(i));
2995 
2996  SensorData data;
2997  dbDriver_->getNodeData(ids.at(i), data);
2998  data.uncompressData();
2999 
3000  int mapId, weight;
3001  Transform odomPose, groundTruth;
3002  std::string label;
3003  double stamp;
3004  QString msg;
3005  std::vector<float> velocity;
3006  GPS gps;
3007  if(dbDriver_->getNodeInfo(data.id(), odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps))
3008  {
3009  Signature s = data;
3010  s.setPose(odomPose);
3011  cv::Mat ground, obstacles, empty;
3012  cv::Point3f viewpoint;
3013 
3014  if(ui_->checkBox_grid_regenerateFromSavedGrid->isChecked() && s.sensorData().gridCellSize() > 0.0f)
3015  {
3016  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridObstacleCellsRaw()));
3017  *cloud+=*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridGroundCellsRaw()));
3018 
3019  if(cloud->size())
3020  {
3021  // update viewpoint
3022  if(s.sensorData().cameraModels().size())
3023  {
3024  // average of all local transforms
3025  float sum = 0;
3026  for(unsigned int i=0; i<s.sensorData().cameraModels().size(); ++i)
3027  {
3028  const Transform & t = s.sensorData().cameraModels()[i].localTransform();
3029  if(!t.isNull())
3030  {
3031  viewpoint.x += t.x();
3032  viewpoint.y += t.y();
3033  viewpoint.z += t.z();
3034  sum += 1.0f;
3035  }
3036  }
3037  if(sum > 0.0f)
3038  {
3039  viewpoint.x /= sum;
3040  viewpoint.y /= sum;
3041  viewpoint.z /= sum;
3042  }
3043  }
3044  else
3045  {
3046  const Transform & t = s.sensorData().stereoCameraModel().localTransform();
3047  viewpoint = cv::Point3f(t.x(), t.y(), t.z());
3048  }
3049 
3050  grid.createLocalMap(LaserScan::backwardCompatibility(util3d::laserScanFromPointCloud(*cloud)), s.getPose(), ground, obstacles, empty, viewpoint);
3051  }
3052  }
3053  else
3054  {
3055  grid.createLocalMap(s, ground, obstacles, empty, viewpoint);
3056  }
3057 
3058 
3059  uInsert(generatedLocalMaps_, std::make_pair(data.id(), std::make_pair(std::make_pair(ground, obstacles),empty)));
3060  uInsert(generatedLocalMapsInfo_, std::make_pair(data.id(), std::make_pair(grid.getCellSize(), viewpoint)));
3061  msg = QString("Generated local occupancy grid map %1/%2 (%3s)").arg(i+1).arg((int)ids.size()).arg(time.ticks());
3062  }
3063 
3064  progressDialog.appendText(msg);
3065  progressDialog.incrementStep();
3066  QApplication::processEvents();
3067  }
3068  progressDialog.setValue(progressDialog.maximumSteps());
3069 
3070  if(graphes_.size())
3071  {
3072  update3dView();
3073  sliderIterationsValueChanged((int)graphes_.size()-1);
3074  }
3075  else
3076  {
3077  updateGrid();
3078  }
3079 }
3080 
3081 void DatabaseViewer::view3DMap()
3082 {
3083  if(!ids_.size() || !dbDriver_)
3084  {
3085  QMessageBox::warning(this, tr("Cannot view 3D map"), tr("The database is empty..."));
3086  return;
3087  }
3088 
3089  if(graphes_.empty())
3090  {
3091  this->updateGraphView();
3092  if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
3093  {
3094  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
3095  return;
3096  }
3097  }
3098 
3099  std::map<int, Transform> optimizedPoses;
3100  if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty())
3101  {
3102  optimizedPoses = groundTruthPoses_;
3103  }
3104  else
3105  {
3106  optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
3107  }
3108  if(ui_->groupBox_posefiltering->isChecked())
3109  {
3110  optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
3111  ui_->doubleSpinBox_posefilteringRadius->value(),
3112  ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
3113  }
3114  if(optimizedPoses.size() > 0)
3115  {
3116  exportDialog_->setDBDriver(dbDriver_);
3117  exportDialog_->viewClouds(optimizedPoses,
3118  updateLinksWithModifications(links_),
3119  mapIds_,
3120  QMap<int, Signature>(),
3121  std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
3122  std::map<int, LaserScan>(),
3123  pathDatabase_,
3124  ui_->parameters_toolbox->getParameters());
3125  }
3126  else
3127  {
3128  QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
3129  }
3130 }
3131 
3132 void DatabaseViewer::generate3DMap()
3133 {
3134  if(!ids_.size() || !dbDriver_)
3135  {
3136  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
3137  return;
3138  }
3139 
3140  if(graphes_.empty())
3141  {
3142  this->updateGraphView();
3143  if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
3144  {
3145  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
3146  return;
3147  }
3148  }
3149 
3150  std::map<int, Transform> optimizedPoses;
3151  if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty())
3152  {
3153  optimizedPoses = groundTruthPoses_;
3154  }
3155  else
3156  {
3157  optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
3158  }
3159  if(ui_->groupBox_posefiltering->isChecked())
3160  {
3161  optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
3162  ui_->doubleSpinBox_posefilteringRadius->value(),
3163  ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
3164  }
3165  if(optimizedPoses.size() > 0)
3166  {
3167  exportDialog_->setDBDriver(dbDriver_);
3168  exportDialog_->exportClouds(optimizedPoses,
3169  updateLinksWithModifications(links_),
3170  mapIds_,
3171  QMap<int, Signature>(),
3172  std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
3173  std::map<int, LaserScan>(),
3174  pathDatabase_,
3175  ui_->parameters_toolbox->getParameters());
3176  }
3177  else
3178  {
3179  QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
3180  }
3181 }
3182 
3183 void DatabaseViewer::detectMoreLoopClosures()
3184 {
3185  if(graphes_.empty())
3186  {
3187  this->updateGraphView();
3188  if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
3189  {
3190  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
3191  return;
3192  }
3193  }
3194 
3195  const std::map<int, Transform> & optimizedPoses = graphes_.back();
3196 
3197  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
3198  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
3199  progressDialog->setMaximumSteps(1);
3200  progressDialog->setCancelButtonVisible(true);
3201  progressDialog->setMinimumWidth(800);
3202  progressDialog->show();
3203 
3204  int iterations = ui_->spinBox_detectMore_iterations->value();
3205  UASSERT(iterations > 0);
3206  int added = 0;
3207  std::multimap<int, int> checkedLoopClosures;
3208  std::pair<int, int> lastAdded(0,0);
3209  for(int n=0; n<iterations; ++n)
3210  {
3211  UINFO("iteration %d/%d", n+1, iterations);
3212  std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(
3213  optimizedPoses,
3214  ui_->doubleSpinBox_detectMore_radius->value(),
3215  ui_->doubleSpinBox_detectMore_angle->value()*CV_PI/180.0);
3216 
3217  progressDialog->setMaximumSteps(progressDialog->maximumSteps()+(int)clusters.size());
3218  progressDialog->appendText(tr("Looking for more loop closures, clusters found %1 clusters.").arg(clusters.size()));
3219  QApplication::processEvents();
3220  if(progressDialog->isCanceled())
3221  {
3222  break;
3223  }
3224 
3225  std::set<int> addedLinks;
3226  int i=0;
3227  for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end() && !progressDialog->isCanceled(); ++iter, ++i)
3228  {
3229  int from = iter->first;
3230  int to = iter->second;
3231  if(from < to)
3232  {
3233  from = iter->second;
3234  to = iter->first;
3235  }
3236 
3237  // only add new links and one per cluster per iteration
3238  if(rtabmap::graph::findLink(checkedLoopClosures, from, to) == checkedLoopClosures.end())
3239  {
3240  if(!findActiveLink(from, to).isValid() && !containsLink(linksRemoved_, from, to) &&
3241  addedLinks.find(from) == addedLinks.end() && addedLinks.find(to) == addedLinks.end())
3242  {
3243  checkedLoopClosures.insert(std::make_pair(from, to));
3244  if(addConstraint(from, to, true))
3245  {
3246  UINFO("Added new loop closure between %d and %d.", from, to);
3247  ++added;
3248  addedLinks.insert(from);
3249  addedLinks.insert(to);
3250  lastAdded.first = from;
3251  lastAdded.second = to;
3252 
3253  progressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
3254  QApplication::processEvents();
3255  }
3256  }
3257  }
3258  progressDialog->incrementStep();
3259  if(i%100)
3260  {
3261  QApplication::processEvents();
3262  }
3263  }
3264  UINFO("Iteration %d/%d: added %d loop closures.", n+1, iterations, (int)addedLinks.size()/2);
3265  progressDialog->appendText(tr("Iteration %1/%2: Detected %3 loop closures!").arg(n+1).arg(iterations).arg(addedLinks.size()/2));
3266  if(addedLinks.size() == 0)
3267  {
3268  break;
3269  }
3270  }
3271 
3272  if(added)
3273  {
3274  this->updateGraphView();
3275  this->updateLoopClosuresSlider(lastAdded.first, lastAdded.second);
3276  }
3277  UINFO("Total added %d loop closures.", added);
3278 
3279  progressDialog->appendText(tr("Total new loop closures detected=%1").arg(added));
3280  progressDialog->setValue(progressDialog->maximumSteps());
3281 }
3282 
3283 void DatabaseViewer::refineAllNeighborLinks()
3284 {
3285  if(neighborLinks_.size())
3286  {
3287  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
3288  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
3289  progressDialog->setMaximumSteps(neighborLinks_.size());
3290  progressDialog->setCancelButtonVisible(true);
3291  progressDialog->setMinimumWidth(800);
3292  progressDialog->show();
3293 
3294  for(int i=0; i<neighborLinks_.size(); ++i)
3295  {
3296  int from = neighborLinks_[i].from();
3297  int to = neighborLinks_[i].to();
3298  this->refineConstraint(neighborLinks_[i].from(), neighborLinks_[i].to(), true);
3299 
3300  progressDialog->appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(neighborLinks_.size()));
3301  progressDialog->incrementStep();
3302  QApplication::processEvents();
3303  if(progressDialog->isCanceled())
3304  {
3305  break;
3306  }
3307  }
3308  this->updateGraphView();
3309 
3310  progressDialog->setValue(progressDialog->maximumSteps());
3311  progressDialog->appendText("Refining links finished!");
3312  }
3313 }
3314 
3315 void DatabaseViewer::refineAllLoopClosureLinks()
3316 {
3317  if(loopLinks_.size())
3318  {
3319  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
3320  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
3321  progressDialog->setMaximumSteps(loopLinks_.size());
3322  progressDialog->setCancelButtonVisible(true);
3323  progressDialog->setMinimumWidth(800);
3324  progressDialog->show();
3325 
3326  for(int i=0; i<loopLinks_.size(); ++i)
3327  {
3328  int from = loopLinks_[i].from();
3329  int to = loopLinks_[i].to();
3330  this->refineConstraint(loopLinks_[i].from(), loopLinks_[i].to(), true);
3331 
3332  progressDialog->appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(loopLinks_.size()));
3333  progressDialog->incrementStep();
3334  QApplication::processEvents();
3335  if(progressDialog->isCanceled())
3336  {
3337  break;
3338  }
3339  }
3340  this->updateGraphView();
3341 
3342  progressDialog->setValue(progressDialog->maximumSteps());
3343  progressDialog->appendText("Refining links finished!");
3344  }
3345 }
3346 
3347 void DatabaseViewer::resetAllChanges()
3348 {
3349  linksAdded_.clear();
3350  linksRefined_.clear();
3351  linksRemoved_.clear();
3352  generatedLocalMaps_.clear();
3353  generatedLocalMapsInfo_.clear();
3354  updateLoopClosuresSlider();
3355  this->updateGraphView();
3356 }
3357 
3358 void DatabaseViewer::sliderAValueChanged(int value)
3359 {
3360  this->update(value,
3361  ui_->label_indexA,
3362  ui_->label_parentsA,
3363  ui_->label_childrenA,
3364  ui_->label_weightA,
3365  ui_->label_labelA,
3366  ui_->label_stampA,
3367  ui_->graphicsView_A,
3368  ui_->label_idA,
3369  ui_->label_mapA,
3370  ui_->label_poseA,
3371  ui_->label_velA,
3372  ui_->label_calibA,
3373  ui_->label_gpsA,
3374  true);
3375 }
3376 
3377 void DatabaseViewer::sliderBValueChanged(int value)
3378 {
3379  this->update(value,
3380  ui_->label_indexB,
3381  ui_->label_parentsB,
3382  ui_->label_childrenB,
3383  ui_->label_weightB,
3384  ui_->label_labelB,
3385  ui_->label_stampB,
3386  ui_->graphicsView_B,
3387  ui_->label_idB,
3388  ui_->label_mapB,
3389  ui_->label_poseB,
3390  ui_->label_velB,
3391  ui_->label_calibB,
3392  ui_->label_gpsB,
3393  true);
3394 }
3395 
3396 void DatabaseViewer::update(int value,
3397  QLabel * labelIndex,
3398  QLabel * labelParents,
3399  QLabel * labelChildren,
3400  QLabel * weight,
3401  QLabel * label,
3402  QLabel * stamp,
3403  rtabmap::ImageView * view,
3404  QLabel * labelId,
3405  QLabel * labelMapId,
3406  QLabel * labelPose,
3407  QLabel * labelVelocity,
3408  QLabel * labelCalib,
3409  QLabel * labelGps,
3410  bool updateConstraintView)
3411 {
3412  UTimer timer;
3413  labelIndex->setText(QString::number(value));
3414  labelParents->clear();
3415  labelChildren->clear();
3416  weight->clear();
3417  label->clear();
3418  labelMapId->clear();
3419  labelPose->clear();
3420  labelVelocity->clear();
3421  stamp->clear();
3422  labelCalib->clear();
3423  labelGps->clear();
3424  QRectF rect;
3425  if(value >= 0 && value < ids_.size())
3426  {
3427  view->clear();
3428  int id = ids_.at(value);
3429  int mapId = -1;
3430  labelId->setText(QString::number(id));
3431  if(id>0)
3432  {
3433  //image
3434  QImage img;
3435  cv::Mat imgDepth;
3436  if(dbDriver_)
3437  {
3438  SensorData data;
3439  dbDriver_->getNodeData(id, data);
3440  data.uncompressData();
3441  if(!data.imageRaw().empty())
3442  {
3443  img = uCvMat2QImage(ui_->label_indexB==labelIndex?data.imageRaw():data.imageRaw());
3444  }
3445  if(!data.depthOrRightRaw().empty())
3446  {
3447  cv::Mat depth =data.depthOrRightRaw();
3448  if(!data.depthRaw().empty())
3449  {
3450  if(ui_->spinBox_mesh_fillDepthHoles->value() > 0)
3451  {
3452  depth = util2d::fillDepthHoles(depth, ui_->spinBox_mesh_fillDepthHoles->value(), float(ui_->spinBox_mesh_depthError->value())/100.0f);
3453  }
3454  }
3455  imgDepth = depth;
3456  }
3457 
3458  std::list<int> ids;
3459  ids.push_back(id);
3460  std::list<Signature*> signatures;
3461  dbDriver_->loadSignatures(ids, signatures);
3462 
3463  if(signatures.size() && signatures.front()!=0 && signatures.front()->getWords().size())
3464  {
3465  view->setFeatures(signatures.front()->getWords(), data.depthOrRightRaw().type() == CV_8UC1?cv::Mat():data.depthOrRightRaw(), Qt::yellow);
3466  }
3467 
3468  Transform odomPose, g;
3469  int w;
3470  std::string l;
3471  double s;
3472  std::vector<float> v;
3473  GPS gps;
3474  dbDriver_->getNodeInfo(id, odomPose, mapId, w, l, s, g, v, gps);
3475 
3476  weight->setNum(w);
3477  label->setText(l.c_str());
3478  float x,y,z,roll,pitch,yaw;
3479  odomPose.getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw);
3480  labelPose->setText(QString("%1xyz=(%2,%3,%4)\nrpy=(%5,%6,%7)").arg(odomPose.isIdentity()?"* ":"").arg(x).arg(y).arg(z).arg(roll).arg(pitch).arg(yaw));
3481  if(s!=0.0)
3482  {
3483  stamp->setText(QString::number(s, 'f'));
3484  stamp->setToolTip(QDateTime::fromMSecsSinceEpoch(s*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
3485  }
3486  if(v.size()==6)
3487  {
3488  labelVelocity->setText(QString("vx=%1 vy=%2 vz=%3 vroll=%4 vpitch=%5 vyaw=%6").arg(v[0]).arg(v[1]).arg(v[2]).arg(v[3]).arg(v[4]).arg(v[5]));
3489  }
3490  if(gps.stamp()>0.0)
3491  {
3492  labelGps->setText(QString("stamp=%1 longitude=%2 latitude=%3 altitude=%4m error=%5m bearing=%6deg").arg(QString::number(gps.stamp(), 'f')).arg(gps.longitude()).arg(gps.latitude()).arg(gps.altitude()).arg(gps.error()).arg(gps.bearing()));
3493  labelGps->setToolTip(QDateTime::fromMSecsSinceEpoch(gps.stamp()*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
3494  }
3495  if(data.cameraModels().size() || data.stereoCameraModel().isValidForProjection())
3496  {
3497  if(data.cameraModels().size())
3498  {
3499  if(!data.depthRaw().empty() && data.depthRaw().cols!=data.imageRaw().cols && data.imageRaw().cols)
3500  {
3501  labelCalib->setText(tr("%1 %2x%3 [%8x%9] fx=%4 fy=%5 cx=%6 cy=%7 T=%10")
3502  .arg(data.cameraModels().size())
3503  .arg(data.cameraModels()[0].imageWidth()>0?data.cameraModels()[0].imageWidth():data.imageRaw().cols/data.cameraModels().size())
3504  .arg(data.cameraModels()[0].imageHeight()>0?data.cameraModels()[0].imageHeight():data.imageRaw().rows)
3505  .arg(data.cameraModels()[0].fx())
3506  .arg(data.cameraModels()[0].fy())
3507  .arg(data.cameraModels()[0].cx())
3508  .arg(data.cameraModels()[0].cy())
3509  .arg(data.depthRaw().cols/data.cameraModels().size())
3510  .arg(data.depthRaw().rows)
3511  .arg(data.cameraModels()[0].localTransform().prettyPrint().c_str()));
3512  }
3513  else
3514  {
3515  labelCalib->setText(tr("%1 %2x%3 fx=%4 fy=%5 cx=%6 cy=%7 T=%8")
3516  .arg(data.cameraModels().size())
3517  .arg(data.cameraModels()[0].imageWidth()>0?data.cameraModels()[0].imageWidth():data.imageRaw().cols/data.cameraModels().size())
3518  .arg(data.cameraModels()[0].imageHeight()>0?data.cameraModels()[0].imageHeight():data.imageRaw().rows)
3519  .arg(data.cameraModels()[0].fx())
3520  .arg(data.cameraModels()[0].fy())
3521  .arg(data.cameraModels()[0].cx())
3522  .arg(data.cameraModels()[0].cy())
3523  .arg(data.cameraModels()[0].localTransform().prettyPrint().c_str()));
3524  }
3525  }
3526  else
3527  {
3528  //stereo
3529  labelCalib->setText(tr("%1x%2 fx=%3 fy=%4 cx=%5 cy=%6 baseline=%7m T=%8")
3530  .arg(data.stereoCameraModel().left().imageWidth()>0?data.stereoCameraModel().left().imageWidth():data.imageRaw().cols)
3531  .arg(data.stereoCameraModel().left().imageHeight()>0?data.stereoCameraModel().left().imageHeight():data.imageRaw().rows)
3532  .arg(data.stereoCameraModel().left().fx())
3533  .arg(data.stereoCameraModel().left().fy())
3534  .arg(data.stereoCameraModel().left().cx())
3535  .arg(data.stereoCameraModel().left().cy())
3536  .arg(data.stereoCameraModel().baseline())
3537  .arg(data.stereoCameraModel().localTransform().prettyPrint().c_str()));
3538  }
3539 
3540  }
3541  else
3542  {
3543  labelCalib->setText("NA");
3544  }
3545 
3546  //stereo
3547  if(!data.depthOrRightRaw().empty() && data.depthOrRightRaw().type() == CV_8UC1)
3548  {
3549  this->updateStereo(&data);
3550  }
3551  else
3552  {
3553  stereoViewer_->clear();
3554  ui_->graphicsView_stereo->clear();
3555  }
3556 
3557  // 3d view
3558  if(cloudViewer_->isVisible())
3559  {
3560  Transform pose = Transform::getIdentity();
3561  if(signatures.size() && ui_->checkBox_odomFrame_3dview->isChecked())
3562  {
3563  float x, y, z, roll, pitch, yaw;
3564  (*signatures.begin())->getPose().getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
3565  pose = Transform(0,0,z,roll,pitch,0);
3566  }
3567 
3568  cloudViewer_->removeAllFrustums();
3569  cloudViewer_->removeCloud("mesh");
3570  cloudViewer_->removeCloud("cloud");
3571  cloudViewer_->removeCloud("scan");
3572  cloudViewer_->removeCloud("map");
3573  cloudViewer_->removeCloud("ground");
3574  cloudViewer_->removeCloud("obstacles");
3575  cloudViewer_->removeCloud("empty_cells");
3576  cloudViewer_->removeCloud("words");
3577  cloudViewer_->removeOctomap();
3578  if(ui_->checkBox_showCloud->isChecked() || ui_->checkBox_showMesh->isChecked())
3579  {
3580  if(!data.depthOrRightRaw().empty())
3581  {
3582  if(!data.imageRaw().empty())
3583  {
3584  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
3585  pcl::IndicesPtr indices(new std::vector<int>);
3586  if(!data.depthRaw().empty() && data.cameraModels().size()==1)
3587  {
3588  cv::Mat depth = data.depthRaw();
3589  if(ui_->spinBox_mesh_fillDepthHoles->value() > 0)
3590  {
3591  depth = util2d::fillDepthHoles(depth, ui_->spinBox_mesh_fillDepthHoles->value(), float(ui_->spinBox_mesh_depthError->value())/100.0f);
3592  }
3593  cloud = util3d::cloudFromDepthRGB(
3594  data.imageRaw(),
3595  depth,
3596  data.cameraModels()[0],
3597  ui_->spinBox_decimation->value(),0,0,indices.get());
3598  if(indices->size())
3599  {
3600  cloud = util3d::transformPointCloud(cloud, data.cameraModels()[0].localTransform());
3601  }
3602 
3603  }
3604  else
3605  {
3606  cloud = util3d::cloudRGBFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, indices.get(), ui_->parameters_toolbox->getParameters());
3607  }
3608  if(indices->size())
3609  {
3610  if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
3611  {
3612  cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value());
3613  }
3614 
3615  if(ui_->checkBox_showMesh->isChecked() && !cloud->is_dense)
3616  {
3617  Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
3618  if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
3619  {
3620  viewpoint[0] = data.cameraModels()[0].localTransform().x();
3621  viewpoint[1] = data.cameraModels()[0].localTransform().y();
3622  viewpoint[2] = data.cameraModels()[0].localTransform().z();
3623  }
3624  else if(!data.stereoCameraModel().localTransform().isNull())
3625  {
3626  viewpoint[0] = data.stereoCameraModel().localTransform().x();
3627  viewpoint[1] = data.stereoCameraModel().localTransform().y();
3628  viewpoint[2] = data.stereoCameraModel().localTransform().z();
3629  }
3630  std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
3631  cloud,
3632  float(ui_->spinBox_mesh_angleTolerance->value())*M_PI/180.0f,
3633  ui_->checkBox_mesh_quad->isChecked(),
3634  ui_->spinBox_mesh_triangleSize->value(),
3635  viewpoint);
3636 
3637  if(ui_->spinBox_mesh_minClusterSize->value())
3638  {
3639  // filter polygons
3640  std::vector<std::set<int> > neighbors;
3641  std::vector<std::set<int> > vertexToPolygons;
3642  util3d::createPolygonIndexes(polygons,
3643  cloud->size(),
3644  neighbors,
3645  vertexToPolygons);
3646  std::list<std::list<int> > clusters = util3d::clusterPolygons(
3647  neighbors,
3648  ui_->spinBox_mesh_minClusterSize->value());
3649  std::vector<pcl::Vertices> filteredPolygons(polygons.size());
3650  int oi=0;
3651  for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
3652  {
3653  for(std::list<int>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
3654  {
3655  filteredPolygons[oi++] = polygons.at(*jter);
3656  }
3657  }
3658  filteredPolygons.resize(oi);
3659  polygons = filteredPolygons;
3660  }
3661 
3662  cloudViewer_->addCloudMesh("mesh", cloud, polygons, pose);
3663  }
3664  if(ui_->checkBox_showCloud->isChecked())
3665  {
3666  cloudViewer_->addCloud("cloud", cloud, pose);
3667  }
3668  }
3669  }
3670  else if(ui_->checkBox_showCloud->isChecked())
3671  {
3672  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
3673  pcl::IndicesPtr indices(new std::vector<int>);
3674  cloud = util3d::cloudFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, indices.get(), ui_->parameters_toolbox->getParameters());
3675  if(indices->size())
3676  {
3677  if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
3678  {
3679  cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value());
3680  }
3681 
3682  cloudViewer_->addCloud("cloud", cloud, pose);
3683  }
3684  }
3685  }
3686  }
3687 
3688  //frustums
3689  if(cloudViewer_->isFrustumShown())
3690  {
3691  if(data.cameraModels().size())
3692  {
3693  cloudViewer_->updateCameraFrustums(pose, data.cameraModels());
3694  }
3695  else
3696  {
3697  cloudViewer_->updateCameraFrustum(pose, data.stereoCameraModel());
3698  }
3699  }
3700 
3701  //words
3702  if(ui_->checkBox_showWords->isChecked() && signatures.size())
3703  {
3704  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
3705  cloud->resize((*signatures.begin())->getWords3().size());
3706  int i=0;
3707  for(std::multimap<int, cv::Point3f>::const_iterator iter=(*signatures.begin())->getWords3().begin();
3708  iter!=(*signatures.begin())->getWords3().end();
3709  ++iter)
3710  {
3711  cloud->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
3712  }
3713 
3714  if(cloud->size())
3715  {
3716  cloud = rtabmap::util3d::removeNaNFromPointCloud(cloud);
3717  }
3718 
3719  if(cloud->size())
3720  {
3721  cloudViewer_->addCloud("words", cloud, pose, Qt::red);
3722  }
3723  }
3724 
3725  //add scan
3726  if(ui_->checkBox_showScan->isChecked() && data.laserScanRaw().size())
3727  {
3728  if(data.laserScanRaw().hasRGB() && data.laserScanRaw().hasNormals())
3729  {
3730  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr scan = util3d::laserScanToPointCloudRGBNormal(data.laserScanRaw(), data.laserScanRaw().localTransform());
3731  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
3732  }
3733  else if(data.laserScanRaw().hasIntensity() && data.laserScanRaw().hasNormals())
3734  {
3735  pcl::PointCloud<pcl::PointXYZINormal>::Ptr scan = util3d::laserScanToPointCloudINormal(data.laserScanRaw(), data.laserScanRaw().localTransform());
3736  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
3737  }
3738  else if(data.laserScanRaw().hasNormals())
3739  {
3740  pcl::PointCloud<pcl::PointNormal>::Ptr scan = util3d::laserScanToPointCloudNormal(data.laserScanRaw(), data.laserScanRaw().localTransform());
3741  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
3742  }
3743  else if(data.laserScanRaw().hasRGB())
3744  {
3745  pcl::PointCloud<pcl::PointXYZRGB>::Ptr scan = util3d::laserScanToPointCloudRGB(data.laserScanRaw(), data.laserScanRaw().localTransform());
3746  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
3747  }
3748  else
3749  {
3750  pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(data.laserScanRaw(), data.laserScanRaw().localTransform());
3751  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
3752  }
3753  }
3754 
3755  //add occupancy grid
3756  if(ui_->checkBox_showMap->isChecked() || ui_->checkBox_showGrid->isChecked())
3757  {
3758  std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > localMaps;
3759  std::map<int, std::pair<float, cv::Point3f> > localMapsInfo;
3760  if(generatedLocalMaps_.find(data.id()) != generatedLocalMaps_.end())
3761  {
3762  localMaps.insert(*generatedLocalMaps_.find(data.id()));
3763  localMapsInfo.insert(*generatedLocalMapsInfo_.find(data.id()));
3764  }
3765  else if(!data.gridGroundCellsRaw().empty() || !data.gridObstacleCellsRaw().empty())
3766  {
3767  localMaps.insert(std::make_pair(data.id(), std::make_pair(std::make_pair(data.gridGroundCellsRaw(), data.gridObstacleCellsRaw()), data.gridEmptyCellsRaw())));
3768  localMapsInfo.insert(std::make_pair(data.id(), std::make_pair(data.gridCellSize(), data.gridViewPoint())));
3769  }
3770  if(!localMaps.empty())
3771  {
3772  std::map<int, Transform> poses;
3773  poses.insert(std::make_pair(data.id(), pose));
3774 
3775 #ifdef RTABMAP_OCTOMAP
3776  OctoMap * octomap = 0;
3777  if(ui_->checkBox_octomap->isChecked() &&
3778  (!localMaps.begin()->second.first.first.empty() || !localMaps.begin()->second.first.second.empty()) &&
3779  (localMaps.begin()->second.first.first.empty() || localMaps.begin()->second.first.first.channels() > 2) &&
3780  (localMaps.begin()->second.first.second.empty() || localMaps.begin()->second.first.second.channels() > 2) &&
3781  (localMaps.begin()->second.second.empty() || localMaps.begin()->second.second.channels() > 2) &&
3782  localMapsInfo.begin()->second.first > 0.0f)
3783  {
3784  //create local octomap
3785  octomap = new OctoMap(localMapsInfo.begin()->second.first);
3786  octomap->addToCache(data.id(), localMaps.begin()->second.first.first, localMaps.begin()->second.first.second, localMaps.begin()->second.second, localMapsInfo.begin()->second.second);
3787  octomap->update(poses);
3788  }
3789 #endif
3790 
3791  if(ui_->checkBox_showMap->isChecked())
3792  {
3793  float xMin=0.0f, yMin=0.0f;
3794  cv::Mat map8S;
3795  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
3796  float gridCellSize = Parameters::defaultGridCellSize();
3797  Parameters::parse(parameters, Parameters::kGridCellSize(), gridCellSize);
3798 #ifdef RTABMAP_OCTOMAP
3799  if(octomap)
3800  {
3801  map8S = octomap->createProjectionMap(xMin, yMin, gridCellSize, 0);
3802  }
3803  else
3804 #endif
3805  {
3806  OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
3807  grid.setCellSize(gridCellSize);
3808  grid.addToCache(data.id(), localMaps.begin()->second.first.first, localMaps.begin()->second.first.second, localMaps.begin()->second.second);
3809  grid.update(poses);
3810  map8S = grid.getMap(xMin, yMin);
3811  }
3812  if(!map8S.empty())
3813  {
3814  //convert to gray scaled map
3815  cloudViewer_->addOccupancyGridMap(util3d::convertMap2Image8U(map8S), gridCellSize, xMin, yMin, 1);
3816  }
3817  }
3818 
3819  if(ui_->checkBox_showGrid->isChecked())
3820  {
3821 #ifdef RTABMAP_OCTOMAP
3822  if(octomap)
3823  {
3824  if(ui_->comboBox_octomap_rendering_type->currentIndex()== 0)
3825  {
3826  pcl::IndicesPtr obstacles(new std::vector<int>);
3827  pcl::IndicesPtr empty(new std::vector<int>);
3828  pcl::IndicesPtr ground(new std::vector<int>);
3829  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(ui_->spinBox_grid_depth->value(), obstacles.get(), empty.get(), ground.get());
3830  if(octomap->hasColor())
3831  {
3832  pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
3833  pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
3834  cloudViewer_->addCloud("obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
3835  cloudViewer_->setCloudPointSize("obstacles", 5);
3836 
3837  pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
3838  pcl::copyPointCloud(*cloud, *ground, *groundCloud);
3839  cloudViewer_->addCloud("ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
3840  cloudViewer_->setCloudPointSize("ground", 5);
3841  }
3842  else
3843  {
3844  pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
3845  pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
3846  cloudViewer_->addCloud("obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
3847  cloudViewer_->setCloudPointSize("obstacles", 5);
3848 
3849  pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
3850  pcl::copyPointCloud(*cloud, *ground, *groundCloud);
3851  cloudViewer_->addCloud("ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
3852  cloudViewer_->setCloudPointSize("ground", 5);
3853  }
3854 
3855  if(ui_->checkBox_grid_empty->isChecked())
3856  {
3857  pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZ>);
3858  pcl::copyPointCloud(*cloud, *empty, *emptyCloud);
3859  cloudViewer_->addCloud("empty_cells", emptyCloud, Transform::getIdentity(), QColor(ui_->lineEdit_emptyColor->text()));
3860  cloudViewer_->setCloudOpacity("empty_cells", 0.5);
3861  cloudViewer_->setCloudPointSize("empty_cells", 5);
3862  }
3863  }
3864  else
3865  {
3866  cloudViewer_->addOctomap(octomap, ui_->spinBox_grid_depth->value(), ui_->comboBox_octomap_rendering_type->currentIndex()>1);
3867  }
3868  }
3869  else
3870 #endif
3871  {
3872  // occupancy cloud
3873  LaserScan scan = LaserScan::backwardCompatibility(localMaps.begin()->second.first.first);
3874  if(scan.hasRGB())
3875  {
3876  cloudViewer_->addCloud("ground", util3d::laserScanToPointCloudRGB(scan), pose, QColor(ui_->lineEdit_groundColor->text()));
3877  }
3878  else
3879  {
3880  cloudViewer_->addCloud("ground", util3d::laserScanToPointCloud(scan), pose, QColor(ui_->lineEdit_groundColor->text()));
3881  }
3882  scan = LaserScan::backwardCompatibility(localMaps.begin()->second.first.second);
3883  if(scan.hasRGB())
3884  {
3885  cloudViewer_->addCloud("obstacles", util3d::laserScanToPointCloudRGB(scan), pose, QColor(ui_->lineEdit_obstacleColor->text()));
3886  }
3887  else
3888  {
3889  cloudViewer_->addCloud("obstacles", util3d::laserScanToPointCloud(scan), pose, QColor(ui_->lineEdit_obstacleColor->text()));
3890  }
3891 
3892  cloudViewer_->setCloudPointSize("ground", 5);
3893  cloudViewer_->setCloudPointSize("obstacles", 5);
3894 
3895  if(ui_->checkBox_grid_empty->isChecked())
3896  {
3897  cloudViewer_->addCloud("empty_cells",
3898  util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(localMaps.begin()->second.second)),
3899  pose,
3900  QColor(ui_->lineEdit_emptyColor->text()));
3901  cloudViewer_->setCloudPointSize("empty_cells", 5);
3902  cloudViewer_->setCloudOpacity("empty_cells", 0.5);
3903  }
3904  }
3905  }
3906 #ifdef RTABMAP_OCTOMAP
3907  if(octomap)
3908  {
3909  delete octomap;
3910  }
3911 #endif
3912  }
3913  }
3914  cloudViewer_->update();
3915  }
3916 
3917  if(signatures.size())
3918  {
3919  UASSERT(signatures.front() != 0 && signatures.size() == 1);
3920  delete signatures.front();
3921  signatures.clear();
3922  }
3923  }
3924 
3925  if(!img.isNull())
3926  {
3927  view->setImage(img);
3928  rect = img.rect();
3929  }
3930  else
3931  {
3932  ULOGGER_DEBUG("Image is empty");
3933  }
3934 
3935  if(!imgDepth.empty())
3936  {
3937  view->setImageDepth(imgDepth);
3938  if(img.isNull())
3939  {
3940  rect.setWidth(imgDepth.cols);
3941  rect.setHeight(imgDepth.rows);
3942  }
3943  }
3944  else
3945  {
3946  ULOGGER_DEBUG("Image depth is empty");
3947  }
3948 
3949  // loops
3950  std::map<int, rtabmap::Link> links;
3951  dbDriver_->loadLinks(id, links);
3952  if(links.size())
3953  {
3954  QString strParents, strChildren;
3955  for(std::map<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
3956  {
3957  if(iter->second.type() != Link::kNeighbor &&
3958  iter->second.type() != Link::kNeighborMerged)
3959  {
3960  if(iter->first < id)
3961  {
3962  strChildren.append(QString("%1 ").arg(iter->first));
3963  }
3964  else
3965  {
3966  strParents.append(QString("%1 ").arg(iter->first));
3967  }
3968  }
3969  }
3970  labelParents->setText(strParents);
3971  labelChildren->setText(strChildren);
3972  }
3973  }
3974 
3975  if(mapId>=0)
3976  {
3977  labelMapId->setText(QString::number(mapId));
3978  }
3979  }
3980  else if(value != 0)
3981  {
3982  ULOGGER_ERROR("Slider index out of range ?");
3983  }
3984 
3985  updateConstraintButtons();
3986  updateWordsMatching();
3987 
3988  if(updateConstraintView && ui_->dockWidget_constraints->isVisible())
3989  {
3990  // update constraint view
3991  int from = ids_.at(ui_->horizontalSlider_A->value());
3992  int to = ids_.at(ui_->horizontalSlider_B->value());
3993  bool set = false;
3994  for(int i=0; i<loopLinks_.size() || i<neighborLinks_.size(); ++i)
3995  {
3996  if(i < loopLinks_.size())
3997  {
3998  if((loopLinks_[i].from() == from && loopLinks_[i].to() == to) ||
3999  (loopLinks_[i].from() == to && loopLinks_[i].to() == from))
4000  {
4001  if(i != ui_->horizontalSlider_loops->value())
4002  {
4003  ui_->horizontalSlider_loops->blockSignals(true);
4004  ui_->horizontalSlider_loops->setValue(i);
4005  ui_->horizontalSlider_loops->blockSignals(false);
4006  this->updateConstraintView(loopLinks_[i].from() == from?loopLinks_.at(i):loopLinks_.at(i).inverse(), false);
4007  }
4008  ui_->horizontalSlider_neighbors->blockSignals(true);
4009  ui_->horizontalSlider_neighbors->setValue(0);
4010  ui_->horizontalSlider_neighbors->blockSignals(false);
4011  set = true;
4012  break;
4013  }
4014  }
4015  if(i < neighborLinks_.size())
4016  {
4017  if((neighborLinks_[i].from() == from && neighborLinks_[i].to() == to) ||
4018  (neighborLinks_[i].from() == to && neighborLinks_[i].to() == from))
4019  {
4020  if(i != ui_->horizontalSlider_neighbors->value())
4021  {
4022  ui_->horizontalSlider_neighbors->blockSignals(true);
4023  ui_->horizontalSlider_neighbors->setValue(i);
4024  ui_->horizontalSlider_neighbors->blockSignals(false);
4025  this->updateConstraintView(neighborLinks_[i].from() == from?neighborLinks_.at(i):neighborLinks_.at(i).inverse(), false);
4026  }
4027  ui_->horizontalSlider_loops->blockSignals(true);
4028  ui_->horizontalSlider_loops->setValue(0);
4029  ui_->horizontalSlider_loops->blockSignals(false);
4030  set = true;
4031  break;
4032  }
4033  }
4034  }
4035  if(!set)
4036  {
4037  ui_->horizontalSlider_loops->blockSignals(true);
4038  ui_->horizontalSlider_neighbors->blockSignals(true);
4039  ui_->horizontalSlider_loops->setValue(0);
4040  ui_->horizontalSlider_neighbors->setValue(0);
4041  ui_->horizontalSlider_loops->blockSignals(false);
4042  ui_->horizontalSlider_neighbors->blockSignals(false);
4043 
4044  constraintsViewer_->removeAllClouds();
4045 
4046  // make a fake link using globally optimized poses
4047  if(graphes_.size())
4048  {
4049  std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
4050  if(optimizedPoses.size() > 0)
4051  {
4052  std::map<int, Transform>::iterator fromIter = optimizedPoses.find(from);
4053  std::map<int, Transform>::iterator toIter = optimizedPoses.find(to);
4054  if(fromIter != optimizedPoses.end() &&
4055  toIter != optimizedPoses.end())
4056  {
4057  Link link(from, to, Link::kUndef, fromIter->second.inverse() * toIter->second);
4058  this->updateConstraintView(link, false);
4059  }
4060  }
4061  }
4062 
4063  constraintsViewer_->update();
4064 
4065  }
4066  }
4067 
4068  if(rect.isValid())
4069  {
4070  view->setSceneRect(rect);
4071  }
4072 }
4073 
4074 void DatabaseViewer::updateLoggerLevel()
4075 {
4076  if(this->parent() == 0)
4077  {
4078  ULogger::setLevel((ULogger::Level)ui_->comboBox_logger_level->currentIndex());
4079  }
4080 }
4081 
4082 void DatabaseViewer::updateStereo()
4083 {
4084  if(ui_->horizontalSlider_A->maximum())
4085  {
4086  int id = ids_.at(ui_->horizontalSlider_A->value());
4087  SensorData data;
4088  dbDriver_->getNodeData(id, data);
4089  data.uncompressData();
4090  updateStereo(&data);
4091  }
4092 }
4093 
4094 void DatabaseViewer::updateStereo(const SensorData * data)
4095 {
4096  if(data &&
4097  ui_->dockWidget_stereoView->isVisible() &&
4098  !data->imageRaw().empty() &&
4099  !data->depthOrRightRaw().empty() &&
4100  data->depthOrRightRaw().type() == CV_8UC1 &&
4101  data->stereoCameraModel().isValidForProjection())
4102  {
4103  cv::Mat leftMono;
4104  if(data->imageRaw().channels() == 3)
4105  {
4106  cv::cvtColor(data->imageRaw(), leftMono, CV_BGR2GRAY);
4107  }
4108  else
4109  {
4110  leftMono = data->imageRaw();
4111  }
4112 
4113  UTimer timer;
4114  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
4115  Stereo * stereo = Stereo::create(parameters);
4116 
4117  // generate kpts
4118  std::vector<cv::KeyPoint> kpts;
4119  uInsert(parameters, ParametersPair(Parameters::kKpMaxFeatures(), parameters.at(Parameters::kVisMaxFeatures())));
4120  uInsert(parameters, ParametersPair(Parameters::kKpMinDepth(), parameters.at(Parameters::kVisMinDepth())));
4121  uInsert(parameters, ParametersPair(Parameters::kKpMaxDepth(), parameters.at(Parameters::kVisMaxDepth())));
4122  uInsert(parameters, ParametersPair(Parameters::kKpDetectorStrategy(), parameters.at(Parameters::kVisFeatureType())));
4123  uInsert(parameters, ParametersPair(Parameters::kKpRoiRatios(), parameters.at(Parameters::kVisRoiRatios())));
4124  uInsert(parameters, ParametersPair(Parameters::kKpSubPixEps(), parameters.at(Parameters::kVisSubPixEps())));
4125  uInsert(parameters, ParametersPair(Parameters::kKpSubPixIterations(), parameters.at(Parameters::kVisSubPixIterations())));
4126  uInsert(parameters, ParametersPair(Parameters::kKpSubPixWinSize(), parameters.at(Parameters::kVisSubPixWinSize())));
4127  Feature2D * kptDetector = Feature2D::create(parameters);
4128  kpts = kptDetector->generateKeypoints(leftMono);
4129  delete kptDetector;
4130 
4131  float timeKpt = timer.ticks();
4132 
4133  std::vector<cv::Point2f> leftCorners;
4134  cv::KeyPoint::convert(kpts, leftCorners);
4135 
4136  // Find features in the new right image
4137  std::vector<unsigned char> status;
4138  std::vector<cv::Point2f> rightCorners;
4139 
4140  rightCorners = stereo->computeCorrespondences(
4141  leftMono,
4142  data->rightRaw(),
4143  leftCorners,
4144  status);
4145  delete stereo;
4146 
4147  float timeStereo = timer.ticks();
4148 
4149  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
4150  cloud->resize(kpts.size());
4151  float bad_point = std::numeric_limits<float>::quiet_NaN ();
4152  UASSERT(status.size() == kpts.size());
4153  int oi = 0;
4154  int inliers = 0;
4155  int flowOutliers= 0;
4156  int slopeOutliers= 0;
4157  int negativeDisparityOutliers = 0;
4158  for(unsigned int i=0; i<status.size(); ++i)
4159  {
4160  cv::Point3f pt(bad_point, bad_point, bad_point);
4161  if(status[i])
4162  {
4163  float disparity = leftCorners[i].x - rightCorners[i].x;
4164  if(disparity > 0.0f)
4165  {
4166  cv::Point3f tmpPt = util3d::projectDisparityTo3D(
4167  leftCorners[i],
4168  disparity,
4169  data->stereoCameraModel());
4170 
4171  if(util3d::isFinite(tmpPt))
4172  {
4173  pt = util3d::transformPoint(tmpPt, data->stereoCameraModel().left().localTransform());
4174  status[i] = 100; //blue
4175  ++inliers;
4176  cloud->at(oi++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
4177  }
4178  }
4179  else
4180  {
4181  status[i] = 102; //magenta
4182  ++negativeDisparityOutliers;
4183  }
4184  }
4185  else
4186  {
4187  ++flowOutliers;
4188  }
4189  }
4190  cloud->resize(oi);
4191 
4192  UINFO("correspondences = %d/%d (%f) (time kpt=%fs stereo=%fs)",
4193  (int)cloud->size(), (int)leftCorners.size(), float(cloud->size())/float(leftCorners.size()), timeKpt, timeStereo);
4194 
4195  stereoViewer_->updateCameraTargetPosition(Transform::getIdentity());
4196  stereoViewer_->addCloud("stereo", cloud);
4197  stereoViewer_->update();
4198 
4199  ui_->label_stereo_inliers->setNum(inliers);
4200  ui_->label_stereo_flowOutliers->setNum(flowOutliers);
4201  ui_->label_stereo_slopeOutliers->setNum(slopeOutliers);
4202  ui_->label_stereo_disparityOutliers->setNum(negativeDisparityOutliers);
4203 
4204  std::vector<cv::KeyPoint> rightKpts;
4205  cv::KeyPoint::convert(rightCorners, rightKpts);
4206  std::vector<cv::DMatch> good_matches(kpts.size());
4207  for(unsigned int i=0; i<good_matches.size(); ++i)
4208  {
4209  good_matches[i].trainIdx = i;
4210  good_matches[i].queryIdx = i;
4211  }
4212 
4213 
4214  //
4215  //cv::Mat imageMatches;
4216  //cv::drawMatches( leftMono, kpts, data->getDepthRaw(), rightKpts,
4217  // good_matches, imageMatches, cv::Scalar::all(-1), cv::Scalar::all(-1),
4218  // std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
4219 
4220  //ui_->graphicsView_stereo->setImage(uCvMat2QImage(imageMatches));
4221 
4222  ui_->graphicsView_stereo->clear();
4223  ui_->graphicsView_stereo->setLinesShown(true);
4224  ui_->graphicsView_stereo->setFeaturesShown(false);
4225  ui_->graphicsView_stereo->setImageDepthShown(true);
4226 
4227  ui_->graphicsView_stereo->setImage(uCvMat2QImage(data->imageRaw()));
4228  ui_->graphicsView_stereo->setImageDepth(data->depthOrRightRaw());
4229 
4230  // Draw lines between corresponding features...
4231  for(unsigned int i=0; i<kpts.size(); ++i)
4232  {
4233  if(rightKpts[i].pt.x > 0 && rightKpts[i].pt.y > 0)
4234  {
4235  QColor c = Qt::green;
4236  if(status[i] == 0)
4237  {
4238  c = Qt::red;
4239  }
4240  else if(status[i] == 100)
4241  {
4242  c = Qt::blue;
4243  }
4244  else if(status[i] == 101)
4245  {
4246  c = Qt::yellow;
4247  }
4248  else if(status[i] == 102)
4249  {
4250  c = Qt::magenta;
4251  }
4252  else if(status[i] == 110)
4253  {
4254  c = Qt::cyan;
4255  }
4256  ui_->graphicsView_stereo->addLine(
4257  kpts[i].pt.x,
4258  kpts[i].pt.y,
4259  rightKpts[i].pt.x,
4260  rightKpts[i].pt.y,
4261  c,
4262  QString("%1: (%2,%3) -> (%4,%5) d=%6").arg(i).arg(kpts[i].pt.x).arg(kpts[i].pt.y).arg(rightKpts[i].pt.x).arg(rightKpts[i].pt.y).arg(kpts[i].pt.x - rightKpts[i].pt.x));
4263  }
4264  }
4265  ui_->graphicsView_stereo->update();
4266  }
4267 }
4268 
4269 void DatabaseViewer::updateWordsMatching()
4270 {
4271  int from = ids_.at(ui_->horizontalSlider_A->value());
4272  int to = ids_.at(ui_->horizontalSlider_B->value());
4273  if(from && to)
4274  {
4275  int alpha = 70;
4276  ui_->graphicsView_A->clearLines();
4277  ui_->graphicsView_A->setFeaturesColor(QColor(255, 255, 0, alpha)); // yellow
4278  ui_->graphicsView_B->clearLines();
4279  ui_->graphicsView_B->setFeaturesColor(QColor(255, 255, 0, alpha)); // yellow
4280 
4281  const QMultiMap<int, KeypointItem*> & wordsA = ui_->graphicsView_A->getFeatures();
4282  const QMultiMap<int, KeypointItem*> & wordsB = ui_->graphicsView_B->getFeatures();
4283  if(wordsA.size() && wordsB.size())
4284  {
4285  QList<int> ids = wordsA.uniqueKeys();
4286  for(int i=0; i<ids.size(); ++i)
4287  {
4288  if(ids[i] > 0 && wordsA.count(ids[i]) == 1 && wordsB.count(ids[i]) == 1)
4289  {
4290  // PINK features
4291  ui_->graphicsView_A->setFeatureColor(ids[i], Qt::magenta);
4292  ui_->graphicsView_B->setFeatureColor(ids[i], Qt::magenta);
4293 
4294  // Add lines
4295  // Draw lines between corresponding features...
4296  float scaleX = ui_->graphicsView_A->viewScale();
4297  float deltaX = 0;
4298  float deltaY = 0;
4299 
4300  if(ui_->actionVertical_Layout->isChecked())
4301  {
4302  deltaY = ui_->graphicsView_A->height()/scaleX;
4303  }
4304  else
4305  {
4306  deltaX = ui_->graphicsView_A->width()/scaleX;
4307  }
4308 
4309  const KeypointItem * kptA = wordsA.value(ids[i]);
4310  const KeypointItem * kptB = wordsB.value(ids[i]);
4311  ui_->graphicsView_A->addLine(
4312  kptA->rect().x()+kptA->rect().width()/2,
4313  kptA->rect().y()+kptA->rect().height()/2,
4314  kptB->rect().x()+kptB->rect().width()/2+deltaX,
4315  kptB->rect().y()+kptB->rect().height()/2+deltaY,
4316  Qt::cyan);
4317 
4318  ui_->graphicsView_B->addLine(
4319  kptA->rect().x()+kptA->rect().width()/2-deltaX,
4320  kptA->rect().y()+kptA->rect().height()/2-deltaY,
4321  kptB->rect().x()+kptB->rect().width()/2,
4322  kptB->rect().y()+kptB->rect().height()/2,
4323  Qt::cyan);
4324  }
4325  }
4326  ui_->graphicsView_A->update();
4327  ui_->graphicsView_B->update();
4328  }
4329  }
4330 }
4331 
4332 void DatabaseViewer::sliderAMoved(int value)
4333 {
4334  ui_->label_indexA->setText(QString::number(value));
4335  if(value>=0 && value < ids_.size())
4336  {
4337  ui_->label_idA->setText(QString::number(ids_.at(value)));
4338  }
4339  else
4340  {
4341  ULOGGER_ERROR("Slider index out of range ?");
4342  }
4343 }
4344 
4345 void DatabaseViewer::sliderBMoved(int value)
4346 {
4347  ui_->label_indexB->setText(QString::number(value));
4348  if(value>=0 && value < ids_.size())
4349  {
4350  ui_->label_idB->setText(QString::number(ids_.at(value)));
4351  }
4352  else
4353  {
4354  ULOGGER_ERROR("Slider index out of range ?");
4355  }
4356 }
4357 
4358 void DatabaseViewer::update3dView()
4359 {
4360  if(ui_->dockWidget_view3d->isVisible())
4361  {
4362  sliderAValueChanged(ui_->horizontalSlider_A->value());
4363  sliderBValueChanged(ui_->horizontalSlider_B->value());
4364  }
4365 }
4366 
4367 void DatabaseViewer::sliderNeighborValueChanged(int value)
4368 {
4369  if(value < neighborLinks_.size())
4370  {
4371  this->updateConstraintView(neighborLinks_.at(value));
4372  }
4373 }
4374 
4375 void DatabaseViewer::sliderLoopValueChanged(int value)
4376 {
4377  if(value < loopLinks_.size())
4378  {
4379  this->updateConstraintView(loopLinks_.at(value));
4380  }
4381 }
4382 
4383 // only called when ui_->checkBox_showOptimized state changed
4384 void DatabaseViewer::updateConstraintView()
4385 {
4386  if(ids_.size())
4387  {
4388  Link link = this->findActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value()));
4389  if(link.isValid())
4390  {
4391  if(link.type() == Link::kNeighbor ||
4392  link.type() == Link::kNeighborMerged)
4393  {
4394  this->updateConstraintView(neighborLinks_.at(ui_->horizontalSlider_neighbors->value()), false);
4395  }
4396  else
4397  {
4398  this->updateConstraintView(loopLinks_.at(ui_->horizontalSlider_loops->value()), false);
4399  }
4400  }
4401  }
4402 }
4403 
4404 void DatabaseViewer::updateConstraintView(
4405  const rtabmap::Link & linkIn,
4406  bool updateImageSliders,
4407  const Signature & signatureFrom,
4408  const Signature & signatureTo)
4409 {
4410  std::multimap<int, Link>::iterator iterLink = rtabmap::graph::findLink(linksRefined_, linkIn.from(), linkIn.to());
4411  rtabmap::Link link = linkIn;
4412 
4413  if(iterLink != linksRefined_.end())
4414  {
4415  link = iterLink->second;
4416  }
4417  else if(ui_->checkBox_ignorePoseCorrection->isChecked())
4418  {
4419  if(link.type() == Link::kNeighbor ||
4420  link.type() == Link::kNeighborMerged)
4421  {
4422  Transform poseFrom = uValue(odomPoses_, link.from(), Transform());
4423  Transform poseTo = uValue(odomPoses_, link.to(), Transform());
4424  if(!poseFrom.isNull() && !poseTo.isNull())
4425  {
4426  // recompute raw odom transformation and
4427  // reset to identity covariance
4428  link = Link(link.from(),
4429  link.to(),
4430  link.type(),
4431  poseFrom.inverse() * poseTo);
4432  }
4433  }
4434  }
4435  rtabmap::Transform t = link.transform();
4436 
4437  ui_->label_constraint->clear();
4438  ui_->label_constraint_opt->clear();
4439  ui_->checkBox_showOptimized->setEnabled(false);
4440  UASSERT(!t.isNull() && dbDriver_);
4441 
4442  ui_->label_type->setText(tr("%1 (%2)")
4443  .arg(link.type())
4444  .arg(link.type()==Link::kNeighbor?"Neighbor":
4445  link.type()==Link::kNeighborMerged?"Merged neighbor":
4446  link.type()==Link::kGlobalClosure?"Loop closure":
4447  link.type()==Link::kLocalSpaceClosure?"Space proximity link":
4448  link.type()==Link::kLocalTimeClosure?"Time proximity link":
4449  link.type()==Link::kUserClosure?"User link":
4450  link.type()==Link::kVirtualClosure?"Virtual link":"Undefined"));
4451  ui_->label_variance->setText(QString("%1, %2")
4452  .arg(sqrt(link.transVariance()))
4453  .arg(sqrt(link.rotVariance())));
4454  std::stringstream out;
4455  out << link.infMatrix().inv();
4456  ui_->lineEdit_covariance->setText(out.str().c_str());
4457  ui_->label_constraint->setText(QString("%1").arg(t.prettyPrint().c_str()).replace(" ", "\n"));
4458  if(graphes_.size() &&
4459  (int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
4460  {
4461  std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
4462 
4463  std::map<int, rtabmap::Transform>::iterator iterFrom = graph.find(link.from());
4464  std::map<int, rtabmap::Transform>::iterator iterTo = graph.find(link.to());
4465  if(iterFrom != graph.end() && iterTo != graph.end())
4466  {
4467  ui_->checkBox_showOptimized->setEnabled(true);
4468  Transform topt = iterFrom->second.inverse()*iterTo->second;
4469  float diff = topt.getDistance(t);
4470  Transform v1 = t.rotation()*Transform(1,0,0,0,0,0);
4471  Transform v2 = topt.rotation()*Transform(1,0,0,0,0,0);
4472  float a = pcl::getAngle3D(Eigen::Vector4f(v1.x(), v1.y(), v1.z(), 0), Eigen::Vector4f(v2.x(), v2.y(), v2.z(), 0));
4473  a = (a *180.0f) / CV_PI;
4474  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));
4475 
4476  if(ui_->checkBox_showOptimized->isChecked())
4477  {
4478  t = topt;
4479  }
4480  }
4481  }
4482 
4483  if(updateImageSliders)
4484  {
4485  ui_->horizontalSlider_A->blockSignals(true);
4486  ui_->horizontalSlider_B->blockSignals(true);
4487  // set from on left and to on right {
4488  ui_->horizontalSlider_A->setValue(idToIndex_.value(link.from()));
4489  ui_->horizontalSlider_B->setValue(idToIndex_.value(link.to()));
4490  ui_->horizontalSlider_A->blockSignals(false);
4491  ui_->horizontalSlider_B->blockSignals(false);
4492  this->update(idToIndex_.value(link.from()),
4493  ui_->label_indexA,
4494  ui_->label_parentsA,
4495  ui_->label_childrenA,
4496  ui_->label_weightA,
4497  ui_->label_labelA,
4498  ui_->label_stampA,
4499  ui_->graphicsView_A,
4500  ui_->label_idA,
4501  ui_->label_mapA,
4502  ui_->label_poseA,
4503  ui_->label_velA,
4504  ui_->label_calibA,
4505  ui_->label_gpsA,
4506  false); // don't update constraints view!
4507  this->update(idToIndex_.value(link.to()),
4508  ui_->label_indexB,
4509  ui_->label_parentsB,
4510  ui_->label_childrenB,
4511  ui_->label_weightB,
4512  ui_->label_labelB,
4513  ui_->label_stampB,
4514  ui_->graphicsView_B,
4515  ui_->label_idB,
4516  ui_->label_mapB,
4517  ui_->label_poseB,
4518  ui_->label_velB,
4519  ui_->label_calibB,
4520  ui_->label_gpsB,
4521  false); // don't update constraints view!
4522  }
4523 
4524  if(constraintsViewer_->isVisible())
4525  {
4526  SensorData dataFrom, dataTo;
4527 
4528  if(signatureFrom.id()>0)
4529  {
4530  dataFrom = signatureFrom.sensorData();
4531  }
4532  else
4533  {
4534  dbDriver_->getNodeData(link.from(), dataFrom);
4535  }
4536  dataFrom.uncompressData();
4537  UASSERT(dataFrom.imageRaw().empty() || dataFrom.imageRaw().type()==CV_8UC3 || dataFrom.imageRaw().type() == CV_8UC1);
4538  UASSERT(dataFrom.depthOrRightRaw().empty() || dataFrom.depthOrRightRaw().type()==CV_8UC1 || dataFrom.depthOrRightRaw().type() == CV_16UC1 || dataFrom.depthOrRightRaw().type() == CV_32FC1);
4539 
4540  if(signatureTo.id()>0)
4541  {
4542  dataTo = signatureTo.sensorData();
4543  }
4544  else
4545  {
4546  dbDriver_->getNodeData(link.to(), dataTo);
4547  }
4548  dataTo.uncompressData();
4549  UASSERT(dataTo.imageRaw().empty() || dataTo.imageRaw().type()==CV_8UC3 || dataTo.imageRaw().type() == CV_8UC1);
4550  UASSERT(dataTo.depthOrRightRaw().empty() || dataTo.depthOrRightRaw().type()==CV_8UC1 || dataTo.depthOrRightRaw().type() == CV_16UC1 || dataTo.depthOrRightRaw().type() == CV_32FC1);
4551 
4552  // get odom pose
4553  Transform pose = Transform::getIdentity();
4554  if(ui_->checkBox_odomFrame->isChecked())
4555  {
4556  int m,w;
4557  std::string l;
4558  double s;
4559  Transform p,g;
4560  std::vector<float> v;
4561  GPS gps;
4562  dbDriver_->getNodeInfo(link.from(), p, m, w, l, s, g, v, gps);
4563  if(!p.isNull())
4564  {
4565  // keep just the z and roll/pitch rotation
4566  float x, y, z, roll, pitch, yaw;
4567  p.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
4568  pose = Transform(0,0,z,roll,pitch,0);
4569  }
4570  }
4571 
4572  constraintsViewer_->removeCloud("cloud0");
4573  constraintsViewer_->removeCloud("cloud1");
4574  //cloud 3d
4575  if(ui_->checkBox_show3Dclouds->isChecked())
4576  {
4577  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom, cloudTo;
4578  pcl::IndicesPtr indicesFrom(new std::vector<int>);
4579  pcl::IndicesPtr indicesTo(new std::vector<int>);
4580  if(!dataFrom.imageRaw().empty() && !dataFrom.depthOrRightRaw().empty())
4581  {
4582  cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, ui_->spinBox_decimation->value(), 0, 0, indicesFrom.get(), ui_->parameters_toolbox->getParameters());
4583  }
4584  if(!dataTo.imageRaw().empty() && !dataTo.depthOrRightRaw().empty())
4585  {
4586  cloudTo=util3d::cloudRGBFromSensorData(dataTo, ui_->spinBox_decimation->value(), 0, 0, indicesTo.get(), ui_->parameters_toolbox->getParameters());
4587  }
4588 
4589  if(cloudTo.get() && indicesTo->size())
4590  {
4591  cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t);
4592  }
4593 
4594  // Gain compensation
4595  if(ui_->doubleSpinBox_gainCompensationRadius->value()>0.0 &&
4596  cloudFrom.get() && indicesFrom->size() &&
4597  cloudTo.get() && indicesTo->size())
4598  {
4599  UTimer t;
4600  GainCompensator compensator(ui_->doubleSpinBox_gainCompensationRadius->value());
4601  compensator.feed(cloudFrom, indicesFrom, cloudTo, indicesTo, Transform::getIdentity());
4602  compensator.apply(0, cloudFrom, indicesFrom);
4603  compensator.apply(1, cloudTo, indicesTo);
4604  UINFO("Gain compensation time = %fs", t.ticks());
4605  }
4606 
4607  if(cloudFrom.get() && indicesFrom->size())
4608  {
4609  if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
4610  {
4611  cloudFrom = util3d::voxelize(cloudFrom, indicesFrom, ui_->doubleSpinBox_voxelSize->value());
4612  }
4613  constraintsViewer_->addCloud("cloud0", cloudFrom, pose, Qt::red);
4614  }
4615  if(cloudTo.get() && indicesTo->size())
4616  {
4617  if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
4618  {
4619  cloudTo = util3d::voxelize(cloudTo, indicesTo, ui_->doubleSpinBox_voxelSize->value());
4620  }
4621  constraintsViewer_->addCloud("cloud1", cloudTo, pose, Qt::cyan);
4622  }
4623  }
4624 
4625  constraintsViewer_->removeCloud("words0");
4626  constraintsViewer_->removeCloud("words1");
4627  if(ui_->checkBox_show3DWords->isChecked())
4628  {
4629  std::list<int> ids;
4630  ids.push_back(link.from());
4631  ids.push_back(link.to());
4632  std::list<Signature*> signatures;
4633  dbDriver_->loadSignatures(ids, signatures);
4634  if(signatures.size() == 2)
4635  {
4636  const Signature * sFrom = signatureFrom.id()>0?&signatureFrom:signatures.front();
4637  const Signature * sTo = signatureTo.id()>0?&signatureTo:signatures.back();
4638  UASSERT(sFrom && sTo);
4639  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom(new pcl::PointCloud<pcl::PointXYZ>);
4640  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo(new pcl::PointCloud<pcl::PointXYZ>);
4641  cloudFrom->resize(sFrom->getWords3().size());
4642  cloudTo->resize(sTo->getWords3().size());
4643  int i=0;
4644  for(std::multimap<int, cv::Point3f>::const_iterator iter=sFrom->getWords3().begin();
4645  iter!=sFrom->getWords3().end();
4646  ++iter)
4647  {
4648  cloudFrom->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
4649  }
4650  i=0;
4651  for(std::multimap<int, cv::Point3f>::const_iterator iter=sTo->getWords3().begin();
4652  iter!=sTo->getWords3().end();
4653  ++iter)
4654  {
4655  cloudTo->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
4656  }
4657 
4658  if(cloudFrom->size())
4659  {
4660  cloudFrom = rtabmap::util3d::removeNaNFromPointCloud(cloudFrom);
4661  }
4662  if(cloudTo->size())
4663  {
4664  cloudTo = rtabmap::util3d::removeNaNFromPointCloud(cloudTo);
4665  if(cloudTo->size())
4666  {
4667  cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t);
4668  }
4669  }
4670 
4671  if(cloudFrom->size())
4672  {
4673  constraintsViewer_->addCloud("words0", cloudFrom, pose, Qt::red);
4674  }
4675  else
4676  {
4677  UWARN("Empty 3D words for node %d", link.from());
4678  constraintsViewer_->removeCloud("words0");
4679  }
4680  if(cloudTo->size())
4681  {
4682  constraintsViewer_->addCloud("words1", cloudTo, pose, Qt::cyan);
4683  }
4684  else
4685  {
4686  UWARN("Empty 3D words for node %d", link.to());
4687  constraintsViewer_->removeCloud("words1");
4688  }
4689  }
4690  else
4691  {
4692  UERROR("Not found signature %d or %d in RAM", link.from(), link.to());
4693  constraintsViewer_->removeCloud("words0");
4694  constraintsViewer_->removeCloud("words1");
4695  }
4696  //cleanup
4697  for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
4698  {
4699  delete *iter;
4700  }
4701  }
4702 
4703  constraintsViewer_->removeCloud("scan2");
4704  constraintsViewer_->removeCloud("scan2normals");
4705  constraintsViewer_->removeGraph("scan2graph");
4706  constraintsViewer_->removeCloud("scan0");
4707  constraintsViewer_->removeCloud("scan1");
4708  if(ui_->checkBox_show2DScans->isChecked())
4709  {
4710  //cloud 2d
4711  if(link.type() == Link::kLocalSpaceClosure &&
4712  !link.userDataCompressed().empty())
4713  {
4714  std::vector<int> ids;
4715  cv::Mat userData = link.uncompressUserDataConst();
4716  if(userData.type() == CV_8SC1 &&
4717  userData.rows == 1 &&
4718  userData.cols >= 8 && // including null str ending
4719  userData.at<char>(userData.cols-1) == 0 &&
4720  memcmp(userData.data, "SCANS:", 6) == 0)
4721  {
4722  std::string scansStr = (const char *)userData.data;
4723  UINFO("Detected \"%s\" in links's user data", scansStr.c_str());
4724  if(!scansStr.empty())
4725  {
4726  std::list<std::string> strs = uSplit(scansStr, ':');
4727  if(strs.size() == 2)
4728  {
4729  std::list<std::string> strIds = uSplit(strs.rbegin()->c_str(), ';');
4730  for(std::list<std::string>::iterator iter=strIds.begin(); iter!=strIds.end(); ++iter)
4731  {
4732  ids.push_back(atoi(iter->c_str()));
4733  if(ids.back() == link.from())
4734  {
4735  ids.pop_back();
4736  }
4737  }
4738  }
4739  }
4740  }
4741  if(ids.size())
4742  {
4743  //add other scans matching
4744  //optimize the path's poses locally
4745 
4746  std::map<int, rtabmap::Transform> poses;
4747  for(unsigned int i=0; i<ids.size(); ++i)
4748  {
4749  if(uContains(odomPoses_, ids[i]))
4750  {
4751  poses.insert(*odomPoses_.find(ids[i]));
4752  }
4753  else
4754  {
4755  UERROR("Not found %d node!", ids[i]);
4756  }
4757  }
4758  if(poses.size())
4759  {
4760  Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
4761 
4762  UASSERT(uContains(poses, link.to()));
4763  std::map<int, rtabmap::Transform> posesOut;
4764  std::multimap<int, rtabmap::Link> linksOut;
4765  optimizer->getConnectedGraph(
4766  link.to(),
4767  poses,
4768  updateLinksWithModifications(links_),
4769  posesOut,
4770  linksOut);
4771 
4772  if(poses.size() != posesOut.size())
4773  {
4774  UWARN("Scan poses input and output are different! %d vs %d", (int)poses.size(), (int)posesOut.size());
4775  UWARN("Input poses: ");
4776  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
4777  {
4778  UWARN(" %d", iter->first);
4779  }
4780  UWARN("Input links: ");
4781  std::multimap<int, Link> modifiedLinks = updateLinksWithModifications(links_);
4782  for(std::multimap<int, Link>::iterator iter=modifiedLinks.begin(); iter!=modifiedLinks.end(); ++iter)
4783  {
4784  UWARN(" %d->%d", iter->second.from(), iter->second.to());
4785  }
4786  }
4787 
4788  QTime time;
4789  time.start();
4790  std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(link.to(), posesOut, linksOut);
4791  delete optimizer;
4792 
4793  // transform local poses in loop referential
4794  Transform u = t * finalPoses.at(link.to()).inverse();
4795  pcl::PointCloud<pcl::PointXYZ>::Ptr assembledScans(new pcl::PointCloud<pcl::PointXYZ>);
4796  pcl::PointCloud<pcl::PointNormal>::Ptr assembledNormalScans(new pcl::PointCloud<pcl::PointNormal>);
4797  pcl::PointCloud<pcl::PointXYZ>::Ptr graph(new pcl::PointCloud<pcl::PointXYZ>);
4798  for(std::map<int, Transform>::iterator iter=finalPoses.begin(); iter!=finalPoses.end(); ++iter)
4799  {
4800  iter->second = u * iter->second;
4801  if(iter->first != link.to()) // already added to view
4802  {
4803  //create scan
4804  SensorData data;
4805  dbDriver_->getNodeData(iter->first, data);
4806  LaserScan scan;
4807  data.uncompressDataConst(0, 0, &scan, 0);
4808  if(!scan.isEmpty())
4809  {
4810  if(scan.hasNormals())
4811  {
4812  *assembledNormalScans += *util3d::laserScanToPointCloudNormal(scan, iter->second*scan.localTransform());
4813  }
4814  else
4815  {
4816  *assembledScans += *util3d::laserScanToPointCloud(scan, iter->second*scan.localTransform());
4817  }
4818  }
4819  }
4820  graph->push_back(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()));
4821  }
4822 
4823  if(assembledNormalScans->size())
4824  {
4825  constraintsViewer_->addCloud("scan2normals", assembledNormalScans, pose, Qt::cyan);
4826  constraintsViewer_->setCloudColorIndex("scan2normals", 2);
4827  }
4828  if(assembledScans->size())
4829  {
4830  constraintsViewer_->addCloud("scan2", assembledScans, pose, Qt::cyan);
4831  constraintsViewer_->setCloudColorIndex("scan2", 2);
4832  }
4833  if(graph->size())
4834  {
4835  constraintsViewer_->addOrUpdateGraph("scan2graph", graph, Qt::cyan);
4836  }
4837  }
4838  }
4839  }
4840 
4841  // Added loop closure scans
4842  constraintsViewer_->removeCloud("scan0");
4843  constraintsViewer_->removeCloud("scan1");
4844  if(!dataFrom.laserScanRaw().isEmpty())
4845  {
4846  if(dataFrom.laserScanRaw().hasNormals())
4847  {
4848  pcl::PointCloud<pcl::PointNormal>::Ptr scan;
4849  scan = rtabmap::util3d::laserScanToPointCloudNormal(dataFrom.laserScanRaw(), dataFrom.laserScanRaw().localTransform());
4850  constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
4851  constraintsViewer_->setCloudColorIndex("scan0", 2);
4852  }
4853  else
4854  {
4855  pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
4856  scan = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw(), dataFrom.laserScanRaw().localTransform());
4857  constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
4858  constraintsViewer_->setCloudColorIndex("scan0", 2);
4859  }
4860  }
4861  if(!dataTo.laserScanRaw().isEmpty())
4862  {
4863  if(dataTo.laserScanRaw().hasNormals())
4864  {
4865  pcl::PointCloud<pcl::PointNormal>::Ptr scan;
4866  scan = rtabmap::util3d::laserScanToPointCloudNormal(dataTo.laserScanRaw(), t*dataTo.laserScanRaw().localTransform());
4867  constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
4868  constraintsViewer_->setCloudColorIndex("scan1", 2);
4869  }
4870  else
4871  {
4872  pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
4873  scan = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw(), t*dataTo.laserScanRaw().localTransform());
4874  constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
4875  constraintsViewer_->setCloudColorIndex("scan1", 2);
4876  }
4877  }
4878  }
4879 
4880  //update coordinate
4881  constraintsViewer_->addOrUpdateCoordinate("from_coordinate", pose, 0.2);
4882 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
4883  constraintsViewer_->addOrUpdateCoordinate("to_coordinate", pose*t, 0.2);
4884  constraintsViewer_->removeCoordinate("to_coordinate_gt");
4885  if(uContains(groundTruthPoses_, link.from()) && uContains(groundTruthPoses_, link.to()))
4886  {
4887  constraintsViewer_->addOrUpdateCoordinate("to_coordinate_gt",
4888  pose*(groundTruthPoses_.at(link.from()).inverse()*groundTruthPoses_.at(link.to())), 0.1);
4889  }
4890 #endif
4891 
4892  constraintsViewer_->clearTrajectory();
4893 
4894  constraintsViewer_->update();
4895  }
4896 
4897  // update buttons
4898  updateConstraintButtons();
4899 }
4900 
4901 void DatabaseViewer::updateConstraintButtons()
4902 {
4903  ui_->pushButton_refine->setEnabled(false);
4904  ui_->pushButton_reset->setEnabled(false);
4905  ui_->pushButton_add->setEnabled(false);
4906  ui_->pushButton_reject->setEnabled(false);
4907 
4908  int from = ids_.at(ui_->horizontalSlider_A->value());
4909  int to = ids_.at(ui_->horizontalSlider_B->value());
4910  if(from!=to && from && to && odomPoses_.find(from) != odomPoses_.end() && odomPoses_.find(to) != odomPoses_.end())
4911  {
4912  if((!containsLink(links_, from ,to) && !containsLink(linksAdded_, from ,to)) ||
4913  containsLink(linksRemoved_, from ,to))
4914  {
4915  ui_->pushButton_add->setEnabled(true);
4916  }
4917  }
4918 
4919  Link currentLink = findActiveLink(from ,to);
4920 
4921  if(currentLink.isValid() &&
4922  ((currentLink.from() == from && currentLink.to() == to) || (currentLink.from() == to && currentLink.to() == from)))
4923  {
4924  if(!containsLink(linksRemoved_, from ,to))
4925  {
4926  ui_->pushButton_reject->setEnabled(
4927  currentLink.type() != Link::kNeighbor &&
4928  currentLink.type() != Link::kNeighborMerged);
4929  }
4930 
4931  //check for modified link
4932  bool modified = false;
4933  std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, currentLink.from(), currentLink.to());
4934  if(iter != linksRefined_.end())
4935  {
4936  currentLink = iter->second;
4937  ui_->pushButton_reset->setEnabled(true);
4938  modified = true;
4939  }
4940  if(!modified)
4941  {
4942  ui_->pushButton_reset->setEnabled(false);
4943  }
4944  ui_->pushButton_refine->setEnabled(true);
4945  }
4946 }
4947 
4948 void DatabaseViewer::sliderIterationsValueChanged(int value)
4949 {
4950  if(dbDriver_ && value >=0 && value < (int)graphes_.size())
4951  {
4952  std::map<int, rtabmap::Transform> graph = uValueAt(graphes_, value);
4953 
4954  std::map<int, Transform> refPoses = groundTruthPoses_;
4955  if(refPoses.empty())
4956  {
4957  refPoses = gpsPoses_;
4958  }
4959 
4960  // Log ground truth statistics (in TUM's RGBD-SLAM format)
4961  if(refPoses.size())
4962  {
4963  // compute KITTI statistics before aligning the poses
4964  float length = graph::computePathLength(graph);
4965  if(refPoses.size() == graph.size() && length >= 100.0f)
4966  {
4967  float t_err = 0.0f;
4968  float r_err = 0.0f;
4969  graph::calcKittiSequenceErrors(uValues(refPoses), uValues(graph), t_err, r_err);
4970  UINFO("KITTI t_err = %f %%", t_err);
4971  UINFO("KITTI r_err = %f deg/m", r_err);
4972  }
4973 
4974  float translational_rmse = 0.0f;
4975  float translational_mean = 0.0f;
4976  float translational_median = 0.0f;
4977  float translational_std = 0.0f;
4978  float translational_min = 0.0f;
4979  float translational_max = 0.0f;
4980  float rotational_rmse = 0.0f;
4981  float rotational_mean = 0.0f;
4982  float rotational_median = 0.0f;
4983  float rotational_std = 0.0f;
4984  float rotational_min = 0.0f;
4985  float rotational_max = 0.0f;
4986 
4987  Transform gtToMap = graph::calcRMSE(
4988  refPoses,
4989  graph,
4990  translational_rmse,
4991  translational_mean,
4992  translational_median,
4993  translational_std,
4994  translational_min,
4995  translational_max,
4996  rotational_rmse,
4997  rotational_mean,
4998  rotational_median,
4999  rotational_std,
5000  rotational_min,
5001  rotational_max);
5002 
5003  // ground truth live statistics
5004  ui_->label_rmse->setNum(translational_rmse);
5005  UINFO("translational_rmse=%f", translational_rmse);
5006  UINFO("translational_mean=%f", translational_mean);
5007  UINFO("translational_median=%f", translational_median);
5008  UINFO("translational_std=%f", translational_std);
5009  UINFO("translational_min=%f", translational_min);
5010  UINFO("translational_max=%f", translational_max);
5011 
5012  UINFO("rotational_rmse=%f", rotational_rmse);
5013  UINFO("rotational_mean=%f", rotational_mean);
5014  UINFO("rotational_median=%f", rotational_median);
5015  UINFO("rotational_std=%f", rotational_std);
5016  UINFO("rotational_min=%f", rotational_min);
5017  UINFO("rotational_max=%f", rotational_max);
5018 
5019  if(ui_->checkBox_alignPosesWithGroundTruth->isChecked() && !gtToMap.isIdentity())
5020  {
5021  for(std::map<int, Transform>::iterator iter=graph.begin(); iter!=graph.end(); ++iter)
5022  {
5023  iter->second = gtToMap * iter->second;
5024  }
5025  }
5026  }
5027 
5028  std::map<int, rtabmap::Transform> graphFiltered;
5029  if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty())
5030  {
5031  graphFiltered = groundTruthPoses_;
5032  }
5033  else
5034  {
5035  graphFiltered = graph;
5036  }
5037  if(ui_->groupBox_posefiltering->isChecked())
5038  {
5039  graphFiltered = graph::radiusPosesFiltering(graph,
5040  ui_->doubleSpinBox_posefilteringRadius->value(),
5041  ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
5042  }
5043  std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > localMaps;
5044  std::map<int, std::pair<float, cv::Point3f> > localMapsInfo;
5045 #ifdef RTABMAP_OCTOMAP
5046  if(octomap_)
5047  {
5048  delete octomap_;
5049  octomap_ = 0;
5050  }
5051 #endif
5052  if(ui_->dockWidget_graphView->isVisible() || ui_->dockWidget_occupancyGridView->isVisible())
5053  {
5054  //update scans
5055  UINFO("Update local maps list...");
5056  std::vector<int> ids = uKeys(graphFiltered);
5057  for(unsigned int i=0; i<ids.size(); ++i)
5058  {
5059  if(generatedLocalMaps_.find(ids[i]) != generatedLocalMaps_.end())
5060  {
5061  localMaps.insert(*generatedLocalMaps_.find(ids[i]));
5062  localMapsInfo.insert(*generatedLocalMapsInfo_.find(ids[i]));
5063  }
5064  else if(localMaps_.find(ids[i]) != localMaps_.end())
5065  {
5066  if(!localMaps_.find(ids[i])->second.first.first.empty() || !localMaps_.find(ids[i])->second.first.second.empty())
5067  {
5068  localMaps.insert(*localMaps_.find(ids.at(i)));
5069  localMapsInfo.insert(*localMapsInfo_.find(ids[i]));
5070  }
5071  }
5072  else
5073  {
5074  SensorData data;
5075  dbDriver_->getNodeData(ids.at(i), data);
5076  cv::Mat ground, obstacles, empty;
5077  data.uncompressData(0, 0, 0, 0, &ground, &obstacles, &empty);
5078  localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(std::make_pair(ground, obstacles), empty)));
5079  localMapsInfo_.insert(std::make_pair(ids.at(i), std::make_pair(data.gridCellSize(), data.gridViewPoint())));
5080  if(!ground.empty() || !obstacles.empty())
5081  {
5082  localMaps.insert(std::make_pair(ids.at(i), std::make_pair(std::make_pair(ground, obstacles), empty)));
5083  localMapsInfo.insert(std::make_pair(ids.at(i), std::make_pair(data.gridCellSize(), data.gridViewPoint())));
5084  }
5085  }
5086  }
5087  //cleanup
5088  for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end();)
5089  {
5090  if(graphFiltered.find(iter->first) == graphFiltered.end())
5091  {
5092  localMapsInfo_.erase(iter->first);
5093  localMaps_.erase(iter++);
5094  }
5095  else
5096  {
5097  ++iter;
5098  }
5099  }
5100  UINFO("Update local maps list... done (%d local maps, graph size=%d)", (int)localMaps.size(), (int)graph.size());
5101  }
5102 
5103  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
5104  float cellSize = Parameters::defaultGridCellSize();
5105  Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize);
5106 
5107  ui_->graphViewer->updateGTGraph(groundTruthPoses_);
5108  ui_->graphViewer->updateGPSGraph(gpsPoses_, gpsValues_);
5109  ui_->graphViewer->updateGraph(graph, graphLinks_, mapIds_);
5110  ui_->graphViewer->clearMap();
5111  occupancyGridViewer_->clear();
5112  if(graph.size() && localMaps.size() &&
5113  (ui_->graphViewer->isGridMapVisible() || ui_->dockWidget_occupancyGridView->isVisible()))
5114  {
5115  QTime time;
5116  time.start();
5117 
5118 #ifdef RTABMAP_OCTOMAP
5119  if(ui_->checkBox_octomap->isChecked())
5120  {
5121  octomap_ = new OctoMap(parameters);
5122  bool updateAborted = false;
5123  for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter)
5124  {
5125  if(iter->second.first.first.channels() == 2 || iter->second.first.second.channels() == 2)
5126  {
5127  QMessageBox::warning(this, tr(""),
5128  tr("Some local occupancy grids are 2D, but OctoMap requires 3D local "
5129  "occupancy grids. Uncheck OctoMap under GUI parameters or generate "
5130  "3D local occupancy grids (\"Grid/3D\" core parameter)."));
5131  updateAborted = true;
5132  break;
5133  }
5134  octomap_->addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second, localMapsInfo.at(iter->first).second);
5135  }
5136  if(!updateAborted)
5137  {
5138  octomap_->update(graphFiltered);
5139  }
5140  }
5141 #endif
5142 
5143  // Generate 2d grid map?
5144  if((ui_->dockWidget_graphView->isVisible() && ui_->graphViewer->isGridMapVisible()) ||
5145  (ui_->dockWidget_occupancyGridView->isVisible() && ui_->checkBox_grid_2d->isChecked()))
5146  {
5147  bool eroded = Parameters::defaultGridGlobalEroded();
5148  Parameters::parse(parameters, Parameters::kGridGlobalEroded(), eroded);
5149  float xMin, yMin;
5150  cv::Mat map;
5151 
5152 #ifdef RTABMAP_OCTOMAP
5153  if(ui_->checkBox_octomap->isChecked())
5154  {
5155  map = octomap_->createProjectionMap(xMin, yMin, cellSize, 0, ui_->spinBox_grid_depth->value());
5156  }
5157  else
5158 #endif
5159  {
5160  if(eroded)
5161  {
5162  uInsert(parameters, ParametersPair(Parameters::kGridGlobalEroded(), "true"));
5163  }
5164  OccupancyGrid grid(parameters);
5165  grid.setCellSize(cellSize);
5166  for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter)
5167  {
5168  grid.addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second);
5169  }
5170  grid.update(graphFiltered);
5171  map = grid.getMap(xMin, yMin);
5172  }
5173 
5174  ui_->label_timeGrid->setNum(double(time.elapsed())/1000.0);
5175 
5176  if(!map.empty())
5177  {
5178  cv::Mat map8U = rtabmap::util3d::convertMap2Image8U(map);
5179  if(ui_->dockWidget_graphView->isVisible() && ui_->graphViewer->isGridMapVisible())
5180  {
5181  ui_->graphViewer->updateMap(map8U, cellSize, xMin, yMin);
5182  }
5183  if(ui_->dockWidget_occupancyGridView->isVisible() && ui_->checkBox_grid_2d->isChecked())
5184  {
5185  occupancyGridViewer_->addOccupancyGridMap(map8U, cellSize, xMin, yMin, 1.0f);
5186  occupancyGridViewer_->update();
5187  }
5188  }
5189  }
5190 
5191  // Generate 3d grid map?
5192  if(ui_->dockWidget_occupancyGridView->isVisible())
5193  {
5194 #ifdef RTABMAP_OCTOMAP
5195  if(ui_->checkBox_octomap->isChecked())
5196  {
5197  updateOctomapView();
5198  }
5199  else
5200 #endif
5201  {
5202  pcl::PointCloud<pcl::PointXYZ>::Ptr groundXYZ(new pcl::PointCloud<pcl::PointXYZ>);
5203  pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesXYZ(new pcl::PointCloud<pcl::PointXYZ>);
5204  pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCellsXYZ(new pcl::PointCloud<pcl::PointXYZ>);
5205  pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
5206  pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
5207  pcl::PointCloud<pcl::PointXYZRGB>::Ptr emptyCellsRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
5208 
5209  for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter)
5210  {
5211  Transform pose = graphFiltered.at(iter->first);
5212  float x,y,z,roll,pitch,yaw;
5213  pose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
5214  Transform pose2d(x,y, 0, 0, 0, yaw);
5215  if(!iter->second.first.first.empty())
5216  {
5217  if(iter->second.first.first.channels() == 4)
5218  {
5219  *groundRGB += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(iter->second.first.first), pose);
5220  }
5221  else
5222  {
5223  *groundXYZ += *util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(iter->second.first.first), iter->second.first.first.channels()==2?pose2d:pose);
5224  }
5225  }
5226  if(!iter->second.first.second.empty())
5227  {
5228  if(iter->second.first.second.channels() == 4)
5229  {
5230  *obstaclesRGB += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(iter->second.first.second), pose);
5231  }
5232  else
5233  {
5234  *obstaclesXYZ += *util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(iter->second.first.second), iter->second.first.second.channels()==2?pose2d:pose);
5235  }
5236  }
5237  if(ui_->checkBox_grid_empty->isChecked())
5238  {
5239  if(!iter->second.second.empty())
5240  {
5241  if(iter->second.second.channels() == 4)
5242  {
5243  *emptyCellsRGB += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(iter->second.second), pose);
5244  }
5245  else
5246  {
5247  *emptyCellsXYZ += *util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(iter->second.second), iter->second.second.channels()==2?pose2d:pose);
5248  }
5249  }
5250  }
5251  }
5252  // occupancy cloud
5253  if(groundRGB->size())
5254  {
5255  groundRGB = util3d::voxelize(groundRGB, cellSize);
5256  occupancyGridViewer_->addCloud("groundRGB",
5257  groundRGB,
5258  Transform::getIdentity(),
5259  QColor(ui_->lineEdit_groundColor->text()));
5260  occupancyGridViewer_->setCloudPointSize("groundRGB", 5);
5261  }
5262  if(groundXYZ->size())
5263  {
5264  groundXYZ = util3d::voxelize(groundXYZ, cellSize);
5265  occupancyGridViewer_->addCloud("groundXYZ",
5266  groundXYZ,
5267  Transform::getIdentity(),
5268  QColor(ui_->lineEdit_groundColor->text()));
5269  occupancyGridViewer_->setCloudPointSize("groundXYZ", 5);
5270  }
5271  if(obstaclesRGB->size())
5272  {
5273  obstaclesRGB = util3d::voxelize(obstaclesRGB, cellSize);
5274  occupancyGridViewer_->addCloud("obstaclesRGB",
5275  obstaclesRGB,
5276  Transform::getIdentity(),
5277  QColor(ui_->lineEdit_obstacleColor->text()));
5278  occupancyGridViewer_->setCloudPointSize("obstaclesRGB", 5);
5279  }
5280  if(obstaclesXYZ->size())
5281  {
5282  obstaclesXYZ = util3d::voxelize(obstaclesXYZ, cellSize);
5283  occupancyGridViewer_->addCloud("obstaclesXYZ",
5284  obstaclesXYZ,
5285  Transform::getIdentity(),
5286  QColor(ui_->lineEdit_obstacleColor->text()));
5287  occupancyGridViewer_->setCloudPointSize("obstaclesXYZ", 5);
5288  }
5289  if(emptyCellsRGB->size())
5290  {
5291  emptyCellsRGB = util3d::voxelize(emptyCellsRGB, cellSize);
5292  occupancyGridViewer_->addCloud("emptyCellsRGB",
5293  emptyCellsRGB,
5294  Transform::getIdentity(),
5295  QColor(ui_->lineEdit_emptyColor->text()));
5296  occupancyGridViewer_->setCloudPointSize("emptyCellsRGB", 5);
5297  occupancyGridViewer_->setCloudOpacity("emptyCellsRGB", 0.5);
5298  }
5299  if(emptyCellsXYZ->size())
5300  {
5301  emptyCellsXYZ = util3d::voxelize(emptyCellsXYZ, cellSize);
5302  occupancyGridViewer_->addCloud("emptyCellsXYZ",
5303  emptyCellsXYZ,
5304  Transform::getIdentity(),
5305  QColor(ui_->lineEdit_emptyColor->text()));
5306  occupancyGridViewer_->setCloudPointSize("emptyCellsXYZ", 5);
5307  occupancyGridViewer_->setCloudOpacity("emptyCellsXYZ", 0.5);
5308  }
5309  occupancyGridViewer_->update();
5310  }
5311  }
5312  }
5313  ui_->graphViewer->fitInView(ui_->graphViewer->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
5314  ui_->graphViewer->update();
5315  ui_->label_iterations->setNum(value);
5316 
5317  //compute total length (neighbor links)
5318  float length = 0.0f;
5319  for(std::multimap<int, rtabmap::Link>::const_iterator iter=graphLinks_.begin(); iter!=graphLinks_.end(); ++iter)
5320  {
5321  std::map<int, rtabmap::Transform>::const_iterator jterA = graph.find(iter->first);
5322  std::map<int, rtabmap::Transform>::const_iterator jterB = graph.find(iter->second.to());
5323  if(jterA != graph.end() && jterB != graph.end())
5324  {
5325  const rtabmap::Transform & poseA = jterA->second;
5326  const rtabmap::Transform & poseB = jterB->second;
5327  if(iter->second.type() == rtabmap::Link::kNeighbor ||
5328  iter->second.type() == rtabmap::Link::kNeighborMerged)
5329  {
5330  Eigen::Vector3f vA, vB;
5331  float x,y,z;
5332  poseA.getTranslation(x,y,z);
5333  vA[0] = x; vA[1] = y; vA[2] = z;
5334  poseB.getTranslation(x,y,z);
5335  vB[0] = x; vB[1] = y; vB[2] = z;
5336  length += (vB - vA).norm();
5337  }
5338  }
5339  }
5340  ui_->label_pathLength->setNum(length);
5341  }
5342 }
5343 void DatabaseViewer::updateGraphView()
5344 {
5345  ui_->label_loopClosures->clear();
5346  ui_->label_poses->clear();
5347  ui_->label_rmse->clear();
5348 
5349  if(odomPoses_.size())
5350  {
5351  int fromId = ui_->spinBox_optimizationsFrom->value();
5352  if(!uContains(odomPoses_, fromId))
5353  {
5354  QMessageBox::warning(this, tr(""), tr("Graph optimization from id (%1) for which node is not linked to graph.\n Minimum=%2, Maximum=%3")
5355  .arg(fromId)
5356  .arg(odomPoses_.begin()->first)
5357  .arg(odomPoses_.rbegin()->first));
5358  return;
5359  }
5360 
5361  std::map<int, Transform> optimizedGraphGuess;
5362  if(graphes_.size() && useLastOptimizedGraphAsGuess_)
5363  {
5364  optimizedGraphGuess = lastOptimizedGraph_;
5365  }
5366 
5367  graphes_.clear();
5368  graphLinks_.clear();
5369 
5370  std::map<int, rtabmap::Transform> poses = odomPoses_;
5371  if(ui_->checkBox_wmState->isChecked() && uContains(wmStates_, fromId))
5372  {
5373  std::map<int, rtabmap::Transform> wmPoses;
5374  std::vector<int> & wmState = wmStates_.at(fromId);
5375  for(unsigned int i=0; i<wmState.size(); ++i)
5376  {
5377  std::map<int, rtabmap::Transform>::iterator iter = poses.find(wmState[i]);
5378  if(iter!=poses.end())
5379  {
5380  wmPoses.insert(*iter);
5381  }
5382  }
5383  if(!wmPoses.empty())
5384  {
5385  poses = wmPoses;
5386  }
5387  else
5388  {
5389  UWARN("Empty WM poses!? Ignoring WM state... (root id=%d, wmState=%d)", fromId, wmState.size());
5390  }
5391  }
5392 
5393  // filter current map if not spanning to all maps
5394  if(!ui_->checkBox_spanAllMaps->isChecked() && uContains(mapIds_, fromId) && mapIds_.at(fromId) >= 0)
5395  {
5396  int currentMapId = mapIds_.at(fromId);
5397  for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end();)
5398  {
5399  if(!uContains(mapIds_, iter->first) ||
5400  mapIds_.at(iter->first) != currentMapId)
5401  {
5402  poses.erase(iter++);
5403  }
5404  else
5405  {
5406  ++iter;
5407  }
5408  }
5409  }
5410 
5411  ui_->menuExport_poses->setEnabled(true);
5412  std::multimap<int, rtabmap::Link> links = links_;
5413  loopLinks_.clear();
5414 
5415  // filter current map if not spanning to all maps
5416  if(!ui_->checkBox_spanAllMaps->isChecked() && uContains(mapIds_, fromId) && mapIds_.at(fromId) >= 0)
5417  {
5418  int currentMapId = mapIds_.at(fromId);
5419  for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end();)
5420  {
5421  if(!uContains(mapIds_, iter->second.from()) ||
5422  !uContains(mapIds_, iter->second.to()) ||
5423  mapIds_.at(iter->second.from()) != currentMapId ||
5424  mapIds_.at(iter->second.to()) != currentMapId)
5425  {
5426  links.erase(iter++);
5427  }
5428  else
5429  {
5430  ++iter;
5431  }
5432  }
5433  }
5434 
5435  links = updateLinksWithModifications(links);
5436  if(ui_->checkBox_ignorePoseCorrection->isChecked())
5437  {
5438  for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
5439  {
5440  if(iter->second.type() == Link::kNeighbor ||
5441  iter->second.type() == Link::kNeighborMerged)
5442  {
5443  Transform poseFrom = uValue(poses, iter->second.from(), Transform());
5444  Transform poseTo = uValue(poses, iter->second.to(), Transform());
5445  if(!poseFrom.isNull() && !poseTo.isNull())
5446  {
5447  // reset to identity covariance
5448  iter->second = Link(
5449  iter->second.from(),
5450  iter->second.to(),
5451  iter->second.type(),
5452  poseFrom.inverse() * poseTo);
5453  }
5454  }
5455  }
5456  }
5457 
5458  // filter links
5459  int totalNeighbor = 0;
5460  int totalNeighborMerged = 0;
5461  int totalGlobal = 0;
5462  int totalLocalTime = 0;
5463  int totalLocalSpace = 0;
5464  int totalUser = 0;
5465  int totalPriors = 0;
5466  for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end();)
5467  {
5468  if(iter->second.type() == Link::kNeighbor)
5469  {
5470  ++totalNeighbor;
5471  }
5472  else if(iter->second.type() == Link::kNeighborMerged)
5473  {
5474  ++totalNeighborMerged;
5475  }
5476  else if(iter->second.type() == Link::kGlobalClosure)
5477  {
5478  if(ui_->checkBox_ignoreGlobalLoop->isChecked())
5479  {
5480  links.erase(iter++);
5481  continue;
5482  }
5483  loopLinks_.push_back(iter->second);
5484  ++totalGlobal;
5485  }
5486  else if(iter->second.type() == Link::kLocalSpaceClosure)
5487  {
5488  if(ui_->checkBox_ignoreLocalLoopSpace->isChecked())
5489  {
5490  links.erase(iter++);
5491  continue;
5492  }
5493  loopLinks_.push_back(iter->second);
5494  ++totalLocalSpace;
5495  }
5496  else if(iter->second.type() == Link::kLocalTimeClosure)
5497  {
5498  if(ui_->checkBox_ignoreLocalLoopTime->isChecked())
5499  {
5500  links.erase(iter++);
5501  continue;
5502  }
5503  loopLinks_.push_back(iter->second);
5504  ++totalLocalTime;
5505  }
5506  else if(iter->second.type() == Link::kUserClosure)
5507  {
5508  if(ui_->checkBox_ignoreUserLoop->isChecked())
5509  {
5510  links.erase(iter++);
5511  continue;
5512  }
5513  loopLinks_.push_back(iter->second);
5514  ++totalUser;
5515  }
5516  else if(iter->second.type() == Link::kPosePrior)
5517  {
5518  ++totalPriors;
5519  }
5520  else
5521  {
5522  loopLinks_.push_back(iter->second);
5523  }
5524  ++iter;
5525  }
5526  updateLoopClosuresSlider();
5527 
5528  ui_->label_loopClosures->setText(tr("(%1, %2, %3, %4, %5, %6, %7)")
5529  .arg(totalNeighbor)
5530  .arg(totalNeighborMerged)
5531  .arg(totalGlobal)
5532  .arg(totalLocalSpace)
5533  .arg(totalLocalTime)
5534  .arg(totalUser)
5535  .arg(totalPriors));
5536 
5537  // remove intermediate nodes?
5538  if(ui_->checkBox_ignoreIntermediateNodes->isVisible() &&
5539  ui_->checkBox_ignoreIntermediateNodes->isChecked())
5540  {
5541  for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
5542  {
5543  if(iter->second.type() == Link::kNeighbor ||
5544  iter->second.type() == Link::kNeighborMerged)
5545  {
5546  Link link = iter->second;
5547  while(uContains(weights_, link.to()) && weights_.at(link.to()) < 0)
5548  {
5549  std::multimap<int, Link>::iterator uter = links.find(link.to());
5550  if(uter != links.end())
5551  {
5552  UASSERT(links.count(link.to()) == 1);
5553  poses.erase(link.to());
5554  link = link.merge(uter->second, uter->second.type());
5555  links.erase(uter);
5556  }
5557  else
5558  {
5559  break;
5560  }
5561  }
5562 
5563  iter->second = link;
5564  }
5565  }
5566  }
5567 
5568  graphes_.push_back(poses);
5569 
5570  Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
5571 
5572  std::map<int, rtabmap::Transform> posesOut;
5573  std::multimap<int, rtabmap::Link> linksOut;
5574  UINFO("Get connected graph from %d (%d poses, %d links)", fromId, (int)poses.size(), (int)links.size());
5575  optimizer->getConnectedGraph(
5576  fromId,
5577  poses,
5578  links,
5579  posesOut,
5580  linksOut,
5581  ui_->spinBox_optimizationDepth->value());
5582  if(optimizedGraphGuess.size() == posesOut.size())
5583  {
5584  bool identical=true;
5585  for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
5586  {
5587  if(!uContains(optimizedGraphGuess, iter->first))
5588  {
5589  identical = false;
5590  break;
5591  }
5592  }
5593  if(identical)
5594  {
5595  posesOut = optimizedGraphGuess;
5596  }
5597  }
5598  UINFO("Connected graph of %d poses and %d links", (int)posesOut.size(), (int)linksOut.size());
5599  QTime time;
5600  time.start();
5601  std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(fromId, posesOut, linksOut, ui_->checkBox_iterativeOptimization->isChecked()?&graphes_:0);
5602  ui_->label_timeOptimization->setNum(double(time.elapsed())/1000.0);
5603  graphes_.push_back(finalPoses);
5604  graphLinks_ = linksOut;
5605  ui_->label_poses->setNum((int)finalPoses.size());
5606  if(posesOut.size() && finalPoses.empty())
5607  {
5608  UWARN("Optimization failed, trying incremental optimization instead... this may take a while (poses=%d, links=%d).", (int)posesOut.size(), (int)linksOut.size());
5609  finalPoses = optimizer->optimizeIncremental(fromId, posesOut, linksOut, &graphes_);
5610 
5611  if(finalPoses.empty())
5612  {
5613  UWARN("Incremental optimization also failed.");
5614  if(!optimizer->isCovarianceIgnored() || optimizer->type() != Optimizer::kTypeTORO)
5615  {
5616  QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors. "
5617  "Give it a try with %1=0 and %2=true.").arg(Parameters::kOptimizerStrategy().c_str()).arg(Parameters::kOptimizerVarianceIgnored().c_str()));
5618  }
5619  else
5620  {
5621  QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors."));
5622  }
5623  }
5624  else
5625  {
5626  UWARN("Incremental optimization succeeded!");
5627  QMessageBox::information(this, tr("Incremental optimization succeeded!"), tr("Graph optimization has failed but "
5628  "incremental optimization succeeded. Next optimizations will use the current "
5629  "best optimized poses as first guess instead of odometry poses."));
5630  useLastOptimizedGraphAsGuess_ = true;
5631  lastOptimizedGraph_ = finalPoses;
5632 
5633  }
5634  }
5635  delete optimizer;
5636  }
5637  if(graphes_.size())
5638  {
5639  if(ui_->doubleSpinBox_optimizationScale->value()!=-1.0)
5640  {
5641  // scale all poses
5642  for(std::list<std::map<int, Transform> >::iterator iter=graphes_.begin(); iter!=graphes_.end(); ++iter)
5643  {
5644  for(std::map<int, Transform>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
5645  {
5646  jter->second = jter->second.clone();
5647  jter->second.x() *= ui_->doubleSpinBox_optimizationScale->value();
5648  jter->second.y() *= ui_->doubleSpinBox_optimizationScale->value();
5649  jter->second.z() *= ui_->doubleSpinBox_optimizationScale->value();
5650  }
5651  }
5652  }
5653 
5654  ui_->horizontalSlider_iterations->setMaximum((int)graphes_.size()-1);
5655  ui_->horizontalSlider_iterations->setValue((int)graphes_.size()-1);
5656  ui_->horizontalSlider_iterations->setEnabled(true);
5657  ui_->spinBox_optimizationsFrom->setEnabled(true);
5658  sliderIterationsValueChanged((int)graphes_.size()-1);
5659  }
5660  else
5661  {
5662  ui_->horizontalSlider_iterations->setEnabled(false);
5663  ui_->spinBox_optimizationsFrom->setEnabled(false);
5664  }
5665 }
5666 
5667 void DatabaseViewer::updateGrid()
5668 {
5669  if(sender() == ui_->checkBox_grid_2d && !ui_->checkBox_grid_2d->isChecked())
5670  {
5671  //just remove map in occupancy grid view
5672  occupancyGridViewer_->removeOccupancyGridMap();
5673  occupancyGridViewer_->update();
5674  }
5675  else
5676  {
5677  ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked());
5678  ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
5679  ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
5680  ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
5681  ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
5682  ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
5683 
5684  update3dView();
5685  updateGraphView();
5686  }
5687 }
5688 
5689 void DatabaseViewer::updateOctomapView()
5690 {
5691 #ifdef RTABMAP_OCTOMAP
5692  ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked());
5693  ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
5694  ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
5695  ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
5696  ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
5697  ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
5698 
5699  if(ui_->checkBox_octomap->isChecked())
5700  {
5701  if(octomap_)
5702  {
5703  occupancyGridViewer_->removeOctomap();
5704  occupancyGridViewer_->removeCloud("octomap_obstacles");
5705  occupancyGridViewer_->removeCloud("octomap_empty");
5706  if(ui_->comboBox_octomap_rendering_type->currentIndex()>0)
5707  {
5708  occupancyGridViewer_->addOctomap(octomap_, ui_->spinBox_grid_depth->value(), ui_->comboBox_octomap_rendering_type->currentIndex()>1);
5709  }
5710  else
5711  {
5712  pcl::IndicesPtr obstacles(new std::vector<int>);
5713  pcl::IndicesPtr empty(new std::vector<int>);
5714  pcl::IndicesPtr ground(new std::vector<int>);
5715  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(ui_->spinBox_grid_depth->value(), obstacles.get(), empty.get(), ground.get());
5716 
5717  if(octomap_->hasColor())
5718  {
5719  pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
5720  pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
5721  occupancyGridViewer_->addCloud("octomap_obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
5722  occupancyGridViewer_->setCloudPointSize("octomap_obstacles", 5);
5723 
5724  pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
5725  pcl::copyPointCloud(*cloud, *ground, *groundCloud);
5726  occupancyGridViewer_->addCloud("octomap_ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
5727  occupancyGridViewer_->setCloudPointSize("octomap_ground", 5);
5728  }
5729  else
5730  {
5731  pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
5732  pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
5733  occupancyGridViewer_->addCloud("octomap_obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
5734  occupancyGridViewer_->setCloudPointSize("octomap_obstacles", 5);
5735 
5736  pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
5737  pcl::copyPointCloud(*cloud, *ground, *groundCloud);
5738  occupancyGridViewer_->addCloud("octomap_ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
5739  occupancyGridViewer_->setCloudPointSize("octomap_ground", 5);
5740  }
5741 
5742  if(ui_->checkBox_grid_empty->isChecked())
5743  {
5744  pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZ>);
5745  pcl::copyPointCloud(*cloud, *empty, *emptyCloud);
5746  occupancyGridViewer_->addCloud("octomap_empty", emptyCloud, Transform::getIdentity(), QColor(ui_->lineEdit_emptyColor->text()));
5747  occupancyGridViewer_->setCloudOpacity("octomap_empty", 0.5);
5748  occupancyGridViewer_->setCloudPointSize("octomap_empty", 5);
5749  }
5750  }
5751  occupancyGridViewer_->update();
5752  }
5753  if(ui_->dockWidget_view3d->isVisible() && ui_->checkBox_showGrid->isChecked())
5754  {
5755  this->update3dView();
5756  }
5757  }
5758 #endif
5759 }
5760 
5761 Link DatabaseViewer::findActiveLink(int from, int to)
5762 {
5763  Link link;
5764  std::multimap<int, Link>::iterator findIter = rtabmap::graph::findLink(linksRefined_, from ,to);
5765  if(findIter != linksRefined_.end())
5766  {
5767  link = findIter->second;
5768  }
5769  else
5770  {
5771  findIter = rtabmap::graph::findLink(linksAdded_, from ,to);
5772  if(findIter != linksAdded_.end())
5773  {
5774  link = findIter->second;
5775  }
5776  else if(!containsLink(linksRemoved_, from ,to))
5777  {
5778  findIter = rtabmap::graph::findLink(links_, from ,to);
5779  if(findIter != links_.end())
5780  {
5781  link = findIter->second;
5782  }
5783  }
5784  }
5785  return link;
5786 }
5787 
5788 bool DatabaseViewer::containsLink(std::multimap<int, Link> & links, int from, int to)
5789 {
5790  return rtabmap::graph::findLink(links, from, to) != links.end();
5791 }
5792 
5793 void DatabaseViewer::refineConstraint()
5794 {
5795  int from = ids_.at(ui_->horizontalSlider_A->value());
5796  int to = ids_.at(ui_->horizontalSlider_B->value());
5797  refineConstraint(from, to, false);
5798 }
5799 
5800 void DatabaseViewer::refineConstraint(int from, int to, bool silent)
5801 {
5802  if(from == to)
5803  {
5804  UWARN("Cannot refine link to same node");
5805  return;
5806  }
5807 
5808  Link currentLink = findActiveLink(from, to);
5809  if(!currentLink.isValid())
5810  {
5811  UERROR("Not found link! (%d->%d)", from, to);
5812  return;
5813  }
5814  UDEBUG("%d -> %d (type=%d)", from ,to, currentLink.type());
5815  Transform t = currentLink.transform();
5816  if(ui_->checkBox_showOptimized->isChecked() &&
5817  (currentLink.type() == Link::kNeighbor || currentLink.type() == Link::kNeighborMerged) &&
5818  graphes_.size() &&
5819  (int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
5820  {
5821  std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
5822  if(currentLink.type() == Link::kNeighbor || currentLink.type() == Link::kNeighborMerged)
5823  {
5824  std::map<int, rtabmap::Transform>::iterator iterFrom = graph.find(currentLink.from());
5825  std::map<int, rtabmap::Transform>::iterator iterTo = graph.find(currentLink.to());
5826  if(iterFrom != graph.end() && iterTo != graph.end())
5827  {
5828  Transform topt = iterFrom->second.inverse()*iterTo->second;
5829  t = topt;
5830  }
5831  }
5832  }
5833  else if(ui_->checkBox_ignorePoseCorrection->isChecked() &&
5834  graph::findLink(linksRefined_, from, to) == linksRefined_.end())
5835  {
5836  if(currentLink.type() == Link::kNeighbor ||
5837  currentLink.type() == Link::kNeighborMerged)
5838  {
5839  Transform poseFrom = uValue(odomPoses_, currentLink.from(), Transform());
5840  Transform poseTo = uValue(odomPoses_, currentLink.to(), Transform());
5841  if(!poseFrom.isNull() && !poseTo.isNull())
5842  {
5843  t = poseFrom.inverse() * poseTo; // recompute raw odom transformation
5844  }
5845  }
5846  }
5847 
5848  Transform transform;
5849  RegistrationInfo info;
5850  Signature fromS;
5851  Signature toS;
5852 
5853  SensorData dataFrom;
5854  dbDriver_->getNodeData(currentLink.from(), dataFrom);
5855 
5856  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
5857 
5858  UTimer timer;
5859 
5860  // Is it a multi-scan proximity detection?
5861  cv::Mat userData = currentLink.uncompressUserDataConst();
5862  std::map<int, rtabmap::Transform> scanPoses;
5863 
5864  if(currentLink.type() == Link::kLocalSpaceClosure &&
5865  !currentLink.userDataCompressed().empty() &&
5866  userData.type() == CV_8SC1 &&
5867  userData.rows == 1 &&
5868  userData.cols >= 8 && // including null str ending
5869  userData.at<char>(userData.cols-1) == 0 &&
5870  memcmp(userData.data, "SCANS:", 6) == 0 &&
5871  currentLink.from() > currentLink.to())
5872  {
5873  std::string scansStr = (const char *)userData.data;
5874  UINFO("Detected \"%s\" in links's user data", scansStr.c_str());
5875  if(!scansStr.empty())
5876  {
5877  std::list<std::string> strs = uSplit(scansStr, ':');
5878  if(strs.size() == 2)
5879  {
5880  std::list<std::string> strIds = uSplit(strs.rbegin()->c_str(), ';');
5881  for(std::list<std::string>::iterator iter=strIds.begin(); iter!=strIds.end(); ++iter)
5882  {
5883  int id = atoi(iter->c_str());
5884  if(uContains(odomPoses_, id))
5885  {
5886  scanPoses.insert(*odomPoses_.find(id));
5887  }
5888  else
5889  {
5890  UERROR("Not found %d node!", id);
5891  }
5892  }
5893  }
5894  }
5895  }
5896  if(scanPoses.size())
5897  {
5898  //optimize the path's poses locally
5899  Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
5900 
5901  UASSERT(uContains(scanPoses, currentLink.to()));
5902  std::map<int, rtabmap::Transform> posesOut;
5903  std::multimap<int, rtabmap::Link> linksOut;
5904  optimizer->getConnectedGraph(
5905  currentLink.to(),
5906  scanPoses,
5907  updateLinksWithModifications(links_),
5908  posesOut,
5909  linksOut);
5910 
5911  if(scanPoses.size() != posesOut.size())
5912  {
5913  UWARN("Scan poses input and output are different! %d vs %d", (int)scanPoses.size(), (int)posesOut.size());
5914  UWARN("Input poses: ");
5915  for(std::map<int, Transform>::iterator iter=scanPoses.begin(); iter!=scanPoses.end(); ++iter)
5916  {
5917  UWARN(" %d", iter->first);
5918  }
5919  UWARN("Input links: ");
5920  std::multimap<int, Link> modifiedLinks = updateLinksWithModifications(links_);
5921  for(std::multimap<int, Link>::iterator iter=modifiedLinks.begin(); iter!=modifiedLinks.end(); ++iter)
5922  {
5923  UWARN(" %d->%d", iter->second.from(), iter->second.to());
5924  }
5925  }
5926 
5927  scanPoses = optimizer->optimize(currentLink.to(), posesOut, linksOut);
5928  delete optimizer;
5929 
5930  std::map<int, Transform> filteredScanPoses = scanPoses;
5931  float proximityFilteringRadius = 0.0f;
5932  Parameters::parse(parameters, Parameters::kRGBDProximityPathFilteringRadius(), proximityFilteringRadius);
5933  if(scanPoses.size() > 2 && proximityFilteringRadius > 0.0f)
5934  {
5935  // path filtering
5936  filteredScanPoses = graph::radiusPosesFiltering(scanPoses, proximityFilteringRadius, 0, true);
5937  // make sure the current pose is still here
5938  filteredScanPoses.insert(*scanPoses.find(currentLink.to()));
5939  }
5940 
5941  Transform toPoseInv = filteredScanPoses.at(currentLink.to()).inverse();
5942  LaserScan fromScan;
5943  dataFrom.uncompressData(0,0,&fromScan);
5944  int maxPoints = fromScan.size();
5945  pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
5946  pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(new pcl::PointCloud<pcl::PointNormal>);
5947  for(std::map<int, Transform>::const_iterator iter = filteredScanPoses.begin(); iter!=filteredScanPoses.end(); ++iter)
5948  {
5949  if(iter->first != currentLink.from())
5950  {
5951  SensorData data;
5952  dbDriver_->getNodeData(iter->first, data);
5953  if(!data.laserScanCompressed().isEmpty())
5954  {
5955  LaserScan scan;
5956  data.uncompressData(0, 0, &scan);
5957  if(!scan.isEmpty() && fromScan.format() == scan.format())
5958  {
5959  if(scan.hasNormals())
5960  {
5961  *assembledToNormalClouds += *util3d::laserScanToPointCloudNormal(scan, toPoseInv * iter->second * scan.localTransform());
5962  }
5963  else
5964  {
5965  *assembledToClouds += *util3d::laserScanToPointCloud(scan, toPoseInv * iter->second * scan.localTransform());
5966  }
5967 
5968  if(scan.size() > maxPoints)
5969  {
5970  maxPoints = scan.size();
5971  }
5972  }
5973  }
5974  else
5975  {
5976  UWARN("Laser scan not found for signature %d", iter->first);
5977  }
5978  }
5979  }
5980 
5981  cv::Mat assembledScan;
5982  if(assembledToNormalClouds->size())
5983  {
5984  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToNormalClouds):util3d::laserScanFromPointCloud(*assembledToNormalClouds);
5985  }
5986  else if(assembledToClouds->size())
5987  {
5988  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToClouds):util3d::laserScanFromPointCloud(*assembledToClouds);
5989  }
5990  SensorData assembledData;
5991  // scans are in base frame but for 2d scans, set the height so that correspondences matching works
5992  assembledData.setLaserScanRaw(LaserScan(
5993  assembledScan,
5994  fromScan.maxPoints()?fromScan.maxPoints():maxPoints,
5995  fromScan.maxRange(),
5996  fromScan.format(),
5997  fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
5998 
5999  RegistrationIcp registrationIcp(parameters);
6000  transform = registrationIcp.computeTransformation(dataFrom, assembledData, currentLink.transform(), &info);
6001  if(!transform.isNull())
6002  {
6003  // local scan matching proximity detection should have higher variance (see Rtabmap::process())
6004  info.covariance*=100.0;
6005  }
6006  }
6007  else
6008  {
6009  SensorData dataTo;
6010  dbDriver_->getNodeData(currentLink.to(), dataTo);
6011  Registration * registration = Registration::create(parameters);
6012  if(registration->isScanRequired())
6013  {
6014  if(ui_->checkBox_icp_from_depth->isChecked())
6015  {
6016  // generate laser scans from depth image
6017  cv::Mat tmpA, tmpB, tmpC, tmpD;
6018  dataFrom.uncompressData(&tmpA, &tmpB, 0);
6019  dataTo.uncompressData(&tmpC, &tmpD, 0);
6020  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom = util3d::cloudFromSensorData(
6021  dataFrom,
6022  ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
6023  ui_->doubleSpinBox_icp_maxDepth->value(),
6024  ui_->doubleSpinBox_icp_minDepth->value(),
6025  0,
6026  ui_->parameters_toolbox->getParameters());
6027  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo = util3d::cloudFromSensorData(
6028  dataTo,
6029  ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
6030  ui_->doubleSpinBox_icp_maxDepth->value(),
6031  ui_->doubleSpinBox_icp_minDepth->value(),
6032  0,
6033  ui_->parameters_toolbox->getParameters());
6034  int maxLaserScans = cloudFrom->size();
6035  dataFrom.setLaserScanRaw(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0, LaserScan::kXYZ));
6036  dataTo.setLaserScanRaw(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0, LaserScan::kXYZ));
6037 
6038  if(!dataFrom.laserScanCompressed().isEmpty() || !dataTo.laserScanCompressed().isEmpty())
6039  {
6040  UWARN("There are laser scans in data, but generate laser scan from "
6041  "depth image option is activated. Ignoring saved laser scans...");
6042  }
6043  }
6044  else
6045  {
6046  LaserScan tmpA, tmpB;
6047  dataFrom.uncompressData(0, 0, &tmpA);
6048  dataTo.uncompressData(0, 0, &tmpB);
6049  }
6050  }
6051 
6052  if(registration->isImageRequired())
6053  {
6054  cv::Mat tmpA, tmpB, tmpC, tmpD;
6055  dataFrom.uncompressData(&tmpA, &tmpB, 0);
6056  dataTo.uncompressData(&tmpC, &tmpD, 0);
6057  }
6058 
6059  UINFO("Uncompress time: %f s", timer.ticks());
6060 
6061  fromS = Signature(dataFrom);
6062  toS = Signature(dataTo);
6063  transform = registration->computeTransformationMod(fromS, toS, t, &info);
6064  delete registration;
6065  }
6066  UINFO("(%d ->%d) Registration time: %f s", from, to, timer.ticks());
6067 
6068  if(!transform.isNull())
6069  {
6070  if(!transform.isIdentity())
6071  {
6072  if(info.covariance.at<double>(0,0)<=0.0)
6073  {
6074  info.covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
6075  }
6076  }
6077  Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform, info.covariance.inv(), currentLink.userDataCompressed());
6078 
6079  bool updated = false;
6080  std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
6081  while(iter != linksRefined_.end() && iter->first == currentLink.from())
6082  {
6083  if(iter->second.to() == currentLink.to() &&
6084  iter->second.type() == currentLink.type())
6085  {
6086  iter->second = newLink;
6087  updated = true;
6088  break;
6089  }
6090  ++iter;
6091  }
6092  if(!updated)
6093  {
6094  linksRefined_.insert(std::make_pair(newLink.from(), newLink));
6095 
6096  if(!silent)
6097  {
6098  this->updateGraphView();
6099  }
6100  }
6101 
6102  if(!silent && ui_->dockWidget_constraints->isVisible())
6103  {
6104  if(fromS.id() > 0 && toS.id() > 0)
6105  {
6106  this->updateConstraintView(newLink, true, fromS, toS);
6107 
6108  ui_->graphicsView_A->setFeatures(fromS.getWords(), fromS.sensorData().depthRaw());
6109  ui_->graphicsView_B->setFeatures(toS.getWords(), toS.sensorData().depthRaw());
6110  updateWordsMatching();
6111  }
6112  else
6113  {
6114  this->updateConstraintView();
6115  }
6116  }
6117  }
6118 
6119  else if(!silent)
6120  {
6121  QMessageBox::warning(this,
6122  tr("Refine link"),
6123  tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(info.rejectedMsg.c_str()));
6124  }
6125 }
6126 
6127 void DatabaseViewer::addConstraint()
6128 {
6129  int from = ids_.at(ui_->horizontalSlider_A->value());
6130  int to = ids_.at(ui_->horizontalSlider_B->value());
6131  addConstraint(from, to, false);
6132 }
6133 
6134 bool DatabaseViewer::addConstraint(int from, int to, bool silent)
6135 {
6136  bool switchedIds = false;
6137  if(from == to)
6138  {
6139  UWARN("Cannot add link to same node");
6140  return false;
6141  }
6142  else if(from < to)
6143  {
6144  switchedIds = true;
6145  int tmp = from;
6146  from = to;
6147  to = tmp;
6148  }
6149 
6150  Link newLink;
6151  if(!containsLink(linksAdded_, from, to) &&
6152  !containsLink(links_, from, to))
6153  {
6154  UASSERT(!containsLink(linksRemoved_, from, to));
6155  UASSERT(!containsLink(linksRefined_, from, to));
6156 
6157  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
6158  Registration * reg = Registration::create(parameters);
6159 
6160  Transform t;
6161  RegistrationInfo info;
6162 
6163  std::list<int> ids;
6164  ids.push_back(from);
6165  ids.push_back(to);
6166  std::list<Signature*> signatures;
6167  dbDriver_->loadSignatures(ids, signatures);
6168  if(signatures.size() != 2)
6169  {
6170  for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
6171  {
6172  delete *iter;
6173  return false;
6174  }
6175  }
6176  Signature * fromS = *signatures.begin();
6177  Signature * toS = *signatures.rbegin();
6178 
6179  bool reextractVisualFeatures = uStr2Bool(parameters.at(Parameters::kRGBDLoopClosureReextractFeatures()));
6180  if(reg->isScanRequired() ||
6181  reg->isUserDataRequired() ||
6182  reextractVisualFeatures)
6183  {
6184  // Add sensor data to generate features
6185  dbDriver_->getNodeData(from, fromS->sensorData(), reextractVisualFeatures, reg->isScanRequired(), reg->isUserDataRequired(), false);
6186  fromS->sensorData().uncompressData();
6187  dbDriver_->getNodeData(to, toS->sensorData());
6188  toS->sensorData().uncompressData();
6189  if(reextractVisualFeatures)
6190  {
6191  fromS->setWords(std::multimap<int, cv::KeyPoint>());
6192  fromS->setWords3(std::multimap<int, cv::Point3f>());
6193  fromS->setWordsDescriptors(std::multimap<int, cv::Mat>());
6194  fromS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
6195  toS->setWords(std::multimap<int, cv::KeyPoint>());
6196  toS->setWords3(std::multimap<int, cv::Point3f>());
6197  toS->setWordsDescriptors(std::multimap<int, cv::Mat>());
6198  toS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
6199  }
6200  }
6201  else if(!reextractVisualFeatures && fromS->getWords().empty() && toS->getWords().empty())
6202  {
6203  UWARN("\"%s\" is false and signatures (%d and %d) don't have words, "
6204  "registration will not be possible. Set \"%s\" to true.",
6205  Parameters::kRGBDLoopClosureReextractFeatures().c_str(),
6206  fromS->id(),
6207  toS->id(),
6208  Parameters::kRGBDLoopClosureReextractFeatures().c_str());
6209  }
6210 
6211  t = reg->computeTransformationMod(*fromS, *toS, Transform(), &info);
6212  delete reg;
6213  UDEBUG("");
6214 
6215  if(!silent)
6216  {
6217  if(switchedIds)
6218  {
6219  ui_->graphicsView_A->setFeatures(toS->getWords(), toS->sensorData().depthRaw());
6220  ui_->graphicsView_B->setFeatures(fromS->getWords(), fromS->sensorData().depthRaw());
6221  }
6222  else
6223  {
6224  ui_->graphicsView_A->setFeatures(fromS->getWords(), fromS->sensorData().depthRaw());
6225  ui_->graphicsView_B->setFeatures(toS->getWords(), toS->sensorData().depthRaw());
6226  }
6227  updateWordsMatching();
6228  }
6229 
6230  if(!t.isNull())
6231  {
6232  if(!t.isIdentity())
6233  {
6234  // normalize variance
6235  info.covariance *= t.getNorm();
6236  if(info.covariance.at<double>(0,0)<=0.0)
6237  {
6238  info.covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
6239  }
6240  }
6241  newLink = Link(from, to, Link::kUserClosure, t, info.covariance.inv());
6242  }
6243  else if(!silent)
6244  {
6245  QMessageBox::warning(this,
6246  tr("Add link"),
6247  tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(info.rejectedMsg.c_str()));
6248  }
6249 
6250  for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
6251  {
6252  delete *iter;
6253  }
6254  }
6255  else if(containsLink(linksRemoved_, from, to))
6256  {
6257  newLink = rtabmap::graph::findLink(linksRemoved_, from, to)->second;
6258  }
6259 
6260  bool updateConstraints = newLink.isValid();
6261  float maxOptimizationError = uStr2Float(ui_->parameters_toolbox->getParameters().at(Parameters::kRGBDOptimizeMaxError()));
6262  if(newLink.isValid() &&
6263  maxOptimizationError > 0.0f &&
6264  uStr2Int(ui_->parameters_toolbox->getParameters().at(Parameters::kOptimizerIterations())) > 0.0f)
6265  {
6266  int fromId = newLink.from();
6267  int mapId = mapIds_.at(newLink.from());
6268  // use first node of the map containing from
6269  for(std::map<int, int>::iterator iter=mapIds_.begin(); iter!=mapIds_.end(); ++iter)
6270  {
6271  if(iter->second == mapId)
6272  {
6273  fromId = iter->first;
6274  break;
6275  }
6276  }
6277  std::multimap<int, Link> linksIn = updateLinksWithModifications(links_);
6278  linksIn.insert(std::make_pair(newLink.from(), newLink));
6279  const Link * maxLinearLink = 0;
6280  const Link * maxAngularLink = 0;
6281  float maxLinearErrorRatio = 0.0f;
6282  float maxAngularErrorRatio = 0.0f;
6283  Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
6284  std::map<int, Transform> poses;
6285  std::multimap<int, Link> links;
6286  UASSERT(odomPoses_.find(fromId) != odomPoses_.end());
6287  UASSERT_MSG(odomPoses_.find(newLink.from()) != odomPoses_.end(), uFormat("id=%d poses=%d links=%d", newLink.from(), (int)poses.size(), (int)links.size()).c_str());
6288  UASSERT_MSG(odomPoses_.find(newLink.to()) != odomPoses_.end(), uFormat("id=%d poses=%d links=%d", newLink.to(), (int)poses.size(), (int)links.size()).c_str());
6289  optimizer->getConnectedGraph(fromId, odomPoses_, linksIn, poses, links);
6290  UASSERT(poses.find(fromId) != poses.end());
6291  UASSERT_MSG(poses.find(newLink.from()) != poses.end(), uFormat("id=%d poses=%d links=%d", newLink.from(), (int)poses.size(), (int)links.size()).c_str());
6292  UASSERT_MSG(poses.find(newLink.to()) != poses.end(), uFormat("id=%d poses=%d links=%d", newLink.to(), (int)poses.size(), (int)links.size()).c_str());
6293  UASSERT(graph::findLink(links, newLink.from(), newLink.to()) != links.end());
6294  poses = optimizer->optimize(fromId, poses, links);
6295  std::string msg;
6296  if(poses.size())
6297  {
6298  float maxLinearError = 0.0f;
6299  float maxAngularError = 0.0f;
6300  for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
6301  {
6302  // ignore links with high variance
6303  if(iter->second.transVariance() <= 1.0 && iter->second.from() != iter->second.to())
6304  {
6305  Transform t1 = uValue(poses, iter->second.from(), Transform());
6306  Transform t2 = uValue(poses, iter->second.to(), Transform());
6307  Transform t = t1.inverse()*t2;
6308  float linearError = uMax3(
6309  fabs(iter->second.transform().x() - t.x()),
6310  fabs(iter->second.transform().y() - t.y()),
6311  fabs(iter->second.transform().z() - t.z()));
6312  float opt_roll,opt__pitch,opt__yaw;
6313  float link_roll,link_pitch,link_yaw;
6314  t.getEulerAngles(opt_roll, opt__pitch, opt__yaw);
6315  iter->second.transform().getEulerAngles(link_roll, link_pitch, link_yaw);
6316  float angularError = uMax3(
6317  fabs(opt_roll - link_roll),
6318  fabs(opt__pitch - link_pitch),
6319  fabs(opt__yaw - link_yaw));
6320  float stddevLinear = sqrt(iter->second.transVariance());
6321  float linearErrorRatio = linearError/stddevLinear;
6322  if(linearErrorRatio > maxLinearErrorRatio)
6323  {
6324  maxLinearError = linearError;
6325  maxLinearErrorRatio = linearErrorRatio;
6326  maxLinearLink = &iter->second;
6327  }
6328  float stddevAngular = sqrt(iter->second.rotVariance());
6329  float angularErrorRatio = angularError/stddevAngular;
6330  if(angularErrorRatio > maxAngularErrorRatio)
6331  {
6332  maxAngularError = angularError;
6333  maxAngularErrorRatio = angularErrorRatio;
6334  maxAngularLink = &iter->second;
6335  }
6336  }
6337  }
6338  if(maxLinearLink)
6339  {
6340  UINFO("Max optimization linear error = %f m (link %d->%d, var=%f, ratio error/std=%f)", maxLinearError, maxLinearLink->from(), maxLinearLink->to(), maxLinearLink->transVariance(), maxLinearError/sqrt(maxLinearLink->transVariance()));
6341  if(maxLinearErrorRatio > maxOptimizationError)
6342  {
6343  msg = uFormat("Rejecting edge %d->%d because "
6344  "graph error is too large after optimization (ratio %f for edge %d->%d, stddev=%f). "
6345  "\"%s\" is %f.",
6346  newLink.from(),
6347  newLink.to(),
6348  maxLinearErrorRatio,
6349  maxLinearLink->from(),
6350  maxLinearLink->to(),
6351  sqrt(maxLinearLink->transVariance()),
6352  Parameters::kRGBDOptimizeMaxError().c_str(),
6353  maxOptimizationError);
6354  }
6355  }
6356  if(maxAngularLink)
6357  {
6358  UINFO("Max optimization angular error = %f deg (link %d->%d, var=%f, ratio error/std=%f)", maxAngularError*180.0f/CV_PI, maxAngularLink->from(), maxAngularLink->to(), maxAngularLink->rotVariance(), maxAngularError/sqrt(maxAngularLink->rotVariance()));
6359  if(maxAngularErrorRatio > maxOptimizationError)
6360  {
6361  msg = uFormat("Rejecting edge %d->%d because "
6362  "graph error is too large after optimization (ratio %f for edge %d->%d, stddev=%f). "
6363  "\"%s\" is %f.",
6364  newLink.from(),
6365  newLink.to(),
6366  maxAngularErrorRatio,
6367  maxAngularLink->from(),
6368  maxAngularLink->to(),
6369  sqrt(maxAngularLink->rotVariance()),
6370  Parameters::kRGBDOptimizeMaxError().c_str(),
6371  maxOptimizationError);
6372  }
6373  }
6374  }
6375  else
6376  {
6377  msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
6378  newLink.from(),
6379  newLink.to());
6380  }
6381  if(!msg.empty())
6382  {
6383  UWARN("%s", msg.c_str());
6384  if(!silent)
6385  {
6386  QMessageBox::warning(this,
6387  tr("Add link"),
6388  tr("%1").arg(msg.c_str()));
6389  }
6390 
6391  updateConstraints = false;
6392  }
6393  }
6394 
6395  if(updateConstraints)
6396  {
6397  if(containsLink(linksRemoved_, from, to))
6398  {
6399  //simply remove from linksRemoved
6400  linksRemoved_.erase(rtabmap::graph::findLink(linksRemoved_, from, to));
6401  }
6402  else
6403  {
6404  linksAdded_.insert(std::make_pair(newLink.from(), newLink));
6405  }
6406  if(!silent)
6407  {
6408  updateLoopClosuresSlider(from, to);
6409  this->updateGraphView();
6410  }
6411  }
6412 
6413  return updateConstraints;
6414 }
6415 
6416 void DatabaseViewer::resetConstraint()
6417 {
6418  int from = ids_.at(ui_->horizontalSlider_A->value());
6419  int to = ids_.at(ui_->horizontalSlider_B->value());
6420  if(from < to)
6421  {
6422  int tmp = to;
6423  to = from;
6424  from = tmp;
6425  }
6426 
6427  if(from == to)
6428  {
6429  UWARN("Cannot reset link to same node");
6430  return;
6431  }
6432 
6433 
6434  std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, from, to);
6435  if(iter != linksRefined_.end())
6436  {
6437  linksRefined_.erase(iter);
6438  this->updateGraphView();
6439  }
6440 
6441  iter = rtabmap::graph::findLink(links_, from, to);
6442  if(iter != links_.end())
6443  {
6444  this->updateConstraintView(iter->second);
6445  }
6446  iter = rtabmap::graph::findLink(linksAdded_, from, to);
6447  if(iter != linksAdded_.end())
6448  {
6449  this->updateConstraintView(iter->second);
6450  }
6451 }
6452 
6453 void DatabaseViewer::rejectConstraint()
6454 {
6455  int from = ids_.at(ui_->horizontalSlider_A->value());
6456  int to = ids_.at(ui_->horizontalSlider_B->value());
6457  if(from < to)
6458  {
6459  int tmp = to;
6460  to = from;
6461  from = tmp;
6462  }
6463 
6464  if(from == to)
6465  {
6466  UWARN("Cannot reject link to same node");
6467  return;
6468  }
6469 
6470  bool removed = false;
6471 
6472  // find the original one
6473  std::multimap<int, Link>::iterator iter;
6474  iter = rtabmap::graph::findLink(links_, from, to);
6475  if(iter != links_.end())
6476  {
6477  if(iter->second.type() == Link::kNeighbor || iter->second.type() == Link::kNeighborMerged)
6478  {
6479  UWARN("Cannot reject neighbor links (%d->%d)", from, to);
6480  return;
6481  }
6482  linksRemoved_.insert(*iter);
6483  removed = true;
6484  }
6485 
6486  // remove from refined and added
6487  iter = rtabmap::graph::findLink(linksRefined_, from, to);
6488  if(iter != linksRefined_.end())
6489  {
6490  linksRefined_.erase(iter);
6491  removed = true;
6492  }
6493  iter = rtabmap::graph::findLink(linksAdded_, from, to);
6494  if(iter != linksAdded_.end())
6495  {
6496  linksAdded_.erase(iter);
6497  removed = true;
6498  }
6499  if(removed)
6500  {
6501  this->updateGraphView();
6502  }
6503  updateLoopClosuresSlider();
6504 }
6505 
6506 std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
6507  const std::multimap<int, rtabmap::Link> & edgeConstraints)
6508 {
6509  std::multimap<int, rtabmap::Link> links;
6510  for(std::multimap<int, rtabmap::Link>::const_iterator iter=edgeConstraints.begin();
6511  iter!=edgeConstraints.end();
6512  ++iter)
6513  {
6514  std::multimap<int, rtabmap::Link>::iterator findIter;
6515 
6516  findIter = rtabmap::graph::findLink(linksRemoved_, iter->second.from(), iter->second.to());
6517  if(findIter != linksRemoved_.end())
6518  {
6519  if(!(iter->second.from() == findIter->second.from() &&
6520  iter->second.to() == findIter->second.to() &&
6521  iter->second.type() == findIter->second.type()))
6522  {
6523  UWARN("Links (%d->%d,%d) and (%d->%d,%d) are not equal!?",
6524  iter->second.from(), iter->second.to(), iter->second.type(),
6525  findIter->second.from(), findIter->second.to(), findIter->second.type());
6526  }
6527  else
6528  {
6529  //UINFO("Removed link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
6530  continue; // don't add this link
6531  }
6532  }
6533 
6534  findIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
6535  if(findIter!=linksRefined_.end())
6536  {
6537  if(iter->second.from() == findIter->second.from() &&
6538  iter->second.to() == findIter->second.to() &&
6539  iter->second.type() == findIter->second.type())
6540  {
6541  links.insert(*findIter); // add the refined link
6542  //UINFO("Updated link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
6543  continue;
6544  }
6545  else
6546  {
6547  UWARN("Links (%d->%d,%d) and (%d->%d,%d) are not equal!?",
6548  iter->second.from(), iter->second.to(), iter->second.type(),
6549  findIter->second.from(), findIter->second.to(), findIter->second.type());
6550  }
6551  }
6552 
6553  links.insert(*iter); // add original link
6554  }
6555 
6556  //look for added links
6557  for(std::multimap<int, rtabmap::Link>::const_iterator iter=linksAdded_.begin();
6558  iter!=linksAdded_.end();
6559  ++iter)
6560  {
6561  //UINFO("Added link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
6562  links.insert(*iter);
6563  }
6564 
6565  return links;
6566 }
6567 
6568 void DatabaseViewer::updateLoopClosuresSlider(int from, int to)
6569 {
6570  int size = loopLinks_.size();
6571  loopLinks_.clear();
6572  std::multimap<int, Link> links = updateLinksWithModifications(links_);
6573  int position = ui_->horizontalSlider_loops->value();
6574  std::multimap<int, Link> linksSortedByParents;
6575  for(std::multimap<int, rtabmap::Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
6576  {
6577  if(iter->second.to() > iter->second.from())
6578  {
6579  linksSortedByParents.insert(std::make_pair(iter->second.to(), iter->second.inverse()));
6580  }
6581  else
6582  {
6583  linksSortedByParents.insert(*iter);
6584  }
6585  }
6586  for(std::multimap<int, rtabmap::Link>::iterator iter = linksSortedByParents.begin(); iter!=linksSortedByParents.end(); ++iter)
6587  {
6588  if(!iter->second.transform().isNull())
6589  {
6590  if(iter->second.type() != rtabmap::Link::kNeighbor &&
6591  iter->second.type() != rtabmap::Link::kNeighborMerged)
6592  {
6593  if((iter->second.from() == from && iter->second.to() == to) ||
6594  (iter->second.to() == from && iter->second.from() == to))
6595  {
6596  position = loopLinks_.size();
6597  }
6598  loopLinks_.append(iter->second);
6599  }
6600  }
6601  else
6602  {
6603  UERROR("Transform null for link from %d to %d", iter->first, iter->second.to());
6604  }
6605  }
6606 
6607  if(loopLinks_.size())
6608  {
6609  if(loopLinks_.size() == 1)
6610  {
6611  // just to be able to move the cursor of the loop slider
6612  loopLinks_.push_back(loopLinks_.front());
6613  }
6614  ui_->horizontalSlider_loops->setMinimum(0);
6615  ui_->horizontalSlider_loops->setMaximum(loopLinks_.size()-1);
6616  ui_->horizontalSlider_loops->setEnabled(true);
6617  if(position != ui_->horizontalSlider_loops->value())
6618  {
6619  ui_->horizontalSlider_loops->setValue(position);
6620  }
6621  else if(size != loopLinks_.size())
6622  {
6623  this->updateConstraintView(loopLinks_.at(position));
6624  }
6625  }
6626  else
6627  {
6628  ui_->horizontalSlider_loops->setEnabled(false);
6629  constraintsViewer_->removeAllClouds();
6630  constraintsViewer_->update();
6631  updateConstraintButtons();
6632  }
6633 }
6634 
6635 void DatabaseViewer::notifyParametersChanged(const QStringList & parametersChanged)
6636 {
6637  bool updateStereo = false;
6638  bool updateGraphView = false;
6639  for(QStringList::const_iterator iter=parametersChanged.constBegin();
6640  iter!=parametersChanged.constEnd() && (!updateStereo || !updateGraphView);
6641  ++iter)
6642  {
6643  QString group = iter->split('/').first();
6644  if(!updateStereo && group == "Stereo")
6645  {
6646  updateStereo = true;
6647  continue;
6648  }
6649  if(!updateGraphView && group == "Optimize")
6650  {
6651  updateGraphView = true;
6652  continue;
6653  }
6654  }
6655 
6656  if(updateStereo)
6657  {
6658  this->updateStereo();
6659  }
6660  if(updateGraphView)
6661  {
6662  this->updateGraphView();
6663  }
6664  this->configModified();
6665 }
6666 
6667 } // namespace rtabmap
rtabmap::Signature::getWords3
const std::multimap< int, cv::Point3f > & getWords3() const
Definition: Signature.h:128
uStr2Int
int UTILITE_EXP uStr2Int(const std::string &str)
Definition: UConversion.cpp:118
rtabmap::CloudViewer::removeGraph
void removeGraph(const std::string &id)
Definition: CloudViewer.cpp:1860
rtabmap::util3d::laserScanFromPointCloud
LaserScan RTABMAP_EXP laserScanFromPointCloud(const pcl::PCLPointCloud2 &cloud, bool filterNaNs=true)
Definition: util3d.cpp:1266
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
rtabmap::Registration::isUserDataRequired
bool isUserDataRequired() const
Definition: Registration.cpp:109
rtabmap::util3d::isFinite
bool RTABMAP_EXP isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3035
rtabmap::CameraModel::imageWidth
int imageWidth() const
Definition: CameraModel.h:113
rtabmap::EditDepthArea::setImage
void setImage(const cv::Mat &depth, const cv::Mat &rgb=cv::Mat())
Definition: EditDepthArea.cpp:58
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:446
rtabmap::DatabaseViewer::updateLinksWithModifications
std::multimap< int, rtabmap::Link > updateLinksWithModifications(const std::multimap< int, rtabmap::Link > &edgeConstraints)
Definition: DatabaseViewer.cpp:6506
rtabmap::ExportDialog::sessionExported
int sessionExported() const
Definition: ExportDialog.cpp:140
rtabmap::DatabaseViewer::generatedLocalMapsInfo_
std::map< int, std::pair< float, cv::Point3f > > generatedLocalMapsInfo_
Definition: DatabaseViewer.h:211
rtabmap::util3d::mergeTextures
cv::Mat RTABMAP_EXP mergeTextures(pcl::TextureMesh &mesh, const std::map< int, cv::Mat > &images, const std::map< int, CameraModel > &calibrations, const Memory *memory=0, const DBDriver *dbDriver=0, int textureSize=4096, int textureCount=1, const std::vector< std::map< int, pcl::PointXY > > &vertexToPixels=std::vector< std::map< int, pcl::PointXY > >(), bool gainCompensation=true, float gainBeta=10.0f, bool gainRGB=true, bool blending=true, int blendingDecimation=0, int brightnessContrastRatioLow=0, int brightnessContrastRatioHigh=0, bool exposureFusion=false, const ProgressState *state=0)
Definition: util3d_surface.cpp:1412
rtabmap::ProgressDialog::incrementStep
void incrementStep(int steps=1)
Definition: ProgressDialog.cpp:138
rtabmap::EditDepthArea::isModified
bool isModified() const
Definition: EditDepthArea.h:53
rtabmap::DBDriver::loadOptimizedMesh
cv::Mat loadOptimizedMesh(std::vector< std::vector< std::vector< unsigned int > > > *polygons=0, std::vector< std::vector< Eigen::Vector2f > > *texCoords=0, cv::Mat *textures=0) const
Definition: DBDriver.cpp:1108
rtabmap::util3d::transformFromXYZCorrespondencesSVD
Transform RTABMAP_EXP transformFromXYZCorrespondencesSVD(const pcl::PointCloud< pcl::PointXYZ > &cloud1, const pcl::PointCloud< pcl::PointXYZ > &cloud2)
Definition: util3d_registration.cpp:50
rtabmap::DatabaseViewer::getIniFilePath
QString getIniFilePath() const
Definition: DatabaseViewer.cpp:458
rtabmap::SensorData::uncompressDataConst
void uncompressDataConst(cv::Mat *imageRaw, cv::Mat *depthOrRightRaw, LaserScan *laserScanRaw=0, cv::Mat *userDataRaw=0, cv::Mat *groundCellsRaw=0, cv::Mat *obstacleCellsRaw=0, cv::Mat *emptyCellsRaw=0) const
Definition: SensorData.cpp:656
UTimer
Definition: UTimer.h:46
rtabmap::graph::exportPoses
bool RTABMAP_EXP exportPoses(const std::string &filePath, int format, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints=std::multimap< int, Link >(), const std::map< int, double > &stamps=std::map< int, double >(), bool g2oRobust=false)
Definition: Graph.cpp:53
rtabmap::DBDriver::getFeaturesMemoryUsed
long getFeaturesMemoryUsed() const
Definition: DBDriver.cpp:191
rtabmap::DBDriver::getGridsMemoryUsed
long getGridsMemoryUsed() const
Definition: DBDriver.cpp:159
rtabmap::DatabaseViewer::ui_
Ui_DatabaseViewer * ui_
Definition: DatabaseViewer.h:183
uMean
T uMean(const T *v, unsigned int size)
Definition: UMath.h:401
rtabmap::DBDriver::getImagesMemoryUsed
long getImagesMemoryUsed() const
Definition: DBDriver.cpp:135
rtabmap::util3d::projectDisparityTo3D
cv::Point3f RTABMAP_EXP projectDisparityTo3D(const cv::Point2f &pt, float disparity, const StereoCameraModel &model)
Definition: util3d.cpp:2633
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:274
UFile::getName
std::string getName()
Definition: UFile.h:135
UFile::length
long length()
Definition: UFile.h:110
rtabmap::graph::radiusPosesFiltering
std::map< int, Transform > RTABMAP_EXP radiusPosesFiltering(const std::map< int, Transform > &poses, float radius, float angle, bool keepLatest=true)
Definition: Graph.cpp:1045
rtabmap::Transform::getTranslation
void getTranslation(float &x, float &y, float &z) const
Definition: Transform.cpp:217
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:193
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:63
rtabmap::ProgressDialog::setCancelButtonVisible
void setCancelButtonVisible(bool visible)
Definition: ProgressDialog.cpp:97
rtabmap::DBDriver::saveOptimizedPoses
void saveOptimizedPoses(const std::map< int, Transform > &optimizedPoses, const Transform &lastlocalizationPose) const
Definition: DBDriver.cpp:1064
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:66
uKeys
std::vector< K > uKeys(const std::multimap< K, V > &mm)
Definition: UStl.h:67
rtabmap::DBDriver::getUserDataMemoryUsed
long getUserDataMemoryUsed() const
Definition: DBDriver.cpp:175
rtabmap::SensorData::laserScanCompressed
const LaserScan & laserScanCompressed() const
Definition: SensorData.h:159
rtabmap::StereoCameraModel::R
const cv::Mat & R() const
Definition: StereoCameraModel.h:107
uStr2Double
double UTILITE_EXP uStr2Double(const std::string &str)
Definition: UConversion.cpp:132
rtabmap::CloudViewer::updateCameraFrustum
void updateCameraFrustum(const Transform &pose, const StereoCameraModel &model)
Definition: CloudViewer.cpp:2468
rtabmap::LaserScan::hasNormals
bool hasNormals() const
Definition: LaserScan.h:73
rtabmap::util3d::transformPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
Definition: util3d_transforms.cpp:89
uValueAt
V & uValueAt(std::list< V > &list, const unsigned int &pos)
Definition: UStl.h:382
rtabmap::Signature::getWords
const std::multimap< int, cv::KeyPoint > & getWords() const
Definition: Signature.h:108
rtabmap::Signature::setWords3
void setWords3(const std::multimap< int, cv::Point3f > &words3)
Definition: Signature.h:115
rtabmap::DatabaseViewer::resizeEvent
virtual void resizeEvent(QResizeEvent *anEvent)
Definition: DatabaseViewer.cpp:1125
rtabmap::ExportDialog::isDepthExported
bool isDepthExported() const
Definition: ExportDialog.cpp:150
f
f
rtabmap::CloudViewer::addOrUpdateCoordinate
void addOrUpdateCoordinate(const std::string &id, const Transform &transform, double scale, bool foreground=false)
Definition: CloudViewer.cpp:1269
rtabmap::LaserScan::maxRange
float maxRange() const
Definition: LaserScan.h:65
rtabmap::DatabaseViewer::gpsPoses_
std::map< int, rtabmap::Transform > gpsPoses_
Definition: DatabaseViewer.h:202
rtabmap::DBDriver::loadLinks
void loadLinks(int signatureId, std::map< int, Link > &links, Link::Type type=Link::kUndef) const
Definition: DBDriver.cpp:763
rtabmap::OctoMap::update
void update(const std::map< int, Transform > &poses)
Definition: OctoMap.cpp:367
rtabmap::LaserScan::channels
static int channels(Format format)
Definition: LaserScan.cpp:34
rtabmap::Signature::setWords
void setWords(const std::multimap< int, cv::KeyPoint > &words)
Definition: Signature.cpp:231
rtabmap::Registration::computeTransformationMod
Transform computeTransformationMod(Signature &from, Signature &to, Transform guess=Transform::getIdentity(), RegistrationInfo *info=0) const
Definition: Registration.cpp:188
rtabmap::graph::exportGPS
bool RTABMAP_EXP exportGPS(const std::string &filePath, const std::map< int, GPS > &gpsValues, unsigned int rgba=0xFFFFFFFF)
Definition: Graph.cpp:467
rtabmap::Registration::computeTransformation
Transform computeTransformation(const Signature &from, const Signature &to, Transform guess=Transform::getIdentity(), RegistrationInfo *info=0) const
Definition: Registration.cpp:166
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
rtabmap::DatabaseViewer::closeEvent
virtual void closeEvent(QCloseEvent *event)
Definition: DatabaseViewer.cpp:1058
uStrNumCmp
int uStrNumCmp(const std::string &a, const std::string &b)
Definition: UStl.h:719
rtabmap::DBDriver::getAllNodeIds
void getAllNodeIds(std::set< int > &ids, bool ignoreChildren=false, bool ignoreBadSignatures=false) const
Definition: DBDriver.cpp:813
UDirectory::getDir
static std::string getDir(const std::string &filePath)
Definition: UDirectory.cpp:273
rtabmap::DatabaseViewer::showEvent
virtual void showEvent(QShowEvent *anEvent)
Definition: DatabaseViewer.cpp:1102
rtabmap::Transform::rotation
Transform rotation() const
Definition: Transform.cpp:174
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:42
rtabmap::DatabaseViewer::graphes_
std::list< std::map< int, rtabmap::Transform > > graphes_
Definition: DatabaseViewer.h:198
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:364
rtabmap::util3d::laserScanToPointCloudRGBNormal
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_EXP laserScanToPointCloudRGBNormal(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=255, unsigned char g=255, unsigned char b=255)
Definition: util3d.cpp:2399
rtabmap::DatabaseViewer::constraintsViewer_
CloudViewer * constraintsViewer_
Definition: DatabaseViewer.h:184
rtabmap::DataRecorder::addData
void addData(const rtabmap::SensorData &data, const Transform &pose=Transform(), const cv::Mat &infMatrix=cv::Mat::eye(6, 6, CV_64FC1))
Definition: DataRecorder.cpp:126
uStr2Bool
bool UTILITE_EXP uStr2Bool(const char *str)
Definition: UConversion.cpp:155
rtabmap::CloudViewer::removeCloud
bool removeCloud(const std::string &id)
Definition: CloudViewer.cpp:2082
rtabmap::Feature2D::generateKeypoints
std::vector< cv::KeyPoint > generateKeypoints(const cv::Mat &image, const cv::Mat &mask=cv::Mat()) const
Definition: Features2d.cpp:497
rtabmap::util3d::removeNaNFromPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP removeNaNFromPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud)
Definition: util3d_filtering.cpp:745
rtabmap::DBDriver::getAllLinks
void getAllLinks(std::multimap< int, Link > &links, bool ignoreNullLinks=true) const
Definition: DBDriver.cpp:852
rtabmap::util3d::laserScanToPointCloud2
pcl::PCLPointCloud2::Ptr RTABMAP_EXP laserScanToPointCloud2(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2289
rtabmap::DBDriver::getWordsMemoryUsed
long getWordsMemoryUsed() const
Definition: DBDriver.cpp:183
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:69
rtabmap::LaserScan::hasRGB
bool hasRGB() const
Definition: LaserScan.h:74
uStr2Float
float UTILITE_EXP uStr2Float(const std::string &str)
Definition: UConversion.cpp:123
rtabmap::DatabaseViewer::exportGPS
void exportGPS(int format)
Definition: DatabaseViewer.cpp:2333
glm::all
GLM_FUNC_DECL bool all(vecType< bool, P > const &v)
rtabmap::CloudViewer::addOccupancyGridMap
bool addOccupancyGridMap(const cv::Mat &map8U, float resolution, float xMin, float yMin, float opacity)
Definition: CloudViewer.cpp:1203
rtabmap::ExportCloudsDialog::viewClouds
void viewClouds(const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, const std::map< int, int > &mapIds, const QMap< int, Signature > &cachedSignatures, const std::map< int, std::pair< pcl::PointCloud< pcl::PointXYZRGB >::Ptr, pcl::IndicesPtr > > &cachedClouds, const std::map< int, LaserScan > &cachedScans, const QString &workingDirectory, const ParametersMap &parameters)
Definition: ExportCloudsDialog.cpp:929
rtabmap::DBDriver::getNodeData
void getNodeData(int signatureId, SensorData &data, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const
Definition: DBDriver.cpp:642
rtabmap::ExportCloudsDialog::getExportedClouds
bool getExportedClouds(const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, const std::map< int, int > &mapIds, const QMap< int, Signature > &cachedSignatures, const std::map< int, std::pair< pcl::PointCloud< pcl::PointXYZRGB >::Ptr, pcl::IndicesPtr > > &cachedClouds, const std::map< int, LaserScan > &cachedScans, const QString &workingDirectory, const ParametersMap &parameters, std::map< int, pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr > &clouds, std::map< int, pcl::PolygonMesh::Ptr > &meshes, std::map< int, pcl::TextureMesh::Ptr > &textureMeshes, std::vector< std::map< int, pcl::PointXY > > &textureVertexToPixels)
Definition: ExportCloudsDialog.cpp:1289
rtabmap::StereoCameraModel::F
const cv::Mat & F() const
Definition: StereoCameraModel.h:110
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:41
rtabmap::SensorData::gridCellSize
float gridCellSize() const
Definition: SensorData.h:223
rtabmap::Parameters::writeINI
static void writeINI(const std::string &configFile, const ParametersMap &parameters)
Definition: Parameters.cpp:1040
rtabmap::CloudViewer::clear
virtual void clear()
Definition: CloudViewer.cpp:194
rtabmap::LaserScan::maxPoints
int maxPoints() const
Definition: LaserScan.h:64
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:72
rtabmap::Optimizer::isCovarianceIgnored
bool isCovarianceIgnored() const
Definition: Optimizer.h:75
rtabmap::OccupancyGrid::setCellSize
void setCellSize(float cellSize)
Definition: OccupancyGrid.cpp:258
rtabmap::SensorData::setGPS
void setGPS(const GPS &gps)
Definition: SensorData.h:238
rtabmap::OccupancyGrid::update
void update(const std::map< int, Transform > &poses)
Definition: OccupancyGrid.cpp:685
UConversion.h
Some conversion functions.
rtabmap::SensorData::gridGroundCellsRaw
const cv::Mat & gridGroundCellsRaw() const
Definition: SensorData.h:217
UPlotCurve::addValue
void addValue(UPlotItem *data)
Definition: UPlot.cpp:420
rtabmap::DBDriver::getUrl
const std::string & getUrl() const
Definition: DBDriver.h:72
rtabmap::ImageView::setImage
void setImage(const QImage &image)
Definition: ImageView.cpp:871
rtabmap::CloudViewer::setPolygonPicking
void setPolygonPicking(bool enabled)
Definition: CloudViewer.cpp:2234
rtabmap::DBDriver::getNodeInfo
bool getNodeInfo(int signatureId, Transform &pose, int &mapId, int &weight, std::string &label, double &stamp, Transform &groundTruthPose, std::vector< float > &velocity, GPS &gps) const
Definition: DBDriver.cpp:727
rtabmap::DatabaseViewer::linksAdded_
std::multimap< int, rtabmap::Link > linksAdded_
Definition: DatabaseViewer.h:206
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:163
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:70
rtabmap::CloudViewer::buildPickingLocator
void buildPickingLocator(bool enable)
Definition: CloudViewer.cpp:2802
rtabmap::DatabaseViewer::neighborLinks_
QList< rtabmap::Link > neighborLinks_
Definition: DatabaseViewer.h:193
rtabmap::DatabaseViewer::findActiveLink
Link findActiveLink(int from, int to)
Definition: DatabaseViewer.cpp:5761
rtabmap::DatabaseViewer::links_
std::multimap< int, rtabmap::Link > links_
Definition: DatabaseViewer.h:204
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:161
rtabmap::DatabaseViewer::update
void update(int value, QLabel *labelIndex, QLabel *labelParents, QLabel *labelChildren, QLabel *weight, QLabel *label, QLabel *stamp, rtabmap::ImageView *view, QLabel *labelId, QLabel *labelMapId, QLabel *labelPose, QLabel *labelVelocity, QLabel *labeCalib, QLabel *labelGps, bool updateConstraintView)
Definition: DatabaseViewer.cpp:3396
rtabmap::DBDriver::generateGraph
void generateGraph(const std::string &fileName, const std::set< int > &ids=std::set< int >(), const std::map< int, Signature * > &otherSignatures=std::map< int, Signature * >())
Definition: DBDriver.cpp:1123
rtabmap::DatabaseViewer::notifyParametersChanged
void notifyParametersChanged(const QStringList &)
Definition: DatabaseViewer.cpp:6635
rtabmap::DatabaseViewer::exportDialog_
ExportCloudsDialog * exportDialog_
Definition: DatabaseViewer.h:214
ULogger::setLevel
static void setLevel(ULogger::Level level)
Definition: ULogger.h:339
rtabmap::DatabaseViewer::containsLink
bool containsLink(std::multimap< int, Link > &links, int from, int to)
Definition: DatabaseViewer.cpp:5788
rtabmap::util3d::laserScanToPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2328
rtabmap::GeodeticCoords::altitude
const double & altitude() const
Definition: GeodeticCoords.h:60
rtabmap::GeodeticCoords::longitude
const double & longitude() const
Definition: GeodeticCoords.h:59
util3d_registration.h
rtabmap::DatabaseViewer::idToIndex_
QMap< int, int > idToIndex_
Definition: DatabaseViewer.h:192
uMin
T uMin(const T *v, unsigned int size, unsigned int &index)
Definition: UMath.h:159
rtabmap::util3d::convertPolygonsFromPCL
std::vector< std::vector< unsigned int > > RTABMAP_EXP convertPolygonsFromPCL(const std::vector< pcl::Vertices > &polygons)
Definition: util3d_surface.cpp:1201
rtabmap::SensorData::rightRaw
cv::Mat rightRaw() const
Definition: SensorData.h:173
rtabmap::CameraModel::D
cv::Mat D() const
Definition: CameraModel.h:104
rtabmap::ExportCloudsDialog::setDBDriver
void setDBDriver(const DBDriver *dbDriver)
Definition: ExportCloudsDialog.h:66
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
rtabmap::CloudViewer::addCloudTextureMesh
bool addCloudTextureMesh(const std::string &id, const pcl::TextureMesh::Ptr &textureMesh, const cv::Mat &texture, const Transform &pose=Transform::getIdentity())
Definition: CloudViewer.cpp:767
uSplit
std::list< std::string > uSplit(const std::string &str, char separator= ' ')
Definition: UStl.h:566
rtabmap::CloudViewer::addCloud
bool addCloud(const std::string &id, const pcl::PCLPointCloud2Ptr &binaryCloud, const Transform &pose, bool rgb, bool hasNormals, bool hasIntensity, const QColor &color=QColor())
Definition: CloudViewer.cpp:455
rtabmap::util3d::cloudFromDepthRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP cloudFromDepthRGB(const cv::Mat &imageRgb, const cv::Mat &imageDepth, const CameraModel &model, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0)
Definition: util3d.cpp:428
rtabmap::DatabaseViewer::lastOptimizedGraph_
std::map< int, Transform > lastOptimizedGraph_
Definition: DatabaseViewer.h:223
rtabmap::DBDriver::getAllStatisticsWmStates
std::map< int, std::vector< int > > getAllStatisticsWmStates() const
Definition: DBDriver.cpp:266
UPlot
Definition: UPlot.h:483
rtabmap::OctoMap::hasColor
bool hasColor() const
Definition: OctoMap.h:215
rtabmap::CameraModel::R
cv::Mat R() const
Definition: CameraModel.h:105
rtabmap::DatabaseViewer::editDepthArea_
EditDepthArea * editDepthArea_
Definition: DatabaseViewer.h:216
rtabmap::util3d::laserScan2dFromPointCloud
cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
Definition: util3d.cpp:2051
util3d_mapping.h
rtabmap::DatabaseViewer::linksRefined_
std::multimap< int, rtabmap::Link > linksRefined_
Definition: DatabaseViewer.h:205
rtabmap::DBDriver::openConnection
bool openConnection(const std::string &url, bool overwritten=false)
Definition: DBDriver.cpp:86
rtabmap::ExportCloudsDialog::loadSettings
void loadSettings(QSettings &settings, const QString &group="")
Definition: ExportCloudsDialog.cpp:423
UASSERT
#define UASSERT(condition)
rtabmap::ExportDialog::targetFramerate
double targetFramerate() const
Definition: ExportDialog.cpp:135
rtabmap::ExportDialog::isDepth2dExported
bool isDepth2dExported() const
Definition: ExportDialog.cpp:155
rtabmap::EditDepthArea::getModifiedImage
cv::Mat getModifiedImage() const
Definition: EditDepthArea.cpp:82
rtabmap::DatabaseViewer::setupMainLayout
void setupMainLayout(bool vertical)
Definition: DatabaseViewer.cpp:432
rtabmap::Optimizer::optimizeIncremental
std::map< int, Transform > optimizeIncremental(int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints, std::list< std::map< int, Transform > > *intermediateGraphes=0, double *finalError=0, int *iterationsDone=0)
Definition: Optimizer.cpp:266
rtabmap::ProgressDialog::isCanceled
bool isCanceled() const
Definition: ProgressDialog.h:57
rtabmap::OctoMap::addToCache
void addToCache(int nodeId, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &ground, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &obstacles, const pcl::PointXYZ &viewPoint)
Definition: OctoMap.cpp:343
rtabmap::DBDriver::getLaserScanInfo
bool getLaserScanInfo(int signatureId, LaserScan &info) const
Definition: DBDriver.cpp:703
rtabmap::Stereo::computeCorrespondences
virtual std::vector< cv::Point2f > computeCorrespondences(const cv::Mat &leftImage, const cv::Mat &rightImage, const std::vector< cv::Point2f > &leftCorners, std::vector< unsigned char > &status) const
Definition: Stereo.cpp:72
rtabmap::ImageView::setSceneRect
void setSceneRect(const QRectF &rect)
Definition: ImageView.cpp:1037
rtabmap::StereoCameraModel::left
const CameraModel & left() const
Definition: StereoCameraModel.h:119
rtabmap::util3d::clusterPolygons
std::list< std::list< int > > RTABMAP_EXP clusterPolygons(const std::vector< std::set< int > > &neighborPolygons, int minClusterSize=0)
Definition: util3d_surface.cpp:116
rtabmap::CameraModel::cx
double cx() const
Definition: CameraModel.h:97
true
#define true
Definition: ConvertUTF.c:57
uMax3
T uMax3(const T &a, const T &b, const T &c)
Definition: UMath.h:80
rtabmap::Feature2D::create
static Feature2D * create(const ParametersMap &parameters=ParametersMap())
Definition: Features2d.cpp:405
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::DatabaseViewer::mapIds_
std::map< int, int > mapIds_
Definition: DatabaseViewer.h:189
ULOGGER_DEBUG
#define ULOGGER_DEBUG(...)
Definition: ULogger.h:53
rtabmap::DatabaseViewer::gpsValues_
std::map< int, GPS > gpsValues_
Definition: DatabaseViewer.h:203
uCvMat2QImage
QImage uCvMat2QImage(const cv::Mat &image, bool isBgr=true, uCvQtDepthColorMap colorMap=uCvQtDepthWhiteToBlack)
Definition: UCv2Qt.h:44
rtabmap::ExportDialog::framesIgnored
int framesIgnored() const
Definition: ExportDialog.cpp:130
rtabmap::DatabaseViewer::graphLinks_
std::multimap< int, rtabmap::Link > graphLinks_
Definition: DatabaseViewer.h:199
rtabmap::DatabaseViewer::loopLinks_
QList< rtabmap::Link > loopLinks_
Definition: DatabaseViewer.h:194
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
UPlot::addCurve
UPlotCurve * addCurve(const QString &curveName, const QColor &color=QColor())
Definition: UPlot.cpp:1993
rtabmap::CameraModel::fy
double fy() const
Definition: CameraModel.h:96
rtabmap::DBDriver::getLaserScansMemoryUsed
long getLaserScansMemoryUsed() const
Definition: DBDriver.cpp:167
rtabmap::OccupancyGrid::addToCache
void addToCache(int nodeId, const cv::Mat &ground, const cv::Mat &obstacles, const cv::Mat &empty)
Definition: OccupancyGrid.cpp:675
rtabmap::util3d::laserScanToPointCloudINormal
pcl::PointCloud< pcl::PointXYZINormal >::Ptr RTABMAP_EXP laserScanToPointCloudINormal(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
Definition: util3d.cpp:2416
rtabmap::DBDriver::loadSignatures
void loadSignatures(const std::list< int > &ids, std::list< Signature * > &signatures, std::set< int > *loadedFromTrash=0)
Definition: DBDriver.cpp:528
rtabmap::ImageView::setImageDepth
void setImageDepth(const cv::Mat &imageDepth)
Definition: ImageView.cpp:898
util3d_filtering.h
rtabmap::GPS::error
const double & error() const
Definition: GeodeticCoords.h:108
rtabmap::ExportCloudsDialog::getTextureBrightnessConstrastRatioHigh
int getTextureBrightnessConstrastRatioHigh() const
Definition: ExportCloudsDialog.cpp:1257
rtabmap::StereoCameraModel::T
const cv::Mat & T() const
Definition: StereoCameraModel.h:108
rtabmap::Optimizer::optimize
std::map< int, Transform > optimize(int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints, std::list< std::map< int, Transform > > *intermediateGraphes=0, double *finalError=0, int *iterationsDone=0)
Definition: Optimizer.cpp:331
rtabmap::DBDriver::getAllStatistics
std::map< int, std::pair< std::map< std::string, float >, double > > getAllStatistics() const
Definition: DBDriver.cpp:257
rtabmap::StereoCameraModel::E
const cv::Mat & E() const
Definition: StereoCameraModel.h:109
rtabmap::ExportDialog::outputPath
QString outputPath() const
Definition: ExportDialog.cpp:125
rtabmap::graph::calcRMSE
Transform RTABMAP_EXP calcRMSE(const std::map< int, Transform > &groundTruth, const std::map< int, Transform > &poses, float &translational_rmse, float &translational_mean, float &translational_median, float &translational_std, float &translational_min, float &translational_max, float &rotational_rmse, float &rotational_mean, float &rotational_median, float &rotational_std, float &rotational_min, float &rotational_max)
Definition: Graph.cpp:684
rtabmap::Registration::isScanRequired
bool isScanRequired() const
Definition: Registration.cpp:99
rtabmap::util3d::cloudRGBFromSensorData
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP cloudRGBFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:1031
rtabmap::DBDriver::load2DMap
cv::Mat load2DMap(float &xMin, float &yMin, float &cellSize) const
Definition: DBDriver.cpp:1085
rtabmap::DatabaseViewer::wmStates_
std::map< int, std::vector< int > > wmStates_
Definition: DatabaseViewer.h:191
rtabmap::CloudViewer::setCloudPointSize
void setCloudPointSize(const std::string &id, int size)
Definition: CloudViewer.cpp:2592
rtabmap::DBDriver::getCalibration
bool getCalibration(int signatureId, std::vector< CameraModel > &models, StereoCameraModel &stereoModel) const
Definition: DBDriver.cpp:677
rtabmap::Signature::setWordsDescriptors
void setWordsDescriptors(const std::multimap< int, cv::Mat > &descriptors)
Definition: Signature.h:112
rtabmap::DBDriver::getLastDictionarySize
int getLastDictionarySize() const
Definition: DBDriver.cpp:215
rtabmap::OctoMap::createCloud
pcl::PointCloud< pcl::PointXYZRGB >::Ptr createCloud(unsigned int treeDepth=0, std::vector< int > *obstacleIndices=0, std::vector< int > *emptyIndices=0, std::vector< int > *groundIndices=0, bool originalRefPoints=true) const
Definition: OctoMap.cpp:920
rtabmap::ProgressDialog::setMaximumSteps
void setMaximumSteps(int steps)
Definition: ProgressDialog.cpp:133
rtabmap::util3d::assembleTextureMesh
pcl::TextureMesh::Ptr RTABMAP_EXP assembleTextureMesh(const cv::Mat &cloudMat, const std::vector< std::vector< std::vector< unsigned int > > > &polygons, const std::vector< std::vector< Eigen::Vector2f > > &texCoords, cv::Mat &textures, bool mergeTextures=false)
Definition: util3d_surface.cpp:1246
rtabmap::util3d::convertImage8U2Map
cv::Mat RTABMAP_EXP convertImage8U2Map(const cv::Mat &map8U, bool pgmFormat=false)
Definition: util3d_mapping.cpp:930
rtabmap::DatabaseViewer::localMapsInfo_
std::map< int, std::pair< float, cv::Point3f > > localMapsInfo_
Definition: DatabaseViewer.h:209
rtabmap::CloudViewer::updateCameraTargetPosition
void updateCameraTargetPosition(const Transform &pose)
Definition: CloudViewer.cpp:2347
rtabmap::DBDriver::updateLink
void updateLink(const Link &link)
Definition: DBDriver.cpp:477
rtabmap::DBDriver::getStatisticsMemoryUsed
long getStatisticsMemoryUsed() const
Definition: DBDriver.cpp:199
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:67
rtabmap::GeodeticCoords::latitude
const double & latitude() const
Definition: GeodeticCoords.h:58
rtabmap::DBDriver::getCalibrationsMemoryUsed
long getCalibrationsMemoryUsed() const
Definition: DBDriver.cpp:151
rtabmap::DatabaseViewer::weights_
std::map< int, int > weights_
Definition: DatabaseViewer.h:190
rtabmap::DatabaseViewer::linksRemoved_
std::multimap< int, rtabmap::Link > linksRemoved_
Definition: DatabaseViewer.h:207
rtabmap::DBDriver::saveOptimizedMesh
void saveOptimizedMesh(const cv::Mat &cloud, const std::vector< std::vector< std::vector< unsigned int > > > &polygons=std::vector< std::vector< std::vector< unsigned int > > >(), const std::vector< std::vector< Eigen::Vector2f > > &texCoords=std::vector< std::vector< Eigen::Vector2f > >(), const cv::Mat &textures=cv::Mat()) const
Definition: DBDriver.cpp:1093
rtabmap::DBDriver::updateDepthImage
void updateDepthImage(int nodeId, const cv::Mat &image)
Definition: DBDriver.cpp:505
rtabmap::SensorData::depthOrRightRaw
const cv::Mat & depthOrRightRaw() const
Definition: SensorData.h:162
rtabmap::SensorData::id
int id() const
Definition: SensorData.h:152
rtabmap::CameraModel::P
cv::Mat P() const
Definition: CameraModel.h:106
rtabmap::DBDriver::getDepthImagesMemoryUsed
long getDepthImagesMemoryUsed() const
Definition: DBDriver.cpp:143
util3d_transforms.h
rtabmap::util3d::organizedFastMesh
std::vector< pcl::Vertices > RTABMAP_EXP organizedFastMesh(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, double angleTolerance, bool quad, int trianglePixelSize, const Eigen::Vector3f &viewpoint=Eigen::Vector3f(0, 0, 0))
Definition: util3d_surface.cpp:153
rtabmap::DatabaseViewer::exportPoses
void exportPoses(int format)
Definition: DatabaseViewer.cpp:1981
rtabmap::GPS::toGeodeticCoords
GeodeticCoords toGeodeticCoords() const
Definition: GeodeticCoords.h:111
rtabmap::DatabaseViewer::odomPoses_
std::map< int, rtabmap::Transform > odomPoses_
Definition: DatabaseViewer.h:200
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:409
rtabmap::ExportDialog::isUserDataExported
bool isUserDataExported() const
Definition: ExportDialog.cpp:165
rtabmap::graph::radiusPosesClustering
std::multimap< int, int > RTABMAP_EXP radiusPosesClustering(const std::map< int, Transform > &poses, float radius, float angle)
Definition: Graph.cpp:1157
rtabmap::ExportCloudsDialog::exportClouds
void exportClouds(const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, const std::map< int, int > &mapIds, const QMap< int, Signature > &cachedSignatures, const std::map< int, std::pair< pcl::PointCloud< pcl::PointXYZRGB >::Ptr, pcl::IndicesPtr > > &cachedClouds, const std::map< int, LaserScan > &cachedScans, const QString &workingDirectory, const ParametersMap &parameters)
Definition: ExportCloudsDialog.cpp:867
rtabmap::CameraModel::cy
double cy() const
Definition: CameraModel.h:98
state
RecoveryProgressState state
Definition: tools/Recovery/main.cpp:56
rtabmap::CloudViewer::setCameraLockZ
void setCameraLockZ(bool enabled=true)
Definition: CloudViewer.cpp:2618
rtabmap::Signature::setPose
void setPose(const Transform &pose)
Definition: Signature.h:116
rtabmap::DBDriver::create
static DBDriver * create(const ParametersMap &parameters=ParametersMap())
Definition: DBDriver.cpp:41
rtabmap::DatabaseViewer::groundTruthPoses_
std::map< int, rtabmap::Transform > groundTruthPoses_
Definition: DatabaseViewer.h:201
rtabmap::Stereo::create
static Stereo * create(const ParametersMap &parameters=ParametersMap())
Definition: Stereo.cpp:35
rtabmap::DatabaseViewer::stereoViewer_
CloudViewer * stereoViewer_
Definition: DatabaseViewer.h:186
rtabmap::CloudViewer::isFrustumShown
bool isFrustumShown() const
Definition: CloudViewer.cpp:1969
rtabmap::ExportDialog::isRgbExported
bool isRgbExported() const
Definition: ExportDialog.cpp:145
rtabmap::Parameters::getType
static std::string getType(const std::string &paramKey)
Definition: Parameters.cpp:416
rtabmap::SensorData::gridEmptyCellsRaw
const cv::Mat & gridEmptyCellsRaw() const
Definition: SensorData.h:221
uMax
T uMax(const T *v, unsigned int size, unsigned int &index)
Definition: UMath.h:94
false
#define false
Definition: ConvertUTF.c:56
rtabmap::DatabaseViewer::dbDriver_
rtabmap::DBDriver * dbDriver_
Definition: DatabaseViewer.h:195
rtabmap::util3d::convertMap2Image8U
cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat &map8S, bool pgmFormat=false)
Definition: util3d_mapping.cpp:897
rtabmap::Parameters::getDefaultParameters
static const ParametersMap & getDefaultParameters()
Definition: Parameters.h:695
rtabmap::util3d::fixTextureMeshForVisualization
void RTABMAP_EXP fixTextureMeshForVisualization(pcl::TextureMesh &textureMesh)
Definition: util3d_surface.cpp:2093
rtabmap::GPS::longitude
const double & longitude() const
Definition: GeodeticCoords.h:105
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:193
rtabmap::Signature::getPose
const Transform & getPose() const
Definition: Signature.h:129
rtabmap::Transform::getDistance
float getDistance(const Transform &t) const
Definition: Transform.cpp:241
rtabmap::CloudViewer::addOrUpdateGraph
void addOrUpdateGraph(const std::string &id, const pcl::PointCloud< pcl::PointXYZ >::Ptr &graph, const QColor &color=Qt::gray)
Definition: CloudViewer.cpp:1825
uValues
std::vector< V > uValues(const std::multimap< K, V > &mm)
Definition: UStl.h:100
rtabmap::util3d::cloudFromSensorData
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP cloudFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:852
rtabmap::GPS::altitude
const double & altitude() const
Definition: GeodeticCoords.h:107
rtabmap::Optimizer::type
virtual Type type() const =0
rtabmap::Registration::create
static Registration * create(const ParametersMap &parameters)
Definition: Registration.cpp:38
rtabmap::ExportDialog::isOdomExported
bool isOdomExported() const
Definition: ExportDialog.cpp:160
rtabmap::GeodeticCoords::toENU_WGS84
cv::Point3d toENU_WGS84(const GeodeticCoords &origin) const
Definition: GeodeticCoords.cpp:108
UDEBUG
#define UDEBUG(...)
rtabmap::CameraModel::fx
double fx() const
Definition: CameraModel.h:95
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:134
rtabmap::CloudViewer::updateCameraFrustums
void updateCameraFrustums(const Transform &pose, const std::vector< CameraModel > &models)
Definition: CloudViewer.cpp:2488
rtabmap::StereoCameraModel::right
const CameraModel & right() const
Definition: StereoCameraModel.h:120
rtabmap::graph::findLink
std::multimap< int, Link >::iterator RTABMAP_EXP findLink(std::multimap< int, Link > &links, int from, int to, bool checkBothWays=true)
Definition: Graph.cpp:825
rtabmap::DatabaseViewer::showCloseButton
void showCloseButton(bool visible=true)
Definition: DatabaseViewer.cpp:448
rtabmap::databaseRecovery
bool RTABMAP_EXP databaseRecovery(const std::string &corruptedDatabase, bool keepCorruptedDatabase=true, std::string *errorMsg=0, ProgressState *progressState=0)
Definition: Recovery.cpp:39
rtabmap::util3d::laserScanToPointCloudNormal
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2346
rtabmap::util3d::createPolygonIndexes
void RTABMAP_EXP createPolygonIndexes(const std::vector< pcl::Vertices > &polygons, int cloudSize, std::vector< std::set< int > > &neighborPolygons, std::vector< std::set< int > > &vertexPolygons)
Given a set of polygons, create two indexes: polygons to neighbor polygons and vertices to polygons...
Definition: util3d_surface.cpp:79
UERROR
#define UERROR(...)
rtabmap::DatabaseViewer::updateLoopClosuresSlider
void updateLoopClosuresSlider(int from=0, int to=0)
Definition: DatabaseViewer.cpp:6568
rtabmap::util2d::fillDepthHoles
cv::Mat RTABMAP_EXP fillDepthHoles(const cv::Mat &depth, int maximumHoleSize=1, float errorRatio=0.02f)
Definition: util2d.cpp:1434
rtabmap::DBDriver::updateOccupancyGrid
void updateOccupancyGrid(int nodeId, const cv::Mat &ground, const cv::Mat &obstacles, const cv::Mat &empty, float cellSize, const cv::Point3f &viewpoint)
Definition: DBDriver.cpp:483
rtabmap::DatabaseViewer::keyPressEvent
virtual void keyPressEvent(QKeyEvent *event)
Definition: DatabaseViewer.cpp:1133
rtabmap::DBDriver::getLastParameters
ParametersMap getLastParameters() const
Definition: DBDriver.cpp:239
ULogger.h
ULogger class and convenient macros.
rtabmap::SensorData::stereoCameraModel
const StereoCameraModel & stereoCameraModel() const
Definition: SensorData.h:194
UWARN
#define UWARN(...)
rtabmap::GainCompensator::feed
void feed(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloudA, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloudB, const Transform &transformB)
Definition: GainCompensator.cpp:57
rtabmap::Registration::isImageRequired
bool isImageRequired() const
Definition: Registration.cpp:89
UTimer::ticks
double ticks()
Definition: UTimer.cpp:110
rtabmap::DBDriver::getTotalDictionarySize
int getTotalDictionarySize() const
Definition: DBDriver.cpp:231
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
rtabmap::DBDriver::getDatabaseVersion
std::string getDatabaseVersion() const
Definition: DBDriver.cpp:275
UDirectory::exists
static bool exists(const std::string &dirPath)
Definition: UDirectory.cpp:249
rtabmap::Parameters::readINI
static void readINI(const std::string &configFile, ParametersMap &parameters, bool modifiedOnly=false)
Definition: Parameters.cpp:953
rtabmap::CloudViewer::setCloudColorIndex
void setCloudColorIndex(const std::string &id, int index)
Definition: CloudViewer.cpp:2574
rtabmap::CloudViewer::removeCoordinate
void removeCoordinate(const std::string &id)
Definition: CloudViewer.cpp:1311
rtabmap::DataRecorder::init
bool init(const QString &path, bool recordInRAM=true)
Definition: DataRecorder.cpp:67
rtabmap::DatabaseViewer::localMaps_
std::map< int, std::pair< std::pair< cv::Mat, cv::Mat >, cv::Mat > > localMaps_
Definition: DatabaseViewer.h:208
rtabmap::SensorData::setGroundTruth
void setGroundTruth(const Transform &pose)
Definition: SensorData.h:231
rtabmap::util2d::cvtDepthFromFloat
cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat &depth32F)
Definition: util2d.cpp:887
rtabmap::DatabaseViewer::cloudViewer_
CloudViewer * cloudViewer_
Definition: DatabaseViewer.h:185
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:169
rtabmap::OctoMap::createProjectionMap
cv::Mat createProjectionMap(float &xMin, float &yMin, float &gridCellSize, float minGridSize=0.0f, unsigned int treeDepth=0)
Definition: OctoMap.cpp:1043
rtabmap::ExportCloudsDialog::saveSettings
void saveSettings(QSettings &settings, const QString &group="") const
Definition: ExportCloudsDialog.cpp:292
rtabmap::Optimizer::getConnectedGraph
static void getConnectedGraph(int fromId, const std::map< int, Transform > &posesIn, const std::multimap< int, Link > &linksIn, std::map< int, Transform > &posesOut, std::multimap< int, Link > &linksOut, int depth=0)
Definition: Optimizer.cpp:157
rtabmap::CloudViewer::addOctomap
bool addOctomap(const OctoMap *octomap, unsigned int treeDepth=0, bool volumeRepresentation=true)
Definition: CloudViewer.cpp:805
rtabmap::CameraModel::K
cv::Mat K() const
Definition: CameraModel.h:103
rtabmap::CloudViewer::addCloudMesh
bool addCloudMesh(const std::string &id, const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const std::vector< pcl::Vertices > &polygons, const Transform &pose=Transform::getIdentity())
Definition: CloudViewer.cpp:632
rtabmap::DatabaseViewer::DatabaseViewer
DatabaseViewer(const QString &ini=QString(), QWidget *parent=0)
Definition: DatabaseViewer.cpp:97
glm::length
GLM_FUNC_DECL genType::value_type length(genType const &x)
rtabmap::DBDriver::addLink
void addLink(const Link &link)
Definition: DBDriver.cpp:467
rtabmap::ProgressDialog::appendText
void appendText(const QString &text, const QColor &color=Qt::black)
Definition: ProgressDialog.cpp:102
rtabmap::DatabaseViewer::eventFilter
virtual bool eventFilter(QObject *obj, QEvent *event)
Definition: DatabaseViewer.cpp:1142
ULOGGER_ERROR
#define ULOGGER_ERROR(...)
Definition: ULogger.h:56
rtabmap::util3d::voxelize
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
Definition: util3d_filtering.cpp:407
rtabmap::DatabaseViewer::moveEvent
virtual void moveEvent(QMoveEvent *anEvent)
Definition: DatabaseViewer.cpp:1112
rtabmap::GPS::latitude
const double & latitude() const
Definition: GeodeticCoords.h:106
rtabmap::ExportCloudsDialog::getTextureBrightnessConstrastRatioLow
int getTextureBrightnessConstrastRatioLow() const
Definition: ExportCloudsDialog.cpp:1253
rtabmap::StereoCameraModel::localTransform
const Transform & localTransform() const
Definition: StereoCameraModel.h:116
rtabmap::DBDriver::save2DMap
void save2DMap(const cv::Mat &map, float xMin, float yMin, float cellSize) const
Definition: DBDriver.cpp:1078
rtabmap::GPS::bearing
const double & bearing() const
Definition: GeodeticCoords.h:109
rtabmap::DBDriver::removeLink
void removeLink(int from, int to)
Definition: DBDriver.cpp:473
rtabmap::Transform::getTranslationAndEulerAngles
void getTranslationAndEulerAngles(float &x, float &y, float &z, float &roll, float &pitch, float &yaw) const
Definition: Transform.cpp:206
rtabmap::graph::computePathLength
float RTABMAP_EXP computePathLength(const std::vector< std::pair< int, Transform > > &path, unsigned int fromIndex=0, unsigned int toIndex=0)
Definition: Graph.cpp:2039
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:350
rtabmap::OccupancyGrid::getMap
cv::Mat getMap(float &xMin, float &yMin) const
Definition: OccupancyGrid.cpp:604
rtabmap::LaserScan::backwardCompatibility
static LaserScan backwardCompatibility(const cv::Mat &oldScanFormat, int maxPoints=0, int maxRange=0, const Transform &localTransform=Transform::getIdentity())
Definition: LaserScan.cpp:84
rtabmap::CameraModel::imageHeight
int imageHeight() const
Definition: CameraModel.h:114
rtabmap::DBDriver::getLinksMemoryUsed
long getLinksMemoryUsed() const
Definition: DBDriver.cpp:127
rtabmap::DatabaseViewer::occupancyGridViewer_
CloudViewer * occupancyGridViewer_
Definition: DatabaseViewer.h:187
rtabmap::DatabaseViewer::generatedLocalMaps_
std::map< int, std::pair< std::pair< cv::Mat, cv::Mat >, cv::Mat > > generatedLocalMaps_
Definition: DatabaseViewer.h:210
rtabmap::CloudViewer::removeAllFrustums
void removeAllFrustums(bool exceptCameraReference=false)
Definition: CloudViewer.cpp:1812
rtabmap::GeodeticCoords::fromENU_WGS84
void fromENU_WGS84(const cv::Point3d &enu, const GeodeticCoords &origin)
Definition: GeodeticCoords.cpp:169
rtabmap::Optimizer::create
static Optimizer * create(const ParametersMap &parameters)
Definition: Optimizer.cpp:67
rtabmap::ImageView::setFeatures
void setFeatures(const std::multimap< int, cv::KeyPoint > &refWords, const cv::Mat &depth=cv::Mat(), const QColor &color=Qt::yellow)
Definition: ImageView.cpp:774
rtabmap::SensorData::gridViewPoint
const cv::Point3f & gridViewPoint() const
Definition: SensorData.h:224
rtabmap::DBDriver::getLastNodesSize
int getLastNodesSize() const
Definition: DBDriver.cpp:207
rtabmap::util3d::laserScanToPointCloudRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP laserScanToPointCloudRGB(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=255, unsigned char g=255, unsigned char b=255)
Definition: util3d.cpp:2363
uValue
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
Definition: UStl.h:240
rtabmap::SensorData::setFeatures
void setFeatures(const std::vector< cv::KeyPoint > &keypoints, const std::vector< cv::Point3f > &keypoints3D, const cv::Mat &descriptors)
Definition: SensorData.cpp:828
rtabmap::DBDriver::getNodesMemoryUsed
long getNodesMemoryUsed() const
Definition: DBDriver.cpp:119
rtabmap::SensorData::depthRaw
cv::Mat depthRaw() const
Definition: SensorData.h:172
rtabmap::CloudViewer::setCloudOpacity
void setCloudOpacity(const std::string &id, double opacity=1.0)
Definition: CloudViewer.cpp:2582
rtabmap::LaserScan::hasIntensity
bool hasIntensity() const
Definition: LaserScan.h:75
rtabmap::SensorData::gridObstacleCellsRaw
const cv::Mat & gridObstacleCellsRaw() const
Definition: SensorData.h:219
rtabmap::graph::calcKittiSequenceErrors
void RTABMAP_EXP calcKittiSequenceErrors(const std::vector< Transform > &poses_gt, const std::vector< Transform > &poses_result, float &t_err, float &r_err)
Definition: Graph.cpp:616
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:443
rtabmap::CameraModel::localTransform
const Transform & localTransform() const
Definition: CameraModel.h:109
util3d_surface.h
rtabmap::SensorData::setLaserScanRaw
void setLaserScanRaw(const LaserScan &laserScanRaw)
Definition: SensorData.h:166
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
rtabmap::util3d::assemblePolygonMesh
pcl::PolygonMesh::Ptr RTABMAP_EXP assemblePolygonMesh(const cv::Mat &cloudMat, const std::vector< std::vector< unsigned int > > &polygons)
Definition: util3d_surface.cpp:1369
UINFO
#define UINFO(...)
rtabmap::GPS::stamp
const double & stamp() const
Definition: GeodeticCoords.h:104

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:31