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  dbDriver_->updateDepthImage(id, depth);
2427  this->update3dView();
2428  }
2429  }
2430  }
2431 }
2432 
2433 void DatabaseViewer::exportPosesRaw()
2434 {
2435  exportPoses(0);
2436 }
2437 void DatabaseViewer::exportPosesRGBDSLAMMotionCapture()
2438 {
2439  exportPoses(1);
2440 }
2441 void DatabaseViewer::exportPosesRGBDSLAM()
2442 {
2443  exportPoses(10);
2444 }
2445 void DatabaseViewer::exportPosesRGBDSLAMID()
2446 {
2447  exportPoses(11);
2448 }
2449 void DatabaseViewer::exportPosesKITTI()
2450 {
2451  exportPoses(2);
2452 }
2453 void DatabaseViewer::exportPosesTORO()
2454 {
2455  exportPoses(3);
2456 }
2457 void DatabaseViewer::exportPosesG2O()
2458 {
2459  exportPoses(4);
2460 }
2461 void DatabaseViewer::exportPosesKML()
2462 {
2463  exportPoses(5);
2464 }
2465 
2466 void DatabaseViewer::exportPoses(int format)
2467 {
2468  QStringList types;
2469  types.push_back("Map's graph (see Graph View)");
2470  types.push_back("Odometry");
2471  if(!groundTruthPoses_.empty())
2472  {
2473  types.push_back("Ground Truth");
2474  }
2475  bool ok;
2476  QString type = QInputDialog::getItem(this, tr("Which poses?"), tr("Poses:"), types, 0, false, &ok);
2477  if(!ok)
2478  {
2479  return;
2480  }
2481  bool odometry = type.compare("Odometry") == 0;
2482  bool groundTruth = type.compare("Ground Truth") == 0;
2483 
2484  if(groundTruth && groundTruthPoses_.empty())
2485  {
2486  QMessageBox::warning(this, tr("Cannot export poses"), tr("No ground truth poses in database?!"));
2487  return;
2488  }
2489  else if(!odometry && graphes_.empty())
2490  {
2491  this->updateGraphView();
2492  if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
2493  {
2494  QMessageBox::warning(this, tr("Cannot export poses"), tr("No graph in database?!"));
2495  return;
2496  }
2497  }
2498  else if(odometry && odomPoses_.empty())
2499  {
2500  QMessageBox::warning(this, tr("Cannot export poses"), tr("No odometry poses in database?!"));
2501  return;
2502  }
2503 
2504  if(format == 5)
2505  {
2506  if(gpsValues_.empty() || gpsPoses_.empty())
2507  {
2508  QMessageBox::warning(this, tr("Cannot export poses"), tr("No GPS in database?!"));
2509  }
2510  else
2511  {
2512  std::map<int, rtabmap::Transform> graph;
2513  if(groundTruth)
2514  {
2515  graph = groundTruthPoses_;
2516  }
2517  else if(odometry)
2518  {
2519  graph = odomPoses_;
2520  }
2521  else
2522  {
2523  graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
2524  }
2525 
2526 
2527  //align with ground truth for more meaningful results
2528  pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
2529  cloud1.resize(graph.size());
2530  cloud2.resize(graph.size());
2531  int oi = 0;
2532  int idFirst = 0;
2533  for(std::map<int, Transform>::const_iterator iter=gpsPoses_.begin(); iter!=gpsPoses_.end(); ++iter)
2534  {
2535  std::map<int, Transform>::iterator iter2 = graph.find(iter->first);
2536  if(iter2!=graph.end())
2537  {
2538  if(oi==0)
2539  {
2540  idFirst = iter->first;
2541  }
2542  cloud1[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
2543  cloud2[oi++] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z());
2544  }
2545  }
2546 
2547  Transform t = Transform::getIdentity();
2548  if(oi>5)
2549  {
2550  cloud1.resize(oi);
2551  cloud2.resize(oi);
2552 
2553  t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
2554  }
2555  else if(idFirst)
2556  {
2557  t = gpsPoses_.at(idFirst) * graph.at(idFirst).inverse();
2558  }
2559 
2560  std::map<int, GPS> values;
2561  GeodeticCoords origin = gpsValues_.begin()->second.toGeodeticCoords();
2562  for(std::map<int, Transform>::iterator iter=graph.begin(); iter!=graph.end(); ++iter)
2563  {
2564  iter->second = t * iter->second;
2565 
2566  GeodeticCoords coord;
2567  coord.fromENU_WGS84(cv::Point3d(iter->second.x(), iter->second.y(), iter->second.z()), origin);
2568  double bearing = -(iter->second.theta()*180.0/M_PI-90.0);
2569  if(bearing < 0)
2570  {
2571  bearing += 360;
2572  }
2573 
2574  Transform p, g;
2575  int w;
2576  std::string l;
2577  double stamp=0.0;
2578  int mapId;
2579  std::vector<float> v;
2580  GPS gps;
2581  EnvSensors sensors;
2582  dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors);
2583  values.insert(std::make_pair(iter->first, GPS(stamp, coord.longitude(), coord.latitude(), coord.altitude(), 0, 0)));
2584  }
2585 
2586  QString output = pathDatabase_ + QDir::separator() + "poses.kml";
2587  QString path = QFileDialog::getSaveFileName(
2588  this,
2589  tr("Save File"),
2590  output,
2591  tr("Google Earth file (*.kml)"));
2592 
2593  if(!path.isEmpty())
2594  {
2595  bool saved = graph::exportGPS(path.toStdString(), values, ui_->graphViewer->getNodeColor().rgba());
2596 
2597  if(saved)
2598  {
2599  QMessageBox::information(this,
2600  tr("Export poses..."),
2601  tr("GPS coordinates saved to \"%1\".")
2602  .arg(path));
2603  }
2604  else
2605  {
2606  QMessageBox::information(this,
2607  tr("Export poses..."),
2608  tr("Failed to save GPS coordinates to \"%1\"!")
2609  .arg(path));
2610  }
2611  }
2612  }
2613  return;
2614  }
2615 
2616  std::map<int, Transform> optimizedPoses;
2617  if(groundTruth)
2618  {
2619  optimizedPoses = groundTruthPoses_;
2620  }
2621  else
2622  {
2623  if(odometry)
2624  {
2625  optimizedPoses = odomPoses_;
2626  }
2627  else
2628  {
2629  optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
2630  }
2631 
2632  if((ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked()) ||
2633  (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked()))
2634  {
2635  std::map<int, Transform> refPoses = groundTruthPoses_;
2636  if(ui_->checkBox_alignPosesWithGPS->isEnabled() &&
2637  ui_->checkBox_alignPosesWithGPS->isChecked())
2638  {
2639  refPoses = gpsPoses_;
2640  }
2641 
2642  // Log ground truth statistics (in TUM's RGBD-SLAM format)
2643  if(refPoses.size())
2644  {
2645  float translational_rmse = 0.0f;
2646  float translational_mean = 0.0f;
2647  float translational_median = 0.0f;
2648  float translational_std = 0.0f;
2649  float translational_min = 0.0f;
2650  float translational_max = 0.0f;
2651  float rotational_rmse = 0.0f;
2652  float rotational_mean = 0.0f;
2653  float rotational_median = 0.0f;
2654  float rotational_std = 0.0f;
2655  float rotational_min = 0.0f;
2656  float rotational_max = 0.0f;
2657 
2658  Transform gtToMap = graph::calcRMSE(
2659  refPoses,
2660  optimizedPoses,
2661  translational_rmse,
2662  translational_mean,
2663  translational_median,
2664  translational_std,
2665  translational_min,
2666  translational_max,
2667  rotational_rmse,
2668  rotational_mean,
2669  rotational_median,
2670  rotational_std,
2671  rotational_min,
2672  rotational_max);
2673 
2674  if(!gtToMap.isIdentity())
2675  {
2676  for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
2677  {
2678  iter->second = gtToMap * iter->second;
2679  }
2680  }
2681  }
2682  }
2683  }
2684 
2685  if(optimizedPoses.size())
2686  {
2687  std::map<int, Transform> localTransforms;
2688  QStringList items;
2689  items.push_back("Robot (base frame)");
2690  items.push_back("Camera");
2691  items.push_back("Scan");
2692  bool ok;
2693  QString item = QInputDialog::getItem(this, tr("Export Poses"), tr("Frame: "), items, 0, false, &ok);
2694  if(!ok || item.isEmpty())
2695  {
2696  return;
2697  }
2698  if(item.compare("Robot (base frame)") != 0)
2699  {
2700  bool cameraFrame = item.compare("Camera") == 0;
2701  for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
2702  {
2703  Transform localTransform;
2704  if(cameraFrame)
2705  {
2706  std::vector<CameraModel> models;
2707  std::vector<StereoCameraModel> stereoModels;
2708  if(dbDriver_->getCalibration(iter->first, models, stereoModels))
2709  {
2710  if((models.size() == 1 &&
2711  !models.at(0).localTransform().isNull()))
2712  {
2713  localTransform = models.at(0).localTransform();
2714  }
2715  else if(stereoModels.size() == 1 &&
2716  !stereoModels[0].localTransform().isNull())
2717  {
2718  localTransform = stereoModels[0].localTransform();
2719  }
2720  else if(models.size()>1)
2721  {
2722  UWARN("Multi-camera is not supported (node %d)", iter->first);
2723  }
2724  else
2725  {
2726  UWARN("Calibration not valid for node %d", iter->first);
2727  }
2728  }
2729  else
2730  {
2731  UWARN("Missing calibration for node %d", iter->first);
2732  }
2733  }
2734  else
2735  {
2736  LaserScan info;
2737  if(dbDriver_->getLaserScanInfo(iter->first, info))
2738  {
2739  localTransform = info.localTransform();
2740  }
2741  else
2742  {
2743  UWARN("Missing scan info for node %d", iter->first);
2744  }
2745 
2746  }
2747  if(!localTransform.isNull())
2748  {
2749  localTransforms.insert(std::make_pair(iter->first, localTransform));
2750  }
2751  }
2752  if(localTransforms.empty())
2753  {
2754  QMessageBox::warning(this,
2755  tr("Export Poses"),
2756  tr("Could not find any \"%1\" frame, exporting in Robot frame instead.").arg(item));
2757  }
2758  }
2759 
2760  std::map<int, Transform> poses;
2761  std::multimap<int, Link> links;
2762  if(localTransforms.empty())
2763  {
2764  poses = optimizedPoses;
2765  if(!odometry)
2766  {
2767  links = graphLinks_;
2768  }
2769  }
2770  else
2771  {
2772  //adjust poses and links
2773  for(std::map<int, Transform>::iterator iter=localTransforms.begin(); iter!=localTransforms.end(); ++iter)
2774  {
2775  poses.insert(std::make_pair(iter->first, optimizedPoses.at(iter->first) * iter->second));
2776  }
2777  if(!odometry)
2778  {
2779  for(std::multimap<int, Link>::iterator iter=graphLinks_.begin(); iter!=graphLinks_.end(); ++iter)
2780  {
2781  if(uContains(poses, iter->second.from()) && uContains(poses, iter->second.to()))
2782  {
2783  std::multimap<int, Link>::iterator inserted = links.insert(*iter);
2784  int from = iter->second.from();
2785  int to = iter->second.to();
2786  inserted->second.setTransform(localTransforms.at(from).inverse()*iter->second.transform()*localTransforms.at(to));
2787  }
2788  }
2789  }
2790  }
2791 
2792  if(format != 4 && format != 11 && !poses.empty() && poses.begin()->first<0) // not g2o, landmark not supported
2793  {
2794  UWARN("Only g2o format (4) and RGBD format with ID format can export landmarks, they are ignored with format %d", format);
2795  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end() && iter->first < 0;)
2796  {
2797  poses.erase(iter++);
2798  }
2799  for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end();)
2800  {
2801  if(iter->second.from() < 0 || iter->second.to() < 0)
2802  {
2803  links.erase(iter++);
2804  }
2805  else
2806  {
2807  ++iter;
2808  }
2809  }
2810  }
2811 
2812  std::map<int, double> stamps;
2813  if(format == 1 || format == 10 || format == 11)
2814  {
2815  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
2816  {
2817  if(iter->first<0 && format == 11) // in case of landmarks
2818  {
2819  stamps.insert(std::make_pair(iter->first, 0));
2820  }
2821  else
2822  {
2823  Transform p, g;
2824  int w;
2825  std::string l;
2826  double stamp=0.0;
2827  int mapId;
2828  std::vector<float> v;
2829  GPS gps;
2830  EnvSensors sensors;
2831  if(dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors))
2832  {
2833  stamps.insert(std::make_pair(iter->first, stamp));
2834  }
2835  }
2836  }
2837  if(stamps.size()!=poses.size())
2838  {
2839  QMessageBox::warning(this, tr("Export poses..."), tr("Poses (%1) and stamps (%2) have not the same size! Cannot export in RGB-D SLAM format.")
2840  .arg(poses.size()).arg(stamps.size()));
2841  return;
2842  }
2843  }
2844 
2845  QString output = pathDatabase_ + QDir::separator() + (format==3?"toro%1.graph":format==4?"poses%1.g2o":"poses%1.txt");
2846  QString suffix = odometry?"_odom":groundTruth?"_gt":"";
2847  output = output.arg(suffix);
2848 
2849  QString path = QFileDialog::getSaveFileName(
2850  this,
2851  tr("Save File"),
2852  output,
2853  format == 3?tr("TORO file (*.graph)"):format==4?tr("g2o file (*.g2o)"):tr("Text file (*.txt)"));
2854 
2855  if(!path.isEmpty())
2856  {
2857  if(QFileInfo(path).suffix() == "")
2858  {
2859  if(format == 3)
2860  {
2861  path += ".graph";
2862  }
2863  else if(format==4)
2864  {
2865  path += ".g2o";
2866  }
2867  else
2868  {
2869  path += ".txt";
2870  }
2871  }
2872 
2873  bool saved = graph::exportPoses(path.toStdString(), format, poses, links, stamps, ui_->parameters_toolbox->getParameters());
2874 
2875  if(saved)
2876  {
2877  QMessageBox::information(this,
2878  tr("Export poses..."),
2879  tr("%1 saved to \"%2\".")
2880  .arg(format == 3?"TORO graph":format == 4?"g2o graph":"Poses")
2881  .arg(path));
2882  }
2883  else
2884  {
2885  QMessageBox::information(this,
2886  tr("Export poses..."),
2887  tr("Failed to save %1 to \"%2\"!")
2888  .arg(format == 3?"TORO graph":format == 4?"g2o graph":"poses")
2889  .arg(path));
2890  }
2891  }
2892  }
2893 }
2894 
2895 void DatabaseViewer::exportGPS_TXT()
2896 {
2897  exportGPS(0);
2898 }
2899 void DatabaseViewer::exportGPS_KML()
2900 {
2901  exportGPS(1);
2902 }
2903 
2904 void DatabaseViewer::exportGPS(int format)
2905 {
2906  if(!gpsValues_.empty())
2907  {
2908  QString output = pathDatabase_ + QDir::separator() + (format==0?"gps.txt":"gps.kml");
2909  QString path = QFileDialog::getSaveFileName(
2910  this,
2911  tr("Save File"),
2912  output,
2913  format==0?tr("Raw format (*.txt)"):tr("Google Earth file (*.kml)"));
2914 
2915  if(!path.isEmpty())
2916  {
2917  bool saved = graph::exportGPS(path.toStdString(), gpsValues_, ui_->graphViewer->getGPSColor().rgba());
2918 
2919  if(saved)
2920  {
2921  QMessageBox::information(this,
2922  tr("Export poses..."),
2923  tr("GPS coordinates saved to \"%1\".")
2924  .arg(path));
2925  }
2926  else
2927  {
2928  QMessageBox::information(this,
2929  tr("Export poses..."),
2930  tr("Failed to save GPS coordinates to \"%1\"!")
2931  .arg(path));
2932  }
2933  }
2934  }
2935 }
2936 
2937 void DatabaseViewer::editSaved2DMap()
2938 {
2939  if(!dbDriver_)
2940  {
2941  QMessageBox::warning(this, tr("Cannot edit 2D map"), tr("A database must must loaded first...\nUse File->Open database."));
2942  return;
2943  }
2944 
2945  if(uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.17.0") < 0)
2946  {
2947  QMessageBox::warning(this, tr("Cannot edit 2D map"),
2948  tr("The database has too old version (%1) to saved "
2949  "optimized map. Version 0.17.0 minimum required.").arg(dbDriver_->getDatabaseVersion().c_str()));
2950  return;
2951  }
2952 
2953  if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size() || generatedLocalMaps_.size())
2954  {
2955  QMessageBox::warning(this, tr("Cannot edit 2D map"),
2956  tr("The database has modified links and/or modified local "
2957  "occupancy grids, the 2D optimized map cannot be modified."));
2958  return;
2959  }
2960 
2961  float xMin, yMin, cellSize;
2962  cv::Mat map = dbDriver_->load2DMap(xMin, yMin, cellSize);
2963  if(map.empty())
2964  {
2965  QMessageBox::warning(this, tr("Cannot export 2D map"), tr("The database doesn't contain a saved 2D map."));
2966  return;
2967  }
2968 
2969  cv::Mat map8U = rtabmap::util3d::convertMap2Image8U(map, false);
2970  cv::Mat map8UFlip, map8URotated;
2971  cv::flip(map8U, map8UFlip, 0);
2972  if(!ui_->graphViewer->isOrientationENU())
2973  {
2974  //ROTATE_90_COUNTERCLOCKWISE
2975  cv::transpose(map8UFlip, map8URotated);
2976  cv::flip(map8URotated, map8URotated, 0);
2977  }
2978  else
2979  {
2980  map8URotated = map8UFlip;
2981  }
2982 
2983  editMapArea_->setMap(map8URotated);
2984 
2985  if(editMapDialog_->exec() == QDialog::Accepted)
2986  {
2987  cv::Mat mapModified = editMapArea_->getModifiedMap();
2988 
2989  if(!ui_->graphViewer->isOrientationENU())
2990  {
2991  //ROTATE_90_CLOCKWISE
2992  cv::transpose(mapModified, map8URotated);
2993  cv::flip(map8URotated, map8URotated, 1);
2994  }
2995  else
2996  {
2997  map8URotated = mapModified;
2998  }
2999  cv::flip(map8URotated, map8UFlip, 0);
3000 
3001  UASSERT(map8UFlip.type() == map8U.type());
3002  UASSERT(map8UFlip.cols == map8U.cols);
3003  UASSERT(map8UFlip.rows == map8U.rows);
3004 
3005  cv::Mat map8S = rtabmap::util3d::convertImage8U2Map(map8UFlip, false);
3006 
3007  if(editMapArea_->isModified())
3008  {
3009  dbDriver_->save2DMap(map8S, xMin, yMin, cellSize);
3010  QMessageBox::information(this, tr("Edit 2D map"), tr("Map updated!"));
3011  }
3012 
3013  int cropRadius = ui_->spinBox_cropRadius->value();
3014  QMessageBox::StandardButton b = QMessageBox::question(this,
3015  tr("Crop empty space"),
3016  tr("Do you want to clear empty space from local occupancy grids and laser scans?\n\n"
3017  "Advantages:\n"
3018  " * 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"
3019  " * 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"
3020  "Disadvantage:\n"
3021  " * Cropping the laser scans cannot be reverted after the viewer is closed and changes have been saved.\n\n"
3022  "Parameter(s):\n"
3023  " Crop radius = %1 pixels\n\n"
3024  "Press \"Yes\" to filter only grids.\n"
3025  "Press \"Yes to All\" to filter both grids and laser scans.\n").arg(cropRadius),
3026  QMessageBox::Yes | QMessageBox::YesToAll | QMessageBox::No, QMessageBox::No);
3027  if(b == QMessageBox::Yes || b == QMessageBox::YesToAll)
3028  {
3029  std::map<int, Transform> poses = dbDriver_->loadOptimizedPoses(); // poses should match the grid map
3030 
3031  modifiedLaserScans_.clear();
3032 
3033  rtabmap::ProgressDialog progressDialog(this);
3034  progressDialog.setMaximumSteps(poses.size()+1);
3035  progressDialog.show();
3036  progressDialog.setCancelButtonVisible(true);
3037  progressDialog.appendText(QString("Cropping empty space... %1 scans to filter").arg(poses.size()));
3038  progressDialog.setMinimumWidth(800);
3039  QApplication::processEvents();
3040 
3041  UINFO("Cropping empty space... poses=%d cropRadius=%d", poses.size(), cropRadius);
3042  UASSERT(cropRadius>=0);
3043  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end() && !progressDialog.isCanceled(); ++iter)
3044  {
3045  // local grid
3046  cv::Mat gridGround;
3047  cv::Mat gridObstacles;
3048  cv::Mat gridEmpty;
3049 
3050  // scan
3051  SensorData data;
3052  dbDriver_->getNodeData(iter->first, data);
3053  LaserScan scan;
3054  data.uncompressData(0,0,&scan,0,&gridGround,&gridObstacles,&gridEmpty);
3055 
3056  if(generatedLocalMaps_.localGrids().find(iter->first) != generatedLocalMaps_.localGrids().end())
3057  {
3058  gridObstacles = generatedLocalMaps_.localGrids().at(iter->first).obstacleCells;
3059  }
3060  if(!gridObstacles.empty())
3061  {
3062  cv::Mat filtered = cv::Mat(1, gridObstacles.cols, gridObstacles.type());
3063  int oi = 0;
3064  for(int i=0; i<gridObstacles.cols; ++i)
3065  {
3066  const float * ptr = gridObstacles.ptr<float>(0, i);
3067  cv::Point3f pt(ptr[0], ptr[1], gridObstacles.channels()==2?0:ptr[2]);
3068  pt = util3d::transformPoint(pt, iter->second);
3069 
3070  int x = int((pt.x - xMin) / cellSize + 0.5f);
3071  int y = int((pt.y - yMin) / cellSize + 0.5f);
3072 
3073  if(x>=0 && x<map8S.cols &&
3074  y>=0 && y<map8S.rows)
3075  {
3076  bool obstacleDetected = false;
3077 
3078  for(int j=-cropRadius; j<=cropRadius && !obstacleDetected; ++j)
3079  {
3080  for(int k=-cropRadius; k<=cropRadius && !obstacleDetected; ++k)
3081  {
3082  if(x+j>=0 && x+j<map8S.cols &&
3083  y+k>=0 && y+k<map8S.rows &&
3084  map8S.at<unsigned char>(y+k,x+j) == 100)
3085  {
3086  obstacleDetected = true;
3087  }
3088  }
3089  }
3090 
3091  if(map8S.at<unsigned char>(y,x) != 0 || obstacleDetected)
3092  {
3093  // Verify that we don't have an obstacle on neighbor cells
3094  cv::Mat(gridObstacles, cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(filtered, cv::Range::all(), cv::Range(oi,oi+1)));
3095  ++oi;
3096  }
3097  }
3098  }
3099 
3100  if(oi != gridObstacles.cols)
3101  {
3102  progressDialog.appendText(QString("Grid %1 filtered %2 pts -> %3 pts").arg(iter->first).arg(gridObstacles.cols).arg(oi));
3103  UINFO("Grid %d filtered %d -> %d", iter->first, gridObstacles.cols, oi);
3104 
3105  // update
3106  cv::Mat newObstacles = cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi));
3107  if(generatedLocalMaps_.localGrids().find(iter->first) != generatedLocalMaps_.localGrids().end())
3108  {
3109  LocalGrid value = generatedLocalMaps_.localGrids().at(iter->first);
3110  value.obstacleCells = newObstacles;
3111  generatedLocalMaps_.add(iter->first, value);
3112  }
3113  else
3114  {
3115  LocalGrid value(gridGround, newObstacles, gridEmpty, data.gridCellSize(), data.gridViewPoint());
3116  generatedLocalMaps_.add(iter->first, value);
3117  }
3118 
3119  }
3120  }
3121 
3122  if(b == QMessageBox::YesToAll && !scan.isEmpty())
3123  {
3124  Transform mapToScan = iter->second * scan.localTransform();
3125 
3126  cv::Mat filtered = cv::Mat(1, scan.size(), scan.dataType());
3127  int oi = 0;
3128  for(int i=0; i<scan.size(); ++i)
3129  {
3130  const float * ptr = scan.data().ptr<float>(0, i);
3131  cv::Point3f pt(ptr[0], ptr[1], scan.is2d()?0:ptr[2]);
3132  pt = util3d::transformPoint(pt, mapToScan);
3133 
3134  int x = int((pt.x - xMin) / cellSize + 0.5f);
3135  int y = int((pt.y - yMin) / cellSize + 0.5f);
3136 
3137  if(x>=0 && x<map8S.cols &&
3138  y>=0 && y<map8S.rows)
3139  {
3140  bool obstacleDetected = false;
3141 
3142  for(int j=-cropRadius; j<=cropRadius && !obstacleDetected; ++j)
3143  {
3144  for(int k=-cropRadius; k<=cropRadius && !obstacleDetected; ++k)
3145  {
3146  if(x+j>=0 && x+j<map8S.cols &&
3147  y+k>=0 && y+k<map8S.rows &&
3148  map8S.at<unsigned char>(y+k,x+j) == 100)
3149  {
3150  obstacleDetected = true;
3151  }
3152  }
3153  }
3154 
3155  if(map8S.at<unsigned char>(y,x) != 0 || obstacleDetected)
3156  {
3157  // Verify that we don't have an obstacle on neighbor cells
3158  cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(filtered, cv::Range::all(), cv::Range(oi,oi+1)));
3159  ++oi;
3160  }
3161  }
3162  }
3163 
3164  if(oi != scan.size())
3165  {
3166  progressDialog.appendText(QString("Scan %1 filtered %2 pts -> %3 pts").arg(iter->first).arg(scan.size()).arg(oi));
3167  UINFO("Scan %d filtered %d -> %d", iter->first, scan.size(), oi);
3168 
3169  // update
3170  if(scan.angleIncrement()!=0)
3171  {
3172  // copy meta data
3173  scan = LaserScan(
3174  cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi)),
3175  scan.format(),
3176  scan.rangeMin(),
3177  scan.rangeMax(),
3178  scan.angleMin(),
3179  scan.angleMax(),
3180  scan.angleIncrement(),
3181  scan.localTransform());
3182  }
3183  else
3184  {
3185  // copy meta data
3186  scan = LaserScan(
3187  cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi)),
3188  scan.maxPoints(),
3189  scan.rangeMax(),
3190  scan.format(),
3191  scan.localTransform());
3192  }
3193  uInsert(modifiedLaserScans_, std::make_pair(iter->first, scan));
3194  }
3195  }
3196  progressDialog.incrementStep();
3197  QApplication::processEvents();
3198  }
3199  if(progressDialog.isCanceled())
3200  {
3201  modifiedLaserScans_.clear();
3202  }
3203  else
3204  {
3205  update3dView();
3206  updateGraphView();
3207  }
3208  progressDialog.setValue(progressDialog.maximumSteps());
3209  }
3210  }
3211 }
3212 
3213 void DatabaseViewer::exportSaved2DMap()
3214 {
3215  if(!dbDriver_)
3216  {
3217  QMessageBox::warning(this, tr("Cannot export 2D map"), tr("A database must must loaded first...\nUse File->Open database."));
3218  return;
3219  }
3220 
3221  float xMin, yMin, cellSize;
3222  cv::Mat map = dbDriver_->load2DMap(xMin, yMin, cellSize);
3223  if(map.empty())
3224  {
3225  QMessageBox::warning(this, tr("Cannot export 2D map"), tr("The database doesn't contain a saved 2D map."));
3226  }
3227  else
3228  {
3229  cv::Mat map8U = rtabmap::util3d::convertMap2Image8U(map, true);
3230  QString name = QFileInfo(databaseFileName_.c_str()).baseName();
3231  QString path = QFileDialog::getSaveFileName(
3232  this,
3233  tr("Save File"),
3234  pathDatabase_+"/" + name + ".pgm",
3235  tr("Map (*.pgm)"));
3236 
3237  if(!path.isEmpty())
3238  {
3239  if(QFileInfo(path).suffix() == "")
3240  {
3241  path += ".pgm";
3242  }
3243  cv::imwrite(path.toStdString(), map8U);
3244 
3245  QFileInfo info(path);
3246  QString yaml = info.absolutePath() + "/" + info.baseName() + ".yaml";
3247 
3248  float occupancyThr = Parameters::defaultGridGlobalOccupancyThr();
3249  Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kGridGlobalOccupancyThr(), occupancyThr);
3250 
3251  std::ofstream file;
3252  file.open (yaml.toStdString());
3253  file << "image: " << info.baseName().toStdString() << ".pgm" << std::endl;
3254  file << "resolution: " << cellSize << std::endl;
3255  file << "origin: [" << xMin << ", " << yMin << ", 0.0]" << std::endl;
3256  file << "negate: 0" << std::endl;
3257  file << "occupied_thresh: " << occupancyThr << std::endl;
3258  file << "free_thresh: 0.196" << std::endl;
3259  file << std::endl;
3260  file.close();
3261 
3262 
3263  QMessageBox::information(this, tr("Export 2D map"), tr("Exported %1 and %2!").arg(path).arg(yaml));
3264  }
3265  }
3266 }
3267 
3268 void DatabaseViewer::import2DMap()
3269 {
3270  if(!dbDriver_)
3271  {
3272  QMessageBox::warning(this, tr("Cannot import 2D map"), tr("A database must must loaded first...\nUse File->Open database."));
3273  return;
3274  }
3275 
3276  if(uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.17.0") < 0)
3277  {
3278  QMessageBox::warning(this, tr("Cannot edit 2D map"),
3279  tr("The database has too old version (%1) to saved "
3280  "optimized map. Version 0.17.0 minimum required.").arg(dbDriver_->getDatabaseVersion().c_str()));
3281  return;
3282  }
3283 
3284  if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size() || generatedLocalMaps_.size())
3285  {
3286  QMessageBox::warning(this, tr("Cannot import 2D map"),
3287  tr("The database has modified links and/or modified local "
3288  "occupancy grids, the 2D optimized map cannot be modified."));
3289  return;
3290  }
3291 
3292  float xMin, yMin, cellSize;
3293  cv::Mat mapOrg = dbDriver_->load2DMap(xMin, yMin, cellSize);
3294  if(mapOrg.empty())
3295  {
3296  QMessageBox::warning(this, tr("Cannot import 2D map"), tr("The database doesn't contain a saved 2D map."));
3297  }
3298  else
3299  {
3300  QString path = QFileDialog::getOpenFileName(
3301  this,
3302  tr("Open File"),
3303  pathDatabase_,
3304  tr("Map (*.pgm)"));
3305  if(!path.isEmpty())
3306  {
3307  cv::Mat map8U = cv::imread(path.toStdString(), cv::IMREAD_UNCHANGED);
3308  cv::Mat map = rtabmap::util3d::convertImage8U2Map(map8U, true);
3309 
3310  if(mapOrg.cols == map.cols && mapOrg.rows == map8U.rows)
3311  {
3312  dbDriver_->save2DMap(map, xMin, yMin, cellSize);
3313  QMessageBox::information(this, tr("Import 2D map"), tr("Imported %1!").arg(path));
3314  }
3315  else
3316  {
3317  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));
3318  }
3319  }
3320  }
3321 }
3322 
3323 void DatabaseViewer::regenerateSavedMap()
3324 {
3325  if(!dbDriver_)
3326  {
3327  QMessageBox::warning(this, tr("Cannot import 2D map"), tr("A database must must loaded first...\nUse File->Open database."));
3328  return;
3329  }
3330 
3331  if(uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.17.0") < 0)
3332  {
3333  QMessageBox::warning(this, tr("Cannot edit 2D map"),
3334  tr("The database has too old version (%1) to saved "
3335  "optimized map. Version 0.17.0 minimum required.").arg(dbDriver_->getDatabaseVersion().c_str()));
3336  return;
3337  }
3338 
3339  if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size() || generatedLocalMaps_.size())
3340  {
3341  QMessageBox::warning(this, tr("Cannot import 2D map"),
3342  tr("The database has modified links and/or modified local "
3343  "occupancy grids, the 2D optimized map cannot be modified. Try "
3344  "closing the database and re-open it to save the changes."));
3345  return;
3346  }
3347 
3348  if((int)graphes_.empty() || localMaps_.empty())
3349  {
3350  QMessageBox::warning(this, tr("Cannot regenerate 2D map"),
3351  tr("Graph is empty, make sure you opened the "
3352  "Graph View and there is a map shown."));
3353  return;
3354  }
3355 
3356  //
3357 #ifdef RTABMAP_OCTOMAP
3358  QStringList types;
3359  types.push_back("Default occupancy grid");
3360  types.push_back("From OctoMap projection");
3361  bool ok;
3362  QString type = QInputDialog::getItem(this, tr("Which type?"), tr("Type:"), types, 0, false, &ok);
3363  if(!ok)
3364  {
3365  return;
3366  }
3367 #endif
3368 
3369  //update scans
3370  UINFO("Update local maps list...");
3371  float xMin, yMin;
3372  cv::Mat map;
3373  float gridCellSize = Parameters::defaultGridCellSize();
3374  Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kGridCellSize(), gridCellSize);
3375 #ifdef RTABMAP_OCTOMAP
3376  if(type.compare("From OctoMap projection") == 0)
3377  {
3378  //create local octomap
3379  OctoMap octomap(&localMaps_, ui_->parameters_toolbox->getParameters());
3380 
3381  octomap.update(graphes_.back());
3382  map = octomap.createProjectionMap(xMin, yMin, gridCellSize, 0);
3383  }
3384  else
3385 #endif
3386  {
3387  OccupancyGrid grid(&localMaps_, ui_->parameters_toolbox->getParameters());
3388  grid.update(graphes_.back());
3389  map = grid.getMap(xMin, yMin);
3390  }
3391 
3392  if(map.empty())
3393  {
3394  QMessageBox::information(this, tr("Regenerate 2D map"), tr("Failed to renegerate the map, resulting map is empty!"));
3395  }
3396  else
3397  {
3398  dbDriver_->save2DMap(map, xMin, yMin, gridCellSize);
3399  Transform lastlocalizationPose;
3400  dbDriver_->loadOptimizedPoses(&lastlocalizationPose);
3401  if(lastlocalizationPose.isNull() && !graphes_.back().empty())
3402  {
3403  // use last pose by default
3404  lastlocalizationPose = graphes_.back().rbegin()->second;
3405  }
3406  dbDriver_->saveOptimizedPoses(graphes_.back(), lastlocalizationPose);
3407  // reset optimized mesh as poses have changed
3408  dbDriver_->saveOptimizedMesh(cv::Mat());
3409  QMessageBox::information(this, tr("Regenerate 2D map"), tr("Map regenerated!"));
3410  ui_->actionEdit_optimized_2D_map->setEnabled(true);
3411  ui_->actionExport_saved_2D_map->setEnabled(true);
3412  ui_->actionImport_2D_map->setEnabled(true);
3413  }
3414 }
3415 
3416 void DatabaseViewer::viewOptimizedMesh()
3417 {
3418  if(!dbDriver_)
3419  {
3420  QMessageBox::warning(this, tr("Cannot view optimized mesh"), tr("A database must must loaded first...\nUse File->Open database."));
3421  return;
3422  }
3423 
3424  std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
3425 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
3426  std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
3427 #else
3428  std::vector<std::vector<Eigen::Vector2f> > texCoords;
3429 #endif
3430  cv::Mat textures;
3431  cv::Mat cloudMat = dbDriver_->loadOptimizedMesh(&polygons, &texCoords, &textures);
3432  if(cloudMat.empty())
3433  {
3434  QMessageBox::warning(this, tr("Cannot view optimized mesh"), tr("The database doesn't contain a saved optimized mesh."));
3435  }
3436  else
3437  {
3438  CloudViewer * viewer = new CloudViewer(this);
3439  viewer->setWindowFlags(Qt::Window);
3440  viewer->setAttribute(Qt::WA_DeleteOnClose);
3441  viewer->buildPickingLocator(true);
3442  if(!textures.empty())
3443  {
3444  pcl::TextureMeshPtr mesh = util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
3445  util3d::fixTextureMeshForVisualization(*mesh);
3446  viewer->setWindowTitle("Optimized Textured Mesh");
3447  viewer->setPolygonPicking(true);
3448  viewer->addCloudTextureMesh("mesh", mesh, textures);
3449  }
3450  else if(polygons.size() == 1)
3451  {
3452  pcl::PolygonMeshPtr mesh = util3d::assemblePolygonMesh(cloudMat, polygons.at(0));
3453  viewer->setWindowTitle("Optimized Mesh");
3454  viewer->setPolygonPicking(true);
3455  viewer->addCloudMesh("mesh", mesh);
3456  }
3457  else
3458  {
3459  LaserScan scan = LaserScan::backwardCompatibility(cloudMat);
3460  pcl::PCLPointCloud2::Ptr cloud = util3d::laserScanToPointCloud2(scan);
3461  viewer->setWindowTitle("Optimized Point Cloud");
3462  viewer->addCloud("mesh", cloud, Transform::getIdentity(), scan.hasRGB(), scan.hasNormals(), scan.hasIntensity());
3463  }
3464  viewer->show();
3465  }
3466 }
3467 
3468 void DatabaseViewer::exportOptimizedMesh()
3469 {
3470  if(!dbDriver_)
3471  {
3472  QMessageBox::warning(this, tr("Cannot export optimized mesh"), tr("A database must must loaded first...\nUse File->Open database."));
3473  return;
3474  }
3475 
3476  std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
3477 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
3478  std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
3479 #else
3480  std::vector<std::vector<Eigen::Vector2f> > texCoords;
3481 #endif
3482  cv::Mat textures;
3483  cv::Mat cloudMat = dbDriver_->loadOptimizedMesh(&polygons, &texCoords, &textures);
3484  if(cloudMat.empty())
3485  {
3486  QMessageBox::warning(this, tr("Cannot export optimized mesh"), tr("The database doesn't contain a saved optimized mesh."));
3487  }
3488  else
3489  {
3490  QString name = QFileInfo(databaseFileName_.c_str()).baseName();
3491 
3492  if(!textures.empty())
3493  {
3494  pcl::TextureMeshPtr mesh = util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures);
3495  QString path = QFileDialog::getSaveFileName(
3496  this,
3497  tr("Save File"),
3498  pathDatabase_+"/" + name + ".obj",
3499  tr("Mesh (*.obj)"));
3500 
3501  if(!path.isEmpty())
3502  {
3503  if(QFileInfo(path).suffix() == "")
3504  {
3505  path += ".obj";
3506  }
3507  QString baseName = QFileInfo(path).baseName();
3508  if(mesh->tex_materials.size() == 1)
3509  {
3510  mesh->tex_materials.at(0).tex_file = baseName.toStdString() + ".png";
3511  cv::imwrite((QFileInfo(path).absoluteDir().absolutePath()+QDir::separator()+baseName).toStdString() + ".png", textures);
3512  }
3513  else
3514  {
3515  for(unsigned int i=0; i<mesh->tex_materials.size(); ++i)
3516  {
3517  mesh->tex_materials.at(i).tex_file = (baseName+QDir::separator()+QString::number(i)+".png").toStdString();
3518  UASSERT((i+1)*textures.rows <= (unsigned int)textures.cols);
3519  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)));
3520  }
3521  }
3522  pcl::io::saveOBJFile(path.toStdString(), *mesh);
3523 
3524  QMessageBox::information(this, tr("Export Textured Mesh"), tr("Exported %1!").arg(path));
3525  }
3526  }
3527  else if(polygons.size() == 1)
3528  {
3529  pcl::PolygonMeshPtr mesh = util3d::assemblePolygonMesh(cloudMat, polygons.at(0));
3530  QString path = QFileDialog::getSaveFileName(
3531  this,
3532  tr("Save File"),
3533  pathDatabase_+"/" + name + ".ply",
3534  tr("Mesh (*.ply)"));
3535 
3536  if(!path.isEmpty())
3537  {
3538  if(QFileInfo(path).suffix() == "")
3539  {
3540  path += ".ply";
3541  }
3542  pcl::io::savePLYFileBinary(path.toStdString(), *mesh);
3543  QMessageBox::information(this, tr("Export Mesh"), tr("Exported %1!").arg(path));
3544  }
3545  }
3546  else
3547  {
3548  QString path = QFileDialog::getSaveFileName(
3549  this,
3550  tr("Save File"),
3551  pathDatabase_+"/" + name + ".ply",
3552  tr("Point cloud data (*.ply *.pcd)"));
3553 
3554  if(!path.isEmpty())
3555  {
3556  if(QFileInfo(path).suffix() == "")
3557  {
3558  path += ".ply";
3559  }
3560  bool success = false;
3561  pcl::PCLPointCloud2::Ptr cloud = util3d::laserScanToPointCloud2(LaserScan::backwardCompatibility(cloudMat));
3562  if(QFileInfo(path).suffix() == "pcd")
3563  {
3564  success = pcl::io::savePCDFile(path.toStdString(), *cloud) == 0;
3565  }
3566  else
3567  {
3568  success = pcl::io::savePLYFile(path.toStdString(), *cloud) == 0;
3569  }
3570  if(success)
3571  {
3572  QMessageBox::information(this, tr("Export Point Cloud"), tr("Exported %1!").arg(path));
3573  }
3574  else
3575  {
3576  QMessageBox::critical(this, tr("Export Point Cloud"), tr("Failed exporting %1!").arg(path));
3577  }
3578  }
3579  }
3580  }
3581 }
3582 
3583 void DatabaseViewer::updateOptimizedMesh()
3584 {
3585  if(!ids_.size() || !dbDriver_)
3586  {
3587  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
3588  return;
3589  }
3590 
3591  if(graphes_.empty())
3592  {
3593  this->updateGraphView();
3594  if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
3595  {
3596  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
3597  return;
3598  }
3599  }
3600 
3601  std::map<int, Transform> optimizedPoses;
3602  if(ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() &&
3603  ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked()
3604  && !groundTruthPoses_.empty())
3605  {
3606  optimizedPoses = groundTruthPoses_;
3607  }
3608  else
3609  {
3610  optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
3611  }
3612  if(ui_->groupBox_posefiltering->isChecked())
3613  {
3614  optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
3615  ui_->doubleSpinBox_posefilteringRadius->value(),
3616  ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
3617  }
3618  Transform lastlocalizationPose;
3619  dbDriver_->loadOptimizedPoses(&lastlocalizationPose);
3620  //optimized poses have changed, reset 2d map
3621  dbDriver_->save2DMap(cv::Mat(), 0, 0, 0);
3622  if(optimizedPoses.size() > 0)
3623  {
3624  exportDialog_->setDBDriver(dbDriver_);
3625  exportDialog_->forceAssembling(true);
3626  exportDialog_->setOkButton();
3627 
3628  std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> clouds;
3629  std::map<int, pcl::PolygonMesh::Ptr> meshes;
3630  std::map<int, pcl::TextureMesh::Ptr> textureMeshes;
3631  std::vector<std::map<int, pcl::PointXY> > textureVertexToPixels;
3632 
3633  if(exportDialog_->getExportedClouds(
3634  optimizedPoses,
3635  updateLinksWithModifications(links_),
3636  mapIds_,
3637  QMap<int, Signature>(),
3638  std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
3639  std::map<int, LaserScan>(),
3640  pathDatabase_,
3641  ui_->parameters_toolbox->getParameters(),
3642  clouds,
3643  meshes,
3644  textureMeshes,
3645  textureVertexToPixels))
3646  {
3647  if(textureMeshes.size())
3648  {
3649  dbDriver_->saveOptimizedPoses(optimizedPoses, lastlocalizationPose);
3650 
3651  cv::Mat globalTextures;
3652  pcl::TextureMeshPtr textureMesh = textureMeshes.at(0);
3653  if(textureMesh->tex_materials.size()>1)
3654  {
3655  globalTextures = util3d::mergeTextures(
3656  *textureMesh,
3657  std::map<int, cv::Mat>(),
3658  std::map<int, std::vector<CameraModel> >(),
3659  0,
3660  dbDriver_,
3661  exportDialog_->getTextureSize(),
3662  exportDialog_->getMaxTextures(),
3663  textureVertexToPixels,
3664  exportDialog_->isGainCompensation(),
3665  exportDialog_->getGainBeta(),
3666  exportDialog_->isGainRGB(),
3667  exportDialog_->isBlending(),
3668  exportDialog_->getBlendingDecimation(),
3669  exportDialog_->getTextureBrightnessConstrastRatioLow(),
3670  exportDialog_->getTextureBrightnessConstrastRatioHigh(),
3671  exportDialog_->isExposeFusion());
3672  }
3673  dbDriver_->saveOptimizedMesh(
3674  util3d::laserScanFromPointCloud(textureMesh->cloud, false).data(),
3675  util3d::convertPolygonsFromPCL(textureMesh->tex_polygons),
3676  textureMesh->tex_coordinates,
3677  globalTextures);
3678  QMessageBox::information(this, tr("Update Optimized Textured Mesh"), tr("Updated!"));
3679  ui_->actionView_optimized_mesh->setEnabled(true);
3680  ui_->actionExport_optimized_mesh->setEnabled(true);
3681  this->viewOptimizedMesh();
3682  }
3683  else if(meshes.size())
3684  {
3685  dbDriver_->saveOptimizedPoses(optimizedPoses, lastlocalizationPose);
3686  std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons(1);
3687  polygons.at(0) = util3d::convertPolygonsFromPCL(meshes.at(0)->polygons);
3688  dbDriver_->saveOptimizedMesh(util3d::laserScanFromPointCloud(meshes.at(0)->cloud, false).data(), polygons);
3689  QMessageBox::information(this, tr("Update Optimized Mesh"), tr("Updated!"));
3690  ui_->actionView_optimized_mesh->setEnabled(true);
3691  ui_->actionExport_optimized_mesh->setEnabled(true);
3692  this->viewOptimizedMesh();
3693  }
3694  else if(clouds.size())
3695  {
3696  dbDriver_->saveOptimizedPoses(optimizedPoses, lastlocalizationPose);
3697  dbDriver_->saveOptimizedMesh(util3d::laserScanFromPointCloud(*clouds.at(0)).data());
3698  QMessageBox::information(this, tr("Update Optimized PointCloud"), tr("Updated!"));
3699  ui_->actionView_optimized_mesh->setEnabled(true);
3700  ui_->actionExport_optimized_mesh->setEnabled(true);
3701  this->viewOptimizedMesh();
3702  }
3703  else
3704  {
3705  QMessageBox::critical(this, tr("Update Optimized Mesh"), tr("Nothing to save!"));
3706  }
3707  }
3708  exportDialog_->setProgressDialogToMax();
3709  }
3710  else
3711  {
3712  QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
3713  }
3714 }
3715 
3716 void DatabaseViewer::generateGraph()
3717 {
3718  if(!dbDriver_)
3719  {
3720  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("A database must must loaded first...\nUse File->Open database."));
3721  return;
3722  }
3723 
3724  QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/Graph.dot", tr("Graphiz file (*.dot)"));
3725  if(!path.isEmpty())
3726  {
3727  dbDriver_->generateGraph(path.toStdString());
3728  }
3729 }
3730 
3731 void DatabaseViewer::generateLocalGraph()
3732 {
3733  if(!ids_.size() || !dbDriver_)
3734  {
3735  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
3736  return;
3737  }
3738  bool ok = false;
3739  int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID"), ids_.first(), ids_.first(), ids_.last(), 1, &ok);
3740 
3741  if(ok)
3742  {
3743  int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin"), 4, 1, 100, 1, &ok);
3744  if(ok)
3745  {
3746  QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/Graph" + QString::number(id) + ".dot", tr("Graphiz file (*.dot)"));
3747  if(!path.isEmpty() && id>0)
3748  {
3749  std::map<int, int> ids;
3750  std::list<int> curentMarginList;
3751  std::set<int> currentMargin;
3752  std::set<int> nextMargin;
3753  nextMargin.insert(id);
3754  int m = 0;
3755  while((margin == 0 || m < margin) && nextMargin.size())
3756  {
3757  curentMarginList = std::list<int>(nextMargin.rbegin(), nextMargin.rend());
3758  nextMargin.clear();
3759 
3760  for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
3761  {
3762  if(ids.find(*jter) == ids.end())
3763  {
3764  std::multimap<int, Link> links;
3765  ids.insert(std::pair<int, int>(*jter, m));
3766 
3767  UTimer timer;
3768  dbDriver_->loadLinks(*jter, links);
3769 
3770  // links
3771  for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
3772  {
3773  if( !uContains(ids, iter->first))
3774  {
3775  UASSERT(iter->second.type() != Link::kUndef);
3776  if(iter->second.type() == Link::kNeighbor ||
3777  iter->second.type() == Link::kNeighborMerged)
3778  {
3779  nextMargin.insert(iter->first);
3780  }
3781  else
3782  {
3783  // loop closures are on same margin
3784  if(currentMargin.insert(iter->first).second)
3785  {
3786  curentMarginList.push_back(iter->first);
3787  }
3788  }
3789  }
3790  }
3791  }
3792  }
3793  ++m;
3794  }
3795 
3796  if(ids.size() > 0)
3797  {
3798  ids.insert(std::pair<int,int>(id, 0));
3799  std::set<int> idsSet;
3800  for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
3801  {
3802  idsSet.insert(idsSet.end(), iter->first);
3803  UINFO("Node %d", iter->first);
3804  }
3805  UINFO("idsSet=%d", idsSet.size());
3806  dbDriver_->generateGraph(path.toStdString(), idsSet);
3807  }
3808  else
3809  {
3810  QMessageBox::critical(this, tr("Error"), tr("No neighbors found for signature %1.").arg(id));
3811  }
3812  }
3813  }
3814  }
3815 }
3816 
3817 void DatabaseViewer::regenerateLocalMaps()
3818 {
3819  LocalGridMaker localMapMaker(ui_->parameters_toolbox->getParameters());
3820 
3821  generatedLocalMaps_.clear();
3822 
3823  rtabmap::ProgressDialog progressDialog(this);
3824  progressDialog.setMaximumSteps(ids_.size());
3825  progressDialog.show();
3826  progressDialog.setCancelButtonVisible(true);
3827 
3828  UPlot * plot = new UPlot(this);
3829  plot->setWindowFlags(Qt::Window);
3830  plot->setWindowTitle("Local Occupancy Grid Generation Time (ms)");
3831  plot->setAttribute(Qt::WA_DeleteOnClose);
3832  UPlotCurve * decompressionCurve = plot->addCurve("Decompression");
3833  UPlotCurve * gridCreationCurve = plot->addCurve("Grid Creation");
3834  plot->show();
3835 
3836  UPlot * plotCells = new UPlot(this);
3837  plotCells->setWindowFlags(Qt::Window);
3838  plotCells->setWindowTitle("Occupancy Cells");
3839  plotCells->setAttribute(Qt::WA_DeleteOnClose);
3840  UPlotCurve * totalCurve = plotCells->addCurve("Total");
3841  UPlotCurve * emptyCurve = plotCells->addCurve("Empty");
3842  UPlotCurve * obstaclesCurve = plotCells->addCurve("Obstacles");
3843  UPlotCurve * groundCurve = plotCells->addCurve("Ground");
3844  plotCells->show();
3845 
3846  double decompressionTime = 0;
3847  double gridCreationTime = 0;
3848 
3849  for(int i =0; i<ids_.size() && !progressDialog.isCanceled(); ++i)
3850  {
3851  UTimer timer;
3852  SensorData data;
3853  dbDriver_->getNodeData(ids_.at(i), data);
3854  data.uncompressData();
3855  decompressionTime = timer.ticks()*1000.0;
3856 
3857  int mapId, weight;
3858  Transform odomPose, groundTruth;
3859  std::string label;
3860  double stamp;
3861  QString msg;
3862  std::vector<float> velocity;
3863  GPS gps;
3864  EnvSensors sensors;
3865  if(dbDriver_->getNodeInfo(data.id(), odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors))
3866  {
3867  Signature s = data;
3868  s.setPose(odomPose);
3869  cv::Mat ground, obstacles, empty;
3870  cv::Point3f viewpoint;
3871  timer.ticks();
3872 
3873  if(ui_->checkBox_grid_regenerateFromSavedGrid->isChecked() && s.sensorData().gridCellSize() > 0.0f)
3874  {
3875  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridObstacleCellsRaw()));
3876  *cloud+=*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridGroundCellsRaw()));
3877 
3878  if(cloud->size())
3879  {
3880  // update viewpoint
3881  if(s.sensorData().cameraModels().size())
3882  {
3883  // average of all local transforms
3884  float sum = 0;
3885  for(unsigned int i=0; i<s.sensorData().cameraModels().size(); ++i)
3886  {
3887  const Transform & t = s.sensorData().cameraModels()[i].localTransform();
3888  if(!t.isNull())
3889  {
3890  viewpoint.x += t.x();
3891  viewpoint.y += t.y();
3892  viewpoint.z += t.z();
3893  sum += 1.0f;
3894  }
3895  }
3896  if(sum > 0.0f)
3897  {
3898  viewpoint.x /= sum;
3899  viewpoint.y /= sum;
3900  viewpoint.z /= sum;
3901  }
3902  }
3903  else if(s.sensorData().cameraModels().size())
3904  {
3905  // average of all local transforms
3906  float sum = 0;
3907  for(unsigned int i=0; i<s.sensorData().stereoCameraModels().size(); ++i)
3908  {
3909  const Transform & t = s.sensorData().stereoCameraModels()[i].localTransform();
3910  if(!t.isNull())
3911  {
3912  viewpoint.x += t.x();
3913  viewpoint.y += t.y();
3914  viewpoint.z += t.z();
3915  sum += 1.0f;
3916  }
3917  }
3918  if(sum > 0.0f)
3919  {
3920  viewpoint.x /= sum;
3921  viewpoint.y /= sum;
3922  viewpoint.z /= sum;
3923  }
3924  }
3925 
3926  localMapMaker.createLocalMap(util3d::laserScanFromPointCloud(*cloud), s.getPose(), ground, obstacles, empty, viewpoint);
3927  }
3928  }
3929  else
3930  {
3931  localMapMaker.createLocalMap(s, ground, obstacles, empty, viewpoint);
3932  }
3933 
3934  gridCreationTime = timer.ticks()*1000.0;
3935  generatedLocalMaps_.add(data.id(), ground, obstacles, empty, localMapMaker.getCellSize(), viewpoint);
3936  msg = QString("Generated local occupancy grid map %1/%2").arg(i+1).arg((int)ids_.size());
3937 
3938  totalCurve->addValue(ids_.at(i), obstacles.cols+ground.cols+empty.cols);
3939  emptyCurve->addValue(ids_.at(i), empty.cols);
3940  obstaclesCurve->addValue(ids_.at(i), obstacles.cols);
3941  groundCurve->addValue(ids_.at(i), ground.cols);
3942  }
3943 
3944  progressDialog.appendText(msg);
3945  progressDialog.incrementStep();
3946 
3947  decompressionCurve->addValue(ids_.at(i), decompressionTime);
3948  gridCreationCurve->addValue(ids_.at(i), gridCreationTime);
3949 
3950  if(ids_.size() < 50 || (i+1) % 25 == 0)
3951  {
3952  QApplication::processEvents();
3953  }
3954  }
3955  progressDialog.setValue(progressDialog.maximumSteps());
3956 
3957  if(graphes_.size())
3958  {
3959  update3dView();
3960  sliderIterationsValueChanged((int)graphes_.size()-1);
3961  }
3962  else
3963  {
3964  updateGrid();
3965  }
3966 }
3967 
3968 void DatabaseViewer::regenerateCurrentLocalMaps()
3969 {
3970  UTimer time;
3971  LocalGridMaker localMapMaker(ui_->parameters_toolbox->getParameters());
3972 
3973  if(ids_.size() == 0)
3974  {
3975  UWARN("ids_ is empty!");
3976  return;
3977  }
3978 
3979  QSet<int> idsSet;
3980  idsSet.insert(ids_.at(ui_->horizontalSlider_A->value()));
3981  idsSet.insert(ids_.at(ui_->horizontalSlider_B->value()));
3982 #if QT_VERSION >= QT_VERSION_CHECK(5, 15, 3)
3983  QList<int> ids(idsSet.begin(), idsSet.end());
3984 #else
3985  QList<int> ids = idsSet.toList();
3986 #endif
3987 
3988  rtabmap::ProgressDialog progressDialog(this);
3989  progressDialog.setMaximumSteps(ids.size());
3990  progressDialog.show();
3991 
3992  for(int i =0; i<ids.size(); ++i)
3993  {
3994  SensorData data;
3995  dbDriver_->getNodeData(ids.at(i), data);
3996  data.uncompressData();
3997 
3998  int mapId, weight;
3999  Transform odomPose, groundTruth;
4000  std::string label;
4001  double stamp;
4002  QString msg;
4003  std::vector<float> velocity;
4004  GPS gps;
4005  EnvSensors sensors;
4006  if(dbDriver_->getNodeInfo(data.id(), odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors))
4007  {
4008  Signature s = data;
4009  s.setPose(odomPose);
4010  cv::Mat ground, obstacles, empty;
4011  cv::Point3f viewpoint;
4012 
4013  if(ui_->checkBox_grid_regenerateFromSavedGrid->isChecked() && s.sensorData().gridCellSize() > 0.0f)
4014  {
4015  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridObstacleCellsRaw()));
4016  *cloud+=*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridGroundCellsRaw()));
4017 
4018  if(cloud->size())
4019  {
4020  // update viewpoint
4021  if(s.sensorData().cameraModels().size())
4022  {
4023  // average of all local transforms
4024  float sum = 0;
4025  for(unsigned int i=0; i<s.sensorData().cameraModels().size(); ++i)
4026  {
4027  const Transform & t = s.sensorData().cameraModels()[i].localTransform();
4028  if(!t.isNull())
4029  {
4030  viewpoint.x += t.x();
4031  viewpoint.y += t.y();
4032  viewpoint.z += t.z();
4033  sum += 1.0f;
4034  }
4035  }
4036  if(sum > 0.0f)
4037  {
4038  viewpoint.x /= sum;
4039  viewpoint.y /= sum;
4040  viewpoint.z /= sum;
4041  }
4042  }
4043  else if(s.sensorData().stereoCameraModels().size())
4044  {
4045  // average of all local transforms
4046  float sum = 0;
4047  for(unsigned int i=0; i<s.sensorData().stereoCameraModels().size(); ++i)
4048  {
4049  const Transform & t = s.sensorData().stereoCameraModels()[i].localTransform();
4050  if(!t.isNull())
4051  {
4052  viewpoint.x += t.x();
4053  viewpoint.y += t.y();
4054  viewpoint.z += t.z();
4055  sum += 1.0f;
4056  }
4057  }
4058  if(sum > 0.0f)
4059  {
4060  viewpoint.x /= sum;
4061  viewpoint.y /= sum;
4062  viewpoint.z /= sum;
4063  }
4064  }
4065 
4066  localMapMaker.createLocalMap(util3d::laserScanFromPointCloud(*cloud), s.getPose(), ground, obstacles, empty, viewpoint);
4067  }
4068  }
4069  else
4070  {
4071  localMapMaker.createLocalMap(s, ground, obstacles, empty, viewpoint);
4072  }
4073 
4074  generatedLocalMaps_.add(data.id(), ground, obstacles, empty, localMapMaker.getCellSize(), viewpoint);
4075  msg = QString("Generated local occupancy grid map %1/%2 (%3s)").arg(i+1).arg((int)ids.size()).arg(time.ticks());
4076  }
4077 
4078  progressDialog.appendText(msg);
4079  progressDialog.incrementStep();
4080  QApplication::processEvents();
4081  }
4082  progressDialog.setValue(progressDialog.maximumSteps());
4083 
4084  if(graphes_.size())
4085  {
4086  update3dView();
4087  sliderIterationsValueChanged((int)graphes_.size()-1);
4088  }
4089  else
4090  {
4091  updateGrid();
4092  }
4093 }
4094 
4095 void DatabaseViewer::view3DMap()
4096 {
4097  if(!ids_.size() || !dbDriver_)
4098  {
4099  QMessageBox::warning(this, tr("Cannot view 3D map"), tr("The database is empty..."));
4100  return;
4101  }
4102 
4103  if(graphes_.empty())
4104  {
4105  this->updateGraphView();
4106  if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
4107  {
4108  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
4109  return;
4110  }
4111  }
4112 
4113  std::map<int, Transform> optimizedPoses;
4114  if(ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() &&
4115  ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() &&
4116  !groundTruthPoses_.empty())
4117  {
4118  optimizedPoses = groundTruthPoses_;
4119  }
4120  else
4121  {
4122  optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
4123  }
4124  if(ui_->groupBox_posefiltering->isChecked())
4125  {
4126  optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
4127  ui_->doubleSpinBox_posefilteringRadius->value(),
4128  ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
4129  }
4130  if(optimizedPoses.size() > 0)
4131  {
4132  exportDialog_->setDBDriver(dbDriver_);
4133  exportDialog_->viewClouds(optimizedPoses,
4134  updateLinksWithModifications(links_),
4135  mapIds_,
4136  QMap<int, Signature>(),
4137  std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
4138  std::map<int, LaserScan>(),
4139  pathDatabase_,
4140  ui_->parameters_toolbox->getParameters());
4141  }
4142  else
4143  {
4144  QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
4145  }
4146 }
4147 
4148 void DatabaseViewer::generate3DMap()
4149 {
4150  if(!ids_.size() || !dbDriver_)
4151  {
4152  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
4153  return;
4154  }
4155 
4156  if(graphes_.empty())
4157  {
4158  this->updateGraphView();
4159  if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
4160  {
4161  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
4162  return;
4163  }
4164  }
4165 
4166  std::map<int, Transform> optimizedPoses;
4167  if(ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() &&
4168  ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() &&
4169  !groundTruthPoses_.empty())
4170  {
4171  optimizedPoses = groundTruthPoses_;
4172  }
4173  else
4174  {
4175  optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
4176  }
4177  if(ui_->groupBox_posefiltering->isChecked())
4178  {
4179  optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
4180  ui_->doubleSpinBox_posefilteringRadius->value(),
4181  ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
4182  }
4183  if(optimizedPoses.size() > 0)
4184  {
4185  exportDialog_->setDBDriver(dbDriver_);
4186  exportDialog_->exportClouds(optimizedPoses,
4187  updateLinksWithModifications(links_),
4188  mapIds_,
4189  QMap<int, Signature>(),
4190  std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
4191  std::map<int, LaserScan>(),
4192  pathDatabase_,
4193  ui_->parameters_toolbox->getParameters());
4194  }
4195  else
4196  {
4197  QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
4198  }
4199 }
4200 
4201 void DatabaseViewer::detectMoreLoopClosures()
4202 {
4203  if(graphes_.empty())
4204  {
4205  this->updateGraphView();
4206  if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
4207  {
4208  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
4209  return;
4210  }
4211  }
4212 
4213  std::map<int, Transform> optimizedPoses = graphes_.back();
4214 
4215  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
4216  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
4217  progressDialog->setMaximumSteps(1);
4218  progressDialog->setCancelButtonVisible(true);
4219  progressDialog->setMinimumWidth(800);
4220  progressDialog->show();
4221 
4222  const ParametersMap & parameters = ui_->parameters_toolbox->getParameters();
4223  bool loopCovLimited = Parameters::defaultRGBDLoopCovLimited();
4224  Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), loopCovLimited);
4225  if(loopCovLimited)
4226  {
4227  odomMaxInf_ = graph::getMaxOdomInf(updateLinksWithModifications(links_));
4228  }
4229 
4230  int iterations = ui_->spinBox_detectMore_iterations->value();
4231  UASSERT(iterations > 0);
4232  int added = 0;
4233  std::multimap<int, int> checkedLoopClosures;
4234  std::pair<int, int> lastAdded(0,0);
4235  bool intraSession = ui_->checkBox_detectMore_intraSession->isChecked();
4236  bool interSession = ui_->checkBox_detectMore_interSession->isChecked();
4237  bool useOptimizedGraphAsGuess = ui_->checkBox_opt_graph_as_guess->isChecked();
4238  if(!interSession && !intraSession)
4239  {
4240  QMessageBox::warning(this, tr("Cannot detect more loop closures"), tr("Intra and inter session parameters are disabled! Enable one or both."));
4241  return;
4242  }
4243 
4244  for(int n=0; n<iterations; ++n)
4245  {
4246  UINFO("iteration %d/%d", n+1, iterations);
4247  std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(
4248  std::map<int, Transform>(optimizedPoses.upper_bound(0), optimizedPoses.end()),
4249  ui_->doubleSpinBox_detectMore_radius->value(),
4250  ui_->doubleSpinBox_detectMore_angle->value()*CV_PI/180.0);
4251 
4252  progressDialog->setMaximumSteps(progressDialog->maximumSteps()+(int)clusters.size());
4253  progressDialog->appendText(tr("Looking for more loop closures, %1 clusters found.").arg(clusters.size()));
4254  QApplication::processEvents();
4255  if(progressDialog->isCanceled())
4256  {
4257  break;
4258  }
4259 
4260  std::set<int> addedLinks;
4261  int i=0;
4262  for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end() && !progressDialog->isCanceled(); ++iter, ++i)
4263  {
4264  int from = iter->first;
4265  int to = iter->second;
4266  if(from < to)
4267  {
4268  from = iter->second;
4269  to = iter->first;
4270  }
4271 
4272  int mapIdFrom = uValue(mapIds_, from, 0);
4273  int mapIdTo = uValue(mapIds_, to, 0);
4274 
4275  if((interSession && mapIdFrom != mapIdTo) ||
4276  (intraSession && mapIdFrom == mapIdTo))
4277  {
4278  // only add new links and one per cluster per iteration
4279  if(rtabmap::graph::findLink(checkedLoopClosures, from, to) == checkedLoopClosures.end())
4280  {
4281  if(!findActiveLink(from, to).isValid() && !containsLink(linksRemoved_, from, to) &&
4282  addedLinks.find(from) == addedLinks.end() &&
4283  addedLinks.find(to) == addedLinks.end())
4284  {
4285  // Reverify if in the bounds with the current optimized graph
4286  Transform delta = optimizedPoses.at(from).inverse() * optimizedPoses.at(to);
4287  if(delta.getNorm() < ui_->doubleSpinBox_detectMore_radius->value() &&
4288  delta.getNorm() >= ui_->doubleSpinBox_detectMore_radiusMin->value())
4289  {
4290  checkedLoopClosures.insert(std::make_pair(from, to));
4291  if(addConstraint(from, to, true, useOptimizedGraphAsGuess))
4292  {
4293  UINFO("Added new loop closure between %d and %d.", from, to);
4294  ++added;
4295  addedLinks.insert(from);
4296  addedLinks.insert(to);
4297  lastAdded.first = from;
4298  lastAdded.second = to;
4299 
4300  progressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
4301  QApplication::processEvents();
4302 
4303  optimizedPoses = graphes_.back();
4304  }
4305  }
4306  }
4307  }
4308  }
4309  progressDialog->incrementStep();
4310  if(i%100)
4311  {
4312  QApplication::processEvents();
4313  }
4314  }
4315  UINFO("Iteration %d/%d: added %d loop closures.", n+1, iterations, (int)addedLinks.size()/2);
4316  progressDialog->appendText(tr("Iteration %1/%2: Detected %3 loop closures!").arg(n+1).arg(iterations).arg(addedLinks.size()/2));
4317  if(addedLinks.size() == 0)
4318  {
4319  break;
4320  }
4321  if(n+1<iterations)
4322  {
4323  // Re-optimize the map before doing next iterations
4324  this->updateGraphView();
4325  optimizedPoses = graphes_.back();
4326  }
4327  }
4328 
4329  odomMaxInf_.clear();
4330 
4331  if(added)
4332  {
4333  this->updateGraphView();
4334  this->updateLoopClosuresSlider(lastAdded.first, lastAdded.second);
4335  }
4336  UINFO("Total added %d loop closures.", added);
4337 
4338  progressDialog->appendText(tr("Total new loop closures detected=%1").arg(added));
4339  progressDialog->setValue(progressDialog->maximumSteps());
4340 }
4341 
4342 void DatabaseViewer::updateAllNeighborCovariances()
4343 {
4344  std::multimap<int, Link> allLinks = updateLinksWithModifications(links_);
4345  QList<rtabmap::Link> links;
4346  for(std::multimap<int, Link>::iterator iter=allLinks.begin(); iter!=allLinks.end(); ++iter)
4347  {
4348  if(iter->second.type() == Link::kNeighbor ||
4349  iter->second.type() == Link::kNeighborMerged)
4350  {
4351  links.push_back(iter->second);
4352  }
4353  }
4354  updateCovariances(links);
4355 }
4356 void DatabaseViewer::updateAllLoopClosureCovariances()
4357 {
4358  std::multimap<int, Link> allLinks = updateLinksWithModifications(links_);
4359  QList<rtabmap::Link> links;
4360  for(std::multimap<int, Link>::iterator iter=allLinks.begin(); iter!=allLinks.end(); ++iter)
4361  {
4362  if(iter->second.type() != Link::kNeighbor &&
4363  iter->second.type() != Link::kNeighborMerged &&
4364  iter->second.type() != Link::kLandmark &&
4365  iter->second.from() != iter->second.to())
4366  {
4367  links.push_back(iter->second);
4368  }
4369  }
4370  updateCovariances(links);
4371 }
4372 void DatabaseViewer::updateAllLandmarkCovariances()
4373 {
4374  std::multimap<int, Link> allLinks = updateLinksWithModifications(links_);
4375  QList<rtabmap::Link> links;
4376  for(std::multimap<int, Link>::iterator iter=allLinks.begin(); iter!=allLinks.end(); ++iter)
4377  {
4378  if(iter->second.type() == Link::kLandmark)
4379  {
4380  links.push_back(iter->second);
4381  }
4382  }
4383  updateCovariances(links);
4384 }
4385 
4386 void DatabaseViewer::updateCovariances(const QList<Link> & links)
4387 {
4388  if(links.size())
4389  {
4390  cv::Mat infMatrix = links.first().infMatrix();
4391  EditConstraintDialog dialog(Transform::getIdentity(), infMatrix.inv());
4392  dialog.setPoseGroupVisible(false);
4393  if(dialog.exec() != QDialog::Accepted)
4394  {
4395  return;
4396  }
4397 
4398  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
4399  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
4400  progressDialog->setMaximumSteps(links.size());
4401  progressDialog->setCancelButtonVisible(true);
4402  progressDialog->setMinimumWidth(800);
4403  progressDialog->show();
4404 
4405  infMatrix = dialog.getCovariance().inv();
4406 
4407  for(int i=0; i<links.size(); ++i)
4408  {
4409  int from = links[i].from();
4410  int to = links[i].to();
4411 
4412  Link currentLink = findActiveLink(from, to);
4413  if(!currentLink.isValid())
4414  {
4415  UERROR("Not found link! (%d->%d)", from, to);
4416  return;
4417  }
4418  currentLink = Link(
4419  currentLink.from(),
4420  currentLink.to(),
4421  currentLink.type(),
4422  currentLink.transform(),
4423  infMatrix.clone(),
4424  currentLink.userDataCompressed());
4425  bool updated = false;
4426  std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
4427  while(iter != linksRefined_.end() && iter->first == currentLink.from())
4428  {
4429  if(iter->second.to() == currentLink.to() &&
4430  iter->second.type() == currentLink.type())
4431  {
4432  iter->second = currentLink;
4433  updated = true;
4434  break;
4435  }
4436  ++iter;
4437  }
4438  if(!updated)
4439  {
4440  linksRefined_.insert(std::make_pair(currentLink.from(), currentLink));
4441  }
4442 
4443  progressDialog->appendText(tr("Updated link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(links.size()));
4444  progressDialog->incrementStep();
4445  QApplication::processEvents();
4446  if(progressDialog->isCanceled())
4447  {
4448  break;
4449  }
4450  }
4451  this->updateGraphView();
4452 
4453  progressDialog->setValue(progressDialog->maximumSteps());
4454  progressDialog->appendText("Refining links finished!");
4455  }
4456 }
4457 
4458 void DatabaseViewer::refineLinks()
4459 {
4460  int minNodeId = 0;
4461  int maxNodeId = 0;
4462  int minMapId = 0;
4463  int maxMapId = 0;
4464  std::multimap<int, Link> allLinks = updateLinksWithModifications(links_);
4465  for(std::multimap<int, Link>::iterator iter=allLinks.begin(); iter!=allLinks.end(); ++iter)
4466  {
4467  if(iter->second.type() < Link::kPosePrior)
4468  {
4469  int minId = iter->second.from()>iter->second.to()?iter->second.to():iter->second.from();
4470  int maxId = iter->second.from()<iter->second.to()?iter->second.to():iter->second.from();
4471  if(minNodeId == 0 || minNodeId > minId)
4472  {
4473  minNodeId = minId;
4474  }
4475  if(maxNodeId == 0 || maxNodeId < maxId)
4476  {
4477  maxNodeId = maxId;
4478  }
4479  }
4480  }
4481  if(minNodeId > 0)
4482  {
4483  minMapId = uValue(mapIds_, minNodeId, 0);
4484  maxMapId = uValue(mapIds_, maxNodeId, minMapId);
4485 
4486  linkRefiningDialog_->setMinMax(
4487  minNodeId,
4488  maxNodeId,
4489  minMapId,
4490  maxMapId);
4491 
4492  if(linkRefiningDialog_->exec() == QDialog::Accepted)
4493  {
4494  QList<Link> links;
4495  Link::Type type = linkRefiningDialog_->getLinkType();
4496  linkRefiningDialog_->getRangeNodeId(minNodeId, maxNodeId);
4497  linkRefiningDialog_->getRangeNodeId(minMapId, maxMapId);
4498  bool intra, inter;
4499  linkRefiningDialog_->getIntraInterSessions(intra, inter);
4500  for(std::multimap<int, Link>::iterator iter=allLinks.begin(); iter!=allLinks.end(); ++iter)
4501  {
4502  if(iter->second.type() < Link::kPosePrior &&
4503  (type==Link::kEnd || type == iter->second.type()))
4504  {
4505  int from = iter->second.from();
4506  int to = iter->second.to();
4507  int mapFrom = uValue(mapIds_, from, 0);
4508  int mapTo = uValue(mapIds_, to, 0);
4509  if(((linkRefiningDialog_->isRangeByNodeId() &&
4510  ((from >= minNodeId && from <= maxNodeId) ||
4511  (to >= minNodeId && to <= maxNodeId))) ||
4512  (linkRefiningDialog_->isRangeByMapId() &&
4513  ((mapFrom >= minMapId && mapFrom <= maxMapId) ||
4514  (mapTo >= minMapId && mapTo <= maxMapId)))) &&
4515  ((intra && mapTo == mapFrom) ||
4516  (inter && mapTo != mapFrom)))
4517  {
4518  links.push_back(iter->second);
4519  }
4520  }
4521  }
4522  if(links.isEmpty())
4523  {
4524  QMessageBox::warning(this, tr("Refine links"), tr("No links found matching the requested parameters."));
4525  return;
4526  }
4527  else
4528  {
4529  refineLinks(links);
4530  }
4531  }
4532  }
4533  else
4534  {
4535  UWARN("No links can be refined!");
4536  }
4537 }
4538 void DatabaseViewer::refineLinks(const QList<Link> & links)
4539 {
4540  if(links.size())
4541  {
4542  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
4543  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
4544  progressDialog->setMaximumSteps(links.size());
4545  progressDialog->setCancelButtonVisible(true);
4546  progressDialog->setMinimumWidth(800);
4547  progressDialog->show();
4548 
4549  for(int i=0; i<links.size(); ++i)
4550  {
4551  int from = links[i].from();
4552  int to = links[i].to();
4553  if(from > 0 && to > 0)
4554  {
4555  this->refineConstraint(links[i].from(), links[i].to(), true);
4556  progressDialog->appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(links.size()));
4557  }
4558  else
4559  {
4560  progressDialog->appendText(tr("Ignored link %1->%2 (landmark)").arg(from).arg(to));
4561  }
4562  progressDialog->incrementStep();
4563  QApplication::processEvents();
4564  if(progressDialog->isCanceled())
4565  {
4566  break;
4567  }
4568  }
4569  this->updateGraphView();
4570 
4571  progressDialog->setValue(progressDialog->maximumSteps());
4572  progressDialog->appendText("Refining links finished!");
4573  }
4574 }
4575 
4576 void DatabaseViewer::resetAllChanges()
4577 {
4578  if(QMessageBox::question(this,
4579  tr("Reset all changes"),
4580  tr("You are about to reset all changes you've made so far, do you want to continue?"),
4581  QMessageBox::Yes | QMessageBox::No,
4582  QMessageBox::No) == QMessageBox::Yes)
4583  {
4584  linksAdded_.clear();
4585  linksRefined_.clear();
4586  linksRemoved_.clear();
4587  generatedLocalMaps_.clear();
4588  modifiedLaserScans_.clear();
4589  updateNeighborsSlider();
4590  updateLoopClosuresSlider();
4591  this->updateGraphView();
4592  }
4593 }
4594 
4595 void DatabaseViewer::graphNodeSelected(int id)
4596 {
4597  if(id>0 && idToIndex_.contains(id))
4598  {
4599  static bool updateA = true;
4600  if(updateA || ui_->actionConcise_Layout->isChecked())
4601  ui_->horizontalSlider_A->setValue(idToIndex_.value(id));
4602  else
4603  ui_->horizontalSlider_B->setValue(idToIndex_.value(id));
4604  updateA = !updateA || ui_->actionConcise_Layout->isChecked();
4605  }
4606 }
4607 
4608 void DatabaseViewer::graphLinkSelected(int from, int to)
4609 {
4610  if(from>0 && idToIndex_.contains(from))
4611  ui_->horizontalSlider_A->setValue(idToIndex_.value(from));
4612  if(to>0 && idToIndex_.contains(to))
4613  ui_->horizontalSlider_B->setValue(idToIndex_.value(to));
4614 }
4615 
4616 void DatabaseViewer::sliderAValueChanged(int value)
4617 {
4618  this->update(value,
4619  ui_->spinBox_indexA,
4620  ui_->label_parentsA,
4621  ui_->label_childrenA,
4622  ui_->label_weightA,
4623  ui_->label_labelA,
4624  ui_->label_stampA,
4625  ui_->graphicsView_A,
4626  ui_->label_idA,
4627  ui_->label_mapA,
4628  ui_->label_poseA,
4629  ui_->label_optposeA,
4630  ui_->label_velA,
4631  ui_->label_calibA,
4632  ui_->label_scanA,
4633  ui_->label_gravityA,
4634  ui_->label_priorA,
4635  ui_->toolButton_edit_priorA,
4636  ui_->toolButton_remove_priorA,
4637  ui_->label_gpsA,
4638  ui_->label_gtA,
4639  ui_->label_sensorsA,
4640  true);
4641 }
4642 
4643 void DatabaseViewer::sliderBValueChanged(int value)
4644 {
4645  this->update(value,
4646  ui_->spinBox_indexB,
4647  ui_->label_parentsB,
4648  ui_->label_childrenB,
4649  ui_->label_weightB,
4650  ui_->label_labelB,
4651  ui_->label_stampB,
4652  ui_->graphicsView_B,
4653  ui_->label_idB,
4654  ui_->label_mapB,
4655  ui_->label_poseB,
4656  ui_->label_optposeB,
4657  ui_->label_velB,
4658  ui_->label_calibB,
4659  ui_->label_scanB,
4660  ui_->label_gravityB,
4661  ui_->label_priorB,
4662  ui_->toolButton_edit_priorB,
4663  ui_->toolButton_remove_priorB,
4664  ui_->label_gpsB,
4665  ui_->label_gtB,
4666  ui_->label_sensorsB,
4667  true);
4668 }
4669 
4670 void DatabaseViewer::update(int value,
4671  QSpinBox * spinBoxIndex,
4672  QLabel * labelParents,
4673  QLabel * labelChildren,
4674  QLabel * weight,
4675  QLabel * label,
4676  QLabel * stamp,
4677  rtabmap::ImageView * view,
4678  QLabel * labelId,
4679  QLabel * labelMapId,
4680  QLabel * labelPose,
4681  QLabel * labelOptPose,
4682  QLabel * labelVelocity,
4683  QLabel * labelCalib,
4684  QLabel * labelScan,
4685  QLabel * labelGravity,
4686  QLabel * labelPrior,
4687  QToolButton * editPriorButton,
4688  QToolButton * removePriorButton,
4689  QLabel * labelGps,
4690  QLabel * labelGt,
4691  QLabel * labelSensors,
4692  bool updateConstraintView)
4693 {
4694  lastSliderIndexBrowsed_ = value;
4695 
4696  UTimer timer;
4697  spinBoxIndex->blockSignals(true);
4698  spinBoxIndex->setValue(value);
4699  spinBoxIndex->blockSignals(false);
4700  labelParents->clear();
4701  labelChildren->clear();
4702  weight->clear();
4703  label->clear();
4704  labelMapId->clear();
4705  labelPose->clear();
4706  labelOptPose->clear();
4707  labelVelocity->clear();
4708  stamp->clear();
4709  labelCalib->clear();
4710  labelScan ->clear();
4711  labelGravity->clear();
4712  labelPrior->clear();
4713  editPriorButton->setVisible(false);
4714  removePriorButton->setVisible(false);
4715  labelGps->clear();
4716  labelGt->clear();
4717  labelSensors->clear();
4718  if(value >= 0 && value < ids_.size())
4719  {
4720  view->clear();
4721  int id = ids_.at(value);
4722  int mapId = -1;
4723  labelId->setText(QString::number(id));
4724  if(id>0)
4725  {
4726  if(ui_->dockWidget_graphView->isVisible()) {
4727  ui_->graphViewer->highlightNode(id, spinBoxIndex==ui_->spinBox_indexB?1:0);
4728  }
4729 
4730  //image
4731  QImage img;
4732  cv::Mat imgDepth;
4733  if(dbDriver_)
4734  {
4735  SensorData data;
4736  dbDriver_->getNodeData(id, data);
4737  data.uncompressData();
4738  if(!data.imageRaw().empty())
4739  {
4740  img = uCvMat2QImage(data.imageRaw());
4741  }
4742  if(!data.depthOrRightRaw().empty())
4743  {
4744  cv::Mat depth =data.depthOrRightRaw();
4745  if(!data.depthRaw().empty())
4746  {
4747  if(ui_->spinBox_mesh_fillDepthHoles->value() > 0)
4748  {
4749  depth = util2d::fillDepthHoles(depth, ui_->spinBox_mesh_fillDepthHoles->value(), float(ui_->spinBox_mesh_depthError->value())/100.0f);
4750  }
4751  }
4752  if( !data.imageRaw().empty() &&
4753  !data.rightRaw().empty() &&
4754  data.stereoCameraModels().size()==1 && // Multiple stereo cameras not implemented
4755  data.stereoCameraModels()[0].isValidForProjection() &&
4756  ui_->checkBox_showDisparityInsteadOfRight->isChecked())
4757  {
4758  rtabmap::StereoDense * denseStereo = rtabmap::StereoDense::create(ui_->parameters_toolbox->getParameters());
4759  depth = util2d::depthFromDisparity(denseStereo->computeDisparity(data.imageRaw(), data.rightRaw()), data.stereoCameraModels()[0].left().fx(), data.stereoCameraModels()[0].baseline(), CV_32FC1);
4760  delete denseStereo;
4761  }
4762  imgDepth = depth;
4763  }
4764 
4765  QRectF rect;
4766  if(!img.isNull())
4767  {
4768  Transform pose;
4769  if(!graphes_.empty() && graphes_.back().find(data.id())!=graphes_.back().end())
4770  {
4771  pose = graphes_.back().at(data.id());
4772  }
4773  view->setImage(img, data.cameraModels(), pose);
4774  rect = img.rect();
4775  }
4776  else
4777  {
4778  ULOGGER_DEBUG("Image is empty");
4779  }
4780 
4781  if(!imgDepth.empty())
4782  {
4783  view->setImageDepth(imgDepth);
4784  if(img.isNull())
4785  {
4786  rect.setWidth(imgDepth.cols);
4787  rect.setHeight(imgDepth.rows);
4788  }
4789  }
4790  else
4791  {
4792  ULOGGER_DEBUG("Image depth is empty");
4793  }
4794  if(!rect.isValid())
4795  {
4796  if(data.cameraModels().size())
4797  {
4798  for(unsigned int i=0; i<data.cameraModels().size(); ++i)
4799  {
4800  rect.setWidth(rect.width()+data.cameraModels()[i].imageWidth());
4801  rect.setHeight(std::max((int)rect.height(), data.cameraModels()[i].imageHeight()));
4802  }
4803  }
4804  else if(data.stereoCameraModels().size())
4805  {
4806  for(unsigned int i=0; i<data.cameraModels().size(); ++i)
4807  {
4808  rect.setWidth(rect.width()+data.stereoCameraModels()[i].left().imageWidth());
4809  rect.setHeight(std::max((int)rect.height(), data.stereoCameraModels()[i].left().imageHeight()));
4810  }
4811  }
4812  }
4813  if(rect.isValid())
4814  {
4815  view->setSceneRect(rect);
4816  }
4817 
4818  std::list<int> ids;
4819  ids.push_back(id);
4820  std::list<Signature*> signatures;
4821  dbDriver_->loadSignatures(ids, signatures);
4822 
4823  if(signatures.size() && signatures.front()!=0 && !signatures.front()->getWordsKpts().empty())
4824  {
4825  std::multimap<int, cv::KeyPoint> keypoints;
4826  for(std::map<int, int>::const_iterator iter=signatures.front()->getWords().begin(); iter!=signatures.front()->getWords().end(); ++iter)
4827  {
4828  keypoints.insert(std::make_pair(iter->first, signatures.front()->getWordsKpts()[iter->second]));
4829  }
4830  view->setFeatures(keypoints, data.depthOrRightRaw().type() == CV_8UC1?cv::Mat():data.depthOrRightRaw(), Qt::yellow);
4831  }
4832 
4833  Transform odomPose, g;
4834  int w;
4835  std::string l;
4836  double s;
4837  std::vector<float> v;
4838  GPS gps;
4839  EnvSensors sensors;
4840  dbDriver_->getNodeInfo(id, odomPose, mapId, w, l, s, g, v, gps, sensors);
4841 
4842  weight->setNum(w);
4843  label->setText(l.c_str());
4844  float x,y,z,roll,pitch,yaw;
4845  odomPose.getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw);
4846  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));
4847  if(graphes_.size())
4848  {
4849  if(graphes_.back().find(id) == graphes_.back().end())
4850  {
4851  labelOptPose->setText("<Not in optimized graph>");
4852  }
4853  else
4854  {
4855  graphes_.back().find(id)->second.getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw);
4856  labelOptPose->setText(QString("xyz=(%1,%2,%3)\nrpy=(%4,%5,%6)").arg(x).arg(y).arg(z).arg(roll).arg(pitch).arg(yaw));
4857  }
4858  }
4859  if(s!=0.0)
4860  {
4861  stamp->setText(QString::number(s, 'f'));
4862  stamp->setToolTip(QDateTime::fromMSecsSinceEpoch(s*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
4863  }
4864  if(v.size()==6)
4865  {
4866  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]));
4867  }
4868 
4869  std::multimap<int, Link> gravityLink;
4870  dbDriver_->loadLinks(id, gravityLink, Link::kGravity);
4871  if(!gravityLink.empty())
4872  {
4873  float roll,pitch,yaw;
4874  gravityLink.begin()->second.transform().getEulerAngles(roll, pitch, yaw);
4875  Eigen::Vector3d v = Transform(0,0,0,roll,pitch,0).toEigen3d() * -Eigen::Vector3d::UnitZ();
4876  labelGravity->setText(QString("x=%1 y=%2 z=%3").arg(v[0]).arg(v[1]).arg(v[2]));
4877  labelGravity->setToolTip(QString("roll=%1 pitch=%2 yaw=%3").arg(roll).arg(pitch).arg(yaw));
4878  }
4879 
4880  std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
4881  if(graph::findLink(links, id, id, false, Link::kPosePrior)!=links.end())
4882  {
4883  Link & priorLink = graph::findLink(links, id, id, false, Link::kPosePrior)->second;
4884  priorLink.transform().getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw);
4885  labelPrior->setText(QString("xyz=(%1,%2,%3)\nrpy=(%4,%5,%6)").arg(x).arg(y).arg(z).arg(roll).arg(pitch).arg(yaw));
4886  std::stringstream out;
4887  out << priorLink.infMatrix().inv();
4888  labelPrior->setToolTip(QString("%1").arg(out.str().c_str()));
4889  removePriorButton->setVisible(true);
4890  editPriorButton->setToolTip(tr("Edit Prior"));
4891  editPriorButton->setText("...");
4892  editPriorButton->setVisible(true);
4893  }
4894  else if(!odomPose.isNull())
4895  {
4896  editPriorButton->setToolTip(tr("Add Prior"));
4897  editPriorButton->setText("+");
4898  editPriorButton->setVisible(true);
4899  }
4900 
4901  if(gps.stamp()>0.0)
4902  {
4903  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()));
4904  labelGps->setToolTip(QDateTime::fromMSecsSinceEpoch(gps.stamp()*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
4905  }
4906  if(!g.isNull())
4907  {
4908  labelGt->setText(QString("%1").arg(g.prettyPrint().c_str()));
4909  }
4910  if(sensors.size())
4911  {
4912  QString sensorsStr;
4913  QString tooltipStr;
4914  for(EnvSensors::iterator iter=sensors.begin(); iter!=sensors.end(); ++iter)
4915  {
4916  if(iter != sensors.begin())
4917  {
4918  sensorsStr += " | ";
4919  tooltipStr += " | ";
4920  }
4921 
4922  if(iter->first == EnvSensor::kWifiSignalStrength)
4923  {
4924  sensorsStr += uFormat("%.1f dbm", iter->second.value()).c_str();
4925  tooltipStr += "Wifi signal strength";
4926  }
4927  else if(iter->first == EnvSensor::kAmbientTemperature)
4928  {
4929  sensorsStr += uFormat("%.1f \u00B0C", iter->second.value()).c_str();
4930  tooltipStr += "Ambient Temperature";
4931  }
4932  else if(iter->first == EnvSensor::kAmbientAirPressure)
4933  {
4934  sensorsStr += uFormat("%.1f hPa", iter->second.value()).c_str();
4935  tooltipStr += "Ambient Air Pressure";
4936  }
4937  else if(iter->first == EnvSensor::kAmbientLight)
4938  {
4939  sensorsStr += uFormat("%.0f lx", iter->second.value()).c_str();
4940  tooltipStr += "Ambient Light";
4941  }
4942  else if(iter->first == EnvSensor::kAmbientRelativeHumidity)
4943  {
4944  sensorsStr += uFormat("%.0f %%", iter->second.value()).c_str();
4945  tooltipStr += "Ambient Relative Humidity";
4946  }
4947  else
4948  {
4949  sensorsStr += uFormat("%.2f", iter->second.value()).c_str();
4950  tooltipStr += QString("Type %1").arg((int)iter->first);
4951  }
4952 
4953  }
4954  labelSensors->setText(sensorsStr);
4955  labelSensors->setToolTip(tooltipStr);
4956  }
4957  if(data.cameraModels().size() || data.stereoCameraModels().size())
4958  {
4959  std::stringstream calibrationDetails;
4960  if(data.cameraModels().size())
4961  {
4962  if(!data.depthRaw().empty() && data.depthRaw().cols!=data.imageRaw().cols && data.imageRaw().cols)
4963  {
4964  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]")
4965  .arg(data.cameraModels().size())
4966  .arg(data.cameraModels()[0].imageWidth()>0?data.cameraModels()[0].imageWidth():data.imageRaw().cols/data.cameraModels().size())
4967  .arg(data.cameraModels()[0].imageHeight()>0?data.cameraModels()[0].imageHeight():data.imageRaw().rows)
4968  .arg(data.cameraModels()[0].fx())
4969  .arg(data.cameraModels()[0].fy())
4970  .arg(data.cameraModels()[0].cx())
4971  .arg(data.cameraModels()[0].cy())
4972  .arg(data.depthRaw().cols/data.cameraModels().size())
4973  .arg(data.depthRaw().rows)
4974  .arg(data.cameraModels()[0].localTransform().prettyPrint().c_str())
4975  .arg(data.cameraModels()[0].localTransform().r11()).arg(data.cameraModels()[0].localTransform().r12()).arg(data.cameraModels()[0].localTransform().r13()).arg(data.cameraModels()[0].localTransform().o14())
4976  .arg(data.cameraModels()[0].localTransform().r21()).arg(data.cameraModels()[0].localTransform().r22()).arg(data.cameraModels()[0].localTransform().r23()).arg(data.cameraModels()[0].localTransform().o24())
4977  .arg(data.cameraModels()[0].localTransform().r31()).arg(data.cameraModels()[0].localTransform().r32()).arg(data.cameraModels()[0].localTransform().r33()).arg(data.cameraModels()[0].localTransform().o34()));
4978  }
4979  else
4980  {
4981  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]")
4982  .arg(data.cameraModels().size())
4983  .arg(data.cameraModels()[0].imageWidth()>0?data.cameraModels()[0].imageWidth():data.imageRaw().cols/data.cameraModels().size())
4984  .arg(data.cameraModels()[0].imageHeight()>0?data.cameraModels()[0].imageHeight():data.imageRaw().rows)
4985  .arg(data.cameraModels()[0].fx())
4986  .arg(data.cameraModels()[0].fy())
4987  .arg(data.cameraModels()[0].cx())
4988  .arg(data.cameraModels()[0].cy())
4989  .arg(data.cameraModels()[0].localTransform().prettyPrint().c_str())
4990  .arg(data.cameraModels()[0].localTransform().r11()).arg(data.cameraModels()[0].localTransform().r12()).arg(data.cameraModels()[0].localTransform().r13()).arg(data.cameraModels()[0].localTransform().o14())
4991  .arg(data.cameraModels()[0].localTransform().r21()).arg(data.cameraModels()[0].localTransform().r22()).arg(data.cameraModels()[0].localTransform().r23()).arg(data.cameraModels()[0].localTransform().o24())
4992  .arg(data.cameraModels()[0].localTransform().r31()).arg(data.cameraModels()[0].localTransform().r32()).arg(data.cameraModels()[0].localTransform().r33()).arg(data.cameraModels()[0].localTransform().o34()));
4993  }
4994 
4995  for(unsigned int i=0; i<data.cameraModels().size();++i)
4996  {
4997  if(i!=0) calibrationDetails << std::endl;
4998  calibrationDetails << "Id: " << i << " Size=" << data.cameraModels()[i].imageWidth() << "x" << data.cameraModels()[i].imageHeight() << std::endl;
4999  if( data.cameraModels()[i].K_raw().total()) calibrationDetails << "K=" << data.cameraModels()[i].K_raw() << std::endl;
5000  if( data.cameraModels()[i].D_raw().total()) calibrationDetails << "D=" << data.cameraModels()[i].D_raw() << std::endl;
5001  if( data.cameraModels()[i].R().total()) calibrationDetails << "R=" << data.cameraModels()[i].R() << std::endl;
5002  if( data.cameraModels()[i].P().total()) calibrationDetails << "P=" << data.cameraModels()[i].P() << std::endl;
5003  calibrationDetails << "BaseToCam(without opt rot)=" << (data.cameraModels()[i].localTransform()*CameraModel::opticalRotation().inverse()).prettyPrint() << std::endl;
5004  }
5005 
5006  }
5007  else if(data.stereoCameraModels().size())
5008  {
5009  //stereo
5010  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]")
5011  .arg(data.stereoCameraModels()[0].left().imageWidth()>0?data.stereoCameraModels()[0].left().imageWidth():data.imageRaw().cols)
5012  .arg(data.stereoCameraModels()[0].left().imageHeight()>0?data.stereoCameraModels()[0].left().imageHeight():data.imageRaw().rows)
5013  .arg(data.stereoCameraModels()[0].left().fx())
5014  .arg(data.stereoCameraModels()[0].left().fy())
5015  .arg(data.stereoCameraModels()[0].left().cx())
5016  .arg(data.stereoCameraModels()[0].left().cy())
5017  .arg(data.stereoCameraModels()[0].baseline())
5018  .arg(data.stereoCameraModels()[0].localTransform().prettyPrint().c_str())
5019  .arg(data.stereoCameraModels()[0].localTransform().r11()).arg(data.stereoCameraModels()[0].localTransform().r12()).arg(data.stereoCameraModels()[0].localTransform().r13()).arg(data.stereoCameraModels()[0].localTransform().o14())
5020  .arg(data.stereoCameraModels()[0].localTransform().r21()).arg(data.stereoCameraModels()[0].localTransform().r22()).arg(data.stereoCameraModels()[0].localTransform().r23()).arg(data.stereoCameraModels()[0].localTransform().o24())
5021  .arg(data.stereoCameraModels()[0].localTransform().r31()).arg(data.stereoCameraModels()[0].localTransform().r32()).arg(data.stereoCameraModels()[0].localTransform().r33()).arg(data.stereoCameraModels()[0].localTransform().o34()));
5022 
5023  for(unsigned int i=0; i<data.stereoCameraModels().size();++i)
5024  {
5025  calibrationDetails << "Id: " << i << std::endl;
5026  calibrationDetails << " Left:" << " Size=" << data.stereoCameraModels()[i].left().imageWidth() << "x" << data.stereoCameraModels()[i].left().imageHeight() << std::endl;
5027  if( data.stereoCameraModels()[i].left().K_raw().total()) calibrationDetails << " K=" << data.stereoCameraModels()[i].left().K_raw() << std::endl;
5028  if( data.stereoCameraModels()[i].left().D_raw().total()) calibrationDetails << " D=" << data.stereoCameraModels()[i].left().D_raw() << std::endl;
5029  if( data.stereoCameraModels()[i].left().R().total()) calibrationDetails << " R=" << data.stereoCameraModels()[i].left().R() << std::endl;
5030  if( data.stereoCameraModels()[i].left().P().total()) calibrationDetails << " P=" << data.stereoCameraModels()[i].left().P() << std::endl;
5031  calibrationDetails << " Right:" << " Size=" << data.stereoCameraModels()[i].right().imageWidth() << "x" << data.stereoCameraModels()[i].right().imageHeight() << std::endl;
5032  if( data.stereoCameraModels()[i].right().K_raw().total()) calibrationDetails << " K=" << data.stereoCameraModels()[i].right().K_raw() << std::endl;
5033  if( data.stereoCameraModels()[i].right().D_raw().total()) calibrationDetails << " D=" << data.stereoCameraModels()[i].right().D_raw() << std::endl;
5034  if( data.stereoCameraModels()[i].right().R().total()) calibrationDetails << " R=" << data.stereoCameraModels()[i].right().R() << std::endl;
5035  if( data.stereoCameraModels()[i].right().P().total()) calibrationDetails << " P=" << data.stereoCameraModels()[i].right().P() << std::endl;
5036  if( data.stereoCameraModels()[i].R().total()) calibrationDetails << " R=" << data.stereoCameraModels()[i].R() << std::endl;
5037  if( data.stereoCameraModels()[i].T().total()) calibrationDetails << " T=" << data.stereoCameraModels()[i].T() << std::endl;
5038  if( data.stereoCameraModels()[i].F().total()) calibrationDetails << " F=" << data.stereoCameraModels()[i].F() << std::endl;
5039  if( data.stereoCameraModels()[i].E().total()) calibrationDetails << " E=" << data.stereoCameraModels()[i].E() << std::endl;
5040  calibrationDetails << "BaseToLeftCam(without opt rot)=" << (data.stereoCameraModels()[i].left().localTransform()*CameraModel::opticalRotation().inverse()).prettyPrint() << std::endl;
5041  }
5042  }
5043  labelCalib->setToolTip(calibrationDetails.str().c_str());
5044 
5045  }
5046  else
5047  {
5048  labelCalib->setText("NA");
5049  }
5050 
5051  if(data.laserScanRaw().size())
5052  {
5053  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")
5054  .arg(data.laserScanRaw().formatName().c_str())
5055  .arg(data.laserScanRaw().size())
5056  .arg(data.laserScanRaw().maxPoints())
5057  .arg(data.laserScanRaw().rangeMin())
5058  .arg(data.laserScanRaw().rangeMax())
5059  .arg(data.laserScanRaw().angleMin())
5060  .arg(data.laserScanRaw().angleMax())
5061  .arg(data.laserScanRaw().angleIncrement())
5062  .arg(data.laserScanRaw().hasRGB()?1:0)
5063  .arg(data.laserScanRaw().is2d()?1:0)
5064  .arg(data.laserScanRaw().hasNormals()?1:0)
5065  .arg(data.laserScanRaw().hasIntensity()?1:0)
5066  .arg(data.laserScanRaw().localTransform().prettyPrint().c_str()));
5067  }
5068 
5069  //stereo
5070  if(!data.depthOrRightRaw().empty() && data.depthOrRightRaw().type() == CV_8UC1)
5071  {
5072  this->updateStereo(&data);
5073  }
5074  else
5075  {
5076  stereoViewer_->clear();
5077  ui_->graphicsView_stereo->clear();
5078  }
5079 
5080  // 3d view
5081  if(cloudViewer_->isVisible())
5082  {
5083  cloudViewer_->removeAllLines();
5084  cloudViewer_->removeAllFrustums();
5085  cloudViewer_->removeOccupancyGridMap();
5086  cloudViewer_->removeAllClouds();
5087  cloudViewer_->removeOctomap();
5088  cloudViewer_->removeElevationMap();
5089 
5090  Transform pose = Transform::getIdentity();
5091  if(signatures.size() && ui_->checkBox_odomFrame_3dview->isChecked())
5092  {
5093  float x, y, z, roll, pitch, yaw;
5094  (*signatures.begin())->getPose().getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
5095  pose = Transform(0,0,z,roll,pitch,0);
5096  }
5097 
5098  if(!gravityLink.empty() && ui_->checkBox_gravity_3dview->isChecked())
5099  {
5100  Transform gravityT = gravityLink.begin()->second.transform();
5101  Eigen::Vector3f gravity(0,0,-1);
5102  if(pose.isIdentity())
5103  {
5104  gravityT = gravityT.inverse();
5105  }
5106  gravity = (gravityT.rotation()*(pose).rotation().inverse()).toEigen3f()*gravity;
5107  cloudViewer_->addOrUpdateLine("gravity", pose, (pose).translation()*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0)*pose.rotation().inverse(), Qt::yellow, true, false);
5108  }
5109 
5110  //add scan
5111  LaserScan laserScanRaw = data.laserScanRaw();
5112  if(modifiedLaserScans_.find(id)!=modifiedLaserScans_.end())
5113  {
5114  laserScanRaw = modifiedLaserScans_.at(id);
5115  }
5116  if(ui_->checkBox_showScan->isChecked() && laserScanRaw.size())
5117  {
5118  if(laserScanRaw.hasRGB() && laserScanRaw.hasNormals())
5119  {
5120  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr scan = util3d::laserScanToPointCloudRGBNormal(laserScanRaw, laserScanRaw.localTransform());
5121  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
5122  }
5123  else if(laserScanRaw.hasIntensity() && laserScanRaw.hasNormals())
5124  {
5125  pcl::PointCloud<pcl::PointXYZINormal>::Ptr scan = util3d::laserScanToPointCloudINormal(laserScanRaw, laserScanRaw.localTransform());
5126  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
5127  }
5128  else if(laserScanRaw.hasNormals())
5129  {
5130  pcl::PointCloud<pcl::PointNormal>::Ptr scan = util3d::laserScanToPointCloudNormal(laserScanRaw, laserScanRaw.localTransform());
5131  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
5132  }
5133  else if(laserScanRaw.hasRGB())
5134  {
5135  pcl::PointCloud<pcl::PointXYZRGB>::Ptr scan = util3d::laserScanToPointCloudRGB(laserScanRaw, laserScanRaw.localTransform());
5136  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
5137  }
5138  else if(laserScanRaw.hasIntensity())
5139  {
5140  pcl::PointCloud<pcl::PointXYZI>::Ptr scan = util3d::laserScanToPointCloudI(laserScanRaw, laserScanRaw.localTransform());
5141  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
5142  }
5143  else
5144  {
5145  pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(laserScanRaw, laserScanRaw.localTransform());
5146  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
5147  }
5148  }
5149 
5150  // add RGB-D cloud
5151  if(ui_->checkBox_showCloud->isChecked() && ui_->checkBox_cameraProjection->isChecked() &&
5152  !data.imageRaw().empty() && !laserScanRaw.empty() && !laserScanRaw.is2d())
5153  {
5154  pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud = util3d::laserScanToPointCloudINormal(laserScanRaw, laserScanRaw.localTransform());
5155  std::vector<CameraModel> models = data.cameraModels();
5156  if(!data.stereoCameraModels().empty())
5157  {
5158  models.clear();
5159  for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
5160  {
5161  models.push_back(data.stereoCameraModels()[i].left());
5162  }
5163  }
5164 
5165  if(!models.empty() && !models[0].isValidForProjection())
5166  {
5167  models.clear();
5168  }
5169 
5170  if(!models.empty() && models[0].imageWidth() != 0 && models[0].imageHeight() != 0)
5171  {
5172  std::map<int, Transform> cameraPoses;
5173  std::map<int, std::vector<CameraModel> > cameraModels;
5174  cameraPoses.insert(std::make_pair(data.id(), Transform::getIdentity()));
5175  cameraModels.insert(std::make_pair(data.id(), models));
5176 
5177  std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;
5178  pointToPixel = util3d::projectCloudToCameras(
5179  *cloud,
5180  cameraPoses,
5181  cameraModels);
5182  // color the cloud
5183  UASSERT(pointToPixel.empty() || pointToPixel.size() == cloud->size());
5184  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudValidPoints(new pcl::PointCloud<pcl::PointXYZRGB>);
5185  cloudValidPoints->resize(cloud->size());
5186  int oi=0;
5187  for(size_t i=0; i<pointToPixel.size(); ++i)
5188  {
5189  pcl::PointXYZINormal & pt = cloud->at(i);
5190  pcl::PointXYZRGB ptColor;
5191  int nodeID = pointToPixel[i].first.first;
5192  int cameraIndex = pointToPixel[i].first.second;
5193  if(nodeID>0 && cameraIndex>=0)
5194  {
5195  cv::Mat image = data.imageRaw();
5196 
5197  int subImageWidth = image.cols / cameraModels.at(nodeID).size();
5198  image = image(cv::Range::all(), cv::Range(cameraIndex*subImageWidth, (cameraIndex+1)*subImageWidth));
5199 
5200 
5201  int x = pointToPixel[i].second.x * (float)image.cols;
5202  int y = pointToPixel[i].second.y * (float)image.rows;
5203  UASSERT(x>=0 && x<image.cols);
5204  UASSERT(y>=0 && y<image.rows);
5205 
5206  if(image.type()==CV_8UC3)
5207  {
5208  cv::Vec3b bgr = image.at<cv::Vec3b>(y, x);
5209  ptColor.b = bgr[0];
5210  ptColor.g = bgr[1];
5211  ptColor.r = bgr[2];
5212  }
5213  else
5214  {
5215  UASSERT(image.type()==CV_8UC1);
5216  ptColor.r = ptColor.g = ptColor.b = image.at<unsigned char>(pointToPixel[i].second.y * image.rows, pointToPixel[i].second.x * image.cols);
5217  }
5218 
5219  ptColor.x = pt.x;
5220  ptColor.y = pt.y;
5221  ptColor.z = pt.z;
5222  cloudValidPoints->at(oi++) = ptColor;
5223  }
5224  }
5225  cloudValidPoints->resize(oi);
5226  if(!cloudValidPoints->empty())
5227  {
5228  if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
5229  {
5230  cloudValidPoints = util3d::voxelize(cloudValidPoints, ui_->doubleSpinBox_voxelSize->value());
5231  }
5232 
5233  cloudViewer_->addCloud("cloud", cloudValidPoints, pose);
5234  }
5235  else
5236  {
5237  UWARN("Camera projection to scan returned an empty cloud, no visible points from cameras...");
5238  }
5239  }
5240  else
5241  {
5242  UERROR("Node has invalid camera models, camera projection on scan cannot be done!.");
5243  }
5244  }
5245  else if(ui_->checkBox_showCloud->isChecked() || ui_->checkBox_showMesh->isChecked())
5246  {
5247  if(!data.depthOrRightRaw().empty())
5248  {
5249  if(!data.imageRaw().empty())
5250  {
5251  std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
5252  std::vector<pcl::IndicesPtr> allIndices;
5253  if(!data.depthRaw().empty() && data.cameraModels().size()==1)
5254  {
5255  cv::Mat depth = data.depthRaw();
5256  if(ui_->spinBox_mesh_fillDepthHoles->value() > 0)
5257  {
5258  depth = util2d::fillDepthHoles(depth, ui_->spinBox_mesh_fillDepthHoles->value(), float(ui_->spinBox_mesh_depthError->value())/100.0f);
5259  }
5260  pcl::IndicesPtr indices(new std::vector<int>);
5261  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
5262  data.imageRaw(),
5263  depth,
5264  data.cameraModels()[0],
5265  ui_->spinBox_decimation->value(),0,0,indices.get());
5266  if(indices->size())
5267  {
5268  clouds.push_back(util3d::transformPointCloud(cloud, data.cameraModels()[0].localTransform()));
5269  allIndices.push_back(indices);
5270  }
5271  }
5272  else
5273  {
5274  clouds = util3d::cloudsRGBFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, &allIndices, ui_->parameters_toolbox->getParameters());
5275  }
5276  UASSERT(clouds.size() == allIndices.size());
5277  for(size_t i=0; i<allIndices.size(); ++i)
5278  {
5279  if(allIndices[i]->size())
5280  {
5281  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = clouds[i];
5282  pcl::IndicesPtr indices = allIndices[i];
5283  if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
5284  {
5285  cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value());
5286  }
5287 
5288  if(ui_->checkBox_showMesh->isChecked() && !cloud->is_dense)
5289  {
5290  Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
5291  if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
5292  {
5293  viewpoint[0] = data.cameraModels()[0].localTransform().x();
5294  viewpoint[1] = data.cameraModels()[0].localTransform().y();
5295  viewpoint[2] = data.cameraModels()[0].localTransform().z();
5296  }
5297  else if(data.stereoCameraModels().size() && !data.stereoCameraModels()[0].localTransform().isNull())
5298  {
5299  viewpoint[0] = data.stereoCameraModels()[0].localTransform().x();
5300  viewpoint[1] = data.stereoCameraModels()[0].localTransform().y();
5301  viewpoint[2] = data.stereoCameraModels()[0].localTransform().z();
5302  }
5303  std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
5304  cloud,
5305  float(ui_->spinBox_mesh_angleTolerance->value())*M_PI/180.0f,
5306  ui_->checkBox_mesh_quad->isChecked(),
5307  ui_->spinBox_mesh_triangleSize->value(),
5308  viewpoint);
5309 
5310  if(ui_->spinBox_mesh_minClusterSize->value())
5311  {
5312  // filter polygons
5313  std::vector<std::set<int> > neighbors;
5314  std::vector<std::set<int> > vertexToPolygons;
5315  util3d::createPolygonIndexes(polygons,
5316  cloud->size(),
5317  neighbors,
5318  vertexToPolygons);
5319  std::list<std::list<int> > clusters = util3d::clusterPolygons(
5320  neighbors,
5321  ui_->spinBox_mesh_minClusterSize->value());
5322  std::vector<pcl::Vertices> filteredPolygons(polygons.size());
5323  int oi=0;
5324  for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
5325  {
5326  for(std::list<int>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
5327  {
5328  filteredPolygons[oi++] = polygons.at(*jter);
5329  }
5330  }
5331  filteredPolygons.resize(oi);
5332  polygons = filteredPolygons;
5333  }
5334 
5335  cloudViewer_->addCloudMesh(uFormat("mesh_%d", i), cloud, polygons, pose);
5336  }
5337  if(ui_->checkBox_showCloud->isChecked())
5338  {
5339  cloudViewer_->addCloud(uFormat("cloud_%d", i), cloud, pose);
5340  }
5341  }
5342  }
5343  }
5344  else if(ui_->checkBox_showCloud->isChecked())
5345  {
5346  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds;
5347  std::vector<pcl::IndicesPtr> allIndices;
5348 
5349  clouds = util3d::cloudsFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, &allIndices, ui_->parameters_toolbox->getParameters());
5350  UASSERT(clouds.size() == allIndices.size());
5351  for(size_t i=0; i<allIndices.size(); ++i)
5352  {
5353  if(allIndices[i]->size())
5354  {
5355  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = clouds[i];
5356  pcl::IndicesPtr indices = allIndices[i];
5357  if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
5358  {
5359  cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value());
5360  }
5361 
5362  cloudViewer_->addCloud(uFormat("cloud_%d", i), cloud, pose);
5363  }
5364  }
5365  }
5366  }
5367  }
5368 
5369  //frustums
5370  if(cloudViewer_->isFrustumShown())
5371  {
5372  if(data.cameraModels().size())
5373  {
5374  cloudViewer_->updateCameraFrustums(pose, data.cameraModels());
5375  }
5376  else
5377  {
5378  cloudViewer_->updateCameraFrustums(pose, data.stereoCameraModels());
5379  }
5380  }
5381 
5382  //words
5383  if(ui_->checkBox_showWords->isChecked() &&
5384  !signatures.empty() &&
5385  !(*signatures.begin())->getWords3().empty())
5386  {
5387  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
5388  cloud->resize((*signatures.begin())->getWords3().size());
5389  int i=0;
5390  for(std::multimap<int, int>::const_iterator iter=(*signatures.begin())->getWords().begin();
5391  iter!=(*signatures.begin())->getWords().end();
5392  ++iter)
5393  {
5394  const cv::Point3f & pt = (*signatures.begin())->getWords3()[iter->second];
5395  cloud->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
5396  }
5397 
5398  if(cloud->size())
5399  {
5400  cloud = rtabmap::util3d::removeNaNFromPointCloud(cloud);
5401  }
5402 
5403  if(cloud->size())
5404  {
5405  cloudViewer_->addCloud("words", cloud, pose, Qt::red);
5406  }
5407  }
5408 
5409  //add occupancy grid
5410  if(ui_->checkBox_showMap->isChecked() ||
5411  ui_->checkBox_showGrid->isChecked() ||
5412  ui_->checkBox_showElevation->checkState() != Qt::Unchecked)
5413  {
5414  LocalGridCache combinedLocalMaps;
5415  if(generatedLocalMaps_.shareTo(data.id(), combinedLocalMaps))
5416  {
5417  // add local grid
5418  }
5419  else if(!data.gridGroundCellsRaw().empty() || !data.gridObstacleCellsRaw().empty())
5420  {
5421  combinedLocalMaps.add(data.id(), data.gridGroundCellsRaw(), data.gridObstacleCellsRaw(), data.gridEmptyCellsRaw(), data.gridCellSize(), data.gridViewPoint());
5422  }
5423  if(!combinedLocalMaps.empty())
5424  {
5425  std::map<int, Transform> poses;
5426  poses.insert(std::make_pair(data.id(), pose));
5427 
5428 #ifdef RTABMAP_OCTOMAP
5429  OctoMap * octomap = 0;
5430  if(ui_->checkBox_octomap->isChecked() &&
5431  (!combinedLocalMaps.localGrids().begin()->second.groundCells.empty() || !combinedLocalMaps.localGrids().begin()->second.obstacleCells.empty()) &&
5432  combinedLocalMaps.localGrids().begin()->second.is3D() &&
5433  combinedLocalMaps.localGrids().begin()->second.cellSize > 0.0f)
5434  {
5435  //create local octomap
5436  ParametersMap params;
5437  params.insert(ParametersPair(Parameters::kGridCellSize(), uNumber2Str(combinedLocalMaps.localGrids().begin()->second.cellSize)));
5438  octomap = new OctoMap(&combinedLocalMaps, params);
5439  octomap->update(poses);
5440  }
5441 #endif
5442  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
5443  if(ui_->checkBox_showMap->isChecked())
5444  {
5445  float xMin=0.0f, yMin=0.0f;
5446  cv::Mat map8S;
5447  float gridCellSize = Parameters::defaultGridCellSize();
5448  Parameters::parse(parameters, Parameters::kGridCellSize(), gridCellSize);
5449  parameters = Parameters::filterParameters(parameters, "GridGlobal", true);
5450 
5451 #ifdef RTABMAP_OCTOMAP
5452  if(octomap)
5453  {
5454  map8S = octomap->createProjectionMap(xMin, yMin, gridCellSize, 0);
5455  }
5456  else
5457 #endif
5458  {
5459  OccupancyGrid grid(&combinedLocalMaps, parameters);
5460  grid.update(poses);
5461  map8S = grid.getMap(xMin, yMin);
5462  }
5463  if(!map8S.empty())
5464  {
5465  //convert to gray scaled map
5466  cloudViewer_->addOccupancyGridMap(util3d::convertMap2Image8U(map8S), gridCellSize, xMin, yMin, 1);
5467  }
5468  }
5469 
5470  if(ui_->checkBox_showGrid->isChecked())
5471  {
5472 #ifdef RTABMAP_OCTOMAP
5473  if(octomap)
5474  {
5475  if(ui_->comboBox_octomap_rendering_type->currentIndex()== 0)
5476  {
5477  pcl::IndicesPtr obstacles(new std::vector<int>);
5478  pcl::IndicesPtr empty(new std::vector<int>);
5479  pcl::IndicesPtr ground(new std::vector<int>);
5480  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(ui_->spinBox_grid_depth->value(), obstacles.get(), empty.get(), ground.get());
5481  if(octomap->hasColor())
5482  {
5483  pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
5484  pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
5485  cloudViewer_->addCloud("obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
5486  cloudViewer_->setCloudPointSize("obstacles", 5);
5487 
5488  pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
5489  pcl::copyPointCloud(*cloud, *ground, *groundCloud);
5490  cloudViewer_->addCloud("ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
5491  cloudViewer_->setCloudPointSize("ground", 5);
5492  }
5493  else
5494  {
5495  pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
5496  pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
5497  cloudViewer_->addCloud("obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
5498  cloudViewer_->setCloudPointSize("obstacles", 5);
5499 
5500  pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
5501  pcl::copyPointCloud(*cloud, *ground, *groundCloud);
5502  cloudViewer_->addCloud("ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
5503  cloudViewer_->setCloudPointSize("ground", 5);
5504  }
5505 
5506  if(ui_->checkBox_grid_empty->isChecked())
5507  {
5508  pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZ>);
5509  pcl::copyPointCloud(*cloud, *empty, *emptyCloud);
5510  cloudViewer_->addCloud("empty_cells", emptyCloud, Transform::getIdentity(), QColor(ui_->lineEdit_emptyColor->text()));
5511  cloudViewer_->setCloudOpacity("empty_cells", 0.5);
5512  cloudViewer_->setCloudPointSize("empty_cells", 5);
5513  }
5514  }
5515  else
5516  {
5517  cloudViewer_->addOctomap(octomap, ui_->spinBox_grid_depth->value(), ui_->comboBox_octomap_rendering_type->currentIndex()>1);
5518  }
5519  }
5520  else
5521 #endif
5522  {
5523  // occupancy cloud
5524  LaserScan scan = LaserScan::backwardCompatibility(combinedLocalMaps.localGrids().begin()->second.groundCells);
5525  if(scan.hasRGB())
5526  {
5527  cloudViewer_->addCloud("ground", util3d::laserScanToPointCloudRGB(scan), pose, QColor(ui_->lineEdit_groundColor->text()));
5528  }
5529  else
5530  {
5531  cloudViewer_->addCloud("ground", util3d::laserScanToPointCloud(scan), pose, QColor(ui_->lineEdit_groundColor->text()));
5532  }
5533  scan = LaserScan::backwardCompatibility(combinedLocalMaps.localGrids().begin()->second.obstacleCells);
5534  if(scan.hasRGB())
5535  {
5536  cloudViewer_->addCloud("obstacles", util3d::laserScanToPointCloudRGB(scan), pose, QColor(ui_->lineEdit_obstacleColor->text()));
5537  }
5538  else
5539  {
5540  cloudViewer_->addCloud("obstacles", util3d::laserScanToPointCloud(scan), pose, QColor(ui_->lineEdit_obstacleColor->text()));
5541  }
5542 
5543  cloudViewer_->setCloudPointSize("ground", 5);
5544  cloudViewer_->setCloudPointSize("obstacles", 5);
5545 
5546  if(ui_->checkBox_grid_empty->isChecked())
5547  {
5548  cloudViewer_->addCloud("empty_cells",
5549  util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(combinedLocalMaps.localGrids().begin()->second.emptyCells)),
5550  pose,
5551  QColor(ui_->lineEdit_emptyColor->text()));
5552  cloudViewer_->setCloudPointSize("empty_cells", 5);
5553  cloudViewer_->setCloudOpacity("empty_cells", 0.5);
5554  }
5555  }
5556  }
5557 #ifdef RTABMAP_OCTOMAP
5558  if(octomap)
5559  {
5560  delete octomap;
5561  }
5562 #endif
5563 
5564 #ifdef RTABMAP_GRIDMAP
5565  if(ui_->checkBox_showElevation->checkState() != Qt::Unchecked) // Show elevation map?
5566  {
5567  GridMap gridMap(&combinedLocalMaps, parameters);
5568 
5569  if(combinedLocalMaps.localGrids().begin()->second.is3D())
5570  {
5571  gridMap.update(poses);
5572  if(ui_->checkBox_showElevation->checkState() == Qt::PartiallyChecked)
5573  {
5574  float xMin, yMin, gridCellSize;
5575  cv::Mat elevationMap = gridMap.createHeightMap(xMin, yMin, gridCellSize);
5576  cloudViewer_->addElevationMap(elevationMap, gridCellSize, xMin, yMin, 1.0f);
5577  }
5578  else
5579  {
5580  pcl::PolygonMesh::Ptr mesh = gridMap.createTerrainMesh();
5581  cloudViewer_->addCloudMesh("elevation_mesh", mesh);
5582  }
5583  cloudViewer_->refreshView();
5584  }
5585  else
5586  {
5587  UWARN("Local grid is not 3D, cannot generate an elevation map");
5588  }
5589  }
5590 #endif
5591  }
5592  }
5593  cloudViewer_->updateCameraTargetPosition(pose);
5594  cloudViewer_->clearTrajectory();
5595  cloudViewer_->refreshView();
5596  }
5597 
5598  if(signatures.size())
5599  {
5600  UASSERT(signatures.front() != 0 && signatures.size() == 1);
5601  delete signatures.front();
5602  signatures.clear();
5603  }
5604  }
5605 
5606  // loops
5607  std::multimap<int, rtabmap::Link> links;
5608  dbDriver_->loadLinks(id, links);
5609  if(links.size())
5610  {
5611  QString strParents, strChildren;
5612  for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
5613  {
5614  if(iter->second.type() != Link::kNeighbor &&
5615  iter->second.type() != Link::kNeighborMerged)
5616  {
5617  if(iter->first < id)
5618  {
5619  strChildren.append(QString("%1 ").arg(iter->first));
5620  }
5621  else
5622  {
5623  strParents.append(QString("%1 ").arg(iter->first));
5624  }
5625  }
5626  }
5627  labelParents->setText(strParents);
5628  labelChildren->setText(strChildren);
5629  }
5630  }
5631 
5632  if(mapId>=0)
5633  {
5634  labelMapId->setText(QString::number(mapId));
5635  }
5636  }
5637  else if(value != 0)
5638  {
5639  ULOGGER_ERROR("Slider index out of range ?");
5640  }
5641 
5642  updateConstraintButtons();
5643  updateWordsMatching();
5644 
5645  if(updateConstraintView && ui_->dockWidget_constraints->isVisible())
5646  {
5647  // update constraint view
5648  int from = ids_.at(ui_->horizontalSlider_A->value());
5649  int to = ids_.at(ui_->horizontalSlider_B->value());
5650  bool set = false;
5651  for(int i=0; i<loopLinks_.size() || i<neighborLinks_.size(); ++i)
5652  {
5653  if(i < loopLinks_.size())
5654  {
5655  if((loopLinks_[i].from() == from && loopLinks_[i].to() == to) ||
5656  (loopLinks_[i].from() == to && loopLinks_[i].to() == from))
5657  {
5658  if(i != ui_->horizontalSlider_loops->value())
5659  {
5660  ui_->horizontalSlider_loops->blockSignals(true);
5661  ui_->horizontalSlider_loops->setValue(i);
5662  ui_->horizontalSlider_loops->blockSignals(false);
5663  this->updateConstraintView(loopLinks_[i].from() == from?loopLinks_.at(i):loopLinks_.at(i).inverse(), false);
5664  }
5665  ui_->horizontalSlider_neighbors->blockSignals(true);
5666  ui_->horizontalSlider_neighbors->setValue(0);
5667  ui_->horizontalSlider_neighbors->blockSignals(false);
5668  set = true;
5669  break;
5670  }
5671  }
5672  if(i < neighborLinks_.size())
5673  {
5674  if((neighborLinks_[i].from() == from && neighborLinks_[i].to() == to) ||
5675  (neighborLinks_[i].from() == to && neighborLinks_[i].to() == from))
5676  {
5677  if(i != ui_->horizontalSlider_neighbors->value())
5678  {
5679  ui_->horizontalSlider_neighbors->blockSignals(true);
5680  ui_->horizontalSlider_neighbors->setValue(i);
5681  ui_->horizontalSlider_neighbors->blockSignals(false);
5682  this->updateConstraintView(neighborLinks_[i].from() == from?neighborLinks_.at(i):neighborLinks_.at(i).inverse(), false);
5683  }
5684  ui_->horizontalSlider_loops->blockSignals(true);
5685  ui_->horizontalSlider_loops->setValue(0);
5686  ui_->horizontalSlider_loops->blockSignals(false);
5687  set = true;
5688  break;
5689  }
5690  }
5691  }
5692  if(!set)
5693  {
5694  ui_->horizontalSlider_loops->blockSignals(true);
5695  ui_->horizontalSlider_neighbors->blockSignals(true);
5696  ui_->horizontalSlider_loops->setValue(0);
5697  ui_->horizontalSlider_neighbors->setValue(0);
5698  ui_->horizontalSlider_loops->blockSignals(false);
5699  ui_->horizontalSlider_neighbors->blockSignals(false);
5700 
5701  constraintsViewer_->removeAllClouds();
5702 
5703  Link link = this->findActiveLink(from, to);
5704  bool constraintViewUpdated = false;
5705  if(link.isValid() && link.type() != Link::kGravity)
5706  {
5707  this->updateConstraintView(link, false);
5708  constraintViewUpdated = true;
5709  }
5710  else if(graphes_.size())
5711  {
5712  // make a fake link using globally optimized poses
5713  std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
5714  if(optimizedPoses.size() > 0)
5715  {
5716  std::map<int, Transform>::iterator fromIter = optimizedPoses.find(from);
5717  std::map<int, Transform>::iterator toIter = optimizedPoses.find(to);
5718  if(fromIter != optimizedPoses.end() &&
5719  toIter != optimizedPoses.end())
5720  {
5721  Link link(from, to, Link::kUndef, fromIter->second.inverse() * toIter->second);
5722  this->updateConstraintView(link, false);
5723  constraintViewUpdated = true;
5724  }
5725  }
5726  }
5727  if(!constraintViewUpdated)
5728  {
5729  ui_->label_constraint->clear();
5730  ui_->label_constraint_opt->clear();
5731  ui_->label_variance->clear();
5732  ui_->lineEdit_covariance->clear();
5733  ui_->label_type->clear();
5734  ui_->label_type_name->clear();
5735  ui_->checkBox_showOptimized->setEnabled(false);
5736  }
5737  constraintsViewer_->refreshView();
5738 
5739  }
5740  }
5741 }
5742 
5743 void DatabaseViewer::updateLoggerLevel()
5744 {
5745  if(this->parent() == 0)
5746  {
5747  ULogger::setLevel((ULogger::Level)ui_->comboBox_logger_level->currentIndex());
5748  }
5749 }
5750 
5751 void DatabaseViewer::updateStereo()
5752 {
5753  if(ui_->horizontalSlider_A->maximum())
5754  {
5755  int id = ids_.at(ui_->horizontalSlider_A->value());
5756  SensorData data;
5757  dbDriver_->getNodeData(id, data);
5758  data.uncompressData();
5759  updateStereo(&data);
5760  }
5761 }
5762 
5763 void DatabaseViewer::updateStereo(const SensorData * data)
5764 {
5765  if(data &&
5766  ui_->dockWidget_stereoView->isVisible() &&
5767  !data->imageRaw().empty() &&
5768  !data->depthOrRightRaw().empty() &&
5769  data->depthOrRightRaw().type() == CV_8UC1 &&
5770  data->stereoCameraModels().size()==1 && // Not implemented for multiple stereo cameras
5771  data->stereoCameraModels()[0].isValidForProjection())
5772  {
5773  cv::Mat leftMono;
5774  if(data->imageRaw().channels() == 3)
5775  {
5776  cv::cvtColor(data->imageRaw(), leftMono, CV_BGR2GRAY);
5777  }
5778  else
5779  {
5780  leftMono = data->imageRaw();
5781  }
5782 
5783  UTimer timer;
5784  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
5785  Stereo * stereo = Stereo::create(parameters);
5786 
5787  // generate kpts
5788  std::vector<cv::KeyPoint> kpts;
5789  uInsert(parameters, ParametersPair(Parameters::kKpMaxFeatures(), parameters.at(Parameters::kVisMaxFeatures())));
5790  uInsert(parameters, ParametersPair(Parameters::kKpSSC(), parameters.at(Parameters::kVisSSC())));
5791  uInsert(parameters, ParametersPair(Parameters::kKpMinDepth(), parameters.at(Parameters::kVisMinDepth())));
5792  uInsert(parameters, ParametersPair(Parameters::kKpMaxDepth(), parameters.at(Parameters::kVisMaxDepth())));
5793  uInsert(parameters, ParametersPair(Parameters::kKpDetectorStrategy(), parameters.at(Parameters::kVisFeatureType())));
5794  uInsert(parameters, ParametersPair(Parameters::kKpRoiRatios(), parameters.at(Parameters::kVisRoiRatios())));
5795  uInsert(parameters, ParametersPair(Parameters::kKpSubPixEps(), parameters.at(Parameters::kVisSubPixEps())));
5796  uInsert(parameters, ParametersPair(Parameters::kKpSubPixIterations(), parameters.at(Parameters::kVisSubPixIterations())));
5797  uInsert(parameters, ParametersPair(Parameters::kKpSubPixWinSize(), parameters.at(Parameters::kVisSubPixWinSize())));
5798  Feature2D * kptDetector = Feature2D::create(parameters);
5799  kpts = kptDetector->generateKeypoints(leftMono);
5800  delete kptDetector;
5801 
5802  float timeKpt = timer.ticks();
5803 
5804  std::vector<cv::Point2f> leftCorners;
5805  cv::KeyPoint::convert(kpts, leftCorners);
5806 
5807  // Find features in the new right image
5808  std::vector<unsigned char> status;
5809  std::vector<cv::Point2f> rightCorners;
5810 
5811  rightCorners = stereo->computeCorrespondences(
5812  leftMono,
5813  data->rightRaw(),
5814  leftCorners,
5815  status);
5816  delete stereo;
5817 
5818  float timeStereo = timer.ticks();
5819 
5820  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
5821  cloud->resize(kpts.size());
5822  float bad_point = std::numeric_limits<float>::quiet_NaN ();
5823  UASSERT(status.size() == kpts.size());
5824  int oi = 0;
5825  int inliers = 0;
5826  int flowOutliers= 0;
5827  int slopeOutliers= 0;
5828  int negativeDisparityOutliers = 0;
5829  for(unsigned int i=0; i<status.size(); ++i)
5830  {
5831  cv::Point3f pt(bad_point, bad_point, bad_point);
5832  if(status[i])
5833  {
5834  float disparity = leftCorners[i].x - rightCorners[i].x;
5835  if(disparity > 0.0f)
5836  {
5837  cv::Point3f tmpPt = util3d::projectDisparityTo3D(
5838  leftCorners[i],
5839  disparity,
5840  data->stereoCameraModels()[0]);
5841 
5842  if(util3d::isFinite(tmpPt))
5843  {
5844  pt = util3d::transformPoint(tmpPt, data->stereoCameraModels()[0].left().localTransform());
5845  status[i] = 100; //blue
5846  ++inliers;
5847  cloud->at(oi++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
5848  }
5849  }
5850  else
5851  {
5852  status[i] = 102; //magenta
5853  ++negativeDisparityOutliers;
5854  }
5855  }
5856  else
5857  {
5858  ++flowOutliers;
5859  }
5860  }
5861  cloud->resize(oi);
5862 
5863  UINFO("correspondences = %d/%d (%f) (time kpt=%fs stereo=%fs)",
5864  (int)cloud->size(), (int)leftCorners.size(), float(cloud->size())/float(leftCorners.size()), timeKpt, timeStereo);
5865 
5866  stereoViewer_->updateCameraTargetPosition(Transform::getIdentity());
5867  stereoViewer_->addCloud("stereo", cloud);
5868  stereoViewer_->refreshView();
5869 
5870  ui_->label_stereo_inliers->setNum(inliers);
5871  ui_->label_stereo_flowOutliers->setNum(flowOutliers);
5872  ui_->label_stereo_slopeOutliers->setNum(slopeOutliers);
5873  ui_->label_stereo_disparityOutliers->setNum(negativeDisparityOutliers);
5874 
5875  std::vector<cv::KeyPoint> rightKpts;
5876  cv::KeyPoint::convert(rightCorners, rightKpts);
5877  std::vector<cv::DMatch> good_matches(kpts.size());
5878  for(unsigned int i=0; i<good_matches.size(); ++i)
5879  {
5880  good_matches[i].trainIdx = i;
5881  good_matches[i].queryIdx = i;
5882  }
5883 
5884 
5885  //
5886  //cv::Mat imageMatches;
5887  //cv::drawMatches( leftMono, kpts, data->getDepthRaw(), rightKpts,
5888  // good_matches, imageMatches, cv::Scalar::all(-1), cv::Scalar::all(-1),
5889  // std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
5890 
5891  //ui_->graphicsView_stereo->setImage(uCvMat2QImage(imageMatches));
5892 
5893  ui_->graphicsView_stereo->clear();
5894  ui_->graphicsView_stereo->setLinesShown(true);
5895  ui_->graphicsView_stereo->setFeaturesShown(false);
5896  ui_->graphicsView_stereo->setImageDepthShown(true);
5897 
5898  ui_->graphicsView_stereo->setImage(uCvMat2QImage(data->imageRaw()));
5899  ui_->graphicsView_stereo->setImageDepth(data->depthOrRightRaw());
5900 
5901  // Draw lines between corresponding features...
5902  for(unsigned int i=0; i<kpts.size(); ++i)
5903  {
5904  if(rightKpts[i].pt.x > 0 && rightKpts[i].pt.y > 0)
5905  {
5906  QColor c = Qt::green;
5907  if(status[i] == 0)
5908  {
5909  c = Qt::red;
5910  }
5911  else if(status[i] == 100)
5912  {
5913  c = Qt::blue;
5914  }
5915  else if(status[i] == 101)
5916  {
5917  c = Qt::yellow;
5918  }
5919  else if(status[i] == 102)
5920  {
5921  c = Qt::magenta;
5922  }
5923  else if(status[i] == 110)
5924  {
5925  c = Qt::cyan;
5926  }
5927  ui_->graphicsView_stereo->addLine(
5928  kpts[i].pt.x,
5929  kpts[i].pt.y,
5930  rightKpts[i].pt.x,
5931  rightKpts[i].pt.y,
5932  c,
5933  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));
5934  }
5935  }
5936  ui_->graphicsView_stereo->update();
5937  }
5938 }
5939 
5940 void DatabaseViewer::updateWordsMatching(const std::vector<int> & inliers)
5941 {
5942  if(ui_->actionConcise_Layout->isChecked()) {
5943  return;
5944  }
5945 
5946  int from = ids_.at(ui_->horizontalSlider_A->value());
5947  int to = ids_.at(ui_->horizontalSlider_B->value());
5948  if(from && to)
5949  {
5950  ui_->graphicsView_A->clearLines();
5951  ui_->graphicsView_A->setFeaturesColor(ui_->graphicsView_A->getDefaultFeatureColor());
5952  ui_->graphicsView_B->clearLines();
5953  ui_->graphicsView_B->setFeaturesColor(ui_->graphicsView_B->getDefaultFeatureColor());
5954 
5955  const QMultiMap<int, KeypointItem*> & wordsA = ui_->graphicsView_A->getFeatures();
5956  const QMultiMap<int, KeypointItem*> & wordsB = ui_->graphicsView_B->getFeatures();
5957  std::set<int> inliersSet(inliers.begin(), inliers.end());
5958  if(wordsA.size() && wordsB.size())
5959  {
5960  QList<int> ids = wordsA.uniqueKeys();
5961  for(int i=0; i<ids.size(); ++i)
5962  {
5963  if(ids[i] > 0 && wordsA.count(ids[i]) == 1 && wordsB.count(ids[i]) == 1)
5964  {
5965  // Add lines
5966  // Draw lines between corresponding features...
5967  float scaleAX = ui_->graphicsView_A->viewScale();
5968  float scaleBX = ui_->graphicsView_B->viewScale();
5969 
5970  float marginAX = (ui_->graphicsView_A->width() - ui_->graphicsView_A->sceneRect().width()*scaleAX)/2.0f;
5971  float marginAY = (ui_->graphicsView_A->height() - ui_->graphicsView_A->sceneRect().height()*scaleAX)/2.0f;
5972  float marginBX = (ui_->graphicsView_B->width() - ui_->graphicsView_B->sceneRect().width()*scaleBX)/2.0f;
5973  float marginBY = (ui_->graphicsView_B->height() - ui_->graphicsView_B->sceneRect().height()*scaleBX)/2.0f;
5974 
5975  float deltaX = 0;
5976  float deltaY = 0;
5977 
5978  if(ui_->actionVertical_Layout->isChecked())
5979  {
5980  deltaY = ui_->graphicsView_A->height();
5981  }
5982  else
5983  {
5984  deltaX = ui_->graphicsView_A->width();
5985  }
5986 
5987  const KeypointItem * kptA = wordsA.value(ids[i]);
5988  const KeypointItem * kptB = wordsB.value(ids[i]);
5989 
5990  QColor cA = ui_->graphicsView_A->getDefaultMatchingLineColor();
5991  QColor cB = ui_->graphicsView_B->getDefaultMatchingLineColor();
5992  if(inliersSet.find(ids[i])!=inliersSet.end())
5993  {
5994  cA = ui_->graphicsView_A->getDefaultMatchingFeatureColor();
5995  cB = ui_->graphicsView_B->getDefaultMatchingFeatureColor();
5996  ui_->graphicsView_A->setFeatureColor(ids[i], ui_->graphicsView_A->getDefaultMatchingFeatureColor());
5997  ui_->graphicsView_B->setFeatureColor(ids[i], ui_->graphicsView_B->getDefaultMatchingFeatureColor());
5998  }
5999  else
6000  {
6001  ui_->graphicsView_A->setFeatureColor(ids[i], ui_->graphicsView_A->getDefaultMatchingLineColor());
6002  ui_->graphicsView_B->setFeatureColor(ids[i], ui_->graphicsView_B->getDefaultMatchingLineColor());
6003  }
6004 
6005  ui_->graphicsView_A->addLine(
6006  kptA->keypoint().pt.x,
6007  kptA->keypoint().pt.y,
6008  (kptB->keypoint().pt.x*scaleBX+marginBX+deltaX-marginAX)/scaleAX,
6009  (kptB->keypoint().pt.y*scaleBX+marginBY+deltaY-marginAY)/scaleAX,
6010  cA);
6011 
6012  ui_->graphicsView_B->addLine(
6013  (kptA->keypoint().pt.x*scaleAX+marginAX-deltaX-marginBX)/scaleBX,
6014  (kptA->keypoint().pt.y*scaleAX+marginAY-deltaY-marginBY)/scaleBX,
6015  kptB->keypoint().pt.x,
6016  kptB->keypoint().pt.y,
6017  cB);
6018  }
6019  }
6020  ui_->graphicsView_A->update();
6021  ui_->graphicsView_B->update();
6022  }
6023  }
6024 }
6025 
6026 void DatabaseViewer::sliderAMoved(int value)
6027 {
6028  ui_->spinBox_indexA->blockSignals(true);
6029  ui_->spinBox_indexA->setValue(value);
6030  ui_->spinBox_indexA->blockSignals(false);
6031  if(value>=0 && value < ids_.size())
6032  {
6033  ui_->label_idA->setText(QString::number(ids_.at(value)));
6034  }
6035  else
6036  {
6037  ULOGGER_ERROR("Slider index out of range ?");
6038  }
6039 }
6040 
6041 void DatabaseViewer::sliderBMoved(int value)
6042 {
6043  ui_->spinBox_indexB->blockSignals(true);
6044  ui_->spinBox_indexB->setValue(value);
6045  ui_->spinBox_indexB->blockSignals(false);
6046  if(value>=0 && value < ids_.size())
6047  {
6048  ui_->label_idB->setText(QString::number(ids_.at(value)));
6049  }
6050  else
6051  {
6052  ULOGGER_ERROR("Slider index out of range ?");
6053  }
6054 }
6055 
6056 void DatabaseViewer::update3dView()
6057 {
6058  if(lastSliderIndexBrowsed_ == ui_->horizontalSlider_B->value())
6059  {
6060  sliderBValueChanged(ui_->horizontalSlider_B->value());
6061  }
6062  else
6063  {
6064  sliderAValueChanged(ui_->horizontalSlider_A->value());
6065  }
6066 }
6067 
6068 void DatabaseViewer::sliderNeighborValueChanged(int value)
6069 {
6070  if(value < neighborLinks_.size())
6071  {
6072  this->updateConstraintView(neighborLinks_.at(value));
6073  }
6074 }
6075 
6076 void DatabaseViewer::sliderLoopValueChanged(int value)
6077 {
6078  if(value < loopLinks_.size())
6079  {
6080  this->updateConstraintView(loopLinks_.at(value));
6081  }
6082 }
6083 
6084 void DatabaseViewer::editConstraint()
6085 {
6086  if(ids_.size())
6087  {
6088  Link link(0,0,Link::kUndef, Transform::getIdentity());
6089  int priorId = sender() == ui_->toolButton_edit_priorA?ids_.at(ui_->horizontalSlider_A->value()):
6090  sender() == ui_->toolButton_edit_priorB?ids_.at(ui_->horizontalSlider_B->value()):0;
6091  if(priorId>0)
6092  {
6093  // Prior
6094  std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
6095  if(graph::findLink(links, priorId, priorId, false, Link::kPosePrior) != links.end())
6096  {
6097  link = graph::findLink(links, priorId, priorId, false, Link::kPosePrior)->second;
6098  }
6099  else if(odomPoses_.find(priorId) != odomPoses_.end())
6100  {
6101  // fallback to odom pose
6102  // set undef to go in "add" branch below
6103  link = Link(priorId, priorId, Link::kUndef, odomPoses_.at(priorId));
6104  }
6105  else
6106  {
6107  QMessageBox::warning(this, tr(""), tr("Node %1 doesn't have odometry pose, cannot add a prior for it!").arg(priorId));
6108  return;
6109  }
6110  }
6111  else if(ui_->label_type->text().toInt() == Link::kLandmark)
6112  {
6113  link = loopLinks_.at(ui_->horizontalSlider_loops->value());
6114  std::multimap<int, Link>::iterator findIter = rtabmap::graph::findLink(linksRefined_, link.from() ,link.to(), false, link.type());
6115  if(findIter != linksRefined_.end())
6116  {
6117  link = findIter->second;
6118  }
6119  }
6120  else
6121  {
6122  link = this->findActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value()));
6123  }
6124  bool updated = false;
6125  if(link.isValid())
6126  {
6127  cv::Mat covBefore = link.infMatrix().inv();
6128  EditConstraintDialog dialog(link.transform(), covBefore);
6129  if(dialog.exec() == QDialog::Accepted)
6130  {
6131  cv::Mat covariance = dialog.getCovariance();
6132  Link newLink(link.from(), link.to(), link.type(), dialog.getTransform(), covariance.inv());
6133  std::multimap<int, Link>::iterator iter = linksRefined_.find(link.from());
6134  while(iter != linksRefined_.end() && iter->first == link.from())
6135  {
6136  if(iter->second.to() == link.to() &&
6137  iter->second.type() == link.type())
6138  {
6139  iter->second = newLink;
6140  updated = true;
6141  break;
6142  }
6143  ++iter;
6144  }
6145  if(!updated)
6146  {
6147  linksRefined_.insert(std::make_pair(newLink.from(), newLink));
6148  updated = true;
6149  }
6150  if(priorId==0)
6151  {
6152  this->updateGraphView();
6153  updateConstraintView();
6154  }
6155  }
6156  }
6157  else
6158  {
6159  EditConstraintDialog dialog(
6160  link.transform(), cv::Mat::eye(6,6,CV_64FC1) * (priorId>0?0.00001:1));
6161  if(dialog.exec() == QDialog::Accepted)
6162  {
6163  cv::Mat covariance = dialog.getCovariance();
6164  int from = priorId>0?priorId:ids_.at(ui_->horizontalSlider_A->value());
6165  int to = priorId>0?priorId:ids_.at(ui_->horizontalSlider_B->value());
6166  Link newLink(
6167  from,
6168  to,
6169  priorId>0?Link::kPosePrior:Link::kUserClosure,
6170  dialog.getTransform(),
6171  covariance.inv());
6172  if(newLink.from() < newLink.to())
6173  {
6174  newLink = newLink.inverse();
6175  }
6176  linksAdded_.insert(std::make_pair(newLink.from(), newLink));
6177  updated = true;
6178  if(priorId==0)
6179  {
6180  this->updateGraphView();
6181  updateLoopClosuresSlider(from, to);
6182  }
6183  }
6184  }
6185 
6186  if(updated && priorId>0)
6187  {
6188  bool priorsIgnored = Parameters::defaultOptimizerPriorsIgnored();
6189  Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kOptimizerPriorsIgnored(), priorsIgnored);
6190  if(priorsIgnored)
6191  {
6192  if(QMessageBox::question(this,
6193  tr("Updating Prior"),
6194  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()),
6195  QMessageBox::Yes | QMessageBox::No,
6196  QMessageBox::Yes) == QMessageBox::Yes)
6197  {
6198  priorsIgnored = false;
6199  ui_->parameters_toolbox->updateParameter(Parameters::kOptimizerPriorsIgnored(), "false");
6200  }
6201  }
6202  int indexA = ui_->horizontalSlider_A->value();
6203  int indexB = ui_->horizontalSlider_B->value();
6204  if(!priorsIgnored)
6205  {
6206  this->updateGraphView();
6207  }
6208  if(ui_->horizontalSlider_A->value() != indexA)
6209  ui_->horizontalSlider_A->setValue(indexA);
6210  else
6211  sliderAValueChanged(indexA);
6212  if(ui_->horizontalSlider_B->value() != indexB)
6213  ui_->horizontalSlider_B->setValue(indexB);
6214  else
6215  sliderBValueChanged(indexB);
6216  }
6217  }
6218 }
6219 
6220 // only called when ui_->checkBox_showOptimized state changed
6221 void DatabaseViewer::updateConstraintView()
6222 {
6223  if(ids_.size())
6224  {
6225  Link link = this->findActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value()));
6226  if(link.isValid())
6227  {
6228  UDEBUG("link %d->%d type=%d", link.from(), link.to(), link.type());
6229  if((link.type() == Link::kNeighbor ||
6230  link.type() == Link::kNeighborMerged) &&
6231  ui_->horizontalSlider_neighbors->value() >= 0 && ui_->horizontalSlider_neighbors->value() < neighborLinks_.size())
6232  {
6233  this->updateConstraintView(neighborLinks_.at(ui_->horizontalSlider_neighbors->value()), false);
6234  }
6235  else if((link.type() != Link::kPosePrior || link.type() != Link::kGravity) &&
6236  ui_->horizontalSlider_loops->value() >= 0 && ui_->horizontalSlider_loops->value() < loopLinks_.size())
6237  {
6238  this->updateConstraintView(loopLinks_.at(ui_->horizontalSlider_loops->value()), false);
6239  }
6240  else
6241  {
6242  this->updateConstraintView(link, false);
6243  }
6244  }
6245  }
6246 }
6247 
6248 void DatabaseViewer::updateConstraintView(
6249  const rtabmap::Link & linkIn,
6250  bool updateImageSliders,
6251  const Signature & signatureFrom,
6252  const Signature & signatureTo)
6253 {
6254  UDEBUG("%d -> %d", linkIn.from(), linkIn.to());
6255  std::multimap<int, Link>::iterator iterLink = rtabmap::graph::findLink(linksRefined_, linkIn.from(), linkIn.to());
6256  rtabmap::Link link = linkIn;
6257 
6258  if(iterLink != linksRefined_.end())
6259  {
6260  if(iterLink->second.from() == link.to())
6261  {
6262  link = iterLink->second.inverse();
6263  }
6264  else
6265  {
6266  link = iterLink->second;
6267  }
6268  }
6269  else if(ui_->checkBox_ignorePoseCorrection->isChecked())
6270  {
6271  if(link.type() == Link::kNeighbor ||
6272  link.type() == Link::kNeighborMerged)
6273  {
6274  Transform poseFrom = uValue(odomPoses_, link.from(), Transform());
6275  Transform poseTo = uValue(odomPoses_, link.to(), Transform());
6276  if(!poseFrom.isNull() && !poseTo.isNull())
6277  {
6278  // recompute raw odom transformation and
6279  // reset to identity covariance
6280  link = Link(link.from(),
6281  link.to(),
6282  link.type(),
6283  poseFrom.inverse() * poseTo);
6284  }
6285  }
6286  }
6287  UDEBUG("%d -> %d", link.from(), link.to());
6288  rtabmap::Transform t = link.transform();
6289  if(link.type() == Link::kGravity)
6290  {
6291  // remove yaw, keep only roll and pitch
6292  float roll, pitch, yaw;
6293  t.getEulerAngles(roll, pitch, yaw);
6294  t = Transform(0,0,0,roll,pitch,0);
6295  }
6296 
6297  ui_->label_constraint->clear();
6298  ui_->label_constraint_opt->clear();
6299  ui_->checkBox_showOptimized->setEnabled(false);
6300  UASSERT(!t.isNull() && dbDriver_);
6301 
6302  ui_->label_type->setText(QString::number(link.type()));
6303  ui_->label_type_name->setText(tr("(%1)")
6304  .arg(link.type()==Link::kNeighbor?"Neighbor":
6305  link.type()==Link::kNeighborMerged?"Merged neighbor":
6306  link.type()==Link::kGlobalClosure?"Loop closure":
6307  link.type()==Link::kLocalSpaceClosure?"Space proximity link":
6308  link.type()==Link::kLocalTimeClosure?"Time proximity link":
6309  link.type()==Link::kUserClosure?"User link":
6310  link.type()==Link::kLandmark?"Landmark "+QString::number(-link.to()):
6311  link.type()==Link::kVirtualClosure?"Virtual link":
6312  link.type()==Link::kGravity?"Gravity link":"Undefined"));
6313  ui_->label_variance->setText(QString("%1, %2")
6314  .arg(sqrt(link.transVariance()))
6315  .arg(sqrt(link.rotVariance())));
6316  std::stringstream out;
6317  out << link.infMatrix().inv();
6318  ui_->lineEdit_covariance->setText(out.str().c_str());
6319  ui_->label_constraint->setText(QString("%1").arg(t.prettyPrint().c_str()).replace(" ", "\n"));
6320  if(graphes_.size() &&
6321  (int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
6322  {
6323  std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
6324 
6325  std::map<int, rtabmap::Transform>::iterator iterFrom = graph.find(link.from());
6326  std::map<int, rtabmap::Transform>::iterator iterTo = graph.find(link.to());
6327  if(iterFrom != graph.end() && iterTo != graph.end())
6328  {
6329  ui_->checkBox_showOptimized->setEnabled(true);
6330  Transform topt = iterFrom->second.inverse()*iterTo->second;
6331  float diff = topt.getDistance(t);
6332  Transform v1 = t.rotation()*Transform(1,0,0,0,0,0);
6333  Transform v2 = topt.rotation()*Transform(1,0,0,0,0,0);
6334  float a = pcl::getAngle3D(Eigen::Vector4f(v1.x(), v1.y(), v1.z(), 0), Eigen::Vector4f(v2.x(), v2.y(), v2.z(), 0));
6335  a = (a *180.0f) / CV_PI;
6336  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));
6337 
6338  if(ui_->checkBox_showOptimized->isChecked())
6339  {
6340  t = topt;
6341  }
6342  }
6343  }
6344 
6345  if(updateImageSliders)
6346  {
6347  ui_->horizontalSlider_A->blockSignals(true);
6348  ui_->horizontalSlider_B->blockSignals(true);
6349  // set from on left and to on right
6350  if(link.from()>0)
6351  ui_->horizontalSlider_A->setValue(idToIndex_.value(link.from()));
6352  if(link.to() > 0)
6353  ui_->horizontalSlider_B->setValue(idToIndex_.value(link.to()));
6354  ui_->horizontalSlider_A->blockSignals(false);
6355  ui_->horizontalSlider_B->blockSignals(false);
6356  if(link.from()>0)
6357  this->update(idToIndex_.value(link.from()),
6358  ui_->spinBox_indexA,
6359  ui_->label_parentsA,
6360  ui_->label_childrenA,
6361  ui_->label_weightA,
6362  ui_->label_labelA,
6363  ui_->label_stampA,
6364  ui_->graphicsView_A,
6365  ui_->label_idA,
6366  ui_->label_mapA,
6367  ui_->label_poseA,
6368  ui_->label_optposeA,
6369  ui_->label_velA,
6370  ui_->label_calibA,
6371  ui_->label_scanA,
6372  ui_->label_gravityA,
6373  ui_->label_priorA,
6374  ui_->toolButton_edit_priorA,
6375  ui_->toolButton_remove_priorA,
6376  ui_->label_gpsA,
6377  ui_->label_gtA,
6378  ui_->label_sensorsA,
6379  false); // don't update constraints view!
6380  if(link.to()>0)
6381  {
6382  this->update(idToIndex_.value(link.to()),
6383  ui_->spinBox_indexB,
6384  ui_->label_parentsB,
6385  ui_->label_childrenB,
6386  ui_->label_weightB,
6387  ui_->label_labelB,
6388  ui_->label_stampB,
6389  ui_->graphicsView_B,
6390  ui_->label_idB,
6391  ui_->label_mapB,
6392  ui_->label_poseB,
6393  ui_->label_optposeB,
6394  ui_->label_velB,
6395  ui_->label_calibB,
6396  ui_->label_scanB,
6397  ui_->label_gravityB,
6398  ui_->label_priorB,
6399  ui_->toolButton_edit_priorB,
6400  ui_->toolButton_remove_priorB,
6401  ui_->label_gpsB,
6402  ui_->label_gtB,
6403  ui_->label_sensorsB,
6404  false); // don't update constraints view!
6405  }
6406  }
6407 
6408  if(constraintsViewer_->isVisible())
6409  {
6410  SensorData dataFrom, dataTo;
6411 
6412  if(signatureFrom.id()>0)
6413  {
6414  dataFrom = signatureFrom.sensorData();
6415  }
6416  else
6417  {
6418  dbDriver_->getNodeData(link.from(), dataFrom);
6419  }
6420  dataFrom.uncompressData();
6421  UASSERT(dataFrom.imageRaw().empty() || dataFrom.imageRaw().type()==CV_8UC3 || dataFrom.imageRaw().type() == CV_8UC1);
6422  UASSERT(dataFrom.depthOrRightRaw().empty() || dataFrom.depthOrRightRaw().type()==CV_8UC1 || dataFrom.depthOrRightRaw().type() == CV_16UC1 || dataFrom.depthOrRightRaw().type() == CV_32FC1);
6423 
6424  if(signatureTo.id()>0)
6425  {
6426  dataTo = signatureTo.sensorData();
6427  }
6428  else if(link.to()>0)
6429  {
6430  dbDriver_->getNodeData(link.to(), dataTo);
6431  }
6432  dataTo.uncompressData();
6433  UASSERT(dataTo.imageRaw().empty() || dataTo.imageRaw().type()==CV_8UC3 || dataTo.imageRaw().type() == CV_8UC1);
6434  UASSERT(dataTo.depthOrRightRaw().empty() || dataTo.depthOrRightRaw().type()==CV_8UC1 || dataTo.depthOrRightRaw().type() == CV_16UC1 || dataTo.depthOrRightRaw().type() == CV_32FC1);
6435 
6436  // get odom pose
6437  Transform pose = Transform::getIdentity();
6438  if(ui_->checkBox_odomFrame->isChecked())
6439  {
6440  int m,w;
6441  std::string l;
6442  double s;
6443  Transform p,g;
6444  std::vector<float> v;
6445  GPS gps;
6446  EnvSensors sensors;
6447  dbDriver_->getNodeInfo(link.from(), p, m, w, l, s, g, v, gps, sensors);
6448  if(!p.isNull())
6449  {
6450  // keep just the z and roll/pitch rotation
6451  float x, y, z, roll, pitch, yaw;
6452  p.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
6453  pose = Transform(0,0,z,roll,pitch,0);
6454  }
6455  }
6456 
6457  constraintsViewer_->removeCloud("cloud0");
6458  constraintsViewer_->removeCloud("cloud1");
6459  //cloud 3d
6460  if(ui_->checkBox_show3Dclouds->isChecked())
6461  {
6462  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom, cloudTo;
6463  pcl::IndicesPtr indicesFrom(new std::vector<int>);
6464  pcl::IndicesPtr indicesTo(new std::vector<int>);
6465  if(!dataFrom.imageRaw().empty() && !dataFrom.depthOrRightRaw().empty())
6466  {
6467  cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, ui_->spinBox_decimation->value(), 0, 0, indicesFrom.get(), ui_->parameters_toolbox->getParameters());
6468  }
6469  if(!dataTo.imageRaw().empty() && !dataTo.depthOrRightRaw().empty())
6470  {
6471  cloudTo=util3d::cloudRGBFromSensorData(dataTo, ui_->spinBox_decimation->value(), 0, 0, indicesTo.get(), ui_->parameters_toolbox->getParameters());
6472  }
6473 
6474  if(cloudTo.get() && indicesTo->size())
6475  {
6476  cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t);
6477  }
6478 
6479  // Gain compensation
6480  if(ui_->doubleSpinBox_gainCompensationRadius->value()>0.0 &&
6481  cloudFrom.get() && indicesFrom->size() &&
6482  cloudTo.get() && indicesTo->size())
6483  {
6484  UTimer t;
6485  GainCompensator compensator(ui_->doubleSpinBox_gainCompensationRadius->value());
6486  compensator.feed(cloudFrom, indicesFrom, cloudTo, indicesTo, Transform::getIdentity());
6487  compensator.apply(0, cloudFrom, indicesFrom);
6488  compensator.apply(1, cloudTo, indicesTo);
6489  UINFO("Gain compensation time = %fs", t.ticks());
6490  }
6491 
6492  if(cloudFrom.get() && indicesFrom->size())
6493  {
6494  if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
6495  {
6496  cloudFrom = util3d::voxelize(cloudFrom, indicesFrom, ui_->doubleSpinBox_voxelSize->value());
6497  }
6498  constraintsViewer_->addCloud("cloud0", cloudFrom, pose, Qt::red);
6499  }
6500  if(cloudTo.get() && indicesTo->size())
6501  {
6502  if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
6503  {
6504  cloudTo = util3d::voxelize(cloudTo, indicesTo, ui_->doubleSpinBox_voxelSize->value());
6505  }
6506  constraintsViewer_->addCloud("cloud1", cloudTo, pose, Qt::cyan);
6507  }
6508  }
6509 
6510  constraintsViewer_->removeCloud("words0");
6511  constraintsViewer_->removeCloud("words1");
6512  if(ui_->checkBox_show3DWords->isChecked())
6513  {
6514  std::list<int> ids;
6515  ids.push_back(link.from());
6516  if(link.to()>0)
6517  {
6518  ids.push_back(link.to());
6519  }
6520  std::list<Signature*> signatures;
6521  dbDriver_->loadSignatures(ids, signatures);
6522  if(signatures.size() == 2 || (link.to()<0 && signatures.size()==1))
6523  {
6524  const Signature * sFrom = signatureFrom.id()>0?&signatureFrom:signatures.front();
6525  const Signature * sTo = 0;
6526  if(signatures.size()==2)
6527  {
6528  sTo = signatureTo.id()>0?&signatureTo:signatures.back();
6529  UASSERT(sTo);
6530  }
6531  UASSERT(sFrom);
6532  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom(new pcl::PointCloud<pcl::PointXYZ>);
6533  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo(new pcl::PointCloud<pcl::PointXYZ>);
6534  cloudFrom->resize(sFrom->getWords3().size());
6535  if(sTo)
6536  {
6537  cloudTo->resize(sTo->getWords3().size());
6538  }
6539  int i=0;
6540  if(!sFrom->getWords3().empty())
6541  {
6542  for(std::multimap<int, int>::const_iterator iter=sFrom->getWords().begin();
6543  iter!=sFrom->getWords().end();
6544  ++iter)
6545  {
6546  const cv::Point3f & pt = sFrom->getWords3()[iter->second];
6547  cloudFrom->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
6548  }
6549  }
6550  i=0;
6551  if(sTo && !sTo->getWords3().empty())
6552  {
6553  for(std::multimap<int, int>::const_iterator iter=sTo->getWords().begin();
6554  iter!=sTo->getWords().end();
6555  ++iter)
6556  {
6557  const cv::Point3f & pt = sTo->getWords3()[iter->second];
6558  cloudTo->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
6559  }
6560  }
6561 
6562  if(cloudFrom->size())
6563  {
6564  cloudFrom = rtabmap::util3d::removeNaNFromPointCloud(cloudFrom);
6565  }
6566  if(cloudTo->size())
6567  {
6568  cloudTo = rtabmap::util3d::removeNaNFromPointCloud(cloudTo);
6569  if(cloudTo->size())
6570  {
6571  cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t);
6572  }
6573  }
6574 
6575  if(cloudFrom->size())
6576  {
6577  constraintsViewer_->addCloud("words0", cloudFrom, pose, Qt::red);
6578  }
6579  else
6580  {
6581  UWARN("Empty 3D words for node %d", link.from());
6582  constraintsViewer_->removeCloud("words0");
6583  }
6584  if(cloudTo->size())
6585  {
6586  constraintsViewer_->addCloud("words1", cloudTo, pose, Qt::cyan);
6587  }
6588  else
6589  {
6590  if(sTo)
6591  {
6592  UWARN("Empty 3D words for node %d", link.to());
6593  }
6594  constraintsViewer_->removeCloud("words1");
6595  }
6596  }
6597  else
6598  {
6599  UERROR("Not found signature %d or %d in RAM", link.from(), link.to());
6600  constraintsViewer_->removeCloud("words0");
6601  constraintsViewer_->removeCloud("words1");
6602  }
6603  //cleanup
6604  for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
6605  {
6606  delete *iter;
6607  }
6608  }
6609 
6610  constraintsViewer_->removeCloud("scan2");
6611  constraintsViewer_->removeGraph("scan2graph");
6612  constraintsViewer_->removeCloud("scan0");
6613  constraintsViewer_->removeCloud("scan1");
6614  if(ui_->checkBox_show2DScans->isChecked())
6615  {
6616  //cloud 2d
6617  if(link.type() == Link::kLocalSpaceClosure &&
6618  !link.userDataCompressed().empty() &&
6619  signatureTo.id()==0)
6620  {
6621  std::vector<int> ids;
6622  cv::Mat userData = link.uncompressUserDataConst();
6623  if(userData.type() == CV_8SC1 &&
6624  userData.rows == 1 &&
6625  userData.cols >= 8 && // including null str ending
6626  userData.at<char>(userData.cols-1) == 0 &&
6627  memcmp(userData.data, "SCANS:", 6) == 0)
6628  {
6629  std::string scansStr = (const char *)userData.data;
6630  UINFO("Detected \"%s\" in links's user data", scansStr.c_str());
6631  if(!scansStr.empty())
6632  {
6633  std::list<std::string> strs = uSplit(scansStr, ':');
6634  if(strs.size() == 2)
6635  {
6636  std::list<std::string> strIds = uSplit(strs.rbegin()->c_str(), ';');
6637  for(std::list<std::string>::iterator iter=strIds.begin(); iter!=strIds.end(); ++iter)
6638  {
6639  ids.push_back(atoi(iter->c_str()));
6640  if(ids.back() == link.from())
6641  {
6642  ids.pop_back();
6643  }
6644  }
6645  }
6646  }
6647  }
6648  if(ids.size())
6649  {
6650  //add other scans matching
6651  //optimize the path's poses locally
6652 
6653  std::map<int, rtabmap::Transform> poses;
6654  for(unsigned int i=0; i<ids.size(); ++i)
6655  {
6656  if(uContains(odomPoses_, ids[i]))
6657  {
6658  poses.insert(*odomPoses_.find(ids[i]));
6659  }
6660  else
6661  {
6662  UERROR("Not found %d node!", ids[i]);
6663  }
6664  }
6665  if(poses.size())
6666  {
6667  Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
6668 
6669  UASSERT(uContains(poses, link.to()));
6670  std::map<int, rtabmap::Transform> posesOut;
6671  std::multimap<int, rtabmap::Link> linksOut;
6672  optimizer->getConnectedGraph(
6673  link.to(),
6674  poses,
6675  updateLinksWithModifications(links_),
6676  posesOut,
6677  linksOut);
6678 
6679  if(poses.size() != posesOut.size())
6680  {
6681  UWARN("Scan poses input and output are different! %d vs %d", (int)poses.size(), (int)posesOut.size());
6682  }
6683 
6684  UDEBUG("Input poses: ");
6685  for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
6686  {
6687  UDEBUG(" %d=%s", iter->first, iter->second.prettyPrint().c_str());
6688  }
6689  UDEBUG("Input links: ");
6690  for(std::multimap<int, Link>::iterator iter=linksOut.begin(); iter!=linksOut.end(); ++iter)
6691  {
6692  UDEBUG(" %d->%d (type=%s) %s", iter->second.from(), iter->second.to(), iter->second.typeName().c_str(), iter->second.transform().prettyPrint().c_str());
6693  }
6694 
6695  std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(link.to(), posesOut, linksOut);
6696  delete optimizer;
6697 
6698  UDEBUG("Output poses: ");
6699  for(std::map<int, Transform>::iterator iter=finalPoses.begin(); iter!=finalPoses.end(); ++iter)
6700  {
6701  UDEBUG(" %d=%s", iter->first, iter->second.prettyPrint().c_str());
6702  }
6703 
6704  // transform local poses in loop referential
6705  Transform u = t * finalPoses.at(link.to()).inverse();
6706  pcl::PointCloud<pcl::PointXYZ>::Ptr assembledScans(new pcl::PointCloud<pcl::PointXYZ>);
6707  pcl::PointCloud<pcl::PointNormal>::Ptr assembledNormalScans(new pcl::PointCloud<pcl::PointNormal>);
6708  pcl::PointCloud<pcl::PointXYZI>::Ptr assembledIScans(new pcl::PointCloud<pcl::PointXYZI>);
6709  pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembledINormalScans(new pcl::PointCloud<pcl::PointXYZINormal>);
6710  pcl::PointCloud<pcl::PointXYZ>::Ptr graph(new pcl::PointCloud<pcl::PointXYZ>);
6711  for(std::map<int, Transform>::iterator iter=finalPoses.begin(); iter!=finalPoses.end(); ++iter)
6712  {
6713  iter->second = u * iter->second;
6714  if(iter->first != link.to()) // already added to view
6715  {
6716  //create scan
6717  SensorData data;
6718  dbDriver_->getNodeData(iter->first, data);
6719  LaserScan scan;
6720  data.uncompressDataConst(0, 0, &scan, 0);
6721  if(!scan.isEmpty())
6722  {
6723  if(scan.hasNormals() && scan.hasIntensity())
6724  {
6725  *assembledINormalScans += *util3d::laserScanToPointCloudINormal(scan, iter->second*scan.localTransform());
6726  }
6727  else if(scan.hasNormals())
6728  {
6729  *assembledNormalScans += *util3d::laserScanToPointCloudNormal(scan, iter->second*scan.localTransform());
6730  }
6731  else if(scan.hasIntensity())
6732  {
6733  *assembledIScans += *util3d::laserScanToPointCloudI(scan, iter->second*scan.localTransform());
6734  }
6735  else
6736  {
6737  *assembledScans += *util3d::laserScanToPointCloud(scan, iter->second*scan.localTransform());
6738  }
6739  }
6740  }
6741  graph->push_back(util3d::transformPoint(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()), pose));
6742  }
6743 
6744  if(assembledNormalScans->size())
6745  {
6746  constraintsViewer_->addCloud("scan2", assembledNormalScans, pose, Qt::cyan);
6747  constraintsViewer_->setCloudColorIndex("scan2", 2);
6748  }
6749  if(assembledScans->size())
6750  {
6751  constraintsViewer_->addCloud("scan2", assembledScans, pose, Qt::cyan);
6752  constraintsViewer_->setCloudColorIndex("scan2", 2);
6753  }
6754  if(assembledINormalScans->size())
6755  {
6756  constraintsViewer_->addCloud("scan2", assembledINormalScans, pose, Qt::cyan);
6757  constraintsViewer_->setCloudColorIndex("scan2", 2);
6758  }
6759  if(assembledIScans->size())
6760  {
6761  constraintsViewer_->addCloud("scan2", assembledIScans, pose, Qt::cyan);
6762  constraintsViewer_->setCloudColorIndex("scan2", 2);
6763  }
6764  if(graph->size())
6765  {
6766  constraintsViewer_->addOrUpdateGraph("scan2graph", graph, Qt::cyan);
6767  }
6768  }
6769  }
6770  }
6771 
6772  // Added loop closure scans
6773  constraintsViewer_->removeCloud("scan0");
6774  constraintsViewer_->removeCloud("scan1");
6775  if(!dataFrom.laserScanRaw().isEmpty())
6776  {
6777  if(dataFrom.laserScanRaw().hasNormals() && dataFrom.laserScanRaw().hasIntensity())
6778  {
6779  pcl::PointCloud<pcl::PointXYZINormal>::Ptr scan;
6780  scan = rtabmap::util3d::laserScanToPointCloudINormal(dataFrom.laserScanRaw(), dataFrom.laserScanRaw().localTransform());
6781  constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
6782  constraintsViewer_->setCloudColorIndex("scan0", 2);
6783  }
6784  else if(dataFrom.laserScanRaw().hasNormals())
6785  {
6786  pcl::PointCloud<pcl::PointNormal>::Ptr scan;
6787  scan = rtabmap::util3d::laserScanToPointCloudNormal(dataFrom.laserScanRaw(), dataFrom.laserScanRaw().localTransform());
6788  constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
6789  constraintsViewer_->setCloudColorIndex("scan0", 2);
6790  }
6791  else if(dataFrom.laserScanRaw().hasIntensity())
6792  {
6793  pcl::PointCloud<pcl::PointXYZI>::Ptr scan;
6794  scan = rtabmap::util3d::laserScanToPointCloudI(dataFrom.laserScanRaw(), dataFrom.laserScanRaw().localTransform());
6795  constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
6796  constraintsViewer_->setCloudColorIndex("scan0", 2);
6797  }
6798  else
6799  {
6800  pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
6801  scan = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw(), dataFrom.laserScanRaw().localTransform());
6802  constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
6803  constraintsViewer_->setCloudColorIndex("scan0", 2);
6804  }
6805  }
6806  if(!dataTo.laserScanRaw().isEmpty())
6807  {
6808  if(dataTo.laserScanRaw().hasNormals() && dataTo.laserScanRaw().hasIntensity())
6809  {
6810  pcl::PointCloud<pcl::PointXYZINormal>::Ptr scan;
6811  scan = rtabmap::util3d::laserScanToPointCloudINormal(dataTo.laserScanRaw(), t*dataTo.laserScanRaw().localTransform());
6812  constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
6813  constraintsViewer_->setCloudColorIndex("scan1", 2);
6814  }
6815  else if(dataTo.laserScanRaw().hasNormals())
6816  {
6817  pcl::PointCloud<pcl::PointNormal>::Ptr scan;
6818  scan = rtabmap::util3d::laserScanToPointCloudNormal(dataTo.laserScanRaw(), t*dataTo.laserScanRaw().localTransform());
6819  constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
6820  constraintsViewer_->setCloudColorIndex("scan1", 2);
6821  }
6822  else if(dataTo.laserScanRaw().hasIntensity())
6823  {
6824  pcl::PointCloud<pcl::PointXYZI>::Ptr scan;
6825  scan = rtabmap::util3d::laserScanToPointCloudI(dataTo.laserScanRaw(), t*dataTo.laserScanRaw().localTransform());
6826  constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
6827  constraintsViewer_->setCloudColorIndex("scan1", 2);
6828  }
6829  else
6830  {
6831  pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
6832  scan = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw(), t*dataTo.laserScanRaw().localTransform());
6833  constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
6834  constraintsViewer_->setCloudColorIndex("scan1", 2);
6835  }
6836  }
6837  }
6838 
6839  //frustums
6840  constraintsViewer_->removeAllFrustums();
6841  if(constraintsViewer_->isFrustumShown())
6842  {
6843  CameraModel model;
6844  if(dataFrom.cameraModels().size())
6845  {
6846  model = dataFrom.cameraModels()[0];
6847  }
6848  else if(dataFrom.stereoCameraModels().size())
6849  {
6850  model = dataFrom.stereoCameraModels()[0].left();
6851  }
6852  constraintsViewer_->addOrUpdateFrustum("frustum_from", pose, model.localTransform(), constraintsViewer_->getFrustumScale(), constraintsViewer_->getFrustumColor(), model.fovX(), model.fovY());
6853  model = CameraModel();
6854  if(dataTo.cameraModels().size())
6855  {
6856  model = dataTo.cameraModels()[0];
6857  }
6858  else if(dataTo.stereoCameraModels().size())
6859  {
6860  model = dataTo.stereoCameraModels()[0].left();
6861  }
6862  constraintsViewer_->addOrUpdateFrustum("frustum_to", pose*t, model.localTransform(), constraintsViewer_->getFrustumScale(), constraintsViewer_->getFrustumColor(), model.fovX(), model.fovY());
6863  }
6864 
6865  //update coordinate
6866  constraintsViewer_->addOrUpdateCoordinate("from_coordinate", pose, 0.2);
6867 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
6868  constraintsViewer_->addOrUpdateCoordinate("to_coordinate", pose*t, 0.2);
6869  constraintsViewer_->removeCoordinate("to_coordinate_gt");
6870  if(uContains(groundTruthPoses_, link.from()) && uContains(groundTruthPoses_, link.to()))
6871  {
6872  constraintsViewer_->addOrUpdateCoordinate("to_coordinate_gt",
6873  pose*(groundTruthPoses_.at(link.from()).inverse()*groundTruthPoses_.at(link.to())), 0.1);
6874  }
6875 #endif
6876 
6877  constraintsViewer_->clearTrajectory();
6878 
6879  constraintsViewer_->refreshView();
6880  }
6881 
6882  // update buttons
6883  updateConstraintButtons();
6884 }
6885 
6886 void DatabaseViewer::updateConstraintButtons()
6887 {
6888  ui_->pushButton_refine->setEnabled(false);
6889  ui_->pushButton_reset->setEnabled(false);
6890  ui_->pushButton_add->setEnabled(false);
6891  ui_->pushButton_reject->setEnabled(false);
6892  ui_->toolButton_constraint->setEnabled(false);
6893 
6894  Link currentLink;
6895  int from;
6896  int to;
6897  if(ui_->label_type->text().toInt() == Link::kLandmark)
6898  {
6899  //check for modified link
6900  currentLink = loopLinks_.at(ui_->horizontalSlider_loops->value());
6901  from = currentLink.from();
6902  to = currentLink.to();
6903  }
6904  else
6905  {
6906  from = ids_.at(ui_->horizontalSlider_A->value());
6907  to = ids_.at(ui_->horizontalSlider_B->value());
6908  if(from!=to && from && to &&
6909  odomPoses_.find(from) != odomPoses_.end() &&
6910  odomPoses_.find(to) != odomPoses_.end() &&
6911  (ui_->checkBox_enableForAll->isChecked() ||
6912  (weights_.find(from) != weights_.end() && weights_.at(from)>=0 &&
6913  weights_.find(to) != weights_.end() && weights_.at(to)>=0)))
6914  {
6915  if((!containsLink(links_, from ,to) && !containsLink(linksAdded_, from ,to)) ||
6916  containsLink(linksRemoved_, from ,to))
6917  {
6918  ui_->pushButton_add->setEnabled(true);
6919  }
6920  }
6921  else if(ui_->checkBox_enableForAll->isChecked())
6922  {
6923  if(odomPoses_.find(from) == odomPoses_.end())
6924  {
6925  UWARN("Button \"Add\" cannot be enabled even if \"all\" checkbox is checked, as node %d doesn't have odometry set.", from);
6926  }
6927  else if(odomPoses_.find(to) == odomPoses_.end())
6928  {
6929  UWARN("Button \"Add\" cannot be enabled even if \"all\" checkbox is checked, as node %d doesn't have odometry set.", to);
6930  }
6931  }
6932 
6933  currentLink = findActiveLink(from ,to);
6934  }
6935 
6936  if(currentLink.isValid() &&
6937  ((currentLink.from() == from && currentLink.to() == to) || (currentLink.from() == to && currentLink.to() == from)))
6938  {
6939  if(!containsLink(linksRemoved_, from ,to))
6940  {
6941  ui_->pushButton_reject->setEnabled(true);
6942  }
6943 
6944  //check for modified link
6945  std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, currentLink.from(), currentLink.to());
6946  if(iter != linksRefined_.end())
6947  {
6948  currentLink = iter->second;
6949  ui_->pushButton_reset->setEnabled(true);
6950  }
6951  ui_->pushButton_refine->setEnabled(currentLink.from()!=currentLink.to() && currentLink.type() != Link::kLandmark);
6952  ui_->toolButton_constraint->setEnabled(true);
6953  }
6954 }
6955 
6956 void DatabaseViewer::sliderIterationsValueChanged(int value)
6957 {
6958  if(dbDriver_ && value >=0 && value < (int)graphes_.size())
6959  {
6960  std::map<int, rtabmap::Transform> graph = uValueAt(graphes_, value);
6961 
6962  std::map<int, Transform> refPoses = groundTruthPoses_;
6963  if(ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked())
6964  {
6965  refPoses = gpsPoses_;
6966  }
6967 
6968  // Log ground truth statistics (in TUM's RGBD-SLAM format)
6969  if(refPoses.size())
6970  {
6971  // compute KITTI statistics before aligning the poses
6972  float length = graph::computePathLength(graph);
6973  if(refPoses.size() == graph.size() && length >= 100.0f)
6974  {
6975  float t_err = 0.0f;
6976  float r_err = 0.0f;
6977  graph::calcKittiSequenceErrors(uValues(refPoses), uValues(graph), t_err, r_err);
6978  UINFO("KITTI t_err = %f %%", t_err);
6979  UINFO("KITTI r_err = %f deg/m", r_err);
6980  }
6981 
6982  float translational_rmse = 0.0f;
6983  float translational_mean = 0.0f;
6984  float translational_median = 0.0f;
6985  float translational_std = 0.0f;
6986  float translational_min = 0.0f;
6987  float translational_max = 0.0f;
6988  float rotational_rmse = 0.0f;
6989  float rotational_mean = 0.0f;
6990  float rotational_median = 0.0f;
6991  float rotational_std = 0.0f;
6992  float rotational_min = 0.0f;
6993  float rotational_max = 0.0f;
6994 
6995  Transform gtToMap = graph::calcRMSE(
6996  refPoses,
6997  graph,
6998  translational_rmse,
6999  translational_mean,
7000  translational_median,
7001  translational_std,
7002  translational_min,
7003  translational_max,
7004  rotational_rmse,
7005  rotational_mean,
7006  rotational_median,
7007  rotational_std,
7008  rotational_min,
7009  rotational_max);
7010 
7011  // ground truth live statistics
7012  ui_->label_rmse->setNum(translational_rmse);
7013  UINFO("translational_rmse=%f", translational_rmse);
7014  UINFO("translational_mean=%f", translational_mean);
7015  UINFO("translational_median=%f", translational_median);
7016  UINFO("translational_std=%f", translational_std);
7017  UINFO("translational_min=%f", translational_min);
7018  UINFO("translational_max=%f", translational_max);
7019 
7020  UINFO("rotational_rmse=%f", rotational_rmse);
7021  UINFO("rotational_mean=%f", rotational_mean);
7022  UINFO("rotational_median=%f", rotational_median);
7023  UINFO("rotational_std=%f", rotational_std);
7024  UINFO("rotational_min=%f", rotational_min);
7025  UINFO("rotational_max=%f", rotational_max);
7026 
7027  if(((ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked()) ||
7028  (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked())) &&
7029  !gtToMap.isIdentity())
7030  {
7031  for(std::map<int, Transform>::iterator iter=graph.begin(); iter!=graph.end(); ++iter)
7032  {
7033  iter->second = gtToMap * iter->second;
7034  }
7035  }
7036  }
7037 
7038  std::map<int, rtabmap::Transform> graphFiltered;
7039  if(ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() &&
7040  ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() &&
7041  !groundTruthPoses_.empty())
7042  {
7043  graphFiltered = groundTruthPoses_;
7044  }
7045  else
7046  {
7047  graphFiltered = graph;
7048  }
7049  if(ui_->groupBox_posefiltering->isChecked())
7050  {
7051  graphFiltered = graph::radiusPosesFiltering(graph,
7052  ui_->doubleSpinBox_posefilteringRadius->value(),
7053  ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
7054  }
7055  LocalGridCache combinedLocalMaps;
7056 #ifdef RTABMAP_OCTOMAP
7057  delete octomap_;
7058  octomap_ = 0;
7059 #endif
7060  if(ui_->dockWidget_graphView->isVisible() || ui_->dockWidget_occupancyGridView->isVisible())
7061  {
7062  //update scans
7063  UINFO("Update local maps list...");
7064  std::vector<int> ids = uKeys(graphFiltered);
7065  for(unsigned int i=0; i<ids.size(); ++i)
7066  {
7067  if(generatedLocalMaps_.shareTo(ids[i], combinedLocalMaps) ||
7068  localMaps_.shareTo(ids[i], combinedLocalMaps))
7069  {
7070  // Added to combined maps
7071  }
7072  else if(ids.at(i)>0)
7073  {
7074  SensorData data;
7075  dbDriver_->getNodeData(ids.at(i), data, false, false, false);
7076  cv::Mat ground, obstacles, empty;
7077  if(data.gridCellSize()>0.0f)
7078  {
7079  data.uncompressData(0, 0, 0, 0, &ground, &obstacles, &empty);
7080  }
7081  localMaps_.add(ids.at(i), ground, obstacles, empty, data.gridCellSize()>0.0f?data.gridCellSize():Parameters::defaultGridCellSize(), data.gridViewPoint());
7082  if(!ground.empty() || !obstacles.empty())
7083  {
7084  localMaps_.shareTo(ids.at(i), combinedLocalMaps);
7085  }
7086  }
7087  }
7088  UINFO("Update local maps list... done (%d local maps, graph size=%d)", (int)combinedLocalMaps.size(), (int)graph.size());
7089  }
7090 
7091  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
7092  float cellSize = Parameters::defaultGridCellSize();
7093  Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize);
7094 
7095  ui_->graphViewer->updateGTGraph(groundTruthPoses_);
7096  ui_->graphViewer->updateGPSGraph(gpsPoses_, gpsValues_);
7097  ui_->graphViewer->updateGraph(graph, graphLinks_, mapIds_, weights_);
7098  if(ui_->checkBox_wmState->isEnabled() &&
7099  ui_->checkBox_wmState->isChecked() &&
7100  !lastWmIds_.empty())
7101  {
7102  bool allNodesAreInWM = true;
7103  std::map<int, float> colors;
7104  for(std::map<int, rtabmap::Transform>::iterator iter=graph.begin(); iter!=graph.end(); ++iter)
7105  {
7106  if(lastWmIds_.find(iter->first) != lastWmIds_.end())
7107  {
7108  colors.insert(std::make_pair(iter->first, 1));
7109  }
7110  else
7111  {
7112  allNodesAreInWM = false;
7113  }
7114  }
7115  if(!allNodesAreInWM)
7116  {
7117  ui_->graphViewer->updatePosterior(colors, 1, 1);
7118  }
7119  }
7120  QGraphicsRectItem * rectScaleItem = 0;
7121  ui_->graphViewer->clearMap();
7122  occupancyGridViewer_->clear();
7123  if(graph.size() && combinedLocalMaps.size() &&
7124  (ui_->graphViewer->isGridMapVisible() || ui_->dockWidget_occupancyGridView->isVisible()))
7125  {
7126  QElapsedTimer time;
7127  time.start();
7128 
7129 #ifdef RTABMAP_OCTOMAP
7130  if(ui_->checkBox_octomap->isChecked())
7131  {
7132  octomap_ = new OctoMap(&combinedLocalMaps, parameters);
7133  octomap_->update(graphFiltered);
7134  }
7135 #endif
7136  // Generate 2d grid map?
7137  if((ui_->dockWidget_graphView->isVisible() && ui_->graphViewer->isGridMapVisible()) ||
7138  (ui_->dockWidget_occupancyGridView->isVisible() && ui_->checkBox_grid_2d->isChecked()))
7139  {
7140  bool eroded = Parameters::defaultGridGlobalEroded();
7141  Parameters::parse(parameters, Parameters::kGridGlobalEroded(), eroded);
7142  float xMin, yMin;
7143  cv::Mat map;
7144 
7145 #ifdef RTABMAP_OCTOMAP
7146  if(ui_->checkBox_octomap->isChecked())
7147  {
7148  map = octomap_->createProjectionMap(xMin, yMin, cellSize, 0, ui_->spinBox_grid_depth->value());
7149  }
7150  else
7151 #endif
7152  {
7153  if(eroded)
7154  {
7155  uInsert(parameters, ParametersPair(Parameters::kGridGlobalEroded(), "true"));
7156  }
7157  OccupancyGrid grid(&combinedLocalMaps, parameters);
7158  grid.update(graphFiltered);
7159  if(ui_->checkBox_grid_showProbMap->isChecked())
7160  {
7161  map = grid.getProbMap(xMin, yMin);
7162  }
7163  else
7164  {
7165  map = grid.getMap(xMin, yMin);
7166  }
7167  }
7168 
7169  ui_->label_timeGrid->setNum(double(time.elapsed())/1000.0);
7170 
7171  if(!map.empty())
7172  {
7173  cv::Mat map8U = rtabmap::util3d::convertMap2Image8U(map);
7174  if(ui_->dockWidget_graphView->isVisible() && ui_->graphViewer->isGridMapVisible())
7175  {
7176  ui_->graphViewer->updateMap(map8U, cellSize, xMin, yMin);
7177  }
7178  if(ui_->dockWidget_occupancyGridView->isVisible() && ui_->checkBox_grid_2d->isChecked())
7179  {
7180  occupancyGridViewer_->addOccupancyGridMap(map8U, cellSize, xMin, yMin, 1.0f);
7181  occupancyGridViewer_->refreshView();
7182  }
7183 
7184  // Zoom to ignore unknowns
7185  int xFirst = 0;
7186  int yFirst = 0;
7187  int xLast = map.cols;
7188  int yLast = map.rows;
7189  bool firstSet = false;
7190  bool lastSet = false;
7191  for(int x=0; x<map.cols && (!firstSet || !lastSet); ++x)
7192  {
7193  for(int y=0; y<map.rows; ++y)
7194  {
7195  // check for first
7196  if(!firstSet && map.at<signed char>(y, x) != -1)
7197  {
7198  xFirst = x;
7199  firstSet = true;
7200  }
7201  // check for last
7202  int opp = map.cols-(x+1);
7203  if(!lastSet && map.at<signed char>(y, opp) != -1)
7204  {
7205  xLast = opp;
7206  lastSet = true;
7207  }
7208  }
7209  }
7210  firstSet = false;
7211  lastSet = false;
7212  for(int y=0; y<map.rows && (!firstSet || !lastSet); ++y)
7213  {
7214  for(int x=0; x<map.cols; ++x)
7215  {
7216  // check for first
7217  if(!firstSet && map.at<signed char>(y, x) != -1)
7218  {
7219  yFirst = y;
7220  firstSet = true;
7221  }
7222  // check for last
7223  int opp = map.rows-(y+1);
7224  if(!lastSet && map.at<signed char>(map.rows-(y+1), x) != -1)
7225  {
7226  yLast = opp;
7227  lastSet = true;
7228  }
7229  }
7230  }
7231  // Only zoom if there are significant unknowns
7232  if( (xLast > xFirst && yLast > yFirst) &&
7233  (xFirst > 50 ||
7234  xLast < map.cols-50 ||
7235  yFirst > 50 ||
7236  yLast < map.rows-50))
7237  {
7238  rectScaleItem = ui_->graphViewer->scene()->addRect(
7239  xFirst-25,
7240  yFirst-25,
7241  xLast-xFirst+50,
7242  yLast-yFirst+50);
7243  rectScaleItem->setTransform(QTransform::fromScale(cellSize*100.0f, -cellSize*100.0f), true);
7244  rectScaleItem->setRotation(90);
7245  rectScaleItem->setPos(-yMin*100.0f, -xMin*100.0f);
7246  }
7247  }
7248  }
7249 
7250  // Generate 3d grid map?
7251  if(ui_->dockWidget_occupancyGridView->isVisible() && ui_->checkBox_grid_grid->isChecked())
7252  {
7253 #ifdef RTABMAP_OCTOMAP
7254  if(ui_->checkBox_octomap->isChecked())
7255  {
7256  updateOctomapView();
7257  }
7258  else
7259 #endif
7260  {
7261  CloudMap cloudMap(&combinedLocalMaps, parameters);
7262 
7263  cloudMap.update(graphFiltered);
7264 
7265  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & groundCells = cloudMap.getMapGround();
7266  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacleCells = cloudMap.getMapObstacles();
7267  const pcl::PointCloud<pcl::PointXYZ>::Ptr & emptyCells = cloudMap.getMapEmptyCells();
7268 
7269  // occupancy cloud
7270  if(groundCells->size())
7271  {
7272  occupancyGridViewer_->addCloud("groundCells",
7273  groundCells,
7274  Transform::getIdentity(),
7275  QColor(ui_->lineEdit_groundColor->text()));
7276  occupancyGridViewer_->setCloudPointSize("groundCells", 5);
7277  }
7278  if(obstacleCells->size())
7279  {
7280  occupancyGridViewer_->addCloud("obstacleCells",
7281  obstacleCells,
7282  Transform::getIdentity(),
7283  QColor(ui_->lineEdit_obstacleColor->text()));
7284  occupancyGridViewer_->setCloudPointSize("obstacleCells", 5);
7285  }
7286  if(ui_->checkBox_grid_empty->isChecked() && emptyCells->size())
7287  {
7288  occupancyGridViewer_->addCloud("emptyCells",
7289  emptyCells,
7290  Transform::getIdentity(),
7291  QColor(ui_->lineEdit_emptyColor->text()));
7292  occupancyGridViewer_->setCloudPointSize("emptyCells", 5);
7293  occupancyGridViewer_->setCloudOpacity("emptyCells", 0.5);
7294  }
7295  occupancyGridViewer_->refreshView();
7296  }
7297  }
7298 
7299 #ifdef RTABMAP_GRIDMAP
7300  // Show elevation map ?
7301  if(ui_->dockWidget_occupancyGridView->isVisible() &&
7302  ui_->checkBox_grid_elevation->checkState() != Qt::Unchecked)
7303  {
7304  GridMap gridMap(&combinedLocalMaps, parameters);
7305 
7306  gridMap.update(graphFiltered);
7307  if(ui_->checkBox_grid_elevation->checkState() == Qt::PartiallyChecked)
7308  {
7309  float xMin, yMin;
7310  cv::Mat elevationMap = gridMap.createHeightMap(xMin, yMin, cellSize);
7311  occupancyGridViewer_->addElevationMap(elevationMap, cellSize, xMin, yMin, 1.0f);
7312  }
7313  else
7314  {
7315  pcl::PolygonMesh::Ptr mesh = gridMap.createTerrainMesh();
7316  occupancyGridViewer_->addCloudMesh("elevation_mesh", mesh);
7317  }
7318  occupancyGridViewer_->refreshView();
7319  }
7320 #endif
7321  }
7322  ui_->graphViewer->fitInView(ui_->graphViewer->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
7323  if(rectScaleItem != 0)
7324  {
7325  ui_->graphViewer->fitInView(rectScaleItem, Qt::KeepAspectRatio);
7326  ui_->graphViewer->scene()->removeItem(rectScaleItem);
7327  delete rectScaleItem;
7328  }
7329 
7330  ui_->graphViewer->update();
7331  ui_->label_iterations->setNum(value);
7332 
7333  //compute total length (neighbor links)
7334  float length = 0.0f;
7335  for(std::multimap<int, rtabmap::Link>::const_iterator iter=graphLinks_.begin(); iter!=graphLinks_.end(); ++iter)
7336  {
7337  std::map<int, rtabmap::Transform>::const_iterator jterA = graph.find(iter->first);
7338  std::map<int, rtabmap::Transform>::const_iterator jterB = graph.find(iter->second.to());
7339  if(jterA != graph.end() && jterB != graph.end())
7340  {
7341  const rtabmap::Transform & poseA = jterA->second;
7342  const rtabmap::Transform & poseB = jterB->second;
7343  if(iter->second.type() == rtabmap::Link::kNeighbor ||
7344  iter->second.type() == rtabmap::Link::kNeighborMerged)
7345  {
7346  Eigen::Vector3f vA, vB;
7347  float x,y,z;
7348  poseA.getTranslation(x,y,z);
7349  vA[0] = x; vA[1] = y; vA[2] = z;
7350  poseB.getTranslation(x,y,z);
7351  vB[0] = x; vB[1] = y; vB[2] = z;
7352  length += (vB - vA).norm();
7353  }
7354  }
7355  }
7356  ui_->label_pathLength->setNum(length);
7357  }
7358 }
7359 
7360 void DatabaseViewer::updateGraphRotation()
7361 {
7362  if(ui_->horizontalSlider_rotation->isEnabled())
7363  {
7364  float theta = float(ui_->horizontalSlider_rotation->value())*M_PI/1800.0f;
7365  ui_->graphViewer->setWorldMapRotation(theta);
7366  ui_->label_rotation->setText(QString::number(float(-ui_->horizontalSlider_rotation->value())/10.0f, 'f', 1) + " deg");
7367  }
7368  else
7369  {
7370  ui_->graphViewer->setWorldMapRotation(0);
7371  }
7372 }
7373 
7374 void DatabaseViewer::updateGraphView()
7375 {
7376  ui_->label_loopClosures->clear();
7377  ui_->label_poses->clear();
7378  ui_->label_rmse->clear();
7379 
7380  if(sender() == ui_->checkBox_alignPosesWithGPS && ui_->checkBox_alignPosesWithGPS->isChecked())
7381  {
7382  ui_->checkBox_alignPosesWithGroundTruth->setChecked(false);
7383  }
7384  else if(sender() == ui_->checkBox_alignPosesWithGroundTruth && ui_->checkBox_alignPosesWithGroundTruth->isChecked())
7385  {
7386  ui_->checkBox_alignPosesWithGPS->setChecked(false);
7387  }
7388 
7389  if(odomPoses_.size())
7390  {
7391  int fromId = ui_->spinBox_optimizationsFrom->value();
7392  if(!uContains(odomPoses_, fromId))
7393  {
7394  QMessageBox::warning(this, tr(""), tr("Graph optimization from id (%1) for which node is not linked to graph.\n Minimum=%2, Maximum=%3")
7395  .arg(fromId)
7396  .arg(odomPoses_.begin()->first)
7397  .arg(odomPoses_.rbegin()->first));
7398  return;
7399  }
7400 
7401  graphes_.clear();
7402  graphLinks_.clear();
7403 
7404  std::map<int, rtabmap::Transform> poses = odomPoses_;
7405  if(ui_->checkBox_wmState->isEnabled() &&
7406  ui_->checkBox_wmState->isChecked() &&
7407  uContains(wmStates_, fromId))
7408  {
7409  std::map<int, rtabmap::Transform> wmPoses;
7410  std::vector<int> & wmState = wmStates_.at(fromId);
7411  for(unsigned int i=0; i<wmState.size(); ++i)
7412  {
7413  std::map<int, rtabmap::Transform>::iterator iter = poses.find(wmState[i]);
7414  if(iter!=poses.end())
7415  {
7416  wmPoses.insert(*iter);
7417  }
7418  }
7419  if(!wmPoses.empty())
7420  {
7421  poses = wmPoses;
7422  }
7423  else
7424  {
7425  UWARN("Empty WM poses!? Ignoring WM state... (root id=%d, wmState=%d)", fromId, wmState.size());
7426  }
7427  }
7428 
7429  // filter current map if not spanning to all maps
7430  if(!ui_->checkBox_spanAllMaps->isChecked() && uContains(mapIds_, fromId) && mapIds_.at(fromId) >= 0)
7431  {
7432  int currentMapId = mapIds_.at(fromId);
7433  for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end();)
7434  {
7435  if(iter->first>0 && (!uContains(mapIds_, iter->first) || mapIds_.at(iter->first) != currentMapId))
7436  {
7437  poses.erase(iter++);
7438  }
7439  else
7440  {
7441  ++iter;
7442  }
7443  }
7444  }
7445 
7446  ui_->menuExport_poses->setEnabled(true);
7447  std::multimap<int, rtabmap::Link> links = links_;
7448 
7449  // filter current map if not spanning to all maps
7450  if(!ui_->checkBox_spanAllMaps->isChecked() && uContains(mapIds_, fromId) && mapIds_.at(fromId) >= 0)
7451  {
7452  int currentMapId = mapIds_.at(fromId);
7453  for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end();)
7454  {
7455  if((iter->second.from()>0 && (!uContains(mapIds_, iter->second.from()) || mapIds_.at(iter->second.from()) != currentMapId)) ||
7456  (iter->second.to()>0 && (!uContains(mapIds_, iter->second.to()) || mapIds_.at(iter->second.to()) != currentMapId)))
7457  {
7458  links.erase(iter++);
7459  }
7460  else
7461  {
7462  ++iter;
7463  }
7464  }
7465  }
7466 
7467  links = updateLinksWithModifications(links);
7468  if(ui_->checkBox_ignorePoseCorrection->isChecked())
7469  {
7470  for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
7471  {
7472  if(iter->second.type() == Link::kNeighbor ||
7473  iter->second.type() == Link::kNeighborMerged)
7474  {
7475  Transform poseFrom = uValue(poses, iter->second.from(), Transform());
7476  Transform poseTo = uValue(poses, iter->second.to(), Transform());
7477  if(!poseFrom.isNull() && !poseTo.isNull())
7478  {
7479  // reset to identity covariance
7480  iter->second = Link(
7481  iter->second.from(),
7482  iter->second.to(),
7483  iter->second.type(),
7484  poseFrom.inverse() * poseTo);
7485  }
7486  }
7487  }
7488  }
7489 
7490  // Marker priors parameters
7491  double markerPriorsLinearVariance = Parameters::defaultMarkerPriorsVarianceLinear();
7492  double markerPriorsAngularVariance = Parameters::defaultMarkerPriorsVarianceAngular();
7493  std::map<int, Transform> markerPriors;
7494  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
7495  Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), markerPriorsLinearVariance);
7496  UASSERT(markerPriorsLinearVariance>0.0f);
7497  Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceAngular(), markerPriorsAngularVariance);
7498  UASSERT(markerPriorsAngularVariance>0.0f);
7499  std::string markerPriorsStr;
7500  if(Parameters::parse(parameters, Parameters::kMarkerPriors(), markerPriorsStr))
7501  {
7502  std::list<std::string> strList = uSplit(markerPriorsStr, '|');
7503  for(std::list<std::string>::iterator iter=strList.begin(); iter!=strList.end(); ++iter)
7504  {
7505  std::string markerStr = *iter;
7506  while(!markerStr.empty() && !uIsDigit(markerStr[0]))
7507  {
7508  markerStr.erase(markerStr.begin());
7509  }
7510  if(!markerStr.empty())
7511  {
7512  std::string idStr = uSplitNumChar(markerStr).front();
7513  int id = uStr2Int(idStr);
7514  Transform prior = Transform::fromString(markerStr.substr(idStr.size()));
7515  if(!prior.isNull() && id>0)
7516  {
7517  markerPriors.insert(std::make_pair(-id, prior));
7518  UDEBUG("Added landmark prior %d: %s", id, prior.prettyPrint().c_str());
7519  }
7520  else
7521  {
7522  UERROR("Failed to parse element \"%s\" in parameter %s", markerStr.c_str(), Parameters::kMarkerPriors().c_str());
7523  }
7524  }
7525  else if(!iter->empty())
7526  {
7527  UERROR("Failed to parse parameter %s, value=\"%s\"", Parameters::kMarkerPriors().c_str(), iter->c_str());
7528  }
7529  }
7530  }
7531 
7532 
7533  // filter links
7534  int totalNeighbor = 0;
7535  int totalNeighborMerged = 0;
7536  int totalGlobal = 0;
7537  int totalLocalTime = 0;
7538  int totalLocalSpace = 0;
7539  int totalUser = 0;
7540  int totalPriors = 0;
7541  int totalLandmarks = 0;
7542  int totalGravity = 0;
7543  std::multimap<int, int> uniqueLinks;
7544  for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end();)
7545  {
7546  bool isUnique = iter->second.from() == iter->second.to(); // Count all self-reference links
7547  if(graph::findLink(uniqueLinks, iter->second.from(), iter->second.to(), true) == uniqueLinks.end())
7548  {
7549  uniqueLinks.insert(std::make_pair(iter->second.from(), iter->second.to()));
7550  isUnique = true;
7551  }
7552 
7553  if(iter->second.type() == Link::kNeighbor)
7554  {
7555  if(isUnique)
7556  ++totalNeighbor;
7557  }
7558  else if(iter->second.type() == Link::kNeighborMerged)
7559  {
7560  if(isUnique)
7561  ++totalNeighborMerged;
7562  }
7563  else if(iter->second.type() == Link::kGlobalClosure)
7564  {
7565  if(ui_->checkBox_ignoreGlobalLoop->isChecked())
7566  {
7567  links.erase(iter++);
7568  continue;
7569  }
7570  if(isUnique)
7571  ++totalGlobal;
7572  }
7573  else if(iter->second.type() == Link::kLocalSpaceClosure)
7574  {
7575  if(ui_->checkBox_ignoreLocalLoopSpace->isChecked())
7576  {
7577  links.erase(iter++);
7578  continue;
7579  }
7580  if(isUnique)
7581  ++totalLocalSpace;
7582  }
7583  else if(iter->second.type() == Link::kLocalTimeClosure)
7584  {
7585  if(ui_->checkBox_ignoreLocalLoopTime->isChecked())
7586  {
7587  links.erase(iter++);
7588  continue;
7589  }
7590  if(isUnique)
7591  ++totalLocalTime;
7592  }
7593  else if(iter->second.type() == Link::kUserClosure)
7594  {
7595  if(ui_->checkBox_ignoreUserLoop->isChecked())
7596  {
7597  links.erase(iter++);
7598  continue;
7599  }
7600  if(isUnique)
7601  ++totalUser;
7602  }
7603  else if(iter->second.type() == Link::kLandmark)
7604  {
7605  if(ui_->checkBox_ignoreLandmarks->isChecked())
7606  {
7607  links.erase(iter++);
7608  continue;
7609  }
7610  UASSERT(iter->second.from() > 0 && iter->second.to() < 0);
7611  if(poses.find(iter->second.from()) != poses.end() && poses.find(iter->second.to()) == poses.end())
7612  {
7613  poses.insert(std::make_pair(iter->second.to(), poses.at(iter->second.from())*iter->second.transform()));
7614  }
7615  if(isUnique)
7616  ++totalLandmarks;
7617 
7618  // add landmark priors if there are some
7619  int markerId = iter->second.to();
7620  if(markerPriors.find(markerId) != markerPriors.end())
7621  {
7622  cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64FC1);
7623  infMatrix(cv::Range(0,3), cv::Range(0,3)) /= markerPriorsLinearVariance;
7624  infMatrix(cv::Range(3,6), cv::Range(3,6)) /= markerPriorsAngularVariance;
7625  links.insert(std::make_pair(markerId, Link(markerId, markerId, Link::kPosePrior, markerPriors.at(markerId), infMatrix)));
7626  UDEBUG("Added prior %d : %s (variance: lin=%f ang=%f)", markerId, markerPriors.at(markerId).prettyPrint().c_str(),
7627  markerPriorsLinearVariance, markerPriorsAngularVariance);
7628  if(isUnique)
7629  ++totalPriors;
7630  }
7631  }
7632  else if(iter->second.type() == Link::kPosePrior)
7633  {
7634  if(isUnique)
7635  ++totalPriors;
7636  }
7637  else if(iter->second.type() == Link::kGravity)
7638  {
7639  if(isUnique)
7640  ++totalGravity;
7641  }
7642  ++iter;
7643  }
7644  updateLoopClosuresSlider();
7645 
7646  ui_->label_loopClosures->setText(tr("(%1, %2, %3, %4, %5, %6, %7, %8, %9)")
7647  .arg(totalNeighbor)
7648  .arg(totalNeighborMerged)
7649  .arg(totalGlobal)
7650  .arg(totalLocalSpace)
7651  .arg(totalLocalTime)
7652  .arg(totalUser)
7653  .arg(totalPriors)
7654  .arg(totalLandmarks)
7655  .arg(totalGravity));
7656 
7657  // remove intermediate nodes?
7658  if(ui_->checkBox_ignoreIntermediateNodes->isVisible() &&
7659  ui_->checkBox_ignoreIntermediateNodes->isEnabled() &&
7660  ui_->checkBox_ignoreIntermediateNodes->isChecked())
7661  {
7662  for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
7663  {
7664  if(iter->second.type() == Link::kNeighbor ||
7665  iter->second.type() == Link::kNeighborMerged)
7666  {
7667  Link link = iter->second;
7668  while(uContains(weights_, link.to()) && weights_.at(link.to()) < 0)
7669  {
7670  std::multimap<int, Link>::iterator uter = links.find(link.to());
7671  while(uter != links.end() &&
7672  uter->first==link.to() &&
7673  uter->second.from()>uter->second.to())
7674  {
7675  ++uter;
7676  }
7677  if(uter != links.end())
7678  {
7679  poses.erase(link.to());
7680  link = link.merge(uter->second, uter->second.type());
7681  links.erase(uter->first);
7682  }
7683  else
7684  {
7685  break;
7686  }
7687  }
7688 
7689  iter->second = link;
7690  }
7691  }
7692  }
7693 
7694  bool applyRotation = sender() == ui_->pushButton_applyRotation;
7695  if(applyRotation)
7696  {
7697  float xMin, yMin, cellSize;
7698  bool hasMap = !dbDriver_->load2DMap(xMin, yMin, cellSize).empty();
7699  if(hasMap || !dbDriver_->loadOptimizedMesh().empty())
7700  {
7701  QMessageBox::StandardButton r = QMessageBox::question(this,
7702  tr("Rotate Optimized Graph"),
7703  tr("There is a 2D occupancy grid or mesh already saved in "
7704  "database. Applying rotation will clear them (they can be "
7705  "regenerated later from File menu options). "
7706  "Do you want to continue?"),
7707  QMessageBox::Cancel | QMessageBox::Yes,
7708  QMessageBox::Cancel);
7709  if(r != QMessageBox::Yes)
7710  {
7711  applyRotation = false;
7712  }
7713  }
7714  }
7715 
7716  std::map<int, Transform> optPoses;
7717  Transform lastLocalizationPose;
7718  if(applyRotation ||
7719  ui_->comboBox_optimizationFlavor->currentIndex() == 2)
7720  {
7721  optPoses = dbDriver_->loadOptimizedPoses(&lastLocalizationPose);
7722  if(optPoses.empty())
7723  {
7724  ui_->comboBox_optimizationFlavor->setCurrentIndex(0);
7725  QMessageBox::warning(this, tr("Optimization Flavor"),
7726  tr("There is no local optimized graph in the database, "
7727  "falling back to global iterative optimization."));
7728  }
7729  }
7730 
7731  if(applyRotation ||
7732  ui_->comboBox_optimizationFlavor->currentIndex() != 2)
7733  {
7734  if(ui_->horizontalSlider_rotation->value()!=0 && applyRotation)
7735  {
7736  float theta = float(-ui_->horizontalSlider_rotation->value())*M_PI/1800.0f;
7737  Transform rotT(0,0,theta);
7738  poses.at(fromId) = rotT * poses.at(fromId);
7739  }
7740 
7741  graphes_.push_back(poses);
7742 
7743  Optimizer * optimizer = Optimizer::create(parameters);
7744 
7745  std::map<int, rtabmap::Transform> posesOut;
7746  std::multimap<int, rtabmap::Link> linksOut;
7747  UINFO("Get connected graph from %d (%d poses, %d links)", fromId, (int)poses.size(), (int)links.size());
7748  optimizer->getConnectedGraph(
7749  fromId,
7750  poses,
7751  links,
7752  posesOut,
7753  linksOut);
7754  UINFO("Connected graph of %d poses and %d links", (int)posesOut.size(), (int)linksOut.size());
7755  QElapsedTimer time;
7756  time.start();
7757  std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(fromId, posesOut, linksOut, ui_->comboBox_optimizationFlavor->currentIndex()==0?&graphes_:0);
7758  ui_->label_timeOptimization->setNum(double(time.elapsed())/1000.0);
7759  graphLinks_ = linksOut;
7760  if(posesOut.size() && finalPoses.empty())
7761  {
7762  UWARN("Optimization failed... (poses=%d, links=%d).", (int)posesOut.size(), (int)linksOut.size());
7763  if(!optimizer->isCovarianceIgnored() || optimizer->type() != Optimizer::kTypeTORO)
7764  {
7765  QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors. "
7766  "Give it a try with %1=0 and %2=true.").arg(Parameters::kOptimizerStrategy().c_str()).arg(Parameters::kOptimizerVarianceIgnored().c_str()));
7767  }
7768  else
7769  {
7770  QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors."));
7771  }
7772  }
7773  ui_->label_poses->setNum((int)finalPoses.size());
7774  graphes_.push_back(finalPoses);
7775  delete optimizer;
7776 
7777  if(applyRotation && !finalPoses.empty())
7778  {
7779  ui_->comboBox_optimizationFlavor->setCurrentIndex(2);
7780  graphes_.clear();
7781  graphes_.push_back(finalPoses);
7782  if(lastLocalizationPose.isNull())
7783  {
7784  // use last pose by default
7785  lastLocalizationPose = finalPoses.rbegin()->second;
7786  }
7787  dbDriver_->saveOptimizedPoses(finalPoses, lastLocalizationPose);
7788  // reset optimized mesh and map as poses have changed
7789  float xMin, yMin, cellSize;
7790  bool hasMap = !dbDriver_->load2DMap(xMin, yMin, cellSize).empty();
7791  if(hasMap || !dbDriver_->loadOptimizedMesh().empty())
7792  {
7793  dbDriver_->saveOptimizedMesh(cv::Mat());
7794  dbDriver_->save2DMap(cv::Mat(), 0, 0, 0);
7795  QMessageBox::StandardButton r = QMessageBox::question(this,
7796  tr("Rotate Optimized Graph"),
7797  tr("Optimized graph has been rotated and saved back to database. "
7798  "Note that 2D occupancy grid and mesh have been cleared (if set). "
7799  "Do you want to regenerate the 2D occupancy grid now "
7800  "(can be done later from File menu)?"),
7801  QMessageBox::Ignore | QMessageBox::Yes,
7802  QMessageBox::Yes);
7803  ui_->actionEdit_optimized_2D_map->setEnabled(false);
7804  ui_->actionExport_saved_2D_map->setEnabled(false);
7805  ui_->actionImport_2D_map->setEnabled(false);
7806  ui_->actionView_optimized_mesh->setEnabled(false);
7807  ui_->actionExport_optimized_mesh->setEnabled(false);
7808  if(r == QMessageBox::Yes)
7809  {
7810  regenerateSavedMap();
7811  }
7812  }
7813  }
7814  }
7815 
7816  // Update buttons state
7817  if(ui_->comboBox_optimizationFlavor->currentIndex() == 2)
7818  {
7819  // Local optimized graph
7820  if(graphes_.empty())
7821  {
7822  ui_->label_timeOptimization->setNum(0);
7823  ui_->label_poses->setNum((int)optPoses.size());
7824  graphes_.push_back(optPoses);
7825  }
7826  ui_->horizontalSlider_rotation->setEnabled(false);
7827  ui_->pushButton_applyRotation->setEnabled(false);
7828  ui_->spinBox_optimizationsFrom->setEnabled(false);
7829  ui_->checkBox_spanAllMaps->setEnabled(false);
7830  ui_->checkBox_wmState->setEnabled(false);
7831  ui_->checkBox_alignPosesWithGPS->setEnabled(false);
7832  ui_->checkBox_alignPosesWithGroundTruth->setEnabled(false);
7833  ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(false);
7834  ui_->checkBox_ignoreIntermediateNodes->setEnabled(false);
7835  }
7836  else
7837  {
7838  // Global map re-optimized
7839  ui_->pushButton_applyRotation->setEnabled(true);
7840  ui_->horizontalSlider_rotation->setEnabled(true);
7841  ui_->spinBox_optimizationsFrom->setEnabled(true);
7842  ui_->checkBox_spanAllMaps->setEnabled(true);
7843  ui_->checkBox_wmState->setEnabled(true);
7844  ui_->checkBox_alignPosesWithGPS->setEnabled(ui_->checkBox_alignPosesWithGPS->isVisible());
7845  ui_->checkBox_alignPosesWithGroundTruth->setEnabled(ui_->checkBox_alignPosesWithGroundTruth->isVisible());
7846  ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(ui_->checkBox_alignScansCloudsWithGroundTruth->isVisible());
7847  ui_->checkBox_ignoreIntermediateNodes->setEnabled(true);
7848  }
7849  updateGraphRotation();
7850  }
7851  if(graphes_.size())
7852  {
7853  if(ui_->doubleSpinBox_optimizationScale->value()!=-1.0)
7854  {
7855  // scale all poses
7856  for(std::list<std::map<int, Transform> >::iterator iter=graphes_.begin(); iter!=graphes_.end(); ++iter)
7857  {
7858  for(std::map<int, Transform>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
7859  {
7860  jter->second = jter->second.clone();
7861  jter->second.x() *= ui_->doubleSpinBox_optimizationScale->value();
7862  jter->second.y() *= ui_->doubleSpinBox_optimizationScale->value();
7863  jter->second.z() *= ui_->doubleSpinBox_optimizationScale->value();
7864  }
7865  }
7866  }
7867 
7868  ui_->horizontalSlider_iterations->setMaximum((int)graphes_.size()-1);
7869  ui_->horizontalSlider_iterations->setValue((int)graphes_.size()-1);
7870  ui_->horizontalSlider_iterations->setEnabled(true);
7871  ui_->spinBox_optimizationsFrom->setEnabled(true);
7872  sliderIterationsValueChanged((int)graphes_.size()-1);
7873  }
7874  else
7875  {
7876  ui_->horizontalSlider_iterations->setEnabled(false);
7877  ui_->spinBox_optimizationsFrom->setEnabled(false);
7878  }
7879 }
7880 
7881 void DatabaseViewer::updateGrid()
7882 {
7883  if(sender() == ui_->checkBox_grid_2d && !ui_->checkBox_grid_2d->isChecked())
7884  {
7885  //just remove map in occupancy grid view
7886  occupancyGridViewer_->removeOccupancyGridMap();
7887  occupancyGridViewer_->refreshView();
7888  }
7889  else
7890  {
7891  ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked());
7892  ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
7893  ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7894  ui_->checkBox_grid_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7895  ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
7896  ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
7897  ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7898  ui_->label_octomap_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7899 
7900  update3dView();
7901  updateGraphView();
7902  }
7903 }
7904 
7905 void DatabaseViewer::updateOctomapView()
7906 {
7907 #ifdef RTABMAP_OCTOMAP
7908  ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked());
7909  ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
7910  ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7911  ui_->checkBox_grid_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7912  ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
7913  ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
7914  ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7915  ui_->label_octomap_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7916 
7917  if(ui_->checkBox_octomap->isChecked())
7918  {
7919  if(octomap_)
7920  {
7921  occupancyGridViewer_->removeOctomap();
7922  occupancyGridViewer_->removeCloud("octomap_obstacles");
7923  occupancyGridViewer_->removeCloud("octomap_ground");
7924  occupancyGridViewer_->removeCloud("octomap_empty");
7925  occupancyGridViewer_->removeCloud("octomap_frontiers");
7926  occupancyGridViewer_->removeCloud("groundCells");
7927  occupancyGridViewer_->removeCloud("obstacleCells");
7928  occupancyGridViewer_->removeCloud("emptyCells");
7929  if(ui_->comboBox_octomap_rendering_type->currentIndex()>0)
7930  {
7931  occupancyGridViewer_->addOctomap(octomap_, ui_->spinBox_grid_depth->value(), ui_->comboBox_octomap_rendering_type->currentIndex()>1);
7932  }
7933  else
7934  {
7935  pcl::IndicesPtr obstacles(new std::vector<int>);
7936  pcl::IndicesPtr empty(new std::vector<int>);
7937  pcl::IndicesPtr ground(new std::vector<int>);
7938  pcl::IndicesPtr frontiers(new std::vector<int>);
7939  std::vector<double> prob;
7940  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(
7941  ui_->spinBox_grid_depth->value(),
7942  obstacles.get(),
7943  empty.get(),
7944  ground.get(),
7945  true,
7946  frontiers.get(),
7947  &prob);
7948 
7949  if(octomap_->hasColor())
7950  {
7951  pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
7952  pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
7953  occupancyGridViewer_->addCloud("octomap_obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
7954  occupancyGridViewer_->setCloudPointSize("octomap_obstacles", 5);
7955 
7956  pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
7957  pcl::copyPointCloud(*cloud, *ground, *groundCloud);
7958  occupancyGridViewer_->addCloud("octomap_ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
7959  occupancyGridViewer_->setCloudPointSize("octomap_ground", 5);
7960  }
7961  else
7962  {
7963  pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
7964  pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
7965  occupancyGridViewer_->addCloud("octomap_obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
7966  occupancyGridViewer_->setCloudPointSize("octomap_obstacles", 5);
7967 
7968  pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
7969  pcl::copyPointCloud(*cloud, *ground, *groundCloud);
7970  occupancyGridViewer_->addCloud("octomap_ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
7971  occupancyGridViewer_->setCloudPointSize("octomap_ground", 5);
7972  }
7973 
7974  if(ui_->checkBox_grid_empty->isChecked())
7975  {
7976  if(prob.size()==cloud->size())
7977  {
7978  float occThr = Parameters::defaultGridGlobalOccupancyThr();
7979  Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kGridGlobalOccupancyThr(), occThr);
7980  pcl::PointCloud<pcl::PointXYZRGB>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
7981  emptyCloud->resize(empty->size());
7982  for(unsigned int i=0;i<empty->size(); ++i)
7983  {
7984  emptyCloud->points.at(i).x = cloud->points.at(empty->at(i)).x;
7985  emptyCloud->points.at(i).y = cloud->points.at(empty->at(i)).y;
7986  emptyCloud->points.at(i).z = cloud->points.at(empty->at(i)).z;
7987  QColor hsv = QColor::fromHsv(int(prob.at(empty->at(i))/occThr*240.0), 255, 255, 255);
7988  QRgb color = hsv.rgb();
7989  emptyCloud->points.at(i).r = qRed(color);
7990  emptyCloud->points.at(i).g = qGreen(color);
7991  emptyCloud->points.at(i).b = qBlue(color);
7992  }
7993  occupancyGridViewer_->addCloud("octomap_empty", emptyCloud, Transform::getIdentity(), QColor(ui_->lineEdit_emptyColor->text()));
7994  occupancyGridViewer_->setCloudOpacity("octomap_empty", 0.5);
7995  occupancyGridViewer_->setCloudPointSize("octomap_empty", 5);
7996  }
7997  else
7998  {
7999  pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZ>);
8000  pcl::copyPointCloud(*cloud, *empty, *emptyCloud);
8001  occupancyGridViewer_->addCloud("octomap_empty", emptyCloud, Transform::getIdentity(), QColor(ui_->lineEdit_emptyColor->text()));
8002  occupancyGridViewer_->setCloudOpacity("octomap_empty", 0.5);
8003  occupancyGridViewer_->setCloudPointSize("octomap_empty", 5);
8004  }
8005  }
8006 
8007  if(ui_->checkBox_grid_frontiers->isChecked())
8008  {
8009  pcl::PointCloud<pcl::PointXYZ>::Ptr frontiersCloud(new pcl::PointCloud<pcl::PointXYZ>);
8010  pcl::copyPointCloud(*cloud, *frontiers, *frontiersCloud);
8011  occupancyGridViewer_->addCloud("octomap_frontiers", frontiersCloud, Transform::getIdentity(), QColor(ui_->lineEdit_frontierColor->text()));
8012  occupancyGridViewer_->setCloudOpacity("octomap_frontiers", 0.5);
8013  occupancyGridViewer_->setCloudPointSize("octomap_frontiers", 5);
8014  }
8015  }
8016  occupancyGridViewer_->refreshView();
8017  }
8018  if(ui_->dockWidget_view3d->isVisible() && ui_->checkBox_showGrid->isChecked())
8019  {
8020  this->update3dView();
8021  }
8022  }
8023 #endif
8024 }
8025 
8026 Link DatabaseViewer::findActiveLink(int from, int to)
8027 {
8028  Link link;
8029  std::multimap<int, Link>::iterator findIter = rtabmap::graph::findLink(linksRefined_, from ,to);
8030  if(findIter != linksRefined_.end())
8031  {
8032  link = findIter->second;
8033  }
8034  else
8035  {
8036  findIter = rtabmap::graph::findLink(linksAdded_, from ,to);
8037  if(findIter != linksAdded_.end())
8038  {
8039  link = findIter->second;
8040  }
8041  else if(!containsLink(linksRemoved_, from ,to))
8042  {
8043  findIter = rtabmap::graph::findLink(links_, from ,to);
8044  if(findIter != links_.end())
8045  {
8046  link = findIter->second;
8047  }
8048  }
8049  }
8050  return link;
8051 }
8052 
8053 bool DatabaseViewer::containsLink(std::multimap<int, Link> & links, int from, int to)
8054 {
8055  return rtabmap::graph::findLink(links, from, to) != links.end();
8056 }
8057 
8058 void DatabaseViewer::refineConstraint()
8059 {
8060  int from = ids_.at(ui_->horizontalSlider_A->value());
8061  int to = ids_.at(ui_->horizontalSlider_B->value());
8062  refineConstraint(from, to, false);
8063 }
8064 
8065 void DatabaseViewer::refineConstraint(int from, int to, bool silent)
8066 {
8067  UDEBUG("%d -> %d", from, to);
8068  bool switchedIds = false;
8069  if(from == to)
8070  {
8071  UWARN("Cannot refine link to same node");
8072  return;
8073  }
8074 
8075 
8076  Link currentLink = findActiveLink(from, to);
8077  if(!currentLink.isValid())
8078  {
8079  UERROR("Not found link! (%d->%d)", from, to);
8080  return;
8081  }
8082 
8083  UDEBUG("%d -> %d (type=%d)", currentLink.from(), currentLink.to(), currentLink.type());
8084  Transform t = currentLink.transform();
8085  if(ui_->checkBox_showOptimized->isChecked() &&
8086  (currentLink.type() == Link::kNeighbor || currentLink.type() == Link::kNeighborMerged) &&
8087  graphes_.size() &&
8088  (int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
8089  {
8090  std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
8091  if(currentLink.type() == Link::kNeighbor || currentLink.type() == Link::kNeighborMerged)
8092  {
8093  std::map<int, rtabmap::Transform>::iterator iterFrom = graph.find(currentLink.from());
8094  std::map<int, rtabmap::Transform>::iterator iterTo = graph.find(currentLink.to());
8095  if(iterFrom != graph.end() && iterTo != graph.end())
8096  {
8097  Transform topt = iterFrom->second.inverse()*iterTo->second;
8098  t = topt;
8099  }
8100  }
8101  }
8102  else if(ui_->checkBox_ignorePoseCorrection->isChecked() &&
8103  graph::findLink(linksRefined_, currentLink.from(), currentLink.to()) == linksRefined_.end())
8104  {
8105  if(currentLink.type() == Link::kNeighbor ||
8106  currentLink.type() == Link::kNeighborMerged)
8107  {
8108  Transform poseFrom = uValue(odomPoses_, currentLink.from(), Transform());
8109  Transform poseTo = uValue(odomPoses_, currentLink.to(), Transform());
8110  if(!poseFrom.isNull() && !poseTo.isNull())
8111  {
8112  t = poseFrom.inverse() * poseTo; // recompute raw odom transformation
8113  }
8114  }
8115  }
8116 
8117  Transform transform;
8118  RegistrationInfo info;
8119  Signature * fromS = 0;
8120  Signature * toS = 0;
8121 
8122  fromS = dbDriver_->loadSignature(currentLink.from());
8123  if(fromS == 0)
8124  {
8125  UERROR("Signature %d not found!", currentLink.from());
8126  return;
8127  }
8128 
8129  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
8130 
8131  UTimer timer;
8132 
8133  // Is it a multi-scan proximity detection?
8134  cv::Mat userData = currentLink.uncompressUserDataConst();
8135  std::map<int, rtabmap::Transform> scanPoses;
8136 
8137  if(currentLink.type() == Link::kLocalSpaceClosure &&
8138  !currentLink.userDataCompressed().empty() &&
8139  userData.type() == CV_8SC1 &&
8140  userData.rows == 1 &&
8141  userData.cols >= 8 && // including null str ending
8142  userData.at<char>(userData.cols-1) == 0 &&
8143  memcmp(userData.data, "SCANS:", 6) == 0 &&
8144  currentLink.from() > currentLink.to())
8145  {
8146  std::string scansStr = (const char *)userData.data;
8147  UINFO("Detected \"%s\" in links's user data", scansStr.c_str());
8148  if(!scansStr.empty())
8149  {
8150  std::list<std::string> strs = uSplit(scansStr, ':');
8151  if(strs.size() == 2)
8152  {
8153  std::list<std::string> strIds = uSplit(strs.rbegin()->c_str(), ';');
8154  for(std::list<std::string>::iterator iter=strIds.begin(); iter!=strIds.end(); ++iter)
8155  {
8156  int id = atoi(iter->c_str());
8157  if(uContains(odomPoses_, id))
8158  {
8159  scanPoses.insert(*odomPoses_.find(id));
8160  }
8161  else
8162  {
8163  UERROR("Not found %d node!", id);
8164  }
8165  }
8166  }
8167  }
8168  }
8169  if(scanPoses.size()>1)
8170  {
8171  //optimize the path's poses locally
8172  Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
8173 
8174  UASSERT(uContains(scanPoses, currentLink.to()));
8175  std::map<int, rtabmap::Transform> posesOut;
8176  std::multimap<int, rtabmap::Link> linksOut;
8177  optimizer->getConnectedGraph(
8178  currentLink.to(),
8179  scanPoses,
8180  updateLinksWithModifications(links_),
8181  posesOut,
8182  linksOut);
8183 
8184  if(scanPoses.size() != posesOut.size())
8185  {
8186  UWARN("Scan poses input and output are different! %d vs %d", (int)scanPoses.size(), (int)posesOut.size());
8187  UWARN("Input poses: ");
8188  for(std::map<int, Transform>::iterator iter=scanPoses.begin(); iter!=scanPoses.end(); ++iter)
8189  {
8190  UWARN(" %d", iter->first);
8191  }
8192  UWARN("Input links: ");
8193  std::multimap<int, Link> modifiedLinks = updateLinksWithModifications(links_);
8194  for(std::multimap<int, Link>::iterator iter=modifiedLinks.begin(); iter!=modifiedLinks.end(); ++iter)
8195  {
8196  UWARN(" %d->%d", iter->second.from(), iter->second.to());
8197  }
8198  }
8199 
8200  scanPoses = optimizer->optimize(currentLink.to(), posesOut, linksOut);
8201  delete optimizer;
8202 
8203  std::map<int, Transform> filteredScanPoses = scanPoses;
8204  float proximityFilteringRadius = 0.0f;
8205  Parameters::parse(parameters, Parameters::kRGBDProximityPathFilteringRadius(), proximityFilteringRadius);
8206  if(scanPoses.size() > 2 && proximityFilteringRadius > 0.0f)
8207  {
8208  // path filtering
8209  filteredScanPoses = graph::radiusPosesFiltering(scanPoses, proximityFilteringRadius, 0, true);
8210  // make sure the current pose is still here
8211  filteredScanPoses.insert(*scanPoses.find(currentLink.to()));
8212  }
8213 
8214  Transform toPoseInv = filteredScanPoses.at(currentLink.to()).inverse();
8215  dbDriver_->loadNodeData(fromS, !silent, true, !silent, !silent);
8216  fromS->sensorData().uncompressData();
8217  LaserScan fromScan = fromS->sensorData().laserScanRaw();
8218  int maxPoints = fromScan.size();
8219  if(maxPoints == 0)
8220  {
8221  UWARN("From scan %d is empty!", fromS->id());
8222  }
8223  pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
8224  pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(new pcl::PointCloud<pcl::PointNormal>);
8225  pcl::PointCloud<pcl::PointXYZI>::Ptr assembledToIClouds(new pcl::PointCloud<pcl::PointXYZI>);
8226  pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembledToNormalIClouds(new pcl::PointCloud<pcl::PointXYZINormal>);
8227  pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledToRGBClouds(new pcl::PointCloud<pcl::PointXYZRGB>);
8228  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledToNormalRGBClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
8229  for(std::map<int, Transform>::const_iterator iter = filteredScanPoses.begin(); iter!=filteredScanPoses.end(); ++iter)
8230  {
8231  if(iter->first != currentLink.from())
8232  {
8233  SensorData data;
8234  dbDriver_->getNodeData(iter->first, data);
8235  if(!data.laserScanCompressed().isEmpty())
8236  {
8237  LaserScan scan;
8238  data.uncompressData(0, 0, &scan);
8239  if(!scan.isEmpty() && fromScan.format() == scan.format())
8240  {
8241  if(scan.hasIntensity())
8242  {
8243  if(scan.hasNormals())
8244  {
8245  *assembledToNormalIClouds += *util3d::laserScanToPointCloudINormal(scan,
8246  toPoseInv * iter->second * scan.localTransform());
8247  }
8248  else
8249  {
8250  *assembledToIClouds += *util3d::laserScanToPointCloudI(scan,
8251  toPoseInv * iter->second * scan.localTransform());
8252  }
8253  }
8254  else if(scan.hasRGB())
8255  {
8256  if(scan.hasNormals())
8257  {
8258  *assembledToNormalRGBClouds += *util3d::laserScanToPointCloudRGBNormal(scan,
8259  toPoseInv * iter->second * scan.localTransform());
8260  }
8261  else
8262  {
8263  *assembledToRGBClouds += *util3d::laserScanToPointCloudRGB(scan,
8264  toPoseInv * iter->second * scan.localTransform());
8265  }
8266  }
8267  else
8268  {
8269  if(scan.hasNormals())
8270  {
8271  *assembledToNormalClouds += *util3d::laserScanToPointCloudNormal(scan,
8272  toPoseInv * iter->second * scan.localTransform());
8273  }
8274  else
8275  {
8276  *assembledToClouds += *util3d::laserScanToPointCloud(scan,
8277  toPoseInv * iter->second * scan.localTransform());
8278  }
8279  }
8280 
8281  if(scan.size() > maxPoints)
8282  {
8283  maxPoints = scan.size();
8284  }
8285  }
8286  else
8287  {
8288  UWARN("scan format of %d is not the same than from scan %d: %d vs %d", data.id(), fromS->id(), scan.format(), fromScan.format());
8289  }
8290  }
8291  else
8292  {
8293  UWARN("Laser scan not found for signature %d", iter->first);
8294  }
8295  }
8296  }
8297 
8298  LaserScan assembledScan;
8299  if(assembledToNormalClouds->size())
8300  {
8301  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToNormalClouds):util3d::laserScanFromPointCloud(*assembledToNormalClouds);
8302  }
8303  else if(assembledToClouds->size())
8304  {
8305  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToClouds):util3d::laserScanFromPointCloud(*assembledToClouds);
8306  }
8307  else if(assembledToNormalIClouds->size())
8308  {
8309  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToNormalIClouds):util3d::laserScanFromPointCloud(*assembledToNormalIClouds);
8310  }
8311  else if(assembledToIClouds->size())
8312  {
8313  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToIClouds):util3d::laserScanFromPointCloud(*assembledToIClouds);
8314  }
8315  else if(assembledToNormalRGBClouds->size())
8316  {
8317  UASSERT(!fromScan.is2d());
8318  assembledScan = util3d::laserScanFromPointCloud(*assembledToNormalRGBClouds);
8319  }
8320  else if(assembledToRGBClouds->size())
8321  {
8322  UASSERT(!fromScan.is2d());
8323  assembledScan = util3d::laserScanFromPointCloud(*assembledToRGBClouds);
8324  }
8325  else
8326  {
8327  UWARN("Assembled scan is empty!");
8328  }
8329  SensorData assembledData(cv::Mat(), to);
8330  // scans are in base frame but for 2d scans, set the height so that correspondences matching works
8331  assembledData.setLaserScan(LaserScan(
8332  assembledScan,
8333  fromScan.maxPoints()?fromScan.maxPoints():maxPoints,
8334  fromScan.rangeMax(),
8335  fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
8336 
8337  toS = new Signature(assembledData);
8338  RegistrationIcp registrationIcp(parameters);
8339  transform = registrationIcp.computeTransformationMod(*fromS, *toS, currentLink.transform(), &info);
8340  if(!transform.isNull())
8341  {
8342  // local scan matching proximity detection should have higher variance (see Rtabmap::process())
8343  info.covariance*=100.0;
8344  }
8345  }
8346  else
8347  {
8348  toS = dbDriver_->loadSignature(currentLink.to());
8349  if(toS == 0)
8350  {
8351  UERROR("Signature %d not found!", currentLink.to());
8352  delete fromS;
8353  return;
8354  }
8355 
8356  bool reextractVisualFeatures = uStr2Bool(parameters.at(Parameters::kRGBDLoopClosureReextractFeatures()));
8357  Registration * reg = Registration::create(parameters);
8358  if( reg->isScanRequired() ||
8359  reg->isUserDataRequired() ||
8360  reextractVisualFeatures ||
8361  !silent)
8362  {
8363  dbDriver_->loadNodeData(fromS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
8364  dbDriver_->loadNodeData(toS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
8365 
8366  if(!silent)
8367  {
8368  fromS->sensorData().uncompressData();
8369  toS->sensorData().uncompressData();
8370  }
8371  }
8372 
8373  if(reextractVisualFeatures)
8374  {
8375  fromS->removeAllWords();
8376  fromS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
8377  toS->removeAllWords();
8378  toS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
8379  }
8380 
8381  if(reg->isScanRequired())
8382  {
8383  if(ui_->checkBox_icp_from_depth->isChecked())
8384  {
8385  // generate laser scans from depth image
8386  cv::Mat tmpA, tmpB, tmpC, tmpD;
8387  fromS->sensorData().uncompressData(&tmpA, &tmpB, 0);
8388  toS->sensorData().uncompressData(&tmpC, &tmpD, 0);
8389  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom = util3d::cloudFromSensorData(
8390  fromS->sensorData(),
8391  ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
8392  ui_->doubleSpinBox_icp_maxDepth->value(),
8393  ui_->doubleSpinBox_icp_minDepth->value(),
8394  0,
8395  ui_->parameters_toolbox->getParameters());
8396  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo = util3d::cloudFromSensorData(
8397  toS->sensorData(),
8398  ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
8399  ui_->doubleSpinBox_icp_maxDepth->value(),
8400  ui_->doubleSpinBox_icp_minDepth->value(),
8401  0,
8402  ui_->parameters_toolbox->getParameters());
8403 
8404  if(cloudFrom->empty() && cloudTo->empty())
8405  {
8406  std::string msg = "Option to generate scan from depth is checked (GUI Parameters->Refine), but "
8407  "resulting clouds from depth are empty. Transformation estimation will likely "
8408  "fails. Uncheck the parameter to use laser scans.";
8409  UWARN(msg.c_str());
8410  if(!silent)
8411  {
8412  QMessageBox::warning(this,
8413  tr("Refine link"),
8414  tr("%1").arg(msg.c_str()));
8415  }
8416  }
8417  else if(!fromS->sensorData().laserScanCompressed().isEmpty() || !toS->sensorData().laserScanCompressed().isEmpty())
8418  {
8419  UWARN("There are laser scans in data, but generate laser scan from "
8420  "depth image option is activated (GUI Parameters->Refine). "
8421  "Ignoring saved laser scans...");
8422  }
8423 
8424  int maxLaserScans = cloudFrom->size();
8425  fromS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0));
8426  toS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0));
8427  }
8428  else
8429  {
8430  LaserScan tmpA, tmpB;
8431  fromS->sensorData().uncompressData(0, 0, &tmpA);
8432  toS->sensorData().uncompressData(0, 0, &tmpB);
8433  }
8434  }
8435 
8436  if(reg->isImageRequired() && reextractVisualFeatures)
8437  {
8438  cv::Mat tmpA, tmpB, tmpC, tmpD;
8439  fromS->sensorData().uncompressData(&tmpA, &tmpB, 0);
8440  toS->sensorData().uncompressData(&tmpC, &tmpD, 0);
8441  }
8442 
8443  UINFO("Uncompress time: %f s", timer.ticks());
8444 
8445  if(fromS->id() < toS->id())
8446  {
8447  transform = reg->computeTransformationMod(*fromS, *toS, t, &info);
8448  }
8449  else
8450  {
8451  transform = reg->computeTransformationMod(*toS, *fromS, t.isNull()?t:t.inverse(), &info);
8452  switchedIds = true;
8453  }
8454 
8455  delete reg;
8456  }
8457  UINFO("(%d ->%d) Registration time: %f s", currentLink.from(), currentLink.to(), timer.ticks());
8458 
8459  if(!transform.isNull())
8460  {
8461  if(!transform.isIdentity())
8462  {
8463  if(info.covariance.at<double>(0,0)<=0.0)
8464  {
8465  info.covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
8466  }
8467  }
8468  if(switchedIds)
8469  {
8470  transform = transform.inverse();
8471  }
8472 
8473  Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform, info.covariance.inv(), currentLink.userDataCompressed());
8474 
8475  bool updated = false;
8476  std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
8477  while(iter != linksRefined_.end() && iter->first == currentLink.from())
8478  {
8479  if(iter->second.to() == currentLink.to() &&
8480  iter->second.type() == currentLink.type())
8481  {
8482  iter->second = newLink;
8483  updated = true;
8484  break;
8485  }
8486  ++iter;
8487  }
8488  if(!updated)
8489  {
8490  linksRefined_.insert(std::make_pair(newLink.from(), newLink));
8491  updated = true;
8492  }
8493 
8494  if(updated && !silent)
8495  {
8496  this->updateGraphView();
8497  }
8498 
8499  if(!silent && ui_->dockWidget_constraints->isVisible())
8500  {
8501  if(toS && fromS->id() > 0 && toS->id() > 0)
8502  {
8503  updateLoopClosuresSlider(fromS->id(), toS->id());
8504  std::multimap<int, cv::KeyPoint> keypointsFrom;
8505  std::multimap<int, cv::KeyPoint> keypointsTo;
8506  if(!fromS->getWordsKpts().empty())
8507  {
8508  for(std::map<int, int>::const_iterator iter=fromS->getWords().begin(); iter!=fromS->getWords().end(); ++iter)
8509  {
8510  keypointsFrom.insert(keypointsFrom.end(), std::make_pair(iter->first, fromS->getWordsKpts()[iter->second]));
8511  }
8512  }
8513  if(!toS->getWordsKpts().empty())
8514  {
8515  for(std::map<int, int>::const_iterator iter=toS->getWords().begin(); iter!=toS->getWords().end(); ++iter)
8516  {
8517  keypointsTo.insert(keypointsTo.end(), std::make_pair(iter->first, toS->getWordsKpts()[iter->second]));
8518  }
8519  }
8520  if(newLink.type() != Link::kNeighbor && fromS->id() < toS->id())
8521  {
8522  this->updateConstraintView(newLink.inverse(), true, *toS, *fromS);
8523  ui_->graphicsView_A->setFeatures(keypointsTo, toS->sensorData().depthRaw());
8524  ui_->graphicsView_B->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
8525  }
8526  else
8527  {
8528  this->updateConstraintView(newLink, true, *fromS, *toS);
8529  ui_->graphicsView_A->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
8530  ui_->graphicsView_B->setFeatures(keypointsTo, toS->sensorData().depthRaw());
8531  }
8532 
8533  updateWordsMatching(info.inliersIDs);
8534  }
8535  else
8536  {
8537  this->updateConstraintView();
8538  }
8539  }
8540  }
8541  else if(!silent)
8542  {
8543  if(toS && fromS->id() > 0 && toS->id() > 0)
8544  {
8545  // just update matches in the views
8546  std::multimap<int, cv::KeyPoint> keypointsFrom;
8547  std::multimap<int, cv::KeyPoint> keypointsTo;
8548  if(!fromS->getWordsKpts().empty())
8549  {
8550  for(std::map<int, int>::const_iterator iter=fromS->getWords().begin(); iter!=fromS->getWords().end(); ++iter)
8551  {
8552  keypointsFrom.insert(keypointsFrom.end(), std::make_pair(iter->first, fromS->getWordsKpts()[iter->second]));
8553  }
8554  }
8555  if(!toS->getWordsKpts().empty())
8556  {
8557  for(std::map<int, int>::const_iterator iter=toS->getWords().begin(); iter!=toS->getWords().end(); ++iter)
8558  {
8559  keypointsTo.insert(keypointsTo.end(), std::make_pair(iter->first, toS->getWordsKpts()[iter->second]));
8560  }
8561  }
8562  if(currentLink.type() != Link::kNeighbor && fromS->id() < toS->id())
8563  {
8564  ui_->graphicsView_A->setFeatures(keypointsTo, toS->sensorData().depthRaw());
8565  ui_->graphicsView_B->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
8566  }
8567  else
8568  {
8569  ui_->graphicsView_A->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
8570  ui_->graphicsView_B->setFeatures(keypointsTo, toS->sensorData().depthRaw());
8571  }
8572  updateWordsMatching(info.inliersIDs);
8573  }
8574 
8575  QMessageBox::warning(this,
8576  tr("Refine link"),
8577  tr("Cannot find a transformation between nodes %1 and %2: %3").arg(currentLink.from()).arg(currentLink.to()).arg(info.rejectedMsg.c_str()));
8578  }
8579  delete fromS;
8580  delete toS;
8581 }
8582 
8583 void DatabaseViewer::addConstraint()
8584 {
8585  int from = ids_.at(ui_->horizontalSlider_A->value());
8586  int to = ids_.at(ui_->horizontalSlider_B->value());
8587  addConstraint(from, to, false);
8588 }
8589 
8590 bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool silentlyUseOptimizedGraphAsGuess)
8591 {
8592  bool switchedIds = false;
8593  if(from == to)
8594  {
8595  UWARN("Cannot add link to same node");
8596  return false;
8597  }
8598  else if(from > to)
8599  {
8600  int tmp = from;
8601  from = to;
8602  to = tmp;
8603  switchedIds = true;
8604  }
8605  std::list<Signature*> signatures;
8606  Signature * fromS=0;
8607  Signature * toS=0;
8608 
8609  Link newLink;
8610  RegistrationInfo info;
8611  if(!containsLink(linksAdded_, from, to) &&
8612  !containsLink(links_, from, to))
8613  {
8614  UASSERT(!containsLink(linksRemoved_, from, to));
8615  UASSERT(!containsLink(linksRefined_, from, to));
8616 
8617  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
8618  Registration * reg = Registration::create(parameters);
8619 
8620  bool loopCovLimited = Parameters::defaultRGBDLoopCovLimited();
8621  Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), loopCovLimited);
8622  std::vector<double> odomMaxInf = odomMaxInf_;
8623  if(loopCovLimited && odomMaxInf_.empty())
8624  {
8625  odomMaxInf = graph::getMaxOdomInf(updateLinksWithModifications(links_));
8626  }
8627 
8628  Transform t;
8629 
8630  std::list<int> ids;
8631  ids.push_back(from);
8632  ids.push_back(to);
8633  dbDriver_->loadSignatures(ids, signatures);
8634  if(signatures.size() != 2)
8635  {
8636  for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
8637  {
8638  delete *iter;
8639  }
8640  return false;
8641  }
8642  fromS = *signatures.begin();
8643  toS = *signatures.rbegin();
8644 
8645  bool reextractVisualFeatures = uStr2Bool(parameters.at(Parameters::kRGBDLoopClosureReextractFeatures()));
8646  if(reg->isScanRequired() ||
8647  reg->isUserDataRequired() ||
8648  reextractVisualFeatures ||
8649  !silent)
8650  {
8651  // Add sensor data to generate features
8652  dbDriver_->loadNodeData(fromS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
8653  fromS->sensorData().uncompressData();
8654  dbDriver_->loadNodeData(toS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
8655  toS->sensorData().uncompressData();
8656  if(reextractVisualFeatures)
8657  {
8658  fromS->removeAllWords();
8659  fromS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
8660  toS->removeAllWords();
8661  toS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
8662  }
8663  if(reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked())
8664  {
8665  // generate laser scans from depth image
8666  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom = util3d::cloudFromSensorData(
8667  fromS->sensorData(),
8668  ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
8669  ui_->doubleSpinBox_icp_maxDepth->value(),
8670  ui_->doubleSpinBox_icp_minDepth->value(),
8671  0,
8672  ui_->parameters_toolbox->getParameters());
8673  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo = util3d::cloudFromSensorData(
8674  toS->sensorData(),
8675  ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
8676  ui_->doubleSpinBox_icp_maxDepth->value(),
8677  ui_->doubleSpinBox_icp_minDepth->value(),
8678  0,
8679  ui_->parameters_toolbox->getParameters());
8680 
8681  if(cloudFrom->empty() && cloudTo->empty())
8682  {
8683  std::string msg = "Option to generate scan from depth is checked (GUI Parameters->Refine), but "
8684  "resulting clouds from depth are empty. Transformation estimation will likely "
8685  "fails. Uncheck the parameter to use laser scans.";
8686  UWARN(msg.c_str());
8687  if(!silent)
8688  {
8689  QMessageBox::warning(this,
8690  tr("Add link"),
8691  tr("%1").arg(msg.c_str()));
8692  }
8693  }
8694  else if(!fromS->sensorData().laserScanCompressed().isEmpty() || !toS->sensorData().laserScanCompressed().isEmpty())
8695  {
8696  UWARN("There are laser scans in data, but generate laser scan from "
8697  "depth image option is activated (GUI Parameters->Refine). Ignoring saved laser scans...");
8698  }
8699 
8700  int maxLaserScans = cloudFrom->size();
8701  fromS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0));
8702  toS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0));
8703  }
8704  }
8705  else if(!reextractVisualFeatures && fromS->getWords().empty() && toS->getWords().empty())
8706  {
8707  std::string msg = uFormat("\"%s\" is false and signatures (%d and %d) don't have words, "
8708  "registration will not be possible. Set \"%s\" to true.",
8709  Parameters::kRGBDLoopClosureReextractFeatures().c_str(),
8710  fromS->id(),
8711  toS->id(),
8712  Parameters::kRGBDLoopClosureReextractFeatures().c_str());
8713  UWARN(msg.c_str());
8714  if(!silent)
8715  {
8716  QMessageBox::warning(this,
8717  tr("Add link"),
8718  tr("%1").arg(msg.c_str()));
8719  }
8720  }
8721 
8722  Transform guess;
8723  bool guessFromGraphRejected = false;
8724  if(!reg->isImageRequired())
8725  {
8726  // make a fake guess using globally optimized poses
8727  if(graphes_.size())
8728  {
8729  std::map<int, Transform> optimizedPoses = graphes_.back();
8730  if(optimizedPoses.size() > 0)
8731  {
8732  std::map<int, Transform>::iterator fromIter = optimizedPoses.find(from);
8733  std::map<int, Transform>::iterator toIter = optimizedPoses.find(to);
8734  if(fromIter != optimizedPoses.end() &&
8735  toIter != optimizedPoses.end())
8736  {
8737  if(!silent)
8738  {
8739  if(QMessageBox::question(this,
8740  tr("Add constraint from optimized graph"),
8741  tr("Registration is done without vision (see %1 parameter), "
8742  "do you want to use a guess from the optimized graph?"
8743  ""
8744  "\n\nOtherwise, if "
8745  "the database has images, it is recommended to use %2=2 instead so that "
8746  "the guess can be found visually.")
8747  .arg(Parameters::kRegStrategy().c_str()).arg(Parameters::kRegStrategy().c_str()),
8748  QMessageBox::Yes | QMessageBox::No,
8749  QMessageBox::Yes) == QMessageBox::Yes)
8750  {
8751  guess = fromIter->second.inverse() * toIter->second;
8752  }
8753  else
8754  {
8755  guessFromGraphRejected = true;
8756  }
8757  }
8758  else if(silentlyUseOptimizedGraphAsGuess)
8759  {
8760  guess = fromIter->second.inverse() * toIter->second;
8761  }
8762  }
8763  }
8764  }
8765  if(guess.isNull() && !silent && !guessFromGraphRejected)
8766  {
8767  if(QMessageBox::question(this,
8768  tr("Add constraint without guess"),
8769  tr("Registration is done without vision (see %1 parameter) and we cannot (or we don't want to) find relative "
8770  "transformation between the nodes with the current graph. Do you want to use an identity "
8771  "transform for ICP guess? "
8772  ""
8773  "\n\nOtherwise, if the database has images, it is recommended to use %2=2 "
8774  "instead so that the guess can be found visually.")
8775  .arg(Parameters::kRegStrategy().c_str()).arg(Parameters::kRegStrategy().c_str()),
8776  QMessageBox::Yes | QMessageBox::Abort,
8777  QMessageBox::Abort) == QMessageBox::Yes)
8778  {
8779  guess.setIdentity();
8780  }
8781  else
8782  {
8783  guessFromGraphRejected = true;
8784  }
8785  }
8786  }
8787 
8788  if(switchedIds)
8789  {
8790  t = reg->computeTransformationMod(*toS, *fromS, guess.isNull()?guess:guess.inverse(), &info);
8791  if(!t.isNull())
8792  {
8793  t = t.inverse();
8794  }
8795  }
8796  else
8797  {
8798  t = reg->computeTransformationMod(*fromS, *toS, guess, &info);
8799  }
8800  delete reg;
8801  UDEBUG("");
8802 
8803  if(!t.isNull())
8804  {
8805  cv::Mat information = info.covariance.inv();
8806  if(odomMaxInf.size() == 6 && information.cols==6 && information.rows==6)
8807  {
8808  for(int i=0; i<6; ++i)
8809  {
8810  if(information.at<double>(i,i) > odomMaxInf[i])
8811  {
8812  information.at<double>(i,i) = odomMaxInf[i];
8813  }
8814  }
8815  }
8816 
8817  newLink = Link(from, to, Link::kUserClosure, t, information);
8818  }
8819  else if(!silent && !guessFromGraphRejected)
8820  {
8821  QMessageBox::StandardButton button = QMessageBox::warning(this,
8822  tr("Add link"),
8823  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()),
8824  QMessageBox::Yes | QMessageBox::No,
8825  QMessageBox::No);
8826  if(button == QMessageBox::Yes)
8827  {
8828  editConstraint();
8829  silent = true;
8830  }
8831  }
8832  }
8833  else if(containsLink(linksRemoved_, from, to))
8834  {
8835  newLink = rtabmap::graph::findLink(linksRemoved_, from, to)->second;
8836  }
8837 
8838  bool updateConstraints = newLink.isValid();
8839  float maxOptimizationError = uStr2Float(ui_->parameters_toolbox->getParameters().at(Parameters::kRGBDOptimizeMaxError()));
8840  if(newLink.isValid() &&
8841  maxOptimizationError > 0.0f &&
8842  uStr2Int(ui_->parameters_toolbox->getParameters().at(Parameters::kOptimizerIterations())) > 0.0f)
8843  {
8844  int fromId = newLink.from();
8845  std::multimap<int, Link> linksIn = updateLinksWithModifications(links_);
8846  linksIn.insert(std::make_pair(newLink.from(), newLink));
8847  const Link * maxLinearLink = 0;
8848  const Link * maxAngularLink = 0;
8849  float maxLinearErrorRatio = 0.0f;
8850  float maxAngularErrorRatio = 0.0f;
8851  Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
8852  std::map<int, Transform> poses;
8853  std::multimap<int, Link> links;
8854  UASSERT(odomPoses_.find(fromId) != odomPoses_.end());
8855  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());
8856  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());
8857  optimizer->getConnectedGraph(fromId, odomPoses_, linksIn, poses, links);
8858  // use already optimized poses
8859  if(graphes_.size())
8860  {
8861  const std::map<int, Transform> & optimizedPoses = graphes_.back();
8862  for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
8863  {
8864  if(optimizedPoses.find(iter->first) != optimizedPoses.end())
8865  {
8866  iter->second = optimizedPoses.at(iter->first);
8867  }
8868  }
8869  }
8870  UASSERT(poses.find(fromId) != poses.end());
8871  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());
8872  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());
8873  UASSERT(graph::findLink(links, newLink.from(), newLink.to()) != links.end());
8874  std::map<int, Transform> posesIn = poses;
8875  poses = optimizer->optimize(fromId, posesIn, links);
8876  if(posesIn.size() && poses.empty())
8877  {
8878  UWARN("Optimization failed... (poses=%d, links=%d).", (int)posesIn.size(), (int)links.size());
8879  }
8880  std::string msg;
8881  if(poses.size())
8882  {
8883  float maxLinearError = 0.0f;
8884  float maxAngularError = 0.0f;
8885  graph::computeMaxGraphErrors(
8886  poses,
8887  links,
8888  maxLinearErrorRatio,
8889  maxAngularErrorRatio,
8890  maxLinearError,
8891  maxAngularError,
8892  &maxLinearLink,
8893  &maxAngularLink);
8894  if(maxLinearLink)
8895  {
8896  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()));
8897  if(maxLinearErrorRatio > maxOptimizationError)
8898  {
8899  msg = uFormat("Rejecting edge %d->%d because "
8900  "graph error is too large (abs=%f m) after optimization (ratio %f for edge %d->%d, stddev=%f m). "
8901  "\"%s\" is %f.",
8902  newLink.from(),
8903  newLink.to(),
8904  maxLinearError,
8905  maxLinearErrorRatio,
8906  maxLinearLink->from(),
8907  maxLinearLink->to(),
8908  sqrt(maxLinearLink->transVariance()),
8909  Parameters::kRGBDOptimizeMaxError().c_str(),
8910  maxOptimizationError);
8911  }
8912  }
8913  if(maxAngularLink)
8914  {
8915  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()));
8916  if(maxAngularErrorRatio > maxOptimizationError)
8917  {
8918  msg = uFormat("Rejecting edge %d->%d because "
8919  "graph error is too large (abs=%f deg) after optimization (ratio %f for edge %d->%d, stddev=%f deg). "
8920  "\"%s\" is %f.",
8921  newLink.from(),
8922  newLink.to(),
8923  maxAngularError*180.0f/CV_PI,
8924  maxAngularErrorRatio,
8925  maxAngularLink->from(),
8926  maxAngularLink->to(),
8927  sqrt(maxAngularLink->rotVariance()),
8928  Parameters::kRGBDOptimizeMaxError().c_str(),
8929  maxOptimizationError);
8930  }
8931  }
8932  }
8933  else
8934  {
8935  msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
8936  newLink.from(),
8937  newLink.to());
8938  }
8939  if(!msg.empty())
8940  {
8941  UWARN("%s", msg.c_str());
8942  if(!silent)
8943  {
8944  QMessageBox::warning(this,
8945  tr("Add link"),
8946  tr("%1").arg(msg.c_str()));
8947  }
8948 
8949  updateConstraints = false;
8950  }
8951 
8952  if(updateConstraints && silent && !graphes_.empty() && graphes_.back().size() == poses.size())
8953  {
8954  graphes_.back() = poses;
8955  }
8956  }
8957 
8958  if(updateConstraints)
8959  {
8960  if(containsLink(linksRemoved_, from, to))
8961  {
8962  //simply remove from linksRemoved
8963  linksRemoved_.erase(rtabmap::graph::findLink(linksRemoved_, from, to));
8964  }
8965  else
8966  {
8967  if(newLink.from() < newLink.to())
8968  {
8969  newLink = newLink.inverse();
8970  }
8971  linksAdded_.insert(std::make_pair(newLink.from(), newLink));
8972  }
8973  }
8974 
8975  if(!silent)
8976  {
8977  if(fromS && toS)
8978  {
8979  if((updateConstraints && newLink.from() > newLink.to()) || (!updateConstraints && switchedIds))
8980  {
8981  Signature * tmpS = fromS;
8982  fromS = toS;
8983  toS = tmpS;
8984  }
8985 
8986  if(updateConstraints)
8987  {
8988  updateLoopClosuresSlider(fromS->id(), toS->id());
8989  this->updateGraphView();
8990  this->updateConstraintView(newLink, false, *fromS, *toS);
8991  }
8992 
8993  std::multimap<int, cv::KeyPoint> keypointsFrom;
8994  std::multimap<int, cv::KeyPoint> keypointsTo;
8995  if(!fromS->getWordsKpts().empty())
8996  {
8997  for(std::map<int, int>::const_iterator iter=fromS->getWords().begin(); iter!=fromS->getWords().end(); ++iter)
8998  {
8999  keypointsFrom.insert(keypointsFrom.end(), std::make_pair(iter->first, fromS->getWordsKpts()[iter->second]));
9000  }
9001  }
9002  if(!toS->getWordsKpts().empty())
9003  {
9004  for(std::map<int, int>::const_iterator iter=toS->getWords().begin(); iter!=toS->getWords().end(); ++iter)
9005  {
9006  keypointsTo.insert(keypointsTo.end(), std::make_pair(iter->first, toS->getWordsKpts()[iter->second]));
9007  }
9008  }
9009  ui_->graphicsView_A->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
9010  ui_->graphicsView_B->setFeatures(keypointsTo, toS->sensorData().depthRaw());
9011  updateWordsMatching(info.inliersIDs);
9012  }
9013  else if(updateConstraints)
9014  {
9015  updateLoopClosuresSlider(from, to);
9016  this->updateGraphView();
9017  }
9018  }
9019 
9020  for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
9021  {
9022  delete *iter;
9023  }
9024 
9025  return updateConstraints;
9026 }
9027 
9028 void DatabaseViewer::resetConstraint()
9029 {
9030  int from = ids_.at(ui_->horizontalSlider_A->value());
9031  int to = ids_.at(ui_->horizontalSlider_B->value());
9032  if(ui_->label_type->text().toInt() == Link::kLandmark)
9033  {
9034  int position = ui_->horizontalSlider_loops->value();
9035  const rtabmap::Link & link = loopLinks_.at(position);
9036  from = link.from();
9037  to = link.to();
9038  }
9039 
9040  if(from < to)
9041  {
9042  int tmp = to;
9043  to = from;
9044  from = tmp;
9045  }
9046 
9047  if(from == to)
9048  {
9049  UWARN("Cannot reset link to same node");
9050  return;
9051  }
9052 
9053 
9054  std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, from, to);
9055  if(iter != linksRefined_.end())
9056  {
9057  linksRefined_.erase(iter);
9058  this->updateGraphView();
9059  }
9060 
9061  updateConstraintView();
9062 }
9063 
9064 void DatabaseViewer::rejectConstraint()
9065 {
9066  int priorId = sender() == ui_->toolButton_remove_priorA?ids_.at(ui_->horizontalSlider_A->value()):
9067  sender() == ui_->toolButton_remove_priorB?ids_.at(ui_->horizontalSlider_B->value()):0;
9068 
9069  int from = priorId>0?priorId:ids_.at(ui_->horizontalSlider_A->value());
9070  int to = priorId>0?priorId:ids_.at(ui_->horizontalSlider_B->value());
9071  if(priorId==0 && ui_->label_type->text().toInt() == Link::kLandmark)
9072  {
9073  int position = ui_->horizontalSlider_loops->value();
9074  const rtabmap::Link & link = loopLinks_.at(position);
9075  from = link.from();
9076  to = link.to();
9077  }
9078 
9079  if(from < to)
9080  {
9081  int tmp = to;
9082  to = from;
9083  from = tmp;
9084  }
9085 
9086  if(priorId==0 && from == to)
9087  {
9088  UWARN("Cannot reject link to same node");
9089  return;
9090  }
9091 
9092  bool removed = false;
9093 
9094  // find the original one
9095  std::multimap<int, Link>::iterator iter;
9096  iter = rtabmap::graph::findLink(links_, from, to);
9097  if(iter != links_.end())
9098  {
9099  if(iter->second.type() == Link::kNeighbor || iter->second.type() == Link::kNeighborMerged)
9100  {
9101  QMessageBox::StandardButton button = QMessageBox::warning(this, tr("Reject link"),
9102  tr("Removing the neighbor link %1->%2 will split the graph. Do you want to continue?").arg(from).arg(to),
9103  QMessageBox::Yes | QMessageBox::No, QMessageBox::No);
9104  if(button != QMessageBox::Yes)
9105  {
9106  return;
9107  }
9108  }
9109  linksRemoved_.insert(*iter);
9110  removed = true;
9111  }
9112 
9113  // remove from refined and added
9114  iter = rtabmap::graph::findLink(linksRefined_, from, to);
9115  if(iter != linksRefined_.end())
9116  {
9117  linksRefined_.erase(iter);
9118  removed = true;
9119  }
9120  iter = rtabmap::graph::findLink(linksAdded_, from, to);
9121  if(iter != linksAdded_.end())
9122  {
9123  linksAdded_.erase(iter);
9124  removed = true;
9125  }
9126  if(removed)
9127  {
9128  if(priorId==0)
9129  {
9130  this->updateGraphView();
9131  }
9132  else
9133  {
9134  bool priorsIgnored = Parameters::defaultOptimizerPriorsIgnored();
9135  Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kOptimizerPriorsIgnored(), priorsIgnored);
9136  int indexA = ui_->horizontalSlider_A->value();
9137  int indexB = ui_->horizontalSlider_B->value();
9138  if(!priorsIgnored)
9139  {
9140  this->updateGraphView();
9141  }
9142  if(ui_->horizontalSlider_A->value() != indexA)
9143  ui_->horizontalSlider_A->setValue(indexA);
9144  else
9145  sliderAValueChanged(indexA);
9146  if(ui_->horizontalSlider_B->value() != indexB)
9147  ui_->horizontalSlider_B->setValue(indexB);
9148  else
9149  sliderBValueChanged(indexB);
9150  }
9151  }
9152 }
9153 
9154 std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
9155  const std::multimap<int, rtabmap::Link> & edgeConstraints)
9156 {
9157  UDEBUG("linksAdded_=%d linksRefined_=%d linksRemoved_=%d", (int)linksAdded_.size(), (int)linksRefined_.size(), (int)linksRemoved_.size());
9158 
9159  std::multimap<int, rtabmap::Link> links;
9160  for(std::multimap<int, rtabmap::Link>::const_iterator iter=edgeConstraints.begin();
9161  iter!=edgeConstraints.end();
9162  ++iter)
9163  {
9164  std::multimap<int, rtabmap::Link>::iterator findIter;
9165 
9166  findIter = rtabmap::graph::findLink(linksRemoved_, iter->second.from(), iter->second.to());
9167  if(findIter != linksRemoved_.end())
9168  {
9169  UDEBUG("Removed link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
9170  continue; // don't add this link
9171  }
9172 
9173  findIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
9174  if(findIter!=linksRefined_.end())
9175  {
9176  // add the refined link
9177  if(iter->second.from() == findIter->second.to() &&
9178  iter->second.from() != iter->second.to())
9179  {
9180  links.insert(std::make_pair(iter->second.from(), findIter->second.inverse()));
9181  }
9182  else
9183  {
9184  links.insert(*findIter);
9185  }
9186  UDEBUG("Updated link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
9187  continue;
9188  }
9189 
9190  links.insert(*iter); // add original link
9191  }
9192 
9193  //look for added links
9194  for(std::multimap<int, rtabmap::Link>::const_iterator iter=linksAdded_.begin();
9195  iter!=linksAdded_.end();
9196  ++iter)
9197  {
9198  std::multimap<int, rtabmap::Link>::iterator findIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
9199  if(findIter!=linksRefined_.end())
9200  {
9201  links.insert(*findIter); // add the refined link
9202  links.insert(std::make_pair(findIter->second.to(), findIter->second.inverse())); // return both ways
9203  UDEBUG("Added refined link (%d->%d, %d)", findIter->second.from(), findIter->second.to(), findIter->second.type());
9204  continue;
9205  }
9206 
9207  UDEBUG("Added link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
9208  links.insert(*iter);
9209  links.insert(std::make_pair(iter->second.to(), iter->second.inverse())); // return both ways
9210  }
9211 
9212  return links;
9213 }
9214 
9215 void DatabaseViewer::updateNeighborsSlider(int from, int to)
9216 {
9217  UDEBUG("%d %d", from, to);
9218  neighborLinks_.clear();
9219  std::multimap<int, Link> links = updateLinksWithModifications(links_);
9220  int position = ui_->horizontalSlider_neighbors->value();
9221  std::multimap<int, Link> linksSortedByChildren;
9222  for(std::multimap<int, rtabmap::Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
9223  {
9224  if(iter->second.from() < iter->second.to())
9225  {
9226  linksSortedByChildren.insert(*iter);
9227  }
9228  }
9229 
9230  for(std::multimap<int, rtabmap::Link>::iterator iter = linksSortedByChildren.begin(); iter!=linksSortedByChildren.end(); ++iter)
9231  {
9232  if(!iter->second.transform().isNull())
9233  {
9234  if(iter->second.type() == rtabmap::Link::kNeighbor ||
9235  iter->second.type() == rtabmap::Link::kNeighborMerged)
9236  {
9237  if((iter->second.from() == from && iter->second.to() == to) ||
9238  (iter->second.to() == from && iter->second.from() == to))
9239  {
9240  position = neighborLinks_.size();
9241  }
9242  neighborLinks_.append(iter->second);
9243  }
9244  }
9245  else
9246  {
9247  UERROR("Transform null for link from %d to %d", iter->first, iter->second.to());
9248  }
9249  }
9250 
9251  if(neighborLinks_.size())
9252  {
9253  if(neighborLinks_.size() == 1)
9254  {
9255  // just to be able to move the cursor of the neighbor slider
9256  neighborLinks_.push_back(neighborLinks_.front());
9257  }
9258  ui_->horizontalSlider_neighbors->setMinimum(0);
9259  ui_->horizontalSlider_neighbors->setMaximum(neighborLinks_.size()-1);
9260  ui_->horizontalSlider_neighbors->setEnabled(true);
9261  if(position != ui_->horizontalSlider_neighbors->value())
9262  {
9263  ui_->horizontalSlider_neighbors->setValue(position);
9264  }
9265  else
9266  {
9267  this->updateConstraintView(neighborLinks_.at(position));
9268  }
9269  }
9270  else
9271  {
9272  ui_->horizontalSlider_neighbors->setEnabled(false);
9273  constraintsViewer_->removeAllClouds();
9274  constraintsViewer_->refreshView();
9275  updateConstraintButtons();
9276  }
9277 }
9278 
9279 void DatabaseViewer::updateLoopClosuresSlider(int from, int to)
9280 {
9281  UDEBUG("%d %d", from, to);
9282  loopLinks_.clear();
9283  std::multimap<int, Link> links = updateLinksWithModifications(links_);
9284  int position = ui_->horizontalSlider_loops->value();
9285  std::multimap<int, Link> linksSortedByParents;
9286  for(std::multimap<int, rtabmap::Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
9287  {
9288  if(iter->second.to() > iter->second.from() && iter->second.from() < 0) // landmark
9289  {
9290  linksSortedByParents.insert(std::make_pair(iter->second.to(), iter->second.inverse()));
9291  }
9292  else if(iter->second.to() < iter->second.from())
9293  {
9294  linksSortedByParents.insert(*iter);
9295  }
9296  }
9297  for(std::multimap<int, rtabmap::Link>::iterator iter = linksSortedByParents.begin(); iter!=linksSortedByParents.end(); ++iter)
9298  {
9299  if(!iter->second.transform().isNull())
9300  {
9301  if(iter->second.type() != rtabmap::Link::kNeighbor &&
9302  iter->second.type() != rtabmap::Link::kNeighborMerged)
9303  {
9304  if((iter->second.from() == from && iter->second.to() == to) ||
9305  (iter->second.to() == from && iter->second.from() == to))
9306  {
9307  position = loopLinks_.size();
9308  }
9309  loopLinks_.append(iter->second);
9310  }
9311  }
9312  else
9313  {
9314  UERROR("Transform null for link from %d to %d", iter->first, iter->second.to());
9315  }
9316  }
9317 
9318  if(loopLinks_.size())
9319  {
9320  if(loopLinks_.size() == 1)
9321  {
9322  // just to be able to move the cursor of the loop slider
9323  loopLinks_.push_back(loopLinks_.front());
9324  }
9325  ui_->horizontalSlider_loops->setMinimum(0);
9326  ui_->horizontalSlider_loops->setMaximum(loopLinks_.size()-1);
9327  ui_->horizontalSlider_loops->setEnabled(true);
9328  if(position != ui_->horizontalSlider_loops->value())
9329  {
9330  ui_->horizontalSlider_loops->setValue(position);
9331  }
9332  else
9333  {
9334  this->updateConstraintView(loopLinks_.at(position));
9335  }
9336  }
9337  else
9338  {
9339  ui_->horizontalSlider_loops->setEnabled(false);
9340  constraintsViewer_->removeAllClouds();
9341  constraintsViewer_->refreshView();
9342  updateConstraintButtons();
9343  }
9344 }
9345 
9346 void DatabaseViewer::notifyParametersChanged(const QStringList & parametersChanged)
9347 {
9348  bool updateStereo = false;
9349  bool updateGraphView = false;
9350  for(QStringList::const_iterator iter=parametersChanged.constBegin();
9351  iter!=parametersChanged.constEnd() && (!updateStereo || !updateGraphView);
9352  ++iter)
9353  {
9354  QString group = iter->split('/').first();
9355  if(!updateStereo && group == "Stereo")
9356  {
9357  updateStereo = true;
9358  continue;
9359  }
9360  if(!updateGraphView && group == "Optimize")
9361  {
9362  updateGraphView = true;
9363  continue;
9364  }
9365  }
9366 
9367  if(updateStereo)
9368  {
9369  this->updateStereo();
9370  }
9371  if(updateGraphView)
9372  {
9373  this->updateGraphView();
9374  }
9375  this->configModified();
9376 }
9377 
9378 } // 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:616
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:5743
rtabmap::CloudViewer::setCameraLockZ
void setCameraLockZ(bool enabled=true)
Definition: CloudViewer.cpp:3304
rtabmap::DatabaseViewer::refineLinks
void refineLinks()
Definition: DatabaseViewer.cpp:4458
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:1197
rtabmap::DatabaseViewer::regenerateCurrentLocalMaps
void regenerateCurrentLocalMaps()
Definition: DatabaseViewer.cpp:3968
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:685
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:3716
rtabmap::DBDriver::getCalibration
bool getCalibration(int signatureId, std::vector< CameraModel > &models, std::vector< StereoCameraModel > &stereoModels) const
Definition: DBDriver.cpp:736
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:6041
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:2466
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:1009
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::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:1476
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:4608
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:547
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:9346
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:7374
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:3731
rtabmap::DatabaseViewer::resetAllChanges
void resetAllChanges()
Definition: DatabaseViewer.cpp:4576
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:6084
rtabmap::EnvSensor::kAmbientAirPressure
@ kAmbientAirPressure
Definition: EnvSensor.h:41
rtabmap::DatabaseViewer::sliderAMoved
void sliderAMoved(int)
Definition: DatabaseViewer.cpp:6026
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:2449
rtabmap::DatabaseViewer::updateOptimizedMesh
void updateOptimizedMesh()
Definition: DatabaseViewer.cpp:3583
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:8053
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:658
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
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)
Definition: Graph.cpp:772
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:4616
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:1205
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:3468
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:6221
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:4342
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:4670
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:902
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::LocalGridMaker::getCellSize
float getCellSize() const
Definition: LocalGridMaker.h:49
rtabmap::DatabaseViewer::exportGPS_KML
void exportGPS_KML()
Definition: DatabaseViewer.cpp:2899
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:3416
rtabmap::DatabaseViewer::updateConstraintButtons
void updateConstraintButtons()
Definition: DatabaseViewer.cpp:6886
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:4148
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:8026
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:2359
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:2457
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:5751
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:3817
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:1003
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:2461
rtabmap::util3d::isFinite
bool RTABMAP_CORE_EXPORT isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3327
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:4595
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:8583
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:786
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:2904
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:7881
rtabmap::DatabaseViewer::getIniFilePath
QString getIniFilePath() const
Definition: DatabaseViewer.cpp:571
rtabmap::DBDriver::getLaserScanInfo
bool getLaserScanInfo(int signatureId, LaserScan &info) const
Definition: DBDriver.cpp:762
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:1037
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:825
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
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:879
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:4372
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:4356
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:2437
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:4386
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:3323
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:909
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
glm::rotation
GLM_FUNC_DECL detail::tquat< T, P > rotation(detail::tvec3< T, P > const &orig, detail::tvec3< T, P > const &dest)
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:3268
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:2895
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:9028
values
leaf::MyValues values
rtabmap::DatabaseViewer::rejectConstraint
void rejectConstraint()
Definition: DatabaseViewer.cpp:9064
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:1235
ULOGGER_ERROR
#define ULOGGER_ERROR(...)
Definition: ULogger.h:56
rtabmap::DatabaseViewer::exportPosesRaw
void exportPosesRaw()
Definition: DatabaseViewer.cpp:2433
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:2937
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:8058
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:5940
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:1212
rtabmap::DatabaseViewer::sliderIterationsValueChanged
void sliderIterationsValueChanged(int)
Definition: DatabaseViewer.cpp:6956
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:9215
rtabmap::DatabaseViewer::updateLoopClosuresSlider
void updateLoopClosuresSlider(int from=0, int to=0)
Definition: DatabaseViewer.cpp:9279
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:524
rtabmap::Transform::toEigen3d
Eigen::Affine3d toEigen3d() const
Definition: Transform.cpp:396
rtabmap::DatabaseViewer::update3dView
void update3dView()
Definition: DatabaseViewer.cpp:6056
rtabmap::GPS::toGeodeticCoords
GeodeticCoords toGeodeticCoords() const
Definition: GPS.h:66
rtabmap::DBDriver::getLaserScansMemoryUsed
long getLaserScansMemoryUsed() const
Definition: DBDriver.cpp:167
DataRecorder.h
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:886
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:7360
rtabmap::LaserScan::hasIntensity
bool hasIntensity() const
Definition: LaserScan.h:136
rtabmap::DatabaseViewer::detectMoreLoopClosures
void detectMoreLoopClosures()
Definition: DatabaseViewer.cpp:4201
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
rtabmap::DBDriver::updateDepthImage
void updateDepthImage(int nodeId, const cv::Mat &image)
Definition: DBDriver.cpp:515
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:1250
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:1191
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:3213
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:1364
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::Transform::translation
Transform translation() const
Definition: Transform.cpp:203
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:7905
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::util3d::transformFromXYZCorrespondencesSVD
Transform RTABMAP_CORE_EXPORT transformFromXYZCorrespondencesSVD(const pcl::PointCloud< pcl::PointXYZ > &cloud1, const pcl::PointCloud< pcl::PointXYZ > &cloud2)
Definition: util3d_registration.cpp:50
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:2453
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:4643
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:6076
value
value
rtabmap::DBDriver::loadSignatures
void loadSignatures(const std::list< int > &ids, std::list< Signature * > &signatures, std::set< int > *loadedFromTrash=0)
Definition: DBDriver.cpp:564
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:925
rtabmap::EditConstraintDialog::getCovariance
cv::Mat getCovariance() const
Definition: EditConstraintDialog.cpp:133
StereoDense.h
rtabmap::DatabaseViewer::sliderNeighborValueChanged
void sliderNeighborValueChanged(int)
Definition: DatabaseViewer.cpp:6068
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:9154
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:2445
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:4095
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:1220
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:2441
EditDepthArea.h

rtabmap
Author(s): Mathieu Labbe
autogenerated on Sun Dec 1 2024 03:42:43