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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:34:58