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 <QColorDialog>
35 #include <QGraphicsLineItem>
36 #include <QtGui/QCloseEvent>
37 #include <QGraphicsOpacityEffect>
38 #include <QtCore/QBuffer>
39 #include <QtCore/QTextStream>
40 #include <QtCore/QDateTime>
41 #include <QtCore/QSettings>
42 #include <QThread>
43 #include <rtabmap/utilite/ULogger.h>
44 #include <rtabmap/utilite/UDirectory.h>
45 #include <rtabmap/utilite/UConversion.h>
46 #include <opencv2/core/core_c.h>
47 #include <opencv2/imgproc/types_c.h>
48 #include <opencv2/highgui/highgui.hpp>
49 #include <rtabmap/utilite/UTimer.h>
50 #include <rtabmap/utilite/UFile.h>
51 #include "rtabmap/utilite/UPlot.h"
52 #include "rtabmap/core/DBDriver.h"
53 #include "rtabmap/gui/KeypointItem.h"
54 #include "rtabmap/gui/CloudViewer.h"
55 #include "rtabmap/utilite/UCv2Qt.h"
56 #include "rtabmap/core/util3d.h"
57 #include "rtabmap/core/util3d_transforms.h"
58 #include "rtabmap/core/util3d_filtering.h"
59 #include "rtabmap/core/util3d_surface.h"
60 #include "rtabmap/core/util3d_registration.h"
61 #include "rtabmap/core/util3d_mapping.h"
62 #include "rtabmap/core/util2d.h"
63 #include "rtabmap/core/Signature.h"
64 #include "rtabmap/core/Memory.h"
65 #include "rtabmap/core/Features2d.h"
66 #include "rtabmap/core/Compression.h"
67 #include "rtabmap/core/Graph.h"
68 #include "rtabmap/core/Stereo.h"
69 #include "rtabmap/core/StereoDense.h"
70 #include "rtabmap/core/Optimizer.h"
71 #include "rtabmap/core/RegistrationVis.h"
72 #include "rtabmap/core/RegistrationIcp.h"
73 #include "rtabmap/core/global_map/OccupancyGrid.h"
74 #include "rtabmap/core/global_map/CloudMap.h"
75 #include "rtabmap/core/GeodeticCoords.h"
76 #include "rtabmap/core/Recovery.h"
77 #include "rtabmap/gui/DataRecorder.h"
78 #include "rtabmap/gui/ExportCloudsDialog.h"
79 #include "rtabmap/gui/EditDepthArea.h"
80 #include "rtabmap/gui/EditMapArea.h"
81 #include "rtabmap/core/SensorData.h"
82 #include "rtabmap/core/GainCompensator.h"
83 #include "rtabmap/core/VisualWord.h"
84 #include "rtabmap/gui/ExportDialog.h"
85 #include "rtabmap/gui/EditConstraintDialog.h"
86 #include "rtabmap/gui/LinkRefiningDialog.h"
87 #include "rtabmap/gui/ProgressDialog.h"
88 #include "rtabmap/gui/ParametersToolBox.h"
89 #include "rtabmap/gui/RecoveryState.h"
90 #include <pcl/io/pcd_io.h>
91 #include <pcl/io/ply_io.h>
92 #include <pcl/io/obj_io.h>
93 #include <pcl/filters/voxel_grid.h>
94 #include <pcl/filters/crop_box.h>
95 #include <pcl/common/transforms.h>
96 #include <pcl/common/common.h>
97 #include <rtabmap/core/LocalGridMaker.h>
98 
99 #ifdef RTABMAP_OCTOMAP
100 #include "rtabmap/core/global_map/OctoMap.h"
101 #endif
102 
103 #ifdef RTABMAP_GRIDMAP
104 #include "rtabmap/core/global_map/GridMap.h"
105 #endif
106 
107 namespace rtabmap {
108 
109 DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
110  QMainWindow(parent),
111  dbDriver_(0),
112  octomap_(0),
113  exportDialog_(new ExportCloudsDialog(this)),
114  editDepthDialog_(new QDialog(this)),
115  editMapDialog_(new QDialog(this)),
116  linkRefiningDialog_(new LinkRefiningDialog(this)),
117  savedMaximized_(false),
118  firstCall_(true),
119  iniFilePath_(ini),
120  infoReducedGraph_(false),
121  infoTotalOdom_(0.0),
122  infoSessions_(0)
123 {
124  pathDatabase_ = QDir::homePath()+"/Documents/RTAB-Map"; //use home directory by default
125 
126  if(!UDirectory::exists(pathDatabase_.toStdString()))
127  {
128  pathDatabase_ = QDir::homePath();
129  }
130 
131  ui_ = new Ui_DatabaseViewer();
132  ui_->setupUi(this);
133  ui_->buttonBox->setVisible(false);
134  connect(ui_->buttonBox->button(QDialogButtonBox::Close), SIGNAL(clicked()), this, SLOT(close()));
135 
136  ui_->comboBox_logger_level->setVisible(parent==0);
137  ui_->label_logger_level->setVisible(parent==0);
138  connect(ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(int)), this, SLOT(updateLoggerLevel()));
139  connect(ui_->actionVertical_Layout, SIGNAL(toggled(bool)), this, SLOT(setupMainLayout(bool)));
140 
141  editDepthDialog_->resize(640, 480);
142  QVBoxLayout * vLayout = new QVBoxLayout(editDepthDialog_);
143  editDepthArea_ = new EditDepthArea(editDepthDialog_);
144  vLayout->setContentsMargins(0,0,0,0);
145  vLayout->setSpacing(0);
146  vLayout->addWidget(editDepthArea_, 1);
147  QDialogButtonBox * buttonBox = new QDialogButtonBox(QDialogButtonBox::Save | QDialogButtonBox::Cancel | QDialogButtonBox::Reset, Qt::Horizontal, editDepthDialog_);
148  vLayout->addWidget(buttonBox);
149  connect(buttonBox, SIGNAL(accepted()), editDepthDialog_, SLOT(accept()));
150  connect(buttonBox, SIGNAL(rejected()), editDepthDialog_, SLOT(reject()));
151  connect(buttonBox->button(QDialogButtonBox::Reset), SIGNAL(clicked()), editDepthArea_, SLOT(resetChanges()));
152  editDepthDialog_->setLayout(vLayout);
153  editDepthDialog_->setWindowTitle(tr("Edit Depth Image"));
154 
155  editMapDialog_->resize(640, 480);
156  vLayout = new QVBoxLayout(editMapDialog_);
157  editMapArea_ = new EditMapArea(editMapDialog_);
158  vLayout->setContentsMargins(0,0,0,0);
159  vLayout->setSpacing(0);
160  vLayout->addWidget(editMapArea_, 1);
161  buttonBox = new QDialogButtonBox(QDialogButtonBox::Save | QDialogButtonBox::Cancel | QDialogButtonBox::Reset, Qt::Horizontal, editMapDialog_);
162  vLayout->addWidget(buttonBox);
163  connect(buttonBox, SIGNAL(accepted()), editMapDialog_, SLOT(accept()));
164  connect(buttonBox, SIGNAL(rejected()), editMapDialog_, SLOT(reject()));
165  connect(buttonBox->button(QDialogButtonBox::Reset), SIGNAL(clicked()), editMapArea_, SLOT(resetChanges()));
166  editMapDialog_->setLayout(vLayout);
167  editMapDialog_->setWindowTitle(tr("Edit Optimized Map"));
168 
169  QString title("RTAB-Map Database Viewer[*]");
170  this->setWindowTitle(title);
171 
172  ui_->dockWidget_constraints->setVisible(false);
173  ui_->dockWidget_graphView->setVisible(false);
174  ui_->dockWidget_occupancyGridView->setVisible(false);
175  ui_->dockWidget_guiparameters->setVisible(false);
176  ui_->dockWidget_coreparameters->setVisible(false);
177  ui_->dockWidget_info->setVisible(false);
178  ui_->dockWidget_stereoView->setVisible(false);
179  ui_->dockWidget_view3d->setVisible(false);
180  ui_->dockWidget_statistics->setVisible(false);
181 
182  // Create cloud viewers
183  constraintsViewer_ = new CloudViewer(ui_->dockWidgetContents);
184  cloudViewer_ = new CloudViewer(ui_->dockWidgetContents_3dviews);
185  stereoViewer_ = new CloudViewer(ui_->dockWidgetContents_stereo);
186  occupancyGridViewer_ = new CloudViewer(ui_->dockWidgetContents_occupancyGrid);
187  constraintsViewer_->setObjectName("constraintsViewer");
188  cloudViewer_->setObjectName("cloudViewerA");
189  stereoViewer_->setObjectName("stereoViewer");
190  occupancyGridViewer_->setObjectName("occupancyGridView");
191  ui_->layout_constraintsViewer->addWidget(constraintsViewer_);
192  ui_->horizontalLayout_3dviews->addWidget(cloudViewer_, 1);
193  ui_->horizontalLayout_stereo->addWidget(stereoViewer_, 1);
194  ui_->layout_occupancyGridView->addWidget(occupancyGridViewer_, 1);
195 
196  constraintsViewer_->setCameraLockZ(false);
197  constraintsViewer_->setCameraFree();
198  occupancyGridViewer_->setCameraFree();
199 
200  ui_->graphicsView_stereo->setAlpha(255);
201 
202 #ifndef RTABMAP_OCTOMAP
203  ui_->checkBox_octomap->setEnabled(false);
204  ui_->checkBox_octomap->setChecked(false);
205 #endif
206 
207 #ifndef RTABMAP_GRIDMAP
208  ui_->checkBox_showElevation->setEnabled(false);
209  ui_->checkBox_showElevation->setChecked(false);
210  ui_->checkBox_grid_elevation->setEnabled(false);
211  ui_->checkBox_grid_elevation->setChecked(false);
212 #endif
213 
214  ParametersMap parameters;
215  uInsert(parameters, Parameters::getDefaultParameters("SURF"));
216  uInsert(parameters, Parameters::getDefaultParameters("SIFT"));
217  uInsert(parameters, Parameters::getDefaultParameters("BRIEF"));
218  uInsert(parameters, Parameters::getDefaultParameters("FAST"));
219  uInsert(parameters, Parameters::getDefaultParameters("GFTT"));
220  uInsert(parameters, Parameters::getDefaultParameters("ORB"));
221  uInsert(parameters, Parameters::getDefaultParameters("FREAK"));
222  uInsert(parameters, Parameters::getDefaultParameters("BRISK"));
223  uInsert(parameters, Parameters::getDefaultParameters("KAZE"));
224  uInsert(parameters, Parameters::getDefaultParameters("SuperPoint"));
225  uInsert(parameters, Parameters::getDefaultParameters("Optimizer"));
226  uInsert(parameters, Parameters::getDefaultParameters("g2o"));
227  uInsert(parameters, Parameters::getDefaultParameters("GTSAM"));
228  uInsert(parameters, Parameters::getDefaultParameters("Reg"));
229  uInsert(parameters, Parameters::getDefaultParameters("Vis"));
230  uInsert(parameters, Parameters::getDefaultParameters("Icp"));
231  uInsert(parameters, Parameters::getDefaultParameters("PyMatcher"));
232  uInsert(parameters, Parameters::getDefaultParameters("Stereo"));
233  uInsert(parameters, Parameters::getDefaultParameters("StereoBM"));
234  uInsert(parameters, Parameters::getDefaultParameters("StereoSGBM"));
235  uInsert(parameters, Parameters::getDefaultParameters("Grid"));
236  uInsert(parameters, Parameters::getDefaultParameters("GridGlobal"));
237  uInsert(parameters, Parameters::getDefaultParameters("Marker"));
238  parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDOptimizeMaxError()));
239  parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDLoopClosureReextractFeatures()));
240  parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDLoopCovLimited()));
241  parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDProximityPathFilteringRadius()));
242  ui_->parameters_toolbox->setupUi(parameters);
243  exportDialog_->setObjectName("ExportCloudsDialog");
244  restoreDefaultSettings();
245  this->readSettings();
246 
247  setupMainLayout(ui_->actionVertical_Layout->isChecked());
248  ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked());
249  ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
250  ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
251  ui_->checkBox_grid_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0);
252  ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
253  ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
254  ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
255  ui_->label_octomap_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0);
256 
257  ui_->menuView->addAction(ui_->dockWidget_constraints->toggleViewAction());
258  ui_->menuView->addAction(ui_->dockWidget_graphView->toggleViewAction());
259  ui_->menuView->addAction(ui_->dockWidget_occupancyGridView->toggleViewAction());
260  ui_->menuView->addAction(ui_->dockWidget_stereoView->toggleViewAction());
261  ui_->menuView->addAction(ui_->dockWidget_view3d->toggleViewAction());
262  ui_->menuView->addAction(ui_->dockWidget_guiparameters->toggleViewAction());
263  ui_->menuView->addAction(ui_->dockWidget_coreparameters->toggleViewAction());
264  ui_->menuView->addAction(ui_->dockWidget_info->toggleViewAction());
265  ui_->menuView->addAction(ui_->dockWidget_statistics->toggleViewAction());
266  connect(ui_->dockWidget_graphView->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView()));
267  connect(ui_->dockWidget_occupancyGridView->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView()));
268  connect(ui_->dockWidget_statistics->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateStatistics()));
269  connect(ui_->dockWidget_info->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateInfo()));
270 
271  connect(ui_->graphViewer, SIGNAL(nodeSelected(int)), this , SLOT(graphNodeSelected(int)));
272  connect(ui_->graphViewer, SIGNAL(linkSelected(int,int)), this , SLOT(graphLinkSelected(int,int)));
273 
274  connect(ui_->parameters_toolbox, SIGNAL(parametersChanged(const QStringList &)), this, SLOT(notifyParametersChanged(const QStringList &)));
275 
276  connect(ui_->actionQuit, SIGNAL(triggered()), this, SLOT(close()));
277 
278  ui_->actionOpen_database->setEnabled(true);
279  ui_->actionClose_database->setEnabled(false);
280 
281  // connect actions with custom slots
282  ui_->actionSave_config->setShortcut(QKeySequence::Save);
283  connect(ui_->actionSave_config, SIGNAL(triggered()), this, SLOT(writeSettings()));
284  connect(ui_->actionOpen_database, SIGNAL(triggered()), this, SLOT(openDatabase()));
285  connect(ui_->actionClose_database, SIGNAL(triggered()), this, SLOT(closeDatabase()));
286  connect(ui_->actionDatabase_recovery, SIGNAL(triggered()), this, SLOT(recoverDatabase()));
287  connect(ui_->actionExport, SIGNAL(triggered()), this, SLOT(exportDatabase()));
288  connect(ui_->actionExtract_images, SIGNAL(triggered()), this, SLOT(extractImages()));
289  connect(ui_->actionEdit_depth_image, SIGNAL(triggered()), this, SLOT(editDepthImage()));
290  connect(ui_->actionGenerate_graph_dot, SIGNAL(triggered()), this, SLOT(generateGraph()));
291  connect(ui_->actionGenerate_local_graph_dot, SIGNAL(triggered()), this, SLOT(generateLocalGraph()));
292  connect(ui_->actionRaw_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRaw()));
293  connect(ui_->actionRGBD_SLAM_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAM()));
294  connect(ui_->actionRGBD_SLAM_ID_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAMID()));
295  connect(ui_->actionRGBD_SLAM_motion_capture_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAMMotionCapture()));
296  connect(ui_->actionKITTI_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesKITTI()));
297  connect(ui_->actionTORO_graph, SIGNAL(triggered()), this , SLOT(exportPosesTORO()));
298  connect(ui_->actionG2o_g2o, SIGNAL(triggered()), this , SLOT(exportPosesG2O()));
299  connect(ui_->actionPoses_KML, SIGNAL(triggered()), this , SLOT(exportPosesKML()));
300  connect(ui_->actionGPS_TXT, SIGNAL(triggered()), this , SLOT(exportGPS_TXT()));
301  connect(ui_->actionGPS_KML, SIGNAL(triggered()), this , SLOT(exportGPS_KML()));
302  connect(ui_->actionEdit_optimized_2D_map, SIGNAL(triggered()), this , SLOT(editSaved2DMap()));
303  connect(ui_->actionExport_saved_2D_map, SIGNAL(triggered()), this , SLOT(exportSaved2DMap()));
304  connect(ui_->actionImport_2D_map, SIGNAL(triggered()), this , SLOT(import2DMap()));
305  connect(ui_->actionRegenerate_optimized_2D_map, SIGNAL(triggered()), this , SLOT(regenerateSavedMap()));
306  connect(ui_->actionView_optimized_mesh, SIGNAL(triggered()), this , SLOT(viewOptimizedMesh()));
307  connect(ui_->actionExport_optimized_mesh, SIGNAL(triggered()), this , SLOT(exportOptimizedMesh()));
308  connect(ui_->actionUpdate_optimized_mesh, SIGNAL(triggered()), this , SLOT(updateOptimizedMesh()));
309  connect(ui_->actionView_3D_map, SIGNAL(triggered()), this, SLOT(view3DMap()));
310  connect(ui_->actionGenerate_3D_map_pcd, SIGNAL(triggered()), this, SLOT(generate3DMap()));
311  connect(ui_->actionDetect_more_loop_closures, SIGNAL(triggered()), this, SLOT(detectMoreLoopClosures()));
312  connect(ui_->actionUpdate_all_neighbor_covariances, SIGNAL(triggered()), this, SLOT(updateAllNeighborCovariances()));
313  connect(ui_->actionUpdate_all_loop_closure_covariances, SIGNAL(triggered()), this, SLOT(updateAllLoopClosureCovariances()));
314  connect(ui_->actionUpdate_all_landmark_covariances, SIGNAL(triggered()), this, SLOT(updateAllLandmarkCovariances()));
315  connect(ui_->actionRefine_links, SIGNAL(triggered()), this, SLOT(refineLinks()));
316  connect(ui_->actionRegenerate_local_grid_maps, SIGNAL(triggered()), this, SLOT(regenerateLocalMaps()));
317  connect(ui_->actionRegenerate_local_grid_maps_selected, SIGNAL(triggered()), this, SLOT(regenerateCurrentLocalMaps()));
318  connect(ui_->actionReset_all_changes, SIGNAL(triggered()), this, SLOT(resetAllChanges()));
319  connect(ui_->actionRestore_default_GUI_settings, SIGNAL(triggered()), this, SLOT(restoreDefaultSettings()));
320 
321  //ICP buttons
322  connect(ui_->pushButton_refine, SIGNAL(clicked()), this, SLOT(refineConstraint()));
323  connect(ui_->pushButton_add, SIGNAL(clicked()), this, SLOT(addConstraint()));
324  connect(ui_->pushButton_reset, SIGNAL(clicked()), this, SLOT(resetConstraint()));
325  connect(ui_->pushButton_reject, SIGNAL(clicked()), this, SLOT(rejectConstraint()));
326  ui_->pushButton_refine->setEnabled(false);
327  ui_->pushButton_add->setEnabled(false);
328  ui_->pushButton_reset->setEnabled(false);
329  ui_->pushButton_reject->setEnabled(false);
330 
331  ui_->menuEdit->setEnabled(false);
332  ui_->actionGenerate_3D_map_pcd->setEnabled(false);
333  ui_->actionExport->setEnabled(false);
334  ui_->actionExtract_images->setEnabled(false);
335  ui_->menuExport_poses->setEnabled(false);
336  ui_->menuExport_GPS->setEnabled(false);
337  ui_->actionPoses_KML->setEnabled(false);
338  ui_->actionEdit_optimized_2D_map->setEnabled(false);
339  ui_->actionExport_saved_2D_map->setEnabled(false);
340  ui_->actionImport_2D_map->setEnabled(false);
341  ui_->actionRegenerate_optimized_2D_map->setEnabled(false);
342  ui_->actionView_optimized_mesh->setEnabled(false);
343  ui_->actionExport_optimized_mesh->setEnabled(false);
344  ui_->actionUpdate_optimized_mesh->setEnabled(false);
345 
346  ui_->horizontalSlider_A->setTracking(false);
347  ui_->horizontalSlider_B->setTracking(false);
348  ui_->horizontalSlider_A->setEnabled(false);
349  ui_->horizontalSlider_B->setEnabled(false);
350  connect(ui_->horizontalSlider_A, SIGNAL(valueChanged(int)), this, SLOT(sliderAValueChanged(int)));
351  connect(ui_->horizontalSlider_B, SIGNAL(valueChanged(int)), this, SLOT(sliderBValueChanged(int)));
352  connect(ui_->horizontalSlider_A, SIGNAL(sliderMoved(int)), this, SLOT(sliderAMoved(int)));
353  connect(ui_->horizontalSlider_B, SIGNAL(sliderMoved(int)), this, SLOT(sliderBMoved(int)));
354  ui_->spinBox_indexA->setEnabled(false);
355  ui_->spinBox_indexB->setEnabled(false);
356  connect(ui_->spinBox_indexA, SIGNAL(valueChanged(int)), ui_->horizontalSlider_A, SLOT(setValue(int)));
357  connect(ui_->spinBox_indexB, SIGNAL(valueChanged(int)), ui_->horizontalSlider_B, SLOT(setValue(int)));
358 
359  connect(ui_->toolButton_edit_priorA, SIGNAL(clicked(bool)), this, SLOT(editConstraint()));
360  connect(ui_->toolButton_edit_priorB, SIGNAL(clicked(bool)), this, SLOT(editConstraint()));
361  connect(ui_->toolButton_remove_priorA, SIGNAL(clicked(bool)), this, SLOT(rejectConstraint()));
362  connect(ui_->toolButton_remove_priorB, SIGNAL(clicked(bool)), this, SLOT(rejectConstraint()));
363 
364  connect(ui_->spinBox_mesh_angleTolerance, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
365  connect(ui_->spinBox_mesh_minClusterSize, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
366  connect(ui_->spinBox_mesh_fillDepthHoles, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
367  connect(ui_->spinBox_mesh_depthError, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
368  connect(ui_->checkBox_mesh_quad, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
369  connect(ui_->spinBox_mesh_triangleSize, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
370  connect(ui_->checkBox_showWords, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
371  connect(ui_->checkBox_showCloud, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
372  connect(ui_->checkBox_showMesh, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
373  connect(ui_->checkBox_showScan, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
374  connect(ui_->checkBox_showMap, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
375  connect(ui_->checkBox_showGrid, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
376  connect(ui_->checkBox_showElevation, SIGNAL(stateChanged(int)), this, SLOT(update3dView()));
377  connect(ui_->checkBox_odomFrame_3dview, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
378  connect(ui_->checkBox_gravity_3dview, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
379 
380  ui_->horizontalSlider_neighbors->setTracking(false);
381  ui_->horizontalSlider_loops->setTracking(false);
382  ui_->horizontalSlider_neighbors->setEnabled(false);
383  ui_->horizontalSlider_loops->setEnabled(false);
384  connect(ui_->horizontalSlider_neighbors, SIGNAL(valueChanged(int)), this, SLOT(sliderNeighborValueChanged(int)));
385  connect(ui_->horizontalSlider_loops, SIGNAL(valueChanged(int)), this, SLOT(sliderLoopValueChanged(int)));
386  connect(ui_->horizontalSlider_neighbors, SIGNAL(sliderMoved(int)), this, SLOT(sliderNeighborValueChanged(int)));
387  connect(ui_->horizontalSlider_loops, SIGNAL(sliderMoved(int)), this, SLOT(sliderLoopValueChanged(int)));
388  connect(ui_->checkBox_showOptimized, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
389  connect(ui_->checkBox_show3Dclouds, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
390  connect(ui_->checkBox_show2DScans, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
391  connect(ui_->checkBox_show3DWords, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
392  connect(ui_->checkBox_odomFrame, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
393  ui_->checkBox_showOptimized->setEnabled(false);
394  connect(ui_->toolButton_constraint, SIGNAL(clicked(bool)), this, SLOT(editConstraint()));
395  connect(ui_->checkBox_enableForAll, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintButtons()));
396 
397  ui_->horizontalSlider_iterations->setTracking(false);
398  ui_->horizontalSlider_iterations->setEnabled(false);
399  ui_->spinBox_optimizationsFrom->setEnabled(false);
400  connect(ui_->horizontalSlider_iterations, SIGNAL(valueChanged(int)), this, SLOT(sliderIterationsValueChanged(int)));
401  connect(ui_->horizontalSlider_iterations, SIGNAL(sliderMoved(int)), this, SLOT(sliderIterationsValueChanged(int)));
402  connect(ui_->spinBox_optimizationsFrom, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
403  connect(ui_->comboBox_optimizationFlavor, SIGNAL(activated(int)), this, SLOT(updateGraphView()));
404  connect(ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
405  connect(ui_->checkBox_wmState, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
406  connect(ui_->graphViewer, SIGNAL(mapShownRequested()), this, SLOT(updateGraphView()));
407  connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
408  connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
409  connect(ui_->checkBox_ignoreGlobalLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
410  connect(ui_->checkBox_ignoreLocalLoopSpace, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
411  connect(ui_->checkBox_ignoreLocalLoopTime, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
412  connect(ui_->checkBox_ignoreUserLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
413  connect(ui_->checkBox_ignoreLandmarks, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
414  connect(ui_->doubleSpinBox_optimizationScale, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
415  connect(ui_->checkBox_octomap, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
416  connect(ui_->checkBox_grid_grid, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
417  connect(ui_->checkBox_grid_2d, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
418  connect(ui_->checkBox_grid_elevation, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
419  connect(ui_->comboBox_octomap_rendering_type, SIGNAL(currentIndexChanged(int)), this, SLOT(updateOctomapView()));
420  connect(ui_->spinBox_grid_depth, SIGNAL(valueChanged(int)), this, SLOT(updateOctomapView()));
421  connect(ui_->checkBox_grid_empty, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
422  connect(ui_->checkBox_grid_frontiers, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
423  connect(ui_->doubleSpinBox_gainCompensationRadius, SIGNAL(valueChanged(double)), this, SLOT(updateConstraintView()));
424  connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(updateConstraintView()));
425  connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(update3dView()));
426  connect(ui_->checkBox_cameraProjection, SIGNAL(stateChanged(int)), this, SLOT(update3dView()));
427  connect(ui_->checkBox_showDisparityInsteadOfRight, SIGNAL(stateChanged(int)), this, SLOT(update3dView()));
428  connect(ui_->spinBox_decimation, SIGNAL(valueChanged(int)), this, SLOT(updateConstraintView()));
429  connect(ui_->spinBox_decimation, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
430  connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(updateGraphView()));
431  connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
432  connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
433 
434  ui_->label_stereo_inliers_name->setStyleSheet("QLabel {color : blue; }");
435  ui_->label_stereo_flowOutliers_name->setStyleSheet("QLabel {color : red; }");
436  ui_->label_stereo_slopeOutliers_name->setStyleSheet("QLabel {color : yellow; }");
437  ui_->label_stereo_disparityOutliers_name->setStyleSheet("QLabel {color : magenta; }");
438 
439 
440  // connect configuration changed
441  connect(ui_->graphViewer, SIGNAL(configChanged()), this, SLOT(configModified()));
442  connect(ui_->graphicsView_A, SIGNAL(configChanged()), this, SLOT(configModified()));
443  connect(ui_->graphicsView_B, SIGNAL(configChanged()), this, SLOT(configModified()));
444  connect(ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
445  connect(ui_->actionVertical_Layout, SIGNAL(toggled(bool)), this, SLOT(configModified()));
446  connect(ui_->actionConcise_Layout, SIGNAL(toggled(bool)), this, SLOT(configModified()));
447  connect(ui_->checkBox_alignPosesWithGPS, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
448  connect(ui_->checkBox_alignPosesWithGroundTruth, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
449  connect(ui_->checkBox_alignScansCloudsWithGroundTruth, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
450  connect(ui_->checkBox_ignoreIntermediateNodes, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
451  connect(ui_->checkBox_ignoreIntermediateNodes, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
452  connect(ui_->checkBox_timeStats, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
453  connect(ui_->checkBox_timeStats, SIGNAL(stateChanged(int)), this, SLOT(updateStatistics()));
454  // Graph view
455  connect(ui_->doubleSpinBox_gainCompensationRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
456  connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
457  connect(ui_->checkBox_cameraProjection, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
458  connect(ui_->checkBox_showDisparityInsteadOfRight, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
459  connect(ui_->spinBox_decimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
460  connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(configModified()));
461  connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
462  connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
463  connect(ui_->horizontalSlider_rotation, SIGNAL(valueChanged(int)), this, SLOT(updateGraphRotation()));
464  connect(ui_->pushButton_applyRotation, SIGNAL(clicked()), this, SLOT(updateGraphView()));
465 
466  connect(ui_->spinBox_icp_decimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
467  connect(ui_->doubleSpinBox_icp_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
468  connect(ui_->doubleSpinBox_icp_minDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
469  connect(ui_->checkBox_icp_from_depth, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
470 
471  connect(ui_->doubleSpinBox_detectMore_radius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
472  connect(ui_->doubleSpinBox_detectMore_radiusMin, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
473  connect(ui_->doubleSpinBox_detectMore_angle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
474  connect(ui_->spinBox_detectMore_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
475  connect(ui_->checkBox_detectMore_intraSession, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
476  connect(ui_->checkBox_detectMore_interSession, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
477  connect(ui_->checkBox_opt_graph_as_guess, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
478 
479  connect(ui_->lineEdit_obstacleColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified()));
480  connect(ui_->lineEdit_groundColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified()));
481  connect(ui_->lineEdit_emptyColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified()));
482  connect(ui_->lineEdit_frontierColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified()));
483  connect(ui_->lineEdit_obstacleColor, SIGNAL(textChanged(const QString &)), this, SLOT(updateGrid()));
484  connect(ui_->lineEdit_groundColor, SIGNAL(textChanged(const QString &)), this, SLOT(updateGrid()));
485  connect(ui_->lineEdit_emptyColor, SIGNAL(textChanged(const QString &)), this, SLOT(updateGrid()));
486  connect(ui_->lineEdit_frontierColor, SIGNAL(textChanged(const QString &)), this, SLOT(updateGrid()));
487  connect(ui_->toolButton_obstacleColor, SIGNAL(clicked(bool)), this, SLOT(selectObstacleColor()));
488  connect(ui_->toolButton_groundColor, SIGNAL(clicked(bool)), this, SLOT(selectGroundColor()));
489  connect(ui_->toolButton_emptyColor, SIGNAL(clicked(bool)), this, SLOT(selectEmptyColor()));
490  connect(ui_->toolButton_frontierColor, SIGNAL(clicked(bool)), this, SLOT(selectFrontierColor()));
491  connect(ui_->spinBox_cropRadius, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
492  connect(ui_->checkBox_grid_showProbMap, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
493  connect(ui_->checkBox_grid_showProbMap, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
494 
495  connect(exportDialog_, SIGNAL(configChanged()), this, SLOT(configModified()));
496 
497  // dockwidget
498  QList<QDockWidget*> dockWidgets = this->findChildren<QDockWidget*>();
499  for(int i=0; i<dockWidgets.size(); ++i)
500  {
501  connect(dockWidgets[i], SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(configModified()));
502  connect(dockWidgets[i]->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configModified()));
503  }
504  ui_->dockWidget_constraints->installEventFilter(this);
505  ui_->dockWidget_graphView->installEventFilter(this);
506  ui_->dockWidget_occupancyGridView->installEventFilter(this);
507  ui_->dockWidget_stereoView->installEventFilter(this);
508  ui_->dockWidget_view3d->installEventFilter(this);
509  ui_->dockWidget_guiparameters->installEventFilter(this);
510  ui_->dockWidget_coreparameters->installEventFilter(this);
511  ui_->dockWidget_info->installEventFilter(this);
512  ui_->dockWidget_statistics->installEventFilter(this);
513 }
514 
515 DatabaseViewer::~DatabaseViewer()
516 {
517  delete ui_;
518  delete dbDriver_;
519 #ifdef RTABMAP_OCTOMAP
520  delete octomap_;
521 #endif
522 }
523 
524 void DatabaseViewer::setupMainLayout(bool vertical)
525 {
526  if(vertical)
527  {
528  qobject_cast<QHBoxLayout *>(ui_->horizontalLayout_imageViews->layout())->setDirection(QBoxLayout::TopToBottom);
529  }
530  else if(!vertical)
531  {
532  qobject_cast<QHBoxLayout *>(ui_->horizontalLayout_imageViews->layout())->setDirection(QBoxLayout::LeftToRight);
533  }
534  if(ids_.size())
535  {
536  sliderAValueChanged(ui_->horizontalSlider_A->value()); // update matching lines
537  }
538 }
539 
540 void DatabaseViewer::showCloseButton(bool visible)
541 {
542  ui_->buttonBox->setVisible(visible);
543 }
544 
545 void DatabaseViewer::configModified()
546 {
547  if(ui_->actionConcise_Layout->isChecked())
548  {
549  ui_->graphicsView_B->setVisible(false);
550  ui_->scrollArea->setVisible(false);
551  ui_->scrollArea_2->setVisible(false);
552  ui_->spinBox_indexB->setVisible(false);
553  ui_->widget_imageControls_B->setVisible(false);
554  ui_->widget_graphControl->setVisible(false);
555  ui_->graphicsView_A->clearLines();
556  ui_->graphicsView_B->clearLines();
557  }
558  else
559  {
560  ui_->graphicsView_B->setVisible(true);
561  ui_->scrollArea->setVisible(true);
562  ui_->scrollArea_2->setVisible(true);
563  ui_->spinBox_indexB->setVisible(true);
564  ui_->widget_imageControls_B->setVisible(true);
565  ui_->widget_graphControl->setVisible(true);
566  }
567 
568  this->setWindowModified(true);
569 }
570 
571 QString DatabaseViewer::getIniFilePath() const
572 {
573  if(!iniFilePath_.isEmpty())
574  {
575  return iniFilePath_;
576  }
577  QString privatePath = QDir::homePath() + "/.rtabmap";
578  if(!QDir(privatePath).exists())
579  {
580  QDir::home().mkdir(".rtabmap");
581  }
582  return privatePath + "/rtabmap.ini";
583 }
584 
585 void DatabaseViewer::readSettings()
586 {
587  QString path = getIniFilePath();
588  QSettings settings(path, QSettings::IniFormat);
589  settings.beginGroup("DatabaseViewer");
590 
591  //load window state / geometry
592  QByteArray bytes;
593  bytes = settings.value("geometry", QByteArray()).toByteArray();
594  if(!bytes.isEmpty())
595  {
596  this->restoreGeometry(bytes);
597  }
598  bytes = settings.value("state", QByteArray()).toByteArray();
599  if(!bytes.isEmpty())
600  {
601  this->restoreState(bytes);
602  }
603  savedMaximized_ = settings.value("maximized", false).toBool();
604 
605  ui_->comboBox_logger_level->setCurrentIndex(settings.value("loggerLevel", ui_->comboBox_logger_level->currentIndex()).toInt());
606  ui_->actionVertical_Layout->setChecked(settings.value("verticalLayout", ui_->actionVertical_Layout->isChecked()).toBool());
607  ui_->actionConcise_Layout->setChecked(settings.value("conciseLayout", ui_->actionConcise_Layout->isChecked()).toBool());
608  ui_->checkBox_ignoreIntermediateNodes->setChecked(settings.value("ignoreIntermediateNodes", ui_->checkBox_ignoreIntermediateNodes->isChecked()).toBool());
609  ui_->checkBox_timeStats->setChecked(settings.value("timeStats", ui_->checkBox_timeStats->isChecked()).toBool());
610 
611  // GraphViewer settings
612  ui_->graphViewer->loadSettings(settings, "GraphView");
613  ui_->graphViewer->setReferentialVisible(false);
614 
615  settings.beginGroup("optimization");
616  ui_->doubleSpinBox_gainCompensationRadius->setValue(settings.value("gainCompensationRadius", ui_->doubleSpinBox_gainCompensationRadius->value()).toDouble());
617  ui_->doubleSpinBox_voxelSize->setValue(settings.value("voxelSize", ui_->doubleSpinBox_voxelSize->value()).toDouble());
618  ui_->spinBox_decimation->setValue(settings.value("decimation", ui_->spinBox_decimation->value()).toInt());
619  ui_->checkBox_cameraProjection->setChecked(settings.value("camProj", ui_->checkBox_cameraProjection->isChecked()).toBool());
620  ui_->checkBox_showDisparityInsteadOfRight->setChecked(settings.value("showDisp", ui_->checkBox_showDisparityInsteadOfRight->isChecked()).toBool());
621  settings.endGroup();
622 
623  settings.beginGroup("grid");
624  ui_->groupBox_posefiltering->setChecked(settings.value("poseFiltering", ui_->groupBox_posefiltering->isChecked()).toBool());
625  ui_->doubleSpinBox_posefilteringRadius->setValue(settings.value("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value()).toDouble());
626  ui_->doubleSpinBox_posefilteringAngle->setValue(settings.value("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value()).toDouble());
627  ui_->lineEdit_obstacleColor->setText(settings.value("colorObstacle", ui_->lineEdit_obstacleColor->text()).toString());
628  ui_->lineEdit_groundColor->setText(settings.value("colorGround", ui_->lineEdit_groundColor->text()).toString());
629  ui_->lineEdit_emptyColor->setText(settings.value("colorEmpty", ui_->lineEdit_emptyColor->text()).toString());
630  ui_->lineEdit_frontierColor->setText(settings.value("colorFrontier", ui_->lineEdit_frontierColor->text()).toString());
631  ui_->spinBox_cropRadius->setValue(settings.value("cropRadius", ui_->spinBox_cropRadius->value()).toInt());
632  ui_->checkBox_grid_showProbMap->setChecked(settings.value("probMap", ui_->checkBox_grid_showProbMap->isChecked()).toBool());
633  settings.endGroup();
634 
635  settings.beginGroup("mesh");
636  ui_->checkBox_mesh_quad->setChecked(settings.value("quad", ui_->checkBox_mesh_quad->isChecked()).toBool());
637  ui_->spinBox_mesh_angleTolerance->setValue(settings.value("angleTolerance", ui_->spinBox_mesh_angleTolerance->value()).toInt());
638  ui_->spinBox_mesh_minClusterSize->setValue(settings.value("minClusterSize", ui_->spinBox_mesh_minClusterSize->value()).toInt());
639  ui_->spinBox_mesh_fillDepthHoles->setValue(settings.value("fillDepthHolesSize", ui_->spinBox_mesh_fillDepthHoles->value()).toInt());
640  ui_->spinBox_mesh_depthError->setValue(settings.value("fillDepthHolesError", ui_->spinBox_mesh_depthError->value()).toInt());
641  ui_->spinBox_mesh_triangleSize->setValue(settings.value("triangleSize", ui_->spinBox_mesh_triangleSize->value()).toInt());
642  settings.endGroup();
643 
644  // ImageViews
645  ui_->graphicsView_A->loadSettings(settings, "ImageViewA");
646  ui_->graphicsView_B->loadSettings(settings, "ImageViewB");
647 
648  // ICP parameters
649  settings.beginGroup("icp");
650  ui_->spinBox_icp_decimation->setValue(settings.value("decimation", ui_->spinBox_icp_decimation->value()).toInt());
651  ui_->doubleSpinBox_icp_maxDepth->setValue(settings.value("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value()).toDouble());
652  ui_->doubleSpinBox_icp_minDepth->setValue(settings.value("minDepth", ui_->doubleSpinBox_icp_minDepth->value()).toDouble());
653  ui_->checkBox_icp_from_depth->setChecked(settings.value("icpFromDepth", ui_->checkBox_icp_from_depth->isChecked()).toBool());
654  settings.endGroup();
655 
656  settings.endGroup(); // DatabaseViewer
657 
658  // Use same parameters used by RTAB-Map
659  settings.beginGroup("Gui");
660  exportDialog_->loadSettings(settings, exportDialog_->objectName());
661  settings.beginGroup("PostProcessingDialog");
662  ui_->doubleSpinBox_detectMore_radius->setValue(settings.value("cluster_radius", ui_->doubleSpinBox_detectMore_radius->value()).toDouble());
663  ui_->doubleSpinBox_detectMore_radiusMin->setValue(settings.value("cluster_radius_min", ui_->doubleSpinBox_detectMore_radiusMin->value()).toDouble());
664  ui_->doubleSpinBox_detectMore_angle->setValue(settings.value("cluster_angle", ui_->doubleSpinBox_detectMore_angle->value()).toDouble());
665  ui_->spinBox_detectMore_iterations->setValue(settings.value("iterations", ui_->spinBox_detectMore_iterations->value()).toInt());
666  ui_->checkBox_detectMore_intraSession->setChecked(settings.value("intra_session", ui_->checkBox_detectMore_intraSession->isChecked()).toBool());
667  ui_->checkBox_detectMore_interSession->setChecked(settings.value("inter_session", ui_->checkBox_detectMore_interSession->isChecked()).toBool());
668  ui_->checkBox_opt_graph_as_guess->setChecked(settings.value("opt_graph_as_guess", ui_->checkBox_opt_graph_as_guess->isChecked()).toBool());
669  settings.endGroup();
670  settings.endGroup();
671 
672 
673  ParametersMap parameters;
674  Parameters::readINI(path.toStdString(), parameters);
675  for(ParametersMap::iterator iter = parameters.begin(); iter!= parameters.end(); ++iter)
676  {
677  ui_->parameters_toolbox->updateParameter(iter->first, iter->second);
678  }
679 }
680 
681 void DatabaseViewer::writeSettings()
682 {
683  QString path = getIniFilePath();
684  QSettings settings(path, QSettings::IniFormat);
685  settings.beginGroup("DatabaseViewer");
686 
687  //save window state / geometry
688  if(!this->isMaximized())
689  {
690  settings.setValue("geometry", this->saveGeometry());
691  }
692  settings.setValue("state", this->saveState());
693  settings.setValue("maximized", this->isMaximized());
694  savedMaximized_ = this->isMaximized();
695 
696  settings.setValue("loggerLevel", ui_->comboBox_logger_level->currentIndex());
697  settings.setValue("verticalLayout", ui_->actionVertical_Layout->isChecked());
698  settings.setValue("conciseLayout", ui_->actionConcise_Layout->isChecked());
699  settings.setValue("ignoreIntermediateNodes", ui_->checkBox_ignoreIntermediateNodes->isChecked());
700  settings.setValue("timeStats", ui_->checkBox_timeStats->isChecked());
701 
702  // save GraphViewer settings
703  ui_->graphViewer->saveSettings(settings, "GraphView");
704 
705  // save optimization settings
706  settings.beginGroup("optimization");
707  settings.setValue("gainCompensationRadius", ui_->doubleSpinBox_gainCompensationRadius->value());
708  settings.setValue("voxelSize", ui_->doubleSpinBox_voxelSize->value());
709  settings.setValue("decimation", ui_->spinBox_decimation->value());
710  settings.setValue("camProj", ui_->checkBox_cameraProjection->isChecked());
711  settings.setValue("showDisp", ui_->checkBox_showDisparityInsteadOfRight->isChecked());
712  settings.endGroup();
713 
714  // save Grid settings
715  settings.beginGroup("grid");
716  settings.setValue("poseFiltering", ui_->groupBox_posefiltering->isChecked());
717  settings.setValue("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value());
718  settings.setValue("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value());
719  settings.setValue("colorObstacle", ui_->lineEdit_obstacleColor->text());
720  settings.setValue("colorGround", ui_->lineEdit_groundColor->text());
721  settings.setValue("colorEmpty", ui_->lineEdit_emptyColor->text());
722  settings.setValue("colorFrontier", ui_->lineEdit_frontierColor->text());
723  settings.setValue("cropRadius", ui_->spinBox_cropRadius->value());
724  settings.setValue("probMap", ui_->checkBox_grid_showProbMap->isChecked());
725  settings.endGroup();
726 
727  settings.beginGroup("mesh");
728  settings.setValue("quad", ui_->checkBox_mesh_quad->isChecked());
729  settings.setValue("angleTolerance", ui_->spinBox_mesh_angleTolerance->value());
730  settings.setValue("minClusterSize", ui_->spinBox_mesh_minClusterSize->value());
731  settings.setValue("fillDepthHolesSize", ui_->spinBox_mesh_fillDepthHoles->value());
732  settings.setValue("fillDepthHolesError", ui_->spinBox_mesh_depthError->value());
733  settings.setValue("triangleSize", ui_->spinBox_mesh_triangleSize->value());
734  settings.endGroup();
735 
736  // ImageViews
737  ui_->graphicsView_A->saveSettings(settings, "ImageViewA");
738  ui_->graphicsView_B->saveSettings(settings, "ImageViewB");
739 
740  // save ICP parameters
741  settings.beginGroup("icp");
742  settings.setValue("decimation", ui_->spinBox_icp_decimation->value());
743  settings.setValue("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value());
744  settings.setValue("minDepth", ui_->doubleSpinBox_icp_minDepth->value());
745  settings.setValue("icpFromDepth", ui_->checkBox_icp_from_depth->isChecked());
746  settings.endGroup();
747 
748  settings.endGroup(); // DatabaseViewer
749 
750  // Use same parameters used by RTAB-Map
751  settings.beginGroup("Gui");
752  exportDialog_->saveSettings(settings, exportDialog_->objectName());
753  settings.beginGroup("PostProcessingDialog");
754  settings.setValue("cluster_radius", ui_->doubleSpinBox_detectMore_radius->value());
755  settings.setValue("cluster_radius_min", ui_->doubleSpinBox_detectMore_radiusMin->value());
756  settings.setValue("cluster_angle", ui_->doubleSpinBox_detectMore_angle->value());
757  settings.setValue("iterations", ui_->spinBox_detectMore_iterations->value());
758  settings.setValue("intra_session", ui_->checkBox_detectMore_intraSession->isChecked());
759  settings.setValue("inter_session", ui_->checkBox_detectMore_interSession->isChecked());
760  settings.setValue("opt_graph_as_guess", ui_->checkBox_opt_graph_as_guess->isChecked());
761  settings.endGroup();
762  settings.endGroup();
763 
764  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
765  for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end();)
766  {
767  if(!ui_->parameters_toolbox->getParameterWidget(iter->first.c_str()))
768  {
769  parameters.erase(iter++);
770  }
771  else
772  {
773  ++iter;
774  }
775  }
776  Parameters::writeINI(path.toStdString(), parameters);
777 
778  this->setWindowModified(false);
779 }
780 
781 void DatabaseViewer::restoreDefaultSettings()
782 {
783  // reset GUI parameters
784  ui_->comboBox_logger_level->setCurrentIndex(1);
785  ui_->checkBox_alignPosesWithGPS->setChecked(true);
786  ui_->checkBox_alignPosesWithGroundTruth->setChecked(true);
787  ui_->checkBox_alignScansCloudsWithGroundTruth->setChecked(false);
788  ui_->checkBox_ignoreIntermediateNodes->setChecked(false);
789  ui_->checkBox_timeStats->setChecked(true);
790 
791  ui_->comboBox_optimizationFlavor->setCurrentIndex(0);
792  ui_->checkBox_spanAllMaps->setChecked(true);
793  ui_->checkBox_wmState->setChecked(false);
794  ui_->checkBox_ignorePoseCorrection->setChecked(false);
795  ui_->checkBox_ignoreGlobalLoop->setChecked(false);
796  ui_->checkBox_ignoreLocalLoopSpace->setChecked(false);
797  ui_->checkBox_ignoreLocalLoopTime->setChecked(false);
798  ui_->checkBox_ignoreUserLoop->setChecked(false);
799  ui_->checkBox_ignoreLandmarks->setChecked(false);
800  ui_->doubleSpinBox_optimizationScale->setValue(1.0);
801  ui_->doubleSpinBox_gainCompensationRadius->setValue(0.0);
802  ui_->doubleSpinBox_voxelSize->setValue(0.0);
803  ui_->spinBox_decimation->setValue(1);
804  ui_->checkBox_cameraProjection->setChecked(false);
805  ui_->checkBox_showDisparityInsteadOfRight->setChecked(false);
806 
807  ui_->groupBox_posefiltering->setChecked(false);
808  ui_->doubleSpinBox_posefilteringRadius->setValue(0.1);
809  ui_->doubleSpinBox_posefilteringAngle->setValue(30);
810  ui_->checkBox_grid_empty->setChecked(true);
811  ui_->checkBox_grid_frontiers->setChecked(false);
812  ui_->checkBox_octomap->setChecked(false);
813  ui_->lineEdit_obstacleColor->setText(QColor(Qt::red).name());
814  ui_->lineEdit_groundColor->setText(QColor(Qt::green).name());
815  ui_->lineEdit_emptyColor->setText(QColor(Qt::yellow).name());
816  ui_->lineEdit_frontierColor->setText(QColor(Qt::cyan).name());
817  ui_->spinBox_cropRadius->setValue(1);
818  ui_->checkBox_grid_showProbMap->setChecked(false);
819 
820  ui_->checkBox_mesh_quad->setChecked(true);
821  ui_->spinBox_mesh_angleTolerance->setValue(15);
822  ui_->spinBox_mesh_minClusterSize->setValue(0);
823  ui_->spinBox_mesh_fillDepthHoles->setValue(false);
824  ui_->spinBox_mesh_depthError->setValue(10);
825  ui_->spinBox_mesh_triangleSize->setValue(2);
826 
827  ui_->spinBox_icp_decimation->setValue(1);
828  ui_->doubleSpinBox_icp_maxDepth->setValue(0.0);
829  ui_->doubleSpinBox_icp_minDepth->setValue(0.0);
830  ui_->checkBox_icp_from_depth->setChecked(false);
831 
832  ui_->doubleSpinBox_detectMore_radius->setValue(1.0);
833  ui_->doubleSpinBox_detectMore_radiusMin->setValue(0.0);
834  ui_->doubleSpinBox_detectMore_angle->setValue(30.0);
835  ui_->spinBox_detectMore_iterations->setValue(5);
836  ui_->checkBox_detectMore_intraSession->setChecked(true);
837  ui_->checkBox_detectMore_interSession->setChecked(true);
838  ui_->checkBox_opt_graph_as_guess->setChecked(true);
839 }
840 
841 void DatabaseViewer::openDatabase()
842 {
843  QString path = QFileDialog::getOpenFileName(this, tr("Select file"), pathDatabase_, tr("Databases (*.db)"));
844  if(!path.isEmpty())
845  {
846  openDatabase(path);
847  }
848 }
849 
850 bool DatabaseViewer::openDatabase(const QString & path, const ParametersMap & overriddenParameters)
851 {
852  UDEBUG("Open database \"%s\"", path.toStdString().c_str());
853  if(QFile::exists(path))
854  {
855  if(QFileInfo(path).isFile())
856  {
857  std::string driverType = "sqlite3";
858 
859  dbDriver_ = DBDriver::create();
860 
861  if(!dbDriver_->openConnection(path.toStdString()))
862  {
863  ui_->actionClose_database->setEnabled(false);
864  ui_->actionOpen_database->setEnabled(true);
865  delete dbDriver_;
866  dbDriver_ = 0;
867  QMessageBox::warning(this, "Database error", tr("Can't open database \"%1\"").arg(path));
868  }
869  else
870  {
871  ui_->actionClose_database->setEnabled(true);
872  ui_->actionOpen_database->setEnabled(false);
873 
874  pathDatabase_ = UDirectory::getDir(path.toStdString()).c_str();
875  if(pathDatabase_.isEmpty() || pathDatabase_.compare(".") == 0)
876  {
877  pathDatabase_ = QDir::currentPath();
878  }
879  databaseFileName_ = UFile::getName(path.toStdString());
880  ui_->graphViewer->setWorkingDirectory(pathDatabase_);
881 
882  // look if there are saved parameters
883  ParametersMap parameters = dbDriver_->getLastParameters();
884 
885  // add overridden parameters
886  uInsert(parameters, overriddenParameters);
887 
888  if(parameters.size())
889  {
890  const ParametersMap & currentParameters = ui_->parameters_toolbox->getParameters();
891  ParametersMap differentParameters;
892  for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
893  {
894  ParametersMap::const_iterator jter = currentParameters.find(iter->first);
895  if(jter!=currentParameters.end() &&
896  ui_->parameters_toolbox->getParameterWidget(QString(iter->first.c_str())) != 0 &&
897  iter->second.compare(jter->second) != 0 &&
898  iter->first.compare(Parameters::kRtabmapWorkingDirectory()) != 0)
899  {
900  bool different = true;
901  if(Parameters::getType(iter->first).compare("double") ==0 ||
902  Parameters::getType(iter->first).compare("float") == 0)
903  {
904  if(uStr2Double(iter->second) == uStr2Double(jter->second))
905  {
906  different = false;
907  }
908  }
909  if(different)
910  {
911  differentParameters.insert(*iter);
912  QString msg = tr("Parameter \"%1\": database=\"%2\" Preferences=\"%3\"")
913  .arg(iter->first.c_str())
914  .arg(iter->second.c_str())
915  .arg(jter->second.c_str());
916  UWARN(msg.toStdString().c_str());
917  }
918  }
919  }
920 
921  if(differentParameters.size())
922  {
923  int r = QMessageBox::question(this,
924  tr("Update parameters..."),
925  tr("The database is using %1 different parameter(s) than "
926  "those currently set in Core parameters panel. Do you want "
927  "to use database's parameters?").arg(differentParameters.size()),
928  QMessageBox::Yes | QMessageBox::No,
929  QMessageBox::Yes);
930  if(r == QMessageBox::Yes)
931  {
932  QStringList str;
933  for(rtabmap::ParametersMap::const_iterator iter = differentParameters.begin(); iter!=differentParameters.end(); ++iter)
934  {
935  ui_->parameters_toolbox->updateParameter(iter->first, iter->second);
936  str.push_back(iter->first.c_str());
937  }
938  notifyParametersChanged(str);
939  }
940  }
941  }
942 
943  updateIds();
944  this->setWindowTitle("RTAB-Map Database Viewer - " + path + "[*]");
945  return true;
946  }
947  }
948  else // directory
949  {
950  pathDatabase_ = path;
951  if(pathDatabase_.isEmpty() || pathDatabase_.compare(".") == 0)
952  {
953  pathDatabase_ = QDir::currentPath();
954  }
955  ui_->graphViewer->setWorkingDirectory(pathDatabase_);
956  }
957  }
958  else
959  {
960  QMessageBox::warning(this, "Database error", tr("Database \"%1\" does not exist.").arg(path));
961  }
962  return false;
963 }
964 
965 bool DatabaseViewer::closeDatabase()
966 {
967  this->setWindowTitle("RTAB-Map Database Viewer[*]");
968  if(dbDriver_)
969  {
970  if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size())
971  {
972  QMessageBox::StandardButton button = QMessageBox::question(this,
973  tr("Links modified"),
974  tr("Some links are modified (%1 added, %2 refined, %3 removed), do you want to save them?")
975  .arg(linksAdded_.size()).arg(linksRefined_.size()).arg(linksRemoved_.size()),
976  QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
977  QMessageBox::Cancel);
978 
979  if(button == QMessageBox::Yes)
980  {
981  // Added links
982  for(std::multimap<int, rtabmap::Link>::iterator iter=linksAdded_.begin(); iter!=linksAdded_.end(); ++iter)
983  {
984  std::multimap<int, rtabmap::Link>::iterator refinedIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
985  if(refinedIter != linksRefined_.end())
986  {
987  dbDriver_->addLink(refinedIter->second);
988  if(refinedIter->second.from() != refinedIter->second.to())
989  dbDriver_->addLink(refinedIter->second.inverse());
990  }
991  else
992  {
993  dbDriver_->addLink(iter->second);
994  if(iter->second.from() != iter->second.to())
995  dbDriver_->addLink(iter->second.inverse());
996  }
997  }
998 
999  //Refined links
1000  for(std::multimap<int, rtabmap::Link>::iterator iter=linksRefined_.begin(); iter!=linksRefined_.end(); ++iter)
1001  {
1002  if(!containsLink(linksAdded_, iter->second.from(), iter->second.to()))
1003  {
1004  dbDriver_->updateLink(iter->second);
1005  if(iter->second.from() != iter->second.to())
1006  dbDriver_->updateLink(iter->second.inverse());
1007  }
1008  }
1009 
1010  // Rejected links
1011  for(std::multimap<int, rtabmap::Link>::iterator iter=linksRemoved_.begin(); iter!=linksRemoved_.end(); ++iter)
1012  {
1013  dbDriver_->removeLink(iter->second.to(), iter->second.from());
1014  if(iter->second.from() != iter->second.to())
1015  dbDriver_->removeLink(iter->second.from(), iter->second.to());
1016  }
1017  linksAdded_.clear();
1018  linksRefined_.clear();
1019  linksRemoved_.clear();
1020 
1021  // Clear the optimized poses, this will force rtabmap to re-optimize the graph on initialization
1022  Transform lastLocalizationPose;
1023  if(!dbDriver_->loadOptimizedPoses(&lastLocalizationPose).empty())
1024  {
1025  dbDriver_->saveOptimizedPoses(std::map<int, Transform>(), lastLocalizationPose);
1026  }
1027  // This will force rtabmap_ros to regenerate the global occupancy grid if there was one
1028  dbDriver_->save2DMap(cv::Mat(), 0, 0, 0);
1029  dbDriver_->saveOptimizedMesh(cv::Mat());
1030  }
1031 
1032  if(button != QMessageBox::Yes && button != QMessageBox::No)
1033  {
1034  return false;
1035  }
1036  }
1037 
1038  if( generatedLocalMaps_.size() &&
1039  uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.11.10") >= 0)
1040  {
1041  QMessageBox::StandardButton button = QMessageBox::question(this,
1042  tr("Local occupancy grid maps modified"),
1043  tr("%1 occupancy grid maps are modified, do you want to "
1044  "save them? This will overwrite occupancy grids saved in the database.")
1045  .arg(generatedLocalMaps_.size()),
1046  QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
1047  QMessageBox::Cancel);
1048 
1049  if(button == QMessageBox::Yes)
1050  {
1051  for(std::map<int, LocalGrid>::const_iterator mapIter = generatedLocalMaps_.localGrids().begin();
1052  mapIter!=generatedLocalMaps_.localGrids().end();
1053  ++mapIter)
1054  {
1055  dbDriver_->updateOccupancyGrid(
1056  mapIter->first,
1057  mapIter->second.groundCells,
1058  mapIter->second.obstacleCells,
1059  mapIter->second.emptyCells,
1060  mapIter->second.cellSize,
1061  mapIter->second.viewPoint);
1062  }
1063  generatedLocalMaps_.clear();
1064  localMaps_.clear();
1065 
1066  // This will force rtabmap_ros to regenerate the global occupancy grid if there was one
1067  dbDriver_->save2DMap(cv::Mat(), 0, 0, 0);
1068  }
1069 
1070  if(button != QMessageBox::Yes && button != QMessageBox::No)
1071  {
1072  return false;
1073  }
1074  }
1075 
1076  if(!modifiedLaserScans_.empty())
1077  {
1078  QMessageBox::StandardButton button = QMessageBox::question(this,
1079  tr("Laser scans modified"),
1080  tr("%1 laser scans are modified, do you want to "
1081  "save them? This will overwrite laser scans saved in the database.")
1082  .arg(modifiedLaserScans_.size()),
1083  QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
1084  QMessageBox::Cancel);
1085 
1086  if(button == QMessageBox::Yes)
1087  {
1088  for(std::map<int, LaserScan>::iterator iter=modifiedLaserScans_.begin(); iter!=modifiedLaserScans_.end(); ++iter)
1089  {
1090  dbDriver_->updateLaserScan(iter->first, iter->second);
1091  }
1092  modifiedLaserScans_.clear();
1093  }
1094 
1095  if(button != QMessageBox::Yes && button != QMessageBox::No)
1096  {
1097  return false;
1098  }
1099  }
1100 
1101  delete dbDriver_;
1102  dbDriver_ = 0;
1103  ids_.clear();
1104  idToIndex_.clear();
1105  neighborLinks_.clear();
1106  loopLinks_.clear();
1107  graphes_.clear();
1108  graphLinks_.clear();
1109  odomPoses_.clear();
1110  groundTruthPoses_.clear();
1111  gpsPoses_.clear();
1112  gpsValues_.clear();
1113  lastWmIds_.clear();
1114  mapIds_.clear();
1115  weights_.clear();
1116  wmStates_.clear();
1117  links_.clear();
1118  linksAdded_.clear();
1119  linksRefined_.clear();
1120  linksRemoved_.clear();
1121  localMaps_.clear();
1122  generatedLocalMaps_.clear();
1123  modifiedLaserScans_.clear();
1124  ui_->graphViewer->clearAll();
1125  occupancyGridViewer_->clear();
1126  ui_->menuEdit->setEnabled(false);
1127  ui_->actionGenerate_3D_map_pcd->setEnabled(false);
1128  ui_->actionExport->setEnabled(false);
1129  ui_->actionExtract_images->setEnabled(false);
1130  ui_->menuExport_poses->setEnabled(false);
1131  ui_->menuExport_GPS->setEnabled(false);
1132  ui_->actionPoses_KML->setEnabled(false);
1133  ui_->actionEdit_optimized_2D_map->setEnabled(false);
1134  ui_->actionExport_saved_2D_map->setEnabled(false);
1135  ui_->actionImport_2D_map->setEnabled(false);
1136  ui_->actionRegenerate_optimized_2D_map->setEnabled(false);
1137  ui_->actionView_optimized_mesh->setEnabled(false);
1138  ui_->actionExport_optimized_mesh->setEnabled(false);
1139  ui_->actionUpdate_optimized_mesh->setEnabled(false);
1140  ui_->checkBox_showOptimized->setEnabled(false);
1141  ui_->toolBox_statistics->clear();
1142  databaseFileName_.clear();
1143  ui_->checkBox_alignPosesWithGPS->setVisible(false);
1144  ui_->checkBox_alignPosesWithGPS->setEnabled(false);
1145  ui_->checkBox_alignPosesWithGroundTruth->setVisible(false);
1146  ui_->checkBox_alignPosesWithGroundTruth->setEnabled(false);
1147  ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(false);
1148  ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(false);
1149  ui_->doubleSpinBox_optimizationScale->setVisible(false);
1150  ui_->label_scale_title->setVisible(false);
1151  ui_->label_rmse->setVisible(false);
1152  ui_->label_rmse_title->setVisible(false);
1153  ui_->checkBox_ignoreIntermediateNodes->setVisible(false);
1154  ui_->label_ignoreINtermediateNdoes->setVisible(false);
1155  ui_->label_alignPosesWithGPS->setVisible(false);
1156  ui_->label_alignPosesWithGroundTruth->setVisible(false);
1157  ui_->label_alignScansCloudsWithGroundTruth->setVisible(false);
1158  ui_->label_optimizeFrom->setText(tr("Root"));
1159  ui_->textEdit_info->clear();
1160 
1161  ui_->pushButton_refine->setEnabled(false);
1162  ui_->pushButton_add->setEnabled(false);
1163  ui_->pushButton_reset->setEnabled(false);
1164  ui_->pushButton_reject->setEnabled(false);
1165 
1166  ui_->horizontalSlider_loops->setEnabled(false);
1167  ui_->horizontalSlider_loops->setMaximum(0);
1168  ui_->horizontalSlider_iterations->setEnabled(false);
1169  ui_->horizontalSlider_iterations->setMaximum(0);
1170  ui_->horizontalSlider_neighbors->setEnabled(false);
1171  ui_->horizontalSlider_neighbors->setMaximum(0);
1172  ui_->label_constraint->clear();
1173  ui_->label_constraint_opt->clear();
1174  ui_->label_variance->clear();
1175  ui_->lineEdit_covariance->clear();
1176  ui_->label_type->clear();
1177  ui_->label_type_name->clear();
1178  ui_->checkBox_showOptimized->setEnabled(false);
1179 
1180  ui_->horizontalSlider_A->setEnabled(false);
1181  ui_->horizontalSlider_A->setMaximum(0);
1182  ui_->horizontalSlider_B->setEnabled(false);
1183  ui_->horizontalSlider_B->setMaximum(0);
1184  ui_->label_idA->setText("NaN");
1185  ui_->label_idB->setText("NaN");
1186  sliderAValueChanged(0);
1187  sliderBValueChanged(0);
1188 
1189  ui_->spinBox_indexA->setEnabled(false);
1190  ui_->spinBox_indexA->setMaximum(0);
1191  ui_->spinBox_indexB->setEnabled(false);
1192  ui_->spinBox_indexB->setMaximum(0);
1193 
1194  constraintsViewer_->clear();
1195  constraintsViewer_->refreshView();
1196 
1197  cloudViewer_->clear();
1198  cloudViewer_->refreshView();
1199 
1200  occupancyGridViewer_->clear();
1201  occupancyGridViewer_->refreshView();
1202 
1203  ui_->graphViewer->clearAll();
1204  ui_->label_loopClosures->clear();
1205  ui_->label_timeOptimization->clear();
1206  ui_->label_pathLength->clear();
1207  ui_->label_poses->clear();
1208  ui_->label_rmse->clear();
1209  ui_->spinBox_optimizationsFrom->setEnabled(false);
1210 
1211  ui_->graphicsView_A->clear();
1212  ui_->graphicsView_B->clear();
1213 
1214  ui_->graphicsView_stereo->clear();
1215  stereoViewer_->clear();
1216  stereoViewer_->refreshView();
1217 
1218  ui_->toolBox_statistics->clear();
1219 
1220  // This will re-init the dialog
1221  delete linkRefiningDialog_;
1222  linkRefiningDialog_ = new LinkRefiningDialog(this);
1223  }
1224 
1225  ui_->actionClose_database->setEnabled(dbDriver_ != 0);
1226  ui_->actionOpen_database->setEnabled(dbDriver_ == 0);
1227 
1228  return dbDriver_ == 0;
1229 }
1230 
1231 
1232 void DatabaseViewer::recoverDatabase()
1233 {
1234  QString path = QFileDialog::getOpenFileName(this, tr("Select file"), pathDatabase_, tr("Databases (*.db)"));
1235  if(!path.isEmpty())
1236  {
1237  if(path.compare(pathDatabase_+QDir::separator()+databaseFileName_.c_str()) == 0)
1238  {
1239  QMessageBox::information(this, "Database recovery", tr("The selected database is already opened, close it first."));
1240  return;
1241  }
1242  std::string errorMsg;
1243  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
1244  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
1245  progressDialog->setMaximumSteps(100);
1246  progressDialog->show();
1247  progressDialog->setCancelButtonVisible(true);
1248  RecoveryState state(progressDialog);
1249  if(databaseRecovery(path.toStdString(), false, &errorMsg, &state))
1250  {
1251  QMessageBox::information(this, "Database recovery", tr("Database \"%1\" recovered! Try opening it again.").arg(path));
1252  }
1253  else
1254  {
1255  QMessageBox::warning(this, "Database recovery", tr("Database recovery failed: \"%1\".").arg(errorMsg.c_str()));
1256  }
1257  progressDialog->setValue(progressDialog->maximumSteps());
1258  }
1259 }
1260 
1261 void DatabaseViewer::closeEvent(QCloseEvent* event)
1262 {
1263  //write settings before quit?
1264  bool save = false;
1265  if(this->isWindowModified())
1266  {
1267  QMessageBox::Button b=QMessageBox::question(this,
1268  tr("Database Viewer"),
1269  tr("There are unsaved changed settings. Save them?"),
1270  QMessageBox::Save | QMessageBox::Cancel | QMessageBox::Discard);
1271  if(b == QMessageBox::Save)
1272  {
1273  save = true;
1274  }
1275  else if(b != QMessageBox::Discard)
1276  {
1277  event->ignore();
1278  return;
1279  }
1280  }
1281 
1282  if(save)
1283  {
1284  writeSettings();
1285  }
1286 
1287  event->accept();
1288 
1289  if(!closeDatabase())
1290  {
1291  event->ignore();
1292  }
1293 
1294  if(event->isAccepted())
1295  {
1296  ui_->toolBox_statistics->closeFigures();
1297  if(dbDriver_)
1298  {
1299  delete dbDriver_;
1300  dbDriver_ = 0;
1301  }
1302  }
1303 }
1304 
1305 void DatabaseViewer::showEvent(QShowEvent* anEvent)
1306 {
1307  this->setWindowModified(false);
1308 
1309  if((ui_->graphViewer->isVisible() || ui_->dockWidget_occupancyGridView->isVisible()) && graphes_.size() && localMaps_.size()==0)
1310  {
1311  sliderIterationsValueChanged((int)graphes_.size()-1);
1312  }
1313 }
1314 
1315 void DatabaseViewer::moveEvent(QMoveEvent* anEvent)
1316 {
1317  if(this->isVisible())
1318  {
1319  // HACK, there is a move event when the window is shown the first time.
1320  if(!firstCall_)
1321  {
1322  this->configModified();
1323  }
1324  firstCall_ = false;
1325  }
1326 }
1327 
1328 void DatabaseViewer::resizeEvent(QResizeEvent* anEvent)
1329 {
1330  if(this->isVisible())
1331  {
1332  this->configModified();
1333  }
1334 }
1335 
1336 void DatabaseViewer::keyPressEvent(QKeyEvent *event)
1337 {
1338  //catch ctrl-s to save settings
1339  if((event->modifiers() & Qt::ControlModifier) && event->key() == Qt::Key_S)
1340  {
1341  this->writeSettings();
1342  }
1343 }
1344 
1345 bool DatabaseViewer::eventFilter(QObject *obj, QEvent *event)
1346 {
1347  if (event->type() == QEvent::Resize && qobject_cast<QDockWidget*>(obj))
1348  {
1349  this->setWindowModified(true);
1350  }
1351  return QWidget::eventFilter(obj, event);
1352 }
1353 
1354 
1355 void DatabaseViewer::exportDatabase()
1356 {
1357  if(!dbDriver_ || ids_.size() == 0)
1358  {
1359  return;
1360  }
1361 
1362  rtabmap::ExportDialog dialog;
1363 
1364  if(dialog.exec())
1365  {
1366  if(!dialog.outputPath().isEmpty())
1367  {
1368  int framesIgnored = dialog.framesIgnored();
1369  double frameRate = dialog.targetFramerate();
1370  int sessionExported = dialog.sessionExported();
1371  QString path = dialog.outputPath();
1372  rtabmap::DataRecorder recorder;
1373  QList<int> ids;
1374 
1375  double previousStamp = 0;
1376  std::vector<double> delays(ids_.size());
1377  int oi=0;
1378  std::map<int, Transform> poses;
1379  std::map<int, double> stamps;
1380  std::map<int, Transform> groundTruths;
1381  std::map<int, GPS> gpsValues;
1382  std::map<int, EnvSensors> sensorsValues;
1383  for(int i=0; i<ids_.size(); i+=1+framesIgnored)
1384  {
1385  Transform odomPose, groundTruth;
1386  int weight = -1;
1387  int mapId = -1;
1388  std::string label;
1389  double stamp = 0;
1390  std::vector<float> velocity;
1391  GPS gps;
1392  EnvSensors sensors;
1393  if(dbDriver_->getNodeInfo(ids_[i], odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors))
1394  {
1395  if(frameRate == 0 ||
1396  previousStamp == 0 ||
1397  stamp == 0 ||
1398  stamp - previousStamp >= 1.0/frameRate)
1399  {
1400  if(sessionExported < 0 || sessionExported == mapId)
1401  {
1402  ids.push_back(ids_[i]);
1403 
1404  if(previousStamp && stamp)
1405  {
1406  delays[oi++] = stamp - previousStamp;
1407  }
1408  previousStamp = stamp;
1409 
1410  poses.insert(std::make_pair(ids_[i], odomPose));
1411  stamps.insert(std::make_pair(ids_[i], stamp));
1412  groundTruths.insert(std::make_pair(ids_[i], groundTruth));
1413  if(gps.stamp() > 0.0)
1414  {
1415  gpsValues.insert(std::make_pair(ids_[i], gps));
1416  }
1417  if(sensors.size())
1418  {
1419  sensorsValues.insert(std::make_pair(ids_[i], sensors));
1420  }
1421  }
1422  }
1423  if(sessionExported >= 0 && mapId > sessionExported)
1424  {
1425  break;
1426  }
1427  }
1428  }
1429  delays.resize(oi);
1430 
1431  if(recorder.init(path, false))
1432  {
1433  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
1434  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
1435  progressDialog->setMaximumSteps(ids.size());
1436  progressDialog->show();
1437  progressDialog->setCancelButtonVisible(true);
1438  UINFO("Decompress: rgb=%d depth=%d scan=%d userData=%d",
1439  dialog.isRgbExported()?1:0,
1440  dialog.isDepthExported()?1:0,
1441  dialog.isDepth2dExported()?1:0,
1442  dialog.isUserDataExported()?1:0);
1443 
1444  for(int i=0; i<ids.size() && !progressDialog->isCanceled(); ++i)
1445  {
1446  int id = ids.at(i);
1447 
1448  SensorData data;
1449  dbDriver_->getNodeData(id, data);
1450  cv::Mat depth, rgb, userData;
1451  LaserScan scan;
1452  data.uncompressDataConst(
1453  !dialog.isRgbExported()?0:&rgb,
1454  !dialog.isDepthExported()?0:&depth,
1455  !dialog.isDepth2dExported()?0:&scan,
1456  !dialog.isUserDataExported()?0:&userData);
1457  cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
1458  if(dialog.isOdomExported())
1459  {
1460  std::multimap<int, Link> links;
1461  dbDriver_->loadLinks(id, links, Link::kNeighbor);
1462  if(links.size() && links.begin()->first < id)
1463  {
1464  covariance = links.begin()->second.infMatrix().inv();
1465  }
1466  }
1467 
1468  rtabmap::SensorData sensorData;
1469  if(data.cameraModels().size())
1470  {
1471  sensorData = rtabmap::SensorData(
1472  scan,
1473  rgb,
1474  depth,
1475  data.cameraModels(),
1476  id,
1477  stamps.at(id),
1478  userData);
1479  }
1480  else
1481  {
1482  sensorData = rtabmap::SensorData(
1483  scan,
1484  rgb,
1485  depth,
1486  data.stereoCameraModels(),
1487  id,
1488  stamps.at(id),
1489  userData);
1490  }
1491  if(groundTruths.find(id)!=groundTruths.end())
1492  {
1493  sensorData.setGroundTruth(groundTruths.at(id));
1494  }
1495  if(gpsValues.find(id)!=gpsValues.end())
1496  {
1497  sensorData.setGPS(gpsValues.at(id));
1498  }
1499  if(sensorsValues.find(id)!=sensorsValues.end())
1500  {
1501  sensorData.setEnvSensors(sensorsValues.at(id));
1502  }
1503 
1504  recorder.addData(sensorData, dialog.isOdomExported()?poses.at(id):Transform(), covariance);
1505 
1506  progressDialog->appendText(tr("Exported node %1").arg(id));
1507  progressDialog->incrementStep();
1508  QApplication::processEvents();
1509  }
1510  progressDialog->setValue(progressDialog->maximumSteps());
1511  if(delays.size())
1512  {
1513  progressDialog->appendText(tr("Average frame rate=%1 Hz (Min=%2, Max=%3)")
1514  .arg(1.0/uMean(delays)).arg(1.0/uMax(delays)).arg(1.0/uMin(delays)));
1515  }
1516  progressDialog->appendText(tr("Export finished to \"%1\"!").arg(path));
1517  }
1518  else
1519  {
1520  UERROR("DataRecorder init failed?!");
1521  }
1522  }
1523  else
1524  {
1525  QMessageBox::warning(this, tr("Cannot export database"), tr("An output path must be set!"));
1526  }
1527  }
1528 }
1529 
1530 void DatabaseViewer::extractImages()
1531 {
1532  if(!dbDriver_ || ids_.size() == 0)
1533  {
1534  return;
1535  }
1536 
1537  QStringList formats;
1538  formats.push_back("id.jpg");
1539  formats.push_back("id.png");
1540  formats.push_back("timestamp.jpg");
1541  formats.push_back("timestamp.png");
1542  bool ok;
1543  QString format = QInputDialog::getItem(this, tr("Which RGB format?"), tr("Format:"), formats, 0, false, &ok);
1544  if(!ok)
1545  {
1546  return;
1547  }
1548 
1549  QString ext = format.split('.').back();
1550  bool useStamp = format.split('.').front().compare("timestamp") == 0;
1551  bool directoriesCreated = false;
1552  QString path = QFileDialog::getExistingDirectory(this, tr("Select directory where to save images..."), pathDatabase_);
1553  if(!path.isEmpty())
1554  {
1555  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
1556  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
1557  progressDialog->setMaximumSteps(ids_.size());
1558  progressDialog->setCancelButtonVisible(true);
1559  progressDialog->appendText(tr("Saving %1 images to %2...").arg(ids_.size()).arg(path));
1560  progressDialog->show();
1561 
1562  int imagesExported = 0;
1563  for(int i=0; i<ids_.size(); ++i)
1564  {
1565  QString id = QString::number(ids_.at(i));
1566 
1567  SensorData data;
1568  dbDriver_->getNodeData(ids_.at(i), data);
1569  data.uncompressData();
1570 
1571  if(!directoriesCreated)
1572  {
1573  //stereo
1574  if(!data.imageRaw().empty() && !data.rightRaw().empty())
1575  {
1576  QDir dir;
1577  dir.mkdir(QString("%1/left").arg(path));
1578  dir.mkdir(QString("%1/right").arg(path));
1579  dir.mkdir(QString("%1/calib").arg(path));
1580  directoriesCreated = true;
1581  }
1582  else if(!data.imageRaw().empty())
1583  {
1584  if(!data.depthRaw().empty())
1585  {
1586  QDir dir;
1587  dir.mkdir(QString("%1/rgb").arg(path));
1588  dir.mkdir(QString("%1/depth").arg(path));
1589  dir.mkdir(QString("%1/calib").arg(path));
1590  directoriesCreated = true;
1591  }
1592  }
1593  }
1594 
1595  if(!data.imageRaw().empty() && useStamp)
1596  {
1597  Transform p,gt;
1598  int m,w;
1599  std::string l;
1600  double stamp;
1601  std::vector<float> v;
1602  GPS gps;
1603  EnvSensors s;
1604  dbDriver_->getNodeInfo(ids_.at(i), p, m, w, l, stamp, gt, v, gps, s);
1605  if(stamp == 0.0)
1606  {
1607  UWARN("Node %d has null timestamp! Using id instead!", ids_.at(i));
1608  }
1609  else
1610  {
1611  id = QString::number(stamp, 'f');
1612  }
1613  }
1614 
1615  if(!data.imageRaw().empty())
1616  {
1617  if(!data.rightRaw().empty())
1618  {
1619  if(!cv::imwrite(QString("%1/left/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw()))
1620  UWARN("Failed saving \"%s\"", QString("%1/left/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
1621  if(!cv::imwrite(QString("%1/right/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.rightRaw()))
1622  UWARN("Failed saving \"%s\"", QString("%1/right/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
1623  UINFO(QString("Saved left/%1.%2 and right/%1.%2").arg(id).arg(ext).toStdString().c_str());
1624 
1625  if(databaseFileName_.empty())
1626  {
1627  UERROR("Cannot save calibration file, database name is empty!");
1628  }
1629  else if(data.stereoCameraModels().size()>=1 && data.stereoCameraModels().front().isValidForProjection())
1630  {
1631  for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
1632  {
1633  std::string cameraName = id.toStdString();
1634  if(data.stereoCameraModels().size()>1)
1635  {
1636  cameraName+="_"+uNumber2Str((int)i);
1637  }
1638  StereoCameraModel model(
1639  cameraName,
1640  data.imageRaw().size(),
1641  data.stereoCameraModels()[i].left().K_raw(),
1642  data.stereoCameraModels()[i].left().D_raw(),
1643  data.stereoCameraModels()[i].left().R(),
1644  data.stereoCameraModels()[i].left().P(),
1645  data.rightRaw().size(),
1646  data.stereoCameraModels()[i].right().K_raw(),
1647  data.stereoCameraModels()[i].right().D_raw(),
1648  data.stereoCameraModels()[i].right().R(),
1649  data.stereoCameraModels()[i].right().P(),
1650  data.stereoCameraModels()[i].R(),
1651  data.stereoCameraModels()[i].T(),
1652  data.stereoCameraModels()[i].E(),
1653  data.stereoCameraModels()[i].F(),
1654  data.stereoCameraModels()[i].left().localTransform());
1655  if(model.save(path.toStdString() + "/calib"))
1656  {
1657  UINFO("Saved stereo calibration \"%s\"", (path.toStdString()+"/calib/"+cameraName+".yaml").c_str());
1658  }
1659  else
1660  {
1661  UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/calib/"+cameraName+".yaml").c_str());
1662  }
1663  }
1664  }
1665  }
1666  else
1667  {
1668  if(!data.depthRaw().empty())
1669  {
1670  if(!cv::imwrite(QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw()))
1671  UWARN("Failed saving \"%s\"", QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
1672  if(!cv::imwrite(QString("%1/depth/%2.png").arg(path).arg(id).toStdString(), data.depthRaw().type()==CV_32FC1?util2d::cvtDepthFromFloat(data.depthRaw()):data.depthRaw()))
1673  UWARN("Failed saving \"%s\"", QString("%1/depth/%2.png").arg(path).arg(id).toStdString().c_str());
1674  UINFO(QString("Saved rgb/%1.%2 and depth/%1.png").arg(id).arg(ext).toStdString().c_str());
1675  }
1676  else
1677  {
1678  if(!cv::imwrite(QString("%1/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw()))
1679  UWARN("Failed saving \"%s\"", QString("%1/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
1680  else
1681  UINFO(QString("Saved %1.%2").arg(id).arg(ext).toStdString().c_str());
1682  }
1683 
1684  if(databaseFileName_.empty())
1685  {
1686  UERROR("Cannot save calibration file, database name is empty!");
1687  }
1688  else if(data.cameraModels().size() >= 1 && data.cameraModels().front().isValidForProjection())
1689  {
1690  for(size_t i=0; i<data.cameraModels().size(); ++i)
1691  {
1692  std::string cameraName = id.toStdString();
1693  if(data.cameraModels().size()>1)
1694  {
1695  cameraName+="_"+uNumber2Str((int)i);
1696  }
1697  CameraModel model(cameraName,
1698  data.imageRaw().size(),
1699  data.cameraModels()[i].K_raw(),
1700  data.cameraModels()[i].D_raw(),
1701  data.cameraModels()[i].R(),
1702  data.cameraModels()[i].P(),
1703  data.cameraModels()[i].localTransform());
1704  std::string dirPrefix = "";
1705  if(!data.depthRaw().empty())
1706  {
1707  dirPrefix = "/calib";
1708  }
1709  if(model.save(path.toStdString()+dirPrefix))
1710  {
1711  UINFO("Saved calibration \"%s\"", (path.toStdString()+dirPrefix+"/"+cameraName+".yaml").c_str());
1712  }
1713  else
1714  {
1715  UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName+".yaml").c_str());
1716  }
1717  }
1718  }
1719  }
1720  ++imagesExported;
1721  }
1722  progressDialog->incrementStep();
1723  QApplication::processEvents();
1724  if(progressDialog->isCanceled())
1725  {
1726  progressDialog->appendText("Cancel!");
1727  break;
1728  }
1729  }
1730  progressDialog->appendText("Done!");
1731  progressDialog->setValue(progressDialog->maximumSteps());
1732 
1733  QMessageBox::information(this, tr("Exporting"), tr("%1 images exported!").arg(imagesExported));
1734  }
1735 }
1736 
1737 void DatabaseViewer::updateIds()
1738 {
1739  if(!dbDriver_)
1740  {
1741  return;
1742  }
1743 
1744  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
1745  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
1746  int progressSteps = 5;
1747  if(ui_->graphViewer->isVisible() || ui_->dockWidget_occupancyGridView->isVisible())
1748  {
1749  ++progressSteps;
1750  }
1751  if(ui_->textEdit_info->isVisible())
1752  {
1753  ++progressSteps;
1754  }
1755  if(ui_->toolBox_statistics->isVisible())
1756  {
1757  ++progressSteps;
1758  }
1759  progressDialog->setMaximumSteps(progressSteps);
1760  progressDialog->show();
1761  progressDialog->setCancelButtonVisible(false);
1762 
1763  progressDialog->appendText(tr("Loading all ids..."));
1764  QApplication::processEvents();
1765  uSleep(100);
1766  QApplication::processEvents();
1767 
1768  UINFO("Loading all IDs...");
1769  std::set<int> ids;
1770  dbDriver_->getAllNodeIds(ids);
1771 #if QT_VERSION >= QT_VERSION_CHECK(5, 15, 3)
1772  ids_ = QList<int>(ids.begin(), ids.end());
1773 #else
1774  ids_ = QList<int>::fromStdList(std::list<int>(ids.begin(), ids.end()));
1775 #endif
1776  lastWmIds_.clear();
1777  dbDriver_->getLastNodeIds(lastWmIds_);
1778  idToIndex_.clear();
1779  mapIds_.clear();
1780  weights_.clear();
1781  wmStates_.clear();
1782  odomPoses_.clear();
1783  groundTruthPoses_.clear();
1784  gpsPoses_.clear();
1785  gpsValues_.clear();
1786  lastSliderIndexBrowsed_ = 0;
1787  ui_->checkBox_wmState->setVisible(false);
1788  ui_->checkBox_alignPosesWithGPS->setVisible(false);
1789  ui_->checkBox_alignPosesWithGPS->setEnabled(false);
1790  ui_->checkBox_alignPosesWithGroundTruth->setVisible(false);
1791  ui_->checkBox_alignPosesWithGroundTruth->setEnabled(false);
1792  ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(false);
1793  ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(false);
1794  ui_->doubleSpinBox_optimizationScale->setVisible(false);
1795  ui_->label_scale_title->setVisible(false);
1796  ui_->label_rmse->setVisible(false);
1797  ui_->label_rmse_title->setVisible(false);
1798  ui_->checkBox_ignoreIntermediateNodes->setVisible(false);
1799  ui_->label_ignoreINtermediateNdoes->setVisible(false);
1800  ui_->label_alignPosesWithGPS->setVisible(false);
1801  ui_->label_alignPosesWithGroundTruth->setVisible(false);
1802  ui_->label_alignScansCloudsWithGroundTruth->setVisible(false);
1803  ui_->toolButton_edit_priorA->setVisible(false);
1804  ui_->toolButton_edit_priorB->setVisible(false);
1805  ui_->toolButton_remove_priorA->setVisible(false);
1806  ui_->toolButton_remove_priorB->setVisible(false);
1807  ui_->menuEdit->setEnabled(true);
1808  ui_->actionGenerate_3D_map_pcd->setEnabled(true);
1809  ui_->actionExport->setEnabled(true);
1810  ui_->actionExtract_images->setEnabled(true);
1811  ui_->menuExport_poses->setEnabled(false);
1812  ui_->menuExport_GPS->setEnabled(false);
1813  ui_->actionPoses_KML->setEnabled(false);
1814  ui_->actionEdit_optimized_2D_map->setEnabled(false);
1815  ui_->actionExport_saved_2D_map->setEnabled(false);
1816  ui_->actionImport_2D_map->setEnabled(false);
1817  ui_->actionRegenerate_optimized_2D_map->setEnabled(false);
1818  ui_->actionView_optimized_mesh->setEnabled(false);
1819  ui_->actionExport_optimized_mesh->setEnabled(false);
1820  ui_->actionUpdate_optimized_mesh->setEnabled(uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.13.0") >= 0);
1821  links_.clear();
1822  linksAdded_.clear();
1823  linksRefined_.clear();
1824  linksRemoved_.clear();
1825  ui_->toolBox_statistics->clear();
1826  ui_->label_optimizeFrom->setText(tr("Root"));
1827 
1828  progressDialog->appendText(tr("%1 ids loaded!").arg(ids.size()));
1829  progressDialog->incrementStep();
1830  progressDialog->appendText(tr("Loading all links..."));
1831  QApplication::processEvents();
1832  uSleep(100);
1833  QApplication::processEvents();
1834 
1835  std::multimap<int, Link> unilinks;
1836  dbDriver_->getAllLinks(unilinks, true, true);
1837  UDEBUG("%d total links loaded", (int)unilinks.size());
1838  // add both direction links
1839  std::multimap<int, Link> links;
1840  for(std::multimap<int, Link>::iterator iter=unilinks.begin(); iter!=unilinks.end(); ++iter)
1841  {
1842  links.insert(*iter);
1843  if(graph::findLink(unilinks, iter->second.to(), iter->second.from(), false) == unilinks.end())
1844  {
1845  links.insert(std::make_pair(iter->second.to(), iter->second.inverse()));
1846  }
1847  }
1848 
1849  progressDialog->appendText(tr("%1 links loaded!").arg(unilinks.size()));
1850  progressDialog->incrementStep();
1851  progressDialog->appendText("Loading Working Memory state...");
1852  QApplication::processEvents();
1853  uSleep(100);
1854  QApplication::processEvents();
1855 
1856  infoTotalOdom_ = 0.0;
1857  Transform previousPose;
1858  infoSessions_ = ids_.size()?1:0;
1859  infoTotalTime_ = 0.0;
1860  double previousStamp = 0.0;
1861  infoReducedGraph_ = false;
1862  std::map<int, std::vector<int> > wmStates = dbDriver_->getAllStatisticsWmStates();
1863 
1864  progressDialog->appendText("Loading Working Memory state... done!");
1865  progressDialog->incrementStep();
1866  progressDialog->appendText("Loading info for all nodes...");
1867  QApplication::processEvents();
1868  uSleep(100);
1869  QApplication::processEvents();
1870 
1871  int lastValidNodeId = 0;
1872  for(int i=0; i<ids_.size(); ++i)
1873  {
1874  idToIndex_.insert(ids_[i], i);
1875 
1876  Transform p, g;
1877  int w;
1878  std::string l;
1879  double s;
1880  int mapId;
1881  std::vector<float> v;
1882  GPS gps;
1883  EnvSensors sensors;
1884  dbDriver_->getNodeInfo(ids_[i], p, mapId, w, l, s, g, v, gps, sensors);
1885  mapIds_.insert(std::make_pair(ids_[i], mapId));
1886  weights_.insert(std::make_pair(ids_[i], w));
1887  if(w>=0)
1888  {
1889  for(std::multimap<int, Link>::iterator iter=links.find(ids_[i]); iter!=links.end() && iter->first==ids_[i]; ++iter)
1890  {
1891  // Make compatible with old databases, when "weight=-1" was not yet introduced to identify ignored nodes
1892  if(iter->second.type() == Link::kNeighbor || iter->second.type() == Link::kNeighborMerged)
1893  {
1894  lastValidNodeId = ids_[i];
1895  }
1896  }
1897  }
1898  if(wmStates.find(ids_[i]) != wmStates.end())
1899  {
1900  wmStates_.insert(std::make_pair(ids_[i], wmStates.at(ids_[i])));
1901  }
1902  if(w < 0)
1903  {
1904  ui_->checkBox_ignoreIntermediateNodes->setVisible(true);
1905  ui_->label_ignoreINtermediateNdoes->setVisible(true);
1906  }
1907  if(i>0)
1908  {
1909  if(mapIds_.at(ids_[i-1]) == mapId)
1910  {
1911  if(!p.isNull() && !previousPose.isNull())
1912  {
1913  infoTotalOdom_ += p.getDistance(previousPose);
1914  }
1915 
1916  if(previousStamp > 0.0 && s > 0.0)
1917  {
1918  infoTotalTime_ += s-previousStamp;
1919  }
1920  }
1921  else
1922  {
1923  ++infoSessions_;
1924  }
1925  }
1926  previousStamp=s;
1927  previousPose=p;
1928 
1929  //links
1930  for(std::multimap<int, Link>::iterator jter=links.find(ids_[i]); jter!=links.end() && jter->first == ids_[i]; ++jter)
1931  {
1932  if(jter->second.type() == Link::kNeighborMerged)
1933  {
1934  infoReducedGraph_ = true;
1935  }
1936 
1937  std::multimap<int, Link>::iterator invertedLinkIter = graph::findLink(links, jter->second.to(), jter->second.from(), false, jter->second.type());
1938  if( jter->second.isValid() && // null transform means a rehearsed location
1939  ids.find(jter->second.from()) != ids.end() &&
1940  (ids.find(jter->second.to()) != ids.end() || jter->second.to()<0) && // to add landmark links
1941  graph::findLink(links_, jter->second.from(), jter->second.to(), false, jter->second.type()) == links_.end() &&
1942  invertedLinkIter != links.end() &&
1943  w != -9)
1944  {
1945  // check if user_data is set in opposite direction
1946  if(jter->second.userDataCompressed().cols == 0 &&
1947  invertedLinkIter->second.userDataCompressed().cols != 0)
1948  {
1949  links_.insert(std::make_pair(invertedLinkIter->second.from(), invertedLinkIter->second));
1950  }
1951  else
1952  {
1953  links_.insert(std::make_pair(ids_[i], jter->second));
1954  }
1955  }
1956  }
1957  // Add pose
1958  odomPoses_.insert(std::make_pair(ids_[i], p));
1959  if(!g.isNull())
1960  {
1961  groundTruthPoses_.insert(std::make_pair(ids_[i], g));
1962  }
1963  if(gps.stamp() > 0.0)
1964  {
1965  gpsValues_.insert(std::make_pair(ids_[i], gps));
1966 
1967  cv::Point3f p(0.0f,0.0f,0.0f);
1968  if(!gpsPoses_.empty())
1969  {
1970  GeodeticCoords coords = gps.toGeodeticCoords();
1971  GPS originGPS = gpsValues_.begin()->second;
1972  p = coords.toENU_WGS84(originGPS.toGeodeticCoords());
1973  }
1974  Transform pose(p.x, p.y, p.z, 0.0f, 0.0f, (float)((-(gps.bearing()-90))*M_PI/180.0));
1975  gpsPoses_.insert(std::make_pair(ids_[i], pose));
1976  }
1977  }
1978 
1979  ui_->checkBox_wmState->setVisible(!wmStates_.empty() || !lastWmIds_.empty());
1980 
1981  progressDialog->appendText("Loading info for all nodes... done!");
1982  progressDialog->incrementStep();
1983  progressDialog->appendText("Loading optimized poses and maps...");
1984  QApplication::processEvents();
1985  uSleep(100);
1986  QApplication::processEvents();
1987 
1988  ui_->doubleSpinBox_optimizationScale->setVisible(!groundTruthPoses_.empty());
1989  ui_->label_scale_title->setVisible(!groundTruthPoses_.empty());
1990  ui_->label_rmse->setVisible(!groundTruthPoses_.empty());
1991  ui_->label_rmse_title->setVisible(!groundTruthPoses_.empty());
1992 
1993  ui_->checkBox_alignPosesWithGPS->setVisible(!gpsPoses_.empty());
1994  ui_->checkBox_alignPosesWithGPS->setEnabled(!gpsPoses_.empty());
1995  ui_->label_alignPosesWithGPS->setVisible(!gpsPoses_.empty());
1996  ui_->checkBox_alignPosesWithGroundTruth->setVisible(!groundTruthPoses_.empty());
1997  ui_->checkBox_alignPosesWithGroundTruth->setEnabled(!groundTruthPoses_.empty());
1998  ui_->label_alignPosesWithGroundTruth->setVisible(!groundTruthPoses_.empty());
1999  ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(!groundTruthPoses_.empty());
2000  ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(!groundTruthPoses_.empty());
2001  ui_->label_alignScansCloudsWithGroundTruth->setVisible(!groundTruthPoses_.empty());
2002 
2003  if(!gpsValues_.empty())
2004  {
2005  ui_->menuExport_GPS->setEnabled(true);
2006  ui_->actionPoses_KML->setEnabled(true);
2007  }
2008 
2009  float xMin, yMin, cellSize;
2010  bool hasMap = !dbDriver_->load2DMap(xMin, yMin, cellSize).empty();
2011  ui_->actionEdit_optimized_2D_map->setEnabled(hasMap);
2012  ui_->actionExport_saved_2D_map->setEnabled(hasMap);
2013  ui_->actionImport_2D_map->setEnabled(hasMap);
2014  ui_->actionRegenerate_optimized_2D_map->setEnabled(uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.17.0") >= 0);
2015 
2016  if(!dbDriver_->loadOptimizedMesh().empty())
2017  {
2018  ui_->actionView_optimized_mesh->setEnabled(true);
2019  ui_->actionExport_optimized_mesh->setEnabled(true);
2020  }
2021 
2022  UINFO("Loaded %d ids, %d poses and %d links", (int)ids_.size(), (int)odomPoses_.size(), (int)links_.size());
2023 
2024  progressDialog->appendText("Loading optimized poses and maps... done!");
2025  progressDialog->incrementStep();
2026  QApplication::processEvents();
2027  uSleep(100);
2028  QApplication::processEvents();
2029 
2030  if(ids_.size() && ui_->toolBox_statistics->isVisible())
2031  {
2032  progressDialog->appendText("Loading statistics...");
2033  QApplication::processEvents();
2034  uSleep(100);
2035  QApplication::processEvents();
2036 
2037  UINFO("Update statistics...");
2038  updateStatistics();
2039 
2040  progressDialog->appendText("Loading statistics... done!");
2041  progressDialog->incrementStep();
2042  QApplication::processEvents();
2043  uSleep(100);
2044  QApplication::processEvents();
2045  }
2046 
2047 
2048  ui_->textEdit_info->clear();
2049  if(ui_->textEdit_info->isVisible())
2050  {
2051  progressDialog->appendText("Update database info...");
2052  QApplication::processEvents();
2053  uSleep(100);
2054  QApplication::processEvents();
2055 
2056  updateInfo();
2057 
2058  progressDialog->appendText("Update database info... done!");
2059  progressDialog->incrementStep();
2060  QApplication::processEvents();
2061  uSleep(100);
2062  QApplication::processEvents();
2063  }
2064 
2065  if(ids.size())
2066  {
2067  if(odomPoses_.size())
2068  {
2069  bool nullPoses = odomPoses_.begin()->second.isNull();
2070  for(std::map<int,Transform>::iterator iter=odomPoses_.begin(); iter!=odomPoses_.end(); ++iter)
2071  {
2072  if((!iter->second.isNull() && nullPoses) ||
2073  (iter->second.isNull() && !nullPoses))
2074  {
2075  if(iter->second.isNull())
2076  {
2077  UWARN("Pose %d is null!", iter->first);
2078  }
2079  UWARN("Mixed valid and null poses! Ignoring graph...");
2080  odomPoses_.clear();
2081  links_.clear();
2082  break;
2083  }
2084  }
2085  if(nullPoses)
2086  {
2087  odomPoses_.clear();
2088  links_.clear();
2089  }
2090 
2091  if(odomPoses_.size())
2092  {
2093  ui_->spinBox_optimizationsFrom->setRange(odomPoses_.begin()->first, odomPoses_.rbegin()->first);
2094  ui_->spinBox_optimizationsFrom->setValue(odomPoses_.begin()->first);
2095  ui_->label_optimizeFrom->setText(tr("Root [%1, %2]").arg(odomPoses_.begin()->first).arg(odomPoses_.rbegin()->first));
2096  }
2097  }
2098 
2099  if(lastValidNodeId>0)
2100  {
2101  // find full connected graph from last node in working memory
2102  Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
2103 
2104  std::map<int, rtabmap::Transform> posesOut;
2105  std::multimap<int, rtabmap::Link> linksOut;
2106  UINFO("Get connected graph from %d (%d poses, %d links)", lastValidNodeId, (int)odomPoses_.size(), (int)links_.size());
2107  optimizer->getConnectedGraph(
2108  lastValidNodeId,
2109  odomPoses_,
2110  links_,
2111  posesOut,
2112  linksOut);
2113 
2114  if(!posesOut.empty())
2115  {
2116  bool optimizeFromGraphEnd = Parameters::defaultRGBDOptimizeFromGraphEnd();
2117  Parameters::parse(dbDriver_->getLastParameters(), Parameters::kRGBDOptimizeFromGraphEnd(), optimizeFromGraphEnd);
2118  if(optimizeFromGraphEnd)
2119  {
2120  ui_->spinBox_optimizationsFrom->setValue(posesOut.rbegin()->first);
2121  }
2122  else
2123  {
2124  ui_->spinBox_optimizationsFrom->setValue(posesOut.lower_bound(1)->first);
2125  }
2126  }
2127 
2128  delete optimizer;
2129  }
2130  }
2131 
2132  ui_->menuExport_poses->setEnabled(!odomPoses_.empty());
2133  graphes_.clear();
2134  graphLinks_.clear();
2135 
2136  if(ids_.size())
2137  {
2138  ui_->spinBox_indexA->setMinimum(0);
2139  ui_->spinBox_indexB->setMinimum(0);
2140  ui_->spinBox_indexA->setMaximum(ids_.size()-1);
2141  ui_->spinBox_indexB->setMaximum(ids_.size()-1);
2142  ui_->spinBox_indexA->setEnabled(true);
2143  ui_->spinBox_indexB->setEnabled(true);
2144 
2145  ui_->horizontalSlider_A->setMinimum(0);
2146  ui_->horizontalSlider_B->setMinimum(0);
2147  ui_->horizontalSlider_A->setMaximum(ids_.size()-1);
2148  ui_->horizontalSlider_B->setMaximum(ids_.size()-1);
2149  ui_->horizontalSlider_A->setEnabled(true);
2150  ui_->horizontalSlider_B->setEnabled(true);
2151  ui_->horizontalSlider_A->setSliderPosition(0);
2152  ui_->horizontalSlider_B->setSliderPosition(0);
2153  sliderAValueChanged(0);
2154  sliderBValueChanged(0);
2155  }
2156  else
2157  {
2158  ui_->horizontalSlider_A->setEnabled(false);
2159  ui_->horizontalSlider_B->setEnabled(false);
2160 
2161  ui_->spinBox_indexA->setEnabled(false);
2162  ui_->spinBox_indexB->setEnabled(false);
2163 
2164  ui_->label_idA->setText("NaN");
2165  ui_->label_idB->setText("NaN");
2166  }
2167 
2168  if(ids_.size())
2169  {
2170  updateNeighborsSlider();
2171  updateLoopClosuresSlider();
2172  if(ui_->graphViewer->isVisible() || ui_->dockWidget_occupancyGridView->isVisible())
2173  {
2174  progressDialog->appendText("Updating Graph View...");
2175  QApplication::processEvents();
2176  uSleep(100);
2177  QApplication::processEvents();
2178 
2179  updateGraphView();
2180 
2181  progressDialog->appendText("Updating Graph View... done!");
2182  progressDialog->incrementStep();
2183  QApplication::processEvents();
2184  uSleep(100);
2185  QApplication::processEvents();
2186  }
2187  }
2188  progressDialog->setValue(progressDialog->maximumSteps());
2189 }
2190 
2191 void DatabaseViewer::updateInfo()
2192 {
2193  UINFO("Update database info...");
2194  if(dbDriver_)
2195  {
2196  if(ui_->textEdit_info->toPlainText().isEmpty())
2197  {
2198  ui_->textEdit_info->append(tr("Path:\t\t%1").arg(dbDriver_->getUrl().c_str()));
2199  ui_->textEdit_info->append(tr("Version:\t\t%1").arg(dbDriver_->getDatabaseVersion().c_str()));
2200  ui_->textEdit_info->append(tr("Sessions:\t\t%1").arg(infoSessions_));
2201  if(infoReducedGraph_)
2202  {
2203  ui_->textEdit_info->append(tr("Total odometry length:\t%1 m (approx. as graph has been reduced)").arg(infoTotalOdom_));
2204  }
2205  else
2206  {
2207  ui_->textEdit_info->append(tr("Total odometry length:\t%1 m").arg(infoTotalOdom_));
2208  }
2209 
2210  int lastWordIdId = 0;
2211  int wordsDim = 0;
2212  int wordsType = 0;
2213  dbDriver_->getLastWordId(lastWordIdId);
2214  if(lastWordIdId>0)
2215  {
2216  std::set<int> ids;
2217  ids.insert(lastWordIdId);
2218  std::list<VisualWord *> vws;
2219  dbDriver_->loadWords(ids, vws);
2220  if(!vws.empty())
2221  {
2222  wordsDim = vws.front()->getDescriptor().cols;
2223  wordsType = vws.front()->getDescriptor().type();
2224  delete vws.front();
2225  vws.clear();
2226  }
2227  }
2228 
2229  ui_->textEdit_info->append(tr("Total time:\t\t%1").arg(QDateTime::fromMSecsSinceEpoch(infoTotalTime_*1000).toUTC().toString("hh:mm:ss.zzz")));
2230  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()));
2231  ui_->textEdit_info->append(tr("WM:\t\t%1 nodes and %2 words").arg(dbDriver_->getLastNodesSize()).arg(dbDriver_->getLastDictionarySize()));
2232  ui_->textEdit_info->append(tr("Global graph:\t%1 poses and %2 links").arg(odomPoses_.size()).arg(links_.size()));
2233  ui_->textEdit_info->append(tr("Ground truth:\t%1 poses").arg(groundTruthPoses_.size()));
2234  ui_->textEdit_info->append(tr("GPS:\t%1 poses").arg(gpsValues_.size()));
2235  ui_->textEdit_info->append("");
2236  long total = 0;
2237  long dbSize = UFile::length(dbDriver_->getUrl());
2238  long mem = dbSize;
2239  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"));
2240  mem = dbDriver_->getNodesMemoryUsed();
2241  total+=mem;
2242  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"));
2243  mem = dbDriver_->getLinksMemoryUsed();
2244  total+=mem;
2245  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"));
2246  mem = dbDriver_->getImagesMemoryUsed();
2247  total+=mem;
2248  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"));
2249  mem = dbDriver_->getDepthImagesMemoryUsed();
2250  total+=mem;
2251  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"));
2252  mem = dbDriver_->getCalibrationsMemoryUsed();
2253  total+=mem;
2254  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"));
2255  mem = dbDriver_->getGridsMemoryUsed();
2256  total+=mem;
2257  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"));
2258  mem = dbDriver_->getLaserScansMemoryUsed();
2259  total+=mem;
2260  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"));
2261  mem = dbDriver_->getUserDataMemoryUsed();
2262  total+=mem;
2263  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"));
2264  mem = dbDriver_->getWordsMemoryUsed();
2265  total+=mem;
2266  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"));
2267  mem = dbDriver_->getFeaturesMemoryUsed();
2268  total+=mem;
2269  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"));
2270  mem = dbDriver_->getStatisticsMemoryUsed();
2271  total+=mem;
2272  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"));
2273  mem = dbSize - total;
2274  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"));
2275  ui_->textEdit_info->append("");
2276  std::set<int> idsWithoutBad;
2277  dbDriver_->getAllNodeIds(idsWithoutBad, false, true);
2278  int infoBadcountInLTM = 0;
2279  int infoBadCountInGraph = 0;
2280  for(int i=0; i<ids_.size(); ++i)
2281  {
2282  if(idsWithoutBad.find(ids_[i]) == idsWithoutBad.end())
2283  {
2284  ++infoBadcountInLTM;
2285  if(odomPoses_.find(ids_[i]) != odomPoses_.end())
2286  {
2287  ++infoBadCountInGraph;
2288  }
2289  }
2290  }
2291  ui_->textEdit_info->append(tr("%1 bad signatures in LTM").arg(infoBadcountInLTM));
2292  ui_->textEdit_info->append(tr("%1 bad signatures in the global graph").arg(infoBadCountInGraph));
2293  ui_->textEdit_info->append("");
2294  ParametersMap parameters = dbDriver_->getLastParameters();
2295  QFontMetrics metrics(ui_->textEdit_info->font());
2296 #if QT_VERSION >= QT_VERSION_CHECK(5, 15, 0)
2297  int tabW = ui_->textEdit_info->tabStopDistance();
2298 #else
2299  int tabW = ui_->textEdit_info->tabStopWidth();
2300 #endif
2301  for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
2302  {
2303 #if QT_VERSION >= QT_VERSION_CHECK(5, 15, 0)
2304  int strW = metrics.horizontalAdvance(QString(iter->first.c_str()) + "=");
2305 #else
2306  int strW = metrics.width(QString(iter->first.c_str()) + "=");
2307 #endif
2308  ui_->textEdit_info->append(tr("%1=%2%3")
2309  .arg(iter->first.c_str())
2310  .arg(strW < tabW?"\t\t\t\t":strW < tabW*2?"\t\t\t":strW < tabW*3?"\t\t":"\t")
2311  .arg(iter->second.c_str()));
2312  }
2313 
2314  // move back the cursor at the beginning
2315  ui_->textEdit_info->moveCursor(QTextCursor::Start) ;
2316  ui_->textEdit_info->ensureCursorVisible() ;
2317  }
2318  }
2319  else
2320  {
2321  ui_->textEdit_info->clear();
2322  }
2323 }
2324 
2325 void DatabaseViewer::updateStatistics()
2326 {
2327  UDEBUG("");
2328  if(dbDriver_)
2329  {
2330  ui_->toolBox_statistics->clear();
2331  double firstStamp = 0.0;
2332  std::map<int, std::pair<std::map<std::string, float>, double> > allStats = dbDriver_->getAllStatistics();
2333 
2334  std::map<std::string, std::pair<std::vector<qreal>, std::vector<qreal> > > allData;
2335  std::map<std::string, int > allDataOi;
2336 
2337  for(std::map<int, std::pair<std::map<std::string, float>, double> >::iterator jter=allStats.begin(); jter!=allStats.end(); ++jter)
2338  {
2339  double stamp=jter->second.second;
2340  std::map<std::string, float> & statistics = jter->second.first;
2341  if(firstStamp==0.0)
2342  {
2343  firstStamp = stamp;
2344  }
2345  for(std::map<std::string, float>::iterator iter=statistics.begin(); iter!=statistics.end(); ++iter)
2346  {
2347  if(allData.find(iter->first) == allData.end())
2348  {
2349  //initialize data vectors
2350  allData.insert(std::make_pair(iter->first, std::make_pair(std::vector<qreal>(allStats.size(), 0.0f), std::vector<qreal>(allStats.size(), 0.0f) )));
2351  allDataOi.insert(std::make_pair(iter->first, 0));
2352  }
2353 
2354  int & oi = allDataOi.at(iter->first);
2355  allData.at(iter->first).first[oi] = ui_->checkBox_timeStats->isChecked()?qreal(stamp-firstStamp):jter->first;
2356  allData.at(iter->first).second[oi] = iter->second;
2357  ++oi;
2358  }
2359  }
2360 
2361  for(std::map<std::string, std::pair<std::vector<qreal>, std::vector<qreal> > >::iterator iter=allData.begin(); iter!=allData.end(); ++iter)
2362  {
2363  int oi = allDataOi.at(iter->first);
2364  iter->second.first.resize(oi);
2365  iter->second.second.resize(oi);
2366  ui_->toolBox_statistics->updateStat(iter->first.c_str(), iter->second.first, iter->second.second, true);
2367  }
2368  }
2369  UDEBUG("");
2370 }
2371 
2372 void DatabaseViewer::selectObstacleColor()
2373 {
2374  QColor c = QColorDialog::getColor(ui_->lineEdit_obstacleColor->text(), this);
2375  if(c.isValid())
2376  {
2377  ui_->lineEdit_obstacleColor->setText(c.name());
2378  }
2379 }
2380 void DatabaseViewer::selectGroundColor()
2381 {
2382  QColor c = QColorDialog::getColor(ui_->lineEdit_groundColor->text(), this);
2383  if(c.isValid())
2384  {
2385  ui_->lineEdit_groundColor->setText(c.name());
2386  }
2387 }
2388 void DatabaseViewer::selectEmptyColor()
2389 {
2390  QColor c = QColorDialog::getColor(ui_->lineEdit_emptyColor->text(), this);
2391  if(c.isValid())
2392  {
2393  ui_->lineEdit_emptyColor->setText(c.name());
2394  }
2395 }
2396 void DatabaseViewer::selectFrontierColor()
2397 {
2398  QColor c = QColorDialog::getColor(ui_->lineEdit_frontierColor->text(), this);
2399  if(c.isValid())
2400  {
2401  ui_->lineEdit_frontierColor->setText(c.name());
2402  }
2403 }
2404 void DatabaseViewer::editDepthImage()
2405 {
2406  if(dbDriver_ && ids_.size())
2407  {
2408  if(lastSliderIndexBrowsed_>= ids_.size())
2409  {
2410  lastSliderIndexBrowsed_ = ui_->horizontalSlider_A->value();
2411  }
2412  int id = ids_.at(lastSliderIndexBrowsed_);
2413  SensorData data;
2414  dbDriver_->getNodeData(id, data, true, false, false, false);
2415  data.uncompressData();
2416  if(!data.depthRaw().empty())
2417  {
2418  editDepthArea_->setColorMap(lastSliderIndexBrowsed_ == ui_->horizontalSlider_B->value()?ui_->graphicsView_B->getDepthColorMap():ui_->graphicsView_A->getDepthColorMap());
2419  editDepthArea_->setImage(data.depthRaw(), data.imageRaw());
2420  if(editDepthDialog_->exec() == QDialog::Accepted && editDepthArea_->isModified())
2421  {
2422  cv::Mat depth = editDepthArea_->getModifiedImage();
2423  UASSERT(data.depthRaw().type() == depth.type());
2424  UASSERT(data.depthRaw().cols == depth.cols);
2425  UASSERT(data.depthRaw().rows == depth.rows);
2426  std::string depthFormat = compressedDepthFormat(data.depthOrRightCompressed());
2427  dbDriver_->updateDepthImage(id, depth, depthFormat);
2428  this->update3dView();
2429  }
2430  }
2431  }
2432 }
2433 
2434 void DatabaseViewer::exportPosesRaw()
2435 {
2436  exportPoses(0);
2437 }
2438 void DatabaseViewer::exportPosesRGBDSLAMMotionCapture()
2439 {
2440  exportPoses(1);
2441 }
2442 void DatabaseViewer::exportPosesRGBDSLAM()
2443 {
2444  exportPoses(10);
2445 }
2446 void DatabaseViewer::exportPosesRGBDSLAMID()
2447 {
2448  exportPoses(11);
2449 }
2450 void DatabaseViewer::exportPosesKITTI()
2451 {
2452  exportPoses(2);
2453 }
2454 void DatabaseViewer::exportPosesTORO()
2455 {
2456  exportPoses(3);
2457 }
2458 void DatabaseViewer::exportPosesG2O()
2459 {
2460  exportPoses(4);
2461 }
2462 void DatabaseViewer::exportPosesKML()
2463 {
2464  exportPoses(5);
2465 }
2466 
2467 void DatabaseViewer::exportPoses(int format)
2468 {
2469  QStringList types;
2470  types.push_back("Map's graph (see Graph View)");
2471  types.push_back("Odometry");
2472  if(!groundTruthPoses_.empty())
2473  {
2474  types.push_back("Ground Truth");
2475  }
2476  bool ok;
2477  QString type = QInputDialog::getItem(this, tr("Which poses?"), tr("Poses:"), types, 0, false, &ok);
2478  if(!ok)
2479  {
2480  return;
2481  }
2482  bool odometry = type.compare("Odometry") == 0;
2483  bool groundTruth = type.compare("Ground Truth") == 0;
2484 
2485  if(groundTruth && groundTruthPoses_.empty())
2486  {
2487  QMessageBox::warning(this, tr("Cannot export poses"), tr("No ground truth poses in database?!"));
2488  return;
2489  }
2490  else if(!odometry && graphes_.empty())
2491  {
2492  this->updateGraphView();
2493  if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
2494  {
2495  QMessageBox::warning(this, tr("Cannot export poses"), tr("No graph in database?!"));
2496  return;
2497  }
2498  }
2499  else if(odometry && odomPoses_.empty())
2500  {
2501  QMessageBox::warning(this, tr("Cannot export poses"), tr("No odometry poses in database?!"));
2502  return;
2503  }
2504 
2505  if(format == 5 && (gpsValues_.empty() || gpsPoses_.empty()))
2506  {
2507  QMessageBox::warning(this, tr("Cannot export poses in KML format"), tr("No GPS in database?!"));
2508  return;
2509  }
2510 
2511  std::map<int, Transform> optimizedPoses;
2512  if(groundTruth)
2513  {
2514  optimizedPoses = groundTruthPoses_;
2515  }
2516  else if(odometry)
2517  {
2518  optimizedPoses = odomPoses_;
2519  }
2520  else
2521  {
2522  optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
2523  }
2524 
2525  bool alignToGPS =
2526  (ui_->checkBox_alignPosesWithGPS->isEnabled() &&
2527  ui_->checkBox_alignPosesWithGPS->isChecked()) ||
2528  format == 5;
2529 
2530  if(alignToGPS ||
2531  (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked()))
2532  {
2533  std::map<int, Transform> refPoses = groundTruthPoses_;
2534  if(alignToGPS)
2535  {
2536  refPoses = gpsPoses_;
2537  }
2538 
2539  // Log ground truth statistics (in TUM's RGBD-SLAM format)
2540  if(refPoses.size())
2541  {
2542  float translational_rmse = 0.0f;
2543  float translational_mean = 0.0f;
2544  float translational_median = 0.0f;
2545  float translational_std = 0.0f;
2546  float translational_min = 0.0f;
2547  float translational_max = 0.0f;
2548  float rotational_rmse = 0.0f;
2549  float rotational_mean = 0.0f;
2550  float rotational_median = 0.0f;
2551  float rotational_std = 0.0f;
2552  float rotational_min = 0.0f;
2553  float rotational_max = 0.0f;
2554 
2555  Transform gtToMap = graph::calcRMSE(
2556  refPoses,
2557  optimizedPoses,
2558  translational_rmse,
2559  translational_mean,
2560  translational_median,
2561  translational_std,
2562  translational_min,
2563  translational_max,
2564  rotational_rmse,
2565  rotational_mean,
2566  rotational_median,
2567  rotational_std,
2568  rotational_min,
2569  rotational_max,
2570  alignToGPS);
2571 
2572  if(!gtToMap.isIdentity())
2573  {
2574  for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
2575  {
2576  iter->second = gtToMap * iter->second;
2577  }
2578  if(alignToGPS && format != 5 && optimizedPoses.find(gpsValues_.begin()->first)!=optimizedPoses.end())
2579  {
2580  // This will make the exported first pose the GPS origin. Don't do it for KML format as is it done implicitly below.
2581  int originId = gpsValues_.begin()->first;
2582  Transform offset = optimizedPoses.at(originId).translation().inverse();
2583  for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
2584  {
2585  iter->second = offset * iter->second;
2586  }
2587  }
2588  }
2589  }
2590  }
2591 
2592  if(format == 5)
2593  {
2594  std::map<int, GPS> values;
2595  GeodeticCoords origin = gpsValues_.begin()->second.toGeodeticCoords();
2596  for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
2597  {
2598  GeodeticCoords coord;
2599  coord.fromENU_WGS84(cv::Point3d(iter->second.x(), iter->second.y(), iter->second.z()), origin);
2600 
2601  Transform p, g;
2602  int w;
2603  std::string l;
2604  double stamp=0.0;
2605  int mapId;
2606  std::vector<float> v;
2607  GPS gps;
2608  EnvSensors sensors;
2609  dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors);
2610  values.insert(std::make_pair(iter->first, GPS(stamp, coord.longitude(), coord.latitude(), coord.altitude(), 0, 0)));
2611  }
2612 
2613  QString output = pathDatabase_ + QDir::separator() + "poses.kml";
2614  QString path = QFileDialog::getSaveFileName(
2615  this,
2616  tr("Save File"),
2617  output,
2618  tr("Google Earth file (*.kml)"));
2619 
2620  if(!path.isEmpty())
2621  {
2622  bool saved = graph::exportGPS(path.toStdString(), values, ui_->graphViewer->getNodeColor().rgba());
2623 
2624  if(saved)
2625  {
2626  QMessageBox::information(this,
2627  tr("Export poses..."),
2628  tr("GPS coordinates saved to \"%1\".")
2629  .arg(path));
2630  }
2631  else
2632  {
2633  QMessageBox::information(this,
2634  tr("Export poses..."),
2635  tr("Failed to save GPS coordinates to \"%1\"!")
2636  .arg(path));
2637  }
2638  }
2639  return;
2640  }
2641 
2642  if(optimizedPoses.size())
2643  {
2644  std::map<int, Transform> localTransforms;
2645  QStringList items;
2646  items.push_back("Robot (base frame)");
2647  items.push_back("Camera");
2648  items.push_back("Scan");
2649  bool ok;
2650  QString item = QInputDialog::getItem(this, tr("Export Poses"), tr("Frame: "), items, 0, false, &ok);
2651  if(!ok || item.isEmpty())
2652  {
2653  return;
2654  }
2655  if(item.compare("Robot (base frame)") != 0)
2656  {
2657  bool cameraFrame = item.compare("Camera") == 0;
2658  for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
2659  {
2660  Transform localTransform;
2661  if(cameraFrame)
2662  {
2663  std::vector<CameraModel> models;
2664  std::vector<StereoCameraModel> stereoModels;
2665  if(dbDriver_->getCalibration(iter->first, models, stereoModels))
2666  {
2667  if((models.size() == 1 &&
2668  !models.at(0).localTransform().isNull()))
2669  {
2670  localTransform = models.at(0).localTransform();
2671  }
2672  else if(stereoModels.size() == 1 &&
2673  !stereoModels[0].localTransform().isNull())
2674  {
2675  localTransform = stereoModels[0].localTransform();
2676  }
2677  else if(models.size()>1)
2678  {
2679  UWARN("Multi-camera is not supported (node %d)", iter->first);
2680  }
2681  else
2682  {
2683  UWARN("Calibration not valid for node %d", iter->first);
2684  }
2685  }
2686  else
2687  {
2688  UWARN("Missing calibration for node %d", iter->first);
2689  }
2690  }
2691  else
2692  {
2693  LaserScan info;
2694  if(dbDriver_->getLaserScanInfo(iter->first, info))
2695  {
2696  localTransform = info.localTransform();
2697  }
2698  else
2699  {
2700  UWARN("Missing scan info for node %d", iter->first);
2701  }
2702 
2703  }
2704  if(!localTransform.isNull())
2705  {
2706  localTransforms.insert(std::make_pair(iter->first, localTransform));
2707  }
2708  }
2709  if(localTransforms.empty())
2710  {
2711  QMessageBox::warning(this,
2712  tr("Export Poses"),
2713  tr("Could not find any \"%1\" frame, exporting in Robot frame instead.").arg(item));
2714  }
2715  }
2716 
2717  std::map<int, Transform> poses;
2718  std::multimap<int, Link> links;
2719  if(localTransforms.empty())
2720  {
2721  poses = optimizedPoses;
2722  if(!odometry)
2723  {
2724  links = graphLinks_;
2725  }
2726  }
2727  else
2728  {
2729  //adjust poses and links
2730  for(std::map<int, Transform>::iterator iter=localTransforms.begin(); iter!=localTransforms.end(); ++iter)
2731  {
2732  poses.insert(std::make_pair(iter->first, optimizedPoses.at(iter->first) * iter->second));
2733  }
2734  if(!odometry)
2735  {
2736  for(std::multimap<int, Link>::iterator iter=graphLinks_.begin(); iter!=graphLinks_.end(); ++iter)
2737  {
2738  if(uContains(poses, iter->second.from()) && uContains(poses, iter->second.to()))
2739  {
2740  std::multimap<int, Link>::iterator inserted = links.insert(*iter);
2741  int from = iter->second.from();
2742  int to = iter->second.to();
2743  inserted->second.setTransform(localTransforms.at(from).inverse()*iter->second.transform()*localTransforms.at(to));
2744  }
2745  }
2746  }
2747  }
2748 
2749  if(format != 4 && format != 11 && !poses.empty() && poses.begin()->first<0) // not g2o, landmark not supported
2750  {
2751  UWARN("Only g2o format (4) and RGBD format with ID format can export landmarks, they are ignored with format %d", format);
2752  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end() && iter->first < 0;)
2753  {
2754  poses.erase(iter++);
2755  }
2756  for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end();)
2757  {
2758  if(iter->second.from() < 0 || iter->second.to() < 0)
2759  {
2760  links.erase(iter++);
2761  }
2762  else
2763  {
2764  ++iter;
2765  }
2766  }
2767  }
2768 
2769  std::map<int, double> stamps;
2770  if(format == 1 || format == 10 || format == 11)
2771  {
2772  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
2773  {
2774  if(iter->first<0 && format == 11) // in case of landmarks
2775  {
2776  stamps.insert(std::make_pair(iter->first, 0));
2777  }
2778  else
2779  {
2780  Transform p, g;
2781  int w;
2782  std::string l;
2783  double stamp=0.0;
2784  int mapId;
2785  std::vector<float> v;
2786  GPS gps;
2787  EnvSensors sensors;
2788  if(dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors))
2789  {
2790  stamps.insert(std::make_pair(iter->first, stamp));
2791  }
2792  }
2793  }
2794  if(stamps.size()!=poses.size())
2795  {
2796  QMessageBox::warning(this, tr("Export poses..."), tr("Poses (%1) and stamps (%2) have not the same size! Cannot export in RGB-D SLAM format.")
2797  .arg(poses.size()).arg(stamps.size()));
2798  return;
2799  }
2800  }
2801 
2802  QString output = pathDatabase_ + QDir::separator() + (format==3?"toro%1.graph":format==4?"poses%1.g2o":"poses%1.txt");
2803  QString suffix = odometry?"_odom":groundTruth?"_gt":"";
2804  output = output.arg(suffix);
2805 
2806  QString path = QFileDialog::getSaveFileName(
2807  this,
2808  tr("Save File"),
2809  output,
2810  format == 3?tr("TORO file (*.graph)"):format==4?tr("g2o file (*.g2o)"):tr("Text file (*.txt)"));
2811 
2812  if(!path.isEmpty())
2813  {
2814  if(QFileInfo(path).suffix() == "")
2815  {
2816  if(format == 3)
2817  {
2818  path += ".graph";
2819  }
2820  else if(format==4)
2821  {
2822  path += ".g2o";
2823  }
2824  else
2825  {
2826  path += ".txt";
2827  }
2828  }
2829 
2830  bool saved = graph::exportPoses(path.toStdString(), format, poses, links, stamps, ui_->parameters_toolbox->getParameters());
2831 
2832  if(saved)
2833  {
2834  QMessageBox::information(this,
2835  tr("Export poses..."),
2836  tr("%1 saved to \"%2\".")
2837  .arg(format == 3?"TORO graph":format == 4?"g2o graph":"Poses")
2838  .arg(path));
2839  }
2840  else
2841  {
2842  QMessageBox::information(this,
2843  tr("Export poses..."),
2844  tr("Failed to save %1 to \"%2\"!")
2845  .arg(format == 3?"TORO graph":format == 4?"g2o graph":"poses")
2846  .arg(path));
2847  }
2848  }
2849  }
2850 }
2851 
2852 void DatabaseViewer::exportGPS_TXT()
2853 {
2854  exportGPS(0);
2855 }
2856 void DatabaseViewer::exportGPS_KML()
2857 {
2858  exportGPS(1);
2859 }
2860 
2861 void DatabaseViewer::exportGPS(int format)
2862 {
2863  if(!gpsValues_.empty())
2864  {
2865  QString output = pathDatabase_ + QDir::separator() + (format==0?"gps.txt":"gps.kml");
2866  QString path = QFileDialog::getSaveFileName(
2867  this,
2868  tr("Save File"),
2869  output,
2870  format==0?tr("Raw format (*.txt)"):tr("Google Earth file (*.kml)"));
2871 
2872  if(!path.isEmpty())
2873  {
2874  bool saved = graph::exportGPS(path.toStdString(), gpsValues_, ui_->graphViewer->getGPSColor().rgba());
2875 
2876  if(saved)
2877  {
2878  QMessageBox::information(this,
2879  tr("Export poses..."),
2880  tr("GPS coordinates saved to \"%1\".")
2881  .arg(path));
2882  }
2883  else
2884  {
2885  QMessageBox::information(this,
2886  tr("Export poses..."),
2887  tr("Failed to save GPS coordinates to \"%1\"!")
2888  .arg(path));
2889  }
2890  }
2891  }
2892 }
2893 
2894 void DatabaseViewer::editSaved2DMap()
2895 {
2896  if(!dbDriver_)
2897  {
2898  QMessageBox::warning(this, tr("Cannot edit 2D map"), tr("A database must must loaded first...\nUse File->Open database."));
2899  return;
2900  }
2901 
2902  if(uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.17.0") < 0)
2903  {
2904  QMessageBox::warning(this, tr("Cannot edit 2D map"),
2905  tr("The database has too old version (%1) to saved "
2906  "optimized map. Version 0.17.0 minimum required.").arg(dbDriver_->getDatabaseVersion().c_str()));
2907  return;
2908  }
2909 
2910  if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size() || generatedLocalMaps_.size())
2911  {
2912  QMessageBox::warning(this, tr("Cannot edit 2D map"),
2913  tr("The database has modified links and/or modified local "
2914  "occupancy grids, the 2D optimized map cannot be modified."));
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  return;
2924  }
2925 
2926  cv::Mat map8U = rtabmap::util3d::convertMap2Image8U(map, false);
2927  cv::Mat map8UFlip, map8URotated;
2928  cv::flip(map8U, map8UFlip, 0);
2929  if(!ui_->graphViewer->isOrientationENU())
2930  {
2931  //ROTATE_90_COUNTERCLOCKWISE
2932  cv::transpose(map8UFlip, map8URotated);
2933  cv::flip(map8URotated, map8URotated, 0);
2934  }
2935  else
2936  {
2937  map8URotated = map8UFlip;
2938  }
2939 
2940  editMapArea_->setMap(map8URotated);
2941 
2942  if(editMapDialog_->exec() == QDialog::Accepted)
2943  {
2944  cv::Mat mapModified = editMapArea_->getModifiedMap();
2945 
2946  if(!ui_->graphViewer->isOrientationENU())
2947  {
2948  //ROTATE_90_CLOCKWISE
2949  cv::transpose(mapModified, map8URotated);
2950  cv::flip(map8URotated, map8URotated, 1);
2951  }
2952  else
2953  {
2954  map8URotated = mapModified;
2955  }
2956  cv::flip(map8URotated, map8UFlip, 0);
2957 
2958  UASSERT(map8UFlip.type() == map8U.type());
2959  UASSERT(map8UFlip.cols == map8U.cols);
2960  UASSERT(map8UFlip.rows == map8U.rows);
2961 
2962  cv::Mat map8S = rtabmap::util3d::convertImage8U2Map(map8UFlip, false);
2963 
2964  if(editMapArea_->isModified())
2965  {
2966  dbDriver_->save2DMap(map8S, xMin, yMin, cellSize);
2967  QMessageBox::information(this, tr("Edit 2D map"), tr("Map updated!"));
2968  }
2969 
2970  int cropRadius = ui_->spinBox_cropRadius->value();
2971  QMessageBox::StandardButton b = QMessageBox::question(this,
2972  tr("Crop empty space"),
2973  tr("Do you want to clear empty space from local occupancy grids and laser scans?\n\n"
2974  "Advantages:\n"
2975  " * 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"
2976  " * 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"
2977  "Disadvantage:\n"
2978  " * Cropping the laser scans cannot be reverted after the viewer is closed and changes have been saved.\n\n"
2979  "Parameter(s):\n"
2980  " Crop radius = %1 pixels\n\n"
2981  "Press \"Yes\" to filter only grids.\n"
2982  "Press \"Yes to All\" to filter both grids and laser scans.\n").arg(cropRadius),
2983  QMessageBox::Yes | QMessageBox::YesToAll | QMessageBox::No, QMessageBox::No);
2984  if(b == QMessageBox::Yes || b == QMessageBox::YesToAll)
2985  {
2986  std::map<int, Transform> poses = dbDriver_->loadOptimizedPoses(); // poses should match the grid map
2987 
2988  modifiedLaserScans_.clear();
2989 
2990  rtabmap::ProgressDialog progressDialog(this);
2991  progressDialog.setMaximumSteps(poses.size()+1);
2992  progressDialog.show();
2993  progressDialog.setCancelButtonVisible(true);
2994  progressDialog.appendText(QString("Cropping empty space... %1 scans to filter").arg(poses.size()));
2995  progressDialog.setMinimumWidth(800);
2996  QApplication::processEvents();
2997 
2998  UINFO("Cropping empty space... poses=%d cropRadius=%d", poses.size(), cropRadius);
2999  UASSERT(cropRadius>=0);
3000  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end() && !progressDialog.isCanceled(); ++iter)
3001  {
3002  // local grid
3003  cv::Mat gridGround;
3004  cv::Mat gridObstacles;
3005  cv::Mat gridEmpty;
3006 
3007  // scan
3008  SensorData data;
3009  dbDriver_->getNodeData(iter->first, data);
3010  LaserScan scan;
3011  data.uncompressData(0,0,&scan,0,&gridGround,&gridObstacles,&gridEmpty);
3012 
3013  if(generatedLocalMaps_.localGrids().find(iter->first) != generatedLocalMaps_.localGrids().end())
3014  {
3015  gridObstacles = generatedLocalMaps_.localGrids().at(iter->first).obstacleCells;
3016  }
3017  if(!gridObstacles.empty())
3018  {
3019  cv::Mat filtered = cv::Mat(1, gridObstacles.cols, gridObstacles.type());
3020  int oi = 0;
3021  for(int i=0; i<gridObstacles.cols; ++i)
3022  {
3023  const float * ptr = gridObstacles.ptr<float>(0, i);
3024  cv::Point3f pt(ptr[0], ptr[1], gridObstacles.channels()==2?0:ptr[2]);
3025  pt = util3d::transformPoint(pt, iter->second);
3026 
3027  int x = int((pt.x - xMin) / cellSize + 0.5f);
3028  int y = int((pt.y - yMin) / cellSize + 0.5f);
3029 
3030  if(x>=0 && x<map8S.cols &&
3031  y>=0 && y<map8S.rows)
3032  {
3033  bool obstacleDetected = false;
3034 
3035  for(int j=-cropRadius; j<=cropRadius && !obstacleDetected; ++j)
3036  {
3037  for(int k=-cropRadius; k<=cropRadius && !obstacleDetected; ++k)
3038  {
3039  if(x+j>=0 && x+j<map8S.cols &&
3040  y+k>=0 && y+k<map8S.rows &&
3041  map8S.at<unsigned char>(y+k,x+j) == 100)
3042  {
3043  obstacleDetected = true;
3044  }
3045  }
3046  }
3047 
3048  if(map8S.at<unsigned char>(y,x) != 0 || obstacleDetected)
3049  {
3050  // Verify that we don't have an obstacle on neighbor cells
3051  cv::Mat(gridObstacles, cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(filtered, cv::Range::all(), cv::Range(oi,oi+1)));
3052  ++oi;
3053  }
3054  }
3055  }
3056 
3057  if(oi != gridObstacles.cols)
3058  {
3059  progressDialog.appendText(QString("Grid %1 filtered %2 pts -> %3 pts").arg(iter->first).arg(gridObstacles.cols).arg(oi));
3060  UINFO("Grid %d filtered %d -> %d", iter->first, gridObstacles.cols, oi);
3061 
3062  // update
3063  cv::Mat newObstacles = cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi));
3064  if(generatedLocalMaps_.localGrids().find(iter->first) != generatedLocalMaps_.localGrids().end())
3065  {
3066  LocalGrid value = generatedLocalMaps_.localGrids().at(iter->first);
3067  value.obstacleCells = newObstacles;
3068  generatedLocalMaps_.add(iter->first, value);
3069  }
3070  else
3071  {
3072  LocalGrid value(gridGround, newObstacles, gridEmpty, data.gridCellSize(), data.gridViewPoint());
3073  generatedLocalMaps_.add(iter->first, value);
3074  }
3075 
3076  }
3077  }
3078 
3079  if(b == QMessageBox::YesToAll && !scan.isEmpty())
3080  {
3081  Transform mapToScan = iter->second * scan.localTransform();
3082 
3083  cv::Mat filtered = cv::Mat(1, scan.size(), scan.dataType());
3084  int oi = 0;
3085  for(int i=0; i<scan.size(); ++i)
3086  {
3087  const float * ptr = scan.data().ptr<float>(0, i);
3088  cv::Point3f pt(ptr[0], ptr[1], scan.is2d()?0:ptr[2]);
3089  pt = util3d::transformPoint(pt, mapToScan);
3090 
3091  int x = int((pt.x - xMin) / cellSize + 0.5f);
3092  int y = int((pt.y - yMin) / cellSize + 0.5f);
3093 
3094  if(x>=0 && x<map8S.cols &&
3095  y>=0 && y<map8S.rows)
3096  {
3097  bool obstacleDetected = false;
3098 
3099  for(int j=-cropRadius; j<=cropRadius && !obstacleDetected; ++j)
3100  {
3101  for(int k=-cropRadius; k<=cropRadius && !obstacleDetected; ++k)
3102  {
3103  if(x+j>=0 && x+j<map8S.cols &&
3104  y+k>=0 && y+k<map8S.rows &&
3105  map8S.at<unsigned char>(y+k,x+j) == 100)
3106  {
3107  obstacleDetected = true;
3108  }
3109  }
3110  }
3111 
3112  if(map8S.at<unsigned char>(y,x) != 0 || obstacleDetected)
3113  {
3114  // Verify that we don't have an obstacle on neighbor cells
3115  cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(filtered, cv::Range::all(), cv::Range(oi,oi+1)));
3116  ++oi;
3117  }
3118  }
3119  }
3120 
3121  if(oi != scan.size())
3122  {
3123  progressDialog.appendText(QString("Scan %1 filtered %2 pts -> %3 pts").arg(iter->first).arg(scan.size()).arg(oi));
3124  UINFO("Scan %d filtered %d -> %d", iter->first, scan.size(), oi);
3125 
3126  // update
3127  if(scan.angleIncrement()!=0)
3128  {
3129  // copy meta data
3130  scan = LaserScan(
3131  cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi)),
3132  scan.format(),
3133  scan.rangeMin(),
3134  scan.rangeMax(),
3135  scan.angleMin(),
3136  scan.angleMax(),
3137  scan.angleIncrement(),
3138  scan.localTransform());
3139  }
3140  else
3141  {
3142  // copy meta data
3143  scan = LaserScan(
3144  cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi)),
3145  scan.maxPoints(),
3146  scan.rangeMax(),
3147  scan.format(),
3148  scan.localTransform());
3149  }
3150  uInsert(modifiedLaserScans_, std::make_pair(iter->first, scan));
3151  }
3152  }
3153  progressDialog.incrementStep();
3154  QApplication::processEvents();
3155  }
3156  if(progressDialog.isCanceled())
3157  {
3158  modifiedLaserScans_.clear();
3159  }
3160  else
3161  {
3162  update3dView();
3163  updateGraphView();
3164  }
3165  progressDialog.setValue(progressDialog.maximumSteps());
3166  }
3167  }
3168 }
3169 
3170 void DatabaseViewer::exportSaved2DMap()
3171 {
3172  if(!dbDriver_)
3173  {
3174  QMessageBox::warning(this, tr("Cannot export 2D map"), tr("A database must must loaded first...\nUse File->Open database."));
3175  return;
3176  }
3177 
3178  float xMin, yMin, cellSize;
3179  cv::Mat map = dbDriver_->load2DMap(xMin, yMin, cellSize);
3180  if(map.empty())
3181  {
3182  QMessageBox::warning(this, tr("Cannot export 2D map"), tr("The database doesn't contain a saved 2D map."));
3183  }
3184  else
3185  {
3186  cv::Mat map8U = rtabmap::util3d::convertMap2Image8U(map, true);
3187  QString name = QFileInfo(databaseFileName_.c_str()).baseName();
3188  QString path = QFileDialog::getSaveFileName(
3189  this,
3190  tr("Save File"),
3191  pathDatabase_+"/" + name + ".pgm",
3192  tr("Map (*.pgm)"));
3193 
3194  if(!path.isEmpty())
3195  {
3196  if(QFileInfo(path).suffix() == "")
3197  {
3198  path += ".pgm";
3199  }
3200  cv::imwrite(path.toStdString(), map8U);
3201 
3202  QFileInfo info(path);
3203  QString yaml = info.absolutePath() + "/" + info.baseName() + ".yaml";
3204 
3205  float occupancyThr = Parameters::defaultGridGlobalOccupancyThr();
3206  Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kGridGlobalOccupancyThr(), occupancyThr);
3207 
3208  std::ofstream file;
3209  file.open (yaml.toStdString());
3210  file << "image: " << info.baseName().toStdString() << ".pgm" << std::endl;
3211  file << "resolution: " << cellSize << std::endl;
3212  file << "origin: [" << xMin << ", " << yMin << ", 0.0]" << std::endl;
3213  file << "negate: 0" << std::endl;
3214  file << "occupied_thresh: " << occupancyThr << std::endl;
3215  file << "free_thresh: 0.196" << std::endl;
3216  file << std::endl;
3217  file.close();
3218 
3219 
3220  QMessageBox::information(this, tr("Export 2D map"), tr("Exported %1 and %2!").arg(path).arg(yaml));
3221  }
3222  }
3223 }
3224 
3225 void DatabaseViewer::import2DMap()
3226 {
3227  if(!dbDriver_)
3228  {
3229  QMessageBox::warning(this, tr("Cannot import 2D map"), tr("A database must must loaded first...\nUse File->Open database."));
3230  return;
3231  }
3232 
3233  if(uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.17.0") < 0)
3234  {
3235  QMessageBox::warning(this, tr("Cannot edit 2D map"),
3236  tr("The database has too old version (%1) to saved "
3237  "optimized map. Version 0.17.0 minimum required.").arg(dbDriver_->getDatabaseVersion().c_str()));
3238  return;
3239  }
3240 
3241  if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size() || generatedLocalMaps_.size())
3242  {
3243  QMessageBox::warning(this, tr("Cannot import 2D map"),
3244  tr("The database has modified links and/or modified local "
3245  "occupancy grids, the 2D optimized map cannot be modified."));
3246  return;
3247  }
3248 
3249  float xMin, yMin, cellSize;
3250  cv::Mat mapOrg = dbDriver_->load2DMap(xMin, yMin, cellSize);
3251  if(mapOrg.empty())
3252  {
3253  QMessageBox::warning(this, tr("Cannot import 2D map"), tr("The database doesn't contain a saved 2D map."));
3254  }
3255  else
3256  {
3257  QString path = QFileDialog::getOpenFileName(
3258  this,
3259  tr("Open File"),
3260  pathDatabase_,
3261  tr("Map (*.pgm)"));
3262  if(!path.isEmpty())
3263  {
3264  cv::Mat map8U = cv::imread(path.toStdString(), cv::IMREAD_UNCHANGED);
3265  cv::Mat map = rtabmap::util3d::convertImage8U2Map(map8U, true);
3266 
3267  if(mapOrg.cols == map.cols && mapOrg.rows == map8U.rows)
3268  {
3269  dbDriver_->save2DMap(map, xMin, yMin, cellSize);
3270  QMessageBox::information(this, tr("Import 2D map"), tr("Imported %1!").arg(path));
3271  }
3272  else
3273  {
3274  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));
3275  }
3276  }
3277  }
3278 }
3279 
3280 void DatabaseViewer::regenerateSavedMap()
3281 {
3282  if(!dbDriver_)
3283  {
3284  QMessageBox::warning(this, tr("Cannot import 2D map"), tr("A database must must loaded first...\nUse File->Open database."));
3285  return;
3286  }
3287 
3288  if(uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.17.0") < 0)
3289  {
3290  QMessageBox::warning(this, tr("Cannot edit 2D map"),
3291  tr("The database has too old version (%1) to saved "
3292  "optimized map. Version 0.17.0 minimum required.").arg(dbDriver_->getDatabaseVersion().c_str()));
3293  return;
3294  }
3295 
3296  if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size() || generatedLocalMaps_.size())
3297  {
3298  QMessageBox::warning(this, tr("Cannot import 2D map"),
3299  tr("The database has modified links and/or modified local "
3300  "occupancy grids, the 2D optimized map cannot be modified. Try "
3301  "closing the database and re-open it to save the changes."));
3302  return;
3303  }
3304 
3305  if((int)graphes_.empty() || localMaps_.empty())
3306  {
3307  QMessageBox::warning(this, tr("Cannot regenerate 2D map"),
3308  tr("Graph is empty, make sure you opened the "
3309  "Graph View and there is a map shown."));
3310  return;
3311  }
3312 
3313  //
3314 #ifdef RTABMAP_OCTOMAP
3315  QStringList types;
3316  types.push_back("Default occupancy grid");
3317  types.push_back("From OctoMap projection");
3318  bool ok;
3319  QString type = QInputDialog::getItem(this, tr("Which type?"), tr("Type:"), types, 0, false, &ok);
3320  if(!ok)
3321  {
3322  return;
3323  }
3324 #endif
3325 
3326  //update scans
3327  UINFO("Update local maps list...");
3328  float xMin, yMin;
3329  cv::Mat map;
3330  float gridCellSize = Parameters::defaultGridCellSize();
3331  Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kGridCellSize(), gridCellSize);
3332 #ifdef RTABMAP_OCTOMAP
3333  if(type.compare("From OctoMap projection") == 0)
3334  {
3335  //create local octomap
3336  OctoMap octomap(&localMaps_, ui_->parameters_toolbox->getParameters());
3337 
3338  octomap.update(graphes_.back());
3339  map = octomap.createProjectionMap(xMin, yMin, gridCellSize, 0);
3340  }
3341  else
3342 #endif
3343  {
3344  OccupancyGrid grid(&localMaps_, ui_->parameters_toolbox->getParameters());
3345  grid.update(graphes_.back());
3346  map = grid.getMap(xMin, yMin);
3347  }
3348 
3349  if(map.empty())
3350  {
3351  QMessageBox::information(this, tr("Regenerate 2D map"), tr("Failed to renegerate the map, resulting map is empty!"));
3352  }
3353  else
3354  {
3355  dbDriver_->save2DMap(map, xMin, yMin, gridCellSize);
3356  Transform lastlocalizationPose;
3357  dbDriver_->loadOptimizedPoses(&lastlocalizationPose);
3358  if(lastlocalizationPose.isNull() && !graphes_.back().empty())
3359  {
3360  // use last pose by default
3361  lastlocalizationPose = graphes_.back().rbegin()->second;
3362  }
3363  dbDriver_->saveOptimizedPoses(graphes_.back(), lastlocalizationPose);
3364  // reset optimized mesh as poses have changed
3365  dbDriver_->saveOptimizedMesh(cv::Mat());
3366  QMessageBox::information(this, tr("Regenerate 2D map"), tr("Map regenerated!"));
3367  ui_->actionEdit_optimized_2D_map->setEnabled(true);
3368  ui_->actionExport_saved_2D_map->setEnabled(true);
3369  ui_->actionImport_2D_map->setEnabled(true);
3370  }
3371 }
3372 
3373 void DatabaseViewer::viewOptimizedMesh()
3374 {
3375  if(!dbDriver_)
3376  {
3377  QMessageBox::warning(this, tr("Cannot view optimized mesh"), tr("A database must must loaded first...\nUse File->Open database."));
3378  return;
3379  }
3380 
3381  std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
3382 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
3383  std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
3384 #else
3385  std::vector<std::vector<Eigen::Vector2f> > texCoords;
3386 #endif
3387  cv::Mat textures;
3388  cv::Mat cloudMat = dbDriver_->loadOptimizedMesh(&polygons, &texCoords, &textures);
3389  if(cloudMat.empty())
3390  {
3391  QMessageBox::warning(this, tr("Cannot view optimized mesh"), tr("The database doesn't contain a saved optimized mesh."));
3392  }
3393  else
3394  {
3395  CloudViewer * viewer = new CloudViewer(this);
3396  viewer->setWindowFlags(Qt::Window);
3397  viewer->setAttribute(Qt::WA_DeleteOnClose);
3398  viewer->buildPickingLocator(true);
3399  if(!textures.empty())
3400  {
3401  pcl::TextureMeshPtr mesh = util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
3402  util3d::fixTextureMeshForVisualization(*mesh);
3403  viewer->setWindowTitle("Optimized Textured Mesh");
3404  viewer->setPolygonPicking(true);
3405  viewer->addCloudTextureMesh("mesh", mesh, textures);
3406  }
3407  else if(polygons.size() == 1)
3408  {
3409  pcl::PolygonMeshPtr mesh = util3d::assemblePolygonMesh(cloudMat, polygons.at(0));
3410  viewer->setWindowTitle("Optimized Mesh");
3411  viewer->setPolygonPicking(true);
3412  viewer->addCloudMesh("mesh", mesh);
3413  }
3414  else
3415  {
3416  LaserScan scan = LaserScan::backwardCompatibility(cloudMat);
3417  pcl::PCLPointCloud2::Ptr cloud = util3d::laserScanToPointCloud2(scan);
3418  viewer->setWindowTitle("Optimized Point Cloud");
3419  viewer->addCloud("mesh", cloud, Transform::getIdentity(), scan.hasRGB(), scan.hasNormals(), scan.hasIntensity());
3420  }
3421  viewer->show();
3422  }
3423 }
3424 
3425 void DatabaseViewer::exportOptimizedMesh()
3426 {
3427  if(!dbDriver_)
3428  {
3429  QMessageBox::warning(this, tr("Cannot export optimized mesh"), tr("A database must must loaded first...\nUse File->Open database."));
3430  return;
3431  }
3432 
3433  std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
3434 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
3435  std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
3436 #else
3437  std::vector<std::vector<Eigen::Vector2f> > texCoords;
3438 #endif
3439  cv::Mat textures;
3440  cv::Mat cloudMat = dbDriver_->loadOptimizedMesh(&polygons, &texCoords, &textures);
3441  if(cloudMat.empty())
3442  {
3443  QMessageBox::warning(this, tr("Cannot export optimized mesh"), tr("The database doesn't contain a saved optimized mesh."));
3444  }
3445  else
3446  {
3447  QString name = QFileInfo(databaseFileName_.c_str()).baseName();
3448 
3449  if(!textures.empty())
3450  {
3451  pcl::TextureMeshPtr mesh = util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures);
3452  QString path = QFileDialog::getSaveFileName(
3453  this,
3454  tr("Save File"),
3455  pathDatabase_+"/" + name + ".obj",
3456  tr("Mesh (*.obj)"));
3457 
3458  if(!path.isEmpty())
3459  {
3460  if(QFileInfo(path).suffix() == "")
3461  {
3462  path += ".obj";
3463  }
3464  QString baseName = QFileInfo(path).baseName();
3465  if(mesh->tex_materials.size() == 1)
3466  {
3467  mesh->tex_materials.at(0).tex_file = baseName.toStdString() + ".png";
3468  cv::imwrite((QFileInfo(path).absoluteDir().absolutePath()+QDir::separator()+baseName).toStdString() + ".png", textures);
3469  }
3470  else
3471  {
3472  for(unsigned int i=0; i<mesh->tex_materials.size(); ++i)
3473  {
3474  mesh->tex_materials.at(i).tex_file = (baseName+QDir::separator()+QString::number(i)+".png").toStdString();
3475  UASSERT((i+1)*textures.rows <= (unsigned int)textures.cols);
3476  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)));
3477  }
3478  }
3479  pcl::io::saveOBJFile(path.toStdString(), *mesh);
3480 
3481  QMessageBox::information(this, tr("Export Textured Mesh"), tr("Exported %1!").arg(path));
3482  }
3483  }
3484  else if(polygons.size() == 1)
3485  {
3486  pcl::PolygonMeshPtr mesh = util3d::assemblePolygonMesh(cloudMat, polygons.at(0));
3487  QString path = QFileDialog::getSaveFileName(
3488  this,
3489  tr("Save File"),
3490  pathDatabase_+"/" + name + ".ply",
3491  tr("Mesh (*.ply)"));
3492 
3493  if(!path.isEmpty())
3494  {
3495  if(QFileInfo(path).suffix() == "")
3496  {
3497  path += ".ply";
3498  }
3499  pcl::io::savePLYFileBinary(path.toStdString(), *mesh);
3500  QMessageBox::information(this, tr("Export Mesh"), tr("Exported %1!").arg(path));
3501  }
3502  }
3503  else
3504  {
3505  QString path = QFileDialog::getSaveFileName(
3506  this,
3507  tr("Save File"),
3508  pathDatabase_+"/" + name + ".ply",
3509  tr("Point cloud data (*.ply *.pcd)"));
3510 
3511  if(!path.isEmpty())
3512  {
3513  if(QFileInfo(path).suffix() == "")
3514  {
3515  path += ".ply";
3516  }
3517  bool success = false;
3518  pcl::PCLPointCloud2::Ptr cloud = util3d::laserScanToPointCloud2(LaserScan::backwardCompatibility(cloudMat));
3519  if(QFileInfo(path).suffix() == "pcd")
3520  {
3521  success = pcl::io::savePCDFile(path.toStdString(), *cloud) == 0;
3522  }
3523  else
3524  {
3525  success = pcl::io::savePLYFile(path.toStdString(), *cloud) == 0;
3526  }
3527  if(success)
3528  {
3529  QMessageBox::information(this, tr("Export Point Cloud"), tr("Exported %1!").arg(path));
3530  }
3531  else
3532  {
3533  QMessageBox::critical(this, tr("Export Point Cloud"), tr("Failed exporting %1!").arg(path));
3534  }
3535  }
3536  }
3537  }
3538 }
3539 
3540 void DatabaseViewer::updateOptimizedMesh()
3541 {
3542  if(!ids_.size() || !dbDriver_)
3543  {
3544  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
3545  return;
3546  }
3547 
3548  if(graphes_.empty())
3549  {
3550  this->updateGraphView();
3551  if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
3552  {
3553  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
3554  return;
3555  }
3556  }
3557 
3558  std::map<int, Transform> optimizedPoses;
3559  if(ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() &&
3560  ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked()
3561  && !groundTruthPoses_.empty())
3562  {
3563  optimizedPoses = groundTruthPoses_;
3564  }
3565  else
3566  {
3567  optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
3568  }
3569  if(ui_->groupBox_posefiltering->isChecked())
3570  {
3571  optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
3572  ui_->doubleSpinBox_posefilteringRadius->value(),
3573  ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
3574  }
3575  Transform lastlocalizationPose;
3576  dbDriver_->loadOptimizedPoses(&lastlocalizationPose);
3577  //optimized poses have changed, reset 2d map
3578  dbDriver_->save2DMap(cv::Mat(), 0, 0, 0);
3579  if(optimizedPoses.size() > 0)
3580  {
3581  exportDialog_->setDBDriver(dbDriver_);
3582  exportDialog_->forceAssembling(true);
3583  exportDialog_->setOkButton();
3584 
3585  std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> clouds;
3586  std::map<int, pcl::PolygonMesh::Ptr> meshes;
3587  std::map<int, pcl::TextureMesh::Ptr> textureMeshes;
3588  std::vector<std::map<int, pcl::PointXY> > textureVertexToPixels;
3589 
3590  if(exportDialog_->getExportedClouds(
3591  optimizedPoses,
3592  updateLinksWithModifications(links_),
3593  mapIds_,
3594  QMap<int, Signature>(),
3595  std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
3596  std::map<int, LaserScan>(),
3597  pathDatabase_,
3598  ui_->parameters_toolbox->getParameters(),
3599  clouds,
3600  meshes,
3601  textureMeshes,
3602  textureVertexToPixels))
3603  {
3604  if(textureMeshes.size())
3605  {
3606  dbDriver_->saveOptimizedPoses(optimizedPoses, lastlocalizationPose);
3607 
3608  cv::Mat globalTextures;
3609  pcl::TextureMeshPtr textureMesh = textureMeshes.at(0);
3610  if(textureMesh->tex_materials.size()>1)
3611  {
3612  globalTextures = util3d::mergeTextures(
3613  *textureMesh,
3614  std::map<int, cv::Mat>(),
3615  std::map<int, std::vector<CameraModel> >(),
3616  0,
3617  dbDriver_,
3618  exportDialog_->getTextureSize(),
3619  exportDialog_->getMaxTextures(),
3620  textureVertexToPixels,
3621  exportDialog_->isGainCompensation(),
3622  exportDialog_->getGainBeta(),
3623  exportDialog_->isGainRGB(),
3624  exportDialog_->isBlending(),
3625  exportDialog_->getBlendingDecimation(),
3626  exportDialog_->getTextureBrightnessConstrastRatioLow(),
3627  exportDialog_->getTextureBrightnessConstrastRatioHigh(),
3628  exportDialog_->isExposeFusion());
3629  }
3630  dbDriver_->saveOptimizedMesh(
3631  util3d::laserScanFromPointCloud(textureMesh->cloud, false).data(),
3632  util3d::convertPolygonsFromPCL(textureMesh->tex_polygons),
3633  textureMesh->tex_coordinates,
3634  globalTextures);
3635  QMessageBox::information(this, tr("Update Optimized Textured Mesh"), tr("Updated!"));
3636  ui_->actionView_optimized_mesh->setEnabled(true);
3637  ui_->actionExport_optimized_mesh->setEnabled(true);
3638  this->viewOptimizedMesh();
3639  }
3640  else if(meshes.size())
3641  {
3642  dbDriver_->saveOptimizedPoses(optimizedPoses, lastlocalizationPose);
3643  std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons(1);
3644  polygons.at(0) = util3d::convertPolygonsFromPCL(meshes.at(0)->polygons);
3645  dbDriver_->saveOptimizedMesh(util3d::laserScanFromPointCloud(meshes.at(0)->cloud, false).data(), polygons);
3646  QMessageBox::information(this, tr("Update Optimized Mesh"), tr("Updated!"));
3647  ui_->actionView_optimized_mesh->setEnabled(true);
3648  ui_->actionExport_optimized_mesh->setEnabled(true);
3649  this->viewOptimizedMesh();
3650  }
3651  else if(clouds.size())
3652  {
3653  dbDriver_->saveOptimizedPoses(optimizedPoses, lastlocalizationPose);
3654  dbDriver_->saveOptimizedMesh(util3d::laserScanFromPointCloud(*clouds.at(0)).data());
3655  QMessageBox::information(this, tr("Update Optimized PointCloud"), tr("Updated!"));
3656  ui_->actionView_optimized_mesh->setEnabled(true);
3657  ui_->actionExport_optimized_mesh->setEnabled(true);
3658  this->viewOptimizedMesh();
3659  }
3660  else
3661  {
3662  QMessageBox::critical(this, tr("Update Optimized Mesh"), tr("Nothing to save!"));
3663  }
3664  }
3665  exportDialog_->setProgressDialogToMax();
3666  }
3667  else
3668  {
3669  QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
3670  }
3671 }
3672 
3673 void DatabaseViewer::generateGraph()
3674 {
3675  if(!dbDriver_)
3676  {
3677  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("A database must must loaded first...\nUse File->Open database."));
3678  return;
3679  }
3680 
3681  QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/Graph.dot", tr("Graphiz file (*.dot)"));
3682  if(!path.isEmpty())
3683  {
3684  dbDriver_->generateGraph(path.toStdString());
3685  }
3686 }
3687 
3688 void DatabaseViewer::generateLocalGraph()
3689 {
3690  if(!ids_.size() || !dbDriver_)
3691  {
3692  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
3693  return;
3694  }
3695  bool ok = false;
3696  int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID"), ids_.first(), ids_.first(), ids_.last(), 1, &ok);
3697 
3698  if(ok)
3699  {
3700  int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin"), 4, 1, 100, 1, &ok);
3701  if(ok)
3702  {
3703  QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/Graph" + QString::number(id) + ".dot", tr("Graphiz file (*.dot)"));
3704  if(!path.isEmpty() && id>0)
3705  {
3706  std::map<int, int> ids;
3707  std::list<int> curentMarginList;
3708  std::set<int> currentMargin;
3709  std::set<int> nextMargin;
3710  nextMargin.insert(id);
3711  int m = 0;
3712  while((margin == 0 || m < margin) && nextMargin.size())
3713  {
3714  curentMarginList = std::list<int>(nextMargin.rbegin(), nextMargin.rend());
3715  nextMargin.clear();
3716 
3717  for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
3718  {
3719  if(ids.find(*jter) == ids.end())
3720  {
3721  std::multimap<int, Link> links;
3722  ids.insert(std::pair<int, int>(*jter, m));
3723 
3724  UTimer timer;
3725  dbDriver_->loadLinks(*jter, links);
3726 
3727  // links
3728  for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
3729  {
3730  if( !uContains(ids, iter->first))
3731  {
3732  UASSERT(iter->second.type() != Link::kUndef);
3733  if(iter->second.type() == Link::kNeighbor ||
3734  iter->second.type() == Link::kNeighborMerged)
3735  {
3736  nextMargin.insert(iter->first);
3737  }
3738  else
3739  {
3740  // loop closures are on same margin
3741  if(currentMargin.insert(iter->first).second)
3742  {
3743  curentMarginList.push_back(iter->first);
3744  }
3745  }
3746  }
3747  }
3748  }
3749  }
3750  ++m;
3751  }
3752 
3753  if(ids.size() > 0)
3754  {
3755  ids.insert(std::pair<int,int>(id, 0));
3756  std::set<int> idsSet;
3757  for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
3758  {
3759  idsSet.insert(idsSet.end(), iter->first);
3760  UINFO("Node %d", iter->first);
3761  }
3762  UINFO("idsSet=%d", idsSet.size());
3763  dbDriver_->generateGraph(path.toStdString(), idsSet);
3764  }
3765  else
3766  {
3767  QMessageBox::critical(this, tr("Error"), tr("No neighbors found for signature %1.").arg(id));
3768  }
3769  }
3770  }
3771  }
3772 }
3773 
3774 void DatabaseViewer::regenerateLocalMaps()
3775 {
3776  LocalGridMaker localMapMaker(ui_->parameters_toolbox->getParameters());
3777 
3778  generatedLocalMaps_.clear();
3779 
3780  rtabmap::ProgressDialog progressDialog(this);
3781  progressDialog.setMaximumSteps(ids_.size());
3782  progressDialog.show();
3783  progressDialog.setCancelButtonVisible(true);
3784 
3785  UPlot * plot = new UPlot(this);
3786  plot->setWindowFlags(Qt::Window);
3787  plot->setWindowTitle("Local Occupancy Grid Generation Time (ms)");
3788  plot->setAttribute(Qt::WA_DeleteOnClose);
3789  UPlotCurve * decompressionCurve = plot->addCurve("Decompression");
3790  UPlotCurve * gridCreationCurve = plot->addCurve("Grid Creation");
3791  plot->show();
3792 
3793  UPlot * plotCells = new UPlot(this);
3794  plotCells->setWindowFlags(Qt::Window);
3795  plotCells->setWindowTitle("Occupancy Cells");
3796  plotCells->setAttribute(Qt::WA_DeleteOnClose);
3797  UPlotCurve * totalCurve = plotCells->addCurve("Total");
3798  UPlotCurve * emptyCurve = plotCells->addCurve("Empty");
3799  UPlotCurve * obstaclesCurve = plotCells->addCurve("Obstacles");
3800  UPlotCurve * groundCurve = plotCells->addCurve("Ground");
3801  plotCells->show();
3802 
3803  double decompressionTime = 0;
3804  double gridCreationTime = 0;
3805 
3806  for(int i =0; i<ids_.size() && !progressDialog.isCanceled(); ++i)
3807  {
3808  UTimer timer;
3809  SensorData data;
3810  dbDriver_->getNodeData(ids_.at(i), data);
3811  data.uncompressData();
3812  decompressionTime = timer.ticks()*1000.0;
3813 
3814  int mapId, weight;
3815  Transform odomPose, groundTruth;
3816  std::string label;
3817  double stamp;
3818  QString msg;
3819  std::vector<float> velocity;
3820  GPS gps;
3821  EnvSensors sensors;
3822  if(dbDriver_->getNodeInfo(data.id(), odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors))
3823  {
3824  Signature s = data;
3825  s.setPose(odomPose);
3826  cv::Mat ground, obstacles, empty;
3827  cv::Point3f viewpoint;
3828  timer.ticks();
3829 
3830  if(ui_->checkBox_grid_regenerateFromSavedGrid->isChecked() && s.sensorData().gridCellSize() > 0.0f)
3831  {
3832  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridObstacleCellsRaw()));
3833  *cloud+=*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridGroundCellsRaw()));
3834 
3835  if(cloud->size())
3836  {
3837  // update viewpoint
3838  if(s.sensorData().cameraModels().size())
3839  {
3840  // average of all local transforms
3841  float sum = 0;
3842  for(unsigned int i=0; i<s.sensorData().cameraModels().size(); ++i)
3843  {
3844  const Transform & t = s.sensorData().cameraModels()[i].localTransform();
3845  if(!t.isNull())
3846  {
3847  viewpoint.x += t.x();
3848  viewpoint.y += t.y();
3849  viewpoint.z += t.z();
3850  sum += 1.0f;
3851  }
3852  }
3853  if(sum > 0.0f)
3854  {
3855  viewpoint.x /= sum;
3856  viewpoint.y /= sum;
3857  viewpoint.z /= sum;
3858  }
3859  }
3860  else if(s.sensorData().cameraModels().size())
3861  {
3862  // average of all local transforms
3863  float sum = 0;
3864  for(unsigned int i=0; i<s.sensorData().stereoCameraModels().size(); ++i)
3865  {
3866  const Transform & t = s.sensorData().stereoCameraModels()[i].localTransform();
3867  if(!t.isNull())
3868  {
3869  viewpoint.x += t.x();
3870  viewpoint.y += t.y();
3871  viewpoint.z += t.z();
3872  sum += 1.0f;
3873  }
3874  }
3875  if(sum > 0.0f)
3876  {
3877  viewpoint.x /= sum;
3878  viewpoint.y /= sum;
3879  viewpoint.z /= sum;
3880  }
3881  }
3882 
3883  localMapMaker.createLocalMap(util3d::laserScanFromPointCloud(*cloud), s.getPose(), ground, obstacles, empty, viewpoint);
3884  }
3885  }
3886  else
3887  {
3888  localMapMaker.createLocalMap(s, ground, obstacles, empty, viewpoint);
3889  }
3890 
3891  gridCreationTime = timer.ticks()*1000.0;
3892  generatedLocalMaps_.add(data.id(), ground, obstacles, empty, localMapMaker.getCellSize(), viewpoint);
3893  msg = QString("Generated local occupancy grid map %1/%2").arg(i+1).arg((int)ids_.size());
3894 
3895  totalCurve->addValue(ids_.at(i), obstacles.cols+ground.cols+empty.cols);
3896  emptyCurve->addValue(ids_.at(i), empty.cols);
3897  obstaclesCurve->addValue(ids_.at(i), obstacles.cols);
3898  groundCurve->addValue(ids_.at(i), ground.cols);
3899  }
3900 
3901  progressDialog.appendText(msg);
3902  progressDialog.incrementStep();
3903 
3904  decompressionCurve->addValue(ids_.at(i), decompressionTime);
3905  gridCreationCurve->addValue(ids_.at(i), gridCreationTime);
3906 
3907  if(ids_.size() < 50 || (i+1) % 25 == 0)
3908  {
3909  QApplication::processEvents();
3910  }
3911  }
3912  progressDialog.setValue(progressDialog.maximumSteps());
3913 
3914  if(graphes_.size())
3915  {
3916  update3dView();
3917  sliderIterationsValueChanged((int)graphes_.size()-1);
3918  }
3919  else
3920  {
3921  updateGrid();
3922  }
3923 }
3924 
3925 void DatabaseViewer::regenerateCurrentLocalMaps()
3926 {
3927  UTimer time;
3928  LocalGridMaker localMapMaker(ui_->parameters_toolbox->getParameters());
3929 
3930  if(ids_.size() == 0)
3931  {
3932  UWARN("ids_ is empty!");
3933  return;
3934  }
3935 
3936  QSet<int> idsSet;
3937  idsSet.insert(ids_.at(ui_->horizontalSlider_A->value()));
3938  idsSet.insert(ids_.at(ui_->horizontalSlider_B->value()));
3939 #if QT_VERSION >= QT_VERSION_CHECK(5, 15, 3)
3940  QList<int> ids(idsSet.begin(), idsSet.end());
3941 #else
3942  QList<int> ids = idsSet.toList();
3943 #endif
3944 
3945  rtabmap::ProgressDialog progressDialog(this);
3946  progressDialog.setMaximumSteps(ids.size());
3947  progressDialog.show();
3948 
3949  for(int i =0; i<ids.size(); ++i)
3950  {
3951  SensorData data;
3952  dbDriver_->getNodeData(ids.at(i), data);
3953  data.uncompressData();
3954 
3955  int mapId, weight;
3956  Transform odomPose, groundTruth;
3957  std::string label;
3958  double stamp;
3959  QString msg;
3960  std::vector<float> velocity;
3961  GPS gps;
3962  EnvSensors sensors;
3963  if(dbDriver_->getNodeInfo(data.id(), odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors))
3964  {
3965  Signature s = data;
3966  s.setPose(odomPose);
3967  cv::Mat ground, obstacles, empty;
3968  cv::Point3f viewpoint;
3969 
3970  if(ui_->checkBox_grid_regenerateFromSavedGrid->isChecked() && s.sensorData().gridCellSize() > 0.0f)
3971  {
3972  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridObstacleCellsRaw()));
3973  *cloud+=*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridGroundCellsRaw()));
3974 
3975  if(cloud->size())
3976  {
3977  // update viewpoint
3978  if(s.sensorData().cameraModels().size())
3979  {
3980  // average of all local transforms
3981  float sum = 0;
3982  for(unsigned int i=0; i<s.sensorData().cameraModels().size(); ++i)
3983  {
3984  const Transform & t = s.sensorData().cameraModels()[i].localTransform();
3985  if(!t.isNull())
3986  {
3987  viewpoint.x += t.x();
3988  viewpoint.y += t.y();
3989  viewpoint.z += t.z();
3990  sum += 1.0f;
3991  }
3992  }
3993  if(sum > 0.0f)
3994  {
3995  viewpoint.x /= sum;
3996  viewpoint.y /= sum;
3997  viewpoint.z /= sum;
3998  }
3999  }
4000  else if(s.sensorData().stereoCameraModels().size())
4001  {
4002  // average of all local transforms
4003  float sum = 0;
4004  for(unsigned int i=0; i<s.sensorData().stereoCameraModels().size(); ++i)
4005  {
4006  const Transform & t = s.sensorData().stereoCameraModels()[i].localTransform();
4007  if(!t.isNull())
4008  {
4009  viewpoint.x += t.x();
4010  viewpoint.y += t.y();
4011  viewpoint.z += t.z();
4012  sum += 1.0f;
4013  }
4014  }
4015  if(sum > 0.0f)
4016  {
4017  viewpoint.x /= sum;
4018  viewpoint.y /= sum;
4019  viewpoint.z /= sum;
4020  }
4021  }
4022 
4023  localMapMaker.createLocalMap(util3d::laserScanFromPointCloud(*cloud), s.getPose(), ground, obstacles, empty, viewpoint);
4024  }
4025  }
4026  else
4027  {
4028  localMapMaker.createLocalMap(s, ground, obstacles, empty, viewpoint);
4029  }
4030 
4031  generatedLocalMaps_.add(data.id(), ground, obstacles, empty, localMapMaker.getCellSize(), viewpoint);
4032  msg = QString("Generated local occupancy grid map %1/%2 (%3s)").arg(i+1).arg((int)ids.size()).arg(time.ticks());
4033  }
4034 
4035  progressDialog.appendText(msg);
4036  progressDialog.incrementStep();
4037  QApplication::processEvents();
4038  }
4039  progressDialog.setValue(progressDialog.maximumSteps());
4040 
4041  if(graphes_.size())
4042  {
4043  update3dView();
4044  sliderIterationsValueChanged((int)graphes_.size()-1);
4045  }
4046  else
4047  {
4048  updateGrid();
4049  }
4050 }
4051 
4052 void DatabaseViewer::view3DMap()
4053 {
4054  if(!ids_.size() || !dbDriver_)
4055  {
4056  QMessageBox::warning(this, tr("Cannot view 3D map"), tr("The database is empty..."));
4057  return;
4058  }
4059 
4060  if(graphes_.empty())
4061  {
4062  this->updateGraphView();
4063  if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
4064  {
4065  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
4066  return;
4067  }
4068  }
4069 
4070  std::map<int, Transform> optimizedPoses;
4071  if(ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() &&
4072  ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() &&
4073  !groundTruthPoses_.empty())
4074  {
4075  optimizedPoses = groundTruthPoses_;
4076  }
4077  else
4078  {
4079  optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
4080  }
4081  if(ui_->groupBox_posefiltering->isChecked())
4082  {
4083  optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
4084  ui_->doubleSpinBox_posefilteringRadius->value(),
4085  ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
4086  }
4087  if(optimizedPoses.size() > 0)
4088  {
4089  exportDialog_->setDBDriver(dbDriver_);
4090  exportDialog_->viewClouds(optimizedPoses,
4091  updateLinksWithModifications(links_),
4092  mapIds_,
4093  QMap<int, Signature>(),
4094  std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
4095  std::map<int, LaserScan>(),
4096  pathDatabase_,
4097  ui_->parameters_toolbox->getParameters());
4098  }
4099  else
4100  {
4101  QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
4102  }
4103 }
4104 
4105 void DatabaseViewer::generate3DMap()
4106 {
4107  if(!ids_.size() || !dbDriver_)
4108  {
4109  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
4110  return;
4111  }
4112 
4113  if(graphes_.empty())
4114  {
4115  this->updateGraphView();
4116  if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
4117  {
4118  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
4119  return;
4120  }
4121  }
4122 
4123  std::map<int, Transform> optimizedPoses;
4124  if(ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() &&
4125  ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() &&
4126  !groundTruthPoses_.empty())
4127  {
4128  optimizedPoses = groundTruthPoses_;
4129  }
4130  else
4131  {
4132  optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
4133 
4134  bool alignToGPS =
4135  ui_->checkBox_alignPosesWithGPS->isEnabled() &&
4136  ui_->checkBox_alignPosesWithGPS->isChecked();
4137 
4138  if(alignToGPS ||
4139  (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked()))
4140  {
4141  std::map<int, Transform> refPoses = groundTruthPoses_;
4142  if(alignToGPS)
4143  {
4144  refPoses = gpsPoses_;
4145  }
4146 
4147  // Log ground truth statistics (in TUM's RGBD-SLAM format)
4148  if(refPoses.size())
4149  {
4150  float translational_rmse = 0.0f;
4151  float translational_mean = 0.0f;
4152  float translational_median = 0.0f;
4153  float translational_std = 0.0f;
4154  float translational_min = 0.0f;
4155  float translational_max = 0.0f;
4156  float rotational_rmse = 0.0f;
4157  float rotational_mean = 0.0f;
4158  float rotational_median = 0.0f;
4159  float rotational_std = 0.0f;
4160  float rotational_min = 0.0f;
4161  float rotational_max = 0.0f;
4162 
4163  Transform gtToMap = graph::calcRMSE(
4164  refPoses,
4165  optimizedPoses,
4166  translational_rmse,
4167  translational_mean,
4168  translational_median,
4169  translational_std,
4170  translational_min,
4171  translational_max,
4172  rotational_rmse,
4173  rotational_mean,
4174  rotational_median,
4175  rotational_std,
4176  rotational_min,
4177  rotational_max,
4178  alignToGPS);
4179 
4180  if(!gtToMap.isIdentity())
4181  {
4182  for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
4183  {
4184  iter->second = gtToMap * iter->second;
4185  }
4186  if(alignToGPS && optimizedPoses.find(gpsValues_.begin()->first)!=optimizedPoses.end())
4187  {
4188  // This will make the exported first pose the GPS origin.
4189  int originId = gpsValues_.begin()->first;
4190  Transform offset = optimizedPoses.at(originId).translation().inverse();
4191  for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
4192  {
4193  iter->second = offset * iter->second;
4194  }
4195  }
4196  }
4197  }
4198  }
4199  }
4200  if(ui_->groupBox_posefiltering->isChecked())
4201  {
4202  optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
4203  ui_->doubleSpinBox_posefilteringRadius->value(),
4204  ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
4205  }
4206  if(optimizedPoses.size() > 0)
4207  {
4208  exportDialog_->setDBDriver(dbDriver_);
4209  exportDialog_->exportClouds(optimizedPoses,
4210  updateLinksWithModifications(links_),
4211  mapIds_,
4212  QMap<int, Signature>(),
4213  std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
4214  std::map<int, LaserScan>(),
4215  pathDatabase_,
4216  ui_->parameters_toolbox->getParameters());
4217  }
4218  else
4219  {
4220  QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
4221  }
4222 }
4223 
4224 void DatabaseViewer::detectMoreLoopClosures()
4225 {
4226  if(graphes_.empty())
4227  {
4228  this->updateGraphView();
4229  if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
4230  {
4231  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
4232  return;
4233  }
4234  }
4235 
4236  std::map<int, Transform> optimizedPoses = graphes_.back();
4237 
4238  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
4239  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
4240  progressDialog->setMaximumSteps(1);
4241  progressDialog->setCancelButtonVisible(true);
4242  progressDialog->setMinimumWidth(800);
4243  progressDialog->show();
4244 
4245  const ParametersMap & parameters = ui_->parameters_toolbox->getParameters();
4246  bool loopCovLimited = Parameters::defaultRGBDLoopCovLimited();
4247  Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), loopCovLimited);
4248  if(loopCovLimited)
4249  {
4250  odomMaxInf_ = graph::getMaxOdomInf(updateLinksWithModifications(links_));
4251  }
4252 
4253  int iterations = ui_->spinBox_detectMore_iterations->value();
4254  UASSERT(iterations > 0);
4255  int added = 0;
4256  std::multimap<int, int> checkedLoopClosures;
4257  std::pair<int, int> lastAdded(0,0);
4258  bool intraSession = ui_->checkBox_detectMore_intraSession->isChecked();
4259  bool interSession = ui_->checkBox_detectMore_interSession->isChecked();
4260  bool useOptimizedGraphAsGuess = ui_->checkBox_opt_graph_as_guess->isChecked();
4261  if(!interSession && !intraSession)
4262  {
4263  QMessageBox::warning(this, tr("Cannot detect more loop closures"), tr("Intra and inter session parameters are disabled! Enable one or both."));
4264  return;
4265  }
4266 
4267  for(int n=0; n<iterations; ++n)
4268  {
4269  UINFO("iteration %d/%d", n+1, iterations);
4270  std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(
4271  std::map<int, Transform>(optimizedPoses.upper_bound(0), optimizedPoses.end()),
4272  ui_->doubleSpinBox_detectMore_radius->value(),
4273  ui_->doubleSpinBox_detectMore_angle->value()*CV_PI/180.0);
4274 
4275  progressDialog->setMaximumSteps(progressDialog->maximumSteps()+(int)clusters.size());
4276  progressDialog->appendText(tr("Looking for more loop closures, %1 clusters found.").arg(clusters.size()));
4277  QApplication::processEvents();
4278  if(progressDialog->isCanceled())
4279  {
4280  break;
4281  }
4282 
4283  std::set<int> addedLinks;
4284  int i=0;
4285  for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end() && !progressDialog->isCanceled(); ++iter, ++i)
4286  {
4287  int from = iter->first;
4288  int to = iter->second;
4289  if(from < to)
4290  {
4291  from = iter->second;
4292  to = iter->first;
4293  }
4294 
4295  int mapIdFrom = uValue(mapIds_, from, 0);
4296  int mapIdTo = uValue(mapIds_, to, 0);
4297 
4298  if((interSession && mapIdFrom != mapIdTo) ||
4299  (intraSession && mapIdFrom == mapIdTo))
4300  {
4301  // only add new links and one per cluster per iteration
4302  if(rtabmap::graph::findLink(checkedLoopClosures, from, to) == checkedLoopClosures.end())
4303  {
4304  if(!findActiveLink(from, to).isValid() && !containsLink(linksRemoved_, from, to) &&
4305  addedLinks.find(from) == addedLinks.end() &&
4306  addedLinks.find(to) == addedLinks.end())
4307  {
4308  // Reverify if in the bounds with the current optimized graph
4309  Transform delta = optimizedPoses.at(from).inverse() * optimizedPoses.at(to);
4310  if(delta.getNorm() < ui_->doubleSpinBox_detectMore_radius->value() &&
4311  delta.getNorm() >= ui_->doubleSpinBox_detectMore_radiusMin->value())
4312  {
4313  checkedLoopClosures.insert(std::make_pair(from, to));
4314  if(addConstraint(from, to, true, useOptimizedGraphAsGuess))
4315  {
4316  UINFO("Added new loop closure between %d and %d.", from, to);
4317  ++added;
4318  addedLinks.insert(from);
4319  addedLinks.insert(to);
4320  lastAdded.first = from;
4321  lastAdded.second = to;
4322 
4323  progressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
4324  QApplication::processEvents();
4325 
4326  optimizedPoses = graphes_.back();
4327  }
4328  }
4329  }
4330  }
4331  }
4332  progressDialog->incrementStep();
4333  if(i%100)
4334  {
4335  QApplication::processEvents();
4336  }
4337  }
4338  UINFO("Iteration %d/%d: added %d loop closures.", n+1, iterations, (int)addedLinks.size()/2);
4339  progressDialog->appendText(tr("Iteration %1/%2: Detected %3 loop closures!").arg(n+1).arg(iterations).arg(addedLinks.size()/2));
4340  if(addedLinks.size() == 0)
4341  {
4342  break;
4343  }
4344  if(n+1<iterations)
4345  {
4346  // Re-optimize the map before doing next iterations
4347  this->updateGraphView();
4348  optimizedPoses = graphes_.back();
4349  }
4350  }
4351 
4352  odomMaxInf_.clear();
4353 
4354  if(added)
4355  {
4356  this->updateGraphView();
4357  this->updateLoopClosuresSlider(lastAdded.first, lastAdded.second);
4358  }
4359  UINFO("Total added %d loop closures.", added);
4360 
4361  progressDialog->appendText(tr("Total new loop closures detected=%1").arg(added));
4362  progressDialog->setValue(progressDialog->maximumSteps());
4363 }
4364 
4365 void DatabaseViewer::updateAllNeighborCovariances()
4366 {
4367  std::multimap<int, Link> allLinks = updateLinksWithModifications(links_);
4368  QList<rtabmap::Link> links;
4369  for(std::multimap<int, Link>::iterator iter=allLinks.begin(); iter!=allLinks.end(); ++iter)
4370  {
4371  if(iter->second.type() == Link::kNeighbor ||
4372  iter->second.type() == Link::kNeighborMerged)
4373  {
4374  links.push_back(iter->second);
4375  }
4376  }
4377  updateCovariances(links);
4378 }
4379 void DatabaseViewer::updateAllLoopClosureCovariances()
4380 {
4381  std::multimap<int, Link> allLinks = updateLinksWithModifications(links_);
4382  QList<rtabmap::Link> links;
4383  for(std::multimap<int, Link>::iterator iter=allLinks.begin(); iter!=allLinks.end(); ++iter)
4384  {
4385  if(iter->second.type() != Link::kNeighbor &&
4386  iter->second.type() != Link::kNeighborMerged &&
4387  iter->second.type() != Link::kLandmark &&
4388  iter->second.from() != iter->second.to())
4389  {
4390  links.push_back(iter->second);
4391  }
4392  }
4393  updateCovariances(links);
4394 }
4395 void DatabaseViewer::updateAllLandmarkCovariances()
4396 {
4397  std::multimap<int, Link> allLinks = updateLinksWithModifications(links_);
4398  QList<rtabmap::Link> links;
4399  for(std::multimap<int, Link>::iterator iter=allLinks.begin(); iter!=allLinks.end(); ++iter)
4400  {
4401  if(iter->second.type() == Link::kLandmark)
4402  {
4403  links.push_back(iter->second);
4404  }
4405  }
4406  updateCovariances(links);
4407 }
4408 
4409 void DatabaseViewer::updateCovariances(const QList<Link> & links)
4410 {
4411  if(links.size())
4412  {
4413  cv::Mat infMatrix = links.first().infMatrix();
4414  EditConstraintDialog dialog(Transform::getIdentity(), infMatrix.inv());
4415  dialog.setPoseGroupVisible(false);
4416  if(dialog.exec() != QDialog::Accepted)
4417  {
4418  return;
4419  }
4420 
4421  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
4422  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
4423  progressDialog->setMaximumSteps(links.size());
4424  progressDialog->setCancelButtonVisible(true);
4425  progressDialog->setMinimumWidth(800);
4426  progressDialog->show();
4427 
4428  infMatrix = dialog.getCovariance().inv();
4429 
4430  for(int i=0; i<links.size(); ++i)
4431  {
4432  int from = links[i].from();
4433  int to = links[i].to();
4434 
4435  Link currentLink = findActiveLink(from, to);
4436  if(!currentLink.isValid())
4437  {
4438  UERROR("Not found link! (%d->%d)", from, to);
4439  return;
4440  }
4441  currentLink = Link(
4442  currentLink.from(),
4443  currentLink.to(),
4444  currentLink.type(),
4445  currentLink.transform(),
4446  infMatrix.clone(),
4447  currentLink.userDataCompressed());
4448  bool updated = false;
4449  std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
4450  while(iter != linksRefined_.end() && iter->first == currentLink.from())
4451  {
4452  if(iter->second.to() == currentLink.to() &&
4453  iter->second.type() == currentLink.type())
4454  {
4455  iter->second = currentLink;
4456  updated = true;
4457  break;
4458  }
4459  ++iter;
4460  }
4461  if(!updated)
4462  {
4463  linksRefined_.insert(std::make_pair(currentLink.from(), currentLink));
4464  }
4465 
4466  progressDialog->appendText(tr("Updated link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(links.size()));
4467  progressDialog->incrementStep();
4468  QApplication::processEvents();
4469  if(progressDialog->isCanceled())
4470  {
4471  break;
4472  }
4473  }
4474  this->updateGraphView();
4475 
4476  progressDialog->setValue(progressDialog->maximumSteps());
4477  progressDialog->appendText("Refining links finished!");
4478  }
4479 }
4480 
4481 void DatabaseViewer::refineLinks()
4482 {
4483  int minNodeId = 0;
4484  int maxNodeId = 0;
4485  int minMapId = 0;
4486  int maxMapId = 0;
4487  std::multimap<int, Link> allLinks = updateLinksWithModifications(links_);
4488  for(std::multimap<int, Link>::iterator iter=allLinks.begin(); iter!=allLinks.end(); ++iter)
4489  {
4490  if(iter->second.type() < Link::kPosePrior)
4491  {
4492  int minId = iter->second.from()>iter->second.to()?iter->second.to():iter->second.from();
4493  int maxId = iter->second.from()<iter->second.to()?iter->second.to():iter->second.from();
4494  if(minNodeId == 0 || minNodeId > minId)
4495  {
4496  minNodeId = minId;
4497  }
4498  if(maxNodeId == 0 || maxNodeId < maxId)
4499  {
4500  maxNodeId = maxId;
4501  }
4502  }
4503  }
4504  if(minNodeId > 0)
4505  {
4506  minMapId = uValue(mapIds_, minNodeId, 0);
4507  maxMapId = uValue(mapIds_, maxNodeId, minMapId);
4508 
4509  linkRefiningDialog_->setMinMax(
4510  minNodeId,
4511  maxNodeId,
4512  minMapId,
4513  maxMapId);
4514 
4515  if(linkRefiningDialog_->exec() == QDialog::Accepted)
4516  {
4517  QList<Link> links;
4518  Link::Type type = linkRefiningDialog_->getLinkType();
4519  linkRefiningDialog_->getRangeNodeId(minNodeId, maxNodeId);
4520  linkRefiningDialog_->getRangeNodeId(minMapId, maxMapId);
4521  bool intra, inter;
4522  linkRefiningDialog_->getIntraInterSessions(intra, inter);
4523  for(std::multimap<int, Link>::iterator iter=allLinks.begin(); iter!=allLinks.end(); ++iter)
4524  {
4525  if(iter->second.type() < Link::kPosePrior &&
4526  (type==Link::kEnd || type == iter->second.type()))
4527  {
4528  int from = iter->second.from();
4529  int to = iter->second.to();
4530  int mapFrom = uValue(mapIds_, from, 0);
4531  int mapTo = uValue(mapIds_, to, 0);
4532  if(((linkRefiningDialog_->isRangeByNodeId() &&
4533  ((from >= minNodeId && from <= maxNodeId) ||
4534  (to >= minNodeId && to <= maxNodeId))) ||
4535  (linkRefiningDialog_->isRangeByMapId() &&
4536  ((mapFrom >= minMapId && mapFrom <= maxMapId) ||
4537  (mapTo >= minMapId && mapTo <= maxMapId)))) &&
4538  ((intra && mapTo == mapFrom) ||
4539  (inter && mapTo != mapFrom)))
4540  {
4541  links.push_back(iter->second);
4542  }
4543  }
4544  }
4545  if(links.isEmpty())
4546  {
4547  QMessageBox::warning(this, tr("Refine links"), tr("No links found matching the requested parameters."));
4548  return;
4549  }
4550  else
4551  {
4552  refineLinks(links);
4553  }
4554  }
4555  }
4556  else
4557  {
4558  UWARN("No links can be refined!");
4559  }
4560 }
4561 void DatabaseViewer::refineLinks(const QList<Link> & links)
4562 {
4563  if(links.size())
4564  {
4565  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
4566  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
4567  progressDialog->setMaximumSteps(links.size());
4568  progressDialog->setCancelButtonVisible(true);
4569  progressDialog->setMinimumWidth(800);
4570  progressDialog->show();
4571 
4572  for(int i=0; i<links.size(); ++i)
4573  {
4574  int from = links[i].from();
4575  int to = links[i].to();
4576  if(from > 0 && to > 0)
4577  {
4578  this->refineConstraint(links[i].from(), links[i].to(), true);
4579  progressDialog->appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(links.size()));
4580  }
4581  else
4582  {
4583  progressDialog->appendText(tr("Ignored link %1->%2 (landmark)").arg(from).arg(to));
4584  }
4585  progressDialog->incrementStep();
4586  QApplication::processEvents();
4587  if(progressDialog->isCanceled())
4588  {
4589  break;
4590  }
4591  }
4592  this->updateGraphView();
4593 
4594  progressDialog->setValue(progressDialog->maximumSteps());
4595  progressDialog->appendText("Refining links finished!");
4596  }
4597 }
4598 
4599 void DatabaseViewer::resetAllChanges()
4600 {
4601  if(QMessageBox::question(this,
4602  tr("Reset all changes"),
4603  tr("You are about to reset all changes you've made so far, do you want to continue?"),
4604  QMessageBox::Yes | QMessageBox::No,
4605  QMessageBox::No) == QMessageBox::Yes)
4606  {
4607  linksAdded_.clear();
4608  linksRefined_.clear();
4609  linksRemoved_.clear();
4610  generatedLocalMaps_.clear();
4611  modifiedLaserScans_.clear();
4612  updateNeighborsSlider();
4613  updateLoopClosuresSlider();
4614  this->updateGraphView();
4615  }
4616 }
4617 
4618 void DatabaseViewer::graphNodeSelected(int id)
4619 {
4620  if(id>0 && idToIndex_.contains(id))
4621  {
4622  static bool updateA = true;
4623  if(updateA || ui_->actionConcise_Layout->isChecked())
4624  ui_->horizontalSlider_A->setValue(idToIndex_.value(id));
4625  else
4626  ui_->horizontalSlider_B->setValue(idToIndex_.value(id));
4627  updateA = !updateA || ui_->actionConcise_Layout->isChecked();
4628  }
4629 }
4630 
4631 void DatabaseViewer::graphLinkSelected(int from, int to)
4632 {
4633  if(from>0 && idToIndex_.contains(from))
4634  ui_->horizontalSlider_A->setValue(idToIndex_.value(from));
4635  if(to>0 && idToIndex_.contains(to))
4636  ui_->horizontalSlider_B->setValue(idToIndex_.value(to));
4637 }
4638 
4639 void DatabaseViewer::sliderAValueChanged(int value)
4640 {
4641  this->update(value,
4642  ui_->spinBox_indexA,
4643  ui_->label_parentsA,
4644  ui_->label_childrenA,
4645  ui_->label_weightA,
4646  ui_->label_labelA,
4647  ui_->label_stampA,
4648  ui_->graphicsView_A,
4649  ui_->label_idA,
4650  ui_->label_mapA,
4651  ui_->label_poseA,
4652  ui_->label_optposeA,
4653  ui_->label_velA,
4654  ui_->label_calibA,
4655  ui_->label_scanA,
4656  ui_->label_gravityA,
4657  ui_->label_priorA,
4658  ui_->toolButton_edit_priorA,
4659  ui_->toolButton_remove_priorA,
4660  ui_->label_gpsA,
4661  ui_->label_gtA,
4662  ui_->label_sensorsA,
4663  true);
4664 }
4665 
4666 void DatabaseViewer::sliderBValueChanged(int value)
4667 {
4668  this->update(value,
4669  ui_->spinBox_indexB,
4670  ui_->label_parentsB,
4671  ui_->label_childrenB,
4672  ui_->label_weightB,
4673  ui_->label_labelB,
4674  ui_->label_stampB,
4675  ui_->graphicsView_B,
4676  ui_->label_idB,
4677  ui_->label_mapB,
4678  ui_->label_poseB,
4679  ui_->label_optposeB,
4680  ui_->label_velB,
4681  ui_->label_calibB,
4682  ui_->label_scanB,
4683  ui_->label_gravityB,
4684  ui_->label_priorB,
4685  ui_->toolButton_edit_priorB,
4686  ui_->toolButton_remove_priorB,
4687  ui_->label_gpsB,
4688  ui_->label_gtB,
4689  ui_->label_sensorsB,
4690  true);
4691 }
4692 
4693 void DatabaseViewer::update(int value,
4694  QSpinBox * spinBoxIndex,
4695  QLabel * labelParents,
4696  QLabel * labelChildren,
4697  QLabel * weight,
4698  QLabel * label,
4699  QLabel * stamp,
4700  rtabmap::ImageView * view,
4701  QLabel * labelId,
4702  QLabel * labelMapId,
4703  QLabel * labelPose,
4704  QLabel * labelOptPose,
4705  QLabel * labelVelocity,
4706  QLabel * labelCalib,
4707  QLabel * labelScan,
4708  QLabel * labelGravity,
4709  QLabel * labelPrior,
4710  QToolButton * editPriorButton,
4711  QToolButton * removePriorButton,
4712  QLabel * labelGps,
4713  QLabel * labelGt,
4714  QLabel * labelSensors,
4715  bool updateConstraintView)
4716 {
4717  lastSliderIndexBrowsed_ = value;
4718 
4719  UTimer timer;
4720  spinBoxIndex->blockSignals(true);
4721  spinBoxIndex->setValue(value);
4722  spinBoxIndex->blockSignals(false);
4723  labelParents->clear();
4724  labelChildren->clear();
4725  weight->clear();
4726  label->clear();
4727  labelMapId->clear();
4728  labelPose->clear();
4729  labelOptPose->clear();
4730  labelVelocity->clear();
4731  stamp->clear();
4732  labelCalib->clear();
4733  labelScan ->clear();
4734  labelGravity->clear();
4735  labelPrior->clear();
4736  editPriorButton->setVisible(false);
4737  removePriorButton->setVisible(false);
4738  labelGps->clear();
4739  labelGt->clear();
4740  labelSensors->clear();
4741  if(value >= 0 && value < ids_.size())
4742  {
4743  view->clear();
4744  int id = ids_.at(value);
4745  int mapId = -1;
4746  labelId->setText(QString::number(id));
4747  if(id>0)
4748  {
4749  if(ui_->dockWidget_graphView->isVisible()) {
4750  ui_->graphViewer->highlightNode(id, spinBoxIndex==ui_->spinBox_indexB?1:0);
4751  }
4752 
4753  //image
4754  QImage img;
4755  cv::Mat imgDepth;
4756  if(dbDriver_)
4757  {
4758  SensorData data;
4759  dbDriver_->getNodeData(id, data);
4760  data.uncompressData();
4761  if(!data.imageRaw().empty())
4762  {
4763  img = uCvMat2QImage(data.imageRaw());
4764  }
4765  if(!data.depthOrRightRaw().empty())
4766  {
4767  cv::Mat depth =data.depthOrRightRaw();
4768  if(!data.depthRaw().empty())
4769  {
4770  if(ui_->spinBox_mesh_fillDepthHoles->value() > 0)
4771  {
4772  depth = util2d::fillDepthHoles(depth, ui_->spinBox_mesh_fillDepthHoles->value(), float(ui_->spinBox_mesh_depthError->value())/100.0f);
4773  }
4774  }
4775  if( !data.imageRaw().empty() &&
4776  !data.rightRaw().empty() &&
4777  data.stereoCameraModels().size()==1 && // Multiple stereo cameras not implemented
4778  data.stereoCameraModels()[0].isValidForProjection() &&
4779  ui_->checkBox_showDisparityInsteadOfRight->isChecked())
4780  {
4781  rtabmap::StereoDense * denseStereo = rtabmap::StereoDense::create(ui_->parameters_toolbox->getParameters());
4782  depth = util2d::depthFromDisparity(denseStereo->computeDisparity(data.imageRaw(), data.rightRaw()), data.stereoCameraModels()[0].left().fx(), data.stereoCameraModels()[0].baseline(), CV_32FC1);
4783  delete denseStereo;
4784  }
4785  imgDepth = depth;
4786  }
4787 
4788  QRectF rect;
4789  if(!img.isNull())
4790  {
4791  Transform pose;
4792  if(!graphes_.empty() && graphes_.back().find(data.id())!=graphes_.back().end())
4793  {
4794  pose = graphes_.back().at(data.id());
4795  }
4796  view->setImage(img, data.cameraModels(), pose);
4797  rect = img.rect();
4798  }
4799  else
4800  {
4801  ULOGGER_DEBUG("Image is empty");
4802  }
4803 
4804  if(!imgDepth.empty())
4805  {
4806  view->setImageDepth(imgDepth);
4807  if(img.isNull())
4808  {
4809  rect.setWidth(imgDepth.cols);
4810  rect.setHeight(imgDepth.rows);
4811  }
4812  }
4813  else
4814  {
4815  ULOGGER_DEBUG("Image depth is empty");
4816  }
4817  if(!rect.isValid())
4818  {
4819  if(data.cameraModels().size())
4820  {
4821  for(unsigned int i=0; i<data.cameraModels().size(); ++i)
4822  {
4823  rect.setWidth(rect.width()+data.cameraModels()[i].imageWidth());
4824  rect.setHeight(std::max((int)rect.height(), data.cameraModels()[i].imageHeight()));
4825  }
4826  }
4827  else if(data.stereoCameraModels().size())
4828  {
4829  for(unsigned int i=0; i<data.cameraModels().size(); ++i)
4830  {
4831  rect.setWidth(rect.width()+data.stereoCameraModels()[i].left().imageWidth());
4832  rect.setHeight(std::max((int)rect.height(), data.stereoCameraModels()[i].left().imageHeight()));
4833  }
4834  }
4835  }
4836  if(rect.isValid())
4837  {
4838  view->setSceneRect(rect);
4839  }
4840 
4841  std::list<int> ids;
4842  ids.push_back(id);
4843  std::list<Signature*> signatures;
4844  dbDriver_->loadSignatures(ids, signatures);
4845 
4846  if(signatures.size() && signatures.front()!=0 && !signatures.front()->getWordsKpts().empty())
4847  {
4848  std::multimap<int, cv::KeyPoint> keypoints;
4849  for(std::map<int, int>::const_iterator iter=signatures.front()->getWords().begin(); iter!=signatures.front()->getWords().end(); ++iter)
4850  {
4851  keypoints.insert(std::make_pair(iter->first, signatures.front()->getWordsKpts()[iter->second]));
4852  }
4853  view->setFeatures(keypoints, data.depthOrRightRaw().type() == CV_8UC1?cv::Mat():data.depthOrRightRaw(), Qt::yellow);
4854  }
4855 
4856  Transform odomPose, g;
4857  int w;
4858  std::string l;
4859  double s;
4860  std::vector<float> v;
4861  GPS gps;
4862  EnvSensors sensors;
4863  dbDriver_->getNodeInfo(id, odomPose, mapId, w, l, s, g, v, gps, sensors);
4864 
4865  weight->setNum(w);
4866  label->setText(l.c_str());
4867  float x,y,z,roll,pitch,yaw;
4868  odomPose.getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw);
4869  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));
4870  if(graphes_.size())
4871  {
4872  if(graphes_.back().find(id) == graphes_.back().end())
4873  {
4874  labelOptPose->setText("<Not in optimized graph>");
4875  }
4876  else
4877  {
4878  graphes_.back().find(id)->second.getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw);
4879  labelOptPose->setText(QString("xyz=(%1,%2,%3)\nrpy=(%4,%5,%6)").arg(x).arg(y).arg(z).arg(roll).arg(pitch).arg(yaw));
4880  }
4881  }
4882  if(s!=0.0)
4883  {
4884  stamp->setText(QString::number(s, 'f'));
4885  stamp->setToolTip(QDateTime::fromMSecsSinceEpoch(s*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
4886  }
4887  if(v.size()==6)
4888  {
4889  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]));
4890  }
4891 
4892  std::multimap<int, Link> gravityLink;
4893  dbDriver_->loadLinks(id, gravityLink, Link::kGravity);
4894  if(!gravityLink.empty())
4895  {
4896  Eigen::Vector3f v = gravityLink.begin()->second.transform().inverse().toEigen3f() * -Eigen::Vector3f::UnitZ();
4897  labelGravity->setText(QString("x=%1 y=%2 z=%3").arg(v[0]).arg(v[1]).arg(v[2]));
4898  labelGravity->setToolTip(QString("roll=%1 pitch=%2 yaw=%3").arg(roll).arg(pitch).arg(yaw));
4899  }
4900 
4901  std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
4902  if(graph::findLink(links, id, id, false, Link::kPosePrior)!=links.end())
4903  {
4904  Link & priorLink = graph::findLink(links, id, id, false, Link::kPosePrior)->second;
4905  priorLink.transform().getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw);
4906  labelPrior->setText(QString("xyz=(%1,%2,%3)\nrpy=(%4,%5,%6)").arg(x).arg(y).arg(z).arg(roll).arg(pitch).arg(yaw));
4907  std::stringstream out;
4908  out << priorLink.infMatrix().inv();
4909  labelPrior->setToolTip(QString("%1").arg(out.str().c_str()));
4910  removePriorButton->setVisible(true);
4911  editPriorButton->setToolTip(tr("Edit Prior"));
4912  editPriorButton->setText("...");
4913  editPriorButton->setVisible(true);
4914  }
4915  else if(!odomPose.isNull())
4916  {
4917  editPriorButton->setToolTip(tr("Add Prior"));
4918  editPriorButton->setText("+");
4919  editPriorButton->setVisible(true);
4920  }
4921 
4922  if(gps.stamp()>0.0)
4923  {
4924  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()));
4925  labelGps->setToolTip(QDateTime::fromMSecsSinceEpoch(gps.stamp()*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
4926  }
4927  if(!g.isNull())
4928  {
4929  labelGt->setText(QString("%1").arg(g.prettyPrint().c_str()));
4930  }
4931  if(sensors.size())
4932  {
4933  QString sensorsStr;
4934  QString tooltipStr;
4935  for(EnvSensors::iterator iter=sensors.begin(); iter!=sensors.end(); ++iter)
4936  {
4937  if(iter != sensors.begin())
4938  {
4939  sensorsStr += " | ";
4940  tooltipStr += " | ";
4941  }
4942 
4943  if(iter->first == EnvSensor::kWifiSignalStrength)
4944  {
4945  sensorsStr += uFormat("%.1f dbm", iter->second.value()).c_str();
4946  tooltipStr += "Wifi signal strength";
4947  }
4948  else if(iter->first == EnvSensor::kAmbientTemperature)
4949  {
4950  sensorsStr += uFormat("%.1f \u00B0C", iter->second.value()).c_str();
4951  tooltipStr += "Ambient Temperature";
4952  }
4953  else if(iter->first == EnvSensor::kAmbientAirPressure)
4954  {
4955  sensorsStr += uFormat("%.1f hPa", iter->second.value()).c_str();
4956  tooltipStr += "Ambient Air Pressure";
4957  }
4958  else if(iter->first == EnvSensor::kAmbientLight)
4959  {
4960  sensorsStr += uFormat("%.0f lx", iter->second.value()).c_str();
4961  tooltipStr += "Ambient Light";
4962  }
4963  else if(iter->first == EnvSensor::kAmbientRelativeHumidity)
4964  {
4965  sensorsStr += uFormat("%.0f %%", iter->second.value()).c_str();
4966  tooltipStr += "Ambient Relative Humidity";
4967  }
4968  else
4969  {
4970  sensorsStr += uFormat("%.2f", iter->second.value()).c_str();
4971  tooltipStr += QString("Type %1").arg((int)iter->first);
4972  }
4973 
4974  }
4975  labelSensors->setText(sensorsStr);
4976  labelSensors->setToolTip(tooltipStr);
4977  }
4978  if(data.cameraModels().size() || data.stereoCameraModels().size())
4979  {
4980  std::stringstream calibrationDetails;
4981  if(data.cameraModels().size())
4982  {
4983  if(!data.depthRaw().empty() && data.depthRaw().cols!=data.imageRaw().cols && data.imageRaw().cols)
4984  {
4985  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]")
4986  .arg(data.cameraModels().size())
4987  .arg(data.cameraModels()[0].imageWidth()>0?data.cameraModels()[0].imageWidth():data.imageRaw().cols/data.cameraModels().size())
4988  .arg(data.cameraModels()[0].imageHeight()>0?data.cameraModels()[0].imageHeight():data.imageRaw().rows)
4989  .arg(data.cameraModels()[0].fx())
4990  .arg(data.cameraModels()[0].fy())
4991  .arg(data.cameraModels()[0].cx())
4992  .arg(data.cameraModels()[0].cy())
4993  .arg(data.depthRaw().cols/data.cameraModels().size())
4994  .arg(data.depthRaw().rows)
4995  .arg(data.cameraModels()[0].localTransform().prettyPrint().c_str())
4996  .arg(data.cameraModels()[0].localTransform().r11()).arg(data.cameraModels()[0].localTransform().r12()).arg(data.cameraModels()[0].localTransform().r13()).arg(data.cameraModels()[0].localTransform().o14())
4997  .arg(data.cameraModels()[0].localTransform().r21()).arg(data.cameraModels()[0].localTransform().r22()).arg(data.cameraModels()[0].localTransform().r23()).arg(data.cameraModels()[0].localTransform().o24())
4998  .arg(data.cameraModels()[0].localTransform().r31()).arg(data.cameraModels()[0].localTransform().r32()).arg(data.cameraModels()[0].localTransform().r33()).arg(data.cameraModels()[0].localTransform().o34()));
4999  }
5000  else
5001  {
5002  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]")
5003  .arg(data.cameraModels().size())
5004  .arg(data.cameraModels()[0].imageWidth()>0?data.cameraModels()[0].imageWidth():data.imageRaw().cols/data.cameraModels().size())
5005  .arg(data.cameraModels()[0].imageHeight()>0?data.cameraModels()[0].imageHeight():data.imageRaw().rows)
5006  .arg(data.cameraModels()[0].fx())
5007  .arg(data.cameraModels()[0].fy())
5008  .arg(data.cameraModels()[0].cx())
5009  .arg(data.cameraModels()[0].cy())
5010  .arg(data.cameraModels()[0].localTransform().prettyPrint().c_str())
5011  .arg(data.cameraModels()[0].localTransform().r11()).arg(data.cameraModels()[0].localTransform().r12()).arg(data.cameraModels()[0].localTransform().r13()).arg(data.cameraModels()[0].localTransform().o14())
5012  .arg(data.cameraModels()[0].localTransform().r21()).arg(data.cameraModels()[0].localTransform().r22()).arg(data.cameraModels()[0].localTransform().r23()).arg(data.cameraModels()[0].localTransform().o24())
5013  .arg(data.cameraModels()[0].localTransform().r31()).arg(data.cameraModels()[0].localTransform().r32()).arg(data.cameraModels()[0].localTransform().r33()).arg(data.cameraModels()[0].localTransform().o34()));
5014  }
5015 
5016  for(unsigned int i=0; i<data.cameraModels().size();++i)
5017  {
5018  if(i!=0) calibrationDetails << std::endl;
5019  calibrationDetails << "Id: " << i << " Size=" << data.cameraModels()[i].imageWidth() << "x" << data.cameraModels()[i].imageHeight() << std::endl;
5020  if( data.cameraModels()[i].K_raw().total()) calibrationDetails << "K=" << data.cameraModels()[i].K_raw() << std::endl;
5021  if( data.cameraModels()[i].D_raw().total()) calibrationDetails << "D=" << data.cameraModels()[i].D_raw() << std::endl;
5022  if( data.cameraModels()[i].R().total()) calibrationDetails << "R=" << data.cameraModels()[i].R() << std::endl;
5023  if( data.cameraModels()[i].P().total()) calibrationDetails << "P=" << data.cameraModels()[i].P() << std::endl;
5024  calibrationDetails << "BaseToCam(without opt rot)=" << (data.cameraModels()[i].localTransform()*CameraModel::opticalRotation().inverse()).prettyPrint() << std::endl;
5025  }
5026 
5027  }
5028  else if(data.stereoCameraModels().size())
5029  {
5030  //stereo
5031  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]")
5032  .arg(data.stereoCameraModels()[0].left().imageWidth()>0?data.stereoCameraModels()[0].left().imageWidth():data.imageRaw().cols)
5033  .arg(data.stereoCameraModels()[0].left().imageHeight()>0?data.stereoCameraModels()[0].left().imageHeight():data.imageRaw().rows)
5034  .arg(data.stereoCameraModels()[0].left().fx())
5035  .arg(data.stereoCameraModels()[0].left().fy())
5036  .arg(data.stereoCameraModels()[0].left().cx())
5037  .arg(data.stereoCameraModels()[0].left().cy())
5038  .arg(data.stereoCameraModels()[0].baseline())
5039  .arg(data.stereoCameraModels()[0].localTransform().prettyPrint().c_str())
5040  .arg(data.stereoCameraModels()[0].localTransform().r11()).arg(data.stereoCameraModels()[0].localTransform().r12()).arg(data.stereoCameraModels()[0].localTransform().r13()).arg(data.stereoCameraModels()[0].localTransform().o14())
5041  .arg(data.stereoCameraModels()[0].localTransform().r21()).arg(data.stereoCameraModels()[0].localTransform().r22()).arg(data.stereoCameraModels()[0].localTransform().r23()).arg(data.stereoCameraModels()[0].localTransform().o24())
5042  .arg(data.stereoCameraModels()[0].localTransform().r31()).arg(data.stereoCameraModels()[0].localTransform().r32()).arg(data.stereoCameraModels()[0].localTransform().r33()).arg(data.stereoCameraModels()[0].localTransform().o34()));
5043 
5044  for(unsigned int i=0; i<data.stereoCameraModels().size();++i)
5045  {
5046  calibrationDetails << "Id: " << i << std::endl;
5047  calibrationDetails << " Left:" << " Size=" << data.stereoCameraModels()[i].left().imageWidth() << "x" << data.stereoCameraModels()[i].left().imageHeight() << std::endl;
5048  if( data.stereoCameraModels()[i].left().K_raw().total()) calibrationDetails << " K=" << data.stereoCameraModels()[i].left().K_raw() << std::endl;
5049  if( data.stereoCameraModels()[i].left().D_raw().total()) calibrationDetails << " D=" << data.stereoCameraModels()[i].left().D_raw() << std::endl;
5050  if( data.stereoCameraModels()[i].left().R().total()) calibrationDetails << " R=" << data.stereoCameraModels()[i].left().R() << std::endl;
5051  if( data.stereoCameraModels()[i].left().P().total()) calibrationDetails << " P=" << data.stereoCameraModels()[i].left().P() << std::endl;
5052  calibrationDetails << " Right:" << " Size=" << data.stereoCameraModels()[i].right().imageWidth() << "x" << data.stereoCameraModels()[i].right().imageHeight() << std::endl;
5053  if( data.stereoCameraModels()[i].right().K_raw().total()) calibrationDetails << " K=" << data.stereoCameraModels()[i].right().K_raw() << std::endl;
5054  if( data.stereoCameraModels()[i].right().D_raw().total()) calibrationDetails << " D=" << data.stereoCameraModels()[i].right().D_raw() << std::endl;
5055  if( data.stereoCameraModels()[i].right().R().total()) calibrationDetails << " R=" << data.stereoCameraModels()[i].right().R() << std::endl;
5056  if( data.stereoCameraModels()[i].right().P().total()) calibrationDetails << " P=" << data.stereoCameraModels()[i].right().P() << std::endl;
5057  if( data.stereoCameraModels()[i].R().total()) calibrationDetails << " R=" << data.stereoCameraModels()[i].R() << std::endl;
5058  if( data.stereoCameraModels()[i].T().total()) calibrationDetails << " T=" << data.stereoCameraModels()[i].T() << std::endl;
5059  if( data.stereoCameraModels()[i].F().total()) calibrationDetails << " F=" << data.stereoCameraModels()[i].F() << std::endl;
5060  if( data.stereoCameraModels()[i].E().total()) calibrationDetails << " E=" << data.stereoCameraModels()[i].E() << std::endl;
5061  calibrationDetails << "BaseToLeftCam(without opt rot)=" << (data.stereoCameraModels()[i].left().localTransform()*CameraModel::opticalRotation().inverse()).prettyPrint() << std::endl;
5062  }
5063  }
5064  labelCalib->setToolTip(calibrationDetails.str().c_str());
5065 
5066  }
5067  else
5068  {
5069  labelCalib->setText("NA");
5070  }
5071 
5072  if(data.laserScanRaw().size())
5073  {
5074  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] %13")
5075  .arg(data.laserScanRaw().formatName().c_str())
5076  .arg(data.laserScanRaw().size())
5077  .arg(data.laserScanRaw().maxPoints())
5078  .arg(data.laserScanRaw().rangeMin())
5079  .arg(data.laserScanRaw().rangeMax())
5080  .arg(data.laserScanRaw().angleMin())
5081  .arg(data.laserScanRaw().angleMax())
5082  .arg(data.laserScanRaw().angleIncrement())
5083  .arg(data.laserScanRaw().hasRGB()?1:0)
5084  .arg(data.laserScanRaw().is2d()?1:0)
5085  .arg(data.laserScanRaw().hasNormals()?1:0)
5086  .arg(data.laserScanRaw().hasIntensity()?1:0)
5087  .arg(data.laserScanRaw().localTransform().prettyPrint().c_str()));
5088  }
5089 
5090  //stereo
5091  if(!data.depthOrRightRaw().empty() && data.depthOrRightRaw().type() == CV_8UC1)
5092  {
5093  this->updateStereo(&data);
5094  }
5095  else
5096  {
5097  stereoViewer_->clear();
5098  ui_->graphicsView_stereo->clear();
5099  }
5100 
5101  // 3d view
5102  if(cloudViewer_->isVisible())
5103  {
5104  cloudViewer_->removeAllLines();
5105  cloudViewer_->removeAllFrustums();
5106  cloudViewer_->removeOccupancyGridMap();
5107  cloudViewer_->removeAllClouds();
5108  cloudViewer_->removeOctomap();
5109  cloudViewer_->removeElevationMap();
5110 
5111  Transform pose = Transform::getIdentity();
5112  if(signatures.size() && ui_->checkBox_odomFrame_3dview->isChecked())
5113  {
5114  float x, y, z, roll, pitch, yaw;
5115  (*signatures.begin())->getPose().getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
5116  pose = Transform(0,0,z,roll,pitch,0);
5117  }
5118 
5119  if(!gravityLink.empty() && ui_->checkBox_gravity_3dview->isChecked())
5120  {
5121  Transform gravityT = gravityLink.begin()->second.transform();
5122  Eigen::Vector3f gravity = gravityT.inverse().toEigen3f()*-Eigen::Vector3f::UnitZ();
5123  cloudViewer_->addOrUpdateLine("gravity", pose, pose*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0), Qt::yellow, true, false);
5124  }
5125 
5126  //add scan
5127  LaserScan laserScanRaw = data.laserScanRaw();
5128  if(modifiedLaserScans_.find(id)!=modifiedLaserScans_.end())
5129  {
5130  laserScanRaw = modifiedLaserScans_.at(id);
5131  }
5132  if(ui_->checkBox_showScan->isChecked() && laserScanRaw.size())
5133  {
5134  if(laserScanRaw.hasRGB() && laserScanRaw.hasNormals())
5135  {
5136  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr scan = util3d::laserScanToPointCloudRGBNormal(laserScanRaw, laserScanRaw.localTransform());
5137  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
5138  }
5139  else if(laserScanRaw.hasIntensity() && laserScanRaw.hasNormals())
5140  {
5141  pcl::PointCloud<pcl::PointXYZINormal>::Ptr scan = util3d::laserScanToPointCloudINormal(laserScanRaw, laserScanRaw.localTransform());
5142  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
5143  }
5144  else if(laserScanRaw.hasNormals())
5145  {
5146  pcl::PointCloud<pcl::PointNormal>::Ptr scan = util3d::laserScanToPointCloudNormal(laserScanRaw, laserScanRaw.localTransform());
5147  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
5148  }
5149  else if(laserScanRaw.hasRGB())
5150  {
5151  pcl::PointCloud<pcl::PointXYZRGB>::Ptr scan = util3d::laserScanToPointCloudRGB(laserScanRaw, laserScanRaw.localTransform());
5152  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
5153  }
5154  else if(laserScanRaw.hasIntensity())
5155  {
5156  pcl::PointCloud<pcl::PointXYZI>::Ptr scan = util3d::laserScanToPointCloudI(laserScanRaw, laserScanRaw.localTransform());
5157  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
5158  }
5159  else
5160  {
5161  pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(laserScanRaw, laserScanRaw.localTransform());
5162  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
5163  }
5164  }
5165 
5166  // add RGB-D cloud
5167  if(ui_->checkBox_showCloud->isChecked() && ui_->checkBox_cameraProjection->isChecked() &&
5168  !data.imageRaw().empty() && !laserScanRaw.empty() && !laserScanRaw.is2d())
5169  {
5170  pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud = util3d::laserScanToPointCloudINormal(laserScanRaw, laserScanRaw.localTransform());
5171  std::vector<CameraModel> models = data.cameraModels();
5172  if(!data.stereoCameraModels().empty())
5173  {
5174  models.clear();
5175  for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
5176  {
5177  models.push_back(data.stereoCameraModels()[i].left());
5178  }
5179  }
5180 
5181  if(!models.empty() && !models[0].isValidForProjection())
5182  {
5183  models.clear();
5184  }
5185 
5186  if(!models.empty() && models[0].imageWidth() != 0 && models[0].imageHeight() != 0)
5187  {
5188  std::map<int, Transform> cameraPoses;
5189  std::map<int, std::vector<CameraModel> > cameraModels;
5190  cameraPoses.insert(std::make_pair(data.id(), Transform::getIdentity()));
5191  cameraModels.insert(std::make_pair(data.id(), models));
5192 
5193  std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;
5194  pointToPixel = util3d::projectCloudToCameras(
5195  *cloud,
5196  cameraPoses,
5197  cameraModels);
5198  // color the cloud
5199  UASSERT(pointToPixel.empty() || pointToPixel.size() == cloud->size());
5200  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudValidPoints(new pcl::PointCloud<pcl::PointXYZRGB>);
5201  cloudValidPoints->resize(cloud->size());
5202  int oi=0;
5203  for(size_t i=0; i<pointToPixel.size(); ++i)
5204  {
5205  pcl::PointXYZINormal & pt = cloud->at(i);
5206  pcl::PointXYZRGB ptColor;
5207  int nodeID = pointToPixel[i].first.first;
5208  int cameraIndex = pointToPixel[i].first.second;
5209  if(nodeID>0 && cameraIndex>=0)
5210  {
5211  cv::Mat image = data.imageRaw();
5212 
5213  int subImageWidth = image.cols / cameraModels.at(nodeID).size();
5214  image = image(cv::Range::all(), cv::Range(cameraIndex*subImageWidth, (cameraIndex+1)*subImageWidth));
5215 
5216 
5217  int x = pointToPixel[i].second.x * (float)image.cols;
5218  int y = pointToPixel[i].second.y * (float)image.rows;
5219  UASSERT(x>=0 && x<image.cols);
5220  UASSERT(y>=0 && y<image.rows);
5221 
5222  if(image.type()==CV_8UC3)
5223  {
5224  cv::Vec3b bgr = image.at<cv::Vec3b>(y, x);
5225  ptColor.b = bgr[0];
5226  ptColor.g = bgr[1];
5227  ptColor.r = bgr[2];
5228  }
5229  else
5230  {
5231  UASSERT(image.type()==CV_8UC1);
5232  ptColor.r = ptColor.g = ptColor.b = image.at<unsigned char>(pointToPixel[i].second.y * image.rows, pointToPixel[i].second.x * image.cols);
5233  }
5234 
5235  ptColor.x = pt.x;
5236  ptColor.y = pt.y;
5237  ptColor.z = pt.z;
5238  cloudValidPoints->at(oi++) = ptColor;
5239  }
5240  }
5241  cloudValidPoints->resize(oi);
5242  if(!cloudValidPoints->empty())
5243  {
5244  if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
5245  {
5246  cloudValidPoints = util3d::voxelize(cloudValidPoints, ui_->doubleSpinBox_voxelSize->value());
5247  }
5248 
5249  cloudViewer_->addCloud("cloud", cloudValidPoints, pose);
5250  }
5251  else
5252  {
5253  UWARN("Camera projection to scan returned an empty cloud, no visible points from cameras...");
5254  }
5255  }
5256  else
5257  {
5258  UERROR("Node has invalid camera models, camera projection on scan cannot be done!.");
5259  }
5260  }
5261  else if(ui_->checkBox_showCloud->isChecked() || ui_->checkBox_showMesh->isChecked())
5262  {
5263  if(!data.depthOrRightRaw().empty())
5264  {
5265  if(!data.imageRaw().empty())
5266  {
5267  std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
5268  std::vector<pcl::IndicesPtr> allIndices;
5269  if(!data.depthRaw().empty() && data.cameraModels().size()==1)
5270  {
5271  cv::Mat depth = data.depthRaw();
5272  if(ui_->spinBox_mesh_fillDepthHoles->value() > 0)
5273  {
5274  depth = util2d::fillDepthHoles(depth, ui_->spinBox_mesh_fillDepthHoles->value(), float(ui_->spinBox_mesh_depthError->value())/100.0f);
5275  }
5276  pcl::IndicesPtr indices(new std::vector<int>);
5277  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
5278  data.imageRaw(),
5279  depth,
5280  data.cameraModels()[0],
5281  ui_->spinBox_decimation->value(),0,0,indices.get());
5282  if(indices->size())
5283  {
5284  clouds.push_back(util3d::transformPointCloud(cloud, data.cameraModels()[0].localTransform()));
5285  allIndices.push_back(indices);
5286  }
5287  }
5288  else
5289  {
5290  clouds = util3d::cloudsRGBFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, &allIndices, ui_->parameters_toolbox->getParameters());
5291  }
5292  UASSERT(clouds.size() == allIndices.size());
5293  for(size_t i=0; i<allIndices.size(); ++i)
5294  {
5295  if(allIndices[i]->size())
5296  {
5297  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = clouds[i];
5298  pcl::IndicesPtr indices = allIndices[i];
5299  if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
5300  {
5301  cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value());
5302  }
5303 
5304  if(ui_->checkBox_showMesh->isChecked() && !cloud->is_dense)
5305  {
5306  Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
5307  if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
5308  {
5309  viewpoint[0] = data.cameraModels()[0].localTransform().x();
5310  viewpoint[1] = data.cameraModels()[0].localTransform().y();
5311  viewpoint[2] = data.cameraModels()[0].localTransform().z();
5312  }
5313  else if(data.stereoCameraModels().size() && !data.stereoCameraModels()[0].localTransform().isNull())
5314  {
5315  viewpoint[0] = data.stereoCameraModels()[0].localTransform().x();
5316  viewpoint[1] = data.stereoCameraModels()[0].localTransform().y();
5317  viewpoint[2] = data.stereoCameraModels()[0].localTransform().z();
5318  }
5319  std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
5320  cloud,
5321  float(ui_->spinBox_mesh_angleTolerance->value())*M_PI/180.0f,
5322  ui_->checkBox_mesh_quad->isChecked(),
5323  ui_->spinBox_mesh_triangleSize->value(),
5324  viewpoint);
5325 
5326  if(ui_->spinBox_mesh_minClusterSize->value())
5327  {
5328  // filter polygons
5329  std::vector<std::set<int> > neighbors;
5330  std::vector<std::set<int> > vertexToPolygons;
5331  util3d::createPolygonIndexes(polygons,
5332  cloud->size(),
5333  neighbors,
5334  vertexToPolygons);
5335  std::list<std::list<int> > clusters = util3d::clusterPolygons(
5336  neighbors,
5337  ui_->spinBox_mesh_minClusterSize->value());
5338  std::vector<pcl::Vertices> filteredPolygons(polygons.size());
5339  int oi=0;
5340  for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
5341  {
5342  for(std::list<int>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
5343  {
5344  filteredPolygons[oi++] = polygons.at(*jter);
5345  }
5346  }
5347  filteredPolygons.resize(oi);
5348  polygons = filteredPolygons;
5349  }
5350 
5351  cloudViewer_->addCloudMesh(uFormat("mesh_%d", i), cloud, polygons, pose);
5352  }
5353  if(ui_->checkBox_showCloud->isChecked())
5354  {
5355  cloudViewer_->addCloud(uFormat("cloud_%d", i), cloud, pose);
5356  }
5357  }
5358  }
5359  }
5360  else if(ui_->checkBox_showCloud->isChecked())
5361  {
5362  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds;
5363  std::vector<pcl::IndicesPtr> allIndices;
5364 
5365  clouds = util3d::cloudsFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, &allIndices, ui_->parameters_toolbox->getParameters());
5366  UASSERT(clouds.size() == allIndices.size());
5367  for(size_t i=0; i<allIndices.size(); ++i)
5368  {
5369  if(allIndices[i]->size())
5370  {
5371  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = clouds[i];
5372  pcl::IndicesPtr indices = allIndices[i];
5373  if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
5374  {
5375  cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value());
5376  }
5377 
5378  cloudViewer_->addCloud(uFormat("cloud_%d", i), cloud, pose);
5379  }
5380  }
5381  }
5382  }
5383  }
5384 
5385  //frustums
5386  if(cloudViewer_->isFrustumShown())
5387  {
5388  if(data.cameraModels().size())
5389  {
5390  cloudViewer_->updateCameraFrustums(pose, data.cameraModels());
5391  }
5392  else
5393  {
5394  cloudViewer_->updateCameraFrustums(pose, data.stereoCameraModels());
5395  }
5396  }
5397 
5398  //words
5399  if(ui_->checkBox_showWords->isChecked() &&
5400  !signatures.empty() &&
5401  !(*signatures.begin())->getWords3().empty())
5402  {
5403  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
5404  cloud->resize((*signatures.begin())->getWords3().size());
5405  int i=0;
5406  for(std::multimap<int, int>::const_iterator iter=(*signatures.begin())->getWords().begin();
5407  iter!=(*signatures.begin())->getWords().end();
5408  ++iter)
5409  {
5410  const cv::Point3f & pt = (*signatures.begin())->getWords3()[iter->second];
5411  cloud->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
5412  }
5413 
5414  if(cloud->size())
5415  {
5416  cloud = rtabmap::util3d::removeNaNFromPointCloud(cloud);
5417  }
5418 
5419  if(cloud->size())
5420  {
5421  cloudViewer_->addCloud("words", cloud, pose, Qt::red);
5422  }
5423  }
5424 
5425  //add occupancy grid
5426  if(ui_->checkBox_showMap->isChecked() ||
5427  ui_->checkBox_showGrid->isChecked() ||
5428  ui_->checkBox_showElevation->checkState() != Qt::Unchecked)
5429  {
5430  LocalGridCache combinedLocalMaps;
5431  if(generatedLocalMaps_.shareTo(data.id(), combinedLocalMaps))
5432  {
5433  // add local grid
5434  }
5435  else if(!data.gridGroundCellsRaw().empty() || !data.gridObstacleCellsRaw().empty())
5436  {
5437  combinedLocalMaps.add(data.id(), data.gridGroundCellsRaw(), data.gridObstacleCellsRaw(), data.gridEmptyCellsRaw(), data.gridCellSize(), data.gridViewPoint());
5438  }
5439  if(!combinedLocalMaps.empty())
5440  {
5441  std::map<int, Transform> poses;
5442  poses.insert(std::make_pair(data.id(), pose));
5443 
5444 #ifdef RTABMAP_OCTOMAP
5445  OctoMap * octomap = 0;
5446  if(ui_->checkBox_octomap->isChecked() &&
5447  (!combinedLocalMaps.localGrids().begin()->second.groundCells.empty() || !combinedLocalMaps.localGrids().begin()->second.obstacleCells.empty()) &&
5448  combinedLocalMaps.localGrids().begin()->second.is3D() &&
5449  combinedLocalMaps.localGrids().begin()->second.cellSize > 0.0f)
5450  {
5451  //create local octomap
5452  ParametersMap params;
5453  params.insert(ParametersPair(Parameters::kGridCellSize(), uNumber2Str(combinedLocalMaps.localGrids().begin()->second.cellSize)));
5454  octomap = new OctoMap(&combinedLocalMaps, params);
5455  octomap->update(poses);
5456  }
5457 #endif
5458  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
5459  if(ui_->checkBox_showMap->isChecked())
5460  {
5461  float xMin=0.0f, yMin=0.0f;
5462  cv::Mat map8S;
5463  float gridCellSize = Parameters::defaultGridCellSize();
5464  Parameters::parse(parameters, Parameters::kGridCellSize(), gridCellSize);
5465  parameters = Parameters::filterParameters(parameters, "GridGlobal", true);
5466 
5467 #ifdef RTABMAP_OCTOMAP
5468  if(octomap)
5469  {
5470  map8S = octomap->createProjectionMap(xMin, yMin, gridCellSize, 0);
5471  }
5472  else
5473 #endif
5474  {
5475  OccupancyGrid grid(&combinedLocalMaps, parameters);
5476  grid.update(poses);
5477  map8S = grid.getMap(xMin, yMin);
5478  }
5479  if(!map8S.empty())
5480  {
5481  //convert to gray scaled map
5482  cloudViewer_->addOccupancyGridMap(util3d::convertMap2Image8U(map8S), gridCellSize, xMin, yMin, 1);
5483  }
5484  }
5485 
5486  if(ui_->checkBox_showGrid->isChecked())
5487  {
5488 #ifdef RTABMAP_OCTOMAP
5489  if(octomap)
5490  {
5491  if(ui_->comboBox_octomap_rendering_type->currentIndex()== 0)
5492  {
5493  pcl::IndicesPtr obstacles(new std::vector<int>);
5494  pcl::IndicesPtr empty(new std::vector<int>);
5495  pcl::IndicesPtr ground(new std::vector<int>);
5496  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(ui_->spinBox_grid_depth->value(), obstacles.get(), empty.get(), ground.get());
5497  if(octomap->hasColor())
5498  {
5499  pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
5500  pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
5501  cloudViewer_->addCloud("obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
5502  cloudViewer_->setCloudPointSize("obstacles", 5);
5503 
5504  pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
5505  pcl::copyPointCloud(*cloud, *ground, *groundCloud);
5506  cloudViewer_->addCloud("ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
5507  cloudViewer_->setCloudPointSize("ground", 5);
5508  }
5509  else
5510  {
5511  pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
5512  pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
5513  cloudViewer_->addCloud("obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
5514  cloudViewer_->setCloudPointSize("obstacles", 5);
5515 
5516  pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
5517  pcl::copyPointCloud(*cloud, *ground, *groundCloud);
5518  cloudViewer_->addCloud("ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
5519  cloudViewer_->setCloudPointSize("ground", 5);
5520  }
5521 
5522  if(ui_->checkBox_grid_empty->isChecked())
5523  {
5524  pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZ>);
5525  pcl::copyPointCloud(*cloud, *empty, *emptyCloud);
5526  cloudViewer_->addCloud("empty_cells", emptyCloud, Transform::getIdentity(), QColor(ui_->lineEdit_emptyColor->text()));
5527  cloudViewer_->setCloudOpacity("empty_cells", 0.5);
5528  cloudViewer_->setCloudPointSize("empty_cells", 5);
5529  }
5530  }
5531  else
5532  {
5533  cloudViewer_->addOctomap(octomap, ui_->spinBox_grid_depth->value(), ui_->comboBox_octomap_rendering_type->currentIndex()>1);
5534  }
5535  }
5536  else
5537 #endif
5538  {
5539  // occupancy cloud
5540  LaserScan scan = LaserScan::backwardCompatibility(combinedLocalMaps.localGrids().begin()->second.groundCells);
5541  if(scan.hasRGB())
5542  {
5543  cloudViewer_->addCloud("ground", util3d::laserScanToPointCloudRGB(scan), pose, QColor(ui_->lineEdit_groundColor->text()));
5544  }
5545  else
5546  {
5547  cloudViewer_->addCloud("ground", util3d::laserScanToPointCloud(scan), pose, QColor(ui_->lineEdit_groundColor->text()));
5548  }
5549  scan = LaserScan::backwardCompatibility(combinedLocalMaps.localGrids().begin()->second.obstacleCells);
5550  if(scan.hasRGB())
5551  {
5552  cloudViewer_->addCloud("obstacles", util3d::laserScanToPointCloudRGB(scan), pose, QColor(ui_->lineEdit_obstacleColor->text()));
5553  }
5554  else
5555  {
5556  cloudViewer_->addCloud("obstacles", util3d::laserScanToPointCloud(scan), pose, QColor(ui_->lineEdit_obstacleColor->text()));
5557  }
5558 
5559  cloudViewer_->setCloudPointSize("ground", 5);
5560  cloudViewer_->setCloudPointSize("obstacles", 5);
5561 
5562  if(ui_->checkBox_grid_empty->isChecked())
5563  {
5564  cloudViewer_->addCloud("empty_cells",
5565  util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(combinedLocalMaps.localGrids().begin()->second.emptyCells)),
5566  pose,
5567  QColor(ui_->lineEdit_emptyColor->text()));
5568  cloudViewer_->setCloudPointSize("empty_cells", 5);
5569  cloudViewer_->setCloudOpacity("empty_cells", 0.5);
5570  }
5571  }
5572  }
5573 #ifdef RTABMAP_OCTOMAP
5574  if(octomap)
5575  {
5576  delete octomap;
5577  }
5578 #endif
5579 
5580 #ifdef RTABMAP_GRIDMAP
5581  if(ui_->checkBox_showElevation->checkState() != Qt::Unchecked) // Show elevation map?
5582  {
5583  GridMap gridMap(&combinedLocalMaps, parameters);
5584 
5585  if(combinedLocalMaps.localGrids().begin()->second.is3D())
5586  {
5587  gridMap.update(poses);
5588  if(ui_->checkBox_showElevation->checkState() == Qt::PartiallyChecked)
5589  {
5590  float xMin, yMin, gridCellSize;
5591  cv::Mat elevationMap = gridMap.createHeightMap(xMin, yMin, gridCellSize);
5592  cloudViewer_->addElevationMap(elevationMap, gridCellSize, xMin, yMin, 1.0f);
5593  }
5594  else
5595  {
5596  pcl::PolygonMesh::Ptr mesh = gridMap.createTerrainMesh();
5597  cloudViewer_->addCloudMesh("elevation_mesh", mesh);
5598  }
5599  cloudViewer_->refreshView();
5600  }
5601  else
5602  {
5603  UWARN("Local grid is not 3D, cannot generate an elevation map");
5604  }
5605  }
5606 #endif
5607  }
5608  }
5609  cloudViewer_->updateCameraTargetPosition(pose);
5610  cloudViewer_->clearTrajectory();
5611  cloudViewer_->refreshView();
5612  }
5613 
5614  if(signatures.size())
5615  {
5616  UASSERT(signatures.front() != 0 && signatures.size() == 1);
5617  delete signatures.front();
5618  signatures.clear();
5619  }
5620  }
5621 
5622  // loops
5623  std::multimap<int, rtabmap::Link> links;
5624  dbDriver_->loadLinks(id, links);
5625  if(links.size())
5626  {
5627  QString strParents, strChildren;
5628  for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
5629  {
5630  if(iter->second.type() != Link::kNeighbor &&
5631  iter->second.type() != Link::kNeighborMerged)
5632  {
5633  if(iter->first < id)
5634  {
5635  strChildren.append(QString("%1 ").arg(iter->first));
5636  }
5637  else
5638  {
5639  strParents.append(QString("%1 ").arg(iter->first));
5640  }
5641  }
5642  }
5643  labelParents->setText(strParents);
5644  labelChildren->setText(strChildren);
5645  }
5646  }
5647 
5648  if(mapId>=0)
5649  {
5650  labelMapId->setText(QString::number(mapId));
5651  }
5652  }
5653  else if(value != 0)
5654  {
5655  ULOGGER_ERROR("Slider index out of range ?");
5656  }
5657 
5658  updateConstraintButtons();
5659  updateWordsMatching();
5660 
5661  if(updateConstraintView && ui_->dockWidget_constraints->isVisible())
5662  {
5663  // update constraint view
5664  int from = ids_.at(ui_->horizontalSlider_A->value());
5665  int to = ids_.at(ui_->horizontalSlider_B->value());
5666  bool set = false;
5667  for(int i=0; i<loopLinks_.size() || i<neighborLinks_.size(); ++i)
5668  {
5669  if(i < loopLinks_.size())
5670  {
5671  if((loopLinks_[i].from() == from && loopLinks_[i].to() == to) ||
5672  (loopLinks_[i].from() == to && loopLinks_[i].to() == from))
5673  {
5674  if(i != ui_->horizontalSlider_loops->value())
5675  {
5676  ui_->horizontalSlider_loops->blockSignals(true);
5677  ui_->horizontalSlider_loops->setValue(i);
5678  ui_->horizontalSlider_loops->blockSignals(false);
5679  this->updateConstraintView(loopLinks_[i].from() == from?loopLinks_.at(i):loopLinks_.at(i).inverse(), false);
5680  }
5681  ui_->horizontalSlider_neighbors->blockSignals(true);
5682  ui_->horizontalSlider_neighbors->setValue(0);
5683  ui_->horizontalSlider_neighbors->blockSignals(false);
5684  set = true;
5685  break;
5686  }
5687  }
5688  if(i < neighborLinks_.size())
5689  {
5690  if((neighborLinks_[i].from() == from && neighborLinks_[i].to() == to) ||
5691  (neighborLinks_[i].from() == to && neighborLinks_[i].to() == from))
5692  {
5693  if(i != ui_->horizontalSlider_neighbors->value())
5694  {
5695  ui_->horizontalSlider_neighbors->blockSignals(true);
5696  ui_->horizontalSlider_neighbors->setValue(i);
5697  ui_->horizontalSlider_neighbors->blockSignals(false);
5698  this->updateConstraintView(neighborLinks_[i].from() == from?neighborLinks_.at(i):neighborLinks_.at(i).inverse(), false);
5699  }
5700  ui_->horizontalSlider_loops->blockSignals(true);
5701  ui_->horizontalSlider_loops->setValue(0);
5702  ui_->horizontalSlider_loops->blockSignals(false);
5703  set = true;
5704  break;
5705  }
5706  }
5707  }
5708  if(!set)
5709  {
5710  ui_->horizontalSlider_loops->blockSignals(true);
5711  ui_->horizontalSlider_neighbors->blockSignals(true);
5712  ui_->horizontalSlider_loops->setValue(0);
5713  ui_->horizontalSlider_neighbors->setValue(0);
5714  ui_->horizontalSlider_loops->blockSignals(false);
5715  ui_->horizontalSlider_neighbors->blockSignals(false);
5716 
5717  constraintsViewer_->removeAllClouds();
5718 
5719  Link link = this->findActiveLink(from, to);
5720  bool constraintViewUpdated = false;
5721  if(link.isValid() && link.type() != Link::kGravity)
5722  {
5723  this->updateConstraintView(link, false);
5724  constraintViewUpdated = true;
5725  }
5726  else if(graphes_.size())
5727  {
5728  // make a fake link using globally optimized poses
5729  std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
5730  if(optimizedPoses.size() > 0)
5731  {
5732  std::map<int, Transform>::iterator fromIter = optimizedPoses.find(from);
5733  std::map<int, Transform>::iterator toIter = optimizedPoses.find(to);
5734  if(fromIter != optimizedPoses.end() &&
5735  toIter != optimizedPoses.end())
5736  {
5737  Link link(from, to, Link::kUndef, fromIter->second.inverse() * toIter->second);
5738  this->updateConstraintView(link, false);
5739  constraintViewUpdated = true;
5740  }
5741  }
5742  }
5743  if(!constraintViewUpdated)
5744  {
5745  ui_->label_constraint->clear();
5746  ui_->label_constraint_opt->clear();
5747  ui_->label_variance->clear();
5748  ui_->lineEdit_covariance->clear();
5749  ui_->label_type->clear();
5750  ui_->label_type_name->clear();
5751  ui_->checkBox_showOptimized->setEnabled(false);
5752  }
5753  constraintsViewer_->refreshView();
5754 
5755  }
5756  }
5757 }
5758 
5759 void DatabaseViewer::updateLoggerLevel()
5760 {
5761  if(this->parent() == 0)
5762  {
5763  ULogger::setLevel((ULogger::Level)ui_->comboBox_logger_level->currentIndex());
5764  }
5765 }
5766 
5767 void DatabaseViewer::updateStereo()
5768 {
5769  if(ui_->horizontalSlider_A->maximum())
5770  {
5771  int id = ids_.at(ui_->horizontalSlider_A->value());
5772  SensorData data;
5773  dbDriver_->getNodeData(id, data);
5774  data.uncompressData();
5775  updateStereo(&data);
5776  }
5777 }
5778 
5779 void DatabaseViewer::updateStereo(const SensorData * data)
5780 {
5781  if(data &&
5782  ui_->dockWidget_stereoView->isVisible() &&
5783  !data->imageRaw().empty() &&
5784  !data->depthOrRightRaw().empty() &&
5785  data->depthOrRightRaw().type() == CV_8UC1 &&
5786  data->stereoCameraModels().size()==1 && // Not implemented for multiple stereo cameras
5787  data->stereoCameraModels()[0].isValidForProjection())
5788  {
5789  cv::Mat leftMono;
5790  if(data->imageRaw().channels() == 3)
5791  {
5792  cv::cvtColor(data->imageRaw(), leftMono, CV_BGR2GRAY);
5793  }
5794  else
5795  {
5796  leftMono = data->imageRaw();
5797  }
5798 
5799  UTimer timer;
5800  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
5801  Stereo * stereo = Stereo::create(parameters);
5802 
5803  // generate kpts
5804  std::vector<cv::KeyPoint> kpts;
5805  uInsert(parameters, ParametersPair(Parameters::kKpMaxFeatures(), parameters.at(Parameters::kVisMaxFeatures())));
5806  uInsert(parameters, ParametersPair(Parameters::kKpSSC(), parameters.at(Parameters::kVisSSC())));
5807  uInsert(parameters, ParametersPair(Parameters::kKpMinDepth(), parameters.at(Parameters::kVisMinDepth())));
5808  uInsert(parameters, ParametersPair(Parameters::kKpMaxDepth(), parameters.at(Parameters::kVisMaxDepth())));
5809  uInsert(parameters, ParametersPair(Parameters::kKpDetectorStrategy(), parameters.at(Parameters::kVisFeatureType())));
5810  uInsert(parameters, ParametersPair(Parameters::kKpRoiRatios(), parameters.at(Parameters::kVisRoiRatios())));
5811  uInsert(parameters, ParametersPair(Parameters::kKpSubPixEps(), parameters.at(Parameters::kVisSubPixEps())));
5812  uInsert(parameters, ParametersPair(Parameters::kKpSubPixIterations(), parameters.at(Parameters::kVisSubPixIterations())));
5813  uInsert(parameters, ParametersPair(Parameters::kKpSubPixWinSize(), parameters.at(Parameters::kVisSubPixWinSize())));
5814  Feature2D * kptDetector = Feature2D::create(parameters);
5815  kpts = kptDetector->generateKeypoints(leftMono);
5816  delete kptDetector;
5817 
5818  float timeKpt = timer.ticks();
5819 
5820  std::vector<cv::Point2f> leftCorners;
5821  cv::KeyPoint::convert(kpts, leftCorners);
5822 
5823  // Find features in the new right image
5824  std::vector<unsigned char> status;
5825  std::vector<cv::Point2f> rightCorners;
5826 
5827  rightCorners = stereo->computeCorrespondences(
5828  leftMono,
5829  data->rightRaw(),
5830  leftCorners,
5831  status);
5832  delete stereo;
5833 
5834  float timeStereo = timer.ticks();
5835 
5836  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
5837  cloud->resize(kpts.size());
5838  float bad_point = std::numeric_limits<float>::quiet_NaN ();
5839  UASSERT(status.size() == kpts.size());
5840  int oi = 0;
5841  int inliers = 0;
5842  int flowOutliers= 0;
5843  int slopeOutliers= 0;
5844  int negativeDisparityOutliers = 0;
5845  for(unsigned int i=0; i<status.size(); ++i)
5846  {
5847  cv::Point3f pt(bad_point, bad_point, bad_point);
5848  if(status[i])
5849  {
5850  float disparity = leftCorners[i].x - rightCorners[i].x;
5851  if(disparity > 0.0f)
5852  {
5853  cv::Point3f tmpPt = util3d::projectDisparityTo3D(
5854  leftCorners[i],
5855  disparity,
5856  data->stereoCameraModels()[0]);
5857 
5858  if(util3d::isFinite(tmpPt))
5859  {
5860  pt = util3d::transformPoint(tmpPt, data->stereoCameraModels()[0].left().localTransform());
5861  status[i] = 100; //blue
5862  ++inliers;
5863  cloud->at(oi++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
5864  }
5865  }
5866  else
5867  {
5868  status[i] = 102; //magenta
5869  ++negativeDisparityOutliers;
5870  }
5871  }
5872  else
5873  {
5874  ++flowOutliers;
5875  }
5876  }
5877  cloud->resize(oi);
5878 
5879  UINFO("correspondences = %d/%d (%f) (time kpt=%fs stereo=%fs)",
5880  (int)cloud->size(), (int)leftCorners.size(), float(cloud->size())/float(leftCorners.size()), timeKpt, timeStereo);
5881 
5882  stereoViewer_->updateCameraTargetPosition(Transform::getIdentity());
5883  stereoViewer_->addCloud("stereo", cloud);
5884  stereoViewer_->refreshView();
5885 
5886  ui_->label_stereo_inliers->setNum(inliers);
5887  ui_->label_stereo_flowOutliers->setNum(flowOutliers);
5888  ui_->label_stereo_slopeOutliers->setNum(slopeOutliers);
5889  ui_->label_stereo_disparityOutliers->setNum(negativeDisparityOutliers);
5890 
5891  std::vector<cv::KeyPoint> rightKpts;
5892  cv::KeyPoint::convert(rightCorners, rightKpts);
5893  std::vector<cv::DMatch> good_matches(kpts.size());
5894  for(unsigned int i=0; i<good_matches.size(); ++i)
5895  {
5896  good_matches[i].trainIdx = i;
5897  good_matches[i].queryIdx = i;
5898  }
5899 
5900 
5901  //
5902  //cv::Mat imageMatches;
5903  //cv::drawMatches( leftMono, kpts, data->getDepthRaw(), rightKpts,
5904  // good_matches, imageMatches, cv::Scalar::all(-1), cv::Scalar::all(-1),
5905  // std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
5906 
5907  //ui_->graphicsView_stereo->setImage(uCvMat2QImage(imageMatches));
5908 
5909  ui_->graphicsView_stereo->clear();
5910  ui_->graphicsView_stereo->setLinesShown(true);
5911  ui_->graphicsView_stereo->setFeaturesShown(false);
5912  ui_->graphicsView_stereo->setImageDepthShown(true);
5913 
5914  ui_->graphicsView_stereo->setImage(uCvMat2QImage(data->imageRaw()));
5915  ui_->graphicsView_stereo->setImageDepth(data->depthOrRightRaw());
5916 
5917  // Draw lines between corresponding features...
5918  for(unsigned int i=0; i<kpts.size(); ++i)
5919  {
5920  if(rightKpts[i].pt.x > 0 && rightKpts[i].pt.y > 0)
5921  {
5922  QColor c = Qt::green;
5923  if(status[i] == 0)
5924  {
5925  c = Qt::red;
5926  }
5927  else if(status[i] == 100)
5928  {
5929  c = Qt::blue;
5930  }
5931  else if(status[i] == 101)
5932  {
5933  c = Qt::yellow;
5934  }
5935  else if(status[i] == 102)
5936  {
5937  c = Qt::magenta;
5938  }
5939  else if(status[i] == 110)
5940  {
5941  c = Qt::cyan;
5942  }
5943  ui_->graphicsView_stereo->addLine(
5944  kpts[i].pt.x,
5945  kpts[i].pt.y,
5946  rightKpts[i].pt.x,
5947  rightKpts[i].pt.y,
5948  c,
5949  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));
5950  }
5951  }
5952  ui_->graphicsView_stereo->update();
5953  }
5954 }
5955 
5956 void DatabaseViewer::updateWordsMatching(const std::vector<int> & inliers)
5957 {
5958  if(ui_->actionConcise_Layout->isChecked()) {
5959  return;
5960  }
5961 
5962  int from = ids_.at(ui_->horizontalSlider_A->value());
5963  int to = ids_.at(ui_->horizontalSlider_B->value());
5964  if(from && to)
5965  {
5966  ui_->graphicsView_A->clearLines();
5967  ui_->graphicsView_A->setFeaturesColor(ui_->graphicsView_A->getDefaultFeatureColor());
5968  ui_->graphicsView_B->clearLines();
5969  ui_->graphicsView_B->setFeaturesColor(ui_->graphicsView_B->getDefaultFeatureColor());
5970 
5971  const QMultiMap<int, KeypointItem*> & wordsA = ui_->graphicsView_A->getFeatures();
5972  const QMultiMap<int, KeypointItem*> & wordsB = ui_->graphicsView_B->getFeatures();
5973  std::set<int> inliersSet(inliers.begin(), inliers.end());
5974  if(wordsA.size() && wordsB.size())
5975  {
5976  QList<int> ids = wordsA.uniqueKeys();
5977  for(int i=0; i<ids.size(); ++i)
5978  {
5979  if(ids[i] > 0 && wordsA.count(ids[i]) == 1 && wordsB.count(ids[i]) == 1)
5980  {
5981  // Add lines
5982  // Draw lines between corresponding features...
5983  float scaleAX = ui_->graphicsView_A->viewScale();
5984  float scaleBX = ui_->graphicsView_B->viewScale();
5985 
5986  float marginAX = (ui_->graphicsView_A->width() - ui_->graphicsView_A->sceneRect().width()*scaleAX)/2.0f;
5987  float marginAY = (ui_->graphicsView_A->height() - ui_->graphicsView_A->sceneRect().height()*scaleAX)/2.0f;
5988  float marginBX = (ui_->graphicsView_B->width() - ui_->graphicsView_B->sceneRect().width()*scaleBX)/2.0f;
5989  float marginBY = (ui_->graphicsView_B->height() - ui_->graphicsView_B->sceneRect().height()*scaleBX)/2.0f;
5990 
5991  float deltaX = 0;
5992  float deltaY = 0;
5993 
5994  if(ui_->actionVertical_Layout->isChecked())
5995  {
5996  deltaY = ui_->graphicsView_A->height();
5997  }
5998  else
5999  {
6000  deltaX = ui_->graphicsView_A->width();
6001  }
6002 
6003  const KeypointItem * kptA = wordsA.value(ids[i]);
6004  const KeypointItem * kptB = wordsB.value(ids[i]);
6005 
6006  QColor cA = ui_->graphicsView_A->getDefaultMatchingLineColor();
6007  QColor cB = ui_->graphicsView_B->getDefaultMatchingLineColor();
6008  if(inliersSet.find(ids[i])!=inliersSet.end())
6009  {
6010  cA = ui_->graphicsView_A->getDefaultMatchingFeatureColor();
6011  cB = ui_->graphicsView_B->getDefaultMatchingFeatureColor();
6012  ui_->graphicsView_A->setFeatureColor(ids[i], ui_->graphicsView_A->getDefaultMatchingFeatureColor());
6013  ui_->graphicsView_B->setFeatureColor(ids[i], ui_->graphicsView_B->getDefaultMatchingFeatureColor());
6014  }
6015  else
6016  {
6017  ui_->graphicsView_A->setFeatureColor(ids[i], ui_->graphicsView_A->getDefaultMatchingLineColor());
6018  ui_->graphicsView_B->setFeatureColor(ids[i], ui_->graphicsView_B->getDefaultMatchingLineColor());
6019  }
6020 
6021  ui_->graphicsView_A->addLine(
6022  kptA->keypoint().pt.x,
6023  kptA->keypoint().pt.y,
6024  (kptB->keypoint().pt.x*scaleBX+marginBX+deltaX-marginAX)/scaleAX,
6025  (kptB->keypoint().pt.y*scaleBX+marginBY+deltaY-marginAY)/scaleAX,
6026  cA);
6027 
6028  ui_->graphicsView_B->addLine(
6029  (kptA->keypoint().pt.x*scaleAX+marginAX-deltaX-marginBX)/scaleBX,
6030  (kptA->keypoint().pt.y*scaleAX+marginAY-deltaY-marginBY)/scaleBX,
6031  kptB->keypoint().pt.x,
6032  kptB->keypoint().pt.y,
6033  cB);
6034  }
6035  }
6036  ui_->graphicsView_A->update();
6037  ui_->graphicsView_B->update();
6038  }
6039  }
6040 }
6041 
6042 void DatabaseViewer::sliderAMoved(int value)
6043 {
6044  ui_->spinBox_indexA->blockSignals(true);
6045  ui_->spinBox_indexA->setValue(value);
6046  ui_->spinBox_indexA->blockSignals(false);
6047  if(value>=0 && value < ids_.size())
6048  {
6049  ui_->label_idA->setText(QString::number(ids_.at(value)));
6050  }
6051  else
6052  {
6053  ULOGGER_ERROR("Slider index out of range ?");
6054  }
6055 }
6056 
6057 void DatabaseViewer::sliderBMoved(int value)
6058 {
6059  ui_->spinBox_indexB->blockSignals(true);
6060  ui_->spinBox_indexB->setValue(value);
6061  ui_->spinBox_indexB->blockSignals(false);
6062  if(value>=0 && value < ids_.size())
6063  {
6064  ui_->label_idB->setText(QString::number(ids_.at(value)));
6065  }
6066  else
6067  {
6068  ULOGGER_ERROR("Slider index out of range ?");
6069  }
6070 }
6071 
6072 void DatabaseViewer::update3dView()
6073 {
6074  if(lastSliderIndexBrowsed_ == ui_->horizontalSlider_B->value())
6075  {
6076  sliderBValueChanged(ui_->horizontalSlider_B->value());
6077  }
6078  else
6079  {
6080  sliderAValueChanged(ui_->horizontalSlider_A->value());
6081  }
6082 }
6083 
6084 void DatabaseViewer::sliderNeighborValueChanged(int value)
6085 {
6086  if(value < neighborLinks_.size())
6087  {
6088  this->updateConstraintView(neighborLinks_.at(value));
6089  }
6090 }
6091 
6092 void DatabaseViewer::sliderLoopValueChanged(int value)
6093 {
6094  if(value < loopLinks_.size())
6095  {
6096  this->updateConstraintView(loopLinks_.at(value));
6097  }
6098 }
6099 
6100 void DatabaseViewer::editConstraint()
6101 {
6102  if(ids_.size())
6103  {
6104  Link link(0,0,Link::kUndef, Transform::getIdentity());
6105  int priorId = sender() == ui_->toolButton_edit_priorA?ids_.at(ui_->horizontalSlider_A->value()):
6106  sender() == ui_->toolButton_edit_priorB?ids_.at(ui_->horizontalSlider_B->value()):0;
6107  if(priorId>0)
6108  {
6109  // Prior
6110  std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
6111  if(graph::findLink(links, priorId, priorId, false, Link::kPosePrior) != links.end())
6112  {
6113  link = graph::findLink(links, priorId, priorId, false, Link::kPosePrior)->second;
6114  }
6115  else if(odomPoses_.find(priorId) != odomPoses_.end())
6116  {
6117  // fallback to odom pose
6118  // set undef to go in "add" branch below
6119  link = Link(priorId, priorId, Link::kUndef, odomPoses_.at(priorId));
6120  }
6121  else
6122  {
6123  QMessageBox::warning(this, tr(""), tr("Node %1 doesn't have odometry pose, cannot add a prior for it!").arg(priorId));
6124  return;
6125  }
6126  }
6127  else if(ui_->label_type->text().toInt() == Link::kLandmark)
6128  {
6129  link = loopLinks_.at(ui_->horizontalSlider_loops->value());
6130  std::multimap<int, Link>::iterator findIter = rtabmap::graph::findLink(linksRefined_, link.from() ,link.to(), false, link.type());
6131  if(findIter != linksRefined_.end())
6132  {
6133  link = findIter->second;
6134  }
6135  }
6136  else
6137  {
6138  link = this->findActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value()));
6139  }
6140  bool updated = false;
6141  if(link.isValid())
6142  {
6143  cv::Mat covBefore = link.infMatrix().inv();
6144  EditConstraintDialog dialog(link.transform(), covBefore);
6145  if(dialog.exec() == QDialog::Accepted)
6146  {
6147  cv::Mat covariance = dialog.getCovariance();
6148  Link newLink(link.from(), link.to(), link.type(), dialog.getTransform(), covariance.inv());
6149  std::multimap<int, Link>::iterator iter = linksRefined_.find(link.from());
6150  while(iter != linksRefined_.end() && iter->first == link.from())
6151  {
6152  if(iter->second.to() == link.to() &&
6153  iter->second.type() == link.type())
6154  {
6155  iter->second = newLink;
6156  updated = true;
6157  break;
6158  }
6159  ++iter;
6160  }
6161  if(!updated)
6162  {
6163  linksRefined_.insert(std::make_pair(newLink.from(), newLink));
6164  updated = true;
6165  }
6166  if(priorId==0)
6167  {
6168  this->updateGraphView();
6169  updateConstraintView();
6170  }
6171  }
6172  }
6173  else
6174  {
6175  EditConstraintDialog dialog(
6176  link.transform(), cv::Mat::eye(6,6,CV_64FC1) * (priorId>0?0.00001:1));
6177  if(dialog.exec() == QDialog::Accepted)
6178  {
6179  cv::Mat covariance = dialog.getCovariance();
6180  int from = priorId>0?priorId:ids_.at(ui_->horizontalSlider_A->value());
6181  int to = priorId>0?priorId:ids_.at(ui_->horizontalSlider_B->value());
6182  Link newLink(
6183  from,
6184  to,
6185  priorId>0?Link::kPosePrior:Link::kUserClosure,
6186  dialog.getTransform(),
6187  covariance.inv());
6188  if(newLink.from() < newLink.to())
6189  {
6190  newLink = newLink.inverse();
6191  }
6192  linksAdded_.insert(std::make_pair(newLink.from(), newLink));
6193  updated = true;
6194  if(priorId==0)
6195  {
6196  this->updateGraphView();
6197  updateLoopClosuresSlider(from, to);
6198  }
6199  }
6200  }
6201 
6202  if(updated && priorId>0)
6203  {
6204  bool priorsIgnored = Parameters::defaultOptimizerPriorsIgnored();
6205  Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kOptimizerPriorsIgnored(), priorsIgnored);
6206  if(priorsIgnored)
6207  {
6208  if(QMessageBox::question(this,
6209  tr("Updating Prior"),
6210  tr("Parameter %1 is true, do you want to turn it off to update the graph with the updated prior?").arg(Parameters::kOptimizerPriorsIgnored().c_str()),
6211  QMessageBox::Yes | QMessageBox::No,
6212  QMessageBox::Yes) == QMessageBox::Yes)
6213  {
6214  priorsIgnored = false;
6215  ui_->parameters_toolbox->updateParameter(Parameters::kOptimizerPriorsIgnored(), "false");
6216  }
6217  }
6218  int indexA = ui_->horizontalSlider_A->value();
6219  int indexB = ui_->horizontalSlider_B->value();
6220  if(!priorsIgnored)
6221  {
6222  this->updateGraphView();
6223  }
6224  if(ui_->horizontalSlider_A->value() != indexA)
6225  ui_->horizontalSlider_A->setValue(indexA);
6226  else
6227  sliderAValueChanged(indexA);
6228  if(ui_->horizontalSlider_B->value() != indexB)
6229  ui_->horizontalSlider_B->setValue(indexB);
6230  else
6231  sliderBValueChanged(indexB);
6232  }
6233  }
6234 }
6235 
6236 // only called when ui_->checkBox_showOptimized state changed
6237 void DatabaseViewer::updateConstraintView()
6238 {
6239  if(ids_.size())
6240  {
6241  Link link = this->findActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value()));
6242  if(link.isValid())
6243  {
6244  UDEBUG("link %d->%d type=%d", link.from(), link.to(), link.type());
6245  if((link.type() == Link::kNeighbor ||
6246  link.type() == Link::kNeighborMerged) &&
6247  ui_->horizontalSlider_neighbors->value() >= 0 && ui_->horizontalSlider_neighbors->value() < neighborLinks_.size())
6248  {
6249  this->updateConstraintView(neighborLinks_.at(ui_->horizontalSlider_neighbors->value()), false);
6250  }
6251  else if((link.type() != Link::kPosePrior || link.type() != Link::kGravity) &&
6252  ui_->horizontalSlider_loops->value() >= 0 && ui_->horizontalSlider_loops->value() < loopLinks_.size())
6253  {
6254  this->updateConstraintView(loopLinks_.at(ui_->horizontalSlider_loops->value()), false);
6255  }
6256  else
6257  {
6258  this->updateConstraintView(link, false);
6259  }
6260  }
6261  }
6262 }
6263 
6264 void DatabaseViewer::updateConstraintView(
6265  const rtabmap::Link & linkIn,
6266  bool updateImageSliders,
6267  const Signature & signatureFrom,
6268  const Signature & signatureTo)
6269 {
6270  UDEBUG("%d -> %d", linkIn.from(), linkIn.to());
6271  std::multimap<int, Link>::iterator iterLink = rtabmap::graph::findLink(linksRefined_, linkIn.from(), linkIn.to());
6272  rtabmap::Link link = linkIn;
6273 
6274  if(iterLink != linksRefined_.end())
6275  {
6276  if(iterLink->second.from() == link.to())
6277  {
6278  link = iterLink->second.inverse();
6279  }
6280  else
6281  {
6282  link = iterLink->second;
6283  }
6284  }
6285  else if(ui_->checkBox_ignorePoseCorrection->isChecked())
6286  {
6287  if(link.type() == Link::kNeighbor ||
6288  link.type() == Link::kNeighborMerged)
6289  {
6290  Transform poseFrom = uValue(odomPoses_, link.from(), Transform());
6291  Transform poseTo = uValue(odomPoses_, link.to(), Transform());
6292  if(!poseFrom.isNull() && !poseTo.isNull())
6293  {
6294  // recompute raw odom transformation and
6295  // reset to identity covariance
6296  link = Link(link.from(),
6297  link.to(),
6298  link.type(),
6299  poseFrom.inverse() * poseTo);
6300  }
6301  }
6302  }
6303  UDEBUG("%d -> %d", link.from(), link.to());
6304  rtabmap::Transform t = link.transform();
6305  if(link.type() == Link::kGravity)
6306  {
6307  // remove yaw, keep only roll and pitch
6308  float roll, pitch, yaw;
6309  t.getEulerAngles(roll, pitch, yaw);
6310  t = Transform(0,0,0,roll,pitch,0);
6311  }
6312 
6313  ui_->label_constraint->clear();
6314  ui_->label_constraint_opt->clear();
6315  ui_->checkBox_showOptimized->setEnabled(false);
6316  UASSERT(!t.isNull() && dbDriver_);
6317 
6318  ui_->label_type->setText(QString::number(link.type()));
6319  ui_->label_type_name->setText(tr("(%1)")
6320  .arg(link.type()==Link::kNeighbor?"Neighbor":
6321  link.type()==Link::kNeighborMerged?"Merged neighbor":
6322  link.type()==Link::kGlobalClosure?"Loop closure":
6323  link.type()==Link::kLocalSpaceClosure?"Space proximity link":
6324  link.type()==Link::kLocalTimeClosure?"Time proximity link":
6325  link.type()==Link::kUserClosure?"User link":
6326  link.type()==Link::kLandmark?"Landmark "+QString::number(-link.to()):
6327  link.type()==Link::kVirtualClosure?"Virtual link":
6328  link.type()==Link::kGravity?"Gravity link":"Undefined"));
6329  ui_->label_variance->setText(QString("%1, %2")
6330  .arg(sqrt(link.transVariance()))
6331  .arg(sqrt(link.rotVariance())));
6332  std::stringstream out;
6333  out << link.infMatrix().inv();
6334  ui_->lineEdit_covariance->setText(out.str().c_str());
6335  ui_->label_constraint->setText(QString("%1").arg(t.prettyPrint().c_str()).replace(" ", "\n"));
6336  if(graphes_.size() &&
6337  (int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
6338  {
6339  std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
6340 
6341  std::map<int, rtabmap::Transform>::iterator iterFrom = graph.find(link.from());
6342  std::map<int, rtabmap::Transform>::iterator iterTo = graph.find(link.to());
6343  if(iterFrom != graph.end() && iterTo != graph.end())
6344  {
6345  ui_->checkBox_showOptimized->setEnabled(true);
6346  Transform topt = iterFrom->second.inverse()*iterTo->second;
6347  float diff = topt.getDistance(t);
6348  Transform v1 = t.rotation()*Transform(1,0,0,0,0,0);
6349  Transform v2 = topt.rotation()*Transform(1,0,0,0,0,0);
6350  float a = pcl::getAngle3D(Eigen::Vector4f(v1.x(), v1.y(), v1.z(), 0), Eigen::Vector4f(v2.x(), v2.y(), v2.z(), 0));
6351  a = (a *180.0f) / CV_PI;
6352  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));
6353 
6354  if(ui_->checkBox_showOptimized->isChecked())
6355  {
6356  t = topt;
6357  }
6358  }
6359  }
6360 
6361  if(updateImageSliders)
6362  {
6363  ui_->horizontalSlider_A->blockSignals(true);
6364  ui_->horizontalSlider_B->blockSignals(true);
6365  // set from on left and to on right
6366  if(link.from()>0)
6367  ui_->horizontalSlider_A->setValue(idToIndex_.value(link.from()));
6368  if(link.to() > 0)
6369  ui_->horizontalSlider_B->setValue(idToIndex_.value(link.to()));
6370  ui_->horizontalSlider_A->blockSignals(false);
6371  ui_->horizontalSlider_B->blockSignals(false);
6372  if(link.from()>0)
6373  this->update(idToIndex_.value(link.from()),
6374  ui_->spinBox_indexA,
6375  ui_->label_parentsA,
6376  ui_->label_childrenA,
6377  ui_->label_weightA,
6378  ui_->label_labelA,
6379  ui_->label_stampA,
6380  ui_->graphicsView_A,
6381  ui_->label_idA,
6382  ui_->label_mapA,
6383  ui_->label_poseA,
6384  ui_->label_optposeA,
6385  ui_->label_velA,
6386  ui_->label_calibA,
6387  ui_->label_scanA,
6388  ui_->label_gravityA,
6389  ui_->label_priorA,
6390  ui_->toolButton_edit_priorA,
6391  ui_->toolButton_remove_priorA,
6392  ui_->label_gpsA,
6393  ui_->label_gtA,
6394  ui_->label_sensorsA,
6395  false); // don't update constraints view!
6396  if(link.to()>0)
6397  {
6398  this->update(idToIndex_.value(link.to()),
6399  ui_->spinBox_indexB,
6400  ui_->label_parentsB,
6401  ui_->label_childrenB,
6402  ui_->label_weightB,
6403  ui_->label_labelB,
6404  ui_->label_stampB,
6405  ui_->graphicsView_B,
6406  ui_->label_idB,
6407  ui_->label_mapB,
6408  ui_->label_poseB,
6409  ui_->label_optposeB,
6410  ui_->label_velB,
6411  ui_->label_calibB,
6412  ui_->label_scanB,
6413  ui_->label_gravityB,
6414  ui_->label_priorB,
6415  ui_->toolButton_edit_priorB,
6416  ui_->toolButton_remove_priorB,
6417  ui_->label_gpsB,
6418  ui_->label_gtB,
6419  ui_->label_sensorsB,
6420  false); // don't update constraints view!
6421  }
6422  }
6423 
6424  if(constraintsViewer_->isVisible())
6425  {
6426  SensorData dataFrom, dataTo;
6427 
6428  if(signatureFrom.id()>0)
6429  {
6430  dataFrom = signatureFrom.sensorData();
6431  }
6432  else
6433  {
6434  dbDriver_->getNodeData(link.from(), dataFrom);
6435  }
6436  dataFrom.uncompressData();
6437  UASSERT(dataFrom.imageRaw().empty() || dataFrom.imageRaw().type()==CV_8UC3 || dataFrom.imageRaw().type() == CV_8UC1);
6438  UASSERT(dataFrom.depthOrRightRaw().empty() || dataFrom.depthOrRightRaw().type()==CV_8UC1 || dataFrom.depthOrRightRaw().type() == CV_16UC1 || dataFrom.depthOrRightRaw().type() == CV_32FC1);
6439 
6440  if(signatureTo.id()>0)
6441  {
6442  dataTo = signatureTo.sensorData();
6443  }
6444  else if(link.to()>0)
6445  {
6446  dbDriver_->getNodeData(link.to(), dataTo);
6447  }
6448  dataTo.uncompressData();
6449  UASSERT(dataTo.imageRaw().empty() || dataTo.imageRaw().type()==CV_8UC3 || dataTo.imageRaw().type() == CV_8UC1);
6450  UASSERT(dataTo.depthOrRightRaw().empty() || dataTo.depthOrRightRaw().type()==CV_8UC1 || dataTo.depthOrRightRaw().type() == CV_16UC1 || dataTo.depthOrRightRaw().type() == CV_32FC1);
6451 
6452  // get odom pose
6453  Transform pose = Transform::getIdentity();
6454  if(ui_->checkBox_odomFrame->isChecked())
6455  {
6456  int m,w;
6457  std::string l;
6458  double s;
6459  Transform p,g;
6460  std::vector<float> v;
6461  GPS gps;
6462  EnvSensors sensors;
6463  dbDriver_->getNodeInfo(link.from(), p, m, w, l, s, g, v, gps, sensors);
6464  if(!p.isNull())
6465  {
6466  // keep just the z and roll/pitch rotation
6467  float x, y, z, roll, pitch, yaw;
6468  p.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
6469  pose = Transform(0,0,z,roll,pitch,0);
6470  }
6471  }
6472 
6473  constraintsViewer_->removeCloud("cloud0");
6474  constraintsViewer_->removeCloud("cloud1");
6475  //cloud 3d
6476  if(ui_->checkBox_show3Dclouds->isChecked())
6477  {
6478  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom, cloudTo;
6479  pcl::IndicesPtr indicesFrom(new std::vector<int>);
6480  pcl::IndicesPtr indicesTo(new std::vector<int>);
6481  if(!dataFrom.imageRaw().empty() && !dataFrom.depthOrRightRaw().empty())
6482  {
6483  cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, ui_->spinBox_decimation->value(), 0, 0, indicesFrom.get(), ui_->parameters_toolbox->getParameters());
6484  }
6485  if(!dataTo.imageRaw().empty() && !dataTo.depthOrRightRaw().empty())
6486  {
6487  cloudTo=util3d::cloudRGBFromSensorData(dataTo, ui_->spinBox_decimation->value(), 0, 0, indicesTo.get(), ui_->parameters_toolbox->getParameters());
6488  }
6489 
6490  if(cloudTo.get() && indicesTo->size())
6491  {
6492  cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t);
6493  }
6494 
6495  // Gain compensation
6496  if(ui_->doubleSpinBox_gainCompensationRadius->value()>0.0 &&
6497  cloudFrom.get() && indicesFrom->size() &&
6498  cloudTo.get() && indicesTo->size())
6499  {
6500  UTimer t;
6501  GainCompensator compensator(ui_->doubleSpinBox_gainCompensationRadius->value());
6502  compensator.feed(cloudFrom, indicesFrom, cloudTo, indicesTo, Transform::getIdentity());
6503  compensator.apply(0, cloudFrom, indicesFrom);
6504  compensator.apply(1, cloudTo, indicesTo);
6505  UINFO("Gain compensation time = %fs", t.ticks());
6506  }
6507 
6508  if(cloudFrom.get() && indicesFrom->size())
6509  {
6510  if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
6511  {
6512  cloudFrom = util3d::voxelize(cloudFrom, indicesFrom, ui_->doubleSpinBox_voxelSize->value());
6513  }
6514  constraintsViewer_->addCloud("cloud0", cloudFrom, pose, Qt::red);
6515  }
6516  if(cloudTo.get() && indicesTo->size())
6517  {
6518  if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
6519  {
6520  cloudTo = util3d::voxelize(cloudTo, indicesTo, ui_->doubleSpinBox_voxelSize->value());
6521  }
6522  constraintsViewer_->addCloud("cloud1", cloudTo, pose, Qt::cyan);
6523  }
6524  }
6525 
6526  constraintsViewer_->removeCloud("words0");
6527  constraintsViewer_->removeCloud("words1");
6528  if(ui_->checkBox_show3DWords->isChecked())
6529  {
6530  std::list<int> ids;
6531  ids.push_back(link.from());
6532  if(link.to()>0)
6533  {
6534  ids.push_back(link.to());
6535  }
6536  std::list<Signature*> signatures;
6537  dbDriver_->loadSignatures(ids, signatures);
6538  if(signatures.size() == 2 || (link.to()<0 && signatures.size()==1))
6539  {
6540  const Signature * sFrom = signatureFrom.id()>0?&signatureFrom:signatures.front();
6541  const Signature * sTo = 0;
6542  if(signatures.size()==2)
6543  {
6544  sTo = signatureTo.id()>0?&signatureTo:signatures.back();
6545  UASSERT(sTo);
6546  }
6547  UASSERT(sFrom);
6548  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom(new pcl::PointCloud<pcl::PointXYZ>);
6549  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo(new pcl::PointCloud<pcl::PointXYZ>);
6550  cloudFrom->resize(sFrom->getWords3().size());
6551  if(sTo)
6552  {
6553  cloudTo->resize(sTo->getWords3().size());
6554  }
6555  int i=0;
6556  if(!sFrom->getWords3().empty())
6557  {
6558  for(std::multimap<int, int>::const_iterator iter=sFrom->getWords().begin();
6559  iter!=sFrom->getWords().end();
6560  ++iter)
6561  {
6562  const cv::Point3f & pt = sFrom->getWords3()[iter->second];
6563  cloudFrom->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
6564  }
6565  }
6566  i=0;
6567  if(sTo && !sTo->getWords3().empty())
6568  {
6569  for(std::multimap<int, int>::const_iterator iter=sTo->getWords().begin();
6570  iter!=sTo->getWords().end();
6571  ++iter)
6572  {
6573  const cv::Point3f & pt = sTo->getWords3()[iter->second];
6574  cloudTo->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
6575  }
6576  }
6577 
6578  if(cloudFrom->size())
6579  {
6580  cloudFrom = rtabmap::util3d::removeNaNFromPointCloud(cloudFrom);
6581  }
6582  if(cloudTo->size())
6583  {
6584  cloudTo = rtabmap::util3d::removeNaNFromPointCloud(cloudTo);
6585  if(cloudTo->size())
6586  {
6587  cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t);
6588  }
6589  }
6590 
6591  if(cloudFrom->size())
6592  {
6593  constraintsViewer_->addCloud("words0", cloudFrom, pose, Qt::red);
6594  }
6595  else
6596  {
6597  UWARN("Empty 3D words for node %d", link.from());
6598  constraintsViewer_->removeCloud("words0");
6599  }
6600  if(cloudTo->size())
6601  {
6602  constraintsViewer_->addCloud("words1", cloudTo, pose, Qt::cyan);
6603  }
6604  else
6605  {
6606  if(sTo)
6607  {
6608  UWARN("Empty 3D words for node %d", link.to());
6609  }
6610  constraintsViewer_->removeCloud("words1");
6611  }
6612  }
6613  else
6614  {
6615  UERROR("Not found signature %d or %d in RAM", link.from(), link.to());
6616  constraintsViewer_->removeCloud("words0");
6617  constraintsViewer_->removeCloud("words1");
6618  }
6619  //cleanup
6620  for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
6621  {
6622  delete *iter;
6623  }
6624  }
6625 
6626  constraintsViewer_->removeCloud("scan2");
6627  constraintsViewer_->removeGraph("scan2graph");
6628  constraintsViewer_->removeCloud("scan0");
6629  constraintsViewer_->removeCloud("scan1");
6630  if(ui_->checkBox_show2DScans->isChecked())
6631  {
6632  //cloud 2d
6633  if(link.type() == Link::kLocalSpaceClosure &&
6634  !link.userDataCompressed().empty() &&
6635  signatureTo.id()==0)
6636  {
6637  std::vector<int> ids;
6638  cv::Mat userData = link.uncompressUserDataConst();
6639  if(userData.type() == CV_8SC1 &&
6640  userData.rows == 1 &&
6641  userData.cols >= 8 && // including null str ending
6642  userData.at<char>(userData.cols-1) == 0 &&
6643  memcmp(userData.data, "SCANS:", 6) == 0)
6644  {
6645  std::string scansStr = (const char *)userData.data;
6646  UINFO("Detected \"%s\" in links's user data", scansStr.c_str());
6647  if(!scansStr.empty())
6648  {
6649  std::list<std::string> strs = uSplit(scansStr, ':');
6650  if(strs.size() == 2)
6651  {
6652  std::list<std::string> strIds = uSplit(strs.rbegin()->c_str(), ';');
6653  for(std::list<std::string>::iterator iter=strIds.begin(); iter!=strIds.end(); ++iter)
6654  {
6655  ids.push_back(atoi(iter->c_str()));
6656  if(ids.back() == link.from())
6657  {
6658  ids.pop_back();
6659  }
6660  }
6661  }
6662  }
6663  }
6664  if(ids.size())
6665  {
6666  //add other scans matching
6667  //optimize the path's poses locally
6668 
6669  std::map<int, rtabmap::Transform> poses;
6670  for(unsigned int i=0; i<ids.size(); ++i)
6671  {
6672  if(uContains(odomPoses_, ids[i]))
6673  {
6674  poses.insert(*odomPoses_.find(ids[i]));
6675  }
6676  else
6677  {
6678  UERROR("Not found %d node!", ids[i]);
6679  }
6680  }
6681  if(poses.size())
6682  {
6683  Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
6684 
6685  UASSERT(uContains(poses, link.to()));
6686  std::map<int, rtabmap::Transform> posesOut;
6687  std::multimap<int, rtabmap::Link> linksOut;
6688  optimizer->getConnectedGraph(
6689  link.to(),
6690  poses,
6691  updateLinksWithModifications(links_),
6692  posesOut,
6693  linksOut);
6694 
6695  if(poses.size() != posesOut.size())
6696  {
6697  UWARN("Scan poses input and output are different! %d vs %d", (int)poses.size(), (int)posesOut.size());
6698  }
6699 
6700  UDEBUG("Input poses: ");
6701  for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
6702  {
6703  UDEBUG(" %d=%s", iter->first, iter->second.prettyPrint().c_str());
6704  }
6705  UDEBUG("Input links: ");
6706  for(std::multimap<int, Link>::iterator iter=linksOut.begin(); iter!=linksOut.end(); ++iter)
6707  {
6708  UDEBUG(" %d->%d (type=%s) %s", iter->second.from(), iter->second.to(), iter->second.typeName().c_str(), iter->second.transform().prettyPrint().c_str());
6709  }
6710 
6711  std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(link.to(), posesOut, linksOut);
6712  delete optimizer;
6713 
6714  UDEBUG("Output poses: ");
6715  for(std::map<int, Transform>::iterator iter=finalPoses.begin(); iter!=finalPoses.end(); ++iter)
6716  {
6717  UDEBUG(" %d=%s", iter->first, iter->second.prettyPrint().c_str());
6718  }
6719 
6720  // transform local poses in loop referential
6721  Transform u = t * finalPoses.at(link.to()).inverse();
6722  pcl::PointCloud<pcl::PointXYZ>::Ptr assembledScans(new pcl::PointCloud<pcl::PointXYZ>);
6723  pcl::PointCloud<pcl::PointNormal>::Ptr assembledNormalScans(new pcl::PointCloud<pcl::PointNormal>);
6724  pcl::PointCloud<pcl::PointXYZI>::Ptr assembledIScans(new pcl::PointCloud<pcl::PointXYZI>);
6725  pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembledINormalScans(new pcl::PointCloud<pcl::PointXYZINormal>);
6726  pcl::PointCloud<pcl::PointXYZ>::Ptr graph(new pcl::PointCloud<pcl::PointXYZ>);
6727  for(std::map<int, Transform>::iterator iter=finalPoses.begin(); iter!=finalPoses.end(); ++iter)
6728  {
6729  iter->second = u * iter->second;
6730  if(iter->first != link.to()) // already added to view
6731  {
6732  //create scan
6733  SensorData data;
6734  dbDriver_->getNodeData(iter->first, data);
6735  LaserScan scan;
6736  data.uncompressDataConst(0, 0, &scan, 0);
6737  if(!scan.isEmpty())
6738  {
6739  if(scan.hasNormals() && scan.hasIntensity())
6740  {
6741  *assembledINormalScans += *util3d::laserScanToPointCloudINormal(scan, iter->second*scan.localTransform());
6742  }
6743  else if(scan.hasNormals())
6744  {
6745  *assembledNormalScans += *util3d::laserScanToPointCloudNormal(scan, iter->second*scan.localTransform());
6746  }
6747  else if(scan.hasIntensity())
6748  {
6749  *assembledIScans += *util3d::laserScanToPointCloudI(scan, iter->second*scan.localTransform());
6750  }
6751  else
6752  {
6753  *assembledScans += *util3d::laserScanToPointCloud(scan, iter->second*scan.localTransform());
6754  }
6755  }
6756  }
6757  graph->push_back(util3d::transformPoint(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()), pose));
6758  }
6759 
6760  if(assembledNormalScans->size())
6761  {
6762  constraintsViewer_->addCloud("scan2", assembledNormalScans, pose, Qt::cyan);
6763  constraintsViewer_->setCloudColorIndex("scan2", 2);
6764  }
6765  if(assembledScans->size())
6766  {
6767  constraintsViewer_->addCloud("scan2", assembledScans, pose, Qt::cyan);
6768  constraintsViewer_->setCloudColorIndex("scan2", 2);
6769  }
6770  if(assembledINormalScans->size())
6771  {
6772  constraintsViewer_->addCloud("scan2", assembledINormalScans, pose, Qt::cyan);
6773  constraintsViewer_->setCloudColorIndex("scan2", 2);
6774  }
6775  if(assembledIScans->size())
6776  {
6777  constraintsViewer_->addCloud("scan2", assembledIScans, pose, Qt::cyan);
6778  constraintsViewer_->setCloudColorIndex("scan2", 2);
6779  }
6780  if(graph->size())
6781  {
6782  constraintsViewer_->addOrUpdateGraph("scan2graph", graph, Qt::cyan);
6783  }
6784  }
6785  }
6786  }
6787 
6788  // Added loop closure scans
6789  constraintsViewer_->removeCloud("scan0");
6790  constraintsViewer_->removeCloud("scan1");
6791  if(!dataFrom.laserScanRaw().isEmpty())
6792  {
6793  if(dataFrom.laserScanRaw().hasNormals() && dataFrom.laserScanRaw().hasIntensity())
6794  {
6795  pcl::PointCloud<pcl::PointXYZINormal>::Ptr scan;
6796  scan = rtabmap::util3d::laserScanToPointCloudINormal(dataFrom.laserScanRaw(), dataFrom.laserScanRaw().localTransform());
6797  constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
6798  constraintsViewer_->setCloudColorIndex("scan0", 2);
6799  }
6800  else if(dataFrom.laserScanRaw().hasNormals())
6801  {
6802  pcl::PointCloud<pcl::PointNormal>::Ptr scan;
6803  scan = rtabmap::util3d::laserScanToPointCloudNormal(dataFrom.laserScanRaw(), dataFrom.laserScanRaw().localTransform());
6804  constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
6805  constraintsViewer_->setCloudColorIndex("scan0", 2);
6806  }
6807  else if(dataFrom.laserScanRaw().hasIntensity())
6808  {
6809  pcl::PointCloud<pcl::PointXYZI>::Ptr scan;
6810  scan = rtabmap::util3d::laserScanToPointCloudI(dataFrom.laserScanRaw(), dataFrom.laserScanRaw().localTransform());
6811  constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
6812  constraintsViewer_->setCloudColorIndex("scan0", 2);
6813  }
6814  else
6815  {
6816  pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
6817  scan = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw(), dataFrom.laserScanRaw().localTransform());
6818  constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
6819  constraintsViewer_->setCloudColorIndex("scan0", 2);
6820  }
6821  }
6822  if(!dataTo.laserScanRaw().isEmpty())
6823  {
6824  if(dataTo.laserScanRaw().hasNormals() && dataTo.laserScanRaw().hasIntensity())
6825  {
6826  pcl::PointCloud<pcl::PointXYZINormal>::Ptr scan;
6827  scan = rtabmap::util3d::laserScanToPointCloudINormal(dataTo.laserScanRaw(), t*dataTo.laserScanRaw().localTransform());
6828  constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
6829  constraintsViewer_->setCloudColorIndex("scan1", 2);
6830  }
6831  else if(dataTo.laserScanRaw().hasNormals())
6832  {
6833  pcl::PointCloud<pcl::PointNormal>::Ptr scan;
6834  scan = rtabmap::util3d::laserScanToPointCloudNormal(dataTo.laserScanRaw(), t*dataTo.laserScanRaw().localTransform());
6835  constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
6836  constraintsViewer_->setCloudColorIndex("scan1", 2);
6837  }
6838  else if(dataTo.laserScanRaw().hasIntensity())
6839  {
6840  pcl::PointCloud<pcl::PointXYZI>::Ptr scan;
6841  scan = rtabmap::util3d::laserScanToPointCloudI(dataTo.laserScanRaw(), t*dataTo.laserScanRaw().localTransform());
6842  constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
6843  constraintsViewer_->setCloudColorIndex("scan1", 2);
6844  }
6845  else
6846  {
6847  pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
6848  scan = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw(), t*dataTo.laserScanRaw().localTransform());
6849  constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
6850  constraintsViewer_->setCloudColorIndex("scan1", 2);
6851  }
6852  }
6853  }
6854 
6855  //frustums
6856  constraintsViewer_->removeAllFrustums();
6857  if(constraintsViewer_->isFrustumShown())
6858  {
6859  CameraModel model;
6860  if(dataFrom.cameraModels().size())
6861  {
6862  model = dataFrom.cameraModels()[0];
6863  }
6864  else if(dataFrom.stereoCameraModels().size())
6865  {
6866  model = dataFrom.stereoCameraModels()[0].left();
6867  }
6868  constraintsViewer_->addOrUpdateFrustum("frustum_from", pose, model.localTransform(), constraintsViewer_->getFrustumScale(), constraintsViewer_->getFrustumColor(), model.fovX(), model.fovY());
6869  model = CameraModel();
6870  if(dataTo.cameraModels().size())
6871  {
6872  model = dataTo.cameraModels()[0];
6873  }
6874  else if(dataTo.stereoCameraModels().size())
6875  {
6876  model = dataTo.stereoCameraModels()[0].left();
6877  }
6878  constraintsViewer_->addOrUpdateFrustum("frustum_to", pose*t, model.localTransform(), constraintsViewer_->getFrustumScale(), constraintsViewer_->getFrustumColor(), model.fovX(), model.fovY());
6879  }
6880 
6881  //update coordinate
6882  constraintsViewer_->addOrUpdateCoordinate("from_coordinate", pose, 0.2);
6883 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
6884  constraintsViewer_->addOrUpdateCoordinate("to_coordinate", pose*t, 0.2);
6885  constraintsViewer_->removeCoordinate("to_coordinate_gt");
6886  if(uContains(groundTruthPoses_, link.from()) && uContains(groundTruthPoses_, link.to()))
6887  {
6888  constraintsViewer_->addOrUpdateCoordinate("to_coordinate_gt",
6889  pose*(groundTruthPoses_.at(link.from()).inverse()*groundTruthPoses_.at(link.to())), 0.1);
6890  }
6891 #endif
6892 
6893  constraintsViewer_->clearTrajectory();
6894 
6895  constraintsViewer_->refreshView();
6896  }
6897 
6898  // update buttons
6899  updateConstraintButtons();
6900 }
6901 
6902 void DatabaseViewer::updateConstraintButtons()
6903 {
6904  ui_->pushButton_refine->setEnabled(false);
6905  ui_->pushButton_reset->setEnabled(false);
6906  ui_->pushButton_add->setEnabled(false);
6907  ui_->pushButton_reject->setEnabled(false);
6908  ui_->toolButton_constraint->setEnabled(false);
6909 
6910  Link currentLink;
6911  int from;
6912  int to;
6913  if(ui_->label_type->text().toInt() == Link::kLandmark)
6914  {
6915  //check for modified link
6916  currentLink = loopLinks_.at(ui_->horizontalSlider_loops->value());
6917  from = currentLink.from();
6918  to = currentLink.to();
6919  }
6920  else
6921  {
6922  from = ids_.at(ui_->horizontalSlider_A->value());
6923  to = ids_.at(ui_->horizontalSlider_B->value());
6924  if(from!=to && from && to &&
6925  odomPoses_.find(from) != odomPoses_.end() &&
6926  odomPoses_.find(to) != odomPoses_.end() &&
6927  (ui_->checkBox_enableForAll->isChecked() ||
6928  (weights_.find(from) != weights_.end() && weights_.at(from)>=0 &&
6929  weights_.find(to) != weights_.end() && weights_.at(to)>=0)))
6930  {
6931  if((!containsLink(links_, from ,to) && !containsLink(linksAdded_, from ,to)) ||
6932  containsLink(linksRemoved_, from ,to))
6933  {
6934  ui_->pushButton_add->setEnabled(true);
6935  }
6936  }
6937  else if(ui_->checkBox_enableForAll->isChecked())
6938  {
6939  if(odomPoses_.find(from) == odomPoses_.end())
6940  {
6941  UWARN("Button \"Add\" cannot be enabled even if \"all\" checkbox is checked, as node %d doesn't have odometry set.", from);
6942  }
6943  else if(odomPoses_.find(to) == odomPoses_.end())
6944  {
6945  UWARN("Button \"Add\" cannot be enabled even if \"all\" checkbox is checked, as node %d doesn't have odometry set.", to);
6946  }
6947  }
6948 
6949  currentLink = findActiveLink(from ,to);
6950  }
6951 
6952  if(currentLink.isValid() &&
6953  ((currentLink.from() == from && currentLink.to() == to) || (currentLink.from() == to && currentLink.to() == from)))
6954  {
6955  if(!containsLink(linksRemoved_, from ,to))
6956  {
6957  ui_->pushButton_reject->setEnabled(true);
6958  }
6959 
6960  //check for modified link
6961  std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, currentLink.from(), currentLink.to());
6962  if(iter != linksRefined_.end())
6963  {
6964  currentLink = iter->second;
6965  ui_->pushButton_reset->setEnabled(true);
6966  }
6967  ui_->pushButton_refine->setEnabled(currentLink.from()!=currentLink.to() && currentLink.type() != Link::kLandmark);
6968  ui_->toolButton_constraint->setEnabled(true);
6969  }
6970 }
6971 
6972 void DatabaseViewer::sliderIterationsValueChanged(int value)
6973 {
6974  if(dbDriver_ && value >=0 && value < (int)graphes_.size())
6975  {
6976  std::map<int, rtabmap::Transform> graph = uValueAt(graphes_, value);
6977 
6978  std::map<int, Transform> refPoses = groundTruthPoses_;
6979  bool alignToGPS = ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked();
6980  if(alignToGPS)
6981  {
6982  refPoses = gpsPoses_;
6983  }
6984 
6985  // Log ground truth statistics (in TUM's RGBD-SLAM format)
6986  if(refPoses.size())
6987  {
6988  // compute KITTI statistics before aligning the poses
6989  float length = graph::computePathLength(graph);
6990  if(refPoses.size() == graph.size() && length >= 100.0f)
6991  {
6992  float t_err = 0.0f;
6993  float r_err = 0.0f;
6994  graph::calcKittiSequenceErrors(uValues(refPoses), uValues(graph), t_err, r_err);
6995  UINFO("KITTI t_err = %f %%", t_err);
6996  UINFO("KITTI r_err = %f deg/m", r_err);
6997  }
6998 
6999  float translational_rmse = 0.0f;
7000  float translational_mean = 0.0f;
7001  float translational_median = 0.0f;
7002  float translational_std = 0.0f;
7003  float translational_min = 0.0f;
7004  float translational_max = 0.0f;
7005  float rotational_rmse = 0.0f;
7006  float rotational_mean = 0.0f;
7007  float rotational_median = 0.0f;
7008  float rotational_std = 0.0f;
7009  float rotational_min = 0.0f;
7010  float rotational_max = 0.0f;
7011 
7012  Transform gtToMap = graph::calcRMSE(
7013  refPoses,
7014  graph,
7015  translational_rmse,
7016  translational_mean,
7017  translational_median,
7018  translational_std,
7019  translational_min,
7020  translational_max,
7021  rotational_rmse,
7022  rotational_mean,
7023  rotational_median,
7024  rotational_std,
7025  rotational_min,
7026  rotational_max,
7027  alignToGPS);
7028 
7029  // ground truth live statistics
7030  ui_->label_rmse->setNum(translational_rmse);
7031  UINFO("translational_rmse=%f", translational_rmse);
7032  UINFO("translational_mean=%f", translational_mean);
7033  UINFO("translational_median=%f", translational_median);
7034  UINFO("translational_std=%f", translational_std);
7035  UINFO("translational_min=%f", translational_min);
7036  UINFO("translational_max=%f", translational_max);
7037 
7038  UINFO("rotational_rmse=%f", rotational_rmse);
7039  UINFO("rotational_mean=%f", rotational_mean);
7040  UINFO("rotational_median=%f", rotational_median);
7041  UINFO("rotational_std=%f", rotational_std);
7042  UINFO("rotational_min=%f", rotational_min);
7043  UINFO("rotational_max=%f", rotational_max);
7044 
7045  if((alignToGPS ||
7046  (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked())) &&
7047  !gtToMap.isIdentity())
7048  {
7049  for(std::map<int, Transform>::iterator iter=graph.begin(); iter!=graph.end(); ++iter)
7050  {
7051  iter->second = gtToMap * iter->second;
7052  }
7053  }
7054  }
7055 
7056  std::map<int, rtabmap::Transform> graphFiltered;
7057  if(ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() &&
7058  ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() &&
7059  !groundTruthPoses_.empty())
7060  {
7061  graphFiltered = groundTruthPoses_;
7062  }
7063  else
7064  {
7065  graphFiltered = graph;
7066  }
7067  if(ui_->groupBox_posefiltering->isChecked())
7068  {
7069  graphFiltered = graph::radiusPosesFiltering(graph,
7070  ui_->doubleSpinBox_posefilteringRadius->value(),
7071  ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
7072  }
7073  LocalGridCache combinedLocalMaps;
7074 #ifdef RTABMAP_OCTOMAP
7075  delete octomap_;
7076  octomap_ = 0;
7077 #endif
7078  if(ui_->dockWidget_graphView->isVisible() || ui_->dockWidget_occupancyGridView->isVisible())
7079  {
7080  //update scans
7081  UINFO("Update local maps list...");
7082  std::vector<int> ids = uKeys(graphFiltered);
7083  for(unsigned int i=0; i<ids.size(); ++i)
7084  {
7085  if(generatedLocalMaps_.shareTo(ids[i], combinedLocalMaps) ||
7086  localMaps_.shareTo(ids[i], combinedLocalMaps))
7087  {
7088  // Added to combined maps
7089  }
7090  else if(ids.at(i)>0)
7091  {
7092  SensorData data;
7093  dbDriver_->getNodeData(ids.at(i), data, false, false, false);
7094  cv::Mat ground, obstacles, empty;
7095  if(data.gridCellSize()>0.0f)
7096  {
7097  data.uncompressData(0, 0, 0, 0, &ground, &obstacles, &empty);
7098  }
7099  localMaps_.add(ids.at(i), ground, obstacles, empty, data.gridCellSize()>0.0f?data.gridCellSize():Parameters::defaultGridCellSize(), data.gridViewPoint());
7100  if(!ground.empty() || !obstacles.empty())
7101  {
7102  localMaps_.shareTo(ids.at(i), combinedLocalMaps);
7103  }
7104  }
7105  }
7106  UINFO("Update local maps list... done (%d local maps, graph size=%d)", (int)combinedLocalMaps.size(), (int)graph.size());
7107  }
7108 
7109  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
7110  float cellSize = Parameters::defaultGridCellSize();
7111  Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize);
7112 
7113  ui_->graphViewer->updateGTGraph(groundTruthPoses_);
7114  ui_->graphViewer->updateGPSGraph(gpsPoses_, gpsValues_);
7115  ui_->graphViewer->updateGraph(graph, graphLinks_, mapIds_, weights_);
7116  if(ui_->checkBox_wmState->isEnabled() &&
7117  ui_->checkBox_wmState->isChecked() &&
7118  !lastWmIds_.empty())
7119  {
7120  bool allNodesAreInWM = true;
7121  std::map<int, float> colors;
7122  for(std::map<int, rtabmap::Transform>::iterator iter=graph.begin(); iter!=graph.end(); ++iter)
7123  {
7124  if(lastWmIds_.find(iter->first) != lastWmIds_.end())
7125  {
7126  colors.insert(std::make_pair(iter->first, 1));
7127  }
7128  else
7129  {
7130  allNodesAreInWM = false;
7131  }
7132  }
7133  if(!allNodesAreInWM)
7134  {
7135  ui_->graphViewer->updatePosterior(colors, 1, 1);
7136  }
7137  }
7138  QGraphicsRectItem * rectScaleItem = 0;
7139  ui_->graphViewer->clearMap();
7140  occupancyGridViewer_->clear();
7141  if(graph.size() && combinedLocalMaps.size() &&
7142  (ui_->graphViewer->isGridMapVisible() || ui_->dockWidget_occupancyGridView->isVisible()))
7143  {
7144  QElapsedTimer time;
7145  time.start();
7146 
7147 #ifdef RTABMAP_OCTOMAP
7148  if(ui_->checkBox_octomap->isChecked())
7149  {
7150  octomap_ = new OctoMap(&combinedLocalMaps, parameters);
7151  octomap_->update(graphFiltered);
7152  }
7153 #endif
7154  // Generate 2d grid map?
7155  if((ui_->dockWidget_graphView->isVisible() && ui_->graphViewer->isGridMapVisible()) ||
7156  (ui_->dockWidget_occupancyGridView->isVisible() && ui_->checkBox_grid_2d->isChecked()))
7157  {
7158  bool eroded = Parameters::defaultGridGlobalEroded();
7159  Parameters::parse(parameters, Parameters::kGridGlobalEroded(), eroded);
7160  float xMin, yMin;
7161  cv::Mat map;
7162 
7163 #ifdef RTABMAP_OCTOMAP
7164  if(ui_->checkBox_octomap->isChecked())
7165  {
7166  map = octomap_->createProjectionMap(xMin, yMin, cellSize, 0, ui_->spinBox_grid_depth->value());
7167  }
7168  else
7169 #endif
7170  {
7171  if(eroded)
7172  {
7173  uInsert(parameters, ParametersPair(Parameters::kGridGlobalEroded(), "true"));
7174  }
7175  OccupancyGrid grid(&combinedLocalMaps, parameters);
7176  grid.update(graphFiltered);
7177  if(ui_->checkBox_grid_showProbMap->isChecked())
7178  {
7179  map = grid.getProbMap(xMin, yMin);
7180  }
7181  else
7182  {
7183  map = grid.getMap(xMin, yMin);
7184  }
7185  }
7186 
7187  ui_->label_timeGrid->setNum(double(time.elapsed())/1000.0);
7188 
7189  if(!map.empty())
7190  {
7191  cv::Mat map8U = rtabmap::util3d::convertMap2Image8U(map);
7192  if(ui_->dockWidget_graphView->isVisible() && ui_->graphViewer->isGridMapVisible())
7193  {
7194  ui_->graphViewer->updateMap(map8U, cellSize, xMin, yMin);
7195  }
7196  if(ui_->dockWidget_occupancyGridView->isVisible() && ui_->checkBox_grid_2d->isChecked())
7197  {
7198  occupancyGridViewer_->addOccupancyGridMap(map8U, cellSize, xMin, yMin, 1.0f);
7199  occupancyGridViewer_->refreshView();
7200  }
7201 
7202  // Zoom to ignore unknowns
7203  int xFirst = 0;
7204  int yFirst = 0;
7205  int xLast = map.cols;
7206  int yLast = map.rows;
7207  bool firstSet = false;
7208  bool lastSet = false;
7209  for(int x=0; x<map.cols && (!firstSet || !lastSet); ++x)
7210  {
7211  for(int y=0; y<map.rows; ++y)
7212  {
7213  // check for first
7214  if(!firstSet && map.at<signed char>(y, x) != -1)
7215  {
7216  xFirst = x;
7217  firstSet = true;
7218  }
7219  // check for last
7220  int opp = map.cols-(x+1);
7221  if(!lastSet && map.at<signed char>(y, opp) != -1)
7222  {
7223  xLast = opp;
7224  lastSet = true;
7225  }
7226  }
7227  }
7228  firstSet = false;
7229  lastSet = false;
7230  for(int y=0; y<map.rows && (!firstSet || !lastSet); ++y)
7231  {
7232  for(int x=0; x<map.cols; ++x)
7233  {
7234  // check for first
7235  if(!firstSet && map.at<signed char>(y, x) != -1)
7236  {
7237  yFirst = y;
7238  firstSet = true;
7239  }
7240  // check for last
7241  int opp = map.rows-(y+1);
7242  if(!lastSet && map.at<signed char>(map.rows-(y+1), x) != -1)
7243  {
7244  yLast = opp;
7245  lastSet = true;
7246  }
7247  }
7248  }
7249  // Only zoom if there are significant unknowns
7250  if( (xLast > xFirst && yLast > yFirst) &&
7251  (xFirst > 50 ||
7252  xLast < map.cols-50 ||
7253  yFirst > 50 ||
7254  yLast < map.rows-50))
7255  {
7256  rectScaleItem = ui_->graphViewer->scene()->addRect(
7257  xFirst-25,
7258  yFirst-25,
7259  xLast-xFirst+50,
7260  yLast-yFirst+50);
7261  rectScaleItem->setTransform(QTransform::fromScale(cellSize*100.0f, -cellSize*100.0f), true);
7262  rectScaleItem->setRotation(90);
7263  rectScaleItem->setPos(-yMin*100.0f, -xMin*100.0f);
7264  }
7265  }
7266  }
7267 
7268  // Generate 3d grid map?
7269  if(ui_->dockWidget_occupancyGridView->isVisible() && ui_->checkBox_grid_grid->isChecked())
7270  {
7271 #ifdef RTABMAP_OCTOMAP
7272  if(ui_->checkBox_octomap->isChecked())
7273  {
7274  updateOctomapView();
7275  }
7276  else
7277 #endif
7278  {
7279  CloudMap cloudMap(&combinedLocalMaps, parameters);
7280 
7281  cloudMap.update(graphFiltered);
7282 
7283  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & groundCells = cloudMap.getMapGround();
7284  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacleCells = cloudMap.getMapObstacles();
7285  const pcl::PointCloud<pcl::PointXYZ>::Ptr & emptyCells = cloudMap.getMapEmptyCells();
7286 
7287  // occupancy cloud
7288  if(groundCells->size())
7289  {
7290  occupancyGridViewer_->addCloud("groundCells",
7291  groundCells,
7292  Transform::getIdentity(),
7293  QColor(ui_->lineEdit_groundColor->text()));
7294  occupancyGridViewer_->setCloudPointSize("groundCells", 5);
7295  }
7296  if(obstacleCells->size())
7297  {
7298  occupancyGridViewer_->addCloud("obstacleCells",
7299  obstacleCells,
7300  Transform::getIdentity(),
7301  QColor(ui_->lineEdit_obstacleColor->text()));
7302  occupancyGridViewer_->setCloudPointSize("obstacleCells", 5);
7303  }
7304  if(ui_->checkBox_grid_empty->isChecked() && emptyCells->size())
7305  {
7306  occupancyGridViewer_->addCloud("emptyCells",
7307  emptyCells,
7308  Transform::getIdentity(),
7309  QColor(ui_->lineEdit_emptyColor->text()));
7310  occupancyGridViewer_->setCloudPointSize("emptyCells", 5);
7311  occupancyGridViewer_->setCloudOpacity("emptyCells", 0.5);
7312  }
7313  occupancyGridViewer_->refreshView();
7314  }
7315  }
7316 
7317 #ifdef RTABMAP_GRIDMAP
7318  // Show elevation map ?
7319  if(ui_->dockWidget_occupancyGridView->isVisible() &&
7320  ui_->checkBox_grid_elevation->checkState() != Qt::Unchecked)
7321  {
7322  GridMap gridMap(&combinedLocalMaps, parameters);
7323 
7324  gridMap.update(graphFiltered);
7325  if(ui_->checkBox_grid_elevation->checkState() == Qt::PartiallyChecked)
7326  {
7327  float xMin, yMin;
7328  cv::Mat elevationMap = gridMap.createHeightMap(xMin, yMin, cellSize);
7329  occupancyGridViewer_->addElevationMap(elevationMap, cellSize, xMin, yMin, 1.0f);
7330  }
7331  else
7332  {
7333  pcl::PolygonMesh::Ptr mesh = gridMap.createTerrainMesh();
7334  occupancyGridViewer_->addCloudMesh("elevation_mesh", mesh);
7335  }
7336  occupancyGridViewer_->refreshView();
7337  }
7338 #endif
7339  }
7340  ui_->graphViewer->fitInView(ui_->graphViewer->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
7341  if(rectScaleItem != 0)
7342  {
7343  ui_->graphViewer->fitInView(rectScaleItem, Qt::KeepAspectRatio);
7344  ui_->graphViewer->scene()->removeItem(rectScaleItem);
7345  delete rectScaleItem;
7346  }
7347 
7348  ui_->graphViewer->update();
7349  ui_->label_iterations->setNum(value);
7350 
7351  //compute total length (neighbor links)
7352  float length = 0.0f;
7353  for(std::multimap<int, rtabmap::Link>::const_iterator iter=graphLinks_.begin(); iter!=graphLinks_.end(); ++iter)
7354  {
7355  std::map<int, rtabmap::Transform>::const_iterator jterA = graph.find(iter->first);
7356  std::map<int, rtabmap::Transform>::const_iterator jterB = graph.find(iter->second.to());
7357  if(jterA != graph.end() && jterB != graph.end())
7358  {
7359  const rtabmap::Transform & poseA = jterA->second;
7360  const rtabmap::Transform & poseB = jterB->second;
7361  if(iter->second.type() == rtabmap::Link::kNeighbor ||
7362  iter->second.type() == rtabmap::Link::kNeighborMerged)
7363  {
7364  Eigen::Vector3f vA, vB;
7365  float x,y,z;
7366  poseA.getTranslation(x,y,z);
7367  vA[0] = x; vA[1] = y; vA[2] = z;
7368  poseB.getTranslation(x,y,z);
7369  vB[0] = x; vB[1] = y; vB[2] = z;
7370  length += (vB - vA).norm();
7371  }
7372  }
7373  }
7374  ui_->label_pathLength->setNum(length);
7375  }
7376 }
7377 
7378 void DatabaseViewer::updateGraphRotation()
7379 {
7380  if(ui_->horizontalSlider_rotation->isEnabled())
7381  {
7382  float theta = float(ui_->horizontalSlider_rotation->value())*M_PI/1800.0f;
7383  ui_->graphViewer->setWorldMapRotation(theta);
7384  ui_->label_rotation->setText(QString::number(float(-ui_->horizontalSlider_rotation->value())/10.0f, 'f', 1) + " deg");
7385  }
7386  else
7387  {
7388  ui_->graphViewer->setWorldMapRotation(0);
7389  }
7390 }
7391 
7392 void DatabaseViewer::updateGraphView()
7393 {
7394  ui_->label_loopClosures->clear();
7395  ui_->label_poses->clear();
7396  ui_->label_rmse->clear();
7397 
7398  if(sender() == ui_->checkBox_alignPosesWithGPS && ui_->checkBox_alignPosesWithGPS->isChecked())
7399  {
7400  ui_->checkBox_alignPosesWithGroundTruth->setChecked(false);
7401  }
7402  else if(sender() == ui_->checkBox_alignPosesWithGroundTruth && ui_->checkBox_alignPosesWithGroundTruth->isChecked())
7403  {
7404  ui_->checkBox_alignPosesWithGPS->setChecked(false);
7405  }
7406 
7407  if(odomPoses_.size())
7408  {
7409  int fromId = ui_->spinBox_optimizationsFrom->value();
7410  if(!uContains(odomPoses_, fromId))
7411  {
7412  QMessageBox::warning(this, tr(""), tr("Graph optimization from id (%1) for which node is not linked to graph.\n Minimum=%2, Maximum=%3")
7413  .arg(fromId)
7414  .arg(odomPoses_.begin()->first)
7415  .arg(odomPoses_.rbegin()->first));
7416  return;
7417  }
7418 
7419  graphes_.clear();
7420  graphLinks_.clear();
7421 
7422  std::map<int, rtabmap::Transform> poses = odomPoses_;
7423  if(ui_->checkBox_wmState->isEnabled() &&
7424  ui_->checkBox_wmState->isChecked() &&
7425  uContains(wmStates_, fromId))
7426  {
7427  std::map<int, rtabmap::Transform> wmPoses;
7428  std::vector<int> & wmState = wmStates_.at(fromId);
7429  for(unsigned int i=0; i<wmState.size(); ++i)
7430  {
7431  std::map<int, rtabmap::Transform>::iterator iter = poses.find(wmState[i]);
7432  if(iter!=poses.end())
7433  {
7434  wmPoses.insert(*iter);
7435  }
7436  }
7437  if(!wmPoses.empty())
7438  {
7439  poses = wmPoses;
7440  }
7441  else
7442  {
7443  UWARN("Empty WM poses!? Ignoring WM state... (root id=%d, wmState=%d)", fromId, wmState.size());
7444  }
7445  }
7446 
7447  // filter current map if not spanning to all maps
7448  if(!ui_->checkBox_spanAllMaps->isChecked() && uContains(mapIds_, fromId) && mapIds_.at(fromId) >= 0)
7449  {
7450  int currentMapId = mapIds_.at(fromId);
7451  for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end();)
7452  {
7453  if(iter->first>0 && (!uContains(mapIds_, iter->first) || mapIds_.at(iter->first) != currentMapId))
7454  {
7455  poses.erase(iter++);
7456  }
7457  else
7458  {
7459  ++iter;
7460  }
7461  }
7462  }
7463 
7464  ui_->menuExport_poses->setEnabled(true);
7465  std::multimap<int, rtabmap::Link> links = links_;
7466 
7467  // filter current map if not spanning to all maps
7468  if(!ui_->checkBox_spanAllMaps->isChecked() && uContains(mapIds_, fromId) && mapIds_.at(fromId) >= 0)
7469  {
7470  int currentMapId = mapIds_.at(fromId);
7471  for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end();)
7472  {
7473  if((iter->second.from()>0 && (!uContains(mapIds_, iter->second.from()) || mapIds_.at(iter->second.from()) != currentMapId)) ||
7474  (iter->second.to()>0 && (!uContains(mapIds_, iter->second.to()) || mapIds_.at(iter->second.to()) != currentMapId)))
7475  {
7476  links.erase(iter++);
7477  }
7478  else
7479  {
7480  ++iter;
7481  }
7482  }
7483  }
7484 
7485  links = updateLinksWithModifications(links);
7486  if(ui_->checkBox_ignorePoseCorrection->isChecked())
7487  {
7488  for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
7489  {
7490  if(iter->second.type() == Link::kNeighbor ||
7491  iter->second.type() == Link::kNeighborMerged)
7492  {
7493  Transform poseFrom = uValue(poses, iter->second.from(), Transform());
7494  Transform poseTo = uValue(poses, iter->second.to(), Transform());
7495  if(!poseFrom.isNull() && !poseTo.isNull())
7496  {
7497  // reset to identity covariance
7498  iter->second = Link(
7499  iter->second.from(),
7500  iter->second.to(),
7501  iter->second.type(),
7502  poseFrom.inverse() * poseTo);
7503  }
7504  }
7505  }
7506  }
7507 
7508  // Marker priors parameters
7509  double markerPriorsLinearVariance = Parameters::defaultMarkerPriorsVarianceLinear();
7510  double markerPriorsAngularVariance = Parameters::defaultMarkerPriorsVarianceAngular();
7511  std::map<int, Transform> markerPriors;
7512  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
7513  Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), markerPriorsLinearVariance);
7514  UASSERT(markerPriorsLinearVariance>0.0f);
7515  Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceAngular(), markerPriorsAngularVariance);
7516  UASSERT(markerPriorsAngularVariance>0.0f);
7517  std::string markerPriorsStr;
7518  if(Parameters::parse(parameters, Parameters::kMarkerPriors(), markerPriorsStr))
7519  {
7520  std::list<std::string> strList = uSplit(markerPriorsStr, '|');
7521  for(std::list<std::string>::iterator iter=strList.begin(); iter!=strList.end(); ++iter)
7522  {
7523  std::string markerStr = *iter;
7524  while(!markerStr.empty() && !uIsDigit(markerStr[0]))
7525  {
7526  markerStr.erase(markerStr.begin());
7527  }
7528  if(!markerStr.empty())
7529  {
7530  std::string idStr = uSplitNumChar(markerStr).front();
7531  int id = uStr2Int(idStr);
7532  Transform prior = Transform::fromString(markerStr.substr(idStr.size()));
7533  if(!prior.isNull() && id>0)
7534  {
7535  markerPriors.insert(std::make_pair(-id, prior));
7536  UDEBUG("Added landmark prior %d: %s", id, prior.prettyPrint().c_str());
7537  }
7538  else
7539  {
7540  UERROR("Failed to parse element \"%s\" in parameter %s", markerStr.c_str(), Parameters::kMarkerPriors().c_str());
7541  }
7542  }
7543  else if(!iter->empty())
7544  {
7545  UERROR("Failed to parse parameter %s, value=\"%s\"", Parameters::kMarkerPriors().c_str(), iter->c_str());
7546  }
7547  }
7548  }
7549 
7550 
7551  // filter links
7552  int totalNeighbor = 0;
7553  int totalNeighborMerged = 0;
7554  int totalGlobal = 0;
7555  int totalLocalTime = 0;
7556  int totalLocalSpace = 0;
7557  int totalUser = 0;
7558  int totalPriors = 0;
7559  int totalLandmarks = 0;
7560  int totalGravity = 0;
7561  std::multimap<int, int> uniqueLinks;
7562  for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end();)
7563  {
7564  bool isUnique = iter->second.from() == iter->second.to(); // Count all self-reference links
7565  if(graph::findLink(uniqueLinks, iter->second.from(), iter->second.to(), true) == uniqueLinks.end())
7566  {
7567  uniqueLinks.insert(std::make_pair(iter->second.from(), iter->second.to()));
7568  isUnique = true;
7569  }
7570 
7571  if(iter->second.type() == Link::kNeighbor)
7572  {
7573  if(isUnique)
7574  ++totalNeighbor;
7575  }
7576  else if(iter->second.type() == Link::kNeighborMerged)
7577  {
7578  if(isUnique)
7579  ++totalNeighborMerged;
7580  }
7581  else if(iter->second.type() == Link::kGlobalClosure)
7582  {
7583  if(ui_->checkBox_ignoreGlobalLoop->isChecked())
7584  {
7585  links.erase(iter++);
7586  continue;
7587  }
7588  if(isUnique)
7589  ++totalGlobal;
7590  }
7591  else if(iter->second.type() == Link::kLocalSpaceClosure)
7592  {
7593  if(ui_->checkBox_ignoreLocalLoopSpace->isChecked())
7594  {
7595  links.erase(iter++);
7596  continue;
7597  }
7598  if(isUnique)
7599  ++totalLocalSpace;
7600  }
7601  else if(iter->second.type() == Link::kLocalTimeClosure)
7602  {
7603  if(ui_->checkBox_ignoreLocalLoopTime->isChecked())
7604  {
7605  links.erase(iter++);
7606  continue;
7607  }
7608  if(isUnique)
7609  ++totalLocalTime;
7610  }
7611  else if(iter->second.type() == Link::kUserClosure)
7612  {
7613  if(ui_->checkBox_ignoreUserLoop->isChecked())
7614  {
7615  links.erase(iter++);
7616  continue;
7617  }
7618  if(isUnique)
7619  ++totalUser;
7620  }
7621  else if(iter->second.type() == Link::kLandmark)
7622  {
7623  if(ui_->checkBox_ignoreLandmarks->isChecked())
7624  {
7625  links.erase(iter++);
7626  continue;
7627  }
7628  UASSERT(iter->second.from() > 0 && iter->second.to() < 0);
7629  if(poses.find(iter->second.from()) != poses.end() && poses.find(iter->second.to()) == poses.end())
7630  {
7631  poses.insert(std::make_pair(iter->second.to(), poses.at(iter->second.from())*iter->second.transform()));
7632  }
7633  if(isUnique)
7634  ++totalLandmarks;
7635 
7636  // add landmark priors if there are some
7637  int markerId = iter->second.to();
7638  if(markerPriors.find(markerId) != markerPriors.end())
7639  {
7640  cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64FC1);
7641  infMatrix(cv::Range(0,3), cv::Range(0,3)) /= markerPriorsLinearVariance;
7642  infMatrix(cv::Range(3,6), cv::Range(3,6)) /= markerPriorsAngularVariance;
7643  links.insert(std::make_pair(markerId, Link(markerId, markerId, Link::kPosePrior, markerPriors.at(markerId), infMatrix)));
7644  UDEBUG("Added prior %d : %s (variance: lin=%f ang=%f)", markerId, markerPriors.at(markerId).prettyPrint().c_str(),
7645  markerPriorsLinearVariance, markerPriorsAngularVariance);
7646  if(isUnique)
7647  ++totalPriors;
7648  }
7649  }
7650  else if(iter->second.type() == Link::kPosePrior)
7651  {
7652  if(isUnique)
7653  ++totalPriors;
7654  }
7655  else if(iter->second.type() == Link::kGravity)
7656  {
7657  if(isUnique)
7658  ++totalGravity;
7659  }
7660  ++iter;
7661  }
7662  updateLoopClosuresSlider();
7663 
7664  ui_->label_loopClosures->setText(tr("(%1, %2, %3, %4, %5, %6, %7, %8, %9)")
7665  .arg(totalNeighbor)
7666  .arg(totalNeighborMerged)
7667  .arg(totalGlobal)
7668  .arg(totalLocalSpace)
7669  .arg(totalLocalTime)
7670  .arg(totalUser)
7671  .arg(totalPriors)
7672  .arg(totalLandmarks)
7673  .arg(totalGravity));
7674 
7675  // remove intermediate nodes?
7676  if(ui_->checkBox_ignoreIntermediateNodes->isVisible() &&
7677  ui_->checkBox_ignoreIntermediateNodes->isEnabled() &&
7678  ui_->checkBox_ignoreIntermediateNodes->isChecked())
7679  {
7680  for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
7681  {
7682  if(iter->second.type() == Link::kNeighbor ||
7683  iter->second.type() == Link::kNeighborMerged)
7684  {
7685  Link link = iter->second;
7686  while(uContains(weights_, link.to()) && weights_.at(link.to()) < 0)
7687  {
7688  std::multimap<int, Link>::iterator uter = links.find(link.to());
7689  while(uter != links.end() &&
7690  uter->first==link.to() &&
7691  uter->second.from()>uter->second.to())
7692  {
7693  ++uter;
7694  }
7695  if(uter != links.end())
7696  {
7697  poses.erase(link.to());
7698  link = link.merge(uter->second, uter->second.type());
7699  links.erase(uter->first);
7700  }
7701  else
7702  {
7703  break;
7704  }
7705  }
7706 
7707  iter->second = link;
7708  }
7709  }
7710  }
7711 
7712  bool applyRotation = sender() == ui_->pushButton_applyRotation;
7713  if(applyRotation)
7714  {
7715  float xMin, yMin, cellSize;
7716  bool hasMap = !dbDriver_->load2DMap(xMin, yMin, cellSize).empty();
7717  if(hasMap || !dbDriver_->loadOptimizedMesh().empty())
7718  {
7719  QMessageBox::StandardButton r = QMessageBox::question(this,
7720  tr("Rotate Optimized Graph"),
7721  tr("There is a 2D occupancy grid or mesh already saved in "
7722  "database. Applying rotation will clear them (they can be "
7723  "regenerated later from File menu options). "
7724  "Do you want to continue?"),
7725  QMessageBox::Cancel | QMessageBox::Yes,
7726  QMessageBox::Cancel);
7727  if(r != QMessageBox::Yes)
7728  {
7729  applyRotation = false;
7730  }
7731  }
7732  }
7733 
7734  std::map<int, Transform> optPoses;
7735  Transform lastLocalizationPose;
7736  if(applyRotation ||
7737  ui_->comboBox_optimizationFlavor->currentIndex() == 2)
7738  {
7739  optPoses = dbDriver_->loadOptimizedPoses(&lastLocalizationPose);
7740  if(optPoses.empty())
7741  {
7742  ui_->comboBox_optimizationFlavor->setCurrentIndex(0);
7743  QMessageBox::warning(this, tr("Optimization Flavor"),
7744  tr("There is no local optimized graph in the database, "
7745  "falling back to global iterative optimization."));
7746  }
7747  }
7748 
7749  if(applyRotation ||
7750  ui_->comboBox_optimizationFlavor->currentIndex() != 2)
7751  {
7752  if(ui_->horizontalSlider_rotation->value()!=0 && applyRotation)
7753  {
7754  float theta = float(-ui_->horizontalSlider_rotation->value())*M_PI/1800.0f;
7755  Transform rotT(0,0,theta);
7756  poses.at(fromId) = rotT * poses.at(fromId);
7757  }
7758 
7759  graphes_.push_back(poses);
7760 
7761  Optimizer * optimizer = Optimizer::create(parameters);
7762 
7763  std::map<int, rtabmap::Transform> posesOut;
7764  std::multimap<int, rtabmap::Link> linksOut;
7765  UINFO("Get connected graph from %d (%d poses, %d links)", fromId, (int)poses.size(), (int)links.size());
7766  optimizer->getConnectedGraph(
7767  fromId,
7768  poses,
7769  links,
7770  posesOut,
7771  linksOut);
7772  UINFO("Connected graph of %d poses and %d links", (int)posesOut.size(), (int)linksOut.size());
7773  QElapsedTimer time;
7774  time.start();
7775  std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(fromId, posesOut, linksOut, ui_->comboBox_optimizationFlavor->currentIndex()==0?&graphes_:0);
7776  ui_->label_timeOptimization->setNum(double(time.elapsed())/1000.0);
7777  graphLinks_ = linksOut;
7778  if(posesOut.size() && finalPoses.empty())
7779  {
7780  UWARN("Optimization failed... (poses=%d, links=%d).", (int)posesOut.size(), (int)linksOut.size());
7781  if(!optimizer->isCovarianceIgnored() || optimizer->type() != Optimizer::kTypeTORO)
7782  {
7783  QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors. "
7784  "Give it a try with %1=0 and %2=true.").arg(Parameters::kOptimizerStrategy().c_str()).arg(Parameters::kOptimizerVarianceIgnored().c_str()));
7785  }
7786  else
7787  {
7788  QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors."));
7789  }
7790  }
7791  ui_->label_poses->setNum((int)finalPoses.size());
7792  graphes_.push_back(finalPoses);
7793  delete optimizer;
7794 
7795  if(applyRotation && !finalPoses.empty())
7796  {
7797  ui_->comboBox_optimizationFlavor->setCurrentIndex(2);
7798  graphes_.clear();
7799  graphes_.push_back(finalPoses);
7800  if(lastLocalizationPose.isNull())
7801  {
7802  // use last pose by default
7803  lastLocalizationPose = finalPoses.rbegin()->second;
7804  }
7805  dbDriver_->saveOptimizedPoses(finalPoses, lastLocalizationPose);
7806  // reset optimized mesh and map as poses have changed
7807  float xMin, yMin, cellSize;
7808  bool hasMap = !dbDriver_->load2DMap(xMin, yMin, cellSize).empty();
7809  if(hasMap || !dbDriver_->loadOptimizedMesh().empty())
7810  {
7811  dbDriver_->saveOptimizedMesh(cv::Mat());
7812  dbDriver_->save2DMap(cv::Mat(), 0, 0, 0);
7813  QMessageBox::StandardButton r = QMessageBox::question(this,
7814  tr("Rotate Optimized Graph"),
7815  tr("Optimized graph has been rotated and saved back to database. "
7816  "Note that 2D occupancy grid and mesh have been cleared (if set). "
7817  "Do you want to regenerate the 2D occupancy grid now "
7818  "(can be done later from File menu)?"),
7819  QMessageBox::Ignore | QMessageBox::Yes,
7820  QMessageBox::Yes);
7821  ui_->actionEdit_optimized_2D_map->setEnabled(false);
7822  ui_->actionExport_saved_2D_map->setEnabled(false);
7823  ui_->actionImport_2D_map->setEnabled(false);
7824  ui_->actionView_optimized_mesh->setEnabled(false);
7825  ui_->actionExport_optimized_mesh->setEnabled(false);
7826  if(r == QMessageBox::Yes)
7827  {
7828  regenerateSavedMap();
7829  }
7830  }
7831  }
7832  }
7833 
7834  // Update buttons state
7835  if(ui_->comboBox_optimizationFlavor->currentIndex() == 2)
7836  {
7837  // Local optimized graph
7838  if(graphes_.empty())
7839  {
7840  ui_->label_timeOptimization->setNum(0);
7841  ui_->label_poses->setNum((int)optPoses.size());
7842  graphes_.push_back(optPoses);
7843  }
7844  ui_->horizontalSlider_rotation->setEnabled(false);
7845  ui_->pushButton_applyRotation->setEnabled(false);
7846  ui_->spinBox_optimizationsFrom->setEnabled(false);
7847  ui_->checkBox_spanAllMaps->setEnabled(false);
7848  ui_->checkBox_wmState->setEnabled(false);
7849  ui_->checkBox_alignPosesWithGPS->setEnabled(false);
7850  ui_->checkBox_alignPosesWithGroundTruth->setEnabled(false);
7851  ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(false);
7852  ui_->checkBox_ignoreIntermediateNodes->setEnabled(false);
7853  }
7854  else
7855  {
7856  // Global map re-optimized
7857  ui_->pushButton_applyRotation->setEnabled(true);
7858  ui_->horizontalSlider_rotation->setEnabled(true);
7859  ui_->spinBox_optimizationsFrom->setEnabled(true);
7860  ui_->checkBox_spanAllMaps->setEnabled(true);
7861  ui_->checkBox_wmState->setEnabled(true);
7862  ui_->checkBox_alignPosesWithGPS->setEnabled(ui_->checkBox_alignPosesWithGPS->isVisible());
7863  ui_->checkBox_alignPosesWithGroundTruth->setEnabled(ui_->checkBox_alignPosesWithGroundTruth->isVisible());
7864  ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(ui_->checkBox_alignScansCloudsWithGroundTruth->isVisible());
7865  ui_->checkBox_ignoreIntermediateNodes->setEnabled(true);
7866  }
7867  updateGraphRotation();
7868  }
7869  if(graphes_.size())
7870  {
7871  if(ui_->doubleSpinBox_optimizationScale->value()!=-1.0)
7872  {
7873  // scale all poses
7874  for(std::list<std::map<int, Transform> >::iterator iter=graphes_.begin(); iter!=graphes_.end(); ++iter)
7875  {
7876  for(std::map<int, Transform>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
7877  {
7878  jter->second = jter->second.clone();
7879  jter->second.x() *= ui_->doubleSpinBox_optimizationScale->value();
7880  jter->second.y() *= ui_->doubleSpinBox_optimizationScale->value();
7881  jter->second.z() *= ui_->doubleSpinBox_optimizationScale->value();
7882  }
7883  }
7884  }
7885 
7886  ui_->horizontalSlider_iterations->setMaximum((int)graphes_.size()-1);
7887  ui_->horizontalSlider_iterations->setValue((int)graphes_.size()-1);
7888  ui_->horizontalSlider_iterations->setEnabled(true);
7889  ui_->spinBox_optimizationsFrom->setEnabled(true);
7890  sliderIterationsValueChanged((int)graphes_.size()-1);
7891  }
7892  else
7893  {
7894  ui_->horizontalSlider_iterations->setEnabled(false);
7895  ui_->spinBox_optimizationsFrom->setEnabled(false);
7896  }
7897 }
7898 
7899 void DatabaseViewer::updateGrid()
7900 {
7901  if(sender() == ui_->checkBox_grid_2d && !ui_->checkBox_grid_2d->isChecked())
7902  {
7903  //just remove map in occupancy grid view
7904  occupancyGridViewer_->removeOccupancyGridMap();
7905  occupancyGridViewer_->refreshView();
7906  }
7907  else
7908  {
7909  ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked());
7910  ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
7911  ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7912  ui_->checkBox_grid_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7913  ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
7914  ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
7915  ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7916  ui_->label_octomap_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7917 
7918  update3dView();
7919  updateGraphView();
7920  }
7921 }
7922 
7923 void DatabaseViewer::updateOctomapView()
7924 {
7925 #ifdef RTABMAP_OCTOMAP
7926  ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked());
7927  ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
7928  ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7929  ui_->checkBox_grid_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7930  ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
7931  ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
7932  ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7933  ui_->label_octomap_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7934 
7935  if(ui_->checkBox_octomap->isChecked())
7936  {
7937  if(octomap_)
7938  {
7939  occupancyGridViewer_->removeOctomap();
7940  occupancyGridViewer_->removeCloud("octomap_obstacles");
7941  occupancyGridViewer_->removeCloud("octomap_ground");
7942  occupancyGridViewer_->removeCloud("octomap_empty");
7943  occupancyGridViewer_->removeCloud("octomap_frontiers");
7944  occupancyGridViewer_->removeCloud("groundCells");
7945  occupancyGridViewer_->removeCloud("obstacleCells");
7946  occupancyGridViewer_->removeCloud("emptyCells");
7947  if(ui_->comboBox_octomap_rendering_type->currentIndex()>0)
7948  {
7949  occupancyGridViewer_->addOctomap(octomap_, ui_->spinBox_grid_depth->value(), ui_->comboBox_octomap_rendering_type->currentIndex()>1);
7950  }
7951  else
7952  {
7953  pcl::IndicesPtr obstacles(new std::vector<int>);
7954  pcl::IndicesPtr empty(new std::vector<int>);
7955  pcl::IndicesPtr ground(new std::vector<int>);
7956  pcl::IndicesPtr frontiers(new std::vector<int>);
7957  std::vector<double> prob;
7958  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(
7959  ui_->spinBox_grid_depth->value(),
7960  obstacles.get(),
7961  empty.get(),
7962  ground.get(),
7963  true,
7964  frontiers.get(),
7965  &prob);
7966 
7967  if(octomap_->hasColor())
7968  {
7969  pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
7970  pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
7971  occupancyGridViewer_->addCloud("octomap_obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
7972  occupancyGridViewer_->setCloudPointSize("octomap_obstacles", 5);
7973 
7974  pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
7975  pcl::copyPointCloud(*cloud, *ground, *groundCloud);
7976  occupancyGridViewer_->addCloud("octomap_ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
7977  occupancyGridViewer_->setCloudPointSize("octomap_ground", 5);
7978  }
7979  else
7980  {
7981  pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
7982  pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
7983  occupancyGridViewer_->addCloud("octomap_obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
7984  occupancyGridViewer_->setCloudPointSize("octomap_obstacles", 5);
7985 
7986  pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
7987  pcl::copyPointCloud(*cloud, *ground, *groundCloud);
7988  occupancyGridViewer_->addCloud("octomap_ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
7989  occupancyGridViewer_->setCloudPointSize("octomap_ground", 5);
7990  }
7991 
7992  if(ui_->checkBox_grid_empty->isChecked())
7993  {
7994  if(prob.size()==cloud->size())
7995  {
7996  float occThr = Parameters::defaultGridGlobalOccupancyThr();
7997  Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kGridGlobalOccupancyThr(), occThr);
7998  pcl::PointCloud<pcl::PointXYZRGB>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
7999  emptyCloud->resize(empty->size());
8000  for(unsigned int i=0;i<empty->size(); ++i)
8001  {
8002  emptyCloud->points.at(i).x = cloud->points.at(empty->at(i)).x;
8003  emptyCloud->points.at(i).y = cloud->points.at(empty->at(i)).y;
8004  emptyCloud->points.at(i).z = cloud->points.at(empty->at(i)).z;
8005  QColor hsv = QColor::fromHsv(int(prob.at(empty->at(i))/occThr*240.0), 255, 255, 255);
8006  QRgb color = hsv.rgb();
8007  emptyCloud->points.at(i).r = qRed(color);
8008  emptyCloud->points.at(i).g = qGreen(color);
8009  emptyCloud->points.at(i).b = qBlue(color);
8010  }
8011  occupancyGridViewer_->addCloud("octomap_empty", emptyCloud, Transform::getIdentity(), QColor(ui_->lineEdit_emptyColor->text()));
8012  occupancyGridViewer_->setCloudOpacity("octomap_empty", 0.5);
8013  occupancyGridViewer_->setCloudPointSize("octomap_empty", 5);
8014  }
8015  else
8016  {
8017  pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZ>);
8018  pcl::copyPointCloud(*cloud, *empty, *emptyCloud);
8019  occupancyGridViewer_->addCloud("octomap_empty", emptyCloud, Transform::getIdentity(), QColor(ui_->lineEdit_emptyColor->text()));
8020  occupancyGridViewer_->setCloudOpacity("octomap_empty", 0.5);
8021  occupancyGridViewer_->setCloudPointSize("octomap_empty", 5);
8022  }
8023  }
8024 
8025  if(ui_->checkBox_grid_frontiers->isChecked())
8026  {
8027  pcl::PointCloud<pcl::PointXYZ>::Ptr frontiersCloud(new pcl::PointCloud<pcl::PointXYZ>);
8028  pcl::copyPointCloud(*cloud, *frontiers, *frontiersCloud);
8029  occupancyGridViewer_->addCloud("octomap_frontiers", frontiersCloud, Transform::getIdentity(), QColor(ui_->lineEdit_frontierColor->text()));
8030  occupancyGridViewer_->setCloudOpacity("octomap_frontiers", 0.5);
8031  occupancyGridViewer_->setCloudPointSize("octomap_frontiers", 5);
8032  }
8033  }
8034  occupancyGridViewer_->refreshView();
8035  }
8036  if(ui_->dockWidget_view3d->isVisible() && ui_->checkBox_showGrid->isChecked())
8037  {
8038  this->update3dView();
8039  }
8040  }
8041 #endif
8042 }
8043 
8044 Link DatabaseViewer::findActiveLink(int from, int to)
8045 {
8046  Link link;
8047  std::multimap<int, Link>::iterator findIter = rtabmap::graph::findLink(linksRefined_, from ,to);
8048  if(findIter != linksRefined_.end())
8049  {
8050  link = findIter->second;
8051  }
8052  else
8053  {
8054  findIter = rtabmap::graph::findLink(linksAdded_, from ,to);
8055  if(findIter != linksAdded_.end())
8056  {
8057  link = findIter->second;
8058  }
8059  else if(!containsLink(linksRemoved_, from ,to))
8060  {
8061  findIter = rtabmap::graph::findLink(links_, from ,to);
8062  if(findIter != links_.end())
8063  {
8064  link = findIter->second;
8065  }
8066  }
8067  }
8068  return link;
8069 }
8070 
8071 bool DatabaseViewer::containsLink(std::multimap<int, Link> & links, int from, int to)
8072 {
8073  return rtabmap::graph::findLink(links, from, to) != links.end();
8074 }
8075 
8076 void DatabaseViewer::refineConstraint()
8077 {
8078  int from = ids_.at(ui_->horizontalSlider_A->value());
8079  int to = ids_.at(ui_->horizontalSlider_B->value());
8080  refineConstraint(from, to, false);
8081 }
8082 
8083 void DatabaseViewer::refineConstraint(int from, int to, bool silent)
8084 {
8085  UDEBUG("%d -> %d", from, to);
8086  bool switchedIds = false;
8087  if(from == to)
8088  {
8089  UWARN("Cannot refine link to same node");
8090  return;
8091  }
8092 
8093 
8094  Link currentLink = findActiveLink(from, to);
8095  if(!currentLink.isValid())
8096  {
8097  UERROR("Not found link! (%d->%d)", from, to);
8098  return;
8099  }
8100 
8101  UDEBUG("%d -> %d (type=%d)", currentLink.from(), currentLink.to(), currentLink.type());
8102  Transform t = currentLink.transform();
8103  if(ui_->checkBox_showOptimized->isChecked() &&
8104  (currentLink.type() == Link::kNeighbor || currentLink.type() == Link::kNeighborMerged) &&
8105  graphes_.size() &&
8106  (int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
8107  {
8108  std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
8109  if(currentLink.type() == Link::kNeighbor || currentLink.type() == Link::kNeighborMerged)
8110  {
8111  std::map<int, rtabmap::Transform>::iterator iterFrom = graph.find(currentLink.from());
8112  std::map<int, rtabmap::Transform>::iterator iterTo = graph.find(currentLink.to());
8113  if(iterFrom != graph.end() && iterTo != graph.end())
8114  {
8115  Transform topt = iterFrom->second.inverse()*iterTo->second;
8116  t = topt;
8117  }
8118  }
8119  }
8120  else if(ui_->checkBox_ignorePoseCorrection->isChecked() &&
8121  graph::findLink(linksRefined_, currentLink.from(), currentLink.to()) == linksRefined_.end())
8122  {
8123  if(currentLink.type() == Link::kNeighbor ||
8124  currentLink.type() == Link::kNeighborMerged)
8125  {
8126  Transform poseFrom = uValue(odomPoses_, currentLink.from(), Transform());
8127  Transform poseTo = uValue(odomPoses_, currentLink.to(), Transform());
8128  if(!poseFrom.isNull() && !poseTo.isNull())
8129  {
8130  t = poseFrom.inverse() * poseTo; // recompute raw odom transformation
8131  }
8132  }
8133  }
8134 
8135  Transform transform;
8136  RegistrationInfo info;
8137  Signature * fromS = 0;
8138  Signature * toS = 0;
8139 
8140  fromS = dbDriver_->loadSignature(currentLink.from());
8141  if(fromS == 0)
8142  {
8143  UERROR("Signature %d not found!", currentLink.from());
8144  return;
8145  }
8146 
8147  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
8148 
8149  UTimer timer;
8150 
8151  // Is it a multi-scan proximity detection?
8152  cv::Mat userData = currentLink.uncompressUserDataConst();
8153  std::map<int, rtabmap::Transform> scanPoses;
8154 
8155  if(currentLink.type() == Link::kLocalSpaceClosure &&
8156  !currentLink.userDataCompressed().empty() &&
8157  userData.type() == CV_8SC1 &&
8158  userData.rows == 1 &&
8159  userData.cols >= 8 && // including null str ending
8160  userData.at<char>(userData.cols-1) == 0 &&
8161  memcmp(userData.data, "SCANS:", 6) == 0 &&
8162  currentLink.from() > currentLink.to())
8163  {
8164  std::string scansStr = (const char *)userData.data;
8165  UINFO("Detected \"%s\" in links's user data", scansStr.c_str());
8166  if(!scansStr.empty())
8167  {
8168  std::list<std::string> strs = uSplit(scansStr, ':');
8169  if(strs.size() == 2)
8170  {
8171  std::list<std::string> strIds = uSplit(strs.rbegin()->c_str(), ';');
8172  for(std::list<std::string>::iterator iter=strIds.begin(); iter!=strIds.end(); ++iter)
8173  {
8174  int id = atoi(iter->c_str());
8175  if(uContains(odomPoses_, id))
8176  {
8177  scanPoses.insert(*odomPoses_.find(id));
8178  }
8179  else
8180  {
8181  UERROR("Not found %d node!", id);
8182  }
8183  }
8184  }
8185  }
8186  }
8187  if(scanPoses.size()>1)
8188  {
8189  //optimize the path's poses locally
8190  Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
8191 
8192  UASSERT(uContains(scanPoses, currentLink.to()));
8193  std::map<int, rtabmap::Transform> posesOut;
8194  std::multimap<int, rtabmap::Link> linksOut;
8195  optimizer->getConnectedGraph(
8196  currentLink.to(),
8197  scanPoses,
8198  updateLinksWithModifications(links_),
8199  posesOut,
8200  linksOut);
8201 
8202  if(scanPoses.size() != posesOut.size())
8203  {
8204  UWARN("Scan poses input and output are different! %d vs %d", (int)scanPoses.size(), (int)posesOut.size());
8205  UWARN("Input poses: ");
8206  for(std::map<int, Transform>::iterator iter=scanPoses.begin(); iter!=scanPoses.end(); ++iter)
8207  {
8208  UWARN(" %d", iter->first);
8209  }
8210  UWARN("Input links: ");
8211  std::multimap<int, Link> modifiedLinks = updateLinksWithModifications(links_);
8212  for(std::multimap<int, Link>::iterator iter=modifiedLinks.begin(); iter!=modifiedLinks.end(); ++iter)
8213  {
8214  UWARN(" %d->%d", iter->second.from(), iter->second.to());
8215  }
8216  }
8217 
8218  scanPoses = optimizer->optimize(currentLink.to(), posesOut, linksOut);
8219  delete optimizer;
8220 
8221  std::map<int, Transform> filteredScanPoses = scanPoses;
8222  float proximityFilteringRadius = 0.0f;
8223  Parameters::parse(parameters, Parameters::kRGBDProximityPathFilteringRadius(), proximityFilteringRadius);
8224  if(scanPoses.size() > 2 && proximityFilteringRadius > 0.0f)
8225  {
8226  // path filtering
8227  filteredScanPoses = graph::radiusPosesFiltering(scanPoses, proximityFilteringRadius, 0, true);
8228  // make sure the current pose is still here
8229  filteredScanPoses.insert(*scanPoses.find(currentLink.to()));
8230  }
8231 
8232  Transform toPoseInv = filteredScanPoses.at(currentLink.to()).inverse();
8233  dbDriver_->loadNodeData(fromS, !silent, true, !silent, !silent);
8234  fromS->sensorData().uncompressData();
8235  LaserScan fromScan = fromS->sensorData().laserScanRaw();
8236  int maxPoints = fromScan.size();
8237  if(maxPoints == 0)
8238  {
8239  UWARN("From scan %d is empty!", fromS->id());
8240  }
8241  pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
8242  pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(new pcl::PointCloud<pcl::PointNormal>);
8243  pcl::PointCloud<pcl::PointXYZI>::Ptr assembledToIClouds(new pcl::PointCloud<pcl::PointXYZI>);
8244  pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembledToNormalIClouds(new pcl::PointCloud<pcl::PointXYZINormal>);
8245  pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledToRGBClouds(new pcl::PointCloud<pcl::PointXYZRGB>);
8246  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledToNormalRGBClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
8247  for(std::map<int, Transform>::const_iterator iter = filteredScanPoses.begin(); iter!=filteredScanPoses.end(); ++iter)
8248  {
8249  if(iter->first != currentLink.from())
8250  {
8251  SensorData data;
8252  dbDriver_->getNodeData(iter->first, data);
8253  if(!data.laserScanCompressed().isEmpty())
8254  {
8255  LaserScan scan;
8256  data.uncompressData(0, 0, &scan);
8257  if(!scan.isEmpty() && fromScan.format() == scan.format())
8258  {
8259  if(scan.hasIntensity())
8260  {
8261  if(scan.hasNormals())
8262  {
8263  *assembledToNormalIClouds += *util3d::laserScanToPointCloudINormal(scan,
8264  toPoseInv * iter->second * scan.localTransform());
8265  }
8266  else
8267  {
8268  *assembledToIClouds += *util3d::laserScanToPointCloudI(scan,
8269  toPoseInv * iter->second * scan.localTransform());
8270  }
8271  }
8272  else if(scan.hasRGB())
8273  {
8274  if(scan.hasNormals())
8275  {
8276  *assembledToNormalRGBClouds += *util3d::laserScanToPointCloudRGBNormal(scan,
8277  toPoseInv * iter->second * scan.localTransform());
8278  }
8279  else
8280  {
8281  *assembledToRGBClouds += *util3d::laserScanToPointCloudRGB(scan,
8282  toPoseInv * iter->second * scan.localTransform());
8283  }
8284  }
8285  else
8286  {
8287  if(scan.hasNormals())
8288  {
8289  *assembledToNormalClouds += *util3d::laserScanToPointCloudNormal(scan,
8290  toPoseInv * iter->second * scan.localTransform());
8291  }
8292  else
8293  {
8294  *assembledToClouds += *util3d::laserScanToPointCloud(scan,
8295  toPoseInv * iter->second * scan.localTransform());
8296  }
8297  }
8298 
8299  if(scan.size() > maxPoints)
8300  {
8301  maxPoints = scan.size();
8302  }
8303  }
8304  else
8305  {
8306  UWARN("scan format of %d is not the same than from scan %d: %d vs %d", data.id(), fromS->id(), scan.format(), fromScan.format());
8307  }
8308  }
8309  else
8310  {
8311  UWARN("Laser scan not found for signature %d", iter->first);
8312  }
8313  }
8314  }
8315 
8316  LaserScan assembledScan;
8317  if(assembledToNormalClouds->size())
8318  {
8319  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToNormalClouds):util3d::laserScanFromPointCloud(*assembledToNormalClouds);
8320  }
8321  else if(assembledToClouds->size())
8322  {
8323  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToClouds):util3d::laserScanFromPointCloud(*assembledToClouds);
8324  }
8325  else if(assembledToNormalIClouds->size())
8326  {
8327  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToNormalIClouds):util3d::laserScanFromPointCloud(*assembledToNormalIClouds);
8328  }
8329  else if(assembledToIClouds->size())
8330  {
8331  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToIClouds):util3d::laserScanFromPointCloud(*assembledToIClouds);
8332  }
8333  else if(assembledToNormalRGBClouds->size())
8334  {
8335  UASSERT(!fromScan.is2d());
8336  assembledScan = util3d::laserScanFromPointCloud(*assembledToNormalRGBClouds);
8337  }
8338  else if(assembledToRGBClouds->size())
8339  {
8340  UASSERT(!fromScan.is2d());
8341  assembledScan = util3d::laserScanFromPointCloud(*assembledToRGBClouds);
8342  }
8343  else
8344  {
8345  UWARN("Assembled scan is empty!");
8346  }
8347  SensorData assembledData(cv::Mat(), to);
8348  // scans are in base frame but for 2d scans, set the height so that correspondences matching works
8349  assembledData.setLaserScan(LaserScan(
8350  assembledScan,
8351  fromScan.maxPoints()?fromScan.maxPoints():maxPoints,
8352  fromScan.rangeMax(),
8353  fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
8354 
8355  toS = new Signature(assembledData);
8356  RegistrationIcp registrationIcp(parameters);
8357  transform = registrationIcp.computeTransformationMod(*fromS, *toS, currentLink.transform(), &info);
8358  if(!transform.isNull())
8359  {
8360  // local scan matching proximity detection should have higher variance (see Rtabmap::process())
8361  info.covariance*=100.0;
8362  }
8363  }
8364  else
8365  {
8366  toS = dbDriver_->loadSignature(currentLink.to());
8367  if(toS == 0)
8368  {
8369  UERROR("Signature %d not found!", currentLink.to());
8370  delete fromS;
8371  return;
8372  }
8373 
8374  bool reextractVisualFeatures = uStr2Bool(parameters.at(Parameters::kRGBDLoopClosureReextractFeatures()));
8375  Registration * reg = Registration::create(parameters);
8376  if( reg->isScanRequired() ||
8377  reg->isUserDataRequired() ||
8378  reextractVisualFeatures ||
8379  !silent)
8380  {
8381  dbDriver_->loadNodeData(fromS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
8382  dbDriver_->loadNodeData(toS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
8383 
8384  if(!silent)
8385  {
8386  fromS->sensorData().uncompressData();
8387  toS->sensorData().uncompressData();
8388  }
8389  }
8390 
8391  if(reextractVisualFeatures)
8392  {
8393  fromS->removeAllWords();
8394  fromS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
8395  toS->removeAllWords();
8396  toS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
8397  }
8398 
8399  if(reg->isScanRequired())
8400  {
8401  if(ui_->checkBox_icp_from_depth->isChecked())
8402  {
8403  // generate laser scans from depth image
8404  cv::Mat tmpA, tmpB, tmpC, tmpD;
8405  fromS->sensorData().uncompressData(&tmpA, &tmpB, 0);
8406  toS->sensorData().uncompressData(&tmpC, &tmpD, 0);
8407  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom = util3d::cloudFromSensorData(
8408  fromS->sensorData(),
8409  ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
8410  ui_->doubleSpinBox_icp_maxDepth->value(),
8411  ui_->doubleSpinBox_icp_minDepth->value(),
8412  0,
8413  ui_->parameters_toolbox->getParameters());
8414  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo = util3d::cloudFromSensorData(
8415  toS->sensorData(),
8416  ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
8417  ui_->doubleSpinBox_icp_maxDepth->value(),
8418  ui_->doubleSpinBox_icp_minDepth->value(),
8419  0,
8420  ui_->parameters_toolbox->getParameters());
8421 
8422  if(cloudFrom->empty() && cloudTo->empty())
8423  {
8424  std::string msg = "Option to generate scan from depth is checked (GUI Parameters->Refine), but "
8425  "resulting clouds from depth are empty. Transformation estimation will likely "
8426  "fails. Uncheck the parameter to use laser scans.";
8427  UWARN(msg.c_str());
8428  if(!silent)
8429  {
8430  QMessageBox::warning(this,
8431  tr("Refine link"),
8432  tr("%1").arg(msg.c_str()));
8433  }
8434  }
8435  else if(!fromS->sensorData().laserScanCompressed().isEmpty() || !toS->sensorData().laserScanCompressed().isEmpty())
8436  {
8437  UWARN("There are laser scans in data, but generate laser scan from "
8438  "depth image option is activated (GUI Parameters->Refine). "
8439  "Ignoring saved laser scans...");
8440  }
8441 
8442  int maxLaserScans = cloudFrom->size();
8443  fromS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0));
8444  toS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0));
8445  }
8446  else
8447  {
8448  LaserScan tmpA, tmpB;
8449  fromS->sensorData().uncompressData(0, 0, &tmpA);
8450  toS->sensorData().uncompressData(0, 0, &tmpB);
8451  }
8452  }
8453 
8454  if(reg->isImageRequired() && reextractVisualFeatures)
8455  {
8456  cv::Mat tmpA, tmpB, tmpC, tmpD;
8457  fromS->sensorData().uncompressData(&tmpA, &tmpB, 0);
8458  toS->sensorData().uncompressData(&tmpC, &tmpD, 0);
8459  }
8460 
8461  UINFO("Uncompress time: %f s", timer.ticks());
8462 
8463  if(fromS->id() < toS->id())
8464  {
8465  transform = reg->computeTransformationMod(*fromS, *toS, t, &info);
8466  }
8467  else
8468  {
8469  transform = reg->computeTransformationMod(*toS, *fromS, t.isNull()?t:t.inverse(), &info);
8470  switchedIds = true;
8471  }
8472 
8473  delete reg;
8474  }
8475  UINFO("(%d ->%d) Registration time: %f s", currentLink.from(), currentLink.to(), timer.ticks());
8476 
8477  if(!transform.isNull())
8478  {
8479  if(!transform.isIdentity())
8480  {
8481  if(info.covariance.at<double>(0,0)<=0.0)
8482  {
8483  info.covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
8484  }
8485  }
8486  if(switchedIds)
8487  {
8488  transform = transform.inverse();
8489  }
8490 
8491  Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform, info.covariance.inv(), currentLink.userDataCompressed());
8492 
8493  bool updated = false;
8494  std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
8495  while(iter != linksRefined_.end() && iter->first == currentLink.from())
8496  {
8497  if(iter->second.to() == currentLink.to() &&
8498  iter->second.type() == currentLink.type())
8499  {
8500  iter->second = newLink;
8501  updated = true;
8502  break;
8503  }
8504  ++iter;
8505  }
8506  if(!updated)
8507  {
8508  linksRefined_.insert(std::make_pair(newLink.from(), newLink));
8509  updated = true;
8510  }
8511 
8512  if(updated && !silent)
8513  {
8514  this->updateGraphView();
8515  }
8516 
8517  if(!silent && ui_->dockWidget_constraints->isVisible())
8518  {
8519  if(toS && fromS->id() > 0 && toS->id() > 0)
8520  {
8521  updateLoopClosuresSlider(fromS->id(), toS->id());
8522  std::multimap<int, cv::KeyPoint> keypointsFrom;
8523  std::multimap<int, cv::KeyPoint> keypointsTo;
8524  if(!fromS->getWordsKpts().empty())
8525  {
8526  for(std::map<int, int>::const_iterator iter=fromS->getWords().begin(); iter!=fromS->getWords().end(); ++iter)
8527  {
8528  keypointsFrom.insert(keypointsFrom.end(), std::make_pair(iter->first, fromS->getWordsKpts()[iter->second]));
8529  }
8530  }
8531  if(!toS->getWordsKpts().empty())
8532  {
8533  for(std::map<int, int>::const_iterator iter=toS->getWords().begin(); iter!=toS->getWords().end(); ++iter)
8534  {
8535  keypointsTo.insert(keypointsTo.end(), std::make_pair(iter->first, toS->getWordsKpts()[iter->second]));
8536  }
8537  }
8538  if(newLink.type() != Link::kNeighbor && fromS->id() < toS->id())
8539  {
8540  this->updateConstraintView(newLink.inverse(), true, *toS, *fromS);
8541  ui_->graphicsView_A->setFeatures(keypointsTo, toS->sensorData().depthRaw());
8542  ui_->graphicsView_B->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
8543  }
8544  else
8545  {
8546  this->updateConstraintView(newLink, true, *fromS, *toS);
8547  ui_->graphicsView_A->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
8548  ui_->graphicsView_B->setFeatures(keypointsTo, toS->sensorData().depthRaw());
8549  }
8550 
8551  updateWordsMatching(info.inliersIDs);
8552  }
8553  else
8554  {
8555  this->updateConstraintView();
8556  }
8557  }
8558  }
8559  else if(!silent)
8560  {
8561  if(toS && fromS->id() > 0 && toS->id() > 0)
8562  {
8563  // just update matches in the views
8564  std::multimap<int, cv::KeyPoint> keypointsFrom;
8565  std::multimap<int, cv::KeyPoint> keypointsTo;
8566  if(!fromS->getWordsKpts().empty())
8567  {
8568  for(std::map<int, int>::const_iterator iter=fromS->getWords().begin(); iter!=fromS->getWords().end(); ++iter)
8569  {
8570  keypointsFrom.insert(keypointsFrom.end(), std::make_pair(iter->first, fromS->getWordsKpts()[iter->second]));
8571  }
8572  }
8573  if(!toS->getWordsKpts().empty())
8574  {
8575  for(std::map<int, int>::const_iterator iter=toS->getWords().begin(); iter!=toS->getWords().end(); ++iter)
8576  {
8577  keypointsTo.insert(keypointsTo.end(), std::make_pair(iter->first, toS->getWordsKpts()[iter->second]));
8578  }
8579  }
8580  if(currentLink.type() != Link::kNeighbor && fromS->id() < toS->id())
8581  {
8582  ui_->graphicsView_A->setFeatures(keypointsTo, toS->sensorData().depthRaw());
8583  ui_->graphicsView_B->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
8584  }
8585  else
8586  {
8587  ui_->graphicsView_A->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
8588  ui_->graphicsView_B->setFeatures(keypointsTo, toS->sensorData().depthRaw());
8589  }
8590  updateWordsMatching(info.inliersIDs);
8591  }
8592 
8593  QMessageBox::warning(this,
8594  tr("Refine link"),
8595  tr("Cannot find a transformation between nodes %1 and %2: %3").arg(currentLink.from()).arg(currentLink.to()).arg(info.rejectedMsg.c_str()));
8596  }
8597  delete fromS;
8598  delete toS;
8599 }
8600 
8601 void DatabaseViewer::addConstraint()
8602 {
8603  int from = ids_.at(ui_->horizontalSlider_A->value());
8604  int to = ids_.at(ui_->horizontalSlider_B->value());
8605  addConstraint(from, to, false);
8606 }
8607 
8608 bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool silentlyUseOptimizedGraphAsGuess)
8609 {
8610  bool switchedIds = false;
8611  if(from == to)
8612  {
8613  UWARN("Cannot add link to same node");
8614  return false;
8615  }
8616  else if(from > to)
8617  {
8618  int tmp = from;
8619  from = to;
8620  to = tmp;
8621  switchedIds = true;
8622  }
8623  std::list<Signature*> signatures;
8624  Signature * fromS=0;
8625  Signature * toS=0;
8626 
8627  Link newLink;
8628  RegistrationInfo info;
8629  if(!containsLink(linksAdded_, from, to) &&
8630  !containsLink(links_, from, to))
8631  {
8632  UASSERT(!containsLink(linksRemoved_, from, to));
8633  UASSERT(!containsLink(linksRefined_, from, to));
8634 
8635  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
8636  Registration * reg = Registration::create(parameters);
8637 
8638  bool loopCovLimited = Parameters::defaultRGBDLoopCovLimited();
8639  Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), loopCovLimited);
8640  std::vector<double> odomMaxInf = odomMaxInf_;
8641  if(loopCovLimited && odomMaxInf_.empty())
8642  {
8643  odomMaxInf = graph::getMaxOdomInf(updateLinksWithModifications(links_));
8644  }
8645 
8646  Transform t;
8647 
8648  std::list<int> ids;
8649  ids.push_back(from);
8650  ids.push_back(to);
8651  dbDriver_->loadSignatures(ids, signatures);
8652  if(signatures.size() != 2)
8653  {
8654  for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
8655  {
8656  delete *iter;
8657  }
8658  return false;
8659  }
8660  fromS = *signatures.begin();
8661  toS = *signatures.rbegin();
8662 
8663  bool reextractVisualFeatures = uStr2Bool(parameters.at(Parameters::kRGBDLoopClosureReextractFeatures()));
8664  if(reg->isScanRequired() ||
8665  reg->isUserDataRequired() ||
8666  reextractVisualFeatures ||
8667  !silent)
8668  {
8669  // Add sensor data to generate features
8670  dbDriver_->loadNodeData(fromS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
8671  fromS->sensorData().uncompressData();
8672  dbDriver_->loadNodeData(toS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
8673  toS->sensorData().uncompressData();
8674  if(reextractVisualFeatures)
8675  {
8676  fromS->removeAllWords();
8677  fromS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
8678  toS->removeAllWords();
8679  toS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
8680  }
8681  if(reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked())
8682  {
8683  // generate laser scans from depth image
8684  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom = util3d::cloudFromSensorData(
8685  fromS->sensorData(),
8686  ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
8687  ui_->doubleSpinBox_icp_maxDepth->value(),
8688  ui_->doubleSpinBox_icp_minDepth->value(),
8689  0,
8690  ui_->parameters_toolbox->getParameters());
8691  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo = util3d::cloudFromSensorData(
8692  toS->sensorData(),
8693  ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
8694  ui_->doubleSpinBox_icp_maxDepth->value(),
8695  ui_->doubleSpinBox_icp_minDepth->value(),
8696  0,
8697  ui_->parameters_toolbox->getParameters());
8698 
8699  if(cloudFrom->empty() && cloudTo->empty())
8700  {
8701  std::string msg = "Option to generate scan from depth is checked (GUI Parameters->Refine), but "
8702  "resulting clouds from depth are empty. Transformation estimation will likely "
8703  "fails. Uncheck the parameter to use laser scans.";
8704  UWARN(msg.c_str());
8705  if(!silent)
8706  {
8707  QMessageBox::warning(this,
8708  tr("Add link"),
8709  tr("%1").arg(msg.c_str()));
8710  }
8711  }
8712  else if(!fromS->sensorData().laserScanCompressed().isEmpty() || !toS->sensorData().laserScanCompressed().isEmpty())
8713  {
8714  UWARN("There are laser scans in data, but generate laser scan from "
8715  "depth image option is activated (GUI Parameters->Refine). Ignoring saved laser scans...");
8716  }
8717 
8718  int maxLaserScans = cloudFrom->size();
8719  fromS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0));
8720  toS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0));
8721  }
8722  }
8723  else if(!reextractVisualFeatures && fromS->getWords().empty() && toS->getWords().empty())
8724  {
8725  std::string msg = uFormat("\"%s\" is false and signatures (%d and %d) don't have words, "
8726  "registration will not be possible. Set \"%s\" to true.",
8727  Parameters::kRGBDLoopClosureReextractFeatures().c_str(),
8728  fromS->id(),
8729  toS->id(),
8730  Parameters::kRGBDLoopClosureReextractFeatures().c_str());
8731  UWARN(msg.c_str());
8732  if(!silent)
8733  {
8734  QMessageBox::warning(this,
8735  tr("Add link"),
8736  tr("%1").arg(msg.c_str()));
8737  }
8738  }
8739 
8740  Transform guess;
8741  bool guessFromGraphRejected = false;
8742  if(!reg->isImageRequired())
8743  {
8744  // make a fake guess using globally optimized poses
8745  if(graphes_.size())
8746  {
8747  std::map<int, Transform> optimizedPoses = graphes_.back();
8748  if(optimizedPoses.size() > 0)
8749  {
8750  std::map<int, Transform>::iterator fromIter = optimizedPoses.find(from);
8751  std::map<int, Transform>::iterator toIter = optimizedPoses.find(to);
8752  if(fromIter != optimizedPoses.end() &&
8753  toIter != optimizedPoses.end())
8754  {
8755  if(!silent)
8756  {
8757  if(QMessageBox::question(this,
8758  tr("Add constraint from optimized graph"),
8759  tr("Registration is done without vision (see %1 parameter), "
8760  "do you want to use a guess from the optimized graph?"
8761  ""
8762  "\n\nOtherwise, if "
8763  "the database has images, it is recommended to use %2=2 instead so that "
8764  "the guess can be found visually.")
8765  .arg(Parameters::kRegStrategy().c_str()).arg(Parameters::kRegStrategy().c_str()),
8766  QMessageBox::Yes | QMessageBox::No,
8767  QMessageBox::Yes) == QMessageBox::Yes)
8768  {
8769  guess = fromIter->second.inverse() * toIter->second;
8770  }
8771  else
8772  {
8773  guessFromGraphRejected = true;
8774  }
8775  }
8776  else if(silentlyUseOptimizedGraphAsGuess)
8777  {
8778  guess = fromIter->second.inverse() * toIter->second;
8779  }
8780  }
8781  }
8782  }
8783  if(guess.isNull() && !silent && !guessFromGraphRejected)
8784  {
8785  if(QMessageBox::question(this,
8786  tr("Add constraint without guess"),
8787  tr("Registration is done without vision (see %1 parameter) and we cannot (or we don't want to) find relative "
8788  "transformation between the nodes with the current graph. Do you want to use an identity "
8789  "transform for ICP guess? "
8790  ""
8791  "\n\nOtherwise, if the database has images, it is recommended to use %2=2 "
8792  "instead so that the guess can be found visually.")
8793  .arg(Parameters::kRegStrategy().c_str()).arg(Parameters::kRegStrategy().c_str()),
8794  QMessageBox::Yes | QMessageBox::Abort,
8795  QMessageBox::Abort) == QMessageBox::Yes)
8796  {
8797  guess.setIdentity();
8798  }
8799  else
8800  {
8801  guessFromGraphRejected = true;
8802  }
8803  }
8804  }
8805 
8806  if(switchedIds)
8807  {
8808  t = reg->computeTransformationMod(*toS, *fromS, guess.isNull()?guess:guess.inverse(), &info);
8809  if(!t.isNull())
8810  {
8811  t = t.inverse();
8812  }
8813  }
8814  else
8815  {
8816  t = reg->computeTransformationMod(*fromS, *toS, guess, &info);
8817  }
8818  delete reg;
8819  UDEBUG("");
8820 
8821  if(!t.isNull())
8822  {
8823  cv::Mat information = info.covariance.inv();
8824  if(odomMaxInf.size() == 6 && information.cols==6 && information.rows==6)
8825  {
8826  for(int i=0; i<6; ++i)
8827  {
8828  if(information.at<double>(i,i) > odomMaxInf[i])
8829  {
8830  information.at<double>(i,i) = odomMaxInf[i];
8831  }
8832  }
8833  }
8834 
8835  newLink = Link(from, to, Link::kUserClosure, t, information);
8836  }
8837  else if(!silent && !guessFromGraphRejected)
8838  {
8839  QMessageBox::StandardButton button = QMessageBox::warning(this,
8840  tr("Add link"),
8841  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()),
8842  QMessageBox::Yes | QMessageBox::No,
8843  QMessageBox::No);
8844  if(button == QMessageBox::Yes)
8845  {
8846  editConstraint();
8847  silent = true;
8848  }
8849  }
8850  }
8851  else if(containsLink(linksRemoved_, from, to))
8852  {
8853  newLink = rtabmap::graph::findLink(linksRemoved_, from, to)->second;
8854  }
8855 
8856  bool updateConstraints = newLink.isValid();
8857  float maxOptimizationError = uStr2Float(ui_->parameters_toolbox->getParameters().at(Parameters::kRGBDOptimizeMaxError()));
8858  if(newLink.isValid() &&
8859  maxOptimizationError > 0.0f &&
8860  uStr2Int(ui_->parameters_toolbox->getParameters().at(Parameters::kOptimizerIterations())) > 0.0f)
8861  {
8862  int fromId = newLink.from();
8863  std::multimap<int, Link> linksIn = updateLinksWithModifications(links_);
8864  linksIn.insert(std::make_pair(newLink.from(), newLink));
8865  const Link * maxLinearLink = 0;
8866  const Link * maxAngularLink = 0;
8867  float maxLinearErrorRatio = 0.0f;
8868  float maxAngularErrorRatio = 0.0f;
8869  Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
8870  std::map<int, Transform> poses;
8871  std::multimap<int, Link> links;
8872  UASSERT(odomPoses_.find(fromId) != odomPoses_.end());
8873  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());
8874  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());
8875  optimizer->getConnectedGraph(fromId, odomPoses_, linksIn, poses, links);
8876  // use already optimized poses
8877  if(graphes_.size())
8878  {
8879  const std::map<int, Transform> & optimizedPoses = graphes_.back();
8880  for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
8881  {
8882  if(optimizedPoses.find(iter->first) != optimizedPoses.end())
8883  {
8884  iter->second = optimizedPoses.at(iter->first);
8885  }
8886  }
8887  }
8888  UASSERT(poses.find(fromId) != poses.end());
8889  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());
8890  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());
8891  UASSERT(graph::findLink(links, newLink.from(), newLink.to()) != links.end());
8892  std::map<int, Transform> posesIn = poses;
8893  poses = optimizer->optimize(fromId, posesIn, links);
8894  if(posesIn.size() && poses.empty())
8895  {
8896  UWARN("Optimization failed... (poses=%d, links=%d).", (int)posesIn.size(), (int)links.size());
8897  }
8898  std::string msg;
8899  if(poses.size())
8900  {
8901  float maxLinearError = 0.0f;
8902  float maxAngularError = 0.0f;
8903  graph::computeMaxGraphErrors(
8904  poses,
8905  links,
8906  maxLinearErrorRatio,
8907  maxAngularErrorRatio,
8908  maxLinearError,
8909  maxAngularError,
8910  &maxLinearLink,
8911  &maxAngularLink);
8912  if(maxLinearLink)
8913  {
8914  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()));
8915  if(maxLinearErrorRatio > maxOptimizationError)
8916  {
8917  msg = uFormat("Rejecting edge %d->%d because "
8918  "graph error is too large (abs=%f m) after optimization (ratio %f for edge %d->%d, stddev=%f m). "
8919  "\"%s\" is %f.",
8920  newLink.from(),
8921  newLink.to(),
8922  maxLinearError,
8923  maxLinearErrorRatio,
8924  maxLinearLink->from(),
8925  maxLinearLink->to(),
8926  sqrt(maxLinearLink->transVariance()),
8927  Parameters::kRGBDOptimizeMaxError().c_str(),
8928  maxOptimizationError);
8929  }
8930  }
8931  if(maxAngularLink)
8932  {
8933  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()));
8934  if(maxAngularErrorRatio > maxOptimizationError)
8935  {
8936  msg = uFormat("Rejecting edge %d->%d because "
8937  "graph error is too large (abs=%f deg) after optimization (ratio %f for edge %d->%d, stddev=%f deg). "
8938  "\"%s\" is %f.",
8939  newLink.from(),
8940  newLink.to(),
8941  maxAngularError*180.0f/CV_PI,
8942  maxAngularErrorRatio,
8943  maxAngularLink->from(),
8944  maxAngularLink->to(),
8945  sqrt(maxAngularLink->rotVariance()),
8946  Parameters::kRGBDOptimizeMaxError().c_str(),
8947  maxOptimizationError);
8948  }
8949  }
8950  }
8951  else
8952  {
8953  msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
8954  newLink.from(),
8955  newLink.to());
8956  }
8957  if(!msg.empty())
8958  {
8959  UWARN("%s", msg.c_str());
8960  if(!silent)
8961  {
8962  QMessageBox::warning(this,
8963  tr("Add link"),
8964  tr("%1").arg(msg.c_str()));
8965  }
8966 
8967  updateConstraints = false;
8968  }
8969 
8970  if(updateConstraints && silent && !graphes_.empty() && graphes_.back().size() == poses.size())
8971  {
8972  graphes_.back() = poses;
8973  }
8974  }
8975 
8976  if(updateConstraints)
8977  {
8978  if(containsLink(linksRemoved_, from, to))
8979  {
8980  //simply remove from linksRemoved
8981  linksRemoved_.erase(rtabmap::graph::findLink(linksRemoved_, from, to));
8982  }
8983  else
8984  {
8985  if(newLink.from() < newLink.to())
8986  {
8987  newLink = newLink.inverse();
8988  }
8989  linksAdded_.insert(std::make_pair(newLink.from(), newLink));
8990  }
8991  }
8992 
8993  if(!silent)
8994  {
8995  if(fromS && toS)
8996  {
8997  if((updateConstraints && newLink.from() > newLink.to()) || (!updateConstraints && switchedIds))
8998  {
8999  Signature * tmpS = fromS;
9000  fromS = toS;
9001  toS = tmpS;
9002  }
9003 
9004  if(updateConstraints)
9005  {
9006  updateLoopClosuresSlider(fromS->id(), toS->id());
9007  this->updateGraphView();
9008  this->updateConstraintView(newLink, false, *fromS, *toS);
9009  }
9010 
9011  std::multimap<int, cv::KeyPoint> keypointsFrom;
9012  std::multimap<int, cv::KeyPoint> keypointsTo;
9013  if(!fromS->getWordsKpts().empty())
9014  {
9015  for(std::map<int, int>::const_iterator iter=fromS->getWords().begin(); iter!=fromS->getWords().end(); ++iter)
9016  {
9017  keypointsFrom.insert(keypointsFrom.end(), std::make_pair(iter->first, fromS->getWordsKpts()[iter->second]));
9018  }
9019  }
9020  if(!toS->getWordsKpts().empty())
9021  {
9022  for(std::map<int, int>::const_iterator iter=toS->getWords().begin(); iter!=toS->getWords().end(); ++iter)
9023  {
9024  keypointsTo.insert(keypointsTo.end(), std::make_pair(iter->first, toS->getWordsKpts()[iter->second]));
9025  }
9026  }
9027  ui_->graphicsView_A->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
9028  ui_->graphicsView_B->setFeatures(keypointsTo, toS->sensorData().depthRaw());
9029  updateWordsMatching(info.inliersIDs);
9030  }
9031  else if(updateConstraints)
9032  {
9033  updateLoopClosuresSlider(from, to);
9034  this->updateGraphView();
9035  }
9036  }
9037 
9038  for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
9039  {
9040  delete *iter;
9041  }
9042 
9043  return updateConstraints;
9044 }
9045 
9046 void DatabaseViewer::resetConstraint()
9047 {
9048  int from = ids_.at(ui_->horizontalSlider_A->value());
9049  int to = ids_.at(ui_->horizontalSlider_B->value());
9050  if(ui_->label_type->text().toInt() == Link::kLandmark)
9051  {
9052  int position = ui_->horizontalSlider_loops->value();
9053  const rtabmap::Link & link = loopLinks_.at(position);
9054  from = link.from();
9055  to = link.to();
9056  }
9057 
9058  if(from < to)
9059  {
9060  int tmp = to;
9061  to = from;
9062  from = tmp;
9063  }
9064 
9065  if(from == to)
9066  {
9067  UWARN("Cannot reset link to same node");
9068  return;
9069  }
9070 
9071 
9072  std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, from, to);
9073  if(iter != linksRefined_.end())
9074  {
9075  linksRefined_.erase(iter);
9076  this->updateGraphView();
9077  }
9078 
9079  updateConstraintView();
9080 }
9081 
9082 void DatabaseViewer::rejectConstraint()
9083 {
9084  int priorId = sender() == ui_->toolButton_remove_priorA?ids_.at(ui_->horizontalSlider_A->value()):
9085  sender() == ui_->toolButton_remove_priorB?ids_.at(ui_->horizontalSlider_B->value()):0;
9086 
9087  int from = priorId>0?priorId:ids_.at(ui_->horizontalSlider_A->value());
9088  int to = priorId>0?priorId:ids_.at(ui_->horizontalSlider_B->value());
9089  if(priorId==0 && ui_->label_type->text().toInt() == Link::kLandmark)
9090  {
9091  int position = ui_->horizontalSlider_loops->value();
9092  const rtabmap::Link & link = loopLinks_.at(position);
9093  from = link.from();
9094  to = link.to();
9095  }
9096 
9097  if(from < to)
9098  {
9099  int tmp = to;
9100  to = from;
9101  from = tmp;
9102  }
9103 
9104  if(priorId==0 && from == to)
9105  {
9106  UWARN("Cannot reject link to same node");
9107  return;
9108  }
9109 
9110  bool removed = false;
9111 
9112  // find the original one
9113  std::multimap<int, Link>::iterator iter;
9114  iter = rtabmap::graph::findLink(links_, from, to);
9115  if(iter != links_.end())
9116  {
9117  if(iter->second.type() == Link::kNeighbor || iter->second.type() == Link::kNeighborMerged)
9118  {
9119  QMessageBox::StandardButton button = QMessageBox::warning(this, tr("Reject link"),
9120  tr("Removing the neighbor link %1->%2 will split the graph. Do you want to continue?").arg(from).arg(to),
9121  QMessageBox::Yes | QMessageBox::No, QMessageBox::No);
9122  if(button != QMessageBox::Yes)
9123  {
9124  return;
9125  }
9126  }
9127  linksRemoved_.insert(*iter);
9128  removed = true;
9129  }
9130 
9131  // remove from refined and added
9132  iter = rtabmap::graph::findLink(linksRefined_, from, to);
9133  if(iter != linksRefined_.end())
9134  {
9135  linksRefined_.erase(iter);
9136  removed = true;
9137  }
9138  iter = rtabmap::graph::findLink(linksAdded_, from, to);
9139  if(iter != linksAdded_.end())
9140  {
9141  linksAdded_.erase(iter);
9142  removed = true;
9143  }
9144  if(removed)
9145  {
9146  if(priorId==0)
9147  {
9148  this->updateGraphView();
9149  }
9150  else
9151  {
9152  bool priorsIgnored = Parameters::defaultOptimizerPriorsIgnored();
9153  Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kOptimizerPriorsIgnored(), priorsIgnored);
9154  int indexA = ui_->horizontalSlider_A->value();
9155  int indexB = ui_->horizontalSlider_B->value();
9156  if(!priorsIgnored)
9157  {
9158  this->updateGraphView();
9159  }
9160  if(ui_->horizontalSlider_A->value() != indexA)
9161  ui_->horizontalSlider_A->setValue(indexA);
9162  else
9163  sliderAValueChanged(indexA);
9164  if(ui_->horizontalSlider_B->value() != indexB)
9165  ui_->horizontalSlider_B->setValue(indexB);
9166  else
9167  sliderBValueChanged(indexB);
9168  }
9169  }
9170 }
9171 
9172 std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
9173  const std::multimap<int, rtabmap::Link> & edgeConstraints)
9174 {
9175  UDEBUG("linksAdded_=%d linksRefined_=%d linksRemoved_=%d", (int)linksAdded_.size(), (int)linksRefined_.size(), (int)linksRemoved_.size());
9176 
9177  std::multimap<int, rtabmap::Link> links;
9178  for(std::multimap<int, rtabmap::Link>::const_iterator iter=edgeConstraints.begin();
9179  iter!=edgeConstraints.end();
9180  ++iter)
9181  {
9182  std::multimap<int, rtabmap::Link>::iterator findIter;
9183 
9184  findIter = rtabmap::graph::findLink(linksRemoved_, iter->second.from(), iter->second.to());
9185  if(findIter != linksRemoved_.end())
9186  {
9187  UDEBUG("Removed link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
9188  continue; // don't add this link
9189  }
9190 
9191  findIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
9192  if(findIter!=linksRefined_.end())
9193  {
9194  // add the refined link
9195  if(iter->second.from() == findIter->second.to() &&
9196  iter->second.from() != iter->second.to())
9197  {
9198  links.insert(std::make_pair(iter->second.from(), findIter->second.inverse()));
9199  }
9200  else
9201  {
9202  links.insert(*findIter);
9203  }
9204  UDEBUG("Updated link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
9205  continue;
9206  }
9207 
9208  links.insert(*iter); // add original link
9209  }
9210 
9211  //look for added links
9212  for(std::multimap<int, rtabmap::Link>::const_iterator iter=linksAdded_.begin();
9213  iter!=linksAdded_.end();
9214  ++iter)
9215  {
9216  std::multimap<int, rtabmap::Link>::iterator findIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
9217  if(findIter!=linksRefined_.end())
9218  {
9219  links.insert(*findIter); // add the refined link
9220  links.insert(std::make_pair(findIter->second.to(), findIter->second.inverse())); // return both ways
9221  UDEBUG("Added refined link (%d->%d, %d)", findIter->second.from(), findIter->second.to(), findIter->second.type());
9222  continue;
9223  }
9224 
9225  UDEBUG("Added link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
9226  links.insert(*iter);
9227  links.insert(std::make_pair(iter->second.to(), iter->second.inverse())); // return both ways
9228  }
9229 
9230  return links;
9231 }
9232 
9233 void DatabaseViewer::updateNeighborsSlider(int from, int to)
9234 {
9235  UDEBUG("%d %d", from, to);
9236  neighborLinks_.clear();
9237  std::multimap<int, Link> links = updateLinksWithModifications(links_);
9238  int position = ui_->horizontalSlider_neighbors->value();
9239  std::multimap<int, Link> linksSortedByChildren;
9240  for(std::multimap<int, rtabmap::Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
9241  {
9242  if(iter->second.from() < iter->second.to())
9243  {
9244  linksSortedByChildren.insert(*iter);
9245  }
9246  }
9247 
9248  for(std::multimap<int, rtabmap::Link>::iterator iter = linksSortedByChildren.begin(); iter!=linksSortedByChildren.end(); ++iter)
9249  {
9250  if(!iter->second.transform().isNull())
9251  {
9252  if(iter->second.type() == rtabmap::Link::kNeighbor ||
9253  iter->second.type() == rtabmap::Link::kNeighborMerged)
9254  {
9255  if((iter->second.from() == from && iter->second.to() == to) ||
9256  (iter->second.to() == from && iter->second.from() == to))
9257  {
9258  position = neighborLinks_.size();
9259  }
9260  neighborLinks_.append(iter->second);
9261  }
9262  }
9263  else
9264  {
9265  UERROR("Transform null for link from %d to %d", iter->first, iter->second.to());
9266  }
9267  }
9268 
9269  if(neighborLinks_.size())
9270  {
9271  if(neighborLinks_.size() == 1)
9272  {
9273  // just to be able to move the cursor of the neighbor slider
9274  neighborLinks_.push_back(neighborLinks_.front());
9275  }
9276  ui_->horizontalSlider_neighbors->setMinimum(0);
9277  ui_->horizontalSlider_neighbors->setMaximum(neighborLinks_.size()-1);
9278  ui_->horizontalSlider_neighbors->setEnabled(true);
9279  if(position != ui_->horizontalSlider_neighbors->value())
9280  {
9281  ui_->horizontalSlider_neighbors->setValue(position);
9282  }
9283  else
9284  {
9285  this->updateConstraintView(neighborLinks_.at(position));
9286  }
9287  }
9288  else
9289  {
9290  ui_->horizontalSlider_neighbors->setEnabled(false);
9291  constraintsViewer_->removeAllClouds();
9292  constraintsViewer_->refreshView();
9293  updateConstraintButtons();
9294  }
9295 }
9296 
9297 void DatabaseViewer::updateLoopClosuresSlider(int from, int to)
9298 {
9299  UDEBUG("%d %d", from, to);
9300  loopLinks_.clear();
9301  std::multimap<int, Link> links = updateLinksWithModifications(links_);
9302  int position = ui_->horizontalSlider_loops->value();
9303  std::multimap<int, Link> linksSortedByParents;
9304  for(std::multimap<int, rtabmap::Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
9305  {
9306  if(iter->second.to() > iter->second.from() && iter->second.from() < 0) // landmark
9307  {
9308  linksSortedByParents.insert(std::make_pair(iter->second.to(), iter->second.inverse()));
9309  }
9310  else if(iter->second.to() < iter->second.from())
9311  {
9312  linksSortedByParents.insert(*iter);
9313  }
9314  }
9315  for(std::multimap<int, rtabmap::Link>::iterator iter = linksSortedByParents.begin(); iter!=linksSortedByParents.end(); ++iter)
9316  {
9317  if(!iter->second.transform().isNull())
9318  {
9319  if(iter->second.type() != rtabmap::Link::kNeighbor &&
9320  iter->second.type() != rtabmap::Link::kNeighborMerged)
9321  {
9322  if((iter->second.from() == from && iter->second.to() == to) ||
9323  (iter->second.to() == from && iter->second.from() == to))
9324  {
9325  position = loopLinks_.size();
9326  }
9327  loopLinks_.append(iter->second);
9328  }
9329  }
9330  else
9331  {
9332  UERROR("Transform null for link from %d to %d", iter->first, iter->second.to());
9333  }
9334  }
9335 
9336  if(loopLinks_.size())
9337  {
9338  if(loopLinks_.size() == 1)
9339  {
9340  // just to be able to move the cursor of the loop slider
9341  loopLinks_.push_back(loopLinks_.front());
9342  }
9343  ui_->horizontalSlider_loops->setMinimum(0);
9344  ui_->horizontalSlider_loops->setMaximum(loopLinks_.size()-1);
9345  ui_->horizontalSlider_loops->setEnabled(true);
9346  if(position != ui_->horizontalSlider_loops->value())
9347  {
9348  ui_->horizontalSlider_loops->setValue(position);
9349  }
9350  else
9351  {
9352  this->updateConstraintView(loopLinks_.at(position));
9353  }
9354  }
9355  else
9356  {
9357  ui_->horizontalSlider_loops->setEnabled(false);
9358  constraintsViewer_->removeAllClouds();
9359  constraintsViewer_->refreshView();
9360  updateConstraintButtons();
9361  }
9362 }
9363 
9364 void DatabaseViewer::notifyParametersChanged(const QStringList & parametersChanged)
9365 {
9366  bool updateStereo = false;
9367  bool updateGraphView = false;
9368  for(QStringList::const_iterator iter=parametersChanged.constBegin();
9369  iter!=parametersChanged.constEnd() && (!updateStereo || !updateGraphView);
9370  ++iter)
9371  {
9372  QString group = iter->split('/').first();
9373  if(!updateStereo && group == "Stereo")
9374  {
9375  updateStereo = true;
9376  continue;
9377  }
9378  if(!updateGraphView && group == "Optimize")
9379  {
9380  updateGraphView = true;
9381  continue;
9382  }
9383  }
9384 
9385  if(updateStereo)
9386  {
9387  this->updateStereo();
9388  }
9389  if(updateGraphView)
9390  {
9391  this->updateGraphView();
9392  }
9393  this->configModified();
9394 }
9395 
9396 } // namespace rtabmap
rtabmap::SensorData
Definition: SensorData.h:51
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
w
RowVector3d w
int
int
rtabmap::DBDriver::loadWords
void loadWords(const std::set< int > &wordIds, std::list< VisualWord * > &vws)
Definition: DBDriver.cpp:617
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:127
rtabmap::util3d::laserScanToPointCloudINormal
pcl::PointCloud< pcl::PointXYZINormal >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudINormal(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
Definition: util3d.cpp:2402
rtabmap::DatabaseViewer::updateLoggerLevel
void updateLoggerLevel()
Definition: DatabaseViewer.cpp:5759
rtabmap::CloudViewer::setCameraLockZ
void setCameraLockZ(bool enabled=true)
Definition: CloudViewer.cpp:3304
rtabmap::DatabaseViewer::refineLinks
void refineLinks()
Definition: DatabaseViewer.cpp:4481
rtabmap::util3d::cloudFromDepthRGB
RTABMAP_DEPRECATED pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT cloudFromDepthRGB(const cv::Mat &imageRgb, const cv::Mat &imageDepth, float cx, float cy, float fx, float fy, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0)
Definition: util3d.cpp:416
Compression.h
rtabmap::DBDriver::loadOptimizedPoses
std::map< int, Transform > loadOptimizedPoses(Transform *lastlocalizationPose=0) const
Definition: DBDriver.cpp:1237
rtabmap::DatabaseViewer::regenerateCurrentLocalMaps
void regenerateCurrentLocalMaps()
Definition: DatabaseViewer.cpp:3925
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:686
rtabmap::SensorData::setLaserScan
void setLaserScan(const LaserScan &laserScan, bool clearPreviousData=true)
Definition: SensorData.cpp:381
rtabmap::DatabaseViewer::DatabaseViewer
DatabaseViewer(const QString &ini=QString(), QWidget *parent=0)
Definition: DatabaseViewer.cpp:109
rtabmap::CloudViewer::updateCameraTargetPosition
void updateCameraTargetPosition(const Transform &pose)
Definition: CloudViewer.cpp:2979
rtabmap::DatabaseViewer::generateGraph
void generateGraph()
Definition: DatabaseViewer.cpp:3673
rtabmap::DBDriver::getCalibration
bool getCalibration(int signatureId, std::vector< CameraModel > &models, std::vector< StereoCameraModel > &stereoModels) const
Definition: DBDriver.cpp:737
rtabmap::ProgressDialog
Definition: ProgressDialog.h:43
rtabmap::DBDriver::getNodesMemoryUsed
long getNodesMemoryUsed() const
Definition: DBDriver.cpp:119
UFile::getName
std::string getName()
Definition: UFile.h:135
gtsam::Values::insert
void insert(const Values &values)
rtabmap::Feature2D::create
static Feature2D * create(const ParametersMap &parameters=ParametersMap())
Definition: Features2d.cpp:608
save
save
rtabmap::DBDriver::updateLink
void updateLink(const Link &link)
Definition: DBDriver.cpp:477
UINFO
#define UINFO(...)
name
rtabmap::CloudViewer::isFrustumShown
bool isFrustumShown() const
Definition: CloudViewer.cpp:2471
rtabmap::StereoCameraModel
Definition: StereoCameraModel.h:35
rtabmap::DatabaseViewer::exportDatabase
void exportDatabase()
Definition: DatabaseViewer.cpp:1355
rtabmap::util3d::createPolygonIndexes
void RTABMAP_CORE_EXPORT 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
rtabmap::DBDriver::getLastParameters
ParametersMap getLastParameters() const
Definition: DBDriver.cpp:239
rtabmap::SensorData::uncompressData
void uncompressData()
Definition: SensorData.cpp:528
uMean
T uMean(const T *v, unsigned int size)
Definition: UMath.h:399
rtabmap::KeypointItem::keypoint
const cv::KeyPoint & keypoint() const
Definition: KeypointItem.h:48
glm::length
GLM_FUNC_DECL genType::value_type length(genType const &x)
rtabmap::EditMapArea::isModified
bool isModified() const
Definition: EditMapArea.h:54
rtabmap::Feature2D::generateKeypoints
std::vector< cv::KeyPoint > generateKeypoints(const cv::Mat &image, const cv::Mat &mask=cv::Mat())
Definition: Features2d.cpp:767
rtabmap::EditDepthArea::getModifiedImage
cv::Mat getModifiedImage() const
Definition: EditDepthArea.cpp:117
rtabmap::util3d::laserScanToPointCloud2
pcl::PCLPointCloud2::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloud2(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2230
rtabmap::DatabaseViewer::sliderBMoved
void sliderBMoved(int)
Definition: DatabaseViewer.cpp:6057
rtabmap::Registration::computeTransformationMod
Transform computeTransformationMod(Signature &from, Signature &to, Transform guess=Transform::getIdentity(), RegistrationInfo *info=0) const
Definition: Registration.cpp:189
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
util3d_surface.h
bytes
format
std::string format(const std::string &str, const std::vector< std::string > &find, const std::vector< std::string > &replace)
set
v1
v1
rtabmap::LaserScan::maxPoints
int maxPoints() const
Definition: LaserScan.h:120
rtabmap::DatabaseViewer::gpsPoses_
std::map< int, rtabmap::Transform > gpsPoses_
Definition: DatabaseViewer.h:232
arg::arg
constexpr arg(const char *name=nullptr)
rtabmap::CloudViewer::removeOccupancyGridMap
void removeOccupancyGridMap()
Definition: CloudViewer.cpp:1595
rtabmap::DatabaseViewer::selectObstacleColor
void selectObstacleColor()
Definition: DatabaseViewer.cpp:2372
rtabmap::LaserScan::backwardCompatibility
static LaserScan backwardCompatibility(const cv::Mat &oldScanFormat, int maxPoints=0, int maxRange=0, const Transform &localTransform=Transform::getIdentity())
Definition: LaserScan.cpp:133
rtabmap::DatabaseViewer::showEvent
virtual void showEvent(QShowEvent *anEvent)
Definition: DatabaseViewer.cpp:1305
rtabmap::LaserScan::rangeMax
float rangeMax() const
Definition: LaserScan.h:122
timer
rtabmap::DataRecorder::init
bool init(const QString &path, bool recordInRAM=true)
Definition: DataRecorder.cpp:67
s
RealScalar s
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:1074
rtabmap::DatabaseViewer::infoReducedGraph_
bool infoReducedGraph_
Definition: DatabaseViewer.h:254
rtabmap::DatabaseViewer::exportPoses
void exportPoses(int format)
Definition: DatabaseViewer.cpp:2467
rtabmap::LaserScan::hasRGB
bool hasRGB() const
Definition: LaserScan.h:135
rtabmap::graph::getMaxOdomInf
std::vector< double > RTABMAP_CORE_EXPORT getMaxOdomInf(const std::multimap< int, Link > &links)
Definition: Graph.cpp:1010
rtabmap::DatabaseViewer::weights_
std::map< int, int > weights_
Definition: DatabaseViewer.h:219
rtabmap::DatabaseViewer::graphes_
std::list< std::map< int, rtabmap::Transform > > graphes_
Definition: DatabaseViewer.h:228
rtabmap::DatabaseViewer::editMapArea_
EditMapArea * editMapArea_
Definition: DatabaseViewer.h:247
b
Array< int, 3, 1 > b
rtabmap::DatabaseViewer::extractImages
void extractImages()
Definition: DatabaseViewer.cpp:1530
rtabmap::DBDriver::getImagesMemoryUsed
long getImagesMemoryUsed() const
Definition: DBDriver.cpp:135
rtabmap::compressedDepthFormat
std::string RTABMAP_CORE_EXPORT compressedDepthFormat(const cv::Mat &bytes)
Definition: Compression.cpp:332
rtabmap::DatabaseViewer::ui_
Ui_DatabaseViewer * ui_
Definition: DatabaseViewer.h:211
Horizontal
Horizontal
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:745
rtabmap::ExportCloudsDialog::forceAssembling
void forceAssembling(bool enabled)
Definition: ExportCloudsDialog.cpp:337
rtabmap::ExportCloudsDialog
Definition: ExportCloudsDialog.h:54
rtabmap::EditMapArea
Definition: EditMapArea.h:45
rtabmap::RegistrationIcp
Definition: RegistrationIcp.h:39
rtabmap::LinkRefiningDialog::isRangeByNodeId
bool isRangeByNodeId() const
Definition: LinkRefiningDialog.cpp:106
rtabmap::DBDriver::openConnection
bool openConnection(const std::string &url, bool overwritten=false)
Definition: DBDriver.cpp:86
rtabmap::LinkRefiningDialog
Definition: LinkRefiningDialog.h:41
rtabmap::graph::radiusPosesClustering
std::multimap< int, int > RTABMAP_CORE_EXPORT radiusPosesClustering(const std::map< int, Transform > &poses, float radius, float angle)
Definition: Graph.cpp:1477
rtabmap::CloudViewer::setPolygonPicking
void setPolygonPicking(bool enabled)
Definition: CloudViewer.cpp:2758
rtabmap::Registration
Definition: Registration.h:39
rtabmap::DBDriver::getLastNodesSize
int getLastNodesSize() const
Definition: DBDriver.cpp:207
rtabmap::CloudViewer::setCloudOpacity
void setCloudOpacity(const std::string &id, double opacity=1.0)
Definition: CloudViewer.cpp:3249
UFile::length
long length()
Definition: UFile.h:110
rtabmap::ExportDialog::sessionExported
int sessionExported() const
Definition: ExportDialog.cpp:140
rtabmap::ExportCloudsDialog::isGainRGB
bool isGainRGB() const
Definition: ExportCloudsDialog.cpp:1571
rtabmap::DBDriver::addLink
void addLink(const Link &link)
Definition: DBDriver.cpp:467
rtabmap::LinkRefiningDialog::isRangeByMapId
bool isRangeByMapId() const
Definition: LinkRefiningDialog.cpp:111
rtabmap::Signature::getWords
const std::multimap< int, int > & getWords() const
Definition: Signature.h:112
c
Scalar Scalar * c
rtabmap::SensorData::laserScanCompressed
const LaserScan & laserScanCompressed() const
Definition: SensorData.h:181
rtabmap::ExportCloudsDialog::loadSettings
void loadSettings(QSettings &settings, const QString &group="")
Definition: ExportCloudsDialog.cpp:535
ExportDialog.h
rtabmap::DatabaseViewer::graphLinkSelected
void graphLinkSelected(int, int)
Definition: DatabaseViewer.cpp:4631
rtabmap::CloudViewer::removeAllLines
void removeAllLines()
Definition: CloudViewer.cpp:1866
rtabmap::RegistrationInfo
Definition: RegistrationInfo.h:34
UDirectory.h
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:411
size
Index size
rtabmap::CloudViewer::addOrUpdateGraph
void addOrUpdateGraph(const std::string &id, const pcl::PointCloud< pcl::PointXYZ >::Ptr &graph, const QColor &color=Qt::gray)
Definition: CloudViewer.cpp:2296
this
this
rtabmap::CameraModel
Definition: CameraModel.h:38
rtabmap::EditMapArea::getModifiedMap
cv::Mat getModifiedMap() const
Definition: EditMapArea.cpp:79
rtabmap::DBDriver::loadSignature
Signature * loadSignature(int id, bool *loadedFromTrash=0)
Definition: DBDriver.cpp:548
uStr2Bool
bool UTILITE_EXPORT uStr2Bool(const char *str)
Definition: UConversion.cpp:170
EditMapArea.h
rtabmap::OctoMap::createProjectionMap
cv::Mat createProjectionMap(float &xMin, float &yMin, float &gridCellSize, float minGridSize=0.0f, unsigned int treeDepth=0)
Definition: OctoMap.cpp:989
rtabmap::DatabaseViewer::notifyParametersChanged
void notifyParametersChanged(const QStringList &)
Definition: DatabaseViewer.cpp:9364
rtabmap::EditDepthArea::setColorMap
void setColorMap(uCvQtDepthColorMap type)
Definition: EditDepthArea.cpp:161
rtabmap::CloudViewer::removeGraph
void removeGraph(const std::string &id)
Definition: CloudViewer.cpp:2333
rtabmap::DatabaseViewer::updateGraphView
void updateGraphView()
Definition: DatabaseViewer.cpp:7392
rtabmap::graph::exportPoses
bool RTABMAP_CORE_EXPORT 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::Signature::getWordsKpts
const std::vector< cv::KeyPoint > & getWordsKpts() const
Definition: Signature.h:113
rtabmap::DatabaseViewer::links_
std::multimap< int, rtabmap::Link > links_
Definition: DatabaseViewer.h:234
uMin
T uMin(const T *v, unsigned int size, unsigned int &index)
Definition: UMath.h:157
ULogger::setLevel
static void setLevel(ULogger::Level level)
Definition: ULogger.h:339
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:116
rtabmap::Optimizer
Definition: Optimizer.h:61
SensorData.h
rtabmap::GeodeticCoords::latitude
const double & latitude() const
Definition: GeodeticCoords.h:58
rtabmap::util3d::voxelize
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
Definition: util3d_filtering.cpp:761
rtabmap::DatabaseViewer::wmStates_
std::map< int, std::vector< int > > wmStates_
Definition: DatabaseViewer.h:220
rtabmap::util3d::projectCloudToCameras
std::vector< std::pair< std::pair< int, int >, pcl::PointXY > > RTABMAP_CORE_EXPORT projectCloudToCameras(const pcl::PointCloud< pcl::PointXYZRGBNormal > &cloud, const std::map< int, Transform > &cameraPoses, const std::map< int, std::vector< CameraModel > > &cameraModels, float maxDistance=0.0f, float maxAngle=0.0f, const std::vector< float > &roiRatios=std::vector< float >(), const cv::Mat &projMask=cv::Mat(), bool distanceToCamPolicy=false, const ProgressState *state=0)
updated
updated
rtabmap::GeodeticCoords::fromENU_WGS84
void fromENU_WGS84(const cv::Point3d &enu, const GeodeticCoords &origin)
Definition: GeodeticCoords.cpp:178
GeodeticCoords.h
rtabmap::DatabaseViewer::generateLocalGraph
void generateLocalGraph()
Definition: DatabaseViewer.cpp:3688
rtabmap::DatabaseViewer::resetAllChanges
void resetAllChanges()
Definition: DatabaseViewer.cpp:4599
rtabmap::Optimizer::create
static Optimizer * create(const ParametersMap &parameters)
Definition: Optimizer.cpp:72
rtabmap::DatabaseViewer::closeEvent
virtual void closeEvent(QCloseEvent *event)
Definition: DatabaseViewer.cpp:1261
rtabmap::LinkRefiningDialog::getRangeNodeId
void getRangeNodeId(int &from, int &to) const
Definition: LinkRefiningDialog.cpp:116
rtabmap::GPS::altitude
const double & altitude() const
Definition: GPS.h:62
rtabmap::GPS
Definition: GPS.h:35
rtabmap::Stereo
Definition: Stereo.h:38
rtabmap::ProgressDialog::setValue
void setValue(int value)
Definition: ProgressDialog.cpp:112
rtabmap_netvlad.img
img
Definition: rtabmap_netvlad.py:78
rtabmap::DatabaseViewer::editDepthDialog_
QDialog * editDepthDialog_
Definition: DatabaseViewer.h:244
rtabmap::DatabaseViewer::selectFrontierColor
void selectFrontierColor()
Definition: DatabaseViewer.cpp:2396
rtabmap::ProgressDialog::appendText
void appendText(const QString &text, const QColor &color=Qt::black)
Definition: ProgressDialog.cpp:102
ProgressDialog.h
rtabmap::DatabaseViewer::eventFilter
virtual bool eventFilter(QObject *obj, QEvent *event)
Definition: DatabaseViewer.cpp:1345
type
rtabmap::DatabaseViewer::showCloseButton
void showCloseButton(bool visible=true)
Definition: DatabaseViewer.cpp:540
rtabmap::LocalGridCache::shareTo
bool shareTo(int nodeId, LocalGridCache &anotherCache) const
Definition: LocalGrid.cpp:77
state
RecoveryProgressState state
Definition: tools/Recovery/main.cpp:56
rtabmap::DBDriver::getAllStatisticsWmStates
std::map< int, std::vector< int > > getAllStatisticsWmStates() const
Definition: DBDriver.cpp:266
rtabmap::DatabaseViewer::editConstraint
void editConstraint()
Definition: DatabaseViewer.cpp:6100
rtabmap::EnvSensor::kAmbientAirPressure
@ kAmbientAirPressure
Definition: EnvSensor.h:41
rtabmap::DatabaseViewer::sliderAMoved
void sliderAMoved(int)
Definition: DatabaseViewer.cpp:6042
rtabmap::DatabaseViewer::constraintsViewer_
CloudViewer * constraintsViewer_
Definition: DatabaseViewer.h:212
rtabmap::GeodeticCoords
Definition: GeodeticCoords.h:52
rtabmap::DatabaseViewer::editDepthArea_
EditDepthArea * editDepthArea_
Definition: DatabaseViewer.h:245
rtabmap::util3d::assembleTextureMesh
pcl::TextureMesh::Ptr RTABMAP_CORE_EXPORT 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:1272
rtabmap::GPS::error
const double & error() const
Definition: GPS.h:63
rtabmap::DatabaseViewer::firstCall_
bool firstCall_
Definition: DatabaseViewer.h:251
rtabmap::util3d::clusterPolygons
std::list< std::list< int > > RTABMAP_CORE_EXPORT clusterPolygons(const std::vector< std::set< int > > &neighborPolygons, int minClusterSize=0)
Definition: util3d_surface.cpp:131
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::exportPosesKITTI
void exportPosesKITTI()
Definition: DatabaseViewer.cpp:2450
rtabmap::DatabaseViewer::updateOptimizedMesh
void updateOptimizedMesh()
Definition: DatabaseViewer.cpp:3540
rtabmap::DatabaseViewer::linksRemoved_
std::multimap< int, rtabmap::Link > linksRemoved_
Definition: DatabaseViewer.h:237
y
Matrix3f y
rtabmap::LocalGridCache
Definition: LocalGrid.h:56
rtabmap::ExportCloudsDialog::isExposeFusion
bool isExposeFusion() const
Definition: ExportCloudsDialog.cpp:1599
true
#define true
Definition: ConvertUTF.c:57
rtabmap::util3d::convertPolygonsFromPCL
std::vector< std::vector< RTABMAP_PCL_INDEX > > RTABMAP_CORE_EXPORT convertPolygonsFromPCL(const std::vector< pcl::Vertices > &polygons)
Definition: util3d_surface.cpp:1227
rtabmap::Transform::fromString
static Transform fromString(const std::string &string)
Definition: Transform.cpp:475
uStr2Double
double UTILITE_EXPORT uStr2Double(const std::string &str)
Definition: UConversion.cpp:147
iterator
rtabmap::LocalGridCache::localGrids
const std::map< int, LocalGrid > & localGrids() const
Definition: LocalGrid.h:78
rtabmap::LaserScan::hasNormals
bool hasNormals() const
Definition: LaserScan.h:134
rtabmap::DatabaseViewer::containsLink
bool containsLink(std::multimap< int, Link > &links, int from, int to)
Definition: DatabaseViewer.cpp:8071
rtabmap::KeypointItem
Definition: KeypointItem.h:41
rtabmap::DBDriver::loadNodeData
void loadNodeData(Signature *signature, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const
Definition: DBDriver.cpp:659
rtabmap::CloudMap::getMapGround
const pcl::PointCloud< pcl::PointXYZRGB >::Ptr & getMapGround() const
Definition: CloudMap.h:47
rtabmap::EditConstraintDialog::setPoseGroupVisible
void setPoseGroupVisible(bool visible)
Definition: EditConstraintDialog.cpp:73
LocalGridMaker.h
util3d.h
rtabmap::LocalGridMaker::createLocalMap
void createLocalMap(const Signature &node, cv::Mat &groundCells, cv::Mat &obstacleCells, cv::Mat &emptyCells, cv::Point3f &viewPoint)
Definition: LocalGridMaker.cpp:188
rtabmap::LaserScan
Definition: LaserScan.h:37
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:825
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
LinkRefiningDialog.h
Vector2::y
float y
rtabmap::DatabaseViewer::idToIndex_
QMap< int, int > idToIndex_
Definition: DatabaseViewer.h:221
rtabmap::util3d::laserScanFromPointCloud
LaserScan laserScanFromPointCloud(const PointCloud2T &cloud, bool filterNaNs, bool is2D, const Transform &transform)
Definition: util3d.hpp:37
rtabmap::CloudViewer::removeAllFrustums
void removeAllFrustums(bool exceptCameraReference=false)
Definition: CloudViewer.cpp:2283
rtabmap::EnvSensor::kAmbientLight
@ kAmbientLight
Definition: EnvSensor.h:42
UTimer.h
ULOGGER_DEBUG
#define ULOGGER_DEBUG(...)
Definition: ULogger.h:53
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:131
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::DatabaseViewer::localMaps_
LocalGridCache localMaps_
Definition: DatabaseViewer.h:240
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:240
rtabmap::CloudViewer::addOctomap
bool addOctomap(const OctoMap *octomap, unsigned int treeDepth=0, bool volumeRepresentation=true)
Definition: CloudViewer.cpp:1113
rtabmap::GridMap
Definition: GridMap.h:44
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:923
rtabmap::DatabaseViewer::lastWmIds_
std::set< int > lastWmIds_
Definition: DatabaseViewer.h:217
KeypointItem.h
rtabmap::OccupancyGrid
Definition: global_map/OccupancyGrid.h:40
vB
Vector vB(size_t i)
rtabmap::DBDriver::getUserDataMemoryUsed
long getUserDataMemoryUsed() const
Definition: DBDriver.cpp:175
rtabmap::DatabaseViewer::configModified
void configModified()
Definition: DatabaseViewer.cpp:545
rtabmap::DatabaseViewer::cloudViewer_
CloudViewer * cloudViewer_
Definition: DatabaseViewer.h:213
rtabmap::DatabaseViewer::groundTruthPoses_
std::map< int, rtabmap::Transform > groundTruthPoses_
Definition: DatabaseViewer.h:231
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:130
rtabmap::DatabaseViewer::mapIds_
std::map< int, int > mapIds_
Definition: DatabaseViewer.h:218
rtabmap::DatabaseViewer::sliderAValueChanged
void sliderAValueChanged(int)
Definition: DatabaseViewer.cpp:4639
rtabmap::DBDriver::getDepthImagesMemoryUsed
long getDepthImagesMemoryUsed() const
Definition: DBDriver.cpp:143
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:503
rtabmap::ExportDialog::isOdomExported
bool isOdomExported() const
Definition: ExportDialog.cpp:160
rtabmap::SensorData::stereoCameraModels
const std::vector< StereoCameraModel > & stereoCameraModels() const
Definition: SensorData.h:241
rtabmap::DBDriver::save2DMap
void save2DMap(const cv::Mat &map, float xMin, float yMin, float cellSize) const
Definition: DBDriver.cpp:1245
indices
indices
rtabmap::Transform::getTranslationAndEulerAngles
void getTranslationAndEulerAngles(float &x, float &y, float &z, float &roll, float &pitch, float &yaw) const
Definition: Transform.cpp:258
rtabmap::util3d::organizedFastMesh
std::vector< pcl::Vertices > RTABMAP_CORE_EXPORT 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::CloudViewer::getFrustumColor
QColor getFrustumColor() const
Definition: CloudViewer.cpp:2481
rtabmap::EnvSensor::kAmbientTemperature
@ kAmbientTemperature
Definition: EnvSensor.h:40
rtabmap::DatabaseViewer::exportOptimizedMesh
void exportOptimizedMesh()
Definition: DatabaseViewer.cpp:3425
uSplitNumChar
std::list< std::string > uSplitNumChar(const std::string &str)
Definition: UStl.h:670
rtabmap::CloudMap::getMapObstacles
const pcl::PointCloud< pcl::PointXYZRGB >::Ptr & getMapObstacles() const
Definition: CloudMap.h:48
VisualWord.h
rtabmap::DatabaseViewer::updateConstraintView
void updateConstraintView()
Definition: DatabaseViewer.cpp:6237
rtabmap::Transform::toEigen3f
Eigen::Affine3f toEigen3f() const
Definition: Transform.cpp:391
odometry
Pose2 odometry(2.0, 0.0, 0.0)
n
int n
GainCompensator.h
rtabmap::util2d::fillDepthHoles
cv::Mat RTABMAP_CORE_EXPORT fillDepthHoles(const cv::Mat &depth, int maximumHoleSize=1, float errorRatio=0.02f)
Definition: util2d.cpp:1439
rtabmap::LocalGridCache::empty
bool empty() const
Definition: LocalGrid.h:77
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:117
view
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set view
rtabmap::ExportCloudsDialog::setProgressDialogToMax
void setProgressDialogToMax()
Definition: ExportCloudsDialog.cpp:350
rtabmap::DBDriver::getLastDictionarySize
int getLastDictionarySize() const
Definition: DBDriver.cpp:215
rtabmap::CloudViewer
Definition: CloudViewer.h:79
rtabmap::DatabaseViewer::updateAllNeighborCovariances
void updateAllNeighborCovariances()
Definition: DatabaseViewer.cpp:4365
rtabmap::LocalGrid
Definition: LocalGrid.h:38
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:441
rtabmap::DataRecorder
Definition: DataRecorder.h:46
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_CORE_EXPORT transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::Parameters::filterParameters
static ParametersMap filterParameters(const ParametersMap &parameters, const std::string &group, bool remove=false)
Definition: Parameters.cpp:217
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:1145
rtabmap::DatabaseViewer::update
void update(int value, QSpinBox *spinBoxIndex, QLabel *labelParents, QLabel *labelChildren, QLabel *weight, QLabel *label, QLabel *stamp, rtabmap::ImageView *view, QLabel *labelId, QLabel *labelMapId, QLabel *labelPose, QLabel *labelOptPose, QLabel *labelVelocity, QLabel *labelCalib, QLabel *labelScan, QLabel *labelGravity, QLabel *labelPrior, QToolButton *editPriorButton, QToolButton *removePriorButton, QLabel *labelGps, QLabel *labelGt, QLabel *labelSensors, bool updateConstraintView)
Definition: DatabaseViewer.cpp:4693
rtabmap::ExportCloudsDialog::getTextureSize
int getTextureSize() const
Definition: ExportCloudsDialog.cpp:1550
rtabmap::util3d::mergeTextures
cv::Mat RTABMAP_CORE_EXPORT 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:1438
rtabmap::Parameters::getDefaultParameters
static const ParametersMap & getDefaultParameters()
Definition: Parameters.h:905
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:407
rtabmap::DatabaseViewer::moveEvent
virtual void moveEvent(QMoveEvent *anEvent)
Definition: DatabaseViewer.cpp:1315
rtabmap::Transform::z
float & z()
Definition: Transform.h:94
vA
Vector vA(size_t i)
rtabmap::DBDriver::getStatisticsMemoryUsed
long getStatisticsMemoryUsed() const
Definition: DBDriver.cpp:199
rtabmap::graph::calcKittiSequenceErrors
void RTABMAP_CORE_EXPORT calcKittiSequenceErrors(const std::vector< Transform > &poses_gt, const std::vector< Transform > &poses_result, float &t_err, float &r_err)
Definition: Graph.cpp:662
uKeys
std::vector< K > uKeys(const std::multimap< K, V > &mm)
Definition: UStl.h:67
rtabmap::ExportCloudsDialog::getGainBeta
double getGainBeta() const
Definition: ExportCloudsDialog.cpp:1567
rtabmap::DatabaseViewer::~DatabaseViewer
virtual ~DatabaseViewer()
Definition: DatabaseViewer.cpp:515
rtabmap::DatabaseViewer::openDatabase
void openDatabase()
Definition: DatabaseViewer.cpp:841
rtabmap::util3d::transformPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
Definition: util3d_transforms.cpp:107
rtabmap::DatabaseViewer::octomap_
OctoMap * octomap_
Definition: DatabaseViewer.h:242
rtabmap::util3d::cloudRGBFromSensorData
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT 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:1268
rtabmap::ProgressDialog::isCanceled
bool isCanceled() const
Definition: ProgressDialog.h:57
rtabmap::ProgressDialog::setCancelButtonVisible
void setCancelButtonVisible(bool visible)
Definition: ProgressDialog.cpp:97
RecoveryState.h
rtabmap::graph::calcRMSE
Transform RTABMAP_CORE_EXPORT 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, bool align2D=false)
Definition: Graph.cpp:772
rtabmap::LocalGridMaker::getCellSize
float getCellSize() const
Definition: LocalGridMaker.h:49
rtabmap::DatabaseViewer::exportGPS_KML
void exportGPS_KML()
Definition: DatabaseViewer.cpp:2856
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:326
uStrNumCmp
int uStrNumCmp(const std::string &a, const std::string &b)
Definition: UStl.h:717
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:185
rtabmap::CloudViewer::addElevationMap
bool addElevationMap(const cv::Mat &map32FC1, float resolution, float xMin, float yMin, float opacity)
Definition: CloudViewer.cpp:1610
data
int data[]
rtabmap::DatabaseViewer::viewOptimizedMesh
void viewOptimizedMesh()
Definition: DatabaseViewer.cpp:3373
rtabmap::DatabaseViewer::updateConstraintButtons
void updateConstraintButtons()
Definition: DatabaseViewer.cpp:6902
rtabmap::Registration::isImageRequired
bool isImageRequired() const
Definition: Registration.cpp:90
rtabmap::CloudViewer::addOrUpdateFrustum
void addOrUpdateFrustum(const std::string &id, const Transform &pose, const Transform &localTransform, double scale, const QColor &color=QColor(), float fovX=1.1, float fovY=0.85)
Definition: CloudViewer.cpp:2146
delta
def delta(g0, g1)
util3d_transforms.h
rtabmap::DatabaseViewer::graphLinks_
std::multimap< int, rtabmap::Link > graphLinks_
Definition: DatabaseViewer.h:229
rtabmap::util3d::convertImage8U2Map
cv::Mat RTABMAP_CORE_EXPORT convertImage8U2Map(const cv::Mat &map8U, bool pgmFormat=false)
Definition: util3d_mapping.cpp:945
UDirectory::getDir
static std::string getDir(const std::string &filePath)
Definition: UDirectory.cpp:273
Features2d.h
rtabmap::DatabaseViewer::restoreDefaultSettings
void restoreDefaultSettings()
Definition: DatabaseViewer.cpp:781
rtabmap::DatabaseViewer::linksAdded_
std::multimap< int, rtabmap::Link > linksAdded_
Definition: DatabaseViewer.h:236
CloudMap.h
g
float g
j
std::ptrdiff_t j
rtabmap::DatabaseViewer::generate3DMap
void generate3DMap()
Definition: DatabaseViewer.cpp:4105
uNumber2Str
std::string UTILITE_EXPORT uNumber2Str(unsigned int number)
Definition: UConversion.cpp:91
rtabmap::DatabaseViewer::findActiveLink
Link findActiveLink(int from, int to)
Definition: DatabaseViewer.cpp:8044
rtabmap::graph::computePathLength
float RTABMAP_CORE_EXPORT computePathLength(const std::vector< std::pair< int, Transform > > &path, unsigned int fromIndex=0, unsigned int toIndex=0)
Definition: Graph.cpp:2360
rtabmap::EditConstraintDialog
Definition: EditConstraintDialog.h:40
UConversion.h
Some conversion functions.
rtabmap::LocalGridMaker
Definition: LocalGridMaker.h:42
rtabmap::DatabaseViewer::exportPosesG2O
void exportPosesG2O()
Definition: DatabaseViewer.cpp:2458
rtabmap::LocalGridCache::clear
void clear(bool temporaryOnly=false)
Definition: LocalGrid.cpp:103
rtabmap::DatabaseViewer::ids_
QList< int > ids_
Definition: DatabaseViewer.h:216
rtabmap::DatabaseViewer::editMapDialog_
QDialog * editMapDialog_
Definition: DatabaseViewer.h:246
rtabmap::DatabaseViewer::updateStatistics
void updateStatistics()
Definition: DatabaseViewer.cpp:2325
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
rtabmap::CloudViewer::getFrustumScale
float getFrustumScale() const
Definition: CloudViewer.cpp:2476
CloudViewer.h
rtabmap::DatabaseViewer::updateStereo
void updateStereo()
Definition: DatabaseViewer.cpp:5767
rtabmap::GainCompensator
Definition: GainCompensator.h:44
rtabmap::DatabaseViewer::linksRefined_
std::multimap< int, rtabmap::Link > linksRefined_
Definition: DatabaseViewer.h:235
rtabmap::DatabaseViewer::keyPressEvent
virtual void keyPressEvent(QKeyEvent *event)
Definition: DatabaseViewer.cpp:1336
rtabmap::StereoDense
Definition: StereoDense.h:38
rtabmap::DatabaseViewer::updateInfo
void updateInfo()
Definition: DatabaseViewer.cpp:2191
util3d_filtering.h
rtabmap::GlobalMap::update
bool update(const std::map< int, Transform > &poses)
Definition: GlobalMap.cpp:102
rtabmap::DatabaseViewer::resizeEvent
virtual void resizeEvent(QResizeEvent *anEvent)
Definition: DatabaseViewer.cpp:1328
rtabmap::DatabaseViewer::savedMaximized_
bool savedMaximized_
Definition: DatabaseViewer.h:250
rtabmap::DatabaseViewer::recoverDatabase
void recoverDatabase()
Definition: DatabaseViewer.cpp:1232
rtabmap::GPS::bearing
const double & bearing() const
Definition: GPS.h:64
rtabmap::util3d::laserScanToPointCloudRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudRGB(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2322
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:133
rtabmap::CloudViewer::buildPickingLocator
void buildPickingLocator(bool enable)
Definition: CloudViewer.cpp:3571
Signature.h
rtabmap::DatabaseViewer::regenerateLocalMaps
void regenerateLocalMaps()
Definition: DatabaseViewer.cpp:3774
rtabmap::CloudViewer::removeCoordinate
void removeCoordinate(const std::string &id)
Definition: CloudViewer.cpp:1777
all
static const Eigen::internal::all_t all
GridMap.h
rtabmap::GPS::latitude
const double & latitude() const
Definition: GPS.h:61
rtabmap::DBDriver::getLastWordId
void getLastWordId(int &id) const
Definition: DBDriver.cpp:1043
rtabmap::GPS::longitude
const double & longitude() const
Definition: GPS.h:60
rtabmap::DBDriver::getUrl
const std::string & getUrl() const
Definition: DBDriver.h:72
arg
rtabmap::CloudViewer::addOrUpdateCoordinate
void addOrUpdateCoordinate(const std::string &id, const Transform &transform, double scale, bool foreground=false)
Definition: CloudViewer.cpp:1735
rtabmap::ExportCloudsDialog::getTextureBrightnessConstrastRatioLow
int getTextureBrightnessConstrastRatioLow() const
Definition: ExportCloudsDialog.cpp:1591
rtabmap::DatabaseViewer::exportPosesKML
void exportPosesKML()
Definition: DatabaseViewer.cpp:2462
rtabmap::util3d::isFinite
bool RTABMAP_CORE_EXPORT isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3436
Optimizer.h
rtabmap::DatabaseViewer::infoTotalOdom_
double infoTotalOdom_
Definition: DatabaseViewer.h:255
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
rtabmap::LinkRefiningDialog::getLinkType
Link::Type getLinkType() const
Definition: LinkRefiningDialog.cpp:91
UCv2Qt.h
rtabmap::ExportDialog::isDepth2dExported
bool isDepth2dExported() const
Definition: ExportDialog.cpp:155
rtabmap::Signature::getWords3
const std::vector< cv::Point3f > & getWords3() const
Definition: Signature.h:132
rtabmap::DatabaseViewer::odomPoses_
std::map< int, rtabmap::Transform > odomPoses_
Definition: DatabaseViewer.h:230
rtabmap::DatabaseViewer::graphNodeSelected
void graphNodeSelected(int)
Definition: DatabaseViewer.cpp:4618
rtabmap::GridMap::createTerrainMesh
pcl::PolygonMesh::Ptr createTerrainMesh() const
Definition: GridMap.cpp:170
rtabmap::DatabaseViewer::linkRefiningDialog_
LinkRefiningDialog * linkRefiningDialog_
Definition: DatabaseViewer.h:248
velocity
Velocity3 velocity(const NavState &X, OptionalJacobian< 3, 9 > H)
rtabmap::ExportCloudsDialog::setDBDriver
void setDBDriver(const DBDriver *dbDriver)
Definition: ExportCloudsDialog.h:66
rtabmap::EnvSensor::kAmbientRelativeHumidity
@ kAmbientRelativeHumidity
Definition: EnvSensor.h:43
rtabmap::LinkRefiningDialog::getIntraInterSessions
void getIntraInterSessions(bool &intra, bool &inter) const
Definition: LinkRefiningDialog.cpp:100
rtabmap::DatabaseViewer::addConstraint
void addConstraint()
Definition: DatabaseViewer.cpp:8601
rtabmap::util3d::laserScan2dFromPointCloud
LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
Definition: util3d.cpp:1992
UPlot::addCurve
UPlotCurve * addCurve(const QString &curveName, const QColor &color=QColor())
Definition: UPlot.cpp:2134
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:787
info
else if n * info
rtabmap::ExportDialog::isUserDataExported
bool isUserDataExported() const
Definition: ExportDialog.cpp:165
rtabmap::graph::exportGPS
bool RTABMAP_CORE_EXPORT exportGPS(const std::string &filePath, const std::map< int, GPS > &gpsValues, unsigned int rgba=0xFFFFFFFF)
Definition: Graph.cpp:510
rtabmap::CloudViewer::addOrUpdateLine
void addOrUpdateLine(const std::string &id, const Transform &from, const Transform &to, const QColor &color, bool arrow=false, bool foreground=false)
Definition: CloudViewer.cpp:1810
UASSERT
#define UASSERT(condition)
rtabmap::DatabaseViewer::exportGPS
void exportGPS(int format)
Definition: DatabaseViewer.cpp:2861
params
SmartProjectionParams params(gtsam::HESSIAN, gtsam::ZERO_ON_DEGENERACY)
rtabmap::Feature2D
Definition: Features2d.h:114
z
z
rtabmap::DBDriver::getDatabaseVersion
std::string getDatabaseVersion() const
Definition: DBDriver.cpp:275
uValue
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
Definition: UStl.h:238
x
x
rtabmap::SensorData::setGroundTruth
void setGroundTruth(const Transform &pose)
Definition: SensorData.h:283
m
Matrix3f m
rtabmap::ExportDialog::isDepthExported
bool isDepthExported() const
Definition: ExportDialog.cpp:150
p
Point3_ p(2)
rtabmap::CloudViewer::setCloudColorIndex
void setCloudColorIndex(const std::string &id, int index)
Definition: CloudViewer.cpp:3241
rtabmap::DatabaseViewer::updateGrid
void updateGrid()
Definition: DatabaseViewer.cpp:7899
rtabmap::DatabaseViewer::getIniFilePath
QString getIniFilePath() const
Definition: DatabaseViewer.cpp:571
rtabmap::DBDriver::getLaserScanInfo
bool getLaserScanInfo(int signatureId, LaserScan &info) const
Definition: DBDriver.cpp:763
rtabmap::GPS::stamp
const double & stamp() const
Definition: GPS.h:59
rtabmap::CloudViewer::updateCameraFrustums
void updateCameraFrustums(const Transform &pose, const std::vector< CameraModel > &models)
Definition: CloudViewer.cpp:3115
rtabmap::OccupancyGrid::getMap
cv::Mat getMap(float &xMin, float &yMin) const
Definition: OccupancyGrid.cpp:99
rtabmap::GeodeticCoords::altitude
const double & altitude() const
Definition: GeodeticCoords.h:60
rtabmap::DBDriver::getAllStatistics
std::map< int, std::pair< std::map< std::string, float >, double > > getAllStatistics() const
Definition: DBDriver.cpp:257
Stereo.h
rtabmap::DBDriver::getWordsMemoryUsed
long getWordsMemoryUsed() const
Definition: DBDriver.cpp:183
DBDriver.h
rtabmap::Registration::isUserDataRequired
bool isUserDataRequired() const
Definition: Registration.cpp:110
rtabmap::Transform::getDistance
float getDistance(const Transform &t) const
Definition: Transform.cpp:293
rtabmap::Transform::setIdentity
void setIdentity()
Definition: Transform.cpp:157
rtabmap::EditConstraintDialog::getTransform
Transform getTransform() const
Definition: EditConstraintDialog.cpp:123
rtabmap::CameraModel::opticalRotation
static Transform opticalRotation()
Definition: CameraModel.h:45
rtabmap::util3d::laserScanToPointCloudI
pcl::PointCloud< pcl::PointXYZI >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudI(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
Definition: util3d.cpp:2349
rtabmap::DBDriver::getCalibrationsMemoryUsed
long getCalibrationsMemoryUsed() const
Definition: DBDriver.cpp:151
rtabmap::ExportCloudsDialog::getBlendingDecimation
int getBlendingDecimation() const
Definition: ExportCloudsDialog.cpp:1579
rtabmap::Registration::isScanRequired
bool isScanRequired() const
Definition: Registration.cpp:100
util3d_mapping.h
rtabmap::DatabaseViewer::generatedLocalMaps_
LocalGridCache generatedLocalMaps_
Definition: DatabaseViewer.h:241
rtabmap::DatabaseViewer::stereoViewer_
CloudViewer * stereoViewer_
Definition: DatabaseViewer.h:214
rtabmap::util2d::depthFromDisparity
cv::Mat RTABMAP_CORE_EXPORT depthFromDisparity(const cv::Mat &disparity, float fx, float baseline, int type=CV_32FC1)
Definition: util2d.cpp:763
rtabmap::ExportDialog::targetFramerate
double targetFramerate() const
Definition: ExportDialog.cpp:135
rtabmap::DatabaseViewer::occupancyGridViewer_
CloudViewer * occupancyGridViewer_
Definition: DatabaseViewer.h:215
time
#define time
rtabmap::DatabaseViewer::modifiedLaserScans_
std::map< int, LaserScan > modifiedLaserScans_
Definition: DatabaseViewer.h:238
rtabmap::ExportDialog
Definition: ExportDialog.h:40
rtabmap::util3d::cloudsRGBFromSensorData
std::vector< pcl::PointCloud< pcl::PointXYZRGB >::Ptr > RTABMAP_CORE_EXPORT cloudsRGBFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< pcl::IndicesPtr > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:1093
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
out
std::ofstream out("Result.txt")
rtabmap::graph::findLink
std::multimap< int, Link >::iterator RTABMAP_CORE_EXPORT findLink(std::multimap< int, Link > &links, int from, int to, bool checkBothWays=true, Link::Type type=Link::kUndef)
Definition: Graph.cpp:1038
rtabmap::LaserScan::angleMax
float angleMax() const
Definition: LaserScan.h:124
rtabmap::Optimizer::type
virtual Type type() const =0
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
rtabmap::OctoMap::hasColor
bool hasColor() const
Definition: global_map/OctoMap.h:203
graph
FactorGraph< FACTOR > * graph
rtabmap::SensorData::setFeatures
void setFeatures(const std::vector< cv::KeyPoint > &keypoints, const std::vector< cv::Point3f > &keypoints3D, const cv::Mat &descriptors)
Definition: SensorData.cpp:801
rtabmap::GainCompensator::apply
void apply(int id, pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, bool rgb=true) const
Definition: GainCompensator.cpp:426
rtabmap::DBDriver::loadLinks
void loadLinks(int signatureId, std::multimap< int, Link > &links, Link::Type type=Link::kUndef) const
Definition: DBDriver.cpp:826
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:399
offset
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate offset
path
path
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
str
rtabmap::CloudViewer::removeOctomap
void removeOctomap()
Definition: CloudViewer.cpp:1324
rtabmap::LocalGridCache::add
void add(int nodeId, const cv::Mat &ground, const cv::Mat &obstacles, const cv::Mat &empty, float cellSize, const cv::Point3f &viewPoint=cv::Point3f(0, 0, 0))
Definition: LocalGrid.cpp:55
UWARN
#define UWARN(...)
rtabmap::ExportDialog::outputPath
QString outputPath() const
Definition: ExportDialog.cpp:125
UPlotCurve::addValue
void addValue(UPlotItem *data)
Definition: UPlot.cpp:423
rtabmap::EditDepthArea::setImage
void setImage(const cv::Mat &depth, const cv::Mat &rgb=cv::Mat())
Definition: EditDepthArea.cpp:78
rtabmap::DBDriver::getLastNodeIds
void getLastNodeIds(std::set< int > &ids) const
Definition: DBDriver.cpp:880
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
rtabmap::DatabaseViewer::exportDialog_
ExportCloudsDialog * exportDialog_
Definition: DatabaseViewer.h:243
rtabmap::CloudViewer::removeCloud
bool removeCloud(const std::string &id)
Definition: CloudViewer.cpp:2588
rtabmap::GridMap::createHeightMap
cv::Mat createHeightMap(float &xMin, float &yMin, float &cellSize) const
Definition: GridMap.cpp:66
rtabmap::DBDriver::getLinksMemoryUsed
long getLinksMemoryUsed() const
Definition: DBDriver.cpp:127
rtabmap::DatabaseViewer::updateAllLandmarkCovariances
void updateAllLandmarkCovariances()
Definition: DatabaseViewer.cpp:4395
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::DatabaseViewer::infoTotalTime_
double infoTotalTime_
Definition: DatabaseViewer.h:256
M_PI
#define M_PI
Definition: tango-gl/include/tango-gl/util.h:66
rtabmap::DatabaseViewer::lastSliderIndexBrowsed_
int lastSliderIndexBrowsed_
Definition: DatabaseViewer.h:224
rtabmap::DatabaseViewer::updateAllLoopClosureCovariances
void updateAllLoopClosureCovariances()
Definition: DatabaseViewer.cpp:4379
rtabmap::CloudMap
Definition: CloudMap.h:40
rtabmap::DBDriver::removeLink
void removeLink(int from, int to)
Definition: DBDriver.cpp:473
getPose
Pose3_ getPose(const Expression< PinholeCamera< CALIBRATION > > &cam)
rtabmap::DatabaseViewer::readSettings
void readSettings()
Definition: DatabaseViewer.cpp:585
cameraPoses
std::array< Pose3, 3 > cameraPoses
rtabmap::DatabaseViewer::exportPosesRGBDSLAMMotionCapture
void exportPosesRGBDSLAMMotionCapture()
Definition: DatabaseViewer.cpp:2438
rtabmap::DatabaseViewer::closeDatabase
bool closeDatabase()
Definition: DatabaseViewer.cpp:965
set::clear
void clear()
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:183
rtabmap::DatabaseViewer::iniFilePath_
QString iniFilePath_
Definition: DatabaseViewer.h:252
ULogger.h
ULogger class and convenient macros.
a
ArrayXXi a
rtabmap::DatabaseViewer::writeSettings
void writeSettings()
Definition: DatabaseViewer.cpp:681
UPlot.h
rtabmap::DatabaseViewer::updateCovariances
void updateCovariances(const QList< Link > &links)
Definition: DatabaseViewer.cpp:4409
rtabmap::util3d::assemblePolygonMesh
pcl::PolygonMesh::Ptr RTABMAP_CORE_EXPORT assemblePolygonMesh(const cv::Mat &cloudMat, const std::vector< std::vector< RTABMAP_PCL_INDEX > > &polygons)
Definition: util3d_surface.cpp:1395
rtabmap::Transform
Definition: Transform.h:41
rtabmap::util3d::cloudFromSensorData
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT 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:1040
rtabmap::CloudViewer::addOccupancyGridMap
bool addOccupancyGridMap(const cv::Mat &map8U, float resolution, float xMin, float yMin, float opacity)
Definition: CloudViewer.cpp:1530
Memory.h
empty
rtabmap::DatabaseViewer::regenerateSavedMap
void regenerateSavedMap()
Definition: DatabaseViewer.cpp:3280
util2d.h
rtabmap::LinkRefiningDialog::setMinMax
void setMinMax(int nodeIdMin, int nodeIdMax, int mapIdMin, int mapIdMax)
Definition: LinkRefiningDialog.cpp:70
rtabmap::graph::computeMaxGraphErrors
void RTABMAP_CORE_EXPORT 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, bool for3DoF=false)
Definition: Graph.cpp:910
rtabmap::util2d::cvtDepthFromFloat
cv::Mat RTABMAP_CORE_EXPORT cvtDepthFromFloat(const cv::Mat &depth32F)
Definition: util2d.cpp:892
rtabmap::CloudMap::getMapEmptyCells
const pcl::PointCloud< pcl::PointXYZ >::Ptr & getMapEmptyCells() const
Definition: CloudMap.h:49
UDirectory::exists
static bool exists(const std::string &dirPath)
Definition: UDirectory.cpp:249
uSplit
std::list< std::string > uSplit(const std::string &str, char separator=' ')
Definition: UStl.h:564
rtabmap::StereoDense::computeDisparity
virtual cv::Mat computeDisparity(const cv::Mat &leftImage, const cv::Mat &rightImage) const =0
rtabmap::DatabaseViewer::selectEmptyColor
void selectEmptyColor()
Definition: DatabaseViewer.cpp:2388
rtabmap::ExportCloudsDialog::isBlending
bool isBlending() const
Definition: ExportCloudsDialog.cpp:1575
rtabmap::EnvSensors
std::map< EnvSensor::Type, EnvSensor > EnvSensors
Definition: EnvSensor.h:81
uStr2Int
int UTILITE_EXPORT uStr2Int(const std::string &str)
Definition: UConversion.cpp:125
rtabmap::Parameters::readINI
static void readINI(const std::string &configFile, ParametersMap &parameters, bool modifiedOnly=false)
Definition: Parameters.cpp:1288
UPlotCurve
Definition: UPlot.h:93
rtabmap::DatabaseViewer::import2DMap
void import2DMap()
Definition: DatabaseViewer.cpp:3225
Graph.h
rtabmap::CloudViewer::clear
virtual void clear()
Definition: CloudViewer.cpp:261
rtabmap::ExportCloudsDialog::isGainCompensation
bool isGainCompensation() const
Definition: ExportCloudsDialog.cpp:1563
rtabmap::DatabaseViewer::gpsValues_
std::map< int, GPS > gpsValues_
Definition: DatabaseViewer.h:233
rtabmap::DatabaseViewer::exportGPS_TXT
void exportGPS_TXT()
Definition: DatabaseViewer.cpp:2852
rtabmap::DatabaseViewer::databaseFileName_
std::string databaseFileName_
Definition: DatabaseViewer.h:227
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
rtabmap::DatabaseViewer::resetConstraint
void resetConstraint()
Definition: DatabaseViewer.cpp:9046
values
leaf::MyValues values
rtabmap::DatabaseViewer::rejectConstraint
void rejectConstraint()
Definition: DatabaseViewer.cpp:9082
rtabmap::ExportDialog::framesIgnored
int framesIgnored() const
Definition: ExportDialog.cpp:130
OctoMap.h
rtabmap::SensorData::depthRaw
cv::Mat depthRaw() const
Definition: SensorData.h:210
iter
iterator iter(handle obj)
rtabmap::DatabaseViewer::infoSessions_
int infoSessions_
Definition: DatabaseViewer.h:257
rtabmap::Transform::rotation
Transform rotation() const
Definition: Transform.cpp:195
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:1275
ULOGGER_ERROR
#define ULOGGER_ERROR(...)
Definition: ULogger.h:56
rtabmap::DatabaseViewer::exportPosesRaw
void exportPosesRaw()
Definition: DatabaseViewer.cpp:2434
ParametersToolBox.h
rtabmap::ExportDialog::isRgbExported
bool isRgbExported() const
Definition: ExportDialog.cpp:145
origin
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set origin
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
rtabmap::DatabaseViewer::editSaved2DMap
void editSaved2DMap()
Definition: DatabaseViewer.cpp:2894
rtabmap::DatabaseViewer::selectGroundColor
void selectGroundColor()
Definition: DatabaseViewer.cpp:2380
rtabmap::Optimizer::kTypeTORO
@ kTypeTORO
Definition: Optimizer.h:66
rtabmap::DatabaseViewer::refineConstraint
void refineConstraint()
Definition: DatabaseViewer.cpp:8076
RegistrationVis.h
util3d_registration.h
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
c_str
const char * c_str(Args &&...args)
rtabmap::GeodeticCoords::toENU_WGS84
cv::Point3d toENU_WGS84(const GeodeticCoords &origin) const
Definition: GeodeticCoords.cpp:108
rtabmap::LaserScan::empty
bool empty() const
Definition: LaserScan.h:129
rtabmap::DBDriver::getFeaturesMemoryUsed
long getFeaturesMemoryUsed() const
Definition: DBDriver.cpp:191
ExportCloudsDialog.h
diff
diff
rtabmap::Registration::create
static Registration * create(const ParametersMap &parameters)
Definition: Registration.cpp:39
v
Array< int, Dynamic, 1 > v
position
Point3 position(const NavState &X, OptionalJacobian< 3, 9 > H)
UDEBUG
#define UDEBUG(...)
rtabmap::ExportCloudsDialog::setOkButton
void setOkButton()
Definition: ExportCloudsDialog.cpp:1096
rtabmap::Transform::getTranslation
void getTranslation(float &x, float &y, float &z) const
Definition: Transform.cpp:269
rtabmap::util3d::removeNaNFromPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT removeNaNFromPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud)
Definition: util3d_filtering.cpp:1150
rtabmap::Parameters::getType
static std::string getType(const std::string &paramKey)
Definition: Parameters.cpp:473
rtabmap::ProgressDialog::incrementStep
void incrementStep(int steps=1)
Definition: ProgressDialog.cpp:138
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
RegistrationIcp.h
rtabmap::util3d::projectDisparityTo3D
cv::Point3f RTABMAP_CORE_EXPORT projectDisparityTo3D(const cv::Point2f &pt, float disparity, const StereoCameraModel &model)
Definition: util3d.cpp:2653
UTimer
Definition: UTimer.h:46
rtabmap::DatabaseViewer::updateWordsMatching
void updateWordsMatching(const std::vector< int > &inliers=std::vector< int >())
Definition: DatabaseViewer.cpp:5956
rtabmap::LaserScan::angleIncrement
float angleIncrement() const
Definition: LaserScan.h:125
rtabmap::DatabaseViewer::updateIds
void updateIds()
Definition: DatabaseViewer.cpp:1737
rtabmap::ExportCloudsDialog::getTextureBrightnessConstrastRatioHigh
int getTextureBrightnessConstrastRatioHigh() const
Definition: ExportCloudsDialog.cpp:1595
rtabmap::Stereo::create
static Stereo * create(const ParametersMap &parameters=ParametersMap())
Definition: Stereo.cpp:39
rtabmap::OccupancyGrid::getProbMap
cv::Mat getProbMap(float &xMin, float &yMin) const
Definition: OccupancyGrid.cpp:142
rtabmap::DBDriver::load2DMap
cv::Mat load2DMap(float &xMin, float &yMin, float &cellSize) const
Definition: DBDriver.cpp:1252
rtabmap::DatabaseViewer::sliderIterationsValueChanged
void sliderIterationsValueChanged(int)
Definition: DatabaseViewer.cpp:6972
rtabmap::ImageView
Definition: ImageView.h:49
rtabmap::CloudViewer::setCameraFree
void setCameraFree()
Definition: CloudViewer.cpp:3298
rtabmap::DatabaseViewer::loopLinks_
QList< rtabmap::Link > loopLinks_
Definition: DatabaseViewer.h:223
rtabmap::CloudViewer::removeAllClouds
void removeAllClouds()
Definition: CloudViewer.cpp:2575
rtabmap::DatabaseViewer::dbDriver_
rtabmap::DBDriver * dbDriver_
Definition: DatabaseViewer.h:225
float
float
rtabmap::DatabaseViewer::updateNeighborsSlider
void updateNeighborsSlider(int from=0, int to=0)
Definition: DatabaseViewer.cpp:9233
rtabmap::DatabaseViewer::updateLoopClosuresSlider
void updateLoopClosuresSlider(int from=0, int to=0)
Definition: DatabaseViewer.cpp:9297
rtabmap::CloudViewer::removeElevationMap
void removeElevationMap()
Definition: CloudViewer.cpp:1727
rtabmap::util3d::laserScanToPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2269
rtabmap::DatabaseViewer::neighborLinks_
QList< rtabmap::Link > neighborLinks_
Definition: DatabaseViewer.h:222
rtabmap::StereoDense::create
static StereoDense * create(const ParametersMap &parameters)
Definition: StereoDense.cpp:33
prior
const auto prior
rtabmap::RecoveryState
Definition: RecoveryState.h:39
rtabmap::DBDriver::updateLaserScan
void updateLaserScan(int nodeId, const LaserScan &scan)
Definition: DBDriver.cpp:525
rtabmap::DatabaseViewer::update3dView
void update3dView()
Definition: DatabaseViewer.cpp:6072
rtabmap::GPS::toGeodeticCoords
GeodeticCoords toGeodeticCoords() const
Definition: GPS.h:66
rtabmap::DBDriver::getLaserScansMemoryUsed
long getLaserScansMemoryUsed() const
Definition: DBDriver.cpp:167
DataRecorder.h
rtabmap::DBDriver::updateDepthImage
void updateDepthImage(int nodeId, const cv::Mat &image, const std::string &format)
Definition: DBDriver.cpp:515
rtabmap::OctoMap
Definition: global_map/OctoMap.h:175
ULogger::Level
Level
Definition: ULogger.h:252
rtabmap::DBDriver::getAllNodeIds
void getAllNodeIds(std::set< int > &ids, bool ignoreChildren=false, bool ignoreBadSignatures=false, bool ignoreIntermediateNodes=false) const
Definition: DBDriver.cpp:887
uMax
T uMax(const T *v, unsigned int size, unsigned int &index)
Definition: UMath.h:92
Vector2::x
float x
rtabmap::GeodeticCoords::longitude
const double & longitude() const
Definition: GeodeticCoords.h:59
rtabmap::DatabaseViewer::updateGraphRotation
void updateGraphRotation()
Definition: DatabaseViewer.cpp:7378
rtabmap::LaserScan::hasIntensity
bool hasIntensity() const
Definition: LaserScan.h:136
rtabmap::DatabaseViewer::detectMoreLoopClosures
void detectMoreLoopClosures()
Definition: DatabaseViewer.cpp:4224
false
#define false
Definition: ConvertUTF.c:56
transform
EIGEN_DONT_INLINE void transform(const Quaternion< Scalar > &t, Data &data)
uIsDigit
bool uIsDigit(const char c)
Definition: UStl.h:620
OccupancyGrid.h
uCvMat2QImage
QImage uCvMat2QImage(const cv::Mat &image, bool isBgr=true, uCvQtDepthColorMap colorMap=uCvQtDepthWhiteToBlack, float depthMin=0, float depthMax=0)
Definition: UCv2Qt.h:47
uValues
std::vector< V > uValues(const std::multimap< K, V > &mm)
Definition: UStl.h:100
rtabmap::SensorData::depthOrRightRaw
const cv::Mat & depthOrRightRaw() const
Definition: SensorData.h:184
rtabmap::LaserScan::dataType
int dataType() const
Definition: LaserScan.h:132
UPlot
Definition: UPlot.h:492
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:1290
rtabmap::CloudViewer::refreshView
void refreshView()
Definition: CloudViewer.cpp:538
rtabmap::DatabaseViewer::odomMaxInf_
std::vector< double > odomMaxInf_
Definition: DatabaseViewer.h:239
rtabmap::SensorData::setGPS
void setGPS(const GPS &gps)
Definition: SensorData.h:290
rtabmap::util3d::convertMap2Image8U
cv::Mat RTABMAP_CORE_EXPORT convertMap2Image8U(const cv::Mat &map8S, bool pgmFormat=false)
Definition: util3d_mapping.cpp:904
rtabmap::Transform::clone
Transform clone() const
Definition: Transform.cpp:102
rtabmap::DBDriver::saveOptimizedPoses
void saveOptimizedPoses(const std::map< int, Transform > &optimizedPoses, const Transform &lastlocalizationPose) const
Definition: DBDriver.cpp:1231
rtabmap::util3d::fixTextureMeshForVisualization
void RTABMAP_CORE_EXPORT fixTextureMeshForVisualization(pcl::TextureMesh &textureMesh)
Definition: util3d_surface.cpp:2195
uStr2Float
float UTILITE_EXPORT uStr2Float(const std::string &str)
Definition: UConversion.cpp:138
rtabmap::DatabaseViewer::setupMainLayout
void setupMainLayout(bool vertical)
Definition: DatabaseViewer.cpp:524
rtabmap::DatabaseViewer::exportSaved2DMap
void exportSaved2DMap()
Definition: DatabaseViewer.cpp:3170
rtabmap::graph::radiusPosesFiltering
std::map< int, Transform > RTABMAP_CORE_EXPORT radiusPosesFiltering(const std::map< int, Transform > &poses, float radius, float angle, bool keepLatest=true)
Definition: Graph.cpp:1365
rtabmap::DBDriver::getTotalDictionarySize
int getTotalDictionarySize() const
Definition: DBDriver.cpp:231
t
Point2 t(10, 10)
rtabmap::Optimizer::isCovarianceIgnored
bool isCovarianceIgnored() const
Definition: Optimizer.h:92
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:138
rtabmap::util3d::laserScanToPointCloudRGBNormal
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudRGBNormal(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2376
rtabmap::ProgressDialog::setMaximumSteps
void setMaximumSteps(int steps)
Definition: ProgressDialog.cpp:133
rtabmap::DatabaseViewer::updateOctomapView
void updateOctomapView()
Definition: DatabaseViewer.cpp:7923
rtabmap::ExportCloudsDialog::saveSettings
void saveSettings(QSettings &settings, const QString &group="") const
Definition: ExportCloudsDialog.cpp:355
rtabmap::CloudViewer::setCloudPointSize
void setCloudPointSize(const std::string &id, int size)
Definition: CloudViewer.cpp:3276
rtabmap::ExportCloudsDialog::getMaxTextures
int getMaxTextures() const
Definition: ExportCloudsDialog.cpp:1559
UFile.h
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::DBDriver::getGridsMemoryUsed
long getGridsMemoryUsed() const
Definition: DBDriver.cpp:159
rtabmap::LocalGridCache::size
size_t size() const
Definition: LocalGrid.h:76
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:76
rtabmap::util3d::laserScanToPointCloudNormal
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudNormal(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2296
rtabmap::DatabaseViewer::editDepthImage
void editDepthImage()
Definition: DatabaseViewer.cpp:2404
rtabmap::DatabaseViewer::exportPosesTORO
void exportPosesTORO()
Definition: DatabaseViewer.cpp:2454
UERROR
#define UERROR(...)
rtabmap::LaserScan::angleMin
float angleMin() const
Definition: LaserScan.h:123
rtabmap::Signature::removeAllWords
void removeAllWords()
Definition: Signature.cpp:346
DatabaseViewer.h
trace.model
model
Definition: trace.py:4
file
file
rtabmap::databaseRecovery
bool RTABMAP_CORE_EXPORT databaseRecovery(const std::string &corruptedDatabase, bool keepCorruptedDatabase=true, std::string *errorMsg=0, ProgressState *progressState=0)
Definition: Recovery.cpp:39
rtabmap::DatabaseViewer::sliderBValueChanged
void sliderBValueChanged(int)
Definition: DatabaseViewer.cpp:4666
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
rtabmap::DatabaseViewer::sliderLoopValueChanged
void sliderLoopValueChanged(int)
Definition: DatabaseViewer.cpp:6092
value
value
rtabmap::DBDriver::loadSignatures
void loadSignatures(const std::list< int > &ids, std::list< Signature * > &signatures, std::set< int > *loadedFromTrash=0)
Definition: DBDriver.cpp:565
i
int i
rtabmap::Parameters::writeINI
static void writeINI(const std::string &configFile, const ParametersMap &parameters)
Definition: Parameters.cpp:1302
rtabmap::SensorData::setEnvSensors
void setEnvSensors(const EnvSensors &sensors)
Definition: SensorData.h:296
rtabmap::DBDriver::getAllLinks
void getAllLinks(std::multimap< int, Link > &links, bool ignoreNullLinks=true, bool withLandmarks=false) const
Definition: DBDriver.cpp:965
rtabmap::EditConstraintDialog::getCovariance
cv::Mat getCovariance() const
Definition: EditConstraintDialog.cpp:133
StereoDense.h
rtabmap::DatabaseViewer::sliderNeighborValueChanged
void sliderNeighborValueChanged(int)
Definition: DatabaseViewer.cpp:6084
rtabmap::LaserScan::rangeMin
float rangeMin() const
Definition: LaserScan.h:121
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
rtabmap::EditMapArea::setMap
void setMap(const cv::Mat &map)
Definition: EditMapArea.cpp:67
rtabmap::CloudViewer::clearTrajectory
void clearTrajectory()
Definition: CloudViewer.cpp:2435
rtabmap::DatabaseViewer::pathDatabase_
QString pathDatabase_
Definition: DatabaseViewer.h:226
uValueAt
V & uValueAt(std::list< V > &list, const unsigned int &pos)
Definition: UStl.h:380
rtabmap::DatabaseViewer::updateLinksWithModifications
std::multimap< int, rtabmap::Link > updateLinksWithModifications(const std::multimap< int, rtabmap::Link > &edgeConstraints)
Definition: DatabaseViewer.cpp:9172
rtabmap::Signature
Definition: Signature.h:48
rtabmap::util3d::cloudsFromSensorData
std::vector< pcl::PointCloud< pcl::PointXYZ >::Ptr > RTABMAP_CORE_EXPORT cloudsFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< pcl::IndicesPtr > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:854
Recovery.h
rtabmap::DatabaseViewer::exportPosesRGBDSLAMID
void exportPosesRGBDSLAMID()
Definition: DatabaseViewer.cpp:2446
EditConstraintDialog.h
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::DatabaseViewer::view3DMap
void view3DMap()
Definition: DatabaseViewer.cpp:4052
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:1260
rtabmap::EditDepthArea
Definition: EditDepthArea.h:45
rtabmap::EditDepthArea::isModified
bool isModified() const
Definition: EditDepthArea.h:54
rtabmap::ProgressDialog::maximumSteps
int maximumSteps() const
Definition: ProgressDialog.cpp:129
v2
v2
rtabmap::EnvSensor::kWifiSignalStrength
@ kWifiSignalStrength
Definition: EnvSensor.h:39
msg
msg
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:1207
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:1627
rtabmap::DBDriver::create
static DBDriver * create(const ParametersMap &parameters=ParametersMap())
Definition: DBDriver.cpp:41
rtabmap::DatabaseViewer::exportPosesRGBDSLAM
void exportPosesRGBDSLAM()
Definition: DatabaseViewer.cpp:2442
EditDepthArea.h

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Feb 13 2025 03:44:52