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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Apr 28 2025 02:45:52