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  connect(ui_->toolButton_edit_priorA, SIGNAL(clicked(bool)), this, SLOT(editConstraint()));
355  connect(ui_->toolButton_edit_priorB, SIGNAL(clicked(bool)), this, SLOT(editConstraint()));
356  connect(ui_->toolButton_remove_priorA, SIGNAL(clicked(bool)), this, SLOT(rejectConstraint()));
357  connect(ui_->toolButton_remove_priorB, SIGNAL(clicked(bool)), this, SLOT(rejectConstraint()));
358 
359  connect(ui_->spinBox_mesh_angleTolerance, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
360  connect(ui_->spinBox_mesh_minClusterSize, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
361  connect(ui_->spinBox_mesh_fillDepthHoles, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
362  connect(ui_->spinBox_mesh_depthError, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
363  connect(ui_->checkBox_mesh_quad, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
364  connect(ui_->spinBox_mesh_triangleSize, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
365  connect(ui_->checkBox_showWords, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
366  connect(ui_->checkBox_showCloud, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
367  connect(ui_->checkBox_showMesh, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
368  connect(ui_->checkBox_showScan, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
369  connect(ui_->checkBox_showMap, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
370  connect(ui_->checkBox_showGrid, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
371  connect(ui_->checkBox_showElevation, SIGNAL(stateChanged(int)), this, SLOT(update3dView()));
372  connect(ui_->checkBox_odomFrame_3dview, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
373  connect(ui_->checkBox_gravity_3dview, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
374 
375  ui_->horizontalSlider_neighbors->setTracking(false);
376  ui_->horizontalSlider_loops->setTracking(false);
377  ui_->horizontalSlider_neighbors->setEnabled(false);
378  ui_->horizontalSlider_loops->setEnabled(false);
379  connect(ui_->horizontalSlider_neighbors, SIGNAL(valueChanged(int)), this, SLOT(sliderNeighborValueChanged(int)));
380  connect(ui_->horizontalSlider_loops, SIGNAL(valueChanged(int)), this, SLOT(sliderLoopValueChanged(int)));
381  connect(ui_->horizontalSlider_neighbors, SIGNAL(sliderMoved(int)), this, SLOT(sliderNeighborValueChanged(int)));
382  connect(ui_->horizontalSlider_loops, SIGNAL(sliderMoved(int)), this, SLOT(sliderLoopValueChanged(int)));
383  connect(ui_->checkBox_showOptimized, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
384  connect(ui_->checkBox_show3Dclouds, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
385  connect(ui_->checkBox_show2DScans, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
386  connect(ui_->checkBox_show3DWords, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
387  connect(ui_->checkBox_odomFrame, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
388  ui_->checkBox_showOptimized->setEnabled(false);
389  connect(ui_->toolButton_constraint, SIGNAL(clicked(bool)), this, SLOT(editConstraint()));
390  connect(ui_->checkBox_enableForAll, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintButtons()));
391 
392  ui_->horizontalSlider_iterations->setTracking(false);
393  ui_->horizontalSlider_iterations->setEnabled(false);
394  ui_->spinBox_optimizationsFrom->setEnabled(false);
395  connect(ui_->horizontalSlider_iterations, SIGNAL(valueChanged(int)), this, SLOT(sliderIterationsValueChanged(int)));
396  connect(ui_->horizontalSlider_iterations, SIGNAL(sliderMoved(int)), this, SLOT(sliderIterationsValueChanged(int)));
397  connect(ui_->spinBox_optimizationsFrom, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
398  connect(ui_->comboBox_optimizationFlavor, SIGNAL(activated(int)), this, SLOT(updateGraphView()));
399  connect(ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
400  connect(ui_->checkBox_wmState, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
401  connect(ui_->graphViewer, SIGNAL(mapShownRequested()), this, SLOT(updateGraphView()));
402  connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
403  connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
404  connect(ui_->checkBox_ignoreGlobalLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
405  connect(ui_->checkBox_ignoreLocalLoopSpace, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
406  connect(ui_->checkBox_ignoreLocalLoopTime, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
407  connect(ui_->checkBox_ignoreUserLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
408  connect(ui_->checkBox_ignoreLandmarks, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
409  connect(ui_->doubleSpinBox_optimizationScale, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
410  connect(ui_->checkBox_octomap, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
411  connect(ui_->checkBox_grid_grid, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
412  connect(ui_->checkBox_grid_2d, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
413  connect(ui_->checkBox_grid_elevation, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
414  connect(ui_->comboBox_octomap_rendering_type, SIGNAL(currentIndexChanged(int)), this, SLOT(updateOctomapView()));
415  connect(ui_->spinBox_grid_depth, SIGNAL(valueChanged(int)), this, SLOT(updateOctomapView()));
416  connect(ui_->checkBox_grid_empty, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
417  connect(ui_->checkBox_grid_frontiers, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
418  connect(ui_->doubleSpinBox_gainCompensationRadius, SIGNAL(valueChanged(double)), this, SLOT(updateConstraintView()));
419  connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(updateConstraintView()));
420  connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(update3dView()));
421  connect(ui_->checkBox_cameraProjection, SIGNAL(stateChanged(int)), this, SLOT(update3dView()));
422  connect(ui_->checkBox_showDisparityInsteadOfRight, SIGNAL(stateChanged(int)), this, SLOT(update3dView()));
423  connect(ui_->spinBox_decimation, SIGNAL(valueChanged(int)), this, SLOT(updateConstraintView()));
424  connect(ui_->spinBox_decimation, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
425  connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(updateGraphView()));
426  connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
427  connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
428 
429  ui_->label_stereo_inliers_name->setStyleSheet("QLabel {color : blue; }");
430  ui_->label_stereo_flowOutliers_name->setStyleSheet("QLabel {color : red; }");
431  ui_->label_stereo_slopeOutliers_name->setStyleSheet("QLabel {color : yellow; }");
432  ui_->label_stereo_disparityOutliers_name->setStyleSheet("QLabel {color : magenta; }");
433 
434 
435  // connect configuration changed
436  connect(ui_->graphViewer, SIGNAL(configChanged()), this, SLOT(configModified()));
437  connect(ui_->graphicsView_A, SIGNAL(configChanged()), this, SLOT(configModified()));
438  connect(ui_->graphicsView_B, SIGNAL(configChanged()), this, SLOT(configModified()));
439  connect(ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
440  connect(ui_->actionVertical_Layout, SIGNAL(toggled(bool)), this, SLOT(configModified()));
441  connect(ui_->checkBox_alignPosesWithGPS, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
442  connect(ui_->checkBox_alignPosesWithGroundTruth, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
443  connect(ui_->checkBox_alignScansCloudsWithGroundTruth, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
444  connect(ui_->checkBox_ignoreIntermediateNodes, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
445  connect(ui_->checkBox_ignoreIntermediateNodes, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
446  connect(ui_->checkBox_timeStats, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
447  connect(ui_->checkBox_timeStats, SIGNAL(stateChanged(int)), this, SLOT(updateStatistics()));
448  // Graph view
449  connect(ui_->doubleSpinBox_gainCompensationRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
450  connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
451  connect(ui_->checkBox_cameraProjection, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
452  connect(ui_->checkBox_showDisparityInsteadOfRight, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
453  connect(ui_->spinBox_decimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
454  connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(configModified()));
455  connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
456  connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
457  connect(ui_->horizontalSlider_rotation, SIGNAL(valueChanged(int)), this, SLOT(updateGraphRotation()));
458  connect(ui_->pushButton_applyRotation, SIGNAL(clicked()), this, SLOT(updateGraphView()));
459 
460  connect(ui_->spinBox_icp_decimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
461  connect(ui_->doubleSpinBox_icp_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
462  connect(ui_->doubleSpinBox_icp_minDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
463  connect(ui_->checkBox_icp_from_depth, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
464 
465  connect(ui_->doubleSpinBox_detectMore_radius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
466  connect(ui_->doubleSpinBox_detectMore_radiusMin, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
467  connect(ui_->doubleSpinBox_detectMore_angle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
468  connect(ui_->spinBox_detectMore_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
469  connect(ui_->checkBox_detectMore_intraSession, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
470  connect(ui_->checkBox_detectMore_interSession, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
471  connect(ui_->checkBox_opt_graph_as_guess, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
472 
473  connect(ui_->lineEdit_obstacleColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified()));
474  connect(ui_->lineEdit_groundColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified()));
475  connect(ui_->lineEdit_emptyColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified()));
476  connect(ui_->lineEdit_frontierColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified()));
477  connect(ui_->lineEdit_obstacleColor, SIGNAL(textChanged(const QString &)), this, SLOT(updateGrid()));
478  connect(ui_->lineEdit_groundColor, SIGNAL(textChanged(const QString &)), this, SLOT(updateGrid()));
479  connect(ui_->lineEdit_emptyColor, SIGNAL(textChanged(const QString &)), this, SLOT(updateGrid()));
480  connect(ui_->lineEdit_frontierColor, SIGNAL(textChanged(const QString &)), this, SLOT(updateGrid()));
481  connect(ui_->toolButton_obstacleColor, SIGNAL(clicked(bool)), this, SLOT(selectObstacleColor()));
482  connect(ui_->toolButton_groundColor, SIGNAL(clicked(bool)), this, SLOT(selectGroundColor()));
483  connect(ui_->toolButton_emptyColor, SIGNAL(clicked(bool)), this, SLOT(selectEmptyColor()));
484  connect(ui_->toolButton_frontierColor, SIGNAL(clicked(bool)), this, SLOT(selectFrontierColor()));
485  connect(ui_->spinBox_cropRadius, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
486  connect(ui_->checkBox_grid_showProbMap, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
487  connect(ui_->checkBox_grid_showProbMap, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
488 
489  connect(exportDialog_, SIGNAL(configChanged()), this, SLOT(configModified()));
490 
491  // dockwidget
492  QList<QDockWidget*> dockWidgets = this->findChildren<QDockWidget*>();
493  for(int i=0; i<dockWidgets.size(); ++i)
494  {
495  connect(dockWidgets[i], SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(configModified()));
496  connect(dockWidgets[i]->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configModified()));
497  }
498  ui_->dockWidget_constraints->installEventFilter(this);
499  ui_->dockWidget_graphView->installEventFilter(this);
500  ui_->dockWidget_occupancyGridView->installEventFilter(this);
501  ui_->dockWidget_stereoView->installEventFilter(this);
502  ui_->dockWidget_view3d->installEventFilter(this);
503  ui_->dockWidget_guiparameters->installEventFilter(this);
504  ui_->dockWidget_coreparameters->installEventFilter(this);
505  ui_->dockWidget_info->installEventFilter(this);
506  ui_->dockWidget_statistics->installEventFilter(this);
507 }
508 
509 DatabaseViewer::~DatabaseViewer()
510 {
511  delete ui_;
512  delete dbDriver_;
513 #ifdef RTABMAP_OCTOMAP
514  delete octomap_;
515 #endif
516 }
517 
518 void DatabaseViewer::setupMainLayout(bool vertical)
519 {
520  if(vertical)
521  {
522  qobject_cast<QHBoxLayout *>(ui_->horizontalLayout_imageViews->layout())->setDirection(QBoxLayout::TopToBottom);
523  }
524  else if(!vertical)
525  {
526  qobject_cast<QHBoxLayout *>(ui_->horizontalLayout_imageViews->layout())->setDirection(QBoxLayout::LeftToRight);
527  }
528  if(ids_.size())
529  {
530  sliderAValueChanged(ui_->horizontalSlider_A->value()); // update matching lines
531  }
532 }
533 
534 void DatabaseViewer::showCloseButton(bool visible)
535 {
536  ui_->buttonBox->setVisible(visible);
537 }
538 
539 void DatabaseViewer::configModified()
540 {
541  this->setWindowModified(true);
542 }
543 
544 QString DatabaseViewer::getIniFilePath() const
545 {
546  if(!iniFilePath_.isEmpty())
547  {
548  return iniFilePath_;
549  }
550  QString privatePath = QDir::homePath() + "/.rtabmap";
551  if(!QDir(privatePath).exists())
552  {
553  QDir::home().mkdir(".rtabmap");
554  }
555  return privatePath + "/rtabmap.ini";
556 }
557 
558 void DatabaseViewer::readSettings()
559 {
560  QString path = getIniFilePath();
561  QSettings settings(path, QSettings::IniFormat);
562  settings.beginGroup("DatabaseViewer");
563 
564  //load window state / geometry
565  QByteArray bytes;
566  bytes = settings.value("geometry", QByteArray()).toByteArray();
567  if(!bytes.isEmpty())
568  {
569  this->restoreGeometry(bytes);
570  }
571  bytes = settings.value("state", QByteArray()).toByteArray();
572  if(!bytes.isEmpty())
573  {
574  this->restoreState(bytes);
575  }
576  savedMaximized_ = settings.value("maximized", false).toBool();
577 
578  ui_->comboBox_logger_level->setCurrentIndex(settings.value("loggerLevel", ui_->comboBox_logger_level->currentIndex()).toInt());
579  ui_->actionVertical_Layout->setChecked(settings.value("verticalLayout", ui_->actionVertical_Layout->isChecked()).toBool());
580  ui_->checkBox_ignoreIntermediateNodes->setChecked(settings.value("ignoreIntermediateNodes", ui_->checkBox_ignoreIntermediateNodes->isChecked()).toBool());
581  ui_->checkBox_timeStats->setChecked(settings.value("timeStats", ui_->checkBox_timeStats->isChecked()).toBool());
582 
583  // GraphViewer settings
584  ui_->graphViewer->loadSettings(settings, "GraphView");
585 
586  settings.beginGroup("optimization");
587  ui_->doubleSpinBox_gainCompensationRadius->setValue(settings.value("gainCompensationRadius", ui_->doubleSpinBox_gainCompensationRadius->value()).toDouble());
588  ui_->doubleSpinBox_voxelSize->setValue(settings.value("voxelSize", ui_->doubleSpinBox_voxelSize->value()).toDouble());
589  ui_->spinBox_decimation->setValue(settings.value("decimation", ui_->spinBox_decimation->value()).toInt());
590  ui_->checkBox_cameraProjection->setChecked(settings.value("camProj", ui_->checkBox_cameraProjection->isChecked()).toBool());
591  ui_->checkBox_showDisparityInsteadOfRight->setChecked(settings.value("showDisp", ui_->checkBox_showDisparityInsteadOfRight->isChecked()).toBool());
592  settings.endGroup();
593 
594  settings.beginGroup("grid");
595  ui_->groupBox_posefiltering->setChecked(settings.value("poseFiltering", ui_->groupBox_posefiltering->isChecked()).toBool());
596  ui_->doubleSpinBox_posefilteringRadius->setValue(settings.value("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value()).toDouble());
597  ui_->doubleSpinBox_posefilteringAngle->setValue(settings.value("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value()).toDouble());
598  ui_->lineEdit_obstacleColor->setText(settings.value("colorObstacle", ui_->lineEdit_obstacleColor->text()).toString());
599  ui_->lineEdit_groundColor->setText(settings.value("colorGround", ui_->lineEdit_groundColor->text()).toString());
600  ui_->lineEdit_emptyColor->setText(settings.value("colorEmpty", ui_->lineEdit_emptyColor->text()).toString());
601  ui_->lineEdit_frontierColor->setText(settings.value("colorFrontier", ui_->lineEdit_frontierColor->text()).toString());
602  ui_->spinBox_cropRadius->setValue(settings.value("cropRadius", ui_->spinBox_cropRadius->value()).toInt());
603  ui_->checkBox_grid_showProbMap->setChecked(settings.value("probMap", ui_->checkBox_grid_showProbMap->isChecked()).toBool());
604  settings.endGroup();
605 
606  settings.beginGroup("mesh");
607  ui_->checkBox_mesh_quad->setChecked(settings.value("quad", ui_->checkBox_mesh_quad->isChecked()).toBool());
608  ui_->spinBox_mesh_angleTolerance->setValue(settings.value("angleTolerance", ui_->spinBox_mesh_angleTolerance->value()).toInt());
609  ui_->spinBox_mesh_minClusterSize->setValue(settings.value("minClusterSize", ui_->spinBox_mesh_minClusterSize->value()).toInt());
610  ui_->spinBox_mesh_fillDepthHoles->setValue(settings.value("fillDepthHolesSize", ui_->spinBox_mesh_fillDepthHoles->value()).toInt());
611  ui_->spinBox_mesh_depthError->setValue(settings.value("fillDepthHolesError", ui_->spinBox_mesh_depthError->value()).toInt());
612  ui_->spinBox_mesh_triangleSize->setValue(settings.value("triangleSize", ui_->spinBox_mesh_triangleSize->value()).toInt());
613  settings.endGroup();
614 
615  // ImageViews
616  ui_->graphicsView_A->loadSettings(settings, "ImageViewA");
617  ui_->graphicsView_B->loadSettings(settings, "ImageViewB");
618 
619  // ICP parameters
620  settings.beginGroup("icp");
621  ui_->spinBox_icp_decimation->setValue(settings.value("decimation", ui_->spinBox_icp_decimation->value()).toInt());
622  ui_->doubleSpinBox_icp_maxDepth->setValue(settings.value("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value()).toDouble());
623  ui_->doubleSpinBox_icp_minDepth->setValue(settings.value("minDepth", ui_->doubleSpinBox_icp_minDepth->value()).toDouble());
624  ui_->checkBox_icp_from_depth->setChecked(settings.value("icpFromDepth", ui_->checkBox_icp_from_depth->isChecked()).toBool());
625  settings.endGroup();
626 
627  settings.endGroup(); // DatabaseViewer
628 
629  // Use same parameters used by RTAB-Map
630  settings.beginGroup("Gui");
631  exportDialog_->loadSettings(settings, exportDialog_->objectName());
632  settings.beginGroup("PostProcessingDialog");
633  ui_->doubleSpinBox_detectMore_radius->setValue(settings.value("cluster_radius", ui_->doubleSpinBox_detectMore_radius->value()).toDouble());
634  ui_->doubleSpinBox_detectMore_radiusMin->setValue(settings.value("cluster_radius_min", ui_->doubleSpinBox_detectMore_radiusMin->value()).toDouble());
635  ui_->doubleSpinBox_detectMore_angle->setValue(settings.value("cluster_angle", ui_->doubleSpinBox_detectMore_angle->value()).toDouble());
636  ui_->spinBox_detectMore_iterations->setValue(settings.value("iterations", ui_->spinBox_detectMore_iterations->value()).toInt());
637  ui_->checkBox_detectMore_intraSession->setChecked(settings.value("intra_session", ui_->checkBox_detectMore_intraSession->isChecked()).toBool());
638  ui_->checkBox_detectMore_interSession->setChecked(settings.value("inter_session", ui_->checkBox_detectMore_interSession->isChecked()).toBool());
639  ui_->checkBox_opt_graph_as_guess->setChecked(settings.value("opt_graph_as_guess", ui_->checkBox_opt_graph_as_guess->isChecked()).toBool());
640  settings.endGroup();
641  settings.endGroup();
642 
643 
644  ParametersMap parameters;
645  Parameters::readINI(path.toStdString(), parameters);
646  for(ParametersMap::iterator iter = parameters.begin(); iter!= parameters.end(); ++iter)
647  {
648  ui_->parameters_toolbox->updateParameter(iter->first, iter->second);
649  }
650 }
651 
652 void DatabaseViewer::writeSettings()
653 {
654  QString path = getIniFilePath();
655  QSettings settings(path, QSettings::IniFormat);
656  settings.beginGroup("DatabaseViewer");
657 
658  //save window state / geometry
659  if(!this->isMaximized())
660  {
661  settings.setValue("geometry", this->saveGeometry());
662  }
663  settings.setValue("state", this->saveState());
664  settings.setValue("maximized", this->isMaximized());
665  savedMaximized_ = this->isMaximized();
666 
667  settings.setValue("loggerLevel", ui_->comboBox_logger_level->currentIndex());
668  settings.setValue("verticalLayout", ui_->actionVertical_Layout->isChecked());
669  settings.setValue("ignoreIntermediateNodes", ui_->checkBox_ignoreIntermediateNodes->isChecked());
670  settings.setValue("timeStats", ui_->checkBox_timeStats->isChecked());
671 
672  // save GraphViewer settings
673  ui_->graphViewer->saveSettings(settings, "GraphView");
674 
675  // save optimization settings
676  settings.beginGroup("optimization");
677  settings.setValue("gainCompensationRadius", ui_->doubleSpinBox_gainCompensationRadius->value());
678  settings.setValue("voxelSize", ui_->doubleSpinBox_voxelSize->value());
679  settings.setValue("decimation", ui_->spinBox_decimation->value());
680  settings.setValue("camProj", ui_->checkBox_cameraProjection->isChecked());
681  settings.setValue("showDisp", ui_->checkBox_showDisparityInsteadOfRight->isChecked());
682  settings.endGroup();
683 
684  // save Grid settings
685  settings.beginGroup("grid");
686  settings.setValue("poseFiltering", ui_->groupBox_posefiltering->isChecked());
687  settings.setValue("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value());
688  settings.setValue("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value());
689  settings.setValue("colorObstacle", ui_->lineEdit_obstacleColor->text());
690  settings.setValue("colorGround", ui_->lineEdit_groundColor->text());
691  settings.setValue("colorEmpty", ui_->lineEdit_emptyColor->text());
692  settings.setValue("colorFrontier", ui_->lineEdit_frontierColor->text());
693  settings.setValue("cropRadius", ui_->spinBox_cropRadius->value());
694  settings.setValue("probMap", ui_->checkBox_grid_showProbMap->isChecked());
695  settings.endGroup();
696 
697  settings.beginGroup("mesh");
698  settings.setValue("quad", ui_->checkBox_mesh_quad->isChecked());
699  settings.setValue("angleTolerance", ui_->spinBox_mesh_angleTolerance->value());
700  settings.setValue("minClusterSize", ui_->spinBox_mesh_minClusterSize->value());
701  settings.setValue("fillDepthHolesSize", ui_->spinBox_mesh_fillDepthHoles->value());
702  settings.setValue("fillDepthHolesError", ui_->spinBox_mesh_depthError->value());
703  settings.setValue("triangleSize", ui_->spinBox_mesh_triangleSize->value());
704  settings.endGroup();
705 
706  // ImageViews
707  ui_->graphicsView_A->saveSettings(settings, "ImageViewA");
708  ui_->graphicsView_B->saveSettings(settings, "ImageViewB");
709 
710  // save ICP parameters
711  settings.beginGroup("icp");
712  settings.setValue("decimation", ui_->spinBox_icp_decimation->value());
713  settings.setValue("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value());
714  settings.setValue("minDepth", ui_->doubleSpinBox_icp_minDepth->value());
715  settings.setValue("icpFromDepth", ui_->checkBox_icp_from_depth->isChecked());
716  settings.endGroup();
717 
718  settings.endGroup(); // DatabaseViewer
719 
720  // Use same parameters used by RTAB-Map
721  settings.beginGroup("Gui");
722  exportDialog_->saveSettings(settings, exportDialog_->objectName());
723  settings.beginGroup("PostProcessingDialog");
724  settings.setValue("cluster_radius", ui_->doubleSpinBox_detectMore_radius->value());
725  settings.setValue("cluster_radius_min", ui_->doubleSpinBox_detectMore_radiusMin->value());
726  settings.setValue("cluster_angle", ui_->doubleSpinBox_detectMore_angle->value());
727  settings.setValue("iterations", ui_->spinBox_detectMore_iterations->value());
728  settings.setValue("intra_session", ui_->checkBox_detectMore_intraSession->isChecked());
729  settings.setValue("inter_session", ui_->checkBox_detectMore_interSession->isChecked());
730  settings.setValue("opt_graph_as_guess", ui_->checkBox_opt_graph_as_guess->isChecked());
731  settings.endGroup();
732  settings.endGroup();
733 
734  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
735  for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end();)
736  {
737  if(!ui_->parameters_toolbox->getParameterWidget(iter->first.c_str()))
738  {
739  parameters.erase(iter++);
740  }
741  else
742  {
743  ++iter;
744  }
745  }
746  Parameters::writeINI(path.toStdString(), parameters);
747 
748  this->setWindowModified(false);
749 }
750 
751 void DatabaseViewer::restoreDefaultSettings()
752 {
753  // reset GUI parameters
754  ui_->comboBox_logger_level->setCurrentIndex(1);
755  ui_->checkBox_alignPosesWithGPS->setChecked(true);
756  ui_->checkBox_alignPosesWithGroundTruth->setChecked(true);
757  ui_->checkBox_alignScansCloudsWithGroundTruth->setChecked(false);
758  ui_->checkBox_ignoreIntermediateNodes->setChecked(false);
759  ui_->checkBox_timeStats->setChecked(true);
760 
761  ui_->comboBox_optimizationFlavor->setCurrentIndex(0);
762  ui_->checkBox_spanAllMaps->setChecked(true);
763  ui_->checkBox_wmState->setChecked(false);
764  ui_->checkBox_ignorePoseCorrection->setChecked(false);
765  ui_->checkBox_ignoreGlobalLoop->setChecked(false);
766  ui_->checkBox_ignoreLocalLoopSpace->setChecked(false);
767  ui_->checkBox_ignoreLocalLoopTime->setChecked(false);
768  ui_->checkBox_ignoreUserLoop->setChecked(false);
769  ui_->checkBox_ignoreLandmarks->setChecked(false);
770  ui_->doubleSpinBox_optimizationScale->setValue(1.0);
771  ui_->doubleSpinBox_gainCompensationRadius->setValue(0.0);
772  ui_->doubleSpinBox_voxelSize->setValue(0.0);
773  ui_->spinBox_decimation->setValue(1);
774  ui_->checkBox_cameraProjection->setChecked(false);
775  ui_->checkBox_showDisparityInsteadOfRight->setChecked(false);
776 
777  ui_->groupBox_posefiltering->setChecked(false);
778  ui_->doubleSpinBox_posefilteringRadius->setValue(0.1);
779  ui_->doubleSpinBox_posefilteringAngle->setValue(30);
780  ui_->checkBox_grid_empty->setChecked(true);
781  ui_->checkBox_grid_frontiers->setChecked(false);
782  ui_->checkBox_octomap->setChecked(false);
783  ui_->lineEdit_obstacleColor->setText(QColor(Qt::red).name());
784  ui_->lineEdit_groundColor->setText(QColor(Qt::green).name());
785  ui_->lineEdit_emptyColor->setText(QColor(Qt::yellow).name());
786  ui_->lineEdit_frontierColor->setText(QColor(Qt::cyan).name());
787  ui_->spinBox_cropRadius->setValue(1);
788  ui_->checkBox_grid_showProbMap->setChecked(false);
789 
790  ui_->checkBox_mesh_quad->setChecked(true);
791  ui_->spinBox_mesh_angleTolerance->setValue(15);
792  ui_->spinBox_mesh_minClusterSize->setValue(0);
793  ui_->spinBox_mesh_fillDepthHoles->setValue(false);
794  ui_->spinBox_mesh_depthError->setValue(10);
795  ui_->spinBox_mesh_triangleSize->setValue(2);
796 
797  ui_->spinBox_icp_decimation->setValue(1);
798  ui_->doubleSpinBox_icp_maxDepth->setValue(0.0);
799  ui_->doubleSpinBox_icp_minDepth->setValue(0.0);
800  ui_->checkBox_icp_from_depth->setChecked(false);
801 
802  ui_->doubleSpinBox_detectMore_radius->setValue(1.0);
803  ui_->doubleSpinBox_detectMore_radiusMin->setValue(0.0);
804  ui_->doubleSpinBox_detectMore_angle->setValue(30.0);
805  ui_->spinBox_detectMore_iterations->setValue(5);
806  ui_->checkBox_detectMore_intraSession->setChecked(true);
807  ui_->checkBox_detectMore_interSession->setChecked(true);
808  ui_->checkBox_opt_graph_as_guess->setChecked(true);
809 }
810 
811 void DatabaseViewer::openDatabase()
812 {
813  QString path = QFileDialog::getOpenFileName(this, tr("Select file"), pathDatabase_, tr("Databases (*.db)"));
814  if(!path.isEmpty())
815  {
816  openDatabase(path);
817  }
818 }
819 
820 bool DatabaseViewer::openDatabase(const QString & path, const ParametersMap & overriddenParameters)
821 {
822  UDEBUG("Open database \"%s\"", path.toStdString().c_str());
823  if(QFile::exists(path))
824  {
825  if(QFileInfo(path).isFile())
826  {
827  std::string driverType = "sqlite3";
828 
829  dbDriver_ = DBDriver::create();
830 
831  if(!dbDriver_->openConnection(path.toStdString()))
832  {
833  ui_->actionClose_database->setEnabled(false);
834  ui_->actionOpen_database->setEnabled(true);
835  delete dbDriver_;
836  dbDriver_ = 0;
837  QMessageBox::warning(this, "Database error", tr("Can't open database \"%1\"").arg(path));
838  }
839  else
840  {
841  ui_->actionClose_database->setEnabled(true);
842  ui_->actionOpen_database->setEnabled(false);
843 
844  pathDatabase_ = UDirectory::getDir(path.toStdString()).c_str();
845  if(pathDatabase_.isEmpty() || pathDatabase_.compare(".") == 0)
846  {
847  pathDatabase_ = QDir::currentPath();
848  }
849  databaseFileName_ = UFile::getName(path.toStdString());
850  ui_->graphViewer->setWorkingDirectory(pathDatabase_);
851 
852  // look if there are saved parameters
853  ParametersMap parameters = dbDriver_->getLastParameters();
854 
855  // add overridden parameters
856  uInsert(parameters, overriddenParameters);
857 
858  if(parameters.size())
859  {
860  const ParametersMap & currentParameters = ui_->parameters_toolbox->getParameters();
861  ParametersMap differentParameters;
862  for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
863  {
864  ParametersMap::const_iterator jter = currentParameters.find(iter->first);
865  if(jter!=currentParameters.end() &&
866  ui_->parameters_toolbox->getParameterWidget(QString(iter->first.c_str())) != 0 &&
867  iter->second.compare(jter->second) != 0 &&
868  iter->first.compare(Parameters::kRtabmapWorkingDirectory()) != 0)
869  {
870  bool different = true;
871  if(Parameters::getType(iter->first).compare("double") ==0 ||
872  Parameters::getType(iter->first).compare("float") == 0)
873  {
874  if(uStr2Double(iter->second) == uStr2Double(jter->second))
875  {
876  different = false;
877  }
878  }
879  if(different)
880  {
881  differentParameters.insert(*iter);
882  QString msg = tr("Parameter \"%1\": database=\"%2\" Preferences=\"%3\"")
883  .arg(iter->first.c_str())
884  .arg(iter->second.c_str())
885  .arg(jter->second.c_str());
886  UWARN(msg.toStdString().c_str());
887  }
888  }
889  }
890 
891  if(differentParameters.size())
892  {
893  int r = QMessageBox::question(this,
894  tr("Update parameters..."),
895  tr("The database is using %1 different parameter(s) than "
896  "those currently set in Core parameters panel. Do you want "
897  "to use database's parameters?").arg(differentParameters.size()),
898  QMessageBox::Yes | QMessageBox::No,
899  QMessageBox::Yes);
900  if(r == QMessageBox::Yes)
901  {
902  QStringList str;
903  for(rtabmap::ParametersMap::const_iterator iter = differentParameters.begin(); iter!=differentParameters.end(); ++iter)
904  {
905  ui_->parameters_toolbox->updateParameter(iter->first, iter->second);
906  str.push_back(iter->first.c_str());
907  }
908  notifyParametersChanged(str);
909  }
910  }
911  }
912 
913  updateIds();
914  this->setWindowTitle("RTAB-Map Database Viewer - " + path + "[*]");
915  return true;
916  }
917  }
918  else // directory
919  {
920  pathDatabase_ = path;
921  if(pathDatabase_.isEmpty() || pathDatabase_.compare(".") == 0)
922  {
923  pathDatabase_ = QDir::currentPath();
924  }
925  ui_->graphViewer->setWorkingDirectory(pathDatabase_);
926  }
927  }
928  else
929  {
930  QMessageBox::warning(this, "Database error", tr("Database \"%1\" does not exist.").arg(path));
931  }
932  return false;
933 }
934 
935 bool DatabaseViewer::closeDatabase()
936 {
937  this->setWindowTitle("RTAB-Map Database Viewer[*]");
938  if(dbDriver_)
939  {
940  if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size())
941  {
942  QMessageBox::StandardButton button = QMessageBox::question(this,
943  tr("Links modified"),
944  tr("Some links are modified (%1 added, %2 refined, %3 removed), do you want to save them?")
945  .arg(linksAdded_.size()).arg(linksRefined_.size()).arg(linksRemoved_.size()),
946  QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
947  QMessageBox::Cancel);
948 
949  if(button == QMessageBox::Yes)
950  {
951  // Added links
952  for(std::multimap<int, rtabmap::Link>::iterator iter=linksAdded_.begin(); iter!=linksAdded_.end(); ++iter)
953  {
954  std::multimap<int, rtabmap::Link>::iterator refinedIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
955  if(refinedIter != linksRefined_.end())
956  {
957  dbDriver_->addLink(refinedIter->second);
958  if(refinedIter->second.from() != refinedIter->second.to())
959  dbDriver_->addLink(refinedIter->second.inverse());
960  }
961  else
962  {
963  dbDriver_->addLink(iter->second);
964  if(iter->second.from() != iter->second.to())
965  dbDriver_->addLink(iter->second.inverse());
966  }
967  }
968 
969  //Refined links
970  for(std::multimap<int, rtabmap::Link>::iterator iter=linksRefined_.begin(); iter!=linksRefined_.end(); ++iter)
971  {
972  if(!containsLink(linksAdded_, iter->second.from(), iter->second.to()))
973  {
974  dbDriver_->updateLink(iter->second);
975  if(iter->second.from() != iter->second.to())
976  dbDriver_->updateLink(iter->second.inverse());
977  }
978  }
979 
980  // Rejected links
981  for(std::multimap<int, rtabmap::Link>::iterator iter=linksRemoved_.begin(); iter!=linksRemoved_.end(); ++iter)
982  {
983  dbDriver_->removeLink(iter->second.to(), iter->second.from());
984  if(iter->second.from() != iter->second.to())
985  dbDriver_->removeLink(iter->second.from(), iter->second.to());
986  }
987  linksAdded_.clear();
988  linksRefined_.clear();
989  linksRemoved_.clear();
990 
991  // Clear the optimized poses, this will force rtabmap to re-optimize the graph on initialization
992  Transform lastLocalizationPose;
993  if(!dbDriver_->loadOptimizedPoses(&lastLocalizationPose).empty())
994  {
995  dbDriver_->saveOptimizedPoses(std::map<int, Transform>(), lastLocalizationPose);
996  }
997  // This will force rtabmap_ros to regenerate the global occupancy grid if there was one
998  dbDriver_->save2DMap(cv::Mat(), 0, 0, 0);
999  dbDriver_->saveOptimizedMesh(cv::Mat());
1000  }
1001 
1002  if(button != QMessageBox::Yes && button != QMessageBox::No)
1003  {
1004  return false;
1005  }
1006  }
1007 
1008  if( generatedLocalMaps_.size() &&
1009  uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.11.10") >= 0)
1010  {
1011  QMessageBox::StandardButton button = QMessageBox::question(this,
1012  tr("Local occupancy grid maps modified"),
1013  tr("%1 occupancy grid maps are modified, do you want to "
1014  "save them? This will overwrite occupancy grids saved in the database.")
1015  .arg(generatedLocalMaps_.size()),
1016  QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
1017  QMessageBox::Cancel);
1018 
1019  if(button == QMessageBox::Yes)
1020  {
1021  for(std::map<int, LocalGrid>::const_iterator mapIter = generatedLocalMaps_.localGrids().begin();
1022  mapIter!=generatedLocalMaps_.localGrids().end();
1023  ++mapIter)
1024  {
1025  dbDriver_->updateOccupancyGrid(
1026  mapIter->first,
1027  mapIter->second.groundCells,
1028  mapIter->second.obstacleCells,
1029  mapIter->second.emptyCells,
1030  mapIter->second.cellSize,
1031  mapIter->second.viewPoint);
1032  }
1033  generatedLocalMaps_.clear();
1034  localMaps_.clear();
1035 
1036  // This will force rtabmap_ros to regenerate the global occupancy grid if there was one
1037  dbDriver_->save2DMap(cv::Mat(), 0, 0, 0);
1038  }
1039 
1040  if(button != QMessageBox::Yes && button != QMessageBox::No)
1041  {
1042  return false;
1043  }
1044  }
1045 
1046  if(!modifiedLaserScans_.empty())
1047  {
1048  QMessageBox::StandardButton button = QMessageBox::question(this,
1049  tr("Laser scans modified"),
1050  tr("%1 laser scans are modified, do you want to "
1051  "save them? This will overwrite laser scans saved in the database.")
1052  .arg(modifiedLaserScans_.size()),
1053  QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
1054  QMessageBox::Cancel);
1055 
1056  if(button == QMessageBox::Yes)
1057  {
1058  for(std::map<int, LaserScan>::iterator iter=modifiedLaserScans_.begin(); iter!=modifiedLaserScans_.end(); ++iter)
1059  {
1060  dbDriver_->updateLaserScan(iter->first, iter->second);
1061  }
1062  modifiedLaserScans_.clear();
1063  }
1064 
1065  if(button != QMessageBox::Yes && button != QMessageBox::No)
1066  {
1067  return false;
1068  }
1069  }
1070 
1071  delete dbDriver_;
1072  dbDriver_ = 0;
1073  ids_.clear();
1074  idToIndex_.clear();
1075  neighborLinks_.clear();
1076  loopLinks_.clear();
1077  graphes_.clear();
1078  graphLinks_.clear();
1079  odomPoses_.clear();
1080  groundTruthPoses_.clear();
1081  gpsPoses_.clear();
1082  gpsValues_.clear();
1083  lastWmIds_.clear();
1084  mapIds_.clear();
1085  weights_.clear();
1086  wmStates_.clear();
1087  links_.clear();
1088  linksAdded_.clear();
1089  linksRefined_.clear();
1090  linksRemoved_.clear();
1091  localMaps_.clear();
1092  generatedLocalMaps_.clear();
1093  modifiedLaserScans_.clear();
1094  ui_->graphViewer->clearAll();
1095  occupancyGridViewer_->clear();
1096  ui_->menuEdit->setEnabled(false);
1097  ui_->actionGenerate_3D_map_pcd->setEnabled(false);
1098  ui_->actionExport->setEnabled(false);
1099  ui_->actionExtract_images->setEnabled(false);
1100  ui_->menuExport_poses->setEnabled(false);
1101  ui_->menuExport_GPS->setEnabled(false);
1102  ui_->actionPoses_KML->setEnabled(false);
1103  ui_->actionEdit_optimized_2D_map->setEnabled(false);
1104  ui_->actionExport_saved_2D_map->setEnabled(false);
1105  ui_->actionImport_2D_map->setEnabled(false);
1106  ui_->actionRegenerate_optimized_2D_map->setEnabled(false);
1107  ui_->actionView_optimized_mesh->setEnabled(false);
1108  ui_->actionExport_optimized_mesh->setEnabled(false);
1109  ui_->actionUpdate_optimized_mesh->setEnabled(false);
1110  ui_->checkBox_showOptimized->setEnabled(false);
1111  ui_->toolBox_statistics->clear();
1112  databaseFileName_.clear();
1113  ui_->checkBox_alignPosesWithGPS->setVisible(false);
1114  ui_->checkBox_alignPosesWithGPS->setEnabled(false);
1115  ui_->checkBox_alignPosesWithGroundTruth->setVisible(false);
1116  ui_->checkBox_alignPosesWithGroundTruth->setEnabled(false);
1117  ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(false);
1118  ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(false);
1119  ui_->doubleSpinBox_optimizationScale->setVisible(false);
1120  ui_->label_scale_title->setVisible(false);
1121  ui_->label_rmse->setVisible(false);
1122  ui_->label_rmse_title->setVisible(false);
1123  ui_->checkBox_ignoreIntermediateNodes->setVisible(false);
1124  ui_->label_ignoreINtermediateNdoes->setVisible(false);
1125  ui_->label_alignPosesWithGPS->setVisible(false);
1126  ui_->label_alignPosesWithGroundTruth->setVisible(false);
1127  ui_->label_alignScansCloudsWithGroundTruth->setVisible(false);
1128  ui_->label_optimizeFrom->setText(tr("Root"));
1129  ui_->textEdit_info->clear();
1130 
1131  ui_->pushButton_refine->setEnabled(false);
1132  ui_->pushButton_add->setEnabled(false);
1133  ui_->pushButton_reset->setEnabled(false);
1134  ui_->pushButton_reject->setEnabled(false);
1135 
1136  ui_->horizontalSlider_loops->setEnabled(false);
1137  ui_->horizontalSlider_loops->setMaximum(0);
1138  ui_->horizontalSlider_iterations->setEnabled(false);
1139  ui_->horizontalSlider_iterations->setMaximum(0);
1140  ui_->horizontalSlider_neighbors->setEnabled(false);
1141  ui_->horizontalSlider_neighbors->setMaximum(0);
1142  ui_->label_constraint->clear();
1143  ui_->label_constraint_opt->clear();
1144  ui_->label_variance->clear();
1145  ui_->lineEdit_covariance->clear();
1146  ui_->label_type->clear();
1147  ui_->label_type_name->clear();
1148  ui_->checkBox_showOptimized->setEnabled(false);
1149 
1150  ui_->horizontalSlider_A->setEnabled(false);
1151  ui_->horizontalSlider_A->setMaximum(0);
1152  ui_->horizontalSlider_B->setEnabled(false);
1153  ui_->horizontalSlider_B->setMaximum(0);
1154  ui_->label_idA->setText("NaN");
1155  ui_->label_idB->setText("NaN");
1156  sliderAValueChanged(0);
1157  sliderBValueChanged(0);
1158 
1159  constraintsViewer_->clear();
1160  constraintsViewer_->refreshView();
1161 
1162  cloudViewer_->clear();
1163  cloudViewer_->refreshView();
1164 
1165  occupancyGridViewer_->clear();
1166  occupancyGridViewer_->refreshView();
1167 
1168  ui_->graphViewer->clearAll();
1169  ui_->label_loopClosures->clear();
1170  ui_->label_timeOptimization->clear();
1171  ui_->label_pathLength->clear();
1172  ui_->label_poses->clear();
1173  ui_->label_rmse->clear();
1174  ui_->spinBox_optimizationsFrom->setEnabled(false);
1175 
1176  ui_->graphicsView_A->clear();
1177  ui_->graphicsView_B->clear();
1178 
1179  ui_->graphicsView_stereo->clear();
1180  stereoViewer_->clear();
1181  stereoViewer_->refreshView();
1182 
1183  ui_->toolBox_statistics->clear();
1184 
1185  // This will re-init the dialog
1186  delete linkRefiningDialog_;
1187  linkRefiningDialog_ = new LinkRefiningDialog(this);
1188  }
1189 
1190  ui_->actionClose_database->setEnabled(dbDriver_ != 0);
1191  ui_->actionOpen_database->setEnabled(dbDriver_ == 0);
1192 
1193  return dbDriver_ == 0;
1194 }
1195 
1196 
1197 void DatabaseViewer::recoverDatabase()
1198 {
1199  QString path = QFileDialog::getOpenFileName(this, tr("Select file"), pathDatabase_, tr("Databases (*.db)"));
1200  if(!path.isEmpty())
1201  {
1202  if(path.compare(pathDatabase_+QDir::separator()+databaseFileName_.c_str()) == 0)
1203  {
1204  QMessageBox::information(this, "Database recovery", tr("The selected database is already opened, close it first."));
1205  return;
1206  }
1207  std::string errorMsg;
1208  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
1209  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
1210  progressDialog->setMaximumSteps(100);
1211  progressDialog->show();
1212  progressDialog->setCancelButtonVisible(true);
1213  RecoveryState state(progressDialog);
1214  if(databaseRecovery(path.toStdString(), false, &errorMsg, &state))
1215  {
1216  QMessageBox::information(this, "Database recovery", tr("Database \"%1\" recovered! Try opening it again.").arg(path));
1217  }
1218  else
1219  {
1220  QMessageBox::warning(this, "Database recovery", tr("Database recovery failed: \"%1\".").arg(errorMsg.c_str()));
1221  }
1222  progressDialog->setValue(progressDialog->maximumSteps());
1223  }
1224 }
1225 
1226 void DatabaseViewer::closeEvent(QCloseEvent* event)
1227 {
1228  //write settings before quit?
1229  bool save = false;
1230  if(this->isWindowModified())
1231  {
1232  QMessageBox::Button b=QMessageBox::question(this,
1233  tr("Database Viewer"),
1234  tr("There are unsaved changed settings. Save them?"),
1235  QMessageBox::Save | QMessageBox::Cancel | QMessageBox::Discard);
1236  if(b == QMessageBox::Save)
1237  {
1238  save = true;
1239  }
1240  else if(b != QMessageBox::Discard)
1241  {
1242  event->ignore();
1243  return;
1244  }
1245  }
1246 
1247  if(save)
1248  {
1249  writeSettings();
1250  }
1251 
1252  event->accept();
1253 
1254  if(!closeDatabase())
1255  {
1256  event->ignore();
1257  }
1258 
1259  if(event->isAccepted())
1260  {
1261  ui_->toolBox_statistics->closeFigures();
1262  if(dbDriver_)
1263  {
1264  delete dbDriver_;
1265  dbDriver_ = 0;
1266  }
1267  }
1268 }
1269 
1270 void DatabaseViewer::showEvent(QShowEvent* anEvent)
1271 {
1272  this->setWindowModified(false);
1273 
1274  if((ui_->graphViewer->isVisible() || ui_->dockWidget_occupancyGridView->isVisible()) && graphes_.size() && localMaps_.size()==0)
1275  {
1276  sliderIterationsValueChanged((int)graphes_.size()-1);
1277  }
1278 }
1279 
1280 void DatabaseViewer::moveEvent(QMoveEvent* anEvent)
1281 {
1282  if(this->isVisible())
1283  {
1284  // HACK, there is a move event when the window is shown the first time.
1285  if(!firstCall_)
1286  {
1287  this->configModified();
1288  }
1289  firstCall_ = false;
1290  }
1291 }
1292 
1293 void DatabaseViewer::resizeEvent(QResizeEvent* anEvent)
1294 {
1295  if(this->isVisible())
1296  {
1297  this->configModified();
1298  }
1299 }
1300 
1301 void DatabaseViewer::keyPressEvent(QKeyEvent *event)
1302 {
1303  //catch ctrl-s to save settings
1304  if((event->modifiers() & Qt::ControlModifier) && event->key() == Qt::Key_S)
1305  {
1306  this->writeSettings();
1307  }
1308 }
1309 
1310 bool DatabaseViewer::eventFilter(QObject *obj, QEvent *event)
1311 {
1312  if (event->type() == QEvent::Resize && qobject_cast<QDockWidget*>(obj))
1313  {
1314  this->setWindowModified(true);
1315  }
1316  return QWidget::eventFilter(obj, event);
1317 }
1318 
1319 
1320 void DatabaseViewer::exportDatabase()
1321 {
1322  if(!dbDriver_ || ids_.size() == 0)
1323  {
1324  return;
1325  }
1326 
1327  rtabmap::ExportDialog dialog;
1328 
1329  if(dialog.exec())
1330  {
1331  if(!dialog.outputPath().isEmpty())
1332  {
1333  int framesIgnored = dialog.framesIgnored();
1334  double frameRate = dialog.targetFramerate();
1335  int sessionExported = dialog.sessionExported();
1336  QString path = dialog.outputPath();
1337  rtabmap::DataRecorder recorder;
1338  QList<int> ids;
1339 
1340  double previousStamp = 0;
1341  std::vector<double> delays(ids_.size());
1342  int oi=0;
1343  std::map<int, Transform> poses;
1344  std::map<int, double> stamps;
1345  std::map<int, Transform> groundTruths;
1346  std::map<int, GPS> gpsValues;
1347  std::map<int, EnvSensors> sensorsValues;
1348  for(int i=0; i<ids_.size(); i+=1+framesIgnored)
1349  {
1350  Transform odomPose, groundTruth;
1351  int weight = -1;
1352  int mapId = -1;
1353  std::string label;
1354  double stamp = 0;
1355  std::vector<float> velocity;
1356  GPS gps;
1357  EnvSensors sensors;
1358  if(dbDriver_->getNodeInfo(ids_[i], odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors))
1359  {
1360  if(frameRate == 0 ||
1361  previousStamp == 0 ||
1362  stamp == 0 ||
1363  stamp - previousStamp >= 1.0/frameRate)
1364  {
1365  if(sessionExported < 0 || sessionExported == mapId)
1366  {
1367  ids.push_back(ids_[i]);
1368 
1369  if(previousStamp && stamp)
1370  {
1371  delays[oi++] = stamp - previousStamp;
1372  }
1373  previousStamp = stamp;
1374 
1375  poses.insert(std::make_pair(ids_[i], odomPose));
1376  stamps.insert(std::make_pair(ids_[i], stamp));
1377  groundTruths.insert(std::make_pair(ids_[i], groundTruth));
1378  if(gps.stamp() > 0.0)
1379  {
1380  gpsValues.insert(std::make_pair(ids_[i], gps));
1381  }
1382  if(sensors.size())
1383  {
1384  sensorsValues.insert(std::make_pair(ids_[i], sensors));
1385  }
1386  }
1387  }
1388  if(sessionExported >= 0 && mapId > sessionExported)
1389  {
1390  break;
1391  }
1392  }
1393  }
1394  delays.resize(oi);
1395 
1396  if(recorder.init(path, false))
1397  {
1398  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
1399  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
1400  progressDialog->setMaximumSteps(ids.size());
1401  progressDialog->show();
1402  progressDialog->setCancelButtonVisible(true);
1403  UINFO("Decompress: rgb=%d depth=%d scan=%d userData=%d",
1404  dialog.isRgbExported()?1:0,
1405  dialog.isDepthExported()?1:0,
1406  dialog.isDepth2dExported()?1:0,
1407  dialog.isUserDataExported()?1:0);
1408 
1409  for(int i=0; i<ids.size() && !progressDialog->isCanceled(); ++i)
1410  {
1411  int id = ids.at(i);
1412 
1413  SensorData data;
1414  dbDriver_->getNodeData(id, data);
1415  cv::Mat depth, rgb, userData;
1416  LaserScan scan;
1417  data.uncompressDataConst(
1418  !dialog.isRgbExported()?0:&rgb,
1419  !dialog.isDepthExported()?0:&depth,
1420  !dialog.isDepth2dExported()?0:&scan,
1421  !dialog.isUserDataExported()?0:&userData);
1422  cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
1423  if(dialog.isOdomExported())
1424  {
1425  std::multimap<int, Link> links;
1426  dbDriver_->loadLinks(id, links, Link::kNeighbor);
1427  if(links.size() && links.begin()->first < id)
1428  {
1429  covariance = links.begin()->second.infMatrix().inv();
1430  }
1431  }
1432 
1433  rtabmap::SensorData sensorData;
1434  if(data.cameraModels().size())
1435  {
1436  sensorData = rtabmap::SensorData(
1437  scan,
1438  rgb,
1439  depth,
1440  data.cameraModels(),
1441  id,
1442  stamps.at(id),
1443  userData);
1444  }
1445  else
1446  {
1447  sensorData = rtabmap::SensorData(
1448  scan,
1449  rgb,
1450  depth,
1451  data.stereoCameraModels(),
1452  id,
1453  stamps.at(id),
1454  userData);
1455  }
1456  if(groundTruths.find(id)!=groundTruths.end())
1457  {
1458  sensorData.setGroundTruth(groundTruths.at(id));
1459  }
1460  if(gpsValues.find(id)!=gpsValues.end())
1461  {
1462  sensorData.setGPS(gpsValues.at(id));
1463  }
1464  if(sensorsValues.find(id)!=sensorsValues.end())
1465  {
1466  sensorData.setEnvSensors(sensorsValues.at(id));
1467  }
1468 
1469  recorder.addData(sensorData, dialog.isOdomExported()?poses.at(id):Transform(), covariance);
1470 
1471  progressDialog->appendText(tr("Exported node %1").arg(id));
1472  progressDialog->incrementStep();
1473  QApplication::processEvents();
1474  }
1475  progressDialog->setValue(progressDialog->maximumSteps());
1476  if(delays.size())
1477  {
1478  progressDialog->appendText(tr("Average frame rate=%1 Hz (Min=%2, Max=%3)")
1479  .arg(1.0/uMean(delays)).arg(1.0/uMax(delays)).arg(1.0/uMin(delays)));
1480  }
1481  progressDialog->appendText(tr("Export finished to \"%1\"!").arg(path));
1482  }
1483  else
1484  {
1485  UERROR("DataRecorder init failed?!");
1486  }
1487  }
1488  else
1489  {
1490  QMessageBox::warning(this, tr("Cannot export database"), tr("An output path must be set!"));
1491  }
1492  }
1493 }
1494 
1495 void DatabaseViewer::extractImages()
1496 {
1497  if(!dbDriver_ || ids_.size() == 0)
1498  {
1499  return;
1500  }
1501 
1502  QStringList formats;
1503  formats.push_back("id.jpg");
1504  formats.push_back("id.png");
1505  formats.push_back("timestamp.jpg");
1506  formats.push_back("timestamp.png");
1507  bool ok;
1508  QString format = QInputDialog::getItem(this, tr("Which RGB format?"), tr("Format:"), formats, 0, false, &ok);
1509  if(!ok)
1510  {
1511  return;
1512  }
1513 
1514  QString ext = format.split('.').back();
1515  bool useStamp = format.split('.').front().compare("timestamp") == 0;
1516  bool directoriesCreated = false;
1517  QString path = QFileDialog::getExistingDirectory(this, tr("Select directory where to save images..."), pathDatabase_);
1518  if(!path.isEmpty())
1519  {
1520  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
1521  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
1522  progressDialog->setMaximumSteps(ids_.size());
1523  progressDialog->setCancelButtonVisible(true);
1524  progressDialog->appendText(tr("Saving %1 images to %2...").arg(ids_.size()).arg(path));
1525  progressDialog->show();
1526 
1527  int imagesExported = 0;
1528  for(int i=0; i<ids_.size(); ++i)
1529  {
1530  QString id = QString::number(ids_.at(i));
1531 
1532  SensorData data;
1533  dbDriver_->getNodeData(ids_.at(i), data);
1534  data.uncompressData();
1535 
1536  if(!directoriesCreated)
1537  {
1538  //stereo
1539  if(!data.imageRaw().empty() && !data.rightRaw().empty())
1540  {
1541  QDir dir;
1542  dir.mkdir(QString("%1/left").arg(path));
1543  dir.mkdir(QString("%1/right").arg(path));
1544  dir.mkdir(QString("%1/calib").arg(path));
1545  directoriesCreated = true;
1546  }
1547  else if(!data.imageRaw().empty())
1548  {
1549  if(!data.depthRaw().empty())
1550  {
1551  QDir dir;
1552  dir.mkdir(QString("%1/rgb").arg(path));
1553  dir.mkdir(QString("%1/depth").arg(path));
1554  dir.mkdir(QString("%1/calib").arg(path));
1555  directoriesCreated = true;
1556  }
1557  }
1558  }
1559 
1560  if(!data.imageRaw().empty() && useStamp)
1561  {
1562  Transform p,gt;
1563  int m,w;
1564  std::string l;
1565  double stamp;
1566  std::vector<float> v;
1567  GPS gps;
1568  EnvSensors s;
1569  dbDriver_->getNodeInfo(ids_.at(i), p, m, w, l, stamp, gt, v, gps, s);
1570  if(stamp == 0.0)
1571  {
1572  UWARN("Node %d has null timestamp! Using id instead!", ids_.at(i));
1573  }
1574  else
1575  {
1576  id = QString::number(stamp, 'f');
1577  }
1578  }
1579 
1580  if(!data.imageRaw().empty())
1581  {
1582  if(!data.rightRaw().empty())
1583  {
1584  if(!cv::imwrite(QString("%1/left/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw()))
1585  UWARN("Failed saving \"%s\"", QString("%1/left/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
1586  if(!cv::imwrite(QString("%1/right/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.rightRaw()))
1587  UWARN("Failed saving \"%s\"", QString("%1/right/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
1588  UINFO(QString("Saved left/%1.%2 and right/%1.%2").arg(id).arg(ext).toStdString().c_str());
1589 
1590  if(databaseFileName_.empty())
1591  {
1592  UERROR("Cannot save calibration file, database name is empty!");
1593  }
1594  else if(data.stereoCameraModels().size()>=1 && data.stereoCameraModels().front().isValidForProjection())
1595  {
1596  for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
1597  {
1598  std::string cameraName = id.toStdString();
1599  if(data.stereoCameraModels().size()>1)
1600  {
1601  cameraName+="_"+uNumber2Str((int)i);
1602  }
1603  StereoCameraModel model(
1604  cameraName,
1605  data.imageRaw().size(),
1606  data.stereoCameraModels()[i].left().K_raw(),
1607  data.stereoCameraModels()[i].left().D_raw(),
1608  data.stereoCameraModels()[i].left().R(),
1609  data.stereoCameraModels()[i].left().P(),
1610  data.rightRaw().size(),
1611  data.stereoCameraModels()[i].right().K_raw(),
1612  data.stereoCameraModels()[i].right().D_raw(),
1613  data.stereoCameraModels()[i].right().R(),
1614  data.stereoCameraModels()[i].right().P(),
1615  data.stereoCameraModels()[i].R(),
1616  data.stereoCameraModels()[i].T(),
1617  data.stereoCameraModels()[i].E(),
1618  data.stereoCameraModels()[i].F(),
1619  data.stereoCameraModels()[i].left().localTransform());
1620  if(model.save(path.toStdString() + "/calib"))
1621  {
1622  UINFO("Saved stereo calibration \"%s\"", (path.toStdString()+"/calib/"+cameraName+".yaml").c_str());
1623  }
1624  else
1625  {
1626  UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/calib/"+cameraName+".yaml").c_str());
1627  }
1628  }
1629  }
1630  }
1631  else
1632  {
1633  if(!data.depthRaw().empty())
1634  {
1635  if(!cv::imwrite(QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw()))
1636  UWARN("Failed saving \"%s\"", QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
1637  if(!cv::imwrite(QString("%1/depth/%2.png").arg(path).arg(id).toStdString(), data.depthRaw().type()==CV_32FC1?util2d::cvtDepthFromFloat(data.depthRaw()):data.depthRaw()))
1638  UWARN("Failed saving \"%s\"", QString("%1/depth/%2.png").arg(path).arg(id).toStdString().c_str());
1639  UINFO(QString("Saved rgb/%1.%2 and depth/%1.png").arg(id).arg(ext).toStdString().c_str());
1640  }
1641  else
1642  {
1643  if(!cv::imwrite(QString("%1/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw()))
1644  UWARN("Failed saving \"%s\"", QString("%1/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
1645  else
1646  UINFO(QString("Saved %1.%2").arg(id).arg(ext).toStdString().c_str());
1647  }
1648 
1649  if(databaseFileName_.empty())
1650  {
1651  UERROR("Cannot save calibration file, database name is empty!");
1652  }
1653  else if(data.cameraModels().size() >= 1 && data.cameraModels().front().isValidForProjection())
1654  {
1655  for(size_t i=0; i<data.cameraModels().size(); ++i)
1656  {
1657  std::string cameraName = id.toStdString();
1658  if(data.cameraModels().size()>1)
1659  {
1660  cameraName+="_"+uNumber2Str((int)i);
1661  }
1662  CameraModel model(cameraName,
1663  data.imageRaw().size(),
1664  data.cameraModels()[i].K_raw(),
1665  data.cameraModels()[i].D_raw(),
1666  data.cameraModels()[i].R(),
1667  data.cameraModels()[i].P(),
1668  data.cameraModels()[i].localTransform());
1669  std::string dirPrefix = "";
1670  if(!data.depthRaw().empty())
1671  {
1672  dirPrefix = "/calib";
1673  }
1674  if(model.save(path.toStdString()+dirPrefix))
1675  {
1676  UINFO("Saved calibration \"%s\"", (path.toStdString()+dirPrefix+"/"+cameraName+".yaml").c_str());
1677  }
1678  else
1679  {
1680  UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName+".yaml").c_str());
1681  }
1682  }
1683  }
1684  }
1685  ++imagesExported;
1686  }
1687  progressDialog->incrementStep();
1688  QApplication::processEvents();
1689  if(progressDialog->isCanceled())
1690  {
1691  progressDialog->appendText("Cancel!");
1692  break;
1693  }
1694  }
1695  progressDialog->appendText("Done!");
1696  progressDialog->setValue(progressDialog->maximumSteps());
1697 
1698  QMessageBox::information(this, tr("Exporting"), tr("%1 images exported!").arg(imagesExported));
1699  }
1700 }
1701 
1702 void DatabaseViewer::updateIds()
1703 {
1704  if(!dbDriver_)
1705  {
1706  return;
1707  }
1708 
1709  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
1710  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
1711  int progressSteps = 5;
1712  if(ui_->graphViewer->isVisible() || ui_->dockWidget_occupancyGridView->isVisible())
1713  {
1714  ++progressSteps;
1715  }
1716  if(ui_->textEdit_info->isVisible())
1717  {
1718  ++progressSteps;
1719  }
1720  if(ui_->toolBox_statistics->isVisible())
1721  {
1722  ++progressSteps;
1723  }
1724  progressDialog->setMaximumSteps(progressSteps);
1725  progressDialog->show();
1726  progressDialog->setCancelButtonVisible(false);
1727 
1728  progressDialog->appendText(tr("Loading all ids..."));
1729  QApplication::processEvents();
1730  uSleep(100);
1731  QApplication::processEvents();
1732 
1733  UINFO("Loading all IDs...");
1734  std::set<int> ids;
1735  dbDriver_->getAllNodeIds(ids);
1736 #if QT_VERSION >= QT_VERSION_CHECK(5, 15, 3)
1737  ids_ = QList<int>(ids.begin(), ids.end());
1738 #else
1739  ids_ = QList<int>::fromStdList(std::list<int>(ids.begin(), ids.end()));
1740 #endif
1741  lastWmIds_.clear();
1742  dbDriver_->getLastNodeIds(lastWmIds_);
1743  idToIndex_.clear();
1744  mapIds_.clear();
1745  weights_.clear();
1746  wmStates_.clear();
1747  odomPoses_.clear();
1748  groundTruthPoses_.clear();
1749  gpsPoses_.clear();
1750  gpsValues_.clear();
1751  lastSliderIndexBrowsed_ = 0;
1752  ui_->checkBox_wmState->setVisible(false);
1753  ui_->checkBox_alignPosesWithGPS->setVisible(false);
1754  ui_->checkBox_alignPosesWithGPS->setEnabled(false);
1755  ui_->checkBox_alignPosesWithGroundTruth->setVisible(false);
1756  ui_->checkBox_alignPosesWithGroundTruth->setEnabled(false);
1757  ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(false);
1758  ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(false);
1759  ui_->doubleSpinBox_optimizationScale->setVisible(false);
1760  ui_->label_scale_title->setVisible(false);
1761  ui_->label_rmse->setVisible(false);
1762  ui_->label_rmse_title->setVisible(false);
1763  ui_->checkBox_ignoreIntermediateNodes->setVisible(false);
1764  ui_->label_ignoreINtermediateNdoes->setVisible(false);
1765  ui_->label_alignPosesWithGPS->setVisible(false);
1766  ui_->label_alignPosesWithGroundTruth->setVisible(false);
1767  ui_->label_alignScansCloudsWithGroundTruth->setVisible(false);
1768  ui_->toolButton_edit_priorA->setVisible(false);
1769  ui_->toolButton_edit_priorB->setVisible(false);
1770  ui_->toolButton_remove_priorA->setVisible(false);
1771  ui_->toolButton_remove_priorB->setVisible(false);
1772  ui_->menuEdit->setEnabled(true);
1773  ui_->actionGenerate_3D_map_pcd->setEnabled(true);
1774  ui_->actionExport->setEnabled(true);
1775  ui_->actionExtract_images->setEnabled(true);
1776  ui_->menuExport_poses->setEnabled(false);
1777  ui_->menuExport_GPS->setEnabled(false);
1778  ui_->actionPoses_KML->setEnabled(false);
1779  ui_->actionEdit_optimized_2D_map->setEnabled(false);
1780  ui_->actionExport_saved_2D_map->setEnabled(false);
1781  ui_->actionImport_2D_map->setEnabled(false);
1782  ui_->actionRegenerate_optimized_2D_map->setEnabled(false);
1783  ui_->actionView_optimized_mesh->setEnabled(false);
1784  ui_->actionExport_optimized_mesh->setEnabled(false);
1785  ui_->actionUpdate_optimized_mesh->setEnabled(uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.13.0") >= 0);
1786  links_.clear();
1787  linksAdded_.clear();
1788  linksRefined_.clear();
1789  linksRemoved_.clear();
1790  ui_->toolBox_statistics->clear();
1791  ui_->label_optimizeFrom->setText(tr("Root"));
1792 
1793  progressDialog->appendText(tr("%1 ids loaded!").arg(ids.size()));
1794  progressDialog->incrementStep();
1795  progressDialog->appendText(tr("Loading all links..."));
1796  QApplication::processEvents();
1797  uSleep(100);
1798  QApplication::processEvents();
1799 
1800  std::multimap<int, Link> unilinks;
1801  dbDriver_->getAllLinks(unilinks, true, true);
1802  UDEBUG("%d total links loaded", (int)unilinks.size());
1803  // add both direction links
1804  std::multimap<int, Link> links;
1805  for(std::multimap<int, Link>::iterator iter=unilinks.begin(); iter!=unilinks.end(); ++iter)
1806  {
1807  links.insert(*iter);
1808  if(graph::findLink(unilinks, iter->second.to(), iter->second.from(), false) == unilinks.end())
1809  {
1810  links.insert(std::make_pair(iter->second.to(), iter->second.inverse()));
1811  }
1812  }
1813 
1814  progressDialog->appendText(tr("%1 links loaded!").arg(unilinks.size()));
1815  progressDialog->incrementStep();
1816  progressDialog->appendText("Loading Working Memory state...");
1817  QApplication::processEvents();
1818  uSleep(100);
1819  QApplication::processEvents();
1820 
1821  infoTotalOdom_ = 0.0;
1822  Transform previousPose;
1823  infoSessions_ = ids_.size()?1:0;
1824  infoTotalTime_ = 0.0;
1825  double previousStamp = 0.0;
1826  infoReducedGraph_ = false;
1827  std::map<int, std::vector<int> > wmStates = dbDriver_->getAllStatisticsWmStates();
1828 
1829  progressDialog->appendText("Loading Working Memory state... done!");
1830  progressDialog->incrementStep();
1831  progressDialog->appendText("Loading info for all nodes...");
1832  QApplication::processEvents();
1833  uSleep(100);
1834  QApplication::processEvents();
1835 
1836  int lastValidNodeId = 0;
1837  for(int i=0; i<ids_.size(); ++i)
1838  {
1839  idToIndex_.insert(ids_[i], i);
1840 
1841  Transform p, g;
1842  int w;
1843  std::string l;
1844  double s;
1845  int mapId;
1846  std::vector<float> v;
1847  GPS gps;
1848  EnvSensors sensors;
1849  dbDriver_->getNodeInfo(ids_[i], p, mapId, w, l, s, g, v, gps, sensors);
1850  mapIds_.insert(std::make_pair(ids_[i], mapId));
1851  weights_.insert(std::make_pair(ids_[i], w));
1852  if(w>=0)
1853  {
1854  for(std::multimap<int, Link>::iterator iter=links.find(ids_[i]); iter!=links.end() && iter->first==ids_[i]; ++iter)
1855  {
1856  // Make compatible with old databases, when "weight=-1" was not yet introduced to identify ignored nodes
1857  if(iter->second.type() == Link::kNeighbor || iter->second.type() == Link::kNeighborMerged)
1858  {
1859  lastValidNodeId = ids_[i];
1860  }
1861  }
1862  }
1863  if(wmStates.find(ids_[i]) != wmStates.end())
1864  {
1865  wmStates_.insert(std::make_pair(ids_[i], wmStates.at(ids_[i])));
1866  ui_->checkBox_wmState->setVisible(true);
1867  }
1868  if(w < 0)
1869  {
1870  ui_->checkBox_ignoreIntermediateNodes->setVisible(true);
1871  ui_->label_ignoreINtermediateNdoes->setVisible(true);
1872  }
1873  if(i>0)
1874  {
1875  if(mapIds_.at(ids_[i-1]) == mapId)
1876  {
1877  if(!p.isNull() && !previousPose.isNull())
1878  {
1879  infoTotalOdom_ += p.getDistance(previousPose);
1880  }
1881 
1882  if(previousStamp > 0.0 && s > 0.0)
1883  {
1884  infoTotalTime_ += s-previousStamp;
1885  }
1886  }
1887  else
1888  {
1889  ++infoSessions_;
1890  }
1891  }
1892  previousStamp=s;
1893  previousPose=p;
1894 
1895  //links
1896  for(std::multimap<int, Link>::iterator jter=links.find(ids_[i]); jter!=links.end() && jter->first == ids_[i]; ++jter)
1897  {
1898  if(jter->second.type() == Link::kNeighborMerged)
1899  {
1900  infoReducedGraph_ = true;
1901  }
1902 
1903  std::multimap<int, Link>::iterator invertedLinkIter = graph::findLink(links, jter->second.to(), jter->second.from(), false, jter->second.type());
1904  if( jter->second.isValid() && // null transform means a rehearsed location
1905  ids.find(jter->second.from()) != ids.end() &&
1906  (ids.find(jter->second.to()) != ids.end() || jter->second.to()<0) && // to add landmark links
1907  graph::findLink(links_, jter->second.from(), jter->second.to(), false, jter->second.type()) == links_.end() &&
1908  invertedLinkIter != links.end() &&
1909  w != -9)
1910  {
1911  // check if user_data is set in opposite direction
1912  if(jter->second.userDataCompressed().cols == 0 &&
1913  invertedLinkIter->second.userDataCompressed().cols != 0)
1914  {
1915  links_.insert(std::make_pair(invertedLinkIter->second.from(), invertedLinkIter->second));
1916  }
1917  else
1918  {
1919  links_.insert(std::make_pair(ids_[i], jter->second));
1920  }
1921  }
1922  }
1923  // Add pose
1924  odomPoses_.insert(std::make_pair(ids_[i], p));
1925  if(!g.isNull())
1926  {
1927  groundTruthPoses_.insert(std::make_pair(ids_[i], g));
1928  }
1929  if(gps.stamp() > 0.0)
1930  {
1931  gpsValues_.insert(std::make_pair(ids_[i], gps));
1932 
1933  cv::Point3f p(0.0f,0.0f,0.0f);
1934  if(!gpsPoses_.empty())
1935  {
1936  GeodeticCoords coords = gps.toGeodeticCoords();
1937  GPS originGPS = gpsValues_.begin()->second;
1938  p = coords.toENU_WGS84(originGPS.toGeodeticCoords());
1939  }
1940  Transform pose(p.x, p.y, p.z, 0.0f, 0.0f, (float)((-(gps.bearing()-90))*M_PI/180.0));
1941  gpsPoses_.insert(std::make_pair(ids_[i], pose));
1942  }
1943  }
1944 
1945  progressDialog->appendText("Loading info for all nodes... done!");
1946  progressDialog->incrementStep();
1947  progressDialog->appendText("Loading optimized poses and maps...");
1948  QApplication::processEvents();
1949  uSleep(100);
1950  QApplication::processEvents();
1951 
1952  ui_->doubleSpinBox_optimizationScale->setVisible(!groundTruthPoses_.empty());
1953  ui_->label_scale_title->setVisible(!groundTruthPoses_.empty());
1954  ui_->label_rmse->setVisible(!groundTruthPoses_.empty());
1955  ui_->label_rmse_title->setVisible(!groundTruthPoses_.empty());
1956 
1957  ui_->checkBox_alignPosesWithGPS->setVisible(!gpsPoses_.empty());
1958  ui_->checkBox_alignPosesWithGPS->setEnabled(!gpsPoses_.empty());
1959  ui_->label_alignPosesWithGPS->setVisible(!gpsPoses_.empty());
1960  ui_->checkBox_alignPosesWithGroundTruth->setVisible(!groundTruthPoses_.empty());
1961  ui_->checkBox_alignPosesWithGroundTruth->setEnabled(!groundTruthPoses_.empty());
1962  ui_->label_alignPosesWithGroundTruth->setVisible(!groundTruthPoses_.empty());
1963  ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(!groundTruthPoses_.empty());
1964  ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(!groundTruthPoses_.empty());
1965  ui_->label_alignScansCloudsWithGroundTruth->setVisible(!groundTruthPoses_.empty());
1966 
1967  if(!gpsValues_.empty())
1968  {
1969  ui_->menuExport_GPS->setEnabled(true);
1970  ui_->actionPoses_KML->setEnabled(true);
1971  }
1972 
1973  float xMin, yMin, cellSize;
1974  bool hasMap = !dbDriver_->load2DMap(xMin, yMin, cellSize).empty();
1975  ui_->actionEdit_optimized_2D_map->setEnabled(hasMap);
1976  ui_->actionExport_saved_2D_map->setEnabled(hasMap);
1977  ui_->actionImport_2D_map->setEnabled(hasMap);
1978  ui_->actionRegenerate_optimized_2D_map->setEnabled(uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.17.0") >= 0);
1979 
1980  if(!dbDriver_->loadOptimizedMesh().empty())
1981  {
1982  ui_->actionView_optimized_mesh->setEnabled(true);
1983  ui_->actionExport_optimized_mesh->setEnabled(true);
1984  }
1985 
1986  UINFO("Loaded %d ids, %d poses and %d links", (int)ids_.size(), (int)odomPoses_.size(), (int)links_.size());
1987 
1988  progressDialog->appendText("Loading optimized poses and maps... done!");
1989  progressDialog->incrementStep();
1990  QApplication::processEvents();
1991  uSleep(100);
1992  QApplication::processEvents();
1993 
1994  if(ids_.size() && ui_->toolBox_statistics->isVisible())
1995  {
1996  progressDialog->appendText("Loading statistics...");
1997  QApplication::processEvents();
1998  uSleep(100);
1999  QApplication::processEvents();
2000 
2001  UINFO("Update statistics...");
2002  updateStatistics();
2003 
2004  progressDialog->appendText("Loading statistics... done!");
2005  progressDialog->incrementStep();
2006  QApplication::processEvents();
2007  uSleep(100);
2008  QApplication::processEvents();
2009  }
2010 
2011 
2012  ui_->textEdit_info->clear();
2013  if(ui_->textEdit_info->isVisible())
2014  {
2015  progressDialog->appendText("Update database info...");
2016  QApplication::processEvents();
2017  uSleep(100);
2018  QApplication::processEvents();
2019 
2020  updateInfo();
2021 
2022  progressDialog->appendText("Update database info... done!");
2023  progressDialog->incrementStep();
2024  QApplication::processEvents();
2025  uSleep(100);
2026  QApplication::processEvents();
2027  }
2028 
2029  if(ids.size())
2030  {
2031  if(odomPoses_.size())
2032  {
2033  bool nullPoses = odomPoses_.begin()->second.isNull();
2034  for(std::map<int,Transform>::iterator iter=odomPoses_.begin(); iter!=odomPoses_.end(); ++iter)
2035  {
2036  if((!iter->second.isNull() && nullPoses) ||
2037  (iter->second.isNull() && !nullPoses))
2038  {
2039  if(iter->second.isNull())
2040  {
2041  UWARN("Pose %d is null!", iter->first);
2042  }
2043  UWARN("Mixed valid and null poses! Ignoring graph...");
2044  odomPoses_.clear();
2045  links_.clear();
2046  break;
2047  }
2048  }
2049  if(nullPoses)
2050  {
2051  odomPoses_.clear();
2052  links_.clear();
2053  }
2054 
2055  if(odomPoses_.size())
2056  {
2057  ui_->spinBox_optimizationsFrom->setRange(odomPoses_.begin()->first, odomPoses_.rbegin()->first);
2058  ui_->spinBox_optimizationsFrom->setValue(odomPoses_.begin()->first);
2059  ui_->label_optimizeFrom->setText(tr("Root [%1, %2]").arg(odomPoses_.begin()->first).arg(odomPoses_.rbegin()->first));
2060  }
2061  }
2062 
2063  if(lastValidNodeId>0)
2064  {
2065  // find full connected graph from last node in working memory
2066  Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
2067 
2068  std::map<int, rtabmap::Transform> posesOut;
2069  std::multimap<int, rtabmap::Link> linksOut;
2070  UINFO("Get connected graph from %d (%d poses, %d links)", lastValidNodeId, (int)odomPoses_.size(), (int)links_.size());
2071  optimizer->getConnectedGraph(
2072  lastValidNodeId,
2073  odomPoses_,
2074  links_,
2075  posesOut,
2076  linksOut);
2077 
2078  if(!posesOut.empty())
2079  {
2080  bool optimizeFromGraphEnd = Parameters::defaultRGBDOptimizeFromGraphEnd();
2081  Parameters::parse(dbDriver_->getLastParameters(), Parameters::kRGBDOptimizeFromGraphEnd(), optimizeFromGraphEnd);
2082  if(optimizeFromGraphEnd)
2083  {
2084  ui_->spinBox_optimizationsFrom->setValue(posesOut.rbegin()->first);
2085  }
2086  else
2087  {
2088  ui_->spinBox_optimizationsFrom->setValue(posesOut.lower_bound(1)->first);
2089  }
2090  }
2091 
2092  delete optimizer;
2093  }
2094  }
2095 
2096  ui_->menuExport_poses->setEnabled(!odomPoses_.empty());
2097  graphes_.clear();
2098  graphLinks_.clear();
2099  neighborLinks_.clear();
2100  loopLinks_.clear();
2101  for(std::multimap<int, rtabmap::Link>::iterator iter = links_.begin(); iter!=links_.end(); ++iter)
2102  {
2103  if(!iter->second.transform().isNull())
2104  {
2105  if(iter->second.type() == rtabmap::Link::kNeighbor ||
2106  iter->second.type() == rtabmap::Link::kNeighborMerged)
2107  {
2108  neighborLinks_.append(iter->second);
2109  }
2110  else if(iter->second.from()!=iter->second.to())
2111  {
2112  loopLinks_.append(iter->second);
2113  }
2114  }
2115  else
2116  {
2117  UERROR("Transform null for link from %d to %d", iter->first, iter->second.to());
2118  }
2119  }
2120 
2121  if(ids_.size())
2122  {
2123  ui_->horizontalSlider_A->setMinimum(0);
2124  ui_->horizontalSlider_B->setMinimum(0);
2125  ui_->horizontalSlider_A->setMaximum(ids_.size()-1);
2126  ui_->horizontalSlider_B->setMaximum(ids_.size()-1);
2127  ui_->horizontalSlider_A->setEnabled(true);
2128  ui_->horizontalSlider_B->setEnabled(true);
2129  ui_->horizontalSlider_A->setSliderPosition(0);
2130  ui_->horizontalSlider_B->setSliderPosition(0);
2131  sliderAValueChanged(0);
2132  sliderBValueChanged(0);
2133  }
2134  else
2135  {
2136  ui_->horizontalSlider_A->setEnabled(false);
2137  ui_->horizontalSlider_B->setEnabled(false);
2138  ui_->label_idA->setText("NaN");
2139  ui_->label_idB->setText("NaN");
2140  }
2141 
2142  if(neighborLinks_.size())
2143  {
2144  ui_->horizontalSlider_neighbors->setMinimum(0);
2145  ui_->horizontalSlider_neighbors->setMaximum(neighborLinks_.size()-1);
2146  ui_->horizontalSlider_neighbors->setEnabled(true);
2147  ui_->horizontalSlider_neighbors->setSliderPosition(0);
2148  }
2149  else
2150  {
2151  ui_->horizontalSlider_neighbors->setEnabled(false);
2152  }
2153 
2154  if(ids_.size())
2155  {
2156  updateLoopClosuresSlider();
2157  if(ui_->graphViewer->isVisible() || ui_->dockWidget_occupancyGridView->isVisible())
2158  {
2159  progressDialog->appendText("Updating Graph View...");
2160  QApplication::processEvents();
2161  uSleep(100);
2162  QApplication::processEvents();
2163 
2164  updateGraphView();
2165 
2166  progressDialog->appendText("Updating Graph View... done!");
2167  progressDialog->incrementStep();
2168  QApplication::processEvents();
2169  uSleep(100);
2170  QApplication::processEvents();
2171  }
2172  }
2173  progressDialog->setValue(progressDialog->maximumSteps());
2174 }
2175 
2176 void DatabaseViewer::updateInfo()
2177 {
2178  UINFO("Update database info...");
2179  if(dbDriver_)
2180  {
2181  if(ui_->textEdit_info->toPlainText().isEmpty())
2182  {
2183  ui_->textEdit_info->append(tr("Path:\t\t%1").arg(dbDriver_->getUrl().c_str()));
2184  ui_->textEdit_info->append(tr("Version:\t\t%1").arg(dbDriver_->getDatabaseVersion().c_str()));
2185  ui_->textEdit_info->append(tr("Sessions:\t\t%1").arg(infoSessions_));
2186  if(infoReducedGraph_)
2187  {
2188  ui_->textEdit_info->append(tr("Total odometry length:\t%1 m (approx. as graph has been reduced)").arg(infoTotalOdom_));
2189  }
2190  else
2191  {
2192  ui_->textEdit_info->append(tr("Total odometry length:\t%1 m").arg(infoTotalOdom_));
2193  }
2194 
2195  int lastWordIdId = 0;
2196  int wordsDim = 0;
2197  int wordsType = 0;
2198  dbDriver_->getLastWordId(lastWordIdId);
2199  if(lastWordIdId>0)
2200  {
2201  std::set<int> ids;
2202  ids.insert(lastWordIdId);
2203  std::list<VisualWord *> vws;
2204  dbDriver_->loadWords(ids, vws);
2205  if(!vws.empty())
2206  {
2207  wordsDim = vws.front()->getDescriptor().cols;
2208  wordsType = vws.front()->getDescriptor().type();
2209  delete vws.front();
2210  vws.clear();
2211  }
2212  }
2213 
2214  ui_->textEdit_info->append(tr("Total time:\t\t%1").arg(QDateTime::fromMSecsSinceEpoch(infoTotalTime_*1000).toUTC().toString("hh:mm:ss.zzz")));
2215  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()));
2216  ui_->textEdit_info->append(tr("WM:\t\t%1 nodes and %2 words").arg(dbDriver_->getLastNodesSize()).arg(dbDriver_->getLastDictionarySize()));
2217  ui_->textEdit_info->append(tr("Global graph:\t%1 poses and %2 links").arg(odomPoses_.size()).arg(links_.size()));
2218  ui_->textEdit_info->append(tr("Ground truth:\t%1 poses").arg(groundTruthPoses_.size()));
2219  ui_->textEdit_info->append(tr("GPS:\t%1 poses").arg(gpsValues_.size()));
2220  ui_->textEdit_info->append("");
2221  long total = 0;
2222  long dbSize = UFile::length(dbDriver_->getUrl());
2223  long mem = dbSize;
2224  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"));
2225  mem = dbDriver_->getNodesMemoryUsed();
2226  total+=mem;
2227  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"));
2228  mem = dbDriver_->getLinksMemoryUsed();
2229  total+=mem;
2230  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"));
2231  mem = dbDriver_->getImagesMemoryUsed();
2232  total+=mem;
2233  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"));
2234  mem = dbDriver_->getDepthImagesMemoryUsed();
2235  total+=mem;
2236  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"));
2237  mem = dbDriver_->getCalibrationsMemoryUsed();
2238  total+=mem;
2239  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"));
2240  mem = dbDriver_->getGridsMemoryUsed();
2241  total+=mem;
2242  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"));
2243  mem = dbDriver_->getLaserScansMemoryUsed();
2244  total+=mem;
2245  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"));
2246  mem = dbDriver_->getUserDataMemoryUsed();
2247  total+=mem;
2248  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"));
2249  mem = dbDriver_->getWordsMemoryUsed();
2250  total+=mem;
2251  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"));
2252  mem = dbDriver_->getFeaturesMemoryUsed();
2253  total+=mem;
2254  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"));
2255  mem = dbDriver_->getStatisticsMemoryUsed();
2256  total+=mem;
2257  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"));
2258  mem = dbSize - total;
2259  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"));
2260  ui_->textEdit_info->append("");
2261  std::set<int> idsWithoutBad;
2262  dbDriver_->getAllNodeIds(idsWithoutBad, false, true);
2263  int infoBadcountInLTM = 0;
2264  int infoBadCountInGraph = 0;
2265  for(int i=0; i<ids_.size(); ++i)
2266  {
2267  if(idsWithoutBad.find(ids_[i]) == idsWithoutBad.end())
2268  {
2269  ++infoBadcountInLTM;
2270  if(odomPoses_.find(ids_[i]) != odomPoses_.end())
2271  {
2272  ++infoBadCountInGraph;
2273  }
2274  }
2275  }
2276  ui_->textEdit_info->append(tr("%1 bad signatures in LTM").arg(infoBadcountInLTM));
2277  ui_->textEdit_info->append(tr("%1 bad signatures in the global graph").arg(infoBadCountInGraph));
2278  ui_->textEdit_info->append("");
2279  ParametersMap parameters = dbDriver_->getLastParameters();
2280  QFontMetrics metrics(ui_->textEdit_info->font());
2281 #if QT_VERSION >= QT_VERSION_CHECK(5, 15, 0)
2282  int tabW = ui_->textEdit_info->tabStopDistance();
2283 #else
2284  int tabW = ui_->textEdit_info->tabStopWidth();
2285 #endif
2286  for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
2287  {
2288 #if QT_VERSION >= QT_VERSION_CHECK(5, 15, 0)
2289  int strW = metrics.horizontalAdvance(QString(iter->first.c_str()) + "=");
2290 #else
2291  int strW = metrics.width(QString(iter->first.c_str()) + "=");
2292 #endif
2293  ui_->textEdit_info->append(tr("%1=%2%3")
2294  .arg(iter->first.c_str())
2295  .arg(strW < tabW?"\t\t\t\t":strW < tabW*2?"\t\t\t":strW < tabW*3?"\t\t":"\t")
2296  .arg(iter->second.c_str()));
2297  }
2298 
2299  // move back the cursor at the beginning
2300  ui_->textEdit_info->moveCursor(QTextCursor::Start) ;
2301  ui_->textEdit_info->ensureCursorVisible() ;
2302  }
2303  }
2304  else
2305  {
2306  ui_->textEdit_info->clear();
2307  }
2308 }
2309 
2310 void DatabaseViewer::updateStatistics()
2311 {
2312  UDEBUG("");
2313  if(dbDriver_)
2314  {
2315  ui_->toolBox_statistics->clear();
2316  double firstStamp = 0.0;
2317  std::map<int, std::pair<std::map<std::string, float>, double> > allStats = dbDriver_->getAllStatistics();
2318 
2319  std::map<std::string, std::pair<std::vector<qreal>, std::vector<qreal> > > allData;
2320  std::map<std::string, int > allDataOi;
2321 
2322  for(std::map<int, std::pair<std::map<std::string, float>, double> >::iterator jter=allStats.begin(); jter!=allStats.end(); ++jter)
2323  {
2324  double stamp=jter->second.second;
2325  std::map<std::string, float> & statistics = jter->second.first;
2326  if(firstStamp==0.0)
2327  {
2328  firstStamp = stamp;
2329  }
2330  for(std::map<std::string, float>::iterator iter=statistics.begin(); iter!=statistics.end(); ++iter)
2331  {
2332  if(allData.find(iter->first) == allData.end())
2333  {
2334  //initialize data vectors
2335  allData.insert(std::make_pair(iter->first, std::make_pair(std::vector<qreal>(allStats.size(), 0.0f), std::vector<qreal>(allStats.size(), 0.0f) )));
2336  allDataOi.insert(std::make_pair(iter->first, 0));
2337  }
2338 
2339  int & oi = allDataOi.at(iter->first);
2340  allData.at(iter->first).first[oi] = ui_->checkBox_timeStats->isChecked()?qreal(stamp-firstStamp):jter->first;
2341  allData.at(iter->first).second[oi] = iter->second;
2342  ++oi;
2343  }
2344  }
2345 
2346  for(std::map<std::string, std::pair<std::vector<qreal>, std::vector<qreal> > >::iterator iter=allData.begin(); iter!=allData.end(); ++iter)
2347  {
2348  int oi = allDataOi.at(iter->first);
2349  iter->second.first.resize(oi);
2350  iter->second.second.resize(oi);
2351  ui_->toolBox_statistics->updateStat(iter->first.c_str(), iter->second.first, iter->second.second, true);
2352  }
2353  }
2354  UDEBUG("");
2355 }
2356 
2357 void DatabaseViewer::selectObstacleColor()
2358 {
2359  QColor c = QColorDialog::getColor(ui_->lineEdit_obstacleColor->text(), this);
2360  if(c.isValid())
2361  {
2362  ui_->lineEdit_obstacleColor->setText(c.name());
2363  }
2364 }
2365 void DatabaseViewer::selectGroundColor()
2366 {
2367  QColor c = QColorDialog::getColor(ui_->lineEdit_groundColor->text(), this);
2368  if(c.isValid())
2369  {
2370  ui_->lineEdit_groundColor->setText(c.name());
2371  }
2372 }
2373 void DatabaseViewer::selectEmptyColor()
2374 {
2375  QColor c = QColorDialog::getColor(ui_->lineEdit_emptyColor->text(), this);
2376  if(c.isValid())
2377  {
2378  ui_->lineEdit_emptyColor->setText(c.name());
2379  }
2380 }
2381 void DatabaseViewer::selectFrontierColor()
2382 {
2383  QColor c = QColorDialog::getColor(ui_->lineEdit_frontierColor->text(), this);
2384  if(c.isValid())
2385  {
2386  ui_->lineEdit_frontierColor->setText(c.name());
2387  }
2388 }
2389 void DatabaseViewer::editDepthImage()
2390 {
2391  if(dbDriver_ && ids_.size())
2392  {
2393  if(lastSliderIndexBrowsed_>= ids_.size())
2394  {
2395  lastSliderIndexBrowsed_ = ui_->horizontalSlider_A->value();
2396  }
2397  int id = ids_.at(lastSliderIndexBrowsed_);
2398  SensorData data;
2399  dbDriver_->getNodeData(id, data, true, false, false, false);
2400  data.uncompressData();
2401  if(!data.depthRaw().empty())
2402  {
2403  editDepthArea_->setColorMap(lastSliderIndexBrowsed_ == ui_->horizontalSlider_B->value()?ui_->graphicsView_B->getDepthColorMap():ui_->graphicsView_A->getDepthColorMap());
2404  editDepthArea_->setImage(data.depthRaw(), data.imageRaw());
2405  if(editDepthDialog_->exec() == QDialog::Accepted && editDepthArea_->isModified())
2406  {
2407  cv::Mat depth = editDepthArea_->getModifiedImage();
2408  UASSERT(data.depthRaw().type() == depth.type());
2409  UASSERT(data.depthRaw().cols == depth.cols);
2410  UASSERT(data.depthRaw().rows == depth.rows);
2411  dbDriver_->updateDepthImage(id, depth);
2412  this->update3dView();
2413  }
2414  }
2415  }
2416 }
2417 
2418 void DatabaseViewer::exportPosesRaw()
2419 {
2420  exportPoses(0);
2421 }
2422 void DatabaseViewer::exportPosesRGBDSLAMMotionCapture()
2423 {
2424  exportPoses(1);
2425 }
2426 void DatabaseViewer::exportPosesRGBDSLAM()
2427 {
2428  exportPoses(10);
2429 }
2430 void DatabaseViewer::exportPosesRGBDSLAMID()
2431 {
2432  exportPoses(11);
2433 }
2434 void DatabaseViewer::exportPosesKITTI()
2435 {
2436  exportPoses(2);
2437 }
2438 void DatabaseViewer::exportPosesTORO()
2439 {
2440  exportPoses(3);
2441 }
2442 void DatabaseViewer::exportPosesG2O()
2443 {
2444  exportPoses(4);
2445 }
2446 void DatabaseViewer::exportPosesKML()
2447 {
2448  exportPoses(5);
2449 }
2450 
2451 void DatabaseViewer::exportPoses(int format)
2452 {
2453  QStringList types;
2454  types.push_back("Map's graph (see Graph View)");
2455  types.push_back("Odometry");
2456  if(!groundTruthPoses_.empty())
2457  {
2458  types.push_back("Ground Truth");
2459  }
2460  bool ok;
2461  QString type = QInputDialog::getItem(this, tr("Which poses?"), tr("Poses:"), types, 0, false, &ok);
2462  if(!ok)
2463  {
2464  return;
2465  }
2466  bool odometry = type.compare("Odometry") == 0;
2467  bool groundTruth = type.compare("Ground Truth") == 0;
2468 
2469  if(groundTruth && groundTruthPoses_.empty())
2470  {
2471  QMessageBox::warning(this, tr("Cannot export poses"), tr("No ground truth poses in database?!"));
2472  return;
2473  }
2474  else if(!odometry && graphes_.empty())
2475  {
2476  this->updateGraphView();
2477  if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
2478  {
2479  QMessageBox::warning(this, tr("Cannot export poses"), tr("No graph in database?!"));
2480  return;
2481  }
2482  }
2483  else if(odometry && odomPoses_.empty())
2484  {
2485  QMessageBox::warning(this, tr("Cannot export poses"), tr("No odometry poses in database?!"));
2486  return;
2487  }
2488 
2489  if(format == 5)
2490  {
2491  if(gpsValues_.empty() || gpsPoses_.empty())
2492  {
2493  QMessageBox::warning(this, tr("Cannot export poses"), tr("No GPS in database?!"));
2494  }
2495  else
2496  {
2497  std::map<int, rtabmap::Transform> graph;
2498  if(groundTruth)
2499  {
2500  graph = groundTruthPoses_;
2501  }
2502  else if(odometry)
2503  {
2504  graph = odomPoses_;
2505  }
2506  else
2507  {
2508  graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
2509  }
2510 
2511 
2512  //align with ground truth for more meaningful results
2513  pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
2514  cloud1.resize(graph.size());
2515  cloud2.resize(graph.size());
2516  int oi = 0;
2517  int idFirst = 0;
2518  for(std::map<int, Transform>::const_iterator iter=gpsPoses_.begin(); iter!=gpsPoses_.end(); ++iter)
2519  {
2520  std::map<int, Transform>::iterator iter2 = graph.find(iter->first);
2521  if(iter2!=graph.end())
2522  {
2523  if(oi==0)
2524  {
2525  idFirst = iter->first;
2526  }
2527  cloud1[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
2528  cloud2[oi++] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z());
2529  }
2530  }
2531 
2532  Transform t = Transform::getIdentity();
2533  if(oi>5)
2534  {
2535  cloud1.resize(oi);
2536  cloud2.resize(oi);
2537 
2538  t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
2539  }
2540  else if(idFirst)
2541  {
2542  t = gpsPoses_.at(idFirst) * graph.at(idFirst).inverse();
2543  }
2544 
2545  std::map<int, GPS> values;
2546  GeodeticCoords origin = gpsValues_.begin()->second.toGeodeticCoords();
2547  for(std::map<int, Transform>::iterator iter=graph.begin(); iter!=graph.end(); ++iter)
2548  {
2549  iter->second = t * iter->second;
2550 
2551  GeodeticCoords coord;
2552  coord.fromENU_WGS84(cv::Point3d(iter->second.x(), iter->second.y(), iter->second.z()), origin);
2553  double bearing = -(iter->second.theta()*180.0/M_PI-90.0);
2554  if(bearing < 0)
2555  {
2556  bearing += 360;
2557  }
2558 
2559  Transform p, g;
2560  int w;
2561  std::string l;
2562  double stamp=0.0;
2563  int mapId;
2564  std::vector<float> v;
2565  GPS gps;
2566  EnvSensors sensors;
2567  dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors);
2568  values.insert(std::make_pair(iter->first, GPS(stamp, coord.longitude(), coord.latitude(), coord.altitude(), 0, 0)));
2569  }
2570 
2571  QString output = pathDatabase_ + QDir::separator() + "poses.kml";
2572  QString path = QFileDialog::getSaveFileName(
2573  this,
2574  tr("Save File"),
2575  output,
2576  tr("Google Earth file (*.kml)"));
2577 
2578  if(!path.isEmpty())
2579  {
2580  bool saved = graph::exportGPS(path.toStdString(), values, ui_->graphViewer->getNodeColor().rgba());
2581 
2582  if(saved)
2583  {
2584  QMessageBox::information(this,
2585  tr("Export poses..."),
2586  tr("GPS coordinates saved to \"%1\".")
2587  .arg(path));
2588  }
2589  else
2590  {
2591  QMessageBox::information(this,
2592  tr("Export poses..."),
2593  tr("Failed to save GPS coordinates to \"%1\"!")
2594  .arg(path));
2595  }
2596  }
2597  }
2598  return;
2599  }
2600 
2601  std::map<int, Transform> optimizedPoses;
2602  if(groundTruth)
2603  {
2604  optimizedPoses = groundTruthPoses_;
2605  }
2606  else
2607  {
2608  if(odometry)
2609  {
2610  optimizedPoses = odomPoses_;
2611  }
2612  else
2613  {
2614  optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
2615  }
2616 
2617  if((ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked()) ||
2618  (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked()))
2619  {
2620  std::map<int, Transform> refPoses = groundTruthPoses_;
2621  if(ui_->checkBox_alignPosesWithGPS->isEnabled() &&
2622  ui_->checkBox_alignPosesWithGPS->isChecked())
2623  {
2624  refPoses = gpsPoses_;
2625  }
2626 
2627  // Log ground truth statistics (in TUM's RGBD-SLAM format)
2628  if(refPoses.size())
2629  {
2630  float translational_rmse = 0.0f;
2631  float translational_mean = 0.0f;
2632  float translational_median = 0.0f;
2633  float translational_std = 0.0f;
2634  float translational_min = 0.0f;
2635  float translational_max = 0.0f;
2636  float rotational_rmse = 0.0f;
2637  float rotational_mean = 0.0f;
2638  float rotational_median = 0.0f;
2639  float rotational_std = 0.0f;
2640  float rotational_min = 0.0f;
2641  float rotational_max = 0.0f;
2642 
2643  Transform gtToMap = graph::calcRMSE(
2644  refPoses,
2645  optimizedPoses,
2646  translational_rmse,
2647  translational_mean,
2648  translational_median,
2649  translational_std,
2650  translational_min,
2651  translational_max,
2652  rotational_rmse,
2653  rotational_mean,
2654  rotational_median,
2655  rotational_std,
2656  rotational_min,
2657  rotational_max);
2658 
2659  if(!gtToMap.isIdentity())
2660  {
2661  for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
2662  {
2663  iter->second = gtToMap * iter->second;
2664  }
2665  }
2666  }
2667  }
2668  }
2669 
2670  if(optimizedPoses.size())
2671  {
2672  std::map<int, Transform> localTransforms;
2673  QStringList items;
2674  items.push_back("Robot (base frame)");
2675  items.push_back("Camera");
2676  items.push_back("Scan");
2677  bool ok;
2678  QString item = QInputDialog::getItem(this, tr("Export Poses"), tr("Frame: "), items, 0, false, &ok);
2679  if(!ok || item.isEmpty())
2680  {
2681  return;
2682  }
2683  if(item.compare("Robot (base frame)") != 0)
2684  {
2685  bool cameraFrame = item.compare("Camera") == 0;
2686  for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
2687  {
2688  Transform localTransform;
2689  if(cameraFrame)
2690  {
2691  std::vector<CameraModel> models;
2692  std::vector<StereoCameraModel> stereoModels;
2693  if(dbDriver_->getCalibration(iter->first, models, stereoModels))
2694  {
2695  if((models.size() == 1 &&
2696  !models.at(0).localTransform().isNull()))
2697  {
2698  localTransform = models.at(0).localTransform();
2699  }
2700  else if(stereoModels.size() == 1 &&
2701  !stereoModels[0].localTransform().isNull())
2702  {
2703  localTransform = stereoModels[0].localTransform();
2704  }
2705  else if(models.size()>1)
2706  {
2707  UWARN("Multi-camera is not supported (node %d)", iter->first);
2708  }
2709  else
2710  {
2711  UWARN("Calibration not valid for node %d", iter->first);
2712  }
2713  }
2714  else
2715  {
2716  UWARN("Missing calibration for node %d", iter->first);
2717  }
2718  }
2719  else
2720  {
2721  LaserScan info;
2722  if(dbDriver_->getLaserScanInfo(iter->first, info))
2723  {
2724  localTransform = info.localTransform();
2725  }
2726  else
2727  {
2728  UWARN("Missing scan info for node %d", iter->first);
2729  }
2730 
2731  }
2732  if(!localTransform.isNull())
2733  {
2734  localTransforms.insert(std::make_pair(iter->first, localTransform));
2735  }
2736  }
2737  if(localTransforms.empty())
2738  {
2739  QMessageBox::warning(this,
2740  tr("Export Poses"),
2741  tr("Could not find any \"%1\" frame, exporting in Robot frame instead.").arg(item));
2742  }
2743  }
2744 
2745  std::map<int, Transform> poses;
2746  std::multimap<int, Link> links;
2747  if(localTransforms.empty())
2748  {
2749  poses = optimizedPoses;
2750  if(!odometry)
2751  {
2752  links = graphLinks_;
2753  }
2754  }
2755  else
2756  {
2757  //adjust poses and links
2758  for(std::map<int, Transform>::iterator iter=localTransforms.begin(); iter!=localTransforms.end(); ++iter)
2759  {
2760  poses.insert(std::make_pair(iter->first, optimizedPoses.at(iter->first) * iter->second));
2761  }
2762  if(!odometry)
2763  {
2764  for(std::multimap<int, Link>::iterator iter=graphLinks_.begin(); iter!=graphLinks_.end(); ++iter)
2765  {
2766  if(uContains(poses, iter->second.from()) && uContains(poses, iter->second.to()))
2767  {
2768  std::multimap<int, Link>::iterator inserted = links.insert(*iter);
2769  int from = iter->second.from();
2770  int to = iter->second.to();
2771  inserted->second.setTransform(localTransforms.at(from).inverse()*iter->second.transform()*localTransforms.at(to));
2772  }
2773  }
2774  }
2775  }
2776 
2777  if(format != 4 && format != 11 && !poses.empty() && poses.begin()->first<0) // not g2o, landmark not supported
2778  {
2779  UWARN("Only g2o format (4) and RGBD format with ID format can export landmarks, they are ignored with format %d", format);
2780  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end() && iter->first < 0;)
2781  {
2782  poses.erase(iter++);
2783  }
2784  for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end();)
2785  {
2786  if(iter->second.from() < 0 || iter->second.to() < 0)
2787  {
2788  links.erase(iter++);
2789  }
2790  else
2791  {
2792  ++iter;
2793  }
2794  }
2795  }
2796 
2797  std::map<int, double> stamps;
2798  if(format == 1 || format == 10 || format == 11)
2799  {
2800  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
2801  {
2802  if(iter->first<0 && format == 11) // in case of landmarks
2803  {
2804  stamps.insert(std::make_pair(iter->first, 0));
2805  }
2806  else
2807  {
2808  Transform p, g;
2809  int w;
2810  std::string l;
2811  double stamp=0.0;
2812  int mapId;
2813  std::vector<float> v;
2814  GPS gps;
2815  EnvSensors sensors;
2816  if(dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors))
2817  {
2818  stamps.insert(std::make_pair(iter->first, stamp));
2819  }
2820  }
2821  }
2822  if(stamps.size()!=poses.size())
2823  {
2824  QMessageBox::warning(this, tr("Export poses..."), tr("Poses (%1) and stamps (%2) have not the same size! Cannot export in RGB-D SLAM format.")
2825  .arg(poses.size()).arg(stamps.size()));
2826  return;
2827  }
2828  }
2829 
2830  QString output = pathDatabase_ + QDir::separator() + (format==3?"toro%1.graph":format==4?"poses%1.g2o":"poses%1.txt");
2831  QString suffix = odometry?"_odom":groundTruth?"_gt":"";
2832  output = output.arg(suffix);
2833 
2834  QString path = QFileDialog::getSaveFileName(
2835  this,
2836  tr("Save File"),
2837  output,
2838  format == 3?tr("TORO file (*.graph)"):format==4?tr("g2o file (*.g2o)"):tr("Text file (*.txt)"));
2839 
2840  if(!path.isEmpty())
2841  {
2842  if(QFileInfo(path).suffix() == "")
2843  {
2844  if(format == 3)
2845  {
2846  path += ".graph";
2847  }
2848  else if(format==4)
2849  {
2850  path += ".g2o";
2851  }
2852  else
2853  {
2854  path += ".txt";
2855  }
2856  }
2857 
2858  bool saved = graph::exportPoses(path.toStdString(), format, poses, links, stamps, ui_->parameters_toolbox->getParameters());
2859 
2860  if(saved)
2861  {
2862  QMessageBox::information(this,
2863  tr("Export poses..."),
2864  tr("%1 saved to \"%2\".")
2865  .arg(format == 3?"TORO graph":format == 4?"g2o graph":"Poses")
2866  .arg(path));
2867  }
2868  else
2869  {
2870  QMessageBox::information(this,
2871  tr("Export poses..."),
2872  tr("Failed to save %1 to \"%2\"!")
2873  .arg(format == 3?"TORO graph":format == 4?"g2o graph":"poses")
2874  .arg(path));
2875  }
2876  }
2877  }
2878 }
2879 
2880 void DatabaseViewer::exportGPS_TXT()
2881 {
2882  exportGPS(0);
2883 }
2884 void DatabaseViewer::exportGPS_KML()
2885 {
2886  exportGPS(1);
2887 }
2888 
2889 void DatabaseViewer::exportGPS(int format)
2890 {
2891  if(!gpsValues_.empty())
2892  {
2893  QString output = pathDatabase_ + QDir::separator() + (format==0?"gps.txt":"gps.kml");
2894  QString path = QFileDialog::getSaveFileName(
2895  this,
2896  tr("Save File"),
2897  output,
2898  format==0?tr("Raw format (*.txt)"):tr("Google Earth file (*.kml)"));
2899 
2900  if(!path.isEmpty())
2901  {
2902  bool saved = graph::exportGPS(path.toStdString(), gpsValues_, ui_->graphViewer->getGPSColor().rgba());
2903 
2904  if(saved)
2905  {
2906  QMessageBox::information(this,
2907  tr("Export poses..."),
2908  tr("GPS coordinates saved to \"%1\".")
2909  .arg(path));
2910  }
2911  else
2912  {
2913  QMessageBox::information(this,
2914  tr("Export poses..."),
2915  tr("Failed to save GPS coordinates to \"%1\"!")
2916  .arg(path));
2917  }
2918  }
2919  }
2920 }
2921 
2922 void DatabaseViewer::editSaved2DMap()
2923 {
2924  if(!dbDriver_)
2925  {
2926  QMessageBox::warning(this, tr("Cannot edit 2D map"), tr("A database must must loaded first...\nUse File->Open database."));
2927  return;
2928  }
2929 
2930  if(uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.17.0") < 0)
2931  {
2932  QMessageBox::warning(this, tr("Cannot edit 2D map"),
2933  tr("The database has too old version (%1) to saved "
2934  "optimized map. Version 0.17.0 minimum required.").arg(dbDriver_->getDatabaseVersion().c_str()));
2935  return;
2936  }
2937 
2938  if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size() || generatedLocalMaps_.size())
2939  {
2940  QMessageBox::warning(this, tr("Cannot edit 2D map"),
2941  tr("The database has modified links and/or modified local "
2942  "occupancy grids, the 2D optimized map cannot be modified."));
2943  return;
2944  }
2945 
2946  float xMin, yMin, cellSize;
2947  cv::Mat map = dbDriver_->load2DMap(xMin, yMin, cellSize);
2948  if(map.empty())
2949  {
2950  QMessageBox::warning(this, tr("Cannot export 2D map"), tr("The database doesn't contain a saved 2D map."));
2951  return;
2952  }
2953 
2954  cv::Mat map8U = rtabmap::util3d::convertMap2Image8U(map, false);
2955  cv::Mat map8UFlip, map8URotated;
2956  cv::flip(map8U, map8UFlip, 0);
2957  if(!ui_->graphViewer->isOrientationENU())
2958  {
2959  //ROTATE_90_COUNTERCLOCKWISE
2960  cv::transpose(map8UFlip, map8URotated);
2961  cv::flip(map8URotated, map8URotated, 0);
2962  }
2963  else
2964  {
2965  map8URotated = map8UFlip;
2966  }
2967 
2968  editMapArea_->setMap(map8URotated);
2969 
2970  if(editMapDialog_->exec() == QDialog::Accepted)
2971  {
2972  cv::Mat mapModified = editMapArea_->getModifiedMap();
2973 
2974  if(!ui_->graphViewer->isOrientationENU())
2975  {
2976  //ROTATE_90_CLOCKWISE
2977  cv::transpose(mapModified, map8URotated);
2978  cv::flip(map8URotated, map8URotated, 1);
2979  }
2980  else
2981  {
2982  map8URotated = mapModified;
2983  }
2984  cv::flip(map8URotated, map8UFlip, 0);
2985 
2986  UASSERT(map8UFlip.type() == map8U.type());
2987  UASSERT(map8UFlip.cols == map8U.cols);
2988  UASSERT(map8UFlip.rows == map8U.rows);
2989 
2990  cv::Mat map8S = rtabmap::util3d::convertImage8U2Map(map8UFlip, false);
2991 
2992  if(editMapArea_->isModified())
2993  {
2994  dbDriver_->save2DMap(map8S, xMin, yMin, cellSize);
2995  QMessageBox::information(this, tr("Edit 2D map"), tr("Map updated!"));
2996  }
2997 
2998  int cropRadius = ui_->spinBox_cropRadius->value();
2999  QMessageBox::StandardButton b = QMessageBox::question(this,
3000  tr("Crop empty space"),
3001  tr("Do you want to clear empty space from local occupancy grids and laser scans?\n\n"
3002  "Advantages:\n"
3003  " * 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"
3004  " * 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"
3005  "Disadvantage:\n"
3006  " * Cropping the laser scans cannot be reverted after the viewer is closed and changes have been saved.\n\n"
3007  "Parameter(s):\n"
3008  " Crop radius = %1 pixels\n\n"
3009  "Press \"Yes\" to filter only grids.\n"
3010  "Press \"Yes to All\" to filter both grids and laser scans.\n").arg(cropRadius),
3011  QMessageBox::Yes | QMessageBox::YesToAll | QMessageBox::No, QMessageBox::No);
3012  if(b == QMessageBox::Yes || b == QMessageBox::YesToAll)
3013  {
3014  std::map<int, Transform> poses = dbDriver_->loadOptimizedPoses(); // poses should match the grid map
3015 
3016  modifiedLaserScans_.clear();
3017 
3018  rtabmap::ProgressDialog progressDialog(this);
3019  progressDialog.setMaximumSteps(poses.size()+1);
3020  progressDialog.show();
3021  progressDialog.setCancelButtonVisible(true);
3022  progressDialog.appendText(QString("Cropping empty space... %1 scans to filter").arg(poses.size()));
3023  progressDialog.setMinimumWidth(800);
3024  QApplication::processEvents();
3025 
3026  UINFO("Cropping empty space... poses=%d cropRadius=%d", poses.size(), cropRadius);
3027  UASSERT(cropRadius>=0);
3028  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end() && !progressDialog.isCanceled(); ++iter)
3029  {
3030  // local grid
3031  cv::Mat gridGround;
3032  cv::Mat gridObstacles;
3033  cv::Mat gridEmpty;
3034 
3035  // scan
3036  SensorData data;
3037  dbDriver_->getNodeData(iter->first, data);
3038  LaserScan scan;
3039  data.uncompressData(0,0,&scan,0,&gridGround,&gridObstacles,&gridEmpty);
3040 
3041  if(generatedLocalMaps_.localGrids().find(iter->first) != generatedLocalMaps_.localGrids().end())
3042  {
3043  gridObstacles = generatedLocalMaps_.localGrids().at(iter->first).obstacleCells;
3044  }
3045  if(!gridObstacles.empty())
3046  {
3047  cv::Mat filtered = cv::Mat(1, gridObstacles.cols, gridObstacles.type());
3048  int oi = 0;
3049  for(int i=0; i<gridObstacles.cols; ++i)
3050  {
3051  const float * ptr = gridObstacles.ptr<float>(0, i);
3052  cv::Point3f pt(ptr[0], ptr[1], gridObstacles.channels()==2?0:ptr[2]);
3053  pt = util3d::transformPoint(pt, iter->second);
3054 
3055  int x = int((pt.x - xMin) / cellSize + 0.5f);
3056  int y = int((pt.y - yMin) / cellSize + 0.5f);
3057 
3058  if(x>=0 && x<map8S.cols &&
3059  y>=0 && y<map8S.rows)
3060  {
3061  bool obstacleDetected = false;
3062 
3063  for(int j=-cropRadius; j<=cropRadius && !obstacleDetected; ++j)
3064  {
3065  for(int k=-cropRadius; k<=cropRadius && !obstacleDetected; ++k)
3066  {
3067  if(x+j>=0 && x+j<map8S.cols &&
3068  y+k>=0 && y+k<map8S.rows &&
3069  map8S.at<unsigned char>(y+k,x+j) == 100)
3070  {
3071  obstacleDetected = true;
3072  }
3073  }
3074  }
3075 
3076  if(map8S.at<unsigned char>(y,x) != 0 || obstacleDetected)
3077  {
3078  // Verify that we don't have an obstacle on neighbor cells
3079  cv::Mat(gridObstacles, cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(filtered, cv::Range::all(), cv::Range(oi,oi+1)));
3080  ++oi;
3081  }
3082  }
3083  }
3084 
3085  if(oi != gridObstacles.cols)
3086  {
3087  progressDialog.appendText(QString("Grid %1 filtered %2 pts -> %3 pts").arg(iter->first).arg(gridObstacles.cols).arg(oi));
3088  UINFO("Grid %d filtered %d -> %d", iter->first, gridObstacles.cols, oi);
3089 
3090  // update
3091  cv::Mat newObstacles = cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi));
3092  if(generatedLocalMaps_.localGrids().find(iter->first) != generatedLocalMaps_.localGrids().end())
3093  {
3094  LocalGrid value = generatedLocalMaps_.localGrids().at(iter->first);
3095  value.obstacleCells = newObstacles;
3096  generatedLocalMaps_.add(iter->first, value);
3097  }
3098  else
3099  {
3100  LocalGrid value(gridGround, newObstacles, gridEmpty, data.gridCellSize(), data.gridViewPoint());
3101  generatedLocalMaps_.add(iter->first, value);
3102  }
3103 
3104  }
3105  }
3106 
3107  if(b == QMessageBox::YesToAll && !scan.isEmpty())
3108  {
3109  Transform mapToScan = iter->second * scan.localTransform();
3110 
3111  cv::Mat filtered = cv::Mat(1, scan.size(), scan.dataType());
3112  int oi = 0;
3113  for(int i=0; i<scan.size(); ++i)
3114  {
3115  const float * ptr = scan.data().ptr<float>(0, i);
3116  cv::Point3f pt(ptr[0], ptr[1], scan.is2d()?0:ptr[2]);
3117  pt = util3d::transformPoint(pt, mapToScan);
3118 
3119  int x = int((pt.x - xMin) / cellSize + 0.5f);
3120  int y = int((pt.y - yMin) / cellSize + 0.5f);
3121 
3122  if(x>=0 && x<map8S.cols &&
3123  y>=0 && y<map8S.rows)
3124  {
3125  bool obstacleDetected = false;
3126 
3127  for(int j=-cropRadius; j<=cropRadius && !obstacleDetected; ++j)
3128  {
3129  for(int k=-cropRadius; k<=cropRadius && !obstacleDetected; ++k)
3130  {
3131  if(x+j>=0 && x+j<map8S.cols &&
3132  y+k>=0 && y+k<map8S.rows &&
3133  map8S.at<unsigned char>(y+k,x+j) == 100)
3134  {
3135  obstacleDetected = true;
3136  }
3137  }
3138  }
3139 
3140  if(map8S.at<unsigned char>(y,x) != 0 || obstacleDetected)
3141  {
3142  // Verify that we don't have an obstacle on neighbor cells
3143  cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(filtered, cv::Range::all(), cv::Range(oi,oi+1)));
3144  ++oi;
3145  }
3146  }
3147  }
3148 
3149  if(oi != scan.size())
3150  {
3151  progressDialog.appendText(QString("Scan %1 filtered %2 pts -> %3 pts").arg(iter->first).arg(scan.size()).arg(oi));
3152  UINFO("Scan %d filtered %d -> %d", iter->first, scan.size(), oi);
3153 
3154  // update
3155  if(scan.angleIncrement()!=0)
3156  {
3157  // copy meta data
3158  scan = LaserScan(
3159  cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi)),
3160  scan.format(),
3161  scan.rangeMin(),
3162  scan.rangeMax(),
3163  scan.angleMin(),
3164  scan.angleMax(),
3165  scan.angleIncrement(),
3166  scan.localTransform());
3167  }
3168  else
3169  {
3170  // copy meta data
3171  scan = LaserScan(
3172  cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi)),
3173  scan.maxPoints(),
3174  scan.rangeMax(),
3175  scan.format(),
3176  scan.localTransform());
3177  }
3178  uInsert(modifiedLaserScans_, std::make_pair(iter->first, scan));
3179  }
3180  }
3181  progressDialog.incrementStep();
3182  QApplication::processEvents();
3183  }
3184  if(progressDialog.isCanceled())
3185  {
3186  modifiedLaserScans_.clear();
3187  }
3188  else
3189  {
3190  update3dView();
3191  updateGraphView();
3192  }
3193  progressDialog.setValue(progressDialog.maximumSteps());
3194  }
3195  }
3196 }
3197 
3198 void DatabaseViewer::exportSaved2DMap()
3199 {
3200  if(!dbDriver_)
3201  {
3202  QMessageBox::warning(this, tr("Cannot export 2D map"), tr("A database must must loaded first...\nUse File->Open database."));
3203  return;
3204  }
3205 
3206  float xMin, yMin, cellSize;
3207  cv::Mat map = dbDriver_->load2DMap(xMin, yMin, cellSize);
3208  if(map.empty())
3209  {
3210  QMessageBox::warning(this, tr("Cannot export 2D map"), tr("The database doesn't contain a saved 2D map."));
3211  }
3212  else
3213  {
3214  cv::Mat map8U = rtabmap::util3d::convertMap2Image8U(map, true);
3215  QString name = QFileInfo(databaseFileName_.c_str()).baseName();
3216  QString path = QFileDialog::getSaveFileName(
3217  this,
3218  tr("Save File"),
3219  pathDatabase_+"/" + name + ".pgm",
3220  tr("Map (*.pgm)"));
3221 
3222  if(!path.isEmpty())
3223  {
3224  if(QFileInfo(path).suffix() == "")
3225  {
3226  path += ".pgm";
3227  }
3228  cv::imwrite(path.toStdString(), map8U);
3229 
3230  QFileInfo info(path);
3231  QString yaml = info.absolutePath() + "/" + info.baseName() + ".yaml";
3232 
3233  float occupancyThr = Parameters::defaultGridGlobalOccupancyThr();
3234  Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kGridGlobalOccupancyThr(), occupancyThr);
3235 
3236  std::ofstream file;
3237  file.open (yaml.toStdString());
3238  file << "image: " << info.baseName().toStdString() << ".pgm" << std::endl;
3239  file << "resolution: " << cellSize << std::endl;
3240  file << "origin: [" << xMin << ", " << yMin << ", 0.0]" << std::endl;
3241  file << "negate: 0" << std::endl;
3242  file << "occupied_thresh: " << occupancyThr << std::endl;
3243  file << "free_thresh: 0.196" << std::endl;
3244  file << std::endl;
3245  file.close();
3246 
3247 
3248  QMessageBox::information(this, tr("Export 2D map"), tr("Exported %1 and %2!").arg(path).arg(yaml));
3249  }
3250  }
3251 }
3252 
3253 void DatabaseViewer::import2DMap()
3254 {
3255  if(!dbDriver_)
3256  {
3257  QMessageBox::warning(this, tr("Cannot import 2D map"), tr("A database must must loaded first...\nUse File->Open database."));
3258  return;
3259  }
3260 
3261  if(uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.17.0") < 0)
3262  {
3263  QMessageBox::warning(this, tr("Cannot edit 2D map"),
3264  tr("The database has too old version (%1) to saved "
3265  "optimized map. Version 0.17.0 minimum required.").arg(dbDriver_->getDatabaseVersion().c_str()));
3266  return;
3267  }
3268 
3269  if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size() || generatedLocalMaps_.size())
3270  {
3271  QMessageBox::warning(this, tr("Cannot import 2D map"),
3272  tr("The database has modified links and/or modified local "
3273  "occupancy grids, the 2D optimized map cannot be modified."));
3274  return;
3275  }
3276 
3277  float xMin, yMin, cellSize;
3278  cv::Mat mapOrg = dbDriver_->load2DMap(xMin, yMin, cellSize);
3279  if(mapOrg.empty())
3280  {
3281  QMessageBox::warning(this, tr("Cannot import 2D map"), tr("The database doesn't contain a saved 2D map."));
3282  }
3283  else
3284  {
3285  QString path = QFileDialog::getOpenFileName(
3286  this,
3287  tr("Open File"),
3288  pathDatabase_,
3289  tr("Map (*.pgm)"));
3290  if(!path.isEmpty())
3291  {
3292  cv::Mat map8U = cv::imread(path.toStdString(), cv::IMREAD_UNCHANGED);
3293  cv::Mat map = rtabmap::util3d::convertImage8U2Map(map8U, true);
3294 
3295  if(mapOrg.cols == map.cols && mapOrg.rows == map8U.rows)
3296  {
3297  dbDriver_->save2DMap(map, xMin, yMin, cellSize);
3298  QMessageBox::information(this, tr("Import 2D map"), tr("Imported %1!").arg(path));
3299  }
3300  else
3301  {
3302  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));
3303  }
3304  }
3305  }
3306 }
3307 
3308 void DatabaseViewer::regenerateSavedMap()
3309 {
3310  if(!dbDriver_)
3311  {
3312  QMessageBox::warning(this, tr("Cannot import 2D map"), tr("A database must must loaded first...\nUse File->Open database."));
3313  return;
3314  }
3315 
3316  if(uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.17.0") < 0)
3317  {
3318  QMessageBox::warning(this, tr("Cannot edit 2D map"),
3319  tr("The database has too old version (%1) to saved "
3320  "optimized map. Version 0.17.0 minimum required.").arg(dbDriver_->getDatabaseVersion().c_str()));
3321  return;
3322  }
3323 
3324  if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size() || generatedLocalMaps_.size())
3325  {
3326  QMessageBox::warning(this, tr("Cannot import 2D map"),
3327  tr("The database has modified links and/or modified local "
3328  "occupancy grids, the 2D optimized map cannot be modified. Try "
3329  "closing the database and re-open it to save the changes."));
3330  return;
3331  }
3332 
3333  if((int)graphes_.empty() || localMaps_.empty())
3334  {
3335  QMessageBox::warning(this, tr("Cannot regenerate 2D map"),
3336  tr("Graph is empty, make sure you opened the "
3337  "Graph View and there is a map shown."));
3338  return;
3339  }
3340 
3341  //
3342 #ifdef RTABMAP_OCTOMAP
3343  QStringList types;
3344  types.push_back("Default occupancy grid");
3345  types.push_back("From OctoMap projection");
3346  bool ok;
3347  QString type = QInputDialog::getItem(this, tr("Which type?"), tr("Type:"), types, 0, false, &ok);
3348  if(!ok)
3349  {
3350  return;
3351  }
3352 #endif
3353 
3354  //update scans
3355  UINFO("Update local maps list...");
3356  float xMin, yMin;
3357  cv::Mat map;
3358  float gridCellSize = Parameters::defaultGridCellSize();
3359  Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kGridCellSize(), gridCellSize);
3360 #ifdef RTABMAP_OCTOMAP
3361  if(type.compare("From OctoMap projection") == 0)
3362  {
3363  //create local octomap
3364  OctoMap octomap(&localMaps_, ui_->parameters_toolbox->getParameters());
3365 
3366  octomap.update(graphes_.back());
3367  map = octomap.createProjectionMap(xMin, yMin, gridCellSize, 0);
3368  }
3369  else
3370 #endif
3371  {
3372  OccupancyGrid grid(&localMaps_, ui_->parameters_toolbox->getParameters());
3373  grid.update(graphes_.back());
3374  map = grid.getMap(xMin, yMin);
3375  }
3376 
3377  if(map.empty())
3378  {
3379  QMessageBox::information(this, tr("Regenerate 2D map"), tr("Failed to renegerate the map, resulting map is empty!"));
3380  }
3381  else
3382  {
3383  dbDriver_->save2DMap(map, xMin, yMin, gridCellSize);
3384  Transform lastlocalizationPose;
3385  dbDriver_->loadOptimizedPoses(&lastlocalizationPose);
3386  if(lastlocalizationPose.isNull() && !graphes_.back().empty())
3387  {
3388  // use last pose by default
3389  lastlocalizationPose = graphes_.back().rbegin()->second;
3390  }
3391  dbDriver_->saveOptimizedPoses(graphes_.back(), lastlocalizationPose);
3392  // reset optimized mesh as poses have changed
3393  dbDriver_->saveOptimizedMesh(cv::Mat());
3394  QMessageBox::information(this, tr("Regenerate 2D map"), tr("Map regenerated!"));
3395  ui_->actionEdit_optimized_2D_map->setEnabled(true);
3396  ui_->actionExport_saved_2D_map->setEnabled(true);
3397  ui_->actionImport_2D_map->setEnabled(true);
3398  }
3399 }
3400 
3401 void DatabaseViewer::viewOptimizedMesh()
3402 {
3403  if(!dbDriver_)
3404  {
3405  QMessageBox::warning(this, tr("Cannot view optimized mesh"), tr("A database must must loaded first...\nUse File->Open database."));
3406  return;
3407  }
3408 
3409  std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
3410 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
3411  std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
3412 #else
3413  std::vector<std::vector<Eigen::Vector2f> > texCoords;
3414 #endif
3415  cv::Mat textures;
3416  cv::Mat cloudMat = dbDriver_->loadOptimizedMesh(&polygons, &texCoords, &textures);
3417  if(cloudMat.empty())
3418  {
3419  QMessageBox::warning(this, tr("Cannot view optimized mesh"), tr("The database doesn't contain a saved optimized mesh."));
3420  }
3421  else
3422  {
3423  CloudViewer * viewer = new CloudViewer(this);
3424  viewer->setWindowFlags(Qt::Window);
3425  viewer->setAttribute(Qt::WA_DeleteOnClose);
3426  viewer->buildPickingLocator(true);
3427  if(!textures.empty())
3428  {
3429  pcl::TextureMeshPtr mesh = util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
3430  util3d::fixTextureMeshForVisualization(*mesh);
3431  viewer->setWindowTitle("Optimized Textured Mesh");
3432  viewer->setPolygonPicking(true);
3433  viewer->addCloudTextureMesh("mesh", mesh, textures);
3434  }
3435  else if(polygons.size() == 1)
3436  {
3437  pcl::PolygonMeshPtr mesh = util3d::assemblePolygonMesh(cloudMat, polygons.at(0));
3438  viewer->setWindowTitle("Optimized Mesh");
3439  viewer->setPolygonPicking(true);
3440  viewer->addCloudMesh("mesh", mesh);
3441  }
3442  else
3443  {
3444  LaserScan scan = LaserScan::backwardCompatibility(cloudMat);
3445  pcl::PCLPointCloud2::Ptr cloud = util3d::laserScanToPointCloud2(scan);
3446  viewer->setWindowTitle("Optimized Point Cloud");
3447  viewer->addCloud("mesh", cloud, Transform::getIdentity(), scan.hasRGB(), scan.hasNormals(), scan.hasIntensity());
3448  }
3449  viewer->show();
3450  }
3451 }
3452 
3453 void DatabaseViewer::exportOptimizedMesh()
3454 {
3455  if(!dbDriver_)
3456  {
3457  QMessageBox::warning(this, tr("Cannot export optimized mesh"), tr("A database must must loaded first...\nUse File->Open database."));
3458  return;
3459  }
3460 
3461  std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
3462 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
3463  std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
3464 #else
3465  std::vector<std::vector<Eigen::Vector2f> > texCoords;
3466 #endif
3467  cv::Mat textures;
3468  cv::Mat cloudMat = dbDriver_->loadOptimizedMesh(&polygons, &texCoords, &textures);
3469  if(cloudMat.empty())
3470  {
3471  QMessageBox::warning(this, tr("Cannot export optimized mesh"), tr("The database doesn't contain a saved optimized mesh."));
3472  }
3473  else
3474  {
3475  QString name = QFileInfo(databaseFileName_.c_str()).baseName();
3476 
3477  if(!textures.empty())
3478  {
3479  pcl::TextureMeshPtr mesh = util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures);
3480  QString path = QFileDialog::getSaveFileName(
3481  this,
3482  tr("Save File"),
3483  pathDatabase_+"/" + name + ".obj",
3484  tr("Mesh (*.obj)"));
3485 
3486  if(!path.isEmpty())
3487  {
3488  if(QFileInfo(path).suffix() == "")
3489  {
3490  path += ".obj";
3491  }
3492  QString baseName = QFileInfo(path).baseName();
3493  if(mesh->tex_materials.size() == 1)
3494  {
3495  mesh->tex_materials.at(0).tex_file = baseName.toStdString() + ".png";
3496  cv::imwrite((QFileInfo(path).absoluteDir().absolutePath()+QDir::separator()+baseName).toStdString() + ".png", textures);
3497  }
3498  else
3499  {
3500  for(unsigned int i=0; i<mesh->tex_materials.size(); ++i)
3501  {
3502  mesh->tex_materials.at(i).tex_file = (baseName+QDir::separator()+QString::number(i)+".png").toStdString();
3503  UASSERT((i+1)*textures.rows <= (unsigned int)textures.cols);
3504  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)));
3505  }
3506  }
3507  pcl::io::saveOBJFile(path.toStdString(), *mesh);
3508 
3509  QMessageBox::information(this, tr("Export Textured Mesh"), tr("Exported %1!").arg(path));
3510  }
3511  }
3512  else if(polygons.size() == 1)
3513  {
3514  pcl::PolygonMeshPtr mesh = util3d::assemblePolygonMesh(cloudMat, polygons.at(0));
3515  QString path = QFileDialog::getSaveFileName(
3516  this,
3517  tr("Save File"),
3518  pathDatabase_+"/" + name + ".ply",
3519  tr("Mesh (*.ply)"));
3520 
3521  if(!path.isEmpty())
3522  {
3523  if(QFileInfo(path).suffix() == "")
3524  {
3525  path += ".ply";
3526  }
3527  pcl::io::savePLYFileBinary(path.toStdString(), *mesh);
3528  QMessageBox::information(this, tr("Export Mesh"), tr("Exported %1!").arg(path));
3529  }
3530  }
3531  else
3532  {
3533  QString path = QFileDialog::getSaveFileName(
3534  this,
3535  tr("Save File"),
3536  pathDatabase_+"/" + name + ".ply",
3537  tr("Point cloud data (*.ply *.pcd)"));
3538 
3539  if(!path.isEmpty())
3540  {
3541  if(QFileInfo(path).suffix() == "")
3542  {
3543  path += ".ply";
3544  }
3545  bool success = false;
3546  pcl::PCLPointCloud2::Ptr cloud = util3d::laserScanToPointCloud2(LaserScan::backwardCompatibility(cloudMat));
3547  if(QFileInfo(path).suffix() == "pcd")
3548  {
3549  success = pcl::io::savePCDFile(path.toStdString(), *cloud) == 0;
3550  }
3551  else
3552  {
3553  success = pcl::io::savePLYFile(path.toStdString(), *cloud) == 0;
3554  }
3555  if(success)
3556  {
3557  QMessageBox::information(this, tr("Export Point Cloud"), tr("Exported %1!").arg(path));
3558  }
3559  else
3560  {
3561  QMessageBox::critical(this, tr("Export Point Cloud"), tr("Failed exporting %1!").arg(path));
3562  }
3563  }
3564  }
3565  }
3566 }
3567 
3568 void DatabaseViewer::updateOptimizedMesh()
3569 {
3570  if(!ids_.size() || !dbDriver_)
3571  {
3572  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
3573  return;
3574  }
3575 
3576  if(graphes_.empty())
3577  {
3578  this->updateGraphView();
3579  if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
3580  {
3581  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
3582  return;
3583  }
3584  }
3585 
3586  std::map<int, Transform> optimizedPoses;
3587  if(ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() &&
3588  ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked()
3589  && !groundTruthPoses_.empty())
3590  {
3591  optimizedPoses = groundTruthPoses_;
3592  }
3593  else
3594  {
3595  optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
3596  }
3597  if(ui_->groupBox_posefiltering->isChecked())
3598  {
3599  optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
3600  ui_->doubleSpinBox_posefilteringRadius->value(),
3601  ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
3602  }
3603  Transform lastlocalizationPose;
3604  dbDriver_->loadOptimizedPoses(&lastlocalizationPose);
3605  //optimized poses have changed, reset 2d map
3606  dbDriver_->save2DMap(cv::Mat(), 0, 0, 0);
3607  if(optimizedPoses.size() > 0)
3608  {
3609  exportDialog_->setDBDriver(dbDriver_);
3610  exportDialog_->forceAssembling(true);
3611  exportDialog_->setOkButton();
3612 
3613  std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> clouds;
3614  std::map<int, pcl::PolygonMesh::Ptr> meshes;
3615  std::map<int, pcl::TextureMesh::Ptr> textureMeshes;
3616  std::vector<std::map<int, pcl::PointXY> > textureVertexToPixels;
3617 
3618  if(exportDialog_->getExportedClouds(
3619  optimizedPoses,
3620  updateLinksWithModifications(links_),
3621  mapIds_,
3622  QMap<int, Signature>(),
3623  std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
3624  std::map<int, LaserScan>(),
3625  pathDatabase_,
3626  ui_->parameters_toolbox->getParameters(),
3627  clouds,
3628  meshes,
3629  textureMeshes,
3630  textureVertexToPixels))
3631  {
3632  if(textureMeshes.size())
3633  {
3634  dbDriver_->saveOptimizedPoses(optimizedPoses, lastlocalizationPose);
3635 
3636  cv::Mat globalTextures;
3637  pcl::TextureMeshPtr textureMesh = textureMeshes.at(0);
3638  if(textureMesh->tex_materials.size()>1)
3639  {
3640  globalTextures = util3d::mergeTextures(
3641  *textureMesh,
3642  std::map<int, cv::Mat>(),
3643  std::map<int, std::vector<CameraModel> >(),
3644  0,
3645  dbDriver_,
3646  exportDialog_->getTextureSize(),
3647  exportDialog_->getMaxTextures(),
3648  textureVertexToPixels,
3649  exportDialog_->isGainCompensation(),
3650  exportDialog_->getGainBeta(),
3651  exportDialog_->isGainRGB(),
3652  exportDialog_->isBlending(),
3653  exportDialog_->getBlendingDecimation(),
3654  exportDialog_->getTextureBrightnessConstrastRatioLow(),
3655  exportDialog_->getTextureBrightnessConstrastRatioHigh(),
3656  exportDialog_->isExposeFusion());
3657  }
3658  dbDriver_->saveOptimizedMesh(
3659  util3d::laserScanFromPointCloud(textureMesh->cloud, false).data(),
3660  util3d::convertPolygonsFromPCL(textureMesh->tex_polygons),
3661  textureMesh->tex_coordinates,
3662  globalTextures);
3663  QMessageBox::information(this, tr("Update Optimized Textured Mesh"), tr("Updated!"));
3664  ui_->actionView_optimized_mesh->setEnabled(true);
3665  ui_->actionExport_optimized_mesh->setEnabled(true);
3666  this->viewOptimizedMesh();
3667  }
3668  else if(meshes.size())
3669  {
3670  dbDriver_->saveOptimizedPoses(optimizedPoses, lastlocalizationPose);
3671  std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons(1);
3672  polygons.at(0) = util3d::convertPolygonsFromPCL(meshes.at(0)->polygons);
3673  dbDriver_->saveOptimizedMesh(util3d::laserScanFromPointCloud(meshes.at(0)->cloud, false).data(), polygons);
3674  QMessageBox::information(this, tr("Update Optimized Mesh"), tr("Updated!"));
3675  ui_->actionView_optimized_mesh->setEnabled(true);
3676  ui_->actionExport_optimized_mesh->setEnabled(true);
3677  this->viewOptimizedMesh();
3678  }
3679  else if(clouds.size())
3680  {
3681  dbDriver_->saveOptimizedPoses(optimizedPoses, lastlocalizationPose);
3682  dbDriver_->saveOptimizedMesh(util3d::laserScanFromPointCloud(*clouds.at(0)).data());
3683  QMessageBox::information(this, tr("Update Optimized PointCloud"), tr("Updated!"));
3684  ui_->actionView_optimized_mesh->setEnabled(true);
3685  ui_->actionExport_optimized_mesh->setEnabled(true);
3686  this->viewOptimizedMesh();
3687  }
3688  else
3689  {
3690  QMessageBox::critical(this, tr("Update Optimized Mesh"), tr("Nothing to save!"));
3691  }
3692  }
3693  exportDialog_->setProgressDialogToMax();
3694  }
3695  else
3696  {
3697  QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
3698  }
3699 }
3700 
3701 void DatabaseViewer::generateGraph()
3702 {
3703  if(!dbDriver_)
3704  {
3705  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("A database must must loaded first...\nUse File->Open database."));
3706  return;
3707  }
3708 
3709  QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/Graph.dot", tr("Graphiz file (*.dot)"));
3710  if(!path.isEmpty())
3711  {
3712  dbDriver_->generateGraph(path.toStdString());
3713  }
3714 }
3715 
3716 void DatabaseViewer::generateLocalGraph()
3717 {
3718  if(!ids_.size() || !dbDriver_)
3719  {
3720  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
3721  return;
3722  }
3723  bool ok = false;
3724  int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID"), ids_.first(), ids_.first(), ids_.last(), 1, &ok);
3725 
3726  if(ok)
3727  {
3728  int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin"), 4, 1, 100, 1, &ok);
3729  if(ok)
3730  {
3731  QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/Graph" + QString::number(id) + ".dot", tr("Graphiz file (*.dot)"));
3732  if(!path.isEmpty() && id>0)
3733  {
3734  std::map<int, int> ids;
3735  std::list<int> curentMarginList;
3736  std::set<int> currentMargin;
3737  std::set<int> nextMargin;
3738  nextMargin.insert(id);
3739  int m = 0;
3740  while((margin == 0 || m < margin) && nextMargin.size())
3741  {
3742  curentMarginList = std::list<int>(nextMargin.rbegin(), nextMargin.rend());
3743  nextMargin.clear();
3744 
3745  for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
3746  {
3747  if(ids.find(*jter) == ids.end())
3748  {
3749  std::multimap<int, Link> links;
3750  ids.insert(std::pair<int, int>(*jter, m));
3751 
3752  UTimer timer;
3753  dbDriver_->loadLinks(*jter, links);
3754 
3755  // links
3756  for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
3757  {
3758  if( !uContains(ids, iter->first))
3759  {
3760  UASSERT(iter->second.type() != Link::kUndef);
3761  if(iter->second.type() == Link::kNeighbor ||
3762  iter->second.type() == Link::kNeighborMerged)
3763  {
3764  nextMargin.insert(iter->first);
3765  }
3766  else
3767  {
3768  // loop closures are on same margin
3769  if(currentMargin.insert(iter->first).second)
3770  {
3771  curentMarginList.push_back(iter->first);
3772  }
3773  }
3774  }
3775  }
3776  }
3777  }
3778  ++m;
3779  }
3780 
3781  if(ids.size() > 0)
3782  {
3783  ids.insert(std::pair<int,int>(id, 0));
3784  std::set<int> idsSet;
3785  for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
3786  {
3787  idsSet.insert(idsSet.end(), iter->first);
3788  UINFO("Node %d", iter->first);
3789  }
3790  UINFO("idsSet=%d", idsSet.size());
3791  dbDriver_->generateGraph(path.toStdString(), idsSet);
3792  }
3793  else
3794  {
3795  QMessageBox::critical(this, tr("Error"), tr("No neighbors found for signature %1.").arg(id));
3796  }
3797  }
3798  }
3799  }
3800 }
3801 
3802 void DatabaseViewer::regenerateLocalMaps()
3803 {
3804  LocalGridMaker localMapMaker(ui_->parameters_toolbox->getParameters());
3805 
3806  generatedLocalMaps_.clear();
3807 
3808  rtabmap::ProgressDialog progressDialog(this);
3809  progressDialog.setMaximumSteps(ids_.size());
3810  progressDialog.show();
3811  progressDialog.setCancelButtonVisible(true);
3812 
3813  UPlot * plot = new UPlot(this);
3814  plot->setWindowFlags(Qt::Window);
3815  plot->setWindowTitle("Local Occupancy Grid Generation Time (ms)");
3816  plot->setAttribute(Qt::WA_DeleteOnClose);
3817  UPlotCurve * decompressionCurve = plot->addCurve("Decompression");
3818  UPlotCurve * gridCreationCurve = plot->addCurve("Grid Creation");
3819  plot->show();
3820 
3821  UPlot * plotCells = new UPlot(this);
3822  plotCells->setWindowFlags(Qt::Window);
3823  plotCells->setWindowTitle("Occupancy Cells");
3824  plotCells->setAttribute(Qt::WA_DeleteOnClose);
3825  UPlotCurve * totalCurve = plotCells->addCurve("Total");
3826  UPlotCurve * emptyCurve = plotCells->addCurve("Empty");
3827  UPlotCurve * obstaclesCurve = plotCells->addCurve("Obstacles");
3828  UPlotCurve * groundCurve = plotCells->addCurve("Ground");
3829  plotCells->show();
3830 
3831  double decompressionTime = 0;
3832  double gridCreationTime = 0;
3833 
3834  for(int i =0; i<ids_.size() && !progressDialog.isCanceled(); ++i)
3835  {
3836  UTimer timer;
3837  SensorData data;
3838  dbDriver_->getNodeData(ids_.at(i), data);
3839  data.uncompressData();
3840  decompressionTime = timer.ticks()*1000.0;
3841 
3842  int mapId, weight;
3843  Transform odomPose, groundTruth;
3844  std::string label;
3845  double stamp;
3846  QString msg;
3847  std::vector<float> velocity;
3848  GPS gps;
3849  EnvSensors sensors;
3850  if(dbDriver_->getNodeInfo(data.id(), odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors))
3851  {
3852  Signature s = data;
3853  s.setPose(odomPose);
3854  cv::Mat ground, obstacles, empty;
3855  cv::Point3f viewpoint;
3856  timer.ticks();
3857 
3858  if(ui_->checkBox_grid_regenerateFromSavedGrid->isChecked() && s.sensorData().gridCellSize() > 0.0f)
3859  {
3860  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridObstacleCellsRaw()));
3861  *cloud+=*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridGroundCellsRaw()));
3862 
3863  if(cloud->size())
3864  {
3865  // update viewpoint
3866  if(s.sensorData().cameraModels().size())
3867  {
3868  // average of all local transforms
3869  float sum = 0;
3870  for(unsigned int i=0; i<s.sensorData().cameraModels().size(); ++i)
3871  {
3872  const Transform & t = s.sensorData().cameraModels()[i].localTransform();
3873  if(!t.isNull())
3874  {
3875  viewpoint.x += t.x();
3876  viewpoint.y += t.y();
3877  viewpoint.z += t.z();
3878  sum += 1.0f;
3879  }
3880  }
3881  if(sum > 0.0f)
3882  {
3883  viewpoint.x /= sum;
3884  viewpoint.y /= sum;
3885  viewpoint.z /= sum;
3886  }
3887  }
3888  else if(s.sensorData().cameraModels().size())
3889  {
3890  // average of all local transforms
3891  float sum = 0;
3892  for(unsigned int i=0; i<s.sensorData().stereoCameraModels().size(); ++i)
3893  {
3894  const Transform & t = s.sensorData().stereoCameraModels()[i].localTransform();
3895  if(!t.isNull())
3896  {
3897  viewpoint.x += t.x();
3898  viewpoint.y += t.y();
3899  viewpoint.z += t.z();
3900  sum += 1.0f;
3901  }
3902  }
3903  if(sum > 0.0f)
3904  {
3905  viewpoint.x /= sum;
3906  viewpoint.y /= sum;
3907  viewpoint.z /= sum;
3908  }
3909  }
3910 
3911  localMapMaker.createLocalMap(util3d::laserScanFromPointCloud(*cloud), s.getPose(), ground, obstacles, empty, viewpoint);
3912  }
3913  }
3914  else
3915  {
3916  localMapMaker.createLocalMap(s, ground, obstacles, empty, viewpoint);
3917  }
3918 
3919  gridCreationTime = timer.ticks()*1000.0;
3920  generatedLocalMaps_.add(data.id(), ground, obstacles, empty, localMapMaker.getCellSize(), viewpoint);
3921  msg = QString("Generated local occupancy grid map %1/%2").arg(i+1).arg((int)ids_.size());
3922 
3923  totalCurve->addValue(ids_.at(i), obstacles.cols+ground.cols+empty.cols);
3924  emptyCurve->addValue(ids_.at(i), empty.cols);
3925  obstaclesCurve->addValue(ids_.at(i), obstacles.cols);
3926  groundCurve->addValue(ids_.at(i), ground.cols);
3927  }
3928 
3929  progressDialog.appendText(msg);
3930  progressDialog.incrementStep();
3931 
3932  decompressionCurve->addValue(ids_.at(i), decompressionTime);
3933  gridCreationCurve->addValue(ids_.at(i), gridCreationTime);
3934 
3935  if(ids_.size() < 50 || (i+1) % 25 == 0)
3936  {
3937  QApplication::processEvents();
3938  }
3939  }
3940  progressDialog.setValue(progressDialog.maximumSteps());
3941 
3942  if(graphes_.size())
3943  {
3944  update3dView();
3945  sliderIterationsValueChanged((int)graphes_.size()-1);
3946  }
3947  else
3948  {
3949  updateGrid();
3950  }
3951 }
3952 
3953 void DatabaseViewer::regenerateCurrentLocalMaps()
3954 {
3955  UTimer time;
3956  LocalGridMaker localMapMaker(ui_->parameters_toolbox->getParameters());
3957 
3958  if(ids_.size() == 0)
3959  {
3960  UWARN("ids_ is empty!");
3961  return;
3962  }
3963 
3964  QSet<int> idsSet;
3965  idsSet.insert(ids_.at(ui_->horizontalSlider_A->value()));
3966  idsSet.insert(ids_.at(ui_->horizontalSlider_B->value()));
3967 #if QT_VERSION >= QT_VERSION_CHECK(5, 15, 3)
3968  QList<int> ids(idsSet.begin(), idsSet.end());
3969 #else
3970  QList<int> ids = idsSet.toList();
3971 #endif
3972 
3973  rtabmap::ProgressDialog progressDialog(this);
3974  progressDialog.setMaximumSteps(ids.size());
3975  progressDialog.show();
3976 
3977  for(int i =0; i<ids.size(); ++i)
3978  {
3979  SensorData data;
3980  dbDriver_->getNodeData(ids.at(i), data);
3981  data.uncompressData();
3982 
3983  int mapId, weight;
3984  Transform odomPose, groundTruth;
3985  std::string label;
3986  double stamp;
3987  QString msg;
3988  std::vector<float> velocity;
3989  GPS gps;
3990  EnvSensors sensors;
3991  if(dbDriver_->getNodeInfo(data.id(), odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors))
3992  {
3993  Signature s = data;
3994  s.setPose(odomPose);
3995  cv::Mat ground, obstacles, empty;
3996  cv::Point3f viewpoint;
3997 
3998  if(ui_->checkBox_grid_regenerateFromSavedGrid->isChecked() && s.sensorData().gridCellSize() > 0.0f)
3999  {
4000  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridObstacleCellsRaw()));
4001  *cloud+=*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridGroundCellsRaw()));
4002 
4003  if(cloud->size())
4004  {
4005  // update viewpoint
4006  if(s.sensorData().cameraModels().size())
4007  {
4008  // average of all local transforms
4009  float sum = 0;
4010  for(unsigned int i=0; i<s.sensorData().cameraModels().size(); ++i)
4011  {
4012  const Transform & t = s.sensorData().cameraModels()[i].localTransform();
4013  if(!t.isNull())
4014  {
4015  viewpoint.x += t.x();
4016  viewpoint.y += t.y();
4017  viewpoint.z += t.z();
4018  sum += 1.0f;
4019  }
4020  }
4021  if(sum > 0.0f)
4022  {
4023  viewpoint.x /= sum;
4024  viewpoint.y /= sum;
4025  viewpoint.z /= sum;
4026  }
4027  }
4028  else if(s.sensorData().stereoCameraModels().size())
4029  {
4030  // average of all local transforms
4031  float sum = 0;
4032  for(unsigned int i=0; i<s.sensorData().stereoCameraModels().size(); ++i)
4033  {
4034  const Transform & t = s.sensorData().stereoCameraModels()[i].localTransform();
4035  if(!t.isNull())
4036  {
4037  viewpoint.x += t.x();
4038  viewpoint.y += t.y();
4039  viewpoint.z += t.z();
4040  sum += 1.0f;
4041  }
4042  }
4043  if(sum > 0.0f)
4044  {
4045  viewpoint.x /= sum;
4046  viewpoint.y /= sum;
4047  viewpoint.z /= sum;
4048  }
4049  }
4050 
4051  localMapMaker.createLocalMap(util3d::laserScanFromPointCloud(*cloud), s.getPose(), ground, obstacles, empty, viewpoint);
4052  }
4053  }
4054  else
4055  {
4056  localMapMaker.createLocalMap(s, ground, obstacles, empty, viewpoint);
4057  }
4058 
4059  generatedLocalMaps_.add(data.id(), ground, obstacles, empty, localMapMaker.getCellSize(), viewpoint);
4060  msg = QString("Generated local occupancy grid map %1/%2 (%3s)").arg(i+1).arg((int)ids.size()).arg(time.ticks());
4061  }
4062 
4063  progressDialog.appendText(msg);
4064  progressDialog.incrementStep();
4065  QApplication::processEvents();
4066  }
4067  progressDialog.setValue(progressDialog.maximumSteps());
4068 
4069  if(graphes_.size())
4070  {
4071  update3dView();
4072  sliderIterationsValueChanged((int)graphes_.size()-1);
4073  }
4074  else
4075  {
4076  updateGrid();
4077  }
4078 }
4079 
4080 void DatabaseViewer::view3DMap()
4081 {
4082  if(!ids_.size() || !dbDriver_)
4083  {
4084  QMessageBox::warning(this, tr("Cannot view 3D map"), tr("The database is empty..."));
4085  return;
4086  }
4087 
4088  if(graphes_.empty())
4089  {
4090  this->updateGraphView();
4091  if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
4092  {
4093  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
4094  return;
4095  }
4096  }
4097 
4098  std::map<int, Transform> optimizedPoses;
4099  if(ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() &&
4100  ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() &&
4101  !groundTruthPoses_.empty())
4102  {
4103  optimizedPoses = groundTruthPoses_;
4104  }
4105  else
4106  {
4107  optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
4108  }
4109  if(ui_->groupBox_posefiltering->isChecked())
4110  {
4111  optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
4112  ui_->doubleSpinBox_posefilteringRadius->value(),
4113  ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
4114  }
4115  if(optimizedPoses.size() > 0)
4116  {
4117  exportDialog_->setDBDriver(dbDriver_);
4118  exportDialog_->viewClouds(optimizedPoses,
4119  updateLinksWithModifications(links_),
4120  mapIds_,
4121  QMap<int, Signature>(),
4122  std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
4123  std::map<int, LaserScan>(),
4124  pathDatabase_,
4125  ui_->parameters_toolbox->getParameters());
4126  }
4127  else
4128  {
4129  QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
4130  }
4131 }
4132 
4133 void DatabaseViewer::generate3DMap()
4134 {
4135  if(!ids_.size() || !dbDriver_)
4136  {
4137  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
4138  return;
4139  }
4140 
4141  if(graphes_.empty())
4142  {
4143  this->updateGraphView();
4144  if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
4145  {
4146  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
4147  return;
4148  }
4149  }
4150 
4151  std::map<int, Transform> optimizedPoses;
4152  if(ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() &&
4153  ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() &&
4154  !groundTruthPoses_.empty())
4155  {
4156  optimizedPoses = groundTruthPoses_;
4157  }
4158  else
4159  {
4160  optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
4161  }
4162  if(ui_->groupBox_posefiltering->isChecked())
4163  {
4164  optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
4165  ui_->doubleSpinBox_posefilteringRadius->value(),
4166  ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
4167  }
4168  if(optimizedPoses.size() > 0)
4169  {
4170  exportDialog_->setDBDriver(dbDriver_);
4171  exportDialog_->exportClouds(optimizedPoses,
4172  updateLinksWithModifications(links_),
4173  mapIds_,
4174  QMap<int, Signature>(),
4175  std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
4176  std::map<int, LaserScan>(),
4177  pathDatabase_,
4178  ui_->parameters_toolbox->getParameters());
4179  }
4180  else
4181  {
4182  QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
4183  }
4184 }
4185 
4186 void DatabaseViewer::detectMoreLoopClosures()
4187 {
4188  if(graphes_.empty())
4189  {
4190  this->updateGraphView();
4191  if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
4192  {
4193  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
4194  return;
4195  }
4196  }
4197 
4198  std::map<int, Transform> optimizedPoses = graphes_.back();
4199 
4200  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
4201  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
4202  progressDialog->setMaximumSteps(1);
4203  progressDialog->setCancelButtonVisible(true);
4204  progressDialog->setMinimumWidth(800);
4205  progressDialog->show();
4206 
4207  const ParametersMap & parameters = ui_->parameters_toolbox->getParameters();
4208  bool loopCovLimited = Parameters::defaultRGBDLoopCovLimited();
4209  Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), loopCovLimited);
4210  if(loopCovLimited)
4211  {
4212  odomMaxInf_ = graph::getMaxOdomInf(updateLinksWithModifications(links_));
4213  }
4214 
4215  int iterations = ui_->spinBox_detectMore_iterations->value();
4216  UASSERT(iterations > 0);
4217  int added = 0;
4218  std::multimap<int, int> checkedLoopClosures;
4219  std::pair<int, int> lastAdded(0,0);
4220  bool intraSession = ui_->checkBox_detectMore_intraSession->isChecked();
4221  bool interSession = ui_->checkBox_detectMore_interSession->isChecked();
4222  bool useOptimizedGraphAsGuess = ui_->checkBox_opt_graph_as_guess->isChecked();
4223  if(!interSession && !intraSession)
4224  {
4225  QMessageBox::warning(this, tr("Cannot detect more loop closures"), tr("Intra and inter session parameters are disabled! Enable one or both."));
4226  return;
4227  }
4228 
4229  for(int n=0; n<iterations; ++n)
4230  {
4231  UINFO("iteration %d/%d", n+1, iterations);
4232  std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(
4233  std::map<int, Transform>(optimizedPoses.upper_bound(0), optimizedPoses.end()),
4234  ui_->doubleSpinBox_detectMore_radius->value(),
4235  ui_->doubleSpinBox_detectMore_angle->value()*CV_PI/180.0);
4236 
4237  progressDialog->setMaximumSteps(progressDialog->maximumSteps()+(int)clusters.size());
4238  progressDialog->appendText(tr("Looking for more loop closures, %1 clusters found.").arg(clusters.size()));
4239  QApplication::processEvents();
4240  if(progressDialog->isCanceled())
4241  {
4242  break;
4243  }
4244 
4245  std::set<int> addedLinks;
4246  int i=0;
4247  for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end() && !progressDialog->isCanceled(); ++iter, ++i)
4248  {
4249  int from = iter->first;
4250  int to = iter->second;
4251  if(from < to)
4252  {
4253  from = iter->second;
4254  to = iter->first;
4255  }
4256 
4257  int mapIdFrom = uValue(mapIds_, from, 0);
4258  int mapIdTo = uValue(mapIds_, to, 0);
4259 
4260  if((interSession && mapIdFrom != mapIdTo) ||
4261  (intraSession && mapIdFrom == mapIdTo))
4262  {
4263  // only add new links and one per cluster per iteration
4264  if(rtabmap::graph::findLink(checkedLoopClosures, from, to) == checkedLoopClosures.end())
4265  {
4266  if(!findActiveLink(from, to).isValid() && !containsLink(linksRemoved_, from, to) &&
4267  addedLinks.find(from) == addedLinks.end() &&
4268  addedLinks.find(to) == addedLinks.end())
4269  {
4270  // Reverify if in the bounds with the current optimized graph
4271  Transform delta = optimizedPoses.at(from).inverse() * optimizedPoses.at(to);
4272  if(delta.getNorm() < ui_->doubleSpinBox_detectMore_radius->value() &&
4273  delta.getNorm() >= ui_->doubleSpinBox_detectMore_radiusMin->value())
4274  {
4275  checkedLoopClosures.insert(std::make_pair(from, to));
4276  if(addConstraint(from, to, true, useOptimizedGraphAsGuess))
4277  {
4278  UINFO("Added new loop closure between %d and %d.", from, to);
4279  ++added;
4280  addedLinks.insert(from);
4281  addedLinks.insert(to);
4282  lastAdded.first = from;
4283  lastAdded.second = to;
4284 
4285  progressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
4286  QApplication::processEvents();
4287 
4288  optimizedPoses = graphes_.back();
4289  }
4290  }
4291  }
4292  }
4293  }
4294  progressDialog->incrementStep();
4295  if(i%100)
4296  {
4297  QApplication::processEvents();
4298  }
4299  }
4300  UINFO("Iteration %d/%d: added %d loop closures.", n+1, iterations, (int)addedLinks.size()/2);
4301  progressDialog->appendText(tr("Iteration %1/%2: Detected %3 loop closures!").arg(n+1).arg(iterations).arg(addedLinks.size()/2));
4302  if(addedLinks.size() == 0)
4303  {
4304  break;
4305  }
4306  if(n+1<iterations)
4307  {
4308  // Re-optimize the map before doing next iterations
4309  this->updateGraphView();
4310  optimizedPoses = graphes_.back();
4311  }
4312  }
4313 
4314  odomMaxInf_.clear();
4315 
4316  if(added)
4317  {
4318  this->updateGraphView();
4319  this->updateLoopClosuresSlider(lastAdded.first, lastAdded.second);
4320  }
4321  UINFO("Total added %d loop closures.", added);
4322 
4323  progressDialog->appendText(tr("Total new loop closures detected=%1").arg(added));
4324  progressDialog->setValue(progressDialog->maximumSteps());
4325 }
4326 
4327 void DatabaseViewer::updateAllNeighborCovariances()
4328 {
4329  updateCovariances(neighborLinks_);
4330 }
4331 void DatabaseViewer::updateAllLoopClosureCovariances()
4332 {
4333  QList<rtabmap::Link> links;
4334  for(int i=0; i<loopLinks_.size(); ++i)
4335  {
4336  if(loopLinks_.at(i).type() != Link::kLandmark)
4337  {
4338  links.push_back(loopLinks_.at(i));
4339  }
4340  }
4341  updateCovariances(links);
4342 }
4343 void DatabaseViewer::updateAllLandmarkCovariances()
4344 {
4345  QList<rtabmap::Link> links;
4346  for(int i=0; i<loopLinks_.size(); ++i)
4347  {
4348  if(loopLinks_.at(i).type() == Link::kLandmark)
4349  {
4350  links.push_back(loopLinks_.at(i));
4351  }
4352  }
4353  updateCovariances(links);
4354 }
4355 
4356 void DatabaseViewer::updateCovariances(const QList<Link> & links)
4357 {
4358  if(links.size())
4359  {
4360  bool ok = false;
4361  double stddev = QInputDialog::getDouble(this, tr("Linear error"), tr("Std deviation (m) 0=inf"), 0.01, 0.0, 9, 4, &ok);
4362  if(!ok) return;
4363  double linearVar = stddev*stddev;
4364  stddev = QInputDialog::getDouble(this, tr("Angular error"), tr("Std deviation (deg) 0=inf"), 1, 0.0, 90, 2, &ok)*M_PI/180.0;
4365  if(!ok) return;
4366  double angularVar = stddev*stddev;
4367 
4368  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
4369  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
4370  progressDialog->setMaximumSteps(links.size());
4371  progressDialog->setCancelButtonVisible(true);
4372  progressDialog->setMinimumWidth(800);
4373  progressDialog->show();
4374 
4375  cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
4376  if(linearVar == 0.0)
4377  {
4378  infMatrix(cv::Range(0,3), cv::Range(0,3)) /= 9999.9;
4379  }
4380  else
4381  {
4382  infMatrix(cv::Range(0,3), cv::Range(0,3)) /= linearVar;
4383  }
4384  if(angularVar == 0.0)
4385  {
4386  infMatrix(cv::Range(3,6), cv::Range(3,6)) /= 9999.9;
4387  }
4388  else
4389  {
4390  infMatrix(cv::Range(3,6), cv::Range(3,6)) /= angularVar;
4391  }
4392 
4393  for(int i=0; i<links.size(); ++i)
4394  {
4395  int from = links[i].from();
4396  int to = links[i].to();
4397 
4398  Link currentLink = findActiveLink(from, to);
4399  if(!currentLink.isValid())
4400  {
4401  UERROR("Not found link! (%d->%d)", from, to);
4402  return;
4403  }
4404  currentLink = Link(
4405  currentLink.from(),
4406  currentLink.to(),
4407  currentLink.type(),
4408  currentLink.transform(),
4409  infMatrix.clone(),
4410  currentLink.userDataCompressed());
4411  bool updated = false;
4412  std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
4413  while(iter != linksRefined_.end() && iter->first == currentLink.from())
4414  {
4415  if(iter->second.to() == currentLink.to() &&
4416  iter->second.type() == currentLink.type())
4417  {
4418  iter->second = currentLink;
4419  updated = true;
4420  break;
4421  }
4422  ++iter;
4423  }
4424  if(!updated)
4425  {
4426  linksRefined_.insert(std::make_pair(currentLink.from(), currentLink));
4427  }
4428 
4429  progressDialog->appendText(tr("Updated link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(links.size()));
4430  progressDialog->incrementStep();
4431  QApplication::processEvents();
4432  if(progressDialog->isCanceled())
4433  {
4434  break;
4435  }
4436  }
4437  this->updateGraphView();
4438 
4439  progressDialog->setValue(progressDialog->maximumSteps());
4440  progressDialog->appendText("Refining links finished!");
4441  }
4442 }
4443 
4444 void DatabaseViewer::refineLinks()
4445 {
4446  int minNodeId = 0;
4447  int maxNodeId = 0;
4448  int minMapId = 0;
4449  int maxMapId = 0;
4450  std::multimap<int, Link> allLinks = updateLinksWithModifications(links_);
4451  for(std::multimap<int, Link>::iterator iter=allLinks.begin(); iter!=allLinks.end(); ++iter)
4452  {
4453  int minId = iter->second.from()>iter->second.to()?iter->second.to():iter->second.from();
4454  int maxId = iter->second.from()<iter->second.to()?iter->second.to():iter->second.from();
4455  if(minNodeId == 0 || minNodeId > minId)
4456  {
4457  minNodeId = minId;
4458  }
4459  if(maxNodeId == 0 || maxNodeId < maxId)
4460  {
4461  maxNodeId = maxId;
4462  }
4463  }
4464  if(minNodeId > 0)
4465  {
4466  minMapId = uValue(mapIds_, minNodeId, 0);
4467  maxMapId = uValue(mapIds_, maxNodeId, minMapId);
4468 
4469  linkRefiningDialog_->setMinMax(
4470  minNodeId,
4471  maxNodeId,
4472  minMapId,
4473  maxMapId);
4474 
4475  if(linkRefiningDialog_->exec() == QDialog::Accepted)
4476  {
4477  QList<Link> links;
4478  Link::Type type = linkRefiningDialog_->getLinkType();
4479  linkRefiningDialog_->getRangeNodeId(minNodeId, maxNodeId);
4480  linkRefiningDialog_->getRangeNodeId(minMapId, maxMapId);
4481  bool intra, inter;
4482  linkRefiningDialog_->getIntraInterSessions(intra, inter);
4483  for(std::multimap<int, Link>::iterator iter=allLinks.begin(); iter!=allLinks.end(); ++iter)
4484  {
4485  if(type==Link::kEnd || type == iter->second.type())
4486  {
4487  int from = iter->second.from();
4488  int to = iter->second.to();
4489  int mapFrom = uValue(mapIds_, from, 0);
4490  int mapTo = uValue(mapIds_, to, 0);
4491  if(((linkRefiningDialog_->isRangeByNodeId() &&
4492  ((from >= minNodeId && from <= maxNodeId) ||
4493  (to >= minNodeId && to <= maxNodeId))) ||
4494  (linkRefiningDialog_->isRangeByMapId() &&
4495  ((mapFrom >= minMapId && mapFrom <= maxMapId) ||
4496  (mapTo >= minMapId && mapTo <= maxMapId)))) &&
4497  ((intra && mapTo == mapFrom) ||
4498  (inter && mapTo != mapFrom)))
4499  {
4500  links.push_back(iter->second);
4501  }
4502  }
4503  }
4504  if(links.isEmpty())
4505  {
4506  QMessageBox::warning(this, tr("Refine links"), tr("No links found matching the requested parameters."));
4507  return;
4508  }
4509  else
4510  {
4511  refineLinks(links);
4512  }
4513  }
4514  }
4515 }
4516 void DatabaseViewer::refineLinks(const QList<Link> & links)
4517 {
4518  if(links.size())
4519  {
4520  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
4521  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
4522  progressDialog->setMaximumSteps(links.size());
4523  progressDialog->setCancelButtonVisible(true);
4524  progressDialog->setMinimumWidth(800);
4525  progressDialog->show();
4526 
4527  for(int i=0; i<links.size(); ++i)
4528  {
4529  int from = links[i].from();
4530  int to = links[i].to();
4531  if(from > 0 && to > 0)
4532  {
4533  this->refineConstraint(links[i].from(), links[i].to(), true);
4534  progressDialog->appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(links.size()));
4535  }
4536  else
4537  {
4538  progressDialog->appendText(tr("Ignored link %1->%2 (landmark)").arg(from).arg(to));
4539  }
4540  progressDialog->incrementStep();
4541  QApplication::processEvents();
4542  if(progressDialog->isCanceled())
4543  {
4544  break;
4545  }
4546  }
4547  this->updateGraphView();
4548 
4549  progressDialog->setValue(progressDialog->maximumSteps());
4550  progressDialog->appendText("Refining links finished!");
4551  }
4552 }
4553 
4554 void DatabaseViewer::resetAllChanges()
4555 {
4556  if(QMessageBox::question(this,
4557  tr("Reset all changes"),
4558  tr("You are about to reset all changes you've made so far, do you want to continue?"),
4559  QMessageBox::Yes | QMessageBox::No,
4560  QMessageBox::No) == QMessageBox::Yes)
4561  {
4562  linksAdded_.clear();
4563  linksRefined_.clear();
4564  linksRemoved_.clear();
4565  generatedLocalMaps_.clear();
4566  modifiedLaserScans_.clear();
4567  updateLoopClosuresSlider();
4568  this->updateGraphView();
4569  }
4570 }
4571 
4572 void DatabaseViewer::graphNodeSelected(int id)
4573 {
4574  if(id>0 && idToIndex_.contains(id))
4575  {
4576  static bool updateA = true;
4577  if(updateA)
4578  ui_->horizontalSlider_A->setValue(idToIndex_.value(id));
4579  else
4580  ui_->horizontalSlider_B->setValue(idToIndex_.value(id));
4581  updateA = !updateA;
4582  }
4583 }
4584 
4585 void DatabaseViewer::graphLinkSelected(int from, int to)
4586 {
4587  if(from>0 && idToIndex_.contains(from))
4588  ui_->horizontalSlider_A->setValue(idToIndex_.value(from));
4589  if(to>0 && idToIndex_.contains(to))
4590  ui_->horizontalSlider_B->setValue(idToIndex_.value(to));
4591 }
4592 
4593 void DatabaseViewer::sliderAValueChanged(int value)
4594 {
4595  this->update(value,
4596  ui_->label_indexA,
4597  ui_->label_parentsA,
4598  ui_->label_childrenA,
4599  ui_->label_weightA,
4600  ui_->label_labelA,
4601  ui_->label_stampA,
4602  ui_->graphicsView_A,
4603  ui_->label_idA,
4604  ui_->label_mapA,
4605  ui_->label_poseA,
4606  ui_->label_optposeA,
4607  ui_->label_velA,
4608  ui_->label_calibA,
4609  ui_->label_scanA,
4610  ui_->label_gravityA,
4611  ui_->label_priorA,
4612  ui_->toolButton_edit_priorA,
4613  ui_->toolButton_remove_priorA,
4614  ui_->label_gpsA,
4615  ui_->label_gtA,
4616  ui_->label_sensorsA,
4617  true);
4618 }
4619 
4620 void DatabaseViewer::sliderBValueChanged(int value)
4621 {
4622  this->update(value,
4623  ui_->label_indexB,
4624  ui_->label_parentsB,
4625  ui_->label_childrenB,
4626  ui_->label_weightB,
4627  ui_->label_labelB,
4628  ui_->label_stampB,
4629  ui_->graphicsView_B,
4630  ui_->label_idB,
4631  ui_->label_mapB,
4632  ui_->label_poseB,
4633  ui_->label_optposeB,
4634  ui_->label_velB,
4635  ui_->label_calibB,
4636  ui_->label_scanB,
4637  ui_->label_gravityB,
4638  ui_->label_priorB,
4639  ui_->toolButton_edit_priorB,
4640  ui_->toolButton_remove_priorB,
4641  ui_->label_gpsB,
4642  ui_->label_gtB,
4643  ui_->label_sensorsB,
4644  true);
4645 }
4646 
4647 void DatabaseViewer::update(int value,
4648  QLabel * labelIndex,
4649  QLabel * labelParents,
4650  QLabel * labelChildren,
4651  QLabel * weight,
4652  QLabel * label,
4653  QLabel * stamp,
4654  rtabmap::ImageView * view,
4655  QLabel * labelId,
4656  QLabel * labelMapId,
4657  QLabel * labelPose,
4658  QLabel * labelOptPose,
4659  QLabel * labelVelocity,
4660  QLabel * labelCalib,
4661  QLabel * labelScan,
4662  QLabel * labelGravity,
4663  QLabel * labelPrior,
4664  QToolButton * editPriorButton,
4665  QToolButton * removePriorButton,
4666  QLabel * labelGps,
4667  QLabel * labelGt,
4668  QLabel * labelSensors,
4669  bool updateConstraintView)
4670 {
4671  lastSliderIndexBrowsed_ = value;
4672 
4673  UTimer timer;
4674  labelIndex->setText(QString::number(value));
4675  labelParents->clear();
4676  labelChildren->clear();
4677  weight->clear();
4678  label->clear();
4679  labelMapId->clear();
4680  labelPose->clear();
4681  labelOptPose->clear();
4682  labelVelocity->clear();
4683  stamp->clear();
4684  labelCalib->clear();
4685  labelScan ->clear();
4686  labelGravity->clear();
4687  labelPrior->clear();
4688  editPriorButton->setVisible(false);
4689  removePriorButton->setVisible(false);
4690  labelGps->clear();
4691  labelGt->clear();
4692  labelSensors->clear();
4693  if(value >= 0 && value < ids_.size())
4694  {
4695  view->clear();
4696  int id = ids_.at(value);
4697  int mapId = -1;
4698  labelId->setText(QString::number(id));
4699  if(id>0)
4700  {
4701  //image
4702  QImage img;
4703  cv::Mat imgDepth;
4704  if(dbDriver_)
4705  {
4706  SensorData data;
4707  dbDriver_->getNodeData(id, data);
4708  data.uncompressData();
4709  if(!data.imageRaw().empty())
4710  {
4711  img = uCvMat2QImage(data.imageRaw());
4712  }
4713  if(!data.depthOrRightRaw().empty())
4714  {
4715  cv::Mat depth =data.depthOrRightRaw();
4716  if(!data.depthRaw().empty())
4717  {
4718  if(ui_->spinBox_mesh_fillDepthHoles->value() > 0)
4719  {
4720  depth = util2d::fillDepthHoles(depth, ui_->spinBox_mesh_fillDepthHoles->value(), float(ui_->spinBox_mesh_depthError->value())/100.0f);
4721  }
4722  }
4723  if( !data.imageRaw().empty() &&
4724  !data.rightRaw().empty() &&
4725  data.stereoCameraModels().size()==1 && // Multiple stereo cameras not implemented
4726  data.stereoCameraModels()[0].isValidForProjection() &&
4727  ui_->checkBox_showDisparityInsteadOfRight->isChecked())
4728  {
4729  rtabmap::StereoDense * denseStereo = rtabmap::StereoDense::create(ui_->parameters_toolbox->getParameters());
4730  depth = util2d::depthFromDisparity(denseStereo->computeDisparity(data.imageRaw(), data.rightRaw()), data.stereoCameraModels()[0].left().fx(), data.stereoCameraModels()[0].baseline(), CV_32FC1);
4731  delete denseStereo;
4732  }
4733  imgDepth = depth;
4734  }
4735 
4736  QRectF rect;
4737  if(!img.isNull())
4738  {
4739  Transform pose;
4740  if(!graphes_.empty() && graphes_.back().find(data.id())!=graphes_.back().end())
4741  {
4742  pose = graphes_.back().at(data.id());
4743  }
4744  view->setImage(img, data.cameraModels(), pose);
4745  rect = img.rect();
4746  }
4747  else
4748  {
4749  ULOGGER_DEBUG("Image is empty");
4750  }
4751 
4752  if(!imgDepth.empty())
4753  {
4754  view->setImageDepth(imgDepth);
4755  if(img.isNull())
4756  {
4757  rect.setWidth(imgDepth.cols);
4758  rect.setHeight(imgDepth.rows);
4759  }
4760  }
4761  else
4762  {
4763  ULOGGER_DEBUG("Image depth is empty");
4764  }
4765  if(rect.isValid())
4766  {
4767  view->setSceneRect(rect);
4768  }
4769 
4770  std::list<int> ids;
4771  ids.push_back(id);
4772  std::list<Signature*> signatures;
4773  dbDriver_->loadSignatures(ids, signatures);
4774 
4775  if(signatures.size() && signatures.front()!=0 && !signatures.front()->getWordsKpts().empty())
4776  {
4777  std::multimap<int, cv::KeyPoint> keypoints;
4778  for(std::map<int, int>::const_iterator iter=signatures.front()->getWords().begin(); iter!=signatures.front()->getWords().end(); ++iter)
4779  {
4780  keypoints.insert(std::make_pair(iter->first, signatures.front()->getWordsKpts()[iter->second]));
4781  }
4782  view->setFeatures(keypoints, data.depthOrRightRaw().type() == CV_8UC1?cv::Mat():data.depthOrRightRaw(), Qt::yellow);
4783  }
4784 
4785  Transform odomPose, g;
4786  int w;
4787  std::string l;
4788  double s;
4789  std::vector<float> v;
4790  GPS gps;
4791  EnvSensors sensors;
4792  dbDriver_->getNodeInfo(id, odomPose, mapId, w, l, s, g, v, gps, sensors);
4793 
4794  weight->setNum(w);
4795  label->setText(l.c_str());
4796  float x,y,z,roll,pitch,yaw;
4797  odomPose.getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw);
4798  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));
4799  if(graphes_.size())
4800  {
4801  if(graphes_.back().find(id) == graphes_.back().end())
4802  {
4803  labelOptPose->setText("<Not in optimized graph>");
4804  }
4805  else
4806  {
4807  graphes_.back().find(id)->second.getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw);
4808  labelOptPose->setText(QString("xyz=(%1,%2,%3)\nrpy=(%4,%5,%6)").arg(x).arg(y).arg(z).arg(roll).arg(pitch).arg(yaw));
4809  }
4810  }
4811  if(s!=0.0)
4812  {
4813  stamp->setText(QString::number(s, 'f'));
4814  stamp->setToolTip(QDateTime::fromMSecsSinceEpoch(s*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
4815  }
4816  if(v.size()==6)
4817  {
4818  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]));
4819  }
4820 
4821  std::multimap<int, Link> gravityLink;
4822  dbDriver_->loadLinks(id, gravityLink, Link::kGravity);
4823  if(!gravityLink.empty())
4824  {
4825  float roll,pitch,yaw;
4826  gravityLink.begin()->second.transform().getEulerAngles(roll, pitch, yaw);
4827  Eigen::Vector3d v = Transform(0,0,0,roll,pitch,0).toEigen3d() * -Eigen::Vector3d::UnitZ();
4828  labelGravity->setText(QString("x=%1 y=%2 z=%3").arg(v[0]).arg(v[1]).arg(v[2]));
4829  labelGravity->setToolTip(QString("roll=%1 pitch=%2 yaw=%3").arg(roll).arg(pitch).arg(yaw));
4830  }
4831 
4832  std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
4833  if(graph::findLink(links, id, id, false, Link::kPosePrior)!=links.end())
4834  {
4835  Link & priorLink = graph::findLink(links, id, id, false, Link::kPosePrior)->second;
4836  priorLink.transform().getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw);
4837  labelPrior->setText(QString("xyz=(%1,%2,%3)\nrpy=(%4,%5,%6)").arg(x).arg(y).arg(z).arg(roll).arg(pitch).arg(yaw));
4838  std::stringstream out;
4839  out << priorLink.infMatrix().inv();
4840  labelPrior->setToolTip(QString("%1").arg(out.str().c_str()));
4841  removePriorButton->setVisible(true);
4842  editPriorButton->setToolTip(tr("Edit Prior"));
4843  editPriorButton->setText("...");
4844  editPriorButton->setVisible(true);
4845  }
4846  else if(!odomPose.isNull())
4847  {
4848  editPriorButton->setToolTip(tr("Add Prior"));
4849  editPriorButton->setText("+");
4850  editPriorButton->setVisible(true);
4851  }
4852 
4853  if(gps.stamp()>0.0)
4854  {
4855  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()));
4856  labelGps->setToolTip(QDateTime::fromMSecsSinceEpoch(gps.stamp()*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
4857  }
4858  if(!g.isNull())
4859  {
4860  labelGt->setText(QString("%1").arg(g.prettyPrint().c_str()));
4861  }
4862  if(sensors.size())
4863  {
4864  QString sensorsStr;
4865  QString tooltipStr;
4866  for(EnvSensors::iterator iter=sensors.begin(); iter!=sensors.end(); ++iter)
4867  {
4868  if(iter != sensors.begin())
4869  {
4870  sensorsStr += " | ";
4871  tooltipStr += " | ";
4872  }
4873 
4874  if(iter->first == EnvSensor::kWifiSignalStrength)
4875  {
4876  sensorsStr += uFormat("%.1f dbm", iter->second.value()).c_str();
4877  tooltipStr += "Wifi signal strength";
4878  }
4879  else if(iter->first == EnvSensor::kAmbientTemperature)
4880  {
4881  sensorsStr += uFormat("%.1f \u00B0C", iter->second.value()).c_str();
4882  tooltipStr += "Ambient Temperature";
4883  }
4884  else if(iter->first == EnvSensor::kAmbientAirPressure)
4885  {
4886  sensorsStr += uFormat("%.1f hPa", iter->second.value()).c_str();
4887  tooltipStr += "Ambient Air Pressure";
4888  }
4889  else if(iter->first == EnvSensor::kAmbientLight)
4890  {
4891  sensorsStr += uFormat("%.0f lx", iter->second.value()).c_str();
4892  tooltipStr += "Ambient Light";
4893  }
4894  else if(iter->first == EnvSensor::kAmbientRelativeHumidity)
4895  {
4896  sensorsStr += uFormat("%.0f %%", iter->second.value()).c_str();
4897  tooltipStr += "Ambient Relative Humidity";
4898  }
4899  else
4900  {
4901  sensorsStr += uFormat("%.2f", iter->second.value()).c_str();
4902  tooltipStr += QString("Type %1").arg((int)iter->first);
4903  }
4904 
4905  }
4906  labelSensors->setText(sensorsStr);
4907  labelSensors->setToolTip(tooltipStr);
4908  }
4909  if(data.cameraModels().size() || data.stereoCameraModels().size())
4910  {
4911  std::stringstream calibrationDetails;
4912  if(data.cameraModels().size())
4913  {
4914  if(!data.depthRaw().empty() && data.depthRaw().cols!=data.imageRaw().cols && data.imageRaw().cols)
4915  {
4916  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]")
4917  .arg(data.cameraModels().size())
4918  .arg(data.cameraModels()[0].imageWidth()>0?data.cameraModels()[0].imageWidth():data.imageRaw().cols/data.cameraModels().size())
4919  .arg(data.cameraModels()[0].imageHeight()>0?data.cameraModels()[0].imageHeight():data.imageRaw().rows)
4920  .arg(data.cameraModels()[0].fx())
4921  .arg(data.cameraModels()[0].fy())
4922  .arg(data.cameraModels()[0].cx())
4923  .arg(data.cameraModels()[0].cy())
4924  .arg(data.depthRaw().cols/data.cameraModels().size())
4925  .arg(data.depthRaw().rows)
4926  .arg(data.cameraModels()[0].localTransform().prettyPrint().c_str())
4927  .arg(data.cameraModels()[0].localTransform().r11()).arg(data.cameraModels()[0].localTransform().r12()).arg(data.cameraModels()[0].localTransform().r13()).arg(data.cameraModels()[0].localTransform().o14())
4928  .arg(data.cameraModels()[0].localTransform().r21()).arg(data.cameraModels()[0].localTransform().r22()).arg(data.cameraModels()[0].localTransform().r23()).arg(data.cameraModels()[0].localTransform().o24())
4929  .arg(data.cameraModels()[0].localTransform().r31()).arg(data.cameraModels()[0].localTransform().r32()).arg(data.cameraModels()[0].localTransform().r33()).arg(data.cameraModels()[0].localTransform().o34()));
4930  }
4931  else
4932  {
4933  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]")
4934  .arg(data.cameraModels().size())
4935  .arg(data.cameraModels()[0].imageWidth()>0?data.cameraModels()[0].imageWidth():data.imageRaw().cols/data.cameraModels().size())
4936  .arg(data.cameraModels()[0].imageHeight()>0?data.cameraModels()[0].imageHeight():data.imageRaw().rows)
4937  .arg(data.cameraModels()[0].fx())
4938  .arg(data.cameraModels()[0].fy())
4939  .arg(data.cameraModels()[0].cx())
4940  .arg(data.cameraModels()[0].cy())
4941  .arg(data.cameraModels()[0].localTransform().prettyPrint().c_str())
4942  .arg(data.cameraModels()[0].localTransform().r11()).arg(data.cameraModels()[0].localTransform().r12()).arg(data.cameraModels()[0].localTransform().r13()).arg(data.cameraModels()[0].localTransform().o14())
4943  .arg(data.cameraModels()[0].localTransform().r21()).arg(data.cameraModels()[0].localTransform().r22()).arg(data.cameraModels()[0].localTransform().r23()).arg(data.cameraModels()[0].localTransform().o24())
4944  .arg(data.cameraModels()[0].localTransform().r31()).arg(data.cameraModels()[0].localTransform().r32()).arg(data.cameraModels()[0].localTransform().r33()).arg(data.cameraModels()[0].localTransform().o34()));
4945  }
4946 
4947  for(unsigned int i=0; i<data.cameraModels().size();++i)
4948  {
4949  if(i!=0) calibrationDetails << std::endl;
4950  calibrationDetails << "Id: " << i << " Size=" << data.cameraModels()[i].imageWidth() << "x" << data.cameraModels()[i].imageHeight() << std::endl;
4951  if( data.cameraModels()[i].K_raw().total()) calibrationDetails << "K=" << data.cameraModels()[i].K_raw() << std::endl;
4952  if( data.cameraModels()[i].D_raw().total()) calibrationDetails << "D=" << data.cameraModels()[i].D_raw() << std::endl;
4953  if( data.cameraModels()[i].R().total()) calibrationDetails << "R=" << data.cameraModels()[i].R() << std::endl;
4954  if( data.cameraModels()[i].P().total()) calibrationDetails << "P=" << data.cameraModels()[i].P() << std::endl;
4955  }
4956 
4957  }
4958  else if(data.stereoCameraModels().size())
4959  {
4960  //stereo
4961  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]")
4962  .arg(data.stereoCameraModels()[0].left().imageWidth()>0?data.stereoCameraModels()[0].left().imageWidth():data.imageRaw().cols)
4963  .arg(data.stereoCameraModels()[0].left().imageHeight()>0?data.stereoCameraModels()[0].left().imageHeight():data.imageRaw().rows)
4964  .arg(data.stereoCameraModels()[0].left().fx())
4965  .arg(data.stereoCameraModels()[0].left().fy())
4966  .arg(data.stereoCameraModels()[0].left().cx())
4967  .arg(data.stereoCameraModels()[0].left().cy())
4968  .arg(data.stereoCameraModels()[0].baseline())
4969  .arg(data.stereoCameraModels()[0].localTransform().prettyPrint().c_str())
4970  .arg(data.stereoCameraModels()[0].localTransform().r11()).arg(data.stereoCameraModels()[0].localTransform().r12()).arg(data.stereoCameraModels()[0].localTransform().r13()).arg(data.stereoCameraModels()[0].localTransform().o14())
4971  .arg(data.stereoCameraModels()[0].localTransform().r21()).arg(data.stereoCameraModels()[0].localTransform().r22()).arg(data.stereoCameraModels()[0].localTransform().r23()).arg(data.stereoCameraModels()[0].localTransform().o24())
4972  .arg(data.stereoCameraModels()[0].localTransform().r31()).arg(data.stereoCameraModels()[0].localTransform().r32()).arg(data.stereoCameraModels()[0].localTransform().r33()).arg(data.stereoCameraModels()[0].localTransform().o34()));
4973 
4974  for(unsigned int i=0; i<data.stereoCameraModels().size();++i)
4975  {
4976  calibrationDetails << "Id: " << i << std::endl;
4977  calibrationDetails << " Left:" << " Size=" << data.stereoCameraModels()[i].left().imageWidth() << "x" << data.stereoCameraModels()[i].left().imageHeight() << std::endl;
4978  if( data.stereoCameraModels()[i].left().K_raw().total()) calibrationDetails << " K=" << data.stereoCameraModels()[i].left().K_raw() << std::endl;
4979  if( data.stereoCameraModels()[i].left().D_raw().total()) calibrationDetails << " D=" << data.stereoCameraModels()[i].left().D_raw() << std::endl;
4980  if( data.stereoCameraModels()[i].left().R().total()) calibrationDetails << " R=" << data.stereoCameraModels()[i].left().R() << std::endl;
4981  if( data.stereoCameraModels()[i].left().P().total()) calibrationDetails << " P=" << data.stereoCameraModels()[i].left().P() << std::endl;
4982  calibrationDetails << " Right:" << " Size=" << data.stereoCameraModels()[i].right().imageWidth() << "x" << data.stereoCameraModels()[i].right().imageHeight() << std::endl;
4983  if( data.stereoCameraModels()[i].right().K_raw().total()) calibrationDetails << " K=" << data.stereoCameraModels()[i].right().K_raw() << std::endl;
4984  if( data.stereoCameraModels()[i].right().D_raw().total()) calibrationDetails << " D=" << data.stereoCameraModels()[i].right().D_raw() << std::endl;
4985  if( data.stereoCameraModels()[i].right().R().total()) calibrationDetails << " R=" << data.stereoCameraModels()[i].right().R() << std::endl;
4986  if( data.stereoCameraModels()[i].right().P().total()) calibrationDetails << " P=" << data.stereoCameraModels()[i].right().P() << std::endl;
4987  if( data.stereoCameraModels()[i].R().total()) calibrationDetails << " R=" << data.stereoCameraModels()[i].R() << std::endl;
4988  if( data.stereoCameraModels()[i].T().total()) calibrationDetails << " T=" << data.stereoCameraModels()[i].T() << std::endl;
4989  if( data.stereoCameraModels()[i].F().total()) calibrationDetails << " F=" << data.stereoCameraModels()[i].F() << std::endl;
4990  if( data.stereoCameraModels()[i].E().total()) calibrationDetails << " E=" << data.stereoCameraModels()[i].E() << std::endl;
4991  }
4992  }
4993  labelCalib->setToolTip(calibrationDetails.str().c_str());
4994 
4995  }
4996  else
4997  {
4998  labelCalib->setText("NA");
4999  }
5000 
5001  if(data.laserScanRaw().size())
5002  {
5003  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")
5004  .arg(data.laserScanRaw().formatName().c_str())
5005  .arg(data.laserScanRaw().size())
5006  .arg(data.laserScanRaw().maxPoints())
5007  .arg(data.laserScanRaw().rangeMin())
5008  .arg(data.laserScanRaw().rangeMax())
5009  .arg(data.laserScanRaw().angleMin())
5010  .arg(data.laserScanRaw().angleMax())
5011  .arg(data.laserScanRaw().angleIncrement())
5012  .arg(data.laserScanRaw().hasRGB()?1:0)
5013  .arg(data.laserScanRaw().is2d()?1:0)
5014  .arg(data.laserScanRaw().hasNormals()?1:0)
5015  .arg(data.laserScanRaw().hasIntensity()?1:0)
5016  .arg(data.laserScanRaw().localTransform().prettyPrint().c_str()));
5017  }
5018 
5019  //stereo
5020  if(!data.depthOrRightRaw().empty() && data.depthOrRightRaw().type() == CV_8UC1)
5021  {
5022  this->updateStereo(&data);
5023  }
5024  else
5025  {
5026  stereoViewer_->clear();
5027  ui_->graphicsView_stereo->clear();
5028  }
5029 
5030  // 3d view
5031  if(cloudViewer_->isVisible())
5032  {
5033  cloudViewer_->removeAllLines();
5034  cloudViewer_->removeAllFrustums();
5035  cloudViewer_->removeOccupancyGridMap();
5036  cloudViewer_->removeAllClouds();
5037  cloudViewer_->removeOctomap();
5038  cloudViewer_->removeElevationMap();
5039 
5040  Transform pose = Transform::getIdentity();
5041  if(signatures.size() && ui_->checkBox_odomFrame_3dview->isChecked())
5042  {
5043  float x, y, z, roll, pitch, yaw;
5044  (*signatures.begin())->getPose().getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
5045  pose = Transform(0,0,z,roll,pitch,0);
5046  }
5047 
5048  if(!gravityLink.empty() && ui_->checkBox_gravity_3dview->isChecked())
5049  {
5050  Transform gravityT = gravityLink.begin()->second.transform();
5051  Eigen::Vector3f gravity(0,0,-1);
5052  if(pose.isIdentity())
5053  {
5054  gravityT = gravityT.inverse();
5055  }
5056  gravity = (gravityT.rotation()*(pose).rotation().inverse()).toEigen3f()*gravity;
5057  cloudViewer_->addOrUpdateLine("gravity", pose, (pose).translation()*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0)*pose.rotation().inverse(), Qt::yellow, true, false);
5058  }
5059 
5060  //add scan
5061  LaserScan laserScanRaw = data.laserScanRaw();
5062  if(modifiedLaserScans_.find(id)!=modifiedLaserScans_.end())
5063  {
5064  laserScanRaw = modifiedLaserScans_.at(id);
5065  }
5066  if(ui_->checkBox_showScan->isChecked() && laserScanRaw.size())
5067  {
5068  if(laserScanRaw.hasRGB() && laserScanRaw.hasNormals())
5069  {
5070  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr scan = util3d::laserScanToPointCloudRGBNormal(laserScanRaw, laserScanRaw.localTransform());
5071  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
5072  }
5073  else if(laserScanRaw.hasIntensity() && laserScanRaw.hasNormals())
5074  {
5075  pcl::PointCloud<pcl::PointXYZINormal>::Ptr scan = util3d::laserScanToPointCloudINormal(laserScanRaw, laserScanRaw.localTransform());
5076  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
5077  }
5078  else if(laserScanRaw.hasNormals())
5079  {
5080  pcl::PointCloud<pcl::PointNormal>::Ptr scan = util3d::laserScanToPointCloudNormal(laserScanRaw, laserScanRaw.localTransform());
5081  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
5082  }
5083  else if(laserScanRaw.hasRGB())
5084  {
5085  pcl::PointCloud<pcl::PointXYZRGB>::Ptr scan = util3d::laserScanToPointCloudRGB(laserScanRaw, laserScanRaw.localTransform());
5086  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
5087  }
5088  else if(laserScanRaw.hasIntensity())
5089  {
5090  pcl::PointCloud<pcl::PointXYZI>::Ptr scan = util3d::laserScanToPointCloudI(laserScanRaw, laserScanRaw.localTransform());
5091  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
5092  }
5093  else
5094  {
5095  pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(laserScanRaw, laserScanRaw.localTransform());
5096  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
5097  }
5098  }
5099 
5100  // add RGB-D cloud
5101  if(ui_->checkBox_showCloud->isChecked() && ui_->checkBox_cameraProjection->isChecked() &&
5102  !data.imageRaw().empty() && !laserScanRaw.empty() && !laserScanRaw.is2d())
5103  {
5104  pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud = util3d::laserScanToPointCloudINormal(laserScanRaw, laserScanRaw.localTransform());
5105  std::vector<CameraModel> models = data.cameraModels();
5106  if(!data.stereoCameraModels().empty())
5107  {
5108  models.clear();
5109  for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
5110  {
5111  models.push_back(data.stereoCameraModels()[i].left());
5112  }
5113  }
5114 
5115  if(!models.empty() && !models[0].isValidForProjection())
5116  {
5117  models.clear();
5118  }
5119 
5120  if(!models.empty() && models[0].imageWidth() != 0 && models[0].imageHeight() != 0)
5121  {
5122  std::map<int, Transform> cameraPoses;
5123  std::map<int, std::vector<CameraModel> > cameraModels;
5124  cameraPoses.insert(std::make_pair(data.id(), Transform::getIdentity()));
5125  cameraModels.insert(std::make_pair(data.id(), models));
5126 
5127  std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;
5128  pointToPixel = util3d::projectCloudToCameras(
5129  *cloud,
5130  cameraPoses,
5131  cameraModels);
5132  // color the cloud
5133  UASSERT(pointToPixel.empty() || pointToPixel.size() == cloud->size());
5134  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudValidPoints(new pcl::PointCloud<pcl::PointXYZRGB>);
5135  cloudValidPoints->resize(cloud->size());
5136  int oi=0;
5137  for(size_t i=0; i<pointToPixel.size(); ++i)
5138  {
5139  pcl::PointXYZINormal & pt = cloud->at(i);
5140  pcl::PointXYZRGB ptColor;
5141  int nodeID = pointToPixel[i].first.first;
5142  int cameraIndex = pointToPixel[i].first.second;
5143  if(nodeID>0 && cameraIndex>=0)
5144  {
5145  cv::Mat image = data.imageRaw();
5146 
5147  int subImageWidth = image.cols / cameraModels.at(nodeID).size();
5148  image = image(cv::Range::all(), cv::Range(cameraIndex*subImageWidth, (cameraIndex+1)*subImageWidth));
5149 
5150 
5151  int x = pointToPixel[i].second.x * (float)image.cols;
5152  int y = pointToPixel[i].second.y * (float)image.rows;
5153  UASSERT(x>=0 && x<image.cols);
5154  UASSERT(y>=0 && y<image.rows);
5155 
5156  if(image.type()==CV_8UC3)
5157  {
5158  cv::Vec3b bgr = image.at<cv::Vec3b>(y, x);
5159  ptColor.b = bgr[0];
5160  ptColor.g = bgr[1];
5161  ptColor.r = bgr[2];
5162  }
5163  else
5164  {
5165  UASSERT(image.type()==CV_8UC1);
5166  ptColor.r = ptColor.g = ptColor.b = image.at<unsigned char>(pointToPixel[i].second.y * image.rows, pointToPixel[i].second.x * image.cols);
5167  }
5168 
5169  ptColor.x = pt.x;
5170  ptColor.y = pt.y;
5171  ptColor.z = pt.z;
5172  cloudValidPoints->at(oi++) = ptColor;
5173  }
5174  }
5175  cloudValidPoints->resize(oi);
5176  if(!cloudValidPoints->empty())
5177  {
5178  if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
5179  {
5180  cloudValidPoints = util3d::voxelize(cloudValidPoints, ui_->doubleSpinBox_voxelSize->value());
5181  }
5182 
5183  cloudViewer_->addCloud("cloud", cloudValidPoints, pose);
5184  }
5185  else
5186  {
5187  UWARN("Camera projection to scan returned an empty cloud, no visible points from cameras...");
5188  }
5189  }
5190  else
5191  {
5192  UERROR("Node has invalid camera models, camera projection on scan cannot be done!.");
5193  }
5194  }
5195  else if(ui_->checkBox_showCloud->isChecked() || ui_->checkBox_showMesh->isChecked())
5196  {
5197  if(!data.depthOrRightRaw().empty())
5198  {
5199  if(!data.imageRaw().empty())
5200  {
5201  std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
5202  std::vector<pcl::IndicesPtr> allIndices;
5203  if(!data.depthRaw().empty() && data.cameraModels().size()==1)
5204  {
5205  cv::Mat depth = data.depthRaw();
5206  if(ui_->spinBox_mesh_fillDepthHoles->value() > 0)
5207  {
5208  depth = util2d::fillDepthHoles(depth, ui_->spinBox_mesh_fillDepthHoles->value(), float(ui_->spinBox_mesh_depthError->value())/100.0f);
5209  }
5210  pcl::IndicesPtr indices(new std::vector<int>);
5211  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
5212  data.imageRaw(),
5213  depth,
5214  data.cameraModels()[0],
5215  ui_->spinBox_decimation->value(),0,0,indices.get());
5216  if(indices->size())
5217  {
5218  clouds.push_back(util3d::transformPointCloud(cloud, data.cameraModels()[0].localTransform()));
5219  allIndices.push_back(indices);
5220  }
5221  }
5222  else
5223  {
5224  clouds = util3d::cloudsRGBFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, &allIndices, ui_->parameters_toolbox->getParameters());
5225  }
5226  UASSERT(clouds.size() == allIndices.size());
5227  for(size_t i=0; i<allIndices.size(); ++i)
5228  {
5229  if(allIndices[i]->size())
5230  {
5231  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = clouds[i];
5232  pcl::IndicesPtr indices = allIndices[i];
5233  if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
5234  {
5235  cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value());
5236  }
5237 
5238  if(ui_->checkBox_showMesh->isChecked() && !cloud->is_dense)
5239  {
5240  Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
5241  if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
5242  {
5243  viewpoint[0] = data.cameraModels()[0].localTransform().x();
5244  viewpoint[1] = data.cameraModels()[0].localTransform().y();
5245  viewpoint[2] = data.cameraModels()[0].localTransform().z();
5246  }
5247  else if(data.stereoCameraModels().size() && !data.stereoCameraModels()[0].localTransform().isNull())
5248  {
5249  viewpoint[0] = data.stereoCameraModels()[0].localTransform().x();
5250  viewpoint[1] = data.stereoCameraModels()[0].localTransform().y();
5251  viewpoint[2] = data.stereoCameraModels()[0].localTransform().z();
5252  }
5253  std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
5254  cloud,
5255  float(ui_->spinBox_mesh_angleTolerance->value())*M_PI/180.0f,
5256  ui_->checkBox_mesh_quad->isChecked(),
5257  ui_->spinBox_mesh_triangleSize->value(),
5258  viewpoint);
5259 
5260  if(ui_->spinBox_mesh_minClusterSize->value())
5261  {
5262  // filter polygons
5263  std::vector<std::set<int> > neighbors;
5264  std::vector<std::set<int> > vertexToPolygons;
5265  util3d::createPolygonIndexes(polygons,
5266  cloud->size(),
5267  neighbors,
5268  vertexToPolygons);
5269  std::list<std::list<int> > clusters = util3d::clusterPolygons(
5270  neighbors,
5271  ui_->spinBox_mesh_minClusterSize->value());
5272  std::vector<pcl::Vertices> filteredPolygons(polygons.size());
5273  int oi=0;
5274  for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
5275  {
5276  for(std::list<int>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
5277  {
5278  filteredPolygons[oi++] = polygons.at(*jter);
5279  }
5280  }
5281  filteredPolygons.resize(oi);
5282  polygons = filteredPolygons;
5283  }
5284 
5285  cloudViewer_->addCloudMesh(uFormat("mesh_%d", i), cloud, polygons, pose);
5286  }
5287  if(ui_->checkBox_showCloud->isChecked())
5288  {
5289  cloudViewer_->addCloud(uFormat("cloud_%d", i), cloud, pose);
5290  }
5291  }
5292  }
5293  }
5294  else if(ui_->checkBox_showCloud->isChecked())
5295  {
5296  std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds;
5297  std::vector<pcl::IndicesPtr> allIndices;
5298 
5299  clouds = util3d::cloudsFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, &allIndices, ui_->parameters_toolbox->getParameters());
5300  UASSERT(clouds.size() == allIndices.size());
5301  for(size_t i=0; i<allIndices.size(); ++i)
5302  {
5303  if(allIndices[i]->size())
5304  {
5305  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = clouds[i];
5306  pcl::IndicesPtr indices = allIndices[i];
5307  if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
5308  {
5309  cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value());
5310  }
5311 
5312  cloudViewer_->addCloud(uFormat("cloud_%d", i), cloud, pose);
5313  }
5314  }
5315  }
5316  }
5317  }
5318 
5319  //frustums
5320  if(cloudViewer_->isFrustumShown())
5321  {
5322  if(data.cameraModels().size())
5323  {
5324  cloudViewer_->updateCameraFrustums(pose, data.cameraModels());
5325  }
5326  else
5327  {
5328  cloudViewer_->updateCameraFrustums(pose, data.stereoCameraModels());
5329  }
5330  }
5331 
5332  //words
5333  if(ui_->checkBox_showWords->isChecked() &&
5334  !signatures.empty() &&
5335  !(*signatures.begin())->getWords3().empty())
5336  {
5337  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
5338  cloud->resize((*signatures.begin())->getWords3().size());
5339  int i=0;
5340  for(std::multimap<int, int>::const_iterator iter=(*signatures.begin())->getWords().begin();
5341  iter!=(*signatures.begin())->getWords().end();
5342  ++iter)
5343  {
5344  const cv::Point3f & pt = (*signatures.begin())->getWords3()[iter->second];
5345  cloud->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
5346  }
5347 
5348  if(cloud->size())
5349  {
5350  cloud = rtabmap::util3d::removeNaNFromPointCloud(cloud);
5351  }
5352 
5353  if(cloud->size())
5354  {
5355  cloudViewer_->addCloud("words", cloud, pose, Qt::red);
5356  }
5357  }
5358 
5359  //add occupancy grid
5360  if(ui_->checkBox_showMap->isChecked() ||
5361  ui_->checkBox_showGrid->isChecked() ||
5362  ui_->checkBox_showElevation->checkState() != Qt::Unchecked)
5363  {
5364  LocalGridCache combinedLocalMaps;
5365  if(generatedLocalMaps_.shareTo(data.id(), combinedLocalMaps))
5366  {
5367  // add local grid
5368  }
5369  else if(!data.gridGroundCellsRaw().empty() || !data.gridObstacleCellsRaw().empty())
5370  {
5371  combinedLocalMaps.add(data.id(), data.gridGroundCellsRaw(), data.gridObstacleCellsRaw(), data.gridEmptyCellsRaw(), data.gridCellSize(), data.gridViewPoint());
5372  }
5373  if(!combinedLocalMaps.empty())
5374  {
5375  std::map<int, Transform> poses;
5376  poses.insert(std::make_pair(data.id(), pose));
5377 
5378 #ifdef RTABMAP_OCTOMAP
5379  OctoMap * octomap = 0;
5380  if(ui_->checkBox_octomap->isChecked() &&
5381  (!combinedLocalMaps.localGrids().begin()->second.groundCells.empty() || !combinedLocalMaps.localGrids().begin()->second.obstacleCells.empty()) &&
5382  combinedLocalMaps.localGrids().begin()->second.is3D() &&
5383  combinedLocalMaps.localGrids().begin()->second.cellSize > 0.0f)
5384  {
5385  //create local octomap
5386  ParametersMap params;
5387  params.insert(ParametersPair(Parameters::kGridCellSize(), uNumber2Str(combinedLocalMaps.localGrids().begin()->second.cellSize)));
5388  octomap = new OctoMap(&combinedLocalMaps, params);
5389  octomap->update(poses);
5390  }
5391 #endif
5392  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
5393  if(ui_->checkBox_showMap->isChecked())
5394  {
5395  float xMin=0.0f, yMin=0.0f;
5396  cv::Mat map8S;
5397  float gridCellSize = Parameters::defaultGridCellSize();
5398  Parameters::parse(parameters, Parameters::kGridCellSize(), gridCellSize);
5399  parameters = Parameters::filterParameters(parameters, "GridGlobal", true);
5400 
5401 #ifdef RTABMAP_OCTOMAP
5402  if(octomap)
5403  {
5404  map8S = octomap->createProjectionMap(xMin, yMin, gridCellSize, 0);
5405  }
5406  else
5407 #endif
5408  {
5409  OccupancyGrid grid(&combinedLocalMaps, parameters);
5410  grid.update(poses);
5411  map8S = grid.getMap(xMin, yMin);
5412  }
5413  if(!map8S.empty())
5414  {
5415  //convert to gray scaled map
5416  cloudViewer_->addOccupancyGridMap(util3d::convertMap2Image8U(map8S), gridCellSize, xMin, yMin, 1);
5417  }
5418  }
5419 
5420  if(ui_->checkBox_showGrid->isChecked())
5421  {
5422 #ifdef RTABMAP_OCTOMAP
5423  if(octomap)
5424  {
5425  if(ui_->comboBox_octomap_rendering_type->currentIndex()== 0)
5426  {
5427  pcl::IndicesPtr obstacles(new std::vector<int>);
5428  pcl::IndicesPtr empty(new std::vector<int>);
5429  pcl::IndicesPtr ground(new std::vector<int>);
5430  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(ui_->spinBox_grid_depth->value(), obstacles.get(), empty.get(), ground.get());
5431  if(octomap->hasColor())
5432  {
5433  pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
5434  pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
5435  cloudViewer_->addCloud("obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
5436  cloudViewer_->setCloudPointSize("obstacles", 5);
5437 
5438  pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
5439  pcl::copyPointCloud(*cloud, *ground, *groundCloud);
5440  cloudViewer_->addCloud("ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
5441  cloudViewer_->setCloudPointSize("ground", 5);
5442  }
5443  else
5444  {
5445  pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
5446  pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
5447  cloudViewer_->addCloud("obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
5448  cloudViewer_->setCloudPointSize("obstacles", 5);
5449 
5450  pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
5451  pcl::copyPointCloud(*cloud, *ground, *groundCloud);
5452  cloudViewer_->addCloud("ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
5453  cloudViewer_->setCloudPointSize("ground", 5);
5454  }
5455 
5456  if(ui_->checkBox_grid_empty->isChecked())
5457  {
5458  pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZ>);
5459  pcl::copyPointCloud(*cloud, *empty, *emptyCloud);
5460  cloudViewer_->addCloud("empty_cells", emptyCloud, Transform::getIdentity(), QColor(ui_->lineEdit_emptyColor->text()));
5461  cloudViewer_->setCloudOpacity("empty_cells", 0.5);
5462  cloudViewer_->setCloudPointSize("empty_cells", 5);
5463  }
5464  }
5465  else
5466  {
5467  cloudViewer_->addOctomap(octomap, ui_->spinBox_grid_depth->value(), ui_->comboBox_octomap_rendering_type->currentIndex()>1);
5468  }
5469  }
5470  else
5471 #endif
5472  {
5473  // occupancy cloud
5474  LaserScan scan = LaserScan::backwardCompatibility(combinedLocalMaps.localGrids().begin()->second.groundCells);
5475  if(scan.hasRGB())
5476  {
5477  cloudViewer_->addCloud("ground", util3d::laserScanToPointCloudRGB(scan), pose, QColor(ui_->lineEdit_groundColor->text()));
5478  }
5479  else
5480  {
5481  cloudViewer_->addCloud("ground", util3d::laserScanToPointCloud(scan), pose, QColor(ui_->lineEdit_groundColor->text()));
5482  }
5483  scan = LaserScan::backwardCompatibility(combinedLocalMaps.localGrids().begin()->second.obstacleCells);
5484  if(scan.hasRGB())
5485  {
5486  cloudViewer_->addCloud("obstacles", util3d::laserScanToPointCloudRGB(scan), pose, QColor(ui_->lineEdit_obstacleColor->text()));
5487  }
5488  else
5489  {
5490  cloudViewer_->addCloud("obstacles", util3d::laserScanToPointCloud(scan), pose, QColor(ui_->lineEdit_obstacleColor->text()));
5491  }
5492 
5493  cloudViewer_->setCloudPointSize("ground", 5);
5494  cloudViewer_->setCloudPointSize("obstacles", 5);
5495 
5496  if(ui_->checkBox_grid_empty->isChecked())
5497  {
5498  cloudViewer_->addCloud("empty_cells",
5499  util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(combinedLocalMaps.localGrids().begin()->second.emptyCells)),
5500  pose,
5501  QColor(ui_->lineEdit_emptyColor->text()));
5502  cloudViewer_->setCloudPointSize("empty_cells", 5);
5503  cloudViewer_->setCloudOpacity("empty_cells", 0.5);
5504  }
5505  }
5506  }
5507 #ifdef RTABMAP_OCTOMAP
5508  if(octomap)
5509  {
5510  delete octomap;
5511  }
5512 #endif
5513 
5514 #ifdef RTABMAP_GRIDMAP
5515  if(ui_->checkBox_showElevation->checkState() != Qt::Unchecked) // Show elevation map?
5516  {
5517  GridMap gridMap(&combinedLocalMaps, parameters);
5518 
5519  if(combinedLocalMaps.localGrids().begin()->second.is3D())
5520  {
5521  gridMap.update(poses);
5522  if(ui_->checkBox_showElevation->checkState() == Qt::PartiallyChecked)
5523  {
5524  float xMin, yMin, gridCellSize;
5525  cv::Mat elevationMap = gridMap.createHeightMap(xMin, yMin, gridCellSize);
5526  cloudViewer_->addElevationMap(elevationMap, gridCellSize, xMin, yMin, 1.0f);
5527  }
5528  else
5529  {
5530  pcl::PolygonMesh::Ptr mesh = gridMap.createTerrainMesh();
5531  cloudViewer_->addCloudMesh("elevation_mesh", mesh);
5532  }
5533  cloudViewer_->refreshView();
5534  }
5535  else
5536  {
5537  UWARN("Local grid is not 3D, cannot generate an elevation map");
5538  }
5539  }
5540 #endif
5541  }
5542  }
5543  cloudViewer_->updateCameraTargetPosition(pose);
5544  cloudViewer_->clearTrajectory();
5545  cloudViewer_->refreshView();
5546  }
5547 
5548  if(signatures.size())
5549  {
5550  UASSERT(signatures.front() != 0 && signatures.size() == 1);
5551  delete signatures.front();
5552  signatures.clear();
5553  }
5554  }
5555 
5556  // loops
5557  std::multimap<int, rtabmap::Link> links;
5558  dbDriver_->loadLinks(id, links);
5559  if(links.size())
5560  {
5561  QString strParents, strChildren;
5562  for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
5563  {
5564  if(iter->second.type() != Link::kNeighbor &&
5565  iter->second.type() != Link::kNeighborMerged)
5566  {
5567  if(iter->first < id)
5568  {
5569  strChildren.append(QString("%1 ").arg(iter->first));
5570  }
5571  else
5572  {
5573  strParents.append(QString("%1 ").arg(iter->first));
5574  }
5575  }
5576  }
5577  labelParents->setText(strParents);
5578  labelChildren->setText(strChildren);
5579  }
5580  }
5581 
5582  if(mapId>=0)
5583  {
5584  labelMapId->setText(QString::number(mapId));
5585  }
5586  }
5587  else if(value != 0)
5588  {
5589  ULOGGER_ERROR("Slider index out of range ?");
5590  }
5591 
5592  updateConstraintButtons();
5593  updateWordsMatching();
5594 
5595  if(updateConstraintView && ui_->dockWidget_constraints->isVisible())
5596  {
5597  // update constraint view
5598  int from = ids_.at(ui_->horizontalSlider_A->value());
5599  int to = ids_.at(ui_->horizontalSlider_B->value());
5600  bool set = false;
5601  for(int i=0; i<loopLinks_.size() || i<neighborLinks_.size(); ++i)
5602  {
5603  if(i < loopLinks_.size())
5604  {
5605  if((loopLinks_[i].from() == from && loopLinks_[i].to() == to) ||
5606  (loopLinks_[i].from() == to && loopLinks_[i].to() == from))
5607  {
5608  if(i != ui_->horizontalSlider_loops->value())
5609  {
5610  ui_->horizontalSlider_loops->blockSignals(true);
5611  ui_->horizontalSlider_loops->setValue(i);
5612  ui_->horizontalSlider_loops->blockSignals(false);
5613  this->updateConstraintView(loopLinks_[i].from() == from?loopLinks_.at(i):loopLinks_.at(i).inverse(), false);
5614  }
5615  ui_->horizontalSlider_neighbors->blockSignals(true);
5616  ui_->horizontalSlider_neighbors->setValue(0);
5617  ui_->horizontalSlider_neighbors->blockSignals(false);
5618  set = true;
5619  break;
5620  }
5621  }
5622  if(i < neighborLinks_.size())
5623  {
5624  if((neighborLinks_[i].from() == from && neighborLinks_[i].to() == to) ||
5625  (neighborLinks_[i].from() == to && neighborLinks_[i].to() == from))
5626  {
5627  if(i != ui_->horizontalSlider_neighbors->value())
5628  {
5629  ui_->horizontalSlider_neighbors->blockSignals(true);
5630  ui_->horizontalSlider_neighbors->setValue(i);
5631  ui_->horizontalSlider_neighbors->blockSignals(false);
5632  this->updateConstraintView(neighborLinks_[i].from() == from?neighborLinks_.at(i):neighborLinks_.at(i).inverse(), false);
5633  }
5634  ui_->horizontalSlider_loops->blockSignals(true);
5635  ui_->horizontalSlider_loops->setValue(0);
5636  ui_->horizontalSlider_loops->blockSignals(false);
5637  set = true;
5638  break;
5639  }
5640  }
5641  }
5642  if(!set)
5643  {
5644  ui_->horizontalSlider_loops->blockSignals(true);
5645  ui_->horizontalSlider_neighbors->blockSignals(true);
5646  ui_->horizontalSlider_loops->setValue(0);
5647  ui_->horizontalSlider_neighbors->setValue(0);
5648  ui_->horizontalSlider_loops->blockSignals(false);
5649  ui_->horizontalSlider_neighbors->blockSignals(false);
5650 
5651  constraintsViewer_->removeAllClouds();
5652 
5653  Link link = this->findActiveLink(from, to);
5654  bool constraintViewUpdated = false;
5655  if(link.isValid() && link.type() != Link::kGravity)
5656  {
5657  this->updateConstraintView(link, false);
5658  constraintViewUpdated = true;
5659  }
5660  else if(graphes_.size())
5661  {
5662  // make a fake link using globally optimized poses
5663  std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
5664  if(optimizedPoses.size() > 0)
5665  {
5666  std::map<int, Transform>::iterator fromIter = optimizedPoses.find(from);
5667  std::map<int, Transform>::iterator toIter = optimizedPoses.find(to);
5668  if(fromIter != optimizedPoses.end() &&
5669  toIter != optimizedPoses.end())
5670  {
5671  Link link(from, to, Link::kUndef, fromIter->second.inverse() * toIter->second);
5672  this->updateConstraintView(link, false);
5673  constraintViewUpdated = true;
5674  }
5675  }
5676  }
5677  if(!constraintViewUpdated)
5678  {
5679  ui_->label_constraint->clear();
5680  ui_->label_constraint_opt->clear();
5681  ui_->label_variance->clear();
5682  ui_->lineEdit_covariance->clear();
5683  ui_->label_type->clear();
5684  ui_->label_type_name->clear();
5685  ui_->checkBox_showOptimized->setEnabled(false);
5686  }
5687  constraintsViewer_->refreshView();
5688 
5689  }
5690  }
5691 }
5692 
5693 void DatabaseViewer::updateLoggerLevel()
5694 {
5695  if(this->parent() == 0)
5696  {
5697  ULogger::setLevel((ULogger::Level)ui_->comboBox_logger_level->currentIndex());
5698  }
5699 }
5700 
5701 void DatabaseViewer::updateStereo()
5702 {
5703  if(ui_->horizontalSlider_A->maximum())
5704  {
5705  int id = ids_.at(ui_->horizontalSlider_A->value());
5706  SensorData data;
5707  dbDriver_->getNodeData(id, data);
5708  data.uncompressData();
5709  updateStereo(&data);
5710  }
5711 }
5712 
5713 void DatabaseViewer::updateStereo(const SensorData * data)
5714 {
5715  if(data &&
5716  ui_->dockWidget_stereoView->isVisible() &&
5717  !data->imageRaw().empty() &&
5718  !data->depthOrRightRaw().empty() &&
5719  data->depthOrRightRaw().type() == CV_8UC1 &&
5720  data->stereoCameraModels().size()==1 && // Not implemented for multiple stereo cameras
5721  data->stereoCameraModels()[0].isValidForProjection())
5722  {
5723  cv::Mat leftMono;
5724  if(data->imageRaw().channels() == 3)
5725  {
5726  cv::cvtColor(data->imageRaw(), leftMono, CV_BGR2GRAY);
5727  }
5728  else
5729  {
5730  leftMono = data->imageRaw();
5731  }
5732 
5733  UTimer timer;
5734  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
5735  Stereo * stereo = Stereo::create(parameters);
5736 
5737  // generate kpts
5738  std::vector<cv::KeyPoint> kpts;
5739  uInsert(parameters, ParametersPair(Parameters::kKpMaxFeatures(), parameters.at(Parameters::kVisMaxFeatures())));
5740  uInsert(parameters, ParametersPair(Parameters::kKpSSC(), parameters.at(Parameters::kVisSSC())));
5741  uInsert(parameters, ParametersPair(Parameters::kKpMinDepth(), parameters.at(Parameters::kVisMinDepth())));
5742  uInsert(parameters, ParametersPair(Parameters::kKpMaxDepth(), parameters.at(Parameters::kVisMaxDepth())));
5743  uInsert(parameters, ParametersPair(Parameters::kKpDetectorStrategy(), parameters.at(Parameters::kVisFeatureType())));
5744  uInsert(parameters, ParametersPair(Parameters::kKpRoiRatios(), parameters.at(Parameters::kVisRoiRatios())));
5745  uInsert(parameters, ParametersPair(Parameters::kKpSubPixEps(), parameters.at(Parameters::kVisSubPixEps())));
5746  uInsert(parameters, ParametersPair(Parameters::kKpSubPixIterations(), parameters.at(Parameters::kVisSubPixIterations())));
5747  uInsert(parameters, ParametersPair(Parameters::kKpSubPixWinSize(), parameters.at(Parameters::kVisSubPixWinSize())));
5748  Feature2D * kptDetector = Feature2D::create(parameters);
5749  kpts = kptDetector->generateKeypoints(leftMono);
5750  delete kptDetector;
5751 
5752  float timeKpt = timer.ticks();
5753 
5754  std::vector<cv::Point2f> leftCorners;
5755  cv::KeyPoint::convert(kpts, leftCorners);
5756 
5757  // Find features in the new right image
5758  std::vector<unsigned char> status;
5759  std::vector<cv::Point2f> rightCorners;
5760 
5761  rightCorners = stereo->computeCorrespondences(
5762  leftMono,
5763  data->rightRaw(),
5764  leftCorners,
5765  status);
5766  delete stereo;
5767 
5768  float timeStereo = timer.ticks();
5769 
5770  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
5771  cloud->resize(kpts.size());
5772  float bad_point = std::numeric_limits<float>::quiet_NaN ();
5773  UASSERT(status.size() == kpts.size());
5774  int oi = 0;
5775  int inliers = 0;
5776  int flowOutliers= 0;
5777  int slopeOutliers= 0;
5778  int negativeDisparityOutliers = 0;
5779  for(unsigned int i=0; i<status.size(); ++i)
5780  {
5781  cv::Point3f pt(bad_point, bad_point, bad_point);
5782  if(status[i])
5783  {
5784  float disparity = leftCorners[i].x - rightCorners[i].x;
5785  if(disparity > 0.0f)
5786  {
5787  cv::Point3f tmpPt = util3d::projectDisparityTo3D(
5788  leftCorners[i],
5789  disparity,
5790  data->stereoCameraModels()[0]);
5791 
5792  if(util3d::isFinite(tmpPt))
5793  {
5794  pt = util3d::transformPoint(tmpPt, data->stereoCameraModels()[0].left().localTransform());
5795  status[i] = 100; //blue
5796  ++inliers;
5797  cloud->at(oi++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
5798  }
5799  }
5800  else
5801  {
5802  status[i] = 102; //magenta
5803  ++negativeDisparityOutliers;
5804  }
5805  }
5806  else
5807  {
5808  ++flowOutliers;
5809  }
5810  }
5811  cloud->resize(oi);
5812 
5813  UINFO("correspondences = %d/%d (%f) (time kpt=%fs stereo=%fs)",
5814  (int)cloud->size(), (int)leftCorners.size(), float(cloud->size())/float(leftCorners.size()), timeKpt, timeStereo);
5815 
5816  stereoViewer_->updateCameraTargetPosition(Transform::getIdentity());
5817  stereoViewer_->addCloud("stereo", cloud);
5818  stereoViewer_->refreshView();
5819 
5820  ui_->label_stereo_inliers->setNum(inliers);
5821  ui_->label_stereo_flowOutliers->setNum(flowOutliers);
5822  ui_->label_stereo_slopeOutliers->setNum(slopeOutliers);
5823  ui_->label_stereo_disparityOutliers->setNum(negativeDisparityOutliers);
5824 
5825  std::vector<cv::KeyPoint> rightKpts;
5826  cv::KeyPoint::convert(rightCorners, rightKpts);
5827  std::vector<cv::DMatch> good_matches(kpts.size());
5828  for(unsigned int i=0; i<good_matches.size(); ++i)
5829  {
5830  good_matches[i].trainIdx = i;
5831  good_matches[i].queryIdx = i;
5832  }
5833 
5834 
5835  //
5836  //cv::Mat imageMatches;
5837  //cv::drawMatches( leftMono, kpts, data->getDepthRaw(), rightKpts,
5838  // good_matches, imageMatches, cv::Scalar::all(-1), cv::Scalar::all(-1),
5839  // std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
5840 
5841  //ui_->graphicsView_stereo->setImage(uCvMat2QImage(imageMatches));
5842 
5843  ui_->graphicsView_stereo->clear();
5844  ui_->graphicsView_stereo->setLinesShown(true);
5845  ui_->graphicsView_stereo->setFeaturesShown(false);
5846  ui_->graphicsView_stereo->setImageDepthShown(true);
5847 
5848  ui_->graphicsView_stereo->setImage(uCvMat2QImage(data->imageRaw()));
5849  ui_->graphicsView_stereo->setImageDepth(data->depthOrRightRaw());
5850 
5851  // Draw lines between corresponding features...
5852  for(unsigned int i=0; i<kpts.size(); ++i)
5853  {
5854  if(rightKpts[i].pt.x > 0 && rightKpts[i].pt.y > 0)
5855  {
5856  QColor c = Qt::green;
5857  if(status[i] == 0)
5858  {
5859  c = Qt::red;
5860  }
5861  else if(status[i] == 100)
5862  {
5863  c = Qt::blue;
5864  }
5865  else if(status[i] == 101)
5866  {
5867  c = Qt::yellow;
5868  }
5869  else if(status[i] == 102)
5870  {
5871  c = Qt::magenta;
5872  }
5873  else if(status[i] == 110)
5874  {
5875  c = Qt::cyan;
5876  }
5877  ui_->graphicsView_stereo->addLine(
5878  kpts[i].pt.x,
5879  kpts[i].pt.y,
5880  rightKpts[i].pt.x,
5881  rightKpts[i].pt.y,
5882  c,
5883  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));
5884  }
5885  }
5886  ui_->graphicsView_stereo->update();
5887  }
5888 }
5889 
5890 void DatabaseViewer::updateWordsMatching(const std::vector<int> & inliers)
5891 {
5892  int from = ids_.at(ui_->horizontalSlider_A->value());
5893  int to = ids_.at(ui_->horizontalSlider_B->value());
5894  if(from && to)
5895  {
5896  ui_->graphicsView_A->clearLines();
5897  ui_->graphicsView_A->setFeaturesColor(ui_->graphicsView_A->getDefaultFeatureColor());
5898  ui_->graphicsView_B->clearLines();
5899  ui_->graphicsView_B->setFeaturesColor(ui_->graphicsView_B->getDefaultFeatureColor());
5900 
5901  const QMultiMap<int, KeypointItem*> & wordsA = ui_->graphicsView_A->getFeatures();
5902  const QMultiMap<int, KeypointItem*> & wordsB = ui_->graphicsView_B->getFeatures();
5903  std::set<int> inliersSet(inliers.begin(), inliers.end());
5904  if(wordsA.size() && wordsB.size())
5905  {
5906  QList<int> ids = wordsA.uniqueKeys();
5907  for(int i=0; i<ids.size(); ++i)
5908  {
5909  if(ids[i] > 0 && wordsA.count(ids[i]) == 1 && wordsB.count(ids[i]) == 1)
5910  {
5911  // Add lines
5912  // Draw lines between corresponding features...
5913  float scaleAX = ui_->graphicsView_A->viewScale();
5914  float scaleBX = ui_->graphicsView_B->viewScale();
5915 
5916  float marginAX = (ui_->graphicsView_A->width() - ui_->graphicsView_A->sceneRect().width()*scaleAX)/2.0f;
5917  float marginAY = (ui_->graphicsView_A->height() - ui_->graphicsView_A->sceneRect().height()*scaleAX)/2.0f;
5918  float marginBX = (ui_->graphicsView_B->width() - ui_->graphicsView_B->sceneRect().width()*scaleBX)/2.0f;
5919  float marginBY = (ui_->graphicsView_B->height() - ui_->graphicsView_B->sceneRect().height()*scaleBX)/2.0f;
5920 
5921  float deltaX = 0;
5922  float deltaY = 0;
5923 
5924  if(ui_->actionVertical_Layout->isChecked())
5925  {
5926  deltaY = ui_->graphicsView_A->height();
5927  }
5928  else
5929  {
5930  deltaX = ui_->graphicsView_A->width();
5931  }
5932 
5933  const KeypointItem * kptA = wordsA.value(ids[i]);
5934  const KeypointItem * kptB = wordsB.value(ids[i]);
5935 
5936  QColor cA = ui_->graphicsView_A->getDefaultMatchingLineColor();
5937  QColor cB = ui_->graphicsView_B->getDefaultMatchingLineColor();
5938  if(inliersSet.find(ids[i])!=inliersSet.end())
5939  {
5940  cA = ui_->graphicsView_A->getDefaultMatchingFeatureColor();
5941  cB = ui_->graphicsView_B->getDefaultMatchingFeatureColor();
5942  ui_->graphicsView_A->setFeatureColor(ids[i], ui_->graphicsView_A->getDefaultMatchingFeatureColor());
5943  ui_->graphicsView_B->setFeatureColor(ids[i], ui_->graphicsView_B->getDefaultMatchingFeatureColor());
5944  }
5945  else
5946  {
5947  ui_->graphicsView_A->setFeatureColor(ids[i], ui_->graphicsView_A->getDefaultMatchingLineColor());
5948  ui_->graphicsView_B->setFeatureColor(ids[i], ui_->graphicsView_B->getDefaultMatchingLineColor());
5949  }
5950 
5951  ui_->graphicsView_A->addLine(
5952  kptA->keypoint().pt.x,
5953  kptA->keypoint().pt.y,
5954  (kptB->keypoint().pt.x*scaleBX+marginBX+deltaX-marginAX)/scaleAX,
5955  (kptB->keypoint().pt.y*scaleBX+marginBY+deltaY-marginAY)/scaleAX,
5956  cA);
5957 
5958  ui_->graphicsView_B->addLine(
5959  (kptA->keypoint().pt.x*scaleAX+marginAX-deltaX-marginBX)/scaleBX,
5960  (kptA->keypoint().pt.y*scaleAX+marginAY-deltaY-marginBY)/scaleBX,
5961  kptB->keypoint().pt.x,
5962  kptB->keypoint().pt.y,
5963  cB);
5964  }
5965  }
5966  ui_->graphicsView_A->update();
5967  ui_->graphicsView_B->update();
5968  }
5969  }
5970 }
5971 
5972 void DatabaseViewer::sliderAMoved(int value)
5973 {
5974  ui_->label_indexA->setText(QString::number(value));
5975  if(value>=0 && value < ids_.size())
5976  {
5977  ui_->label_idA->setText(QString::number(ids_.at(value)));
5978  }
5979  else
5980  {
5981  ULOGGER_ERROR("Slider index out of range ?");
5982  }
5983 }
5984 
5985 void DatabaseViewer::sliderBMoved(int value)
5986 {
5987  ui_->label_indexB->setText(QString::number(value));
5988  if(value>=0 && value < ids_.size())
5989  {
5990  ui_->label_idB->setText(QString::number(ids_.at(value)));
5991  }
5992  else
5993  {
5994  ULOGGER_ERROR("Slider index out of range ?");
5995  }
5996 }
5997 
5998 void DatabaseViewer::update3dView()
5999 {
6000  if(lastSliderIndexBrowsed_ == ui_->horizontalSlider_B->value())
6001  {
6002  sliderBValueChanged(ui_->horizontalSlider_B->value());
6003  }
6004  else
6005  {
6006  sliderAValueChanged(ui_->horizontalSlider_A->value());
6007  }
6008 }
6009 
6010 void DatabaseViewer::sliderNeighborValueChanged(int value)
6011 {
6012  if(value < neighborLinks_.size())
6013  {
6014  this->updateConstraintView(neighborLinks_.at(value));
6015  }
6016 }
6017 
6018 void DatabaseViewer::sliderLoopValueChanged(int value)
6019 {
6020  if(value < loopLinks_.size())
6021  {
6022  this->updateConstraintView(loopLinks_.at(value));
6023  }
6024 }
6025 
6026 void DatabaseViewer::editConstraint()
6027 {
6028  if(ids_.size())
6029  {
6030  Link link(0,0,Link::kUndef, Transform::getIdentity());
6031  int priorId = sender() == ui_->toolButton_edit_priorA?ids_.at(ui_->horizontalSlider_A->value()):
6032  sender() == ui_->toolButton_edit_priorB?ids_.at(ui_->horizontalSlider_B->value()):0;
6033  if(priorId>0)
6034  {
6035  // Prior
6036  std::multimap<int, rtabmap::Link> links = updateLinksWithModifications(links_);
6037  if(graph::findLink(links, priorId, priorId, false, Link::kPosePrior) != links.end())
6038  {
6039  link = graph::findLink(links, priorId, priorId, false, Link::kPosePrior)->second;
6040  }
6041  else if(odomPoses_.find(priorId) != odomPoses_.end())
6042  {
6043  // fallback to odom pose
6044  // set undef to go in "add" branch below
6045  link = Link(priorId, priorId, Link::kUndef, odomPoses_.at(priorId));
6046  }
6047  else
6048  {
6049  QMessageBox::warning(this, tr(""), tr("Node %1 doesn't have odometry pose, cannot add a prior for it!").arg(priorId));
6050  return;
6051  }
6052  }
6053  else if(ui_->label_type->text().toInt() == Link::kLandmark)
6054  {
6055  link = loopLinks_.at(ui_->horizontalSlider_loops->value());
6056  }
6057  else
6058  {
6059  link = this->findActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value()));
6060  }
6061  bool updated = false;
6062  if(link.isValid())
6063  {
6064  cv::Mat covBefore = link.infMatrix().inv();
6065  EditConstraintDialog dialog(link.transform(),
6066  covBefore.at<double>(0,0)<9999.0?std::sqrt(covBefore.at<double>(0,0)):0.0,
6067  covBefore.at<double>(5,5)<9999.0?std::sqrt(covBefore.at<double>(5,5)):0.0);
6068  if(dialog.exec() == QDialog::Accepted)
6069  {
6070  cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1);
6071  if(dialog.getLinearVariance()>0)
6072  {
6073  covariance(cv::Range(0,3), cv::Range(0,3)) *= dialog.getLinearVariance();
6074  }
6075  else
6076  {
6077  covariance(cv::Range(0,3), cv::Range(0,3)) *= 9999.9;
6078  }
6079  if(dialog.getAngularVariance()>0)
6080  {
6081  covariance(cv::Range(3,6), cv::Range(3,6)) *= dialog.getAngularVariance();
6082  }
6083  else
6084  {
6085  covariance(cv::Range(3,6), cv::Range(3,6)) *= 9999.9;
6086  }
6087  Link newLink(link.from(), link.to(), link.type(), dialog.getTransform(), covariance.inv());
6088  std::multimap<int, Link>::iterator iter = linksRefined_.find(link.from());
6089  while(iter != linksRefined_.end() && iter->first == link.from())
6090  {
6091  if(iter->second.to() == link.to() &&
6092  iter->second.type() == link.type())
6093  {
6094  iter->second = newLink;
6095  updated = true;
6096  break;
6097  }
6098  ++iter;
6099  }
6100  if(!updated)
6101  {
6102  linksRefined_.insert(std::make_pair(newLink.from(), newLink));
6103  updated = true;
6104  }
6105  if(priorId==0)
6106  {
6107  this->updateGraphView();
6108  updateConstraintView();
6109  }
6110  }
6111  }
6112  else
6113  {
6114  EditConstraintDialog dialog(
6115  link.transform(),
6116  priorId>0?0.001:1,
6117  priorId>0?0.001:1);
6118  if(dialog.exec() == QDialog::Accepted)
6119  {
6120  cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1);
6121  if(dialog.getLinearVariance()>0)
6122  {
6123  covariance(cv::Range(0,3), cv::Range(0,3)) *= dialog.getLinearVariance();
6124  }
6125  else
6126  {
6127  covariance(cv::Range(0,3), cv::Range(0,3)) *= 9999.9;
6128  }
6129  if(dialog.getAngularVariance()>0)
6130  {
6131  covariance(cv::Range(3,6), cv::Range(3,6)) *= dialog.getAngularVariance();
6132  }
6133  else
6134  {
6135  covariance(cv::Range(3,6), cv::Range(3,6)) *= 9999.9;
6136  }
6137  int from = priorId>0?priorId:ids_.at(ui_->horizontalSlider_A->value());
6138  int to = priorId>0?priorId:ids_.at(ui_->horizontalSlider_B->value());
6139  Link newLink(
6140  from,
6141  to,
6142  priorId>0?Link::kPosePrior:Link::kUserClosure,
6143  dialog.getTransform(),
6144  covariance.inv());
6145  if(newLink.from() < newLink.to())
6146  {
6147  newLink = newLink.inverse();
6148  }
6149  linksAdded_.insert(std::make_pair(newLink.from(), newLink));
6150  updated = true;
6151  if(priorId==0)
6152  {
6153  this->updateGraphView();
6154  updateLoopClosuresSlider(from, to);
6155  }
6156  }
6157  }
6158 
6159  if(updated && priorId>0)
6160  {
6161  bool priorsIgnored = Parameters::defaultOptimizerPriorsIgnored();
6162  Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kOptimizerPriorsIgnored(), priorsIgnored);
6163  if(priorsIgnored)
6164  {
6165  if(QMessageBox::question(this,
6166  tr("Updating Prior"),
6167  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()),
6168  QMessageBox::Yes | QMessageBox::No,
6169  QMessageBox::Yes) == QMessageBox::Yes)
6170  {
6171  priorsIgnored = false;
6172  ui_->parameters_toolbox->updateParameter(Parameters::kOptimizerPriorsIgnored(), "false");
6173  }
6174  }
6175  int indexA = ui_->horizontalSlider_A->value();
6176  int indexB = ui_->horizontalSlider_B->value();
6177  if(!priorsIgnored)
6178  {
6179  this->updateGraphView();
6180  }
6181  if(ui_->horizontalSlider_A->value() != indexA)
6182  ui_->horizontalSlider_A->setValue(indexA);
6183  else
6184  sliderAValueChanged(indexA);
6185  if(ui_->horizontalSlider_B->value() != indexB)
6186  ui_->horizontalSlider_B->setValue(indexB);
6187  else
6188  sliderBValueChanged(indexB);
6189  }
6190  }
6191 }
6192 
6193 // only called when ui_->checkBox_showOptimized state changed
6194 void DatabaseViewer::updateConstraintView()
6195 {
6196  if(ids_.size())
6197  {
6198  Link link = this->findActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value()));
6199  if(link.isValid())
6200  {
6201  UDEBUG("link %d->%d type=%d", link.from(), link.to(), link.type());
6202  if((link.type() == Link::kNeighbor ||
6203  link.type() == Link::kNeighborMerged) &&
6204  ui_->horizontalSlider_neighbors->value() >= 0 && ui_->horizontalSlider_neighbors->value() < neighborLinks_.size())
6205  {
6206  this->updateConstraintView(neighborLinks_.at(ui_->horizontalSlider_neighbors->value()), false);
6207  }
6208  else if((link.type() != Link::kPosePrior || link.type() != Link::kGravity) &&
6209  ui_->horizontalSlider_loops->value() >= 0 && ui_->horizontalSlider_loops->value() < loopLinks_.size())
6210  {
6211  this->updateConstraintView(loopLinks_.at(ui_->horizontalSlider_loops->value()), false);
6212  }
6213  else
6214  {
6215  this->updateConstraintView(link, false);
6216  }
6217  }
6218  }
6219 }
6220 
6221 void DatabaseViewer::updateConstraintView(
6222  const rtabmap::Link & linkIn,
6223  bool updateImageSliders,
6224  const Signature & signatureFrom,
6225  const Signature & signatureTo)
6226 {
6227  UDEBUG("%d -> %d", linkIn.from(), linkIn.to());
6228  std::multimap<int, Link>::iterator iterLink = rtabmap::graph::findLink(linksRefined_, linkIn.from(), linkIn.to());
6229  rtabmap::Link link = linkIn;
6230 
6231  if(iterLink != linksRefined_.end())
6232  {
6233  if(iterLink->second.from() == link.to())
6234  {
6235  link = iterLink->second.inverse();
6236  }
6237  else
6238  {
6239  link = iterLink->second;
6240  }
6241  }
6242  else if(ui_->checkBox_ignorePoseCorrection->isChecked())
6243  {
6244  if(link.type() == Link::kNeighbor ||
6245  link.type() == Link::kNeighborMerged)
6246  {
6247  Transform poseFrom = uValue(odomPoses_, link.from(), Transform());
6248  Transform poseTo = uValue(odomPoses_, link.to(), Transform());
6249  if(!poseFrom.isNull() && !poseTo.isNull())
6250  {
6251  // recompute raw odom transformation and
6252  // reset to identity covariance
6253  link = Link(link.from(),
6254  link.to(),
6255  link.type(),
6256  poseFrom.inverse() * poseTo);
6257  }
6258  }
6259  }
6260  UDEBUG("%d -> %d", link.from(), link.to());
6261  rtabmap::Transform t = link.transform();
6262  if(link.type() == Link::kGravity)
6263  {
6264  // remove yaw, keep only roll and pitch
6265  float roll, pitch, yaw;
6266  t.getEulerAngles(roll, pitch, yaw);
6267  t = Transform(0,0,0,roll,pitch,0);
6268  }
6269 
6270  ui_->label_constraint->clear();
6271  ui_->label_constraint_opt->clear();
6272  ui_->checkBox_showOptimized->setEnabled(false);
6273  UASSERT(!t.isNull() && dbDriver_);
6274 
6275  ui_->label_type->setText(QString::number(link.type()));
6276  ui_->label_type_name->setText(tr("(%1)")
6277  .arg(link.type()==Link::kNeighbor?"Neighbor":
6278  link.type()==Link::kNeighborMerged?"Merged neighbor":
6279  link.type()==Link::kGlobalClosure?"Loop closure":
6280  link.type()==Link::kLocalSpaceClosure?"Space proximity link":
6281  link.type()==Link::kLocalTimeClosure?"Time proximity link":
6282  link.type()==Link::kUserClosure?"User link":
6283  link.type()==Link::kLandmark?"Landmark "+QString::number(-link.to()):
6284  link.type()==Link::kVirtualClosure?"Virtual link":
6285  link.type()==Link::kGravity?"Gravity link":"Undefined"));
6286  ui_->label_variance->setText(QString("%1, %2")
6287  .arg(sqrt(link.transVariance()))
6288  .arg(sqrt(link.rotVariance())));
6289  std::stringstream out;
6290  out << link.infMatrix().inv();
6291  ui_->lineEdit_covariance->setText(out.str().c_str());
6292  ui_->label_constraint->setText(QString("%1").arg(t.prettyPrint().c_str()).replace(" ", "\n"));
6293  if(graphes_.size() &&
6294  (int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
6295  {
6296  std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
6297 
6298  std::map<int, rtabmap::Transform>::iterator iterFrom = graph.find(link.from());
6299  std::map<int, rtabmap::Transform>::iterator iterTo = graph.find(link.to());
6300  if(iterFrom != graph.end() && iterTo != graph.end())
6301  {
6302  ui_->checkBox_showOptimized->setEnabled(true);
6303  Transform topt = iterFrom->second.inverse()*iterTo->second;
6304  float diff = topt.getDistance(t);
6305  Transform v1 = t.rotation()*Transform(1,0,0,0,0,0);
6306  Transform v2 = topt.rotation()*Transform(1,0,0,0,0,0);
6307  float a = pcl::getAngle3D(Eigen::Vector4f(v1.x(), v1.y(), v1.z(), 0), Eigen::Vector4f(v2.x(), v2.y(), v2.z(), 0));
6308  a = (a *180.0f) / CV_PI;
6309  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));
6310 
6311  if(ui_->checkBox_showOptimized->isChecked())
6312  {
6313  t = topt;
6314  }
6315  }
6316  }
6317 
6318  if(updateImageSliders)
6319  {
6320  ui_->horizontalSlider_A->blockSignals(true);
6321  ui_->horizontalSlider_B->blockSignals(true);
6322  // set from on left and to on right
6323  if(link.from()>0)
6324  ui_->horizontalSlider_A->setValue(idToIndex_.value(link.from()));
6325  if(link.to() > 0)
6326  ui_->horizontalSlider_B->setValue(idToIndex_.value(link.to()));
6327  ui_->horizontalSlider_A->blockSignals(false);
6328  ui_->horizontalSlider_B->blockSignals(false);
6329  if(link.from()>0)
6330  this->update(idToIndex_.value(link.from()),
6331  ui_->label_indexA,
6332  ui_->label_parentsA,
6333  ui_->label_childrenA,
6334  ui_->label_weightA,
6335  ui_->label_labelA,
6336  ui_->label_stampA,
6337  ui_->graphicsView_A,
6338  ui_->label_idA,
6339  ui_->label_mapA,
6340  ui_->label_poseA,
6341  ui_->label_optposeA,
6342  ui_->label_velA,
6343  ui_->label_calibA,
6344  ui_->label_scanA,
6345  ui_->label_gravityA,
6346  ui_->label_priorA,
6347  ui_->toolButton_edit_priorA,
6348  ui_->toolButton_remove_priorA,
6349  ui_->label_gpsA,
6350  ui_->label_gtA,
6351  ui_->label_sensorsA,
6352  false); // don't update constraints view!
6353  if(link.to()>0)
6354  {
6355  this->update(idToIndex_.value(link.to()),
6356  ui_->label_indexB,
6357  ui_->label_parentsB,
6358  ui_->label_childrenB,
6359  ui_->label_weightB,
6360  ui_->label_labelB,
6361  ui_->label_stampB,
6362  ui_->graphicsView_B,
6363  ui_->label_idB,
6364  ui_->label_mapB,
6365  ui_->label_poseB,
6366  ui_->label_optposeB,
6367  ui_->label_velB,
6368  ui_->label_calibB,
6369  ui_->label_scanB,
6370  ui_->label_gravityB,
6371  ui_->label_priorB,
6372  ui_->toolButton_edit_priorB,
6373  ui_->toolButton_remove_priorB,
6374  ui_->label_gpsB,
6375  ui_->label_gtB,
6376  ui_->label_sensorsB,
6377  false); // don't update constraints view!
6378  }
6379  }
6380 
6381  if(constraintsViewer_->isVisible())
6382  {
6383  SensorData dataFrom, dataTo;
6384 
6385  if(signatureFrom.id()>0)
6386  {
6387  dataFrom = signatureFrom.sensorData();
6388  }
6389  else
6390  {
6391  dbDriver_->getNodeData(link.from(), dataFrom);
6392  }
6393  dataFrom.uncompressData();
6394  UASSERT(dataFrom.imageRaw().empty() || dataFrom.imageRaw().type()==CV_8UC3 || dataFrom.imageRaw().type() == CV_8UC1);
6395  UASSERT(dataFrom.depthOrRightRaw().empty() || dataFrom.depthOrRightRaw().type()==CV_8UC1 || dataFrom.depthOrRightRaw().type() == CV_16UC1 || dataFrom.depthOrRightRaw().type() == CV_32FC1);
6396 
6397  if(signatureTo.id()>0)
6398  {
6399  dataTo = signatureTo.sensorData();
6400  }
6401  else if(link.to()>0)
6402  {
6403  dbDriver_->getNodeData(link.to(), dataTo);
6404  }
6405  dataTo.uncompressData();
6406  UASSERT(dataTo.imageRaw().empty() || dataTo.imageRaw().type()==CV_8UC3 || dataTo.imageRaw().type() == CV_8UC1);
6407  UASSERT(dataTo.depthOrRightRaw().empty() || dataTo.depthOrRightRaw().type()==CV_8UC1 || dataTo.depthOrRightRaw().type() == CV_16UC1 || dataTo.depthOrRightRaw().type() == CV_32FC1);
6408 
6409  // get odom pose
6410  Transform pose = Transform::getIdentity();
6411  if(ui_->checkBox_odomFrame->isChecked())
6412  {
6413  int m,w;
6414  std::string l;
6415  double s;
6416  Transform p,g;
6417  std::vector<float> v;
6418  GPS gps;
6419  EnvSensors sensors;
6420  dbDriver_->getNodeInfo(link.from(), p, m, w, l, s, g, v, gps, sensors);
6421  if(!p.isNull())
6422  {
6423  // keep just the z and roll/pitch rotation
6424  float x, y, z, roll, pitch, yaw;
6425  p.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
6426  pose = Transform(0,0,z,roll,pitch,0);
6427  }
6428  }
6429 
6430  constraintsViewer_->removeCloud("cloud0");
6431  constraintsViewer_->removeCloud("cloud1");
6432  //cloud 3d
6433  if(ui_->checkBox_show3Dclouds->isChecked())
6434  {
6435  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom, cloudTo;
6436  pcl::IndicesPtr indicesFrom(new std::vector<int>);
6437  pcl::IndicesPtr indicesTo(new std::vector<int>);
6438  if(!dataFrom.imageRaw().empty() && !dataFrom.depthOrRightRaw().empty())
6439  {
6440  cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, ui_->spinBox_decimation->value(), 0, 0, indicesFrom.get(), ui_->parameters_toolbox->getParameters());
6441  }
6442  if(!dataTo.imageRaw().empty() && !dataTo.depthOrRightRaw().empty())
6443  {
6444  cloudTo=util3d::cloudRGBFromSensorData(dataTo, ui_->spinBox_decimation->value(), 0, 0, indicesTo.get(), ui_->parameters_toolbox->getParameters());
6445  }
6446 
6447  if(cloudTo.get() && indicesTo->size())
6448  {
6449  cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t);
6450  }
6451 
6452  // Gain compensation
6453  if(ui_->doubleSpinBox_gainCompensationRadius->value()>0.0 &&
6454  cloudFrom.get() && indicesFrom->size() &&
6455  cloudTo.get() && indicesTo->size())
6456  {
6457  UTimer t;
6458  GainCompensator compensator(ui_->doubleSpinBox_gainCompensationRadius->value());
6459  compensator.feed(cloudFrom, indicesFrom, cloudTo, indicesTo, Transform::getIdentity());
6460  compensator.apply(0, cloudFrom, indicesFrom);
6461  compensator.apply(1, cloudTo, indicesTo);
6462  UINFO("Gain compensation time = %fs", t.ticks());
6463  }
6464 
6465  if(cloudFrom.get() && indicesFrom->size())
6466  {
6467  if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
6468  {
6469  cloudFrom = util3d::voxelize(cloudFrom, indicesFrom, ui_->doubleSpinBox_voxelSize->value());
6470  }
6471  constraintsViewer_->addCloud("cloud0", cloudFrom, pose, Qt::red);
6472  }
6473  if(cloudTo.get() && indicesTo->size())
6474  {
6475  if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
6476  {
6477  cloudTo = util3d::voxelize(cloudTo, indicesTo, ui_->doubleSpinBox_voxelSize->value());
6478  }
6479  constraintsViewer_->addCloud("cloud1", cloudTo, pose, Qt::cyan);
6480  }
6481  }
6482 
6483  constraintsViewer_->removeCloud("words0");
6484  constraintsViewer_->removeCloud("words1");
6485  if(ui_->checkBox_show3DWords->isChecked())
6486  {
6487  std::list<int> ids;
6488  ids.push_back(link.from());
6489  if(link.to()>0)
6490  {
6491  ids.push_back(link.to());
6492  }
6493  std::list<Signature*> signatures;
6494  dbDriver_->loadSignatures(ids, signatures);
6495  if(signatures.size() == 2 || (link.to()<0 && signatures.size()==1))
6496  {
6497  const Signature * sFrom = signatureFrom.id()>0?&signatureFrom:signatures.front();
6498  const Signature * sTo = 0;
6499  if(signatures.size()==2)
6500  {
6501  sTo = signatureTo.id()>0?&signatureTo:signatures.back();
6502  UASSERT(sTo);
6503  }
6504  UASSERT(sFrom);
6505  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom(new pcl::PointCloud<pcl::PointXYZ>);
6506  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo(new pcl::PointCloud<pcl::PointXYZ>);
6507  cloudFrom->resize(sFrom->getWords3().size());
6508  if(sTo)
6509  {
6510  cloudTo->resize(sTo->getWords3().size());
6511  }
6512  int i=0;
6513  if(!sFrom->getWords3().empty())
6514  {
6515  for(std::multimap<int, int>::const_iterator iter=sFrom->getWords().begin();
6516  iter!=sFrom->getWords().end();
6517  ++iter)
6518  {
6519  const cv::Point3f & pt = sFrom->getWords3()[iter->second];
6520  cloudFrom->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
6521  }
6522  }
6523  i=0;
6524  if(sTo && !sTo->getWords3().empty())
6525  {
6526  for(std::multimap<int, int>::const_iterator iter=sTo->getWords().begin();
6527  iter!=sTo->getWords().end();
6528  ++iter)
6529  {
6530  const cv::Point3f & pt = sTo->getWords3()[iter->second];
6531  cloudTo->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
6532  }
6533  }
6534 
6535  if(cloudFrom->size())
6536  {
6537  cloudFrom = rtabmap::util3d::removeNaNFromPointCloud(cloudFrom);
6538  }
6539  if(cloudTo->size())
6540  {
6541  cloudTo = rtabmap::util3d::removeNaNFromPointCloud(cloudTo);
6542  if(cloudTo->size())
6543  {
6544  cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t);
6545  }
6546  }
6547 
6548  if(cloudFrom->size())
6549  {
6550  constraintsViewer_->addCloud("words0", cloudFrom, pose, Qt::red);
6551  }
6552  else
6553  {
6554  UWARN("Empty 3D words for node %d", link.from());
6555  constraintsViewer_->removeCloud("words0");
6556  }
6557  if(cloudTo->size())
6558  {
6559  constraintsViewer_->addCloud("words1", cloudTo, pose, Qt::cyan);
6560  }
6561  else
6562  {
6563  if(sTo)
6564  {
6565  UWARN("Empty 3D words for node %d", link.to());
6566  }
6567  constraintsViewer_->removeCloud("words1");
6568  }
6569  }
6570  else
6571  {
6572  UERROR("Not found signature %d or %d in RAM", link.from(), link.to());
6573  constraintsViewer_->removeCloud("words0");
6574  constraintsViewer_->removeCloud("words1");
6575  }
6576  //cleanup
6577  for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
6578  {
6579  delete *iter;
6580  }
6581  }
6582 
6583  constraintsViewer_->removeCloud("scan2");
6584  constraintsViewer_->removeGraph("scan2graph");
6585  constraintsViewer_->removeCloud("scan0");
6586  constraintsViewer_->removeCloud("scan1");
6587  if(ui_->checkBox_show2DScans->isChecked())
6588  {
6589  //cloud 2d
6590  if(link.type() == Link::kLocalSpaceClosure &&
6591  !link.userDataCompressed().empty() &&
6592  signatureTo.id()==0)
6593  {
6594  std::vector<int> ids;
6595  cv::Mat userData = link.uncompressUserDataConst();
6596  if(userData.type() == CV_8SC1 &&
6597  userData.rows == 1 &&
6598  userData.cols >= 8 && // including null str ending
6599  userData.at<char>(userData.cols-1) == 0 &&
6600  memcmp(userData.data, "SCANS:", 6) == 0)
6601  {
6602  std::string scansStr = (const char *)userData.data;
6603  UINFO("Detected \"%s\" in links's user data", scansStr.c_str());
6604  if(!scansStr.empty())
6605  {
6606  std::list<std::string> strs = uSplit(scansStr, ':');
6607  if(strs.size() == 2)
6608  {
6609  std::list<std::string> strIds = uSplit(strs.rbegin()->c_str(), ';');
6610  for(std::list<std::string>::iterator iter=strIds.begin(); iter!=strIds.end(); ++iter)
6611  {
6612  ids.push_back(atoi(iter->c_str()));
6613  if(ids.back() == link.from())
6614  {
6615  ids.pop_back();
6616  }
6617  }
6618  }
6619  }
6620  }
6621  if(ids.size())
6622  {
6623  //add other scans matching
6624  //optimize the path's poses locally
6625 
6626  std::map<int, rtabmap::Transform> poses;
6627  for(unsigned int i=0; i<ids.size(); ++i)
6628  {
6629  if(uContains(odomPoses_, ids[i]))
6630  {
6631  poses.insert(*odomPoses_.find(ids[i]));
6632  }
6633  else
6634  {
6635  UERROR("Not found %d node!", ids[i]);
6636  }
6637  }
6638  if(poses.size())
6639  {
6640  Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
6641 
6642  UASSERT(uContains(poses, link.to()));
6643  std::map<int, rtabmap::Transform> posesOut;
6644  std::multimap<int, rtabmap::Link> linksOut;
6645  optimizer->getConnectedGraph(
6646  link.to(),
6647  poses,
6648  updateLinksWithModifications(links_),
6649  posesOut,
6650  linksOut);
6651 
6652  if(poses.size() != posesOut.size())
6653  {
6654  UWARN("Scan poses input and output are different! %d vs %d", (int)poses.size(), (int)posesOut.size());
6655  }
6656 
6657  UDEBUG("Input poses: ");
6658  for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
6659  {
6660  UDEBUG(" %d=%s", iter->first, iter->second.prettyPrint().c_str());
6661  }
6662  UDEBUG("Input links: ");
6663  for(std::multimap<int, Link>::iterator iter=linksOut.begin(); iter!=linksOut.end(); ++iter)
6664  {
6665  UDEBUG(" %d->%d (type=%s) %s", iter->second.from(), iter->second.to(), iter->second.typeName().c_str(), iter->second.transform().prettyPrint().c_str());
6666  }
6667 
6668  std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(link.to(), posesOut, linksOut);
6669  delete optimizer;
6670 
6671  UDEBUG("Output poses: ");
6672  for(std::map<int, Transform>::iterator iter=finalPoses.begin(); iter!=finalPoses.end(); ++iter)
6673  {
6674  UDEBUG(" %d=%s", iter->first, iter->second.prettyPrint().c_str());
6675  }
6676 
6677  // transform local poses in loop referential
6678  Transform u = t * finalPoses.at(link.to()).inverse();
6679  pcl::PointCloud<pcl::PointXYZ>::Ptr assembledScans(new pcl::PointCloud<pcl::PointXYZ>);
6680  pcl::PointCloud<pcl::PointNormal>::Ptr assembledNormalScans(new pcl::PointCloud<pcl::PointNormal>);
6681  pcl::PointCloud<pcl::PointXYZI>::Ptr assembledIScans(new pcl::PointCloud<pcl::PointXYZI>);
6682  pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembledINormalScans(new pcl::PointCloud<pcl::PointXYZINormal>);
6683  pcl::PointCloud<pcl::PointXYZ>::Ptr graph(new pcl::PointCloud<pcl::PointXYZ>);
6684  for(std::map<int, Transform>::iterator iter=finalPoses.begin(); iter!=finalPoses.end(); ++iter)
6685  {
6686  iter->second = u * iter->second;
6687  if(iter->first != link.to()) // already added to view
6688  {
6689  //create scan
6690  SensorData data;
6691  dbDriver_->getNodeData(iter->first, data);
6692  LaserScan scan;
6693  data.uncompressDataConst(0, 0, &scan, 0);
6694  if(!scan.isEmpty())
6695  {
6696  if(scan.hasNormals() && scan.hasIntensity())
6697  {
6698  *assembledINormalScans += *util3d::laserScanToPointCloudINormal(scan, iter->second*scan.localTransform());
6699  }
6700  else if(scan.hasNormals())
6701  {
6702  *assembledNormalScans += *util3d::laserScanToPointCloudNormal(scan, iter->second*scan.localTransform());
6703  }
6704  else if(scan.hasIntensity())
6705  {
6706  *assembledIScans += *util3d::laserScanToPointCloudI(scan, iter->second*scan.localTransform());
6707  }
6708  else
6709  {
6710  *assembledScans += *util3d::laserScanToPointCloud(scan, iter->second*scan.localTransform());
6711  }
6712  }
6713  }
6714  graph->push_back(util3d::transformPoint(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()), pose));
6715  }
6716 
6717  if(assembledNormalScans->size())
6718  {
6719  constraintsViewer_->addCloud("scan2", assembledNormalScans, pose, Qt::cyan);
6720  constraintsViewer_->setCloudColorIndex("scan2", 2);
6721  }
6722  if(assembledScans->size())
6723  {
6724  constraintsViewer_->addCloud("scan2", assembledScans, pose, Qt::cyan);
6725  constraintsViewer_->setCloudColorIndex("scan2", 2);
6726  }
6727  if(assembledINormalScans->size())
6728  {
6729  constraintsViewer_->addCloud("scan2", assembledINormalScans, pose, Qt::cyan);
6730  constraintsViewer_->setCloudColorIndex("scan2", 2);
6731  }
6732  if(assembledIScans->size())
6733  {
6734  constraintsViewer_->addCloud("scan2", assembledIScans, pose, Qt::cyan);
6735  constraintsViewer_->setCloudColorIndex("scan2", 2);
6736  }
6737  if(graph->size())
6738  {
6739  constraintsViewer_->addOrUpdateGraph("scan2graph", graph, Qt::cyan);
6740  }
6741  }
6742  }
6743  }
6744 
6745  // Added loop closure scans
6746  constraintsViewer_->removeCloud("scan0");
6747  constraintsViewer_->removeCloud("scan1");
6748  if(!dataFrom.laserScanRaw().isEmpty())
6749  {
6750  if(dataFrom.laserScanRaw().hasNormals() && dataFrom.laserScanRaw().hasIntensity())
6751  {
6752  pcl::PointCloud<pcl::PointXYZINormal>::Ptr scan;
6753  scan = rtabmap::util3d::laserScanToPointCloudINormal(dataFrom.laserScanRaw(), dataFrom.laserScanRaw().localTransform());
6754  constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
6755  constraintsViewer_->setCloudColorIndex("scan0", 2);
6756  }
6757  else if(dataFrom.laserScanRaw().hasNormals())
6758  {
6759  pcl::PointCloud<pcl::PointNormal>::Ptr scan;
6760  scan = rtabmap::util3d::laserScanToPointCloudNormal(dataFrom.laserScanRaw(), dataFrom.laserScanRaw().localTransform());
6761  constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
6762  constraintsViewer_->setCloudColorIndex("scan0", 2);
6763  }
6764  else if(dataFrom.laserScanRaw().hasIntensity())
6765  {
6766  pcl::PointCloud<pcl::PointXYZI>::Ptr scan;
6767  scan = rtabmap::util3d::laserScanToPointCloudI(dataFrom.laserScanRaw(), dataFrom.laserScanRaw().localTransform());
6768  constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
6769  constraintsViewer_->setCloudColorIndex("scan0", 2);
6770  }
6771  else
6772  {
6773  pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
6774  scan = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw(), dataFrom.laserScanRaw().localTransform());
6775  constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
6776  constraintsViewer_->setCloudColorIndex("scan0", 2);
6777  }
6778  }
6779  if(!dataTo.laserScanRaw().isEmpty())
6780  {
6781  if(dataTo.laserScanRaw().hasNormals() && dataTo.laserScanRaw().hasIntensity())
6782  {
6783  pcl::PointCloud<pcl::PointXYZINormal>::Ptr scan;
6784  scan = rtabmap::util3d::laserScanToPointCloudINormal(dataTo.laserScanRaw(), t*dataTo.laserScanRaw().localTransform());
6785  constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
6786  constraintsViewer_->setCloudColorIndex("scan1", 2);
6787  }
6788  else if(dataTo.laserScanRaw().hasNormals())
6789  {
6790  pcl::PointCloud<pcl::PointNormal>::Ptr scan;
6791  scan = rtabmap::util3d::laserScanToPointCloudNormal(dataTo.laserScanRaw(), t*dataTo.laserScanRaw().localTransform());
6792  constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
6793  constraintsViewer_->setCloudColorIndex("scan1", 2);
6794  }
6795  else if(dataTo.laserScanRaw().hasIntensity())
6796  {
6797  pcl::PointCloud<pcl::PointXYZI>::Ptr scan;
6798  scan = rtabmap::util3d::laserScanToPointCloudI(dataTo.laserScanRaw(), t*dataTo.laserScanRaw().localTransform());
6799  constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
6800  constraintsViewer_->setCloudColorIndex("scan1", 2);
6801  }
6802  else
6803  {
6804  pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
6805  scan = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw(), t*dataTo.laserScanRaw().localTransform());
6806  constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
6807  constraintsViewer_->setCloudColorIndex("scan1", 2);
6808  }
6809  }
6810  }
6811 
6812  //frustums
6813  constraintsViewer_->removeAllFrustums();
6814  if(constraintsViewer_->isFrustumShown())
6815  {
6816  CameraModel model;
6817  if(dataFrom.cameraModels().size())
6818  {
6819  model = dataFrom.cameraModels()[0];
6820  }
6821  else if(dataFrom.stereoCameraModels().size())
6822  {
6823  model = dataFrom.stereoCameraModels()[0].left();
6824  }
6825  constraintsViewer_->addOrUpdateFrustum("frustum_from", pose, model.localTransform(), constraintsViewer_->getFrustumScale(), constraintsViewer_->getFrustumColor(), model.fovX(), model.fovY());
6826  model = CameraModel();
6827  if(dataTo.cameraModels().size())
6828  {
6829  model = dataTo.cameraModels()[0];
6830  }
6831  else if(dataTo.stereoCameraModels().size())
6832  {
6833  model = dataTo.stereoCameraModels()[0].left();
6834  }
6835  constraintsViewer_->addOrUpdateFrustum("frustum_to", pose*t, model.localTransform(), constraintsViewer_->getFrustumScale(), constraintsViewer_->getFrustumColor(), model.fovX(), model.fovY());
6836  }
6837 
6838  //update coordinate
6839  constraintsViewer_->addOrUpdateCoordinate("from_coordinate", pose, 0.2);
6840 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
6841  constraintsViewer_->addOrUpdateCoordinate("to_coordinate", pose*t, 0.2);
6842  constraintsViewer_->removeCoordinate("to_coordinate_gt");
6843  if(uContains(groundTruthPoses_, link.from()) && uContains(groundTruthPoses_, link.to()))
6844  {
6845  constraintsViewer_->addOrUpdateCoordinate("to_coordinate_gt",
6846  pose*(groundTruthPoses_.at(link.from()).inverse()*groundTruthPoses_.at(link.to())), 0.1);
6847  }
6848 #endif
6849 
6850  constraintsViewer_->clearTrajectory();
6851 
6852  constraintsViewer_->refreshView();
6853  }
6854 
6855  // update buttons
6856  updateConstraintButtons();
6857 }
6858 
6859 void DatabaseViewer::updateConstraintButtons()
6860 {
6861  ui_->pushButton_refine->setEnabled(false);
6862  ui_->pushButton_reset->setEnabled(false);
6863  ui_->pushButton_add->setEnabled(false);
6864  ui_->pushButton_reject->setEnabled(false);
6865  ui_->toolButton_constraint->setEnabled(false);
6866 
6867  Link currentLink;
6868  int from;
6869  int to;
6870  if(ui_->label_type->text().toInt() == Link::kLandmark)
6871  {
6872  //check for modified link
6873  currentLink = loopLinks_.at(ui_->horizontalSlider_loops->value());
6874  from = currentLink.from();
6875  to = currentLink.to();
6876  }
6877  else
6878  {
6879  from = ids_.at(ui_->horizontalSlider_A->value());
6880  to = ids_.at(ui_->horizontalSlider_B->value());
6881  if(from!=to && from && to &&
6882  odomPoses_.find(from) != odomPoses_.end() &&
6883  odomPoses_.find(to) != odomPoses_.end() &&
6884  (ui_->checkBox_enableForAll->isChecked() ||
6885  (weights_.find(from) != weights_.end() && weights_.at(from)>=0 &&
6886  weights_.find(to) != weights_.end() && weights_.at(to)>=0)))
6887  {
6888  if((!containsLink(links_, from ,to) && !containsLink(linksAdded_, from ,to)) ||
6889  containsLink(linksRemoved_, from ,to))
6890  {
6891  ui_->pushButton_add->setEnabled(true);
6892  }
6893  }
6894  else if(ui_->checkBox_enableForAll->isChecked())
6895  {
6896  if(odomPoses_.find(from) == odomPoses_.end())
6897  {
6898  UWARN("Button \"Add\" cannot be enabled even if \"all\" checkbox is checked, as node %d doesn't have odometry set.", from);
6899  }
6900  else if(odomPoses_.find(to) == odomPoses_.end())
6901  {
6902  UWARN("Button \"Add\" cannot be enabled even if \"all\" checkbox is checked, as node %d doesn't have odometry set.", to);
6903  }
6904  }
6905 
6906  currentLink = findActiveLink(from ,to);
6907  }
6908 
6909  if(currentLink.isValid() &&
6910  ((currentLink.from() == from && currentLink.to() == to) || (currentLink.from() == to && currentLink.to() == from)))
6911  {
6912  if(!containsLink(linksRemoved_, from ,to))
6913  {
6914  ui_->pushButton_reject->setEnabled(true);
6915  }
6916 
6917  //check for modified link
6918  std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, currentLink.from(), currentLink.to());
6919  if(iter != linksRefined_.end())
6920  {
6921  currentLink = iter->second;
6922  ui_->pushButton_reset->setEnabled(true);
6923  }
6924  ui_->pushButton_refine->setEnabled(currentLink.from()!=currentLink.to() && currentLink.type() != Link::kLandmark);
6925  ui_->toolButton_constraint->setEnabled(true);
6926  }
6927 }
6928 
6929 void DatabaseViewer::sliderIterationsValueChanged(int value)
6930 {
6931  if(dbDriver_ && value >=0 && value < (int)graphes_.size())
6932  {
6933  std::map<int, rtabmap::Transform> graph = uValueAt(graphes_, value);
6934 
6935  std::map<int, Transform> refPoses = groundTruthPoses_;
6936  if(ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked())
6937  {
6938  refPoses = gpsPoses_;
6939  }
6940 
6941  // Log ground truth statistics (in TUM's RGBD-SLAM format)
6942  if(refPoses.size())
6943  {
6944  // compute KITTI statistics before aligning the poses
6945  float length = graph::computePathLength(graph);
6946  if(refPoses.size() == graph.size() && length >= 100.0f)
6947  {
6948  float t_err = 0.0f;
6949  float r_err = 0.0f;
6950  graph::calcKittiSequenceErrors(uValues(refPoses), uValues(graph), t_err, r_err);
6951  UINFO("KITTI t_err = %f %%", t_err);
6952  UINFO("KITTI r_err = %f deg/m", r_err);
6953  }
6954 
6955  float translational_rmse = 0.0f;
6956  float translational_mean = 0.0f;
6957  float translational_median = 0.0f;
6958  float translational_std = 0.0f;
6959  float translational_min = 0.0f;
6960  float translational_max = 0.0f;
6961  float rotational_rmse = 0.0f;
6962  float rotational_mean = 0.0f;
6963  float rotational_median = 0.0f;
6964  float rotational_std = 0.0f;
6965  float rotational_min = 0.0f;
6966  float rotational_max = 0.0f;
6967 
6968  Transform gtToMap = graph::calcRMSE(
6969  refPoses,
6970  graph,
6971  translational_rmse,
6972  translational_mean,
6973  translational_median,
6974  translational_std,
6975  translational_min,
6976  translational_max,
6977  rotational_rmse,
6978  rotational_mean,
6979  rotational_median,
6980  rotational_std,
6981  rotational_min,
6982  rotational_max);
6983 
6984  // ground truth live statistics
6985  ui_->label_rmse->setNum(translational_rmse);
6986  UINFO("translational_rmse=%f", translational_rmse);
6987  UINFO("translational_mean=%f", translational_mean);
6988  UINFO("translational_median=%f", translational_median);
6989  UINFO("translational_std=%f", translational_std);
6990  UINFO("translational_min=%f", translational_min);
6991  UINFO("translational_max=%f", translational_max);
6992 
6993  UINFO("rotational_rmse=%f", rotational_rmse);
6994  UINFO("rotational_mean=%f", rotational_mean);
6995  UINFO("rotational_median=%f", rotational_median);
6996  UINFO("rotational_std=%f", rotational_std);
6997  UINFO("rotational_min=%f", rotational_min);
6998  UINFO("rotational_max=%f", rotational_max);
6999 
7000  if(((ui_->checkBox_alignPosesWithGPS->isEnabled() && ui_->checkBox_alignPosesWithGPS->isChecked()) ||
7001  (ui_->checkBox_alignPosesWithGroundTruth->isEnabled() && ui_->checkBox_alignPosesWithGroundTruth->isChecked())) &&
7002  !gtToMap.isIdentity())
7003  {
7004  for(std::map<int, Transform>::iterator iter=graph.begin(); iter!=graph.end(); ++iter)
7005  {
7006  iter->second = gtToMap * iter->second;
7007  }
7008  }
7009  }
7010 
7011  std::map<int, rtabmap::Transform> graphFiltered;
7012  if(ui_->checkBox_alignScansCloudsWithGroundTruth->isEnabled() &&
7013  ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() &&
7014  !groundTruthPoses_.empty())
7015  {
7016  graphFiltered = groundTruthPoses_;
7017  }
7018  else
7019  {
7020  graphFiltered = graph;
7021  }
7022  if(ui_->groupBox_posefiltering->isChecked())
7023  {
7024  graphFiltered = graph::radiusPosesFiltering(graph,
7025  ui_->doubleSpinBox_posefilteringRadius->value(),
7026  ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
7027  }
7028  LocalGridCache combinedLocalMaps;
7029 #ifdef RTABMAP_OCTOMAP
7030  delete octomap_;
7031  octomap_ = 0;
7032 #endif
7033  if(ui_->dockWidget_graphView->isVisible() || ui_->dockWidget_occupancyGridView->isVisible())
7034  {
7035  //update scans
7036  UINFO("Update local maps list...");
7037  std::vector<int> ids = uKeys(graphFiltered);
7038  for(unsigned int i=0; i<ids.size(); ++i)
7039  {
7040  if(generatedLocalMaps_.shareTo(ids[i], combinedLocalMaps) ||
7041  localMaps_.shareTo(ids[i], combinedLocalMaps))
7042  {
7043  // Added to combined maps
7044  }
7045  else if(ids.at(i)>0)
7046  {
7047  SensorData data;
7048  dbDriver_->getNodeData(ids.at(i), data, false, false, false);
7049  cv::Mat ground, obstacles, empty;
7050  if(data.gridCellSize()>0.0f)
7051  {
7052  data.uncompressData(0, 0, 0, 0, &ground, &obstacles, &empty);
7053  }
7054  localMaps_.add(ids.at(i), ground, obstacles, empty, data.gridCellSize()>0.0f?data.gridCellSize():Parameters::defaultGridCellSize(), data.gridViewPoint());
7055  if(!ground.empty() || !obstacles.empty())
7056  {
7057  localMaps_.shareTo(ids.at(i), combinedLocalMaps);
7058  }
7059  }
7060  }
7061  UINFO("Update local maps list... done (%d local maps, graph size=%d)", (int)combinedLocalMaps.size(), (int)graph.size());
7062  }
7063 
7064  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
7065  float cellSize = Parameters::defaultGridCellSize();
7066  Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize);
7067 
7068  ui_->graphViewer->updateGTGraph(groundTruthPoses_);
7069  ui_->graphViewer->updateGPSGraph(gpsPoses_, gpsValues_);
7070  ui_->graphViewer->updateGraph(graph, graphLinks_, mapIds_, weights_);
7071  if(!ui_->checkBox_wmState->isChecked())
7072  {
7073  bool allNodesAreInWM = true;
7074  std::map<int, float> colors;
7075  for(std::map<int, rtabmap::Transform>::iterator iter=graph.begin(); iter!=graph.end(); ++iter)
7076  {
7077  if(lastWmIds_.find(iter->first) != lastWmIds_.end())
7078  {
7079  colors.insert(std::make_pair(iter->first, 1));
7080  }
7081  else
7082  {
7083  allNodesAreInWM = false;
7084  }
7085  }
7086  if(!allNodesAreInWM)
7087  {
7088  ui_->graphViewer->updatePosterior(colors, 1, 1);
7089  }
7090  }
7091  QGraphicsRectItem * rectScaleItem = 0;
7092  ui_->graphViewer->clearMap();
7093  occupancyGridViewer_->clear();
7094  if(graph.size() && combinedLocalMaps.size() &&
7095  (ui_->graphViewer->isGridMapVisible() || ui_->dockWidget_occupancyGridView->isVisible()))
7096  {
7097  QElapsedTimer time;
7098  time.start();
7099 
7100 #ifdef RTABMAP_OCTOMAP
7101  if(ui_->checkBox_octomap->isChecked())
7102  {
7103  octomap_ = new OctoMap(&combinedLocalMaps, parameters);
7104  octomap_->update(graphFiltered);
7105  }
7106 #endif
7107  // Generate 2d grid map?
7108  if((ui_->dockWidget_graphView->isVisible() && ui_->graphViewer->isGridMapVisible()) ||
7109  (ui_->dockWidget_occupancyGridView->isVisible() && ui_->checkBox_grid_2d->isChecked()))
7110  {
7111  bool eroded = Parameters::defaultGridGlobalEroded();
7112  Parameters::parse(parameters, Parameters::kGridGlobalEroded(), eroded);
7113  float xMin, yMin;
7114  cv::Mat map;
7115 
7116 #ifdef RTABMAP_OCTOMAP
7117  if(ui_->checkBox_octomap->isChecked())
7118  {
7119  map = octomap_->createProjectionMap(xMin, yMin, cellSize, 0, ui_->spinBox_grid_depth->value());
7120  }
7121  else
7122 #endif
7123  {
7124  if(eroded)
7125  {
7126  uInsert(parameters, ParametersPair(Parameters::kGridGlobalEroded(), "true"));
7127  }
7128  OccupancyGrid grid(&combinedLocalMaps, parameters);
7129  grid.update(graphFiltered);
7130  if(ui_->checkBox_grid_showProbMap->isChecked())
7131  {
7132  map = grid.getProbMap(xMin, yMin);
7133  }
7134  else
7135  {
7136  map = grid.getMap(xMin, yMin);
7137  }
7138  }
7139 
7140  ui_->label_timeGrid->setNum(double(time.elapsed())/1000.0);
7141 
7142  if(!map.empty())
7143  {
7144  cv::Mat map8U = rtabmap::util3d::convertMap2Image8U(map);
7145  if(ui_->dockWidget_graphView->isVisible() && ui_->graphViewer->isGridMapVisible())
7146  {
7147  ui_->graphViewer->updateMap(map8U, cellSize, xMin, yMin);
7148  }
7149  if(ui_->dockWidget_occupancyGridView->isVisible() && ui_->checkBox_grid_2d->isChecked())
7150  {
7151  occupancyGridViewer_->addOccupancyGridMap(map8U, cellSize, xMin, yMin, 1.0f);
7152  occupancyGridViewer_->refreshView();
7153  }
7154 
7155  // Zoom to ignore unknowns
7156  int xFirst = 0;
7157  int yFirst = 0;
7158  int xLast = map.cols;
7159  int yLast = map.rows;
7160  bool firstSet = false;
7161  bool lastSet = false;
7162  for(int x=0; x<map.cols && (!firstSet || !lastSet); ++x)
7163  {
7164  for(int y=0; y<map.rows; ++y)
7165  {
7166  // check for first
7167  if(!firstSet && map.at<char>(y, x) != -1)
7168  {
7169  xFirst = x;
7170  firstSet = true;
7171  }
7172  // check for last
7173  int opp = map.cols-(x+1);
7174  if(!lastSet && map.at<char>(y, opp) != -1)
7175  {
7176  xLast = opp;
7177  lastSet = true;
7178  }
7179  }
7180  }
7181  firstSet = false;
7182  lastSet = false;
7183  for(int y=0; y<map.rows && (!firstSet || !lastSet); ++y)
7184  {
7185  for(int x=0; x<map.cols; ++x)
7186  {
7187  // check for first
7188  if(!firstSet && map.at<char>(y, x) != -1)
7189  {
7190  yFirst = y;
7191  firstSet = true;
7192  }
7193  // check for last
7194  int opp = map.rows-(y+1);
7195  if(!lastSet && map.at<char>(map.rows-(y+1), x) != -1)
7196  {
7197  yLast = opp;
7198  lastSet = true;
7199  }
7200  }
7201  }
7202  // Only zoom if there are significant unknowns
7203  if( (xLast > xFirst && yLast > yFirst) &&
7204  (xFirst > 50 ||
7205  xLast < map.cols-50 ||
7206  yFirst > 50 ||
7207  yLast < map.rows-50))
7208  {
7209  rectScaleItem = ui_->graphViewer->scene()->addRect(
7210  xFirst-25,
7211  yFirst-25,
7212  xLast-xFirst+50,
7213  yLast-yFirst+50);
7214  rectScaleItem->setTransform(QTransform::fromScale(cellSize*100.0f, -cellSize*100.0f), true);
7215  rectScaleItem->setRotation(90);
7216  rectScaleItem->setPos(-yMin*100.0f, -xMin*100.0f);
7217  }
7218  }
7219  }
7220 
7221  // Generate 3d grid map?
7222  if(ui_->dockWidget_occupancyGridView->isVisible() && ui_->checkBox_grid_grid->isChecked())
7223  {
7224 #ifdef RTABMAP_OCTOMAP
7225  if(ui_->checkBox_octomap->isChecked())
7226  {
7227  updateOctomapView();
7228  }
7229  else
7230 #endif
7231  {
7232  CloudMap cloudMap(&combinedLocalMaps, parameters);
7233 
7234  cloudMap.update(graphFiltered);
7235 
7236  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & groundCells = cloudMap.getMapGround();
7237  const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & obstacleCells = cloudMap.getMapObstacles();
7238  const pcl::PointCloud<pcl::PointXYZ>::Ptr & emptyCells = cloudMap.getMapEmptyCells();
7239 
7240  // occupancy cloud
7241  if(groundCells->size())
7242  {
7243  occupancyGridViewer_->addCloud("groundCells",
7244  groundCells,
7245  Transform::getIdentity(),
7246  QColor(ui_->lineEdit_groundColor->text()));
7247  occupancyGridViewer_->setCloudPointSize("groundCells", 5);
7248  }
7249  if(obstacleCells->size())
7250  {
7251  occupancyGridViewer_->addCloud("obstacleCells",
7252  obstacleCells,
7253  Transform::getIdentity(),
7254  QColor(ui_->lineEdit_obstacleColor->text()));
7255  occupancyGridViewer_->setCloudPointSize("obstacleCells", 5);
7256  }
7257  if(ui_->checkBox_grid_empty->isChecked() && emptyCells->size())
7258  {
7259  occupancyGridViewer_->addCloud("emptyCells",
7260  emptyCells,
7261  Transform::getIdentity(),
7262  QColor(ui_->lineEdit_emptyColor->text()));
7263  occupancyGridViewer_->setCloudPointSize("emptyCells", 5);
7264  occupancyGridViewer_->setCloudOpacity("emptyCells", 0.5);
7265  }
7266  occupancyGridViewer_->refreshView();
7267  }
7268  }
7269 
7270 #ifdef RTABMAP_GRIDMAP
7271  // Show elevation map ?
7272  if(ui_->dockWidget_occupancyGridView->isVisible() &&
7273  ui_->checkBox_grid_elevation->checkState() != Qt::Unchecked)
7274  {
7275  GridMap gridMap(&combinedLocalMaps, parameters);
7276 
7277  gridMap.update(graphFiltered);
7278  if(ui_->checkBox_grid_elevation->checkState() == Qt::PartiallyChecked)
7279  {
7280  float xMin, yMin;
7281  cv::Mat elevationMap = gridMap.createHeightMap(xMin, yMin, cellSize);
7282  occupancyGridViewer_->addElevationMap(elevationMap, cellSize, xMin, yMin, 1.0f);
7283  }
7284  else
7285  {
7286  pcl::PolygonMesh::Ptr mesh = gridMap.createTerrainMesh();
7287  occupancyGridViewer_->addCloudMesh("elevation_mesh", mesh);
7288  }
7289  occupancyGridViewer_->refreshView();
7290  }
7291 #endif
7292  }
7293  ui_->graphViewer->fitInView(ui_->graphViewer->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
7294  if(rectScaleItem != 0)
7295  {
7296  ui_->graphViewer->fitInView(rectScaleItem, Qt::KeepAspectRatio);
7297  ui_->graphViewer->scene()->removeItem(rectScaleItem);
7298  delete rectScaleItem;
7299  }
7300 
7301  ui_->graphViewer->update();
7302  ui_->label_iterations->setNum(value);
7303 
7304  //compute total length (neighbor links)
7305  float length = 0.0f;
7306  for(std::multimap<int, rtabmap::Link>::const_iterator iter=graphLinks_.begin(); iter!=graphLinks_.end(); ++iter)
7307  {
7308  std::map<int, rtabmap::Transform>::const_iterator jterA = graph.find(iter->first);
7309  std::map<int, rtabmap::Transform>::const_iterator jterB = graph.find(iter->second.to());
7310  if(jterA != graph.end() && jterB != graph.end())
7311  {
7312  const rtabmap::Transform & poseA = jterA->second;
7313  const rtabmap::Transform & poseB = jterB->second;
7314  if(iter->second.type() == rtabmap::Link::kNeighbor ||
7315  iter->second.type() == rtabmap::Link::kNeighborMerged)
7316  {
7317  Eigen::Vector3f vA, vB;
7318  float x,y,z;
7319  poseA.getTranslation(x,y,z);
7320  vA[0] = x; vA[1] = y; vA[2] = z;
7321  poseB.getTranslation(x,y,z);
7322  vB[0] = x; vB[1] = y; vB[2] = z;
7323  length += (vB - vA).norm();
7324  }
7325  }
7326  }
7327  ui_->label_pathLength->setNum(length);
7328  }
7329 }
7330 
7331 void DatabaseViewer::updateGraphRotation()
7332 {
7333  if(ui_->horizontalSlider_rotation->isEnabled())
7334  {
7335  float theta = float(ui_->horizontalSlider_rotation->value())*M_PI/1800.0f;
7336  ui_->graphViewer->setWorldMapRotation(theta);
7337  ui_->label_rotation->setText(QString::number(float(-ui_->horizontalSlider_rotation->value())/10.0f, 'f', 1) + " deg");
7338  }
7339  else
7340  {
7341  ui_->graphViewer->setWorldMapRotation(0);
7342  }
7343 }
7344 
7345 void DatabaseViewer::updateGraphView()
7346 {
7347  ui_->label_loopClosures->clear();
7348  ui_->label_poses->clear();
7349  ui_->label_rmse->clear();
7350 
7351  if(sender() == ui_->checkBox_alignPosesWithGPS && ui_->checkBox_alignPosesWithGPS->isChecked())
7352  {
7353  ui_->checkBox_alignPosesWithGroundTruth->setChecked(false);
7354  }
7355  else if(sender() == ui_->checkBox_alignPosesWithGroundTruth && ui_->checkBox_alignPosesWithGroundTruth->isChecked())
7356  {
7357  ui_->checkBox_alignPosesWithGPS->setChecked(false);
7358  }
7359 
7360  if(odomPoses_.size())
7361  {
7362  int fromId = ui_->spinBox_optimizationsFrom->value();
7363  if(!uContains(odomPoses_, fromId))
7364  {
7365  QMessageBox::warning(this, tr(""), tr("Graph optimization from id (%1) for which node is not linked to graph.\n Minimum=%2, Maximum=%3")
7366  .arg(fromId)
7367  .arg(odomPoses_.begin()->first)
7368  .arg(odomPoses_.rbegin()->first));
7369  return;
7370  }
7371 
7372  graphes_.clear();
7373  graphLinks_.clear();
7374 
7375  std::map<int, rtabmap::Transform> poses = odomPoses_;
7376  if(ui_->checkBox_wmState->isChecked() && uContains(wmStates_, fromId))
7377  {
7378  std::map<int, rtabmap::Transform> wmPoses;
7379  std::vector<int> & wmState = wmStates_.at(fromId);
7380  for(unsigned int i=0; i<wmState.size(); ++i)
7381  {
7382  std::map<int, rtabmap::Transform>::iterator iter = poses.find(wmState[i]);
7383  if(iter!=poses.end())
7384  {
7385  wmPoses.insert(*iter);
7386  }
7387  }
7388  if(!wmPoses.empty())
7389  {
7390  poses = wmPoses;
7391  }
7392  else
7393  {
7394  UWARN("Empty WM poses!? Ignoring WM state... (root id=%d, wmState=%d)", fromId, wmState.size());
7395  }
7396  }
7397 
7398  // filter current map if not spanning to all maps
7399  if(!ui_->checkBox_spanAllMaps->isChecked() && uContains(mapIds_, fromId) && mapIds_.at(fromId) >= 0)
7400  {
7401  int currentMapId = mapIds_.at(fromId);
7402  for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end();)
7403  {
7404  if(iter->first>0 && (!uContains(mapIds_, iter->first) || mapIds_.at(iter->first) != currentMapId))
7405  {
7406  poses.erase(iter++);
7407  }
7408  else
7409  {
7410  ++iter;
7411  }
7412  }
7413  }
7414 
7415  ui_->menuExport_poses->setEnabled(true);
7416  std::multimap<int, rtabmap::Link> links = links_;
7417  loopLinks_.clear();
7418 
7419  // filter current map if not spanning to all maps
7420  if(!ui_->checkBox_spanAllMaps->isChecked() && uContains(mapIds_, fromId) && mapIds_.at(fromId) >= 0)
7421  {
7422  int currentMapId = mapIds_.at(fromId);
7423  for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end();)
7424  {
7425  if((iter->second.from()>0 && (!uContains(mapIds_, iter->second.from()) || mapIds_.at(iter->second.from()) != currentMapId)) ||
7426  (iter->second.to()>0 && (!uContains(mapIds_, iter->second.to()) || mapIds_.at(iter->second.to()) != currentMapId)))
7427  {
7428  links.erase(iter++);
7429  }
7430  else
7431  {
7432  ++iter;
7433  }
7434  }
7435  }
7436 
7437  links = updateLinksWithModifications(links);
7438  if(ui_->checkBox_ignorePoseCorrection->isChecked())
7439  {
7440  for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
7441  {
7442  if(iter->second.type() == Link::kNeighbor ||
7443  iter->second.type() == Link::kNeighborMerged)
7444  {
7445  Transform poseFrom = uValue(poses, iter->second.from(), Transform());
7446  Transform poseTo = uValue(poses, iter->second.to(), Transform());
7447  if(!poseFrom.isNull() && !poseTo.isNull())
7448  {
7449  // reset to identity covariance
7450  iter->second = Link(
7451  iter->second.from(),
7452  iter->second.to(),
7453  iter->second.type(),
7454  poseFrom.inverse() * poseTo);
7455  }
7456  }
7457  }
7458  }
7459 
7460  // Marker priors parameters
7461  double markerPriorsLinearVariance = Parameters::defaultMarkerPriorsVarianceLinear();
7462  double markerPriorsAngularVariance = Parameters::defaultMarkerPriorsVarianceAngular();
7463  std::map<int, Transform> markerPriors;
7464  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
7465  Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), markerPriorsLinearVariance);
7466  UASSERT(markerPriorsLinearVariance>0.0f);
7467  Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceAngular(), markerPriorsAngularVariance);
7468  UASSERT(markerPriorsAngularVariance>0.0f);
7469  std::string markerPriorsStr;
7470  if(Parameters::parse(parameters, Parameters::kMarkerPriors(), markerPriorsStr))
7471  {
7472  std::list<std::string> strList = uSplit(markerPriorsStr, '|');
7473  for(std::list<std::string>::iterator iter=strList.begin(); iter!=strList.end(); ++iter)
7474  {
7475  std::string markerStr = *iter;
7476  while(!markerStr.empty() && !uIsDigit(markerStr[0]))
7477  {
7478  markerStr.erase(markerStr.begin());
7479  }
7480  if(!markerStr.empty())
7481  {
7482  std::string idStr = uSplitNumChar(markerStr).front();
7483  int id = uStr2Int(idStr);
7484  Transform prior = Transform::fromString(markerStr.substr(idStr.size()));
7485  if(!prior.isNull() && id>0)
7486  {
7487  markerPriors.insert(std::make_pair(-id, prior));
7488  UDEBUG("Added landmark prior %d: %s", id, prior.prettyPrint().c_str());
7489  }
7490  else
7491  {
7492  UERROR("Failed to parse element \"%s\" in parameter %s", markerStr.c_str(), Parameters::kMarkerPriors().c_str());
7493  }
7494  }
7495  else if(!iter->empty())
7496  {
7497  UERROR("Failed to parse parameter %s, value=\"%s\"", Parameters::kMarkerPriors().c_str(), iter->c_str());
7498  }
7499  }
7500  }
7501 
7502 
7503  // filter links
7504  int totalNeighbor = 0;
7505  int totalNeighborMerged = 0;
7506  int totalGlobal = 0;
7507  int totalLocalTime = 0;
7508  int totalLocalSpace = 0;
7509  int totalUser = 0;
7510  int totalPriors = 0;
7511  int totalLandmarks = 0;
7512  int totalGravity = 0;
7513  std::multimap<int, int> uniqueLinks;
7514  for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end();)
7515  {
7516  bool isUnique = iter->second.from() == iter->second.to(); // Count all self-reference links
7517  if(graph::findLink(uniqueLinks, iter->second.from(), iter->second.to(), true) == uniqueLinks.end())
7518  {
7519  uniqueLinks.insert(std::make_pair(iter->second.from(), iter->second.to()));
7520  isUnique = true;
7521  }
7522 
7523  if(iter->second.type() == Link::kNeighbor)
7524  {
7525  if(isUnique)
7526  ++totalNeighbor;
7527  }
7528  else if(iter->second.type() == Link::kNeighborMerged)
7529  {
7530  if(isUnique)
7531  ++totalNeighborMerged;
7532  }
7533  else if(iter->second.type() == Link::kGlobalClosure)
7534  {
7535  if(ui_->checkBox_ignoreGlobalLoop->isChecked())
7536  {
7537  links.erase(iter++);
7538  continue;
7539  }
7540  loopLinks_.push_back(iter->second);
7541  if(isUnique)
7542  ++totalGlobal;
7543  }
7544  else if(iter->second.type() == Link::kLocalSpaceClosure)
7545  {
7546  if(ui_->checkBox_ignoreLocalLoopSpace->isChecked())
7547  {
7548  links.erase(iter++);
7549  continue;
7550  }
7551  loopLinks_.push_back(iter->second);
7552  if(isUnique)
7553  ++totalLocalSpace;
7554  }
7555  else if(iter->second.type() == Link::kLocalTimeClosure)
7556  {
7557  if(ui_->checkBox_ignoreLocalLoopTime->isChecked())
7558  {
7559  links.erase(iter++);
7560  continue;
7561  }
7562  loopLinks_.push_back(iter->second);
7563  if(isUnique)
7564  ++totalLocalTime;
7565  }
7566  else if(iter->second.type() == Link::kUserClosure)
7567  {
7568  if(ui_->checkBox_ignoreUserLoop->isChecked())
7569  {
7570  links.erase(iter++);
7571  continue;
7572  }
7573  loopLinks_.push_back(iter->second);
7574  if(isUnique)
7575  ++totalUser;
7576  }
7577  else if(iter->second.type() == Link::kLandmark)
7578  {
7579  if(ui_->checkBox_ignoreLandmarks->isChecked())
7580  {
7581  links.erase(iter++);
7582  continue;
7583  }
7584  UASSERT(iter->second.from() > 0 && iter->second.to() < 0);
7585  if(poses.find(iter->second.from()) != poses.end() && poses.find(iter->second.to()) == poses.end())
7586  {
7587  poses.insert(std::make_pair(iter->second.to(), poses.at(iter->second.from())*iter->second.transform()));
7588  }
7589  loopLinks_.push_back(iter->second);
7590  if(isUnique)
7591  ++totalLandmarks;
7592 
7593  // add landmark priors if there are some
7594  int markerId = iter->second.to();
7595  if(markerPriors.find(markerId) != markerPriors.end())
7596  {
7597  cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64FC1);
7598  infMatrix(cv::Range(0,3), cv::Range(0,3)) /= markerPriorsLinearVariance;
7599  infMatrix(cv::Range(3,6), cv::Range(3,6)) /= markerPriorsAngularVariance;
7600  links.insert(std::make_pair(markerId, Link(markerId, markerId, Link::kPosePrior, markerPriors.at(markerId), infMatrix)));
7601  UDEBUG("Added prior %d : %s (variance: lin=%f ang=%f)", markerId, markerPriors.at(markerId).prettyPrint().c_str(),
7602  markerPriorsLinearVariance, markerPriorsAngularVariance);
7603  if(isUnique)
7604  ++totalPriors;
7605  }
7606  }
7607  else if(iter->second.type() == Link::kPosePrior)
7608  {
7609  if(isUnique)
7610  ++totalPriors;
7611  }
7612  else if(iter->second.type() == Link::kGravity)
7613  {
7614  if(isUnique)
7615  ++totalGravity;
7616  }
7617  else
7618  {
7619  loopLinks_.push_back(iter->second);
7620  }
7621  ++iter;
7622  }
7623  updateLoopClosuresSlider();
7624 
7625  ui_->label_loopClosures->setText(tr("(%1, %2, %3, %4, %5, %6, %7, %8, %9)")
7626  .arg(totalNeighbor)
7627  .arg(totalNeighborMerged)
7628  .arg(totalGlobal)
7629  .arg(totalLocalSpace)
7630  .arg(totalLocalTime)
7631  .arg(totalUser)
7632  .arg(totalPriors)
7633  .arg(totalLandmarks)
7634  .arg(totalGravity));
7635 
7636  // remove intermediate nodes?
7637  if(ui_->checkBox_ignoreIntermediateNodes->isVisible() &&
7638  ui_->checkBox_ignoreIntermediateNodes->isChecked())
7639  {
7640  for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
7641  {
7642  if(iter->second.type() == Link::kNeighbor ||
7643  iter->second.type() == Link::kNeighborMerged)
7644  {
7645  Link link = iter->second;
7646  while(uContains(weights_, link.to()) && weights_.at(link.to()) < 0)
7647  {
7648  std::multimap<int, Link>::iterator uter = links.find(link.to());
7649  while(uter != links.end() &&
7650  uter->first==link.to() &&
7651  uter->second.from()>uter->second.to())
7652  {
7653  ++uter;
7654  }
7655  if(uter != links.end())
7656  {
7657  poses.erase(link.to());
7658  link = link.merge(uter->second, uter->second.type());
7659  links.erase(uter->first);
7660  }
7661  else
7662  {
7663  break;
7664  }
7665  }
7666 
7667  iter->second = link;
7668  }
7669  }
7670  }
7671 
7672  bool applyRotation = sender() == ui_->pushButton_applyRotation;
7673  if(applyRotation)
7674  {
7675  float xMin, yMin, cellSize;
7676  bool hasMap = !dbDriver_->load2DMap(xMin, yMin, cellSize).empty();
7677  if(hasMap || !dbDriver_->loadOptimizedMesh().empty())
7678  {
7679  QMessageBox::StandardButton r = QMessageBox::question(this,
7680  tr("Rotate Optimized Graph"),
7681  tr("There is a 2D occupancy grid or mesh already saved in "
7682  "database. Applying rotation will clear them (they can be "
7683  "regenerated later from File menu options). "
7684  "Do you want to continue?"),
7685  QMessageBox::Cancel | QMessageBox::Yes,
7686  QMessageBox::Cancel);
7687  if(r != QMessageBox::Yes)
7688  {
7689  applyRotation = false;
7690  }
7691  }
7692  }
7693 
7694  std::map<int, Transform> optPoses;
7695  Transform lastLocalizationPose;
7696  if(applyRotation ||
7697  ui_->comboBox_optimizationFlavor->currentIndex() == 2)
7698  {
7699  optPoses = dbDriver_->loadOptimizedPoses(&lastLocalizationPose);
7700  if(optPoses.empty())
7701  {
7702  ui_->comboBox_optimizationFlavor->setCurrentIndex(0);
7703  QMessageBox::warning(this, tr("Optimization Flavor"),
7704  tr("There is no local optimized graph in the database, "
7705  "falling back to global iterative optimization."));
7706  }
7707  }
7708 
7709  if(applyRotation ||
7710  ui_->comboBox_optimizationFlavor->currentIndex() != 2)
7711  {
7712  if(ui_->horizontalSlider_rotation->value()!=0 && applyRotation)
7713  {
7714  float theta = float(-ui_->horizontalSlider_rotation->value())*M_PI/1800.0f;
7715  Transform rotT(0,0,theta);
7716  poses.at(fromId) = rotT * poses.at(fromId);
7717  }
7718 
7719  graphes_.push_back(poses);
7720 
7721  Optimizer * optimizer = Optimizer::create(parameters);
7722 
7723  std::map<int, rtabmap::Transform> posesOut;
7724  std::multimap<int, rtabmap::Link> linksOut;
7725  UINFO("Get connected graph from %d (%d poses, %d links)", fromId, (int)poses.size(), (int)links.size());
7726  optimizer->getConnectedGraph(
7727  fromId,
7728  poses,
7729  links,
7730  posesOut,
7731  linksOut);
7732  UINFO("Connected graph of %d poses and %d links", (int)posesOut.size(), (int)linksOut.size());
7733  QElapsedTimer time;
7734  time.start();
7735  std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(fromId, posesOut, linksOut, ui_->comboBox_optimizationFlavor->currentIndex()==0?&graphes_:0);
7736  ui_->label_timeOptimization->setNum(double(time.elapsed())/1000.0);
7737  graphLinks_ = linksOut;
7738  if(posesOut.size() && finalPoses.empty())
7739  {
7740  UWARN("Optimization failed... (poses=%d, links=%d).", (int)posesOut.size(), (int)linksOut.size());
7741  if(!optimizer->isCovarianceIgnored() || optimizer->type() != Optimizer::kTypeTORO)
7742  {
7743  QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors. "
7744  "Give it a try with %1=0 and %2=true.").arg(Parameters::kOptimizerStrategy().c_str()).arg(Parameters::kOptimizerVarianceIgnored().c_str()));
7745  }
7746  else
7747  {
7748  QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors."));
7749  }
7750  }
7751  ui_->label_poses->setNum((int)finalPoses.size());
7752  graphes_.push_back(finalPoses);
7753  delete optimizer;
7754 
7755  if(applyRotation && !finalPoses.empty())
7756  {
7757  ui_->comboBox_optimizationFlavor->setCurrentIndex(2);
7758  graphes_.clear();
7759  graphes_.push_back(finalPoses);
7760  if(lastLocalizationPose.isNull())
7761  {
7762  // use last pose by default
7763  lastLocalizationPose = finalPoses.rbegin()->second;
7764  }
7765  dbDriver_->saveOptimizedPoses(finalPoses, lastLocalizationPose);
7766  // reset optimized mesh and map as poses have changed
7767  float xMin, yMin, cellSize;
7768  bool hasMap = !dbDriver_->load2DMap(xMin, yMin, cellSize).empty();
7769  if(hasMap || !dbDriver_->loadOptimizedMesh().empty())
7770  {
7771  dbDriver_->saveOptimizedMesh(cv::Mat());
7772  dbDriver_->save2DMap(cv::Mat(), 0, 0, 0);
7773  QMessageBox::StandardButton r = QMessageBox::question(this,
7774  tr("Rotate Optimized Graph"),
7775  tr("Optimized graph has been rotated and saved back to database. "
7776  "Note that 2D occupancy grid and mesh have been cleared (if set). "
7777  "Do you want to regenerate the 2D occupancy grid now "
7778  "(can be done later from File menu)?"),
7779  QMessageBox::Ignore | QMessageBox::Yes,
7780  QMessageBox::Yes);
7781  ui_->actionEdit_optimized_2D_map->setEnabled(false);
7782  ui_->actionExport_saved_2D_map->setEnabled(false);
7783  ui_->actionImport_2D_map->setEnabled(false);
7784  ui_->actionView_optimized_mesh->setEnabled(false);
7785  ui_->actionExport_optimized_mesh->setEnabled(false);
7786  if(r == QMessageBox::Yes)
7787  {
7788  regenerateSavedMap();
7789  }
7790  }
7791  }
7792  }
7793 
7794  // Update buttons state
7795  if(ui_->comboBox_optimizationFlavor->currentIndex() == 2)
7796  {
7797  // Local optimized graph
7798  if(graphes_.empty())
7799  {
7800  ui_->label_timeOptimization->setNum(0);
7801  ui_->label_poses->setNum((int)optPoses.size());
7802  graphes_.push_back(optPoses);
7803  }
7804  ui_->horizontalSlider_rotation->setEnabled(false);
7805  ui_->pushButton_applyRotation->setEnabled(false);
7806  ui_->spinBox_optimizationsFrom->setEnabled(false);
7807  ui_->checkBox_spanAllMaps->setEnabled(false);
7808  ui_->checkBox_wmState->setEnabled(false);
7809  ui_->checkBox_alignPosesWithGPS->setEnabled(false);
7810  ui_->checkBox_alignPosesWithGroundTruth->setEnabled(false);
7811  ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(false);
7812  ui_->checkBox_ignoreIntermediateNodes->setEnabled(false);
7813  }
7814  else
7815  {
7816  // Global map re-optimized
7817  ui_->pushButton_applyRotation->setEnabled(true);
7818  ui_->horizontalSlider_rotation->setEnabled(true);
7819  ui_->spinBox_optimizationsFrom->setEnabled(true);
7820  ui_->checkBox_spanAllMaps->setEnabled(true);
7821  ui_->checkBox_wmState->setEnabled(true);
7822  ui_->checkBox_alignPosesWithGPS->setEnabled(ui_->checkBox_alignPosesWithGPS->isVisible());
7823  ui_->checkBox_alignPosesWithGroundTruth->setEnabled(ui_->checkBox_alignPosesWithGroundTruth->isVisible());
7824  ui_->checkBox_alignScansCloudsWithGroundTruth->setEnabled(ui_->checkBox_alignScansCloudsWithGroundTruth->isVisible());
7825  ui_->checkBox_ignoreIntermediateNodes->setEnabled(true);
7826  }
7827  updateGraphRotation();
7828  }
7829  if(graphes_.size())
7830  {
7831  if(ui_->doubleSpinBox_optimizationScale->value()!=-1.0)
7832  {
7833  // scale all poses
7834  for(std::list<std::map<int, Transform> >::iterator iter=graphes_.begin(); iter!=graphes_.end(); ++iter)
7835  {
7836  for(std::map<int, Transform>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
7837  {
7838  jter->second = jter->second.clone();
7839  jter->second.x() *= ui_->doubleSpinBox_optimizationScale->value();
7840  jter->second.y() *= ui_->doubleSpinBox_optimizationScale->value();
7841  jter->second.z() *= ui_->doubleSpinBox_optimizationScale->value();
7842  }
7843  }
7844  }
7845 
7846  ui_->horizontalSlider_iterations->setMaximum((int)graphes_.size()-1);
7847  ui_->horizontalSlider_iterations->setValue((int)graphes_.size()-1);
7848  ui_->horizontalSlider_iterations->setEnabled(true);
7849  ui_->spinBox_optimizationsFrom->setEnabled(true);
7850  sliderIterationsValueChanged((int)graphes_.size()-1);
7851  }
7852  else
7853  {
7854  ui_->horizontalSlider_iterations->setEnabled(false);
7855  ui_->spinBox_optimizationsFrom->setEnabled(false);
7856  }
7857 }
7858 
7859 void DatabaseViewer::updateGrid()
7860 {
7861  if(sender() == ui_->checkBox_grid_2d && !ui_->checkBox_grid_2d->isChecked())
7862  {
7863  //just remove map in occupancy grid view
7864  occupancyGridViewer_->removeOccupancyGridMap();
7865  occupancyGridViewer_->refreshView();
7866  }
7867  else
7868  {
7869  ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked());
7870  ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
7871  ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7872  ui_->checkBox_grid_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7873  ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
7874  ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
7875  ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7876  ui_->label_octomap_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7877 
7878  update3dView();
7879  updateGraphView();
7880  }
7881 }
7882 
7883 void DatabaseViewer::updateOctomapView()
7884 {
7885 #ifdef RTABMAP_OCTOMAP
7886  ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked());
7887  ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
7888  ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7889  ui_->checkBox_grid_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7890  ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
7891  ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
7892  ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7893  ui_->label_octomap_frontiers->setVisible(ui_->checkBox_octomap->isChecked() && ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7894 
7895  if(ui_->checkBox_octomap->isChecked())
7896  {
7897  if(octomap_)
7898  {
7899  occupancyGridViewer_->removeOctomap();
7900  occupancyGridViewer_->removeCloud("octomap_obstacles");
7901  occupancyGridViewer_->removeCloud("octomap_ground");
7902  occupancyGridViewer_->removeCloud("octomap_empty");
7903  occupancyGridViewer_->removeCloud("octomap_frontiers");
7904  occupancyGridViewer_->removeCloud("groundCells");
7905  occupancyGridViewer_->removeCloud("obstacleCells");
7906  occupancyGridViewer_->removeCloud("emptyCells");
7907  if(ui_->comboBox_octomap_rendering_type->currentIndex()>0)
7908  {
7909  occupancyGridViewer_->addOctomap(octomap_, ui_->spinBox_grid_depth->value(), ui_->comboBox_octomap_rendering_type->currentIndex()>1);
7910  }
7911  else
7912  {
7913  pcl::IndicesPtr obstacles(new std::vector<int>);
7914  pcl::IndicesPtr empty(new std::vector<int>);
7915  pcl::IndicesPtr ground(new std::vector<int>);
7916  pcl::IndicesPtr frontiers(new std::vector<int>);
7917  std::vector<double> prob;
7918  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(
7919  ui_->spinBox_grid_depth->value(),
7920  obstacles.get(),
7921  empty.get(),
7922  ground.get(),
7923  true,
7924  frontiers.get(),
7925  &prob);
7926 
7927  if(octomap_->hasColor())
7928  {
7929  pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
7930  pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
7931  occupancyGridViewer_->addCloud("octomap_obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
7932  occupancyGridViewer_->setCloudPointSize("octomap_obstacles", 5);
7933 
7934  pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
7935  pcl::copyPointCloud(*cloud, *ground, *groundCloud);
7936  occupancyGridViewer_->addCloud("octomap_ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
7937  occupancyGridViewer_->setCloudPointSize("octomap_ground", 5);
7938  }
7939  else
7940  {
7941  pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
7942  pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
7943  occupancyGridViewer_->addCloud("octomap_obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
7944  occupancyGridViewer_->setCloudPointSize("octomap_obstacles", 5);
7945 
7946  pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
7947  pcl::copyPointCloud(*cloud, *ground, *groundCloud);
7948  occupancyGridViewer_->addCloud("octomap_ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
7949  occupancyGridViewer_->setCloudPointSize("octomap_ground", 5);
7950  }
7951 
7952  if(ui_->checkBox_grid_empty->isChecked())
7953  {
7954  if(prob.size()==cloud->size())
7955  {
7956  float occThr = Parameters::defaultGridGlobalOccupancyThr();
7957  Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kGridGlobalOccupancyThr(), occThr);
7958  pcl::PointCloud<pcl::PointXYZRGB>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
7959  emptyCloud->resize(empty->size());
7960  for(unsigned int i=0;i<empty->size(); ++i)
7961  {
7962  emptyCloud->points.at(i).x = cloud->points.at(empty->at(i)).x;
7963  emptyCloud->points.at(i).y = cloud->points.at(empty->at(i)).y;
7964  emptyCloud->points.at(i).z = cloud->points.at(empty->at(i)).z;
7965  QColor hsv = QColor::fromHsv(int(prob.at(empty->at(i))/occThr*240.0), 255, 255, 255);
7966  QRgb color = hsv.rgb();
7967  emptyCloud->points.at(i).r = qRed(color);
7968  emptyCloud->points.at(i).g = qGreen(color);
7969  emptyCloud->points.at(i).b = qBlue(color);
7970  }
7971  occupancyGridViewer_->addCloud("octomap_empty", emptyCloud, Transform::getIdentity(), QColor(ui_->lineEdit_emptyColor->text()));
7972  occupancyGridViewer_->setCloudOpacity("octomap_empty", 0.5);
7973  occupancyGridViewer_->setCloudPointSize("octomap_empty", 5);
7974  }
7975  else
7976  {
7977  pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZ>);
7978  pcl::copyPointCloud(*cloud, *empty, *emptyCloud);
7979  occupancyGridViewer_->addCloud("octomap_empty", emptyCloud, Transform::getIdentity(), QColor(ui_->lineEdit_emptyColor->text()));
7980  occupancyGridViewer_->setCloudOpacity("octomap_empty", 0.5);
7981  occupancyGridViewer_->setCloudPointSize("octomap_empty", 5);
7982  }
7983  }
7984 
7985  if(ui_->checkBox_grid_frontiers->isChecked())
7986  {
7987  pcl::PointCloud<pcl::PointXYZ>::Ptr frontiersCloud(new pcl::PointCloud<pcl::PointXYZ>);
7988  pcl::copyPointCloud(*cloud, *frontiers, *frontiersCloud);
7989  occupancyGridViewer_->addCloud("octomap_frontiers", frontiersCloud, Transform::getIdentity(), QColor(ui_->lineEdit_frontierColor->text()));
7990  occupancyGridViewer_->setCloudOpacity("octomap_frontiers", 0.5);
7991  occupancyGridViewer_->setCloudPointSize("octomap_frontiers", 5);
7992  }
7993  }
7994  occupancyGridViewer_->refreshView();
7995  }
7996  if(ui_->dockWidget_view3d->isVisible() && ui_->checkBox_showGrid->isChecked())
7997  {
7998  this->update3dView();
7999  }
8000  }
8001 #endif
8002 }
8003 
8004 Link DatabaseViewer::findActiveLink(int from, int to)
8005 {
8006  Link link;
8007  std::multimap<int, Link>::iterator findIter = rtabmap::graph::findLink(linksRefined_, from ,to);
8008  if(findIter != linksRefined_.end())
8009  {
8010  link = findIter->second;
8011  }
8012  else
8013  {
8014  findIter = rtabmap::graph::findLink(linksAdded_, from ,to);
8015  if(findIter != linksAdded_.end())
8016  {
8017  link = findIter->second;
8018  }
8019  else if(!containsLink(linksRemoved_, from ,to))
8020  {
8021  findIter = rtabmap::graph::findLink(links_, from ,to);
8022  if(findIter != links_.end())
8023  {
8024  link = findIter->second;
8025  }
8026  }
8027  }
8028  return link;
8029 }
8030 
8031 bool DatabaseViewer::containsLink(std::multimap<int, Link> & links, int from, int to)
8032 {
8033  return rtabmap::graph::findLink(links, from, to) != links.end();
8034 }
8035 
8036 void DatabaseViewer::refineConstraint()
8037 {
8038  int from = ids_.at(ui_->horizontalSlider_A->value());
8039  int to = ids_.at(ui_->horizontalSlider_B->value());
8040  refineConstraint(from, to, false);
8041 }
8042 
8043 void DatabaseViewer::refineConstraint(int from, int to, bool silent)
8044 {
8045  UDEBUG("%d -> %d", from, to);
8046  bool switchedIds = false;
8047  if(from == to)
8048  {
8049  UWARN("Cannot refine link to same node");
8050  return;
8051  }
8052 
8053 
8054  Link currentLink = findActiveLink(from, to);
8055  if(!currentLink.isValid())
8056  {
8057  UERROR("Not found link! (%d->%d)", from, to);
8058  return;
8059  }
8060 
8061  UDEBUG("%d -> %d (type=%d)", currentLink.from(), currentLink.to(), currentLink.type());
8062  Transform t = currentLink.transform();
8063  if(ui_->checkBox_showOptimized->isChecked() &&
8064  (currentLink.type() == Link::kNeighbor || currentLink.type() == Link::kNeighborMerged) &&
8065  graphes_.size() &&
8066  (int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
8067  {
8068  std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
8069  if(currentLink.type() == Link::kNeighbor || currentLink.type() == Link::kNeighborMerged)
8070  {
8071  std::map<int, rtabmap::Transform>::iterator iterFrom = graph.find(currentLink.from());
8072  std::map<int, rtabmap::Transform>::iterator iterTo = graph.find(currentLink.to());
8073  if(iterFrom != graph.end() && iterTo != graph.end())
8074  {
8075  Transform topt = iterFrom->second.inverse()*iterTo->second;
8076  t = topt;
8077  }
8078  }
8079  }
8080  else if(ui_->checkBox_ignorePoseCorrection->isChecked() &&
8081  graph::findLink(linksRefined_, currentLink.from(), currentLink.to()) == linksRefined_.end())
8082  {
8083  if(currentLink.type() == Link::kNeighbor ||
8084  currentLink.type() == Link::kNeighborMerged)
8085  {
8086  Transform poseFrom = uValue(odomPoses_, currentLink.from(), Transform());
8087  Transform poseTo = uValue(odomPoses_, currentLink.to(), Transform());
8088  if(!poseFrom.isNull() && !poseTo.isNull())
8089  {
8090  t = poseFrom.inverse() * poseTo; // recompute raw odom transformation
8091  }
8092  }
8093  }
8094 
8095  Transform transform;
8096  RegistrationInfo info;
8097  Signature * fromS = 0;
8098  Signature * toS = 0;
8099 
8100  fromS = dbDriver_->loadSignature(currentLink.from());
8101  if(fromS == 0)
8102  {
8103  UERROR("Signature %d not found!", currentLink.from());
8104  return;
8105  }
8106 
8107  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
8108 
8109  UTimer timer;
8110 
8111  // Is it a multi-scan proximity detection?
8112  cv::Mat userData = currentLink.uncompressUserDataConst();
8113  std::map<int, rtabmap::Transform> scanPoses;
8114 
8115  if(currentLink.type() == Link::kLocalSpaceClosure &&
8116  !currentLink.userDataCompressed().empty() &&
8117  userData.type() == CV_8SC1 &&
8118  userData.rows == 1 &&
8119  userData.cols >= 8 && // including null str ending
8120  userData.at<char>(userData.cols-1) == 0 &&
8121  memcmp(userData.data, "SCANS:", 6) == 0 &&
8122  currentLink.from() > currentLink.to())
8123  {
8124  std::string scansStr = (const char *)userData.data;
8125  UINFO("Detected \"%s\" in links's user data", scansStr.c_str());
8126  if(!scansStr.empty())
8127  {
8128  std::list<std::string> strs = uSplit(scansStr, ':');
8129  if(strs.size() == 2)
8130  {
8131  std::list<std::string> strIds = uSplit(strs.rbegin()->c_str(), ';');
8132  for(std::list<std::string>::iterator iter=strIds.begin(); iter!=strIds.end(); ++iter)
8133  {
8134  int id = atoi(iter->c_str());
8135  if(uContains(odomPoses_, id))
8136  {
8137  scanPoses.insert(*odomPoses_.find(id));
8138  }
8139  else
8140  {
8141  UERROR("Not found %d node!", id);
8142  }
8143  }
8144  }
8145  }
8146  }
8147  if(scanPoses.size()>1)
8148  {
8149  //optimize the path's poses locally
8150  Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
8151 
8152  UASSERT(uContains(scanPoses, currentLink.to()));
8153  std::map<int, rtabmap::Transform> posesOut;
8154  std::multimap<int, rtabmap::Link> linksOut;
8155  optimizer->getConnectedGraph(
8156  currentLink.to(),
8157  scanPoses,
8158  updateLinksWithModifications(links_),
8159  posesOut,
8160  linksOut);
8161 
8162  if(scanPoses.size() != posesOut.size())
8163  {
8164  UWARN("Scan poses input and output are different! %d vs %d", (int)scanPoses.size(), (int)posesOut.size());
8165  UWARN("Input poses: ");
8166  for(std::map<int, Transform>::iterator iter=scanPoses.begin(); iter!=scanPoses.end(); ++iter)
8167  {
8168  UWARN(" %d", iter->first);
8169  }
8170  UWARN("Input links: ");
8171  std::multimap<int, Link> modifiedLinks = updateLinksWithModifications(links_);
8172  for(std::multimap<int, Link>::iterator iter=modifiedLinks.begin(); iter!=modifiedLinks.end(); ++iter)
8173  {
8174  UWARN(" %d->%d", iter->second.from(), iter->second.to());
8175  }
8176  }
8177 
8178  scanPoses = optimizer->optimize(currentLink.to(), posesOut, linksOut);
8179  delete optimizer;
8180 
8181  std::map<int, Transform> filteredScanPoses = scanPoses;
8182  float proximityFilteringRadius = 0.0f;
8183  Parameters::parse(parameters, Parameters::kRGBDProximityPathFilteringRadius(), proximityFilteringRadius);
8184  if(scanPoses.size() > 2 && proximityFilteringRadius > 0.0f)
8185  {
8186  // path filtering
8187  filteredScanPoses = graph::radiusPosesFiltering(scanPoses, proximityFilteringRadius, 0, true);
8188  // make sure the current pose is still here
8189  filteredScanPoses.insert(*scanPoses.find(currentLink.to()));
8190  }
8191 
8192  Transform toPoseInv = filteredScanPoses.at(currentLink.to()).inverse();
8193  dbDriver_->loadNodeData(fromS, !silent, true, !silent, !silent);
8194  fromS->sensorData().uncompressData();
8195  LaserScan fromScan = fromS->sensorData().laserScanRaw();
8196  int maxPoints = fromScan.size();
8197  if(maxPoints == 0)
8198  {
8199  UWARN("From scan %d is empty!", fromS->id());
8200  }
8201  pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
8202  pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(new pcl::PointCloud<pcl::PointNormal>);
8203  pcl::PointCloud<pcl::PointXYZI>::Ptr assembledToIClouds(new pcl::PointCloud<pcl::PointXYZI>);
8204  pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembledToNormalIClouds(new pcl::PointCloud<pcl::PointXYZINormal>);
8205  pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledToRGBClouds(new pcl::PointCloud<pcl::PointXYZRGB>);
8206  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledToNormalRGBClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
8207  for(std::map<int, Transform>::const_iterator iter = filteredScanPoses.begin(); iter!=filteredScanPoses.end(); ++iter)
8208  {
8209  if(iter->first != currentLink.from())
8210  {
8211  SensorData data;
8212  dbDriver_->getNodeData(iter->first, data);
8213  if(!data.laserScanCompressed().isEmpty())
8214  {
8215  LaserScan scan;
8216  data.uncompressData(0, 0, &scan);
8217  if(!scan.isEmpty() && fromScan.format() == scan.format())
8218  {
8219  if(scan.hasIntensity())
8220  {
8221  if(scan.hasNormals())
8222  {
8223  *assembledToNormalIClouds += *util3d::laserScanToPointCloudINormal(scan,
8224  toPoseInv * iter->second * scan.localTransform());
8225  }
8226  else
8227  {
8228  *assembledToIClouds += *util3d::laserScanToPointCloudI(scan,
8229  toPoseInv * iter->second * scan.localTransform());
8230  }
8231  }
8232  else if(scan.hasRGB())
8233  {
8234  if(scan.hasNormals())
8235  {
8236  *assembledToNormalRGBClouds += *util3d::laserScanToPointCloudRGBNormal(scan,
8237  toPoseInv * iter->second * scan.localTransform());
8238  }
8239  else
8240  {
8241  *assembledToRGBClouds += *util3d::laserScanToPointCloudRGB(scan,
8242  toPoseInv * iter->second * scan.localTransform());
8243  }
8244  }
8245  else
8246  {
8247  if(scan.hasNormals())
8248  {
8249  *assembledToNormalClouds += *util3d::laserScanToPointCloudNormal(scan,
8250  toPoseInv * iter->second * scan.localTransform());
8251  }
8252  else
8253  {
8254  *assembledToClouds += *util3d::laserScanToPointCloud(scan,
8255  toPoseInv * iter->second * scan.localTransform());
8256  }
8257  }
8258 
8259  if(scan.size() > maxPoints)
8260  {
8261  maxPoints = scan.size();
8262  }
8263  }
8264  else
8265  {
8266  UWARN("scan format of %d is not the same than from scan %d: %d vs %d", data.id(), fromS->id(), scan.format(), fromScan.format());
8267  }
8268  }
8269  else
8270  {
8271  UWARN("Laser scan not found for signature %d", iter->first);
8272  }
8273  }
8274  }
8275 
8276  LaserScan assembledScan;
8277  if(assembledToNormalClouds->size())
8278  {
8279  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToNormalClouds):util3d::laserScanFromPointCloud(*assembledToNormalClouds);
8280  }
8281  else if(assembledToClouds->size())
8282  {
8283  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToClouds):util3d::laserScanFromPointCloud(*assembledToClouds);
8284  }
8285  else if(assembledToNormalIClouds->size())
8286  {
8287  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToNormalIClouds):util3d::laserScanFromPointCloud(*assembledToNormalIClouds);
8288  }
8289  else if(assembledToIClouds->size())
8290  {
8291  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToIClouds):util3d::laserScanFromPointCloud(*assembledToIClouds);
8292  }
8293  else if(assembledToNormalRGBClouds->size())
8294  {
8295  UASSERT(!fromScan.is2d());
8296  assembledScan = util3d::laserScanFromPointCloud(*assembledToNormalRGBClouds);
8297  }
8298  else if(assembledToRGBClouds->size())
8299  {
8300  UASSERT(!fromScan.is2d());
8301  assembledScan = util3d::laserScanFromPointCloud(*assembledToRGBClouds);
8302  }
8303  else
8304  {
8305  UWARN("Assembled scan is empty!");
8306  }
8307  SensorData assembledData(cv::Mat(), to);
8308  // scans are in base frame but for 2d scans, set the height so that correspondences matching works
8309  assembledData.setLaserScan(LaserScan(
8310  assembledScan,
8311  fromScan.maxPoints()?fromScan.maxPoints():maxPoints,
8312  fromScan.rangeMax(),
8313  fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
8314 
8315  toS = new Signature(assembledData);
8316  RegistrationIcp registrationIcp(parameters);
8317  transform = registrationIcp.computeTransformationMod(*fromS, *toS, currentLink.transform(), &info);
8318  if(!transform.isNull())
8319  {
8320  // local scan matching proximity detection should have higher variance (see Rtabmap::process())
8321  info.covariance*=100.0;
8322  }
8323  }
8324  else
8325  {
8326  toS = dbDriver_->loadSignature(currentLink.to());
8327  if(toS == 0)
8328  {
8329  UERROR("Signature %d not found!", currentLink.to());
8330  delete fromS;
8331  return;
8332  }
8333 
8334  bool reextractVisualFeatures = uStr2Bool(parameters.at(Parameters::kRGBDLoopClosureReextractFeatures()));
8335  Registration * reg = Registration::create(parameters);
8336  if( reg->isScanRequired() ||
8337  reg->isUserDataRequired() ||
8338  reextractVisualFeatures ||
8339  !silent)
8340  {
8341  dbDriver_->loadNodeData(fromS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
8342  dbDriver_->loadNodeData(toS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
8343 
8344  if(!silent)
8345  {
8346  fromS->sensorData().uncompressData();
8347  toS->sensorData().uncompressData();
8348  }
8349  }
8350 
8351  if(reextractVisualFeatures)
8352  {
8353  fromS->removeAllWords();
8354  fromS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
8355  toS->removeAllWords();
8356  toS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
8357  }
8358 
8359  if(reg->isScanRequired())
8360  {
8361  if(ui_->checkBox_icp_from_depth->isChecked())
8362  {
8363  // generate laser scans from depth image
8364  cv::Mat tmpA, tmpB, tmpC, tmpD;
8365  fromS->sensorData().uncompressData(&tmpA, &tmpB, 0);
8366  toS->sensorData().uncompressData(&tmpC, &tmpD, 0);
8367  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom = util3d::cloudFromSensorData(
8368  fromS->sensorData(),
8369  ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
8370  ui_->doubleSpinBox_icp_maxDepth->value(),
8371  ui_->doubleSpinBox_icp_minDepth->value(),
8372  0,
8373  ui_->parameters_toolbox->getParameters());
8374  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo = util3d::cloudFromSensorData(
8375  toS->sensorData(),
8376  ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
8377  ui_->doubleSpinBox_icp_maxDepth->value(),
8378  ui_->doubleSpinBox_icp_minDepth->value(),
8379  0,
8380  ui_->parameters_toolbox->getParameters());
8381 
8382  if(cloudFrom->empty() && cloudTo->empty())
8383  {
8384  std::string msg = "Option to generate scan from depth is checked (GUI Parameters->Refine), but "
8385  "resulting clouds from depth are empty. Transformation estimation will likely "
8386  "fails. Uncheck the parameter to use laser scans.";
8387  UWARN(msg.c_str());
8388  if(!silent)
8389  {
8390  QMessageBox::warning(this,
8391  tr("Refine link"),
8392  tr("%1").arg(msg.c_str()));
8393  }
8394  }
8395  else if(!fromS->sensorData().laserScanCompressed().isEmpty() || !toS->sensorData().laserScanCompressed().isEmpty())
8396  {
8397  UWARN("There are laser scans in data, but generate laser scan from "
8398  "depth image option is activated (GUI Parameters->Refine). "
8399  "Ignoring saved laser scans...");
8400  }
8401 
8402  int maxLaserScans = cloudFrom->size();
8403  fromS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0));
8404  toS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0));
8405  }
8406  else
8407  {
8408  LaserScan tmpA, tmpB;
8409  fromS->sensorData().uncompressData(0, 0, &tmpA);
8410  toS->sensorData().uncompressData(0, 0, &tmpB);
8411  }
8412  }
8413 
8414  if(reg->isImageRequired() && reextractVisualFeatures)
8415  {
8416  cv::Mat tmpA, tmpB, tmpC, tmpD;
8417  fromS->sensorData().uncompressData(&tmpA, &tmpB, 0);
8418  toS->sensorData().uncompressData(&tmpC, &tmpD, 0);
8419  }
8420 
8421  UINFO("Uncompress time: %f s", timer.ticks());
8422 
8423  if(fromS->id() < toS->id())
8424  {
8425  transform = reg->computeTransformationMod(*fromS, *toS, t, &info);
8426  }
8427  else
8428  {
8429  transform = reg->computeTransformationMod(*toS, *fromS, t.isNull()?t:t.inverse(), &info);
8430  switchedIds = true;
8431  }
8432 
8433  delete reg;
8434  }
8435  UINFO("(%d ->%d) Registration time: %f s", currentLink.from(), currentLink.to(), timer.ticks());
8436 
8437  if(!transform.isNull())
8438  {
8439  if(!transform.isIdentity())
8440  {
8441  if(info.covariance.at<double>(0,0)<=0.0)
8442  {
8443  info.covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
8444  }
8445  }
8446  if(switchedIds)
8447  {
8448  transform = transform.inverse();
8449  }
8450 
8451  Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform, info.covariance.inv(), currentLink.userDataCompressed());
8452 
8453  bool updated = false;
8454  std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
8455  while(iter != linksRefined_.end() && iter->first == currentLink.from())
8456  {
8457  if(iter->second.to() == currentLink.to() &&
8458  iter->second.type() == currentLink.type())
8459  {
8460  iter->second = newLink;
8461  updated = true;
8462  break;
8463  }
8464  ++iter;
8465  }
8466  if(!updated)
8467  {
8468  linksRefined_.insert(std::make_pair(newLink.from(), newLink));
8469  updated = true;
8470  }
8471 
8472  if(updated && !silent)
8473  {
8474  this->updateGraphView();
8475  }
8476 
8477  if(!silent && ui_->dockWidget_constraints->isVisible())
8478  {
8479  if(toS && fromS->id() > 0 && toS->id() > 0)
8480  {
8481  updateLoopClosuresSlider(fromS->id(), toS->id());
8482  std::multimap<int, cv::KeyPoint> keypointsFrom;
8483  std::multimap<int, cv::KeyPoint> keypointsTo;
8484  if(!fromS->getWordsKpts().empty())
8485  {
8486  for(std::map<int, int>::const_iterator iter=fromS->getWords().begin(); iter!=fromS->getWords().end(); ++iter)
8487  {
8488  keypointsFrom.insert(keypointsFrom.end(), std::make_pair(iter->first, fromS->getWordsKpts()[iter->second]));
8489  }
8490  }
8491  if(!toS->getWordsKpts().empty())
8492  {
8493  for(std::map<int, int>::const_iterator iter=toS->getWords().begin(); iter!=toS->getWords().end(); ++iter)
8494  {
8495  keypointsTo.insert(keypointsTo.end(), std::make_pair(iter->first, toS->getWordsKpts()[iter->second]));
8496  }
8497  }
8498  if(newLink.type() != Link::kNeighbor && fromS->id() < toS->id())
8499  {
8500  this->updateConstraintView(newLink.inverse(), true, *toS, *fromS);
8501  ui_->graphicsView_A->setFeatures(keypointsTo, toS->sensorData().depthRaw());
8502  ui_->graphicsView_B->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
8503  }
8504  else
8505  {
8506  this->updateConstraintView(newLink, true, *fromS, *toS);
8507  ui_->graphicsView_A->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
8508  ui_->graphicsView_B->setFeatures(keypointsTo, toS->sensorData().depthRaw());
8509  }
8510 
8511  updateWordsMatching(info.inliersIDs);
8512  }
8513  else
8514  {
8515  this->updateConstraintView();
8516  }
8517  }
8518  }
8519  else if(!silent)
8520  {
8521  if(toS && fromS->id() > 0 && toS->id() > 0)
8522  {
8523  // just update matches in the views
8524  std::multimap<int, cv::KeyPoint> keypointsFrom;
8525  std::multimap<int, cv::KeyPoint> keypointsTo;
8526  if(!fromS->getWordsKpts().empty())
8527  {
8528  for(std::map<int, int>::const_iterator iter=fromS->getWords().begin(); iter!=fromS->getWords().end(); ++iter)
8529  {
8530  keypointsFrom.insert(keypointsFrom.end(), std::make_pair(iter->first, fromS->getWordsKpts()[iter->second]));
8531  }
8532  }
8533  if(!toS->getWordsKpts().empty())
8534  {
8535  for(std::map<int, int>::const_iterator iter=toS->getWords().begin(); iter!=toS->getWords().end(); ++iter)
8536  {
8537  keypointsTo.insert(keypointsTo.end(), std::make_pair(iter->first, toS->getWordsKpts()[iter->second]));
8538  }
8539  }
8540  if(currentLink.type() != Link::kNeighbor && fromS->id() < toS->id())
8541  {
8542  ui_->graphicsView_A->setFeatures(keypointsTo, toS->sensorData().depthRaw());
8543  ui_->graphicsView_B->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
8544  }
8545  else
8546  {
8547  ui_->graphicsView_A->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
8548  ui_->graphicsView_B->setFeatures(keypointsTo, toS->sensorData().depthRaw());
8549  }
8550  updateWordsMatching(info.inliersIDs);
8551  }
8552 
8553  QMessageBox::warning(this,
8554  tr("Refine link"),
8555  tr("Cannot find a transformation between nodes %1 and %2: %3").arg(currentLink.from()).arg(currentLink.to()).arg(info.rejectedMsg.c_str()));
8556  }
8557  delete fromS;
8558  delete toS;
8559 }
8560 
8561 void DatabaseViewer::addConstraint()
8562 {
8563  int from = ids_.at(ui_->horizontalSlider_A->value());
8564  int to = ids_.at(ui_->horizontalSlider_B->value());
8565  addConstraint(from, to, false);
8566 }
8567 
8568 bool DatabaseViewer::addConstraint(int from, int to, bool silent, bool silentlyUseOptimizedGraphAsGuess)
8569 {
8570  bool switchedIds = false;
8571  if(from == to)
8572  {
8573  UWARN("Cannot add link to same node");
8574  return false;
8575  }
8576  else if(from > to)
8577  {
8578  int tmp = from;
8579  from = to;
8580  to = tmp;
8581  switchedIds = true;
8582  }
8583  std::list<Signature*> signatures;
8584  Signature * fromS=0;
8585  Signature * toS=0;
8586 
8587  Link newLink;
8588  RegistrationInfo info;
8589  if(!containsLink(linksAdded_, from, to) &&
8590  !containsLink(links_, from, to))
8591  {
8592  UASSERT(!containsLink(linksRemoved_, from, to));
8593  UASSERT(!containsLink(linksRefined_, from, to));
8594 
8595  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
8596  Registration * reg = Registration::create(parameters);
8597 
8598  bool loopCovLimited = Parameters::defaultRGBDLoopCovLimited();
8599  Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), loopCovLimited);
8600  std::vector<double> odomMaxInf = odomMaxInf_;
8601  if(loopCovLimited && odomMaxInf_.empty())
8602  {
8603  odomMaxInf = graph::getMaxOdomInf(updateLinksWithModifications(links_));
8604  }
8605 
8606  Transform t;
8607 
8608  std::list<int> ids;
8609  ids.push_back(from);
8610  ids.push_back(to);
8611  dbDriver_->loadSignatures(ids, signatures);
8612  if(signatures.size() != 2)
8613  {
8614  for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
8615  {
8616  delete *iter;
8617  }
8618  return false;
8619  }
8620  fromS = *signatures.begin();
8621  toS = *signatures.rbegin();
8622 
8623  bool reextractVisualFeatures = uStr2Bool(parameters.at(Parameters::kRGBDLoopClosureReextractFeatures()));
8624  if(reg->isScanRequired() ||
8625  reg->isUserDataRequired() ||
8626  reextractVisualFeatures ||
8627  !silent)
8628  {
8629  // Add sensor data to generate features
8630  dbDriver_->loadNodeData(fromS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
8631  fromS->sensorData().uncompressData();
8632  dbDriver_->loadNodeData(toS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
8633  toS->sensorData().uncompressData();
8634  if(reextractVisualFeatures)
8635  {
8636  fromS->removeAllWords();
8637  fromS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
8638  toS->removeAllWords();
8639  toS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
8640  }
8641  if(reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked())
8642  {
8643  // generate laser scans from depth image
8644  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom = util3d::cloudFromSensorData(
8645  fromS->sensorData(),
8646  ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
8647  ui_->doubleSpinBox_icp_maxDepth->value(),
8648  ui_->doubleSpinBox_icp_minDepth->value(),
8649  0,
8650  ui_->parameters_toolbox->getParameters());
8651  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo = util3d::cloudFromSensorData(
8652  toS->sensorData(),
8653  ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
8654  ui_->doubleSpinBox_icp_maxDepth->value(),
8655  ui_->doubleSpinBox_icp_minDepth->value(),
8656  0,
8657  ui_->parameters_toolbox->getParameters());
8658 
8659  if(cloudFrom->empty() && cloudTo->empty())
8660  {
8661  std::string msg = "Option to generate scan from depth is checked (GUI Parameters->Refine), but "
8662  "resulting clouds from depth are empty. Transformation estimation will likely "
8663  "fails. Uncheck the parameter to use laser scans.";
8664  UWARN(msg.c_str());
8665  if(!silent)
8666  {
8667  QMessageBox::warning(this,
8668  tr("Add link"),
8669  tr("%1").arg(msg.c_str()));
8670  }
8671  }
8672  else if(!fromS->sensorData().laserScanCompressed().isEmpty() || !toS->sensorData().laserScanCompressed().isEmpty())
8673  {
8674  UWARN("There are laser scans in data, but generate laser scan from "
8675  "depth image option is activated (GUI Parameters->Refine). Ignoring saved laser scans...");
8676  }
8677 
8678  int maxLaserScans = cloudFrom->size();
8679  fromS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0));
8680  toS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0));
8681  }
8682  }
8683  else if(!reextractVisualFeatures && fromS->getWords().empty() && toS->getWords().empty())
8684  {
8685  std::string msg = uFormat("\"%s\" is false and signatures (%d and %d) don't have words, "
8686  "registration will not be possible. Set \"%s\" to true.",
8687  Parameters::kRGBDLoopClosureReextractFeatures().c_str(),
8688  fromS->id(),
8689  toS->id(),
8690  Parameters::kRGBDLoopClosureReextractFeatures().c_str());
8691  UWARN(msg.c_str());
8692  if(!silent)
8693  {
8694  QMessageBox::warning(this,
8695  tr("Add link"),
8696  tr("%1").arg(msg.c_str()));
8697  }
8698  }
8699 
8700  Transform guess;
8701  bool guessFromGraphRejected = false;
8702  if(!reg->isImageRequired())
8703  {
8704  // make a fake guess using globally optimized poses
8705  if(graphes_.size())
8706  {
8707  std::map<int, Transform> optimizedPoses = graphes_.back();
8708  if(optimizedPoses.size() > 0)
8709  {
8710  std::map<int, Transform>::iterator fromIter = optimizedPoses.find(from);
8711  std::map<int, Transform>::iterator toIter = optimizedPoses.find(to);
8712  if(fromIter != optimizedPoses.end() &&
8713  toIter != optimizedPoses.end())
8714  {
8715  if(!silent)
8716  {
8717  if(QMessageBox::question(this,
8718  tr("Add constraint from optimized graph"),
8719  tr("Registration is done without vision (see %1 parameter), "
8720  "do you want to use a guess from the optimized graph?"
8721  ""
8722  "\n\nOtherwise, if "
8723  "the database has images, it is recommended to use %2=2 instead so that "
8724  "the guess can be found visually.")
8725  .arg(Parameters::kRegStrategy().c_str()).arg(Parameters::kRegStrategy().c_str()),
8726  QMessageBox::Yes | QMessageBox::No,
8727  QMessageBox::Yes) == QMessageBox::Yes)
8728  {
8729  guess = fromIter->second.inverse() * toIter->second;
8730  }
8731  else
8732  {
8733  guessFromGraphRejected = true;
8734  }
8735  }
8736  else if(silentlyUseOptimizedGraphAsGuess)
8737  {
8738  guess = fromIter->second.inverse() * toIter->second;
8739  }
8740  }
8741  }
8742  }
8743  if(guess.isNull() && !silent && !guessFromGraphRejected)
8744  {
8745  if(QMessageBox::question(this,
8746  tr("Add constraint without guess"),
8747  tr("Registration is done without vision (see %1 parameter) and we cannot (or we don't want to) find relative "
8748  "transformation between the nodes with the current graph. Do you want to use an identity "
8749  "transform for ICP guess? "
8750  ""
8751  "\n\nOtherwise, if the database has images, it is recommended to use %2=2 "
8752  "instead so that the guess can be found visually.")
8753  .arg(Parameters::kRegStrategy().c_str()).arg(Parameters::kRegStrategy().c_str()),
8754  QMessageBox::Yes | QMessageBox::Abort,
8755  QMessageBox::Abort) == QMessageBox::Yes)
8756  {
8757  guess.setIdentity();
8758  }
8759  else
8760  {
8761  guessFromGraphRejected = true;
8762  }
8763  }
8764  }
8765 
8766  if(switchedIds)
8767  {
8768  t = reg->computeTransformationMod(*toS, *fromS, guess.isNull()?guess:guess.inverse(), &info);
8769  if(!t.isNull())
8770  {
8771  t = t.inverse();
8772  }
8773  }
8774  else
8775  {
8776  t = reg->computeTransformationMod(*fromS, *toS, guess, &info);
8777  }
8778  delete reg;
8779  UDEBUG("");
8780 
8781  if(!t.isNull())
8782  {
8783  cv::Mat information = info.covariance.inv();
8784  if(odomMaxInf.size() == 6 && information.cols==6 && information.rows==6)
8785  {
8786  for(int i=0; i<6; ++i)
8787  {
8788  if(information.at<double>(i,i) > odomMaxInf[i])
8789  {
8790  information.at<double>(i,i) = odomMaxInf[i];
8791  }
8792  }
8793  }
8794 
8795  newLink = Link(from, to, Link::kUserClosure, t, information);
8796  }
8797  else if(!silent && !guessFromGraphRejected)
8798  {
8799  QMessageBox::StandardButton button = QMessageBox::warning(this,
8800  tr("Add link"),
8801  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()),
8802  QMessageBox::Yes | QMessageBox::No,
8803  QMessageBox::No);
8804  if(button == QMessageBox::Yes)
8805  {
8806  editConstraint();
8807  silent = true;
8808  }
8809  }
8810  }
8811  else if(containsLink(linksRemoved_, from, to))
8812  {
8813  newLink = rtabmap::graph::findLink(linksRemoved_, from, to)->second;
8814  }
8815 
8816  bool updateConstraints = newLink.isValid();
8817  float maxOptimizationError = uStr2Float(ui_->parameters_toolbox->getParameters().at(Parameters::kRGBDOptimizeMaxError()));
8818  if(newLink.isValid() &&
8819  maxOptimizationError > 0.0f &&
8820  uStr2Int(ui_->parameters_toolbox->getParameters().at(Parameters::kOptimizerIterations())) > 0.0f)
8821  {
8822  int fromId = newLink.from();
8823  std::multimap<int, Link> linksIn = updateLinksWithModifications(links_);
8824  linksIn.insert(std::make_pair(newLink.from(), newLink));
8825  const Link * maxLinearLink = 0;
8826  const Link * maxAngularLink = 0;
8827  float maxLinearErrorRatio = 0.0f;
8828  float maxAngularErrorRatio = 0.0f;
8829  Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
8830  std::map<int, Transform> poses;
8831  std::multimap<int, Link> links;
8832  UASSERT(odomPoses_.find(fromId) != odomPoses_.end());
8833  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());
8834  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());
8835  optimizer->getConnectedGraph(fromId, odomPoses_, linksIn, poses, links);
8836  // use already optimized poses
8837  if(graphes_.size())
8838  {
8839  const std::map<int, Transform> & optimizedPoses = graphes_.back();
8840  for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
8841  {
8842  if(optimizedPoses.find(iter->first) != optimizedPoses.end())
8843  {
8844  iter->second = optimizedPoses.at(iter->first);
8845  }
8846  }
8847  }
8848  UASSERT(poses.find(fromId) != poses.end());
8849  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());
8850  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());
8851  UASSERT(graph::findLink(links, newLink.from(), newLink.to()) != links.end());
8852  std::map<int, Transform> posesIn = poses;
8853  poses = optimizer->optimize(fromId, posesIn, links);
8854  if(posesIn.size() && poses.empty())
8855  {
8856  UWARN("Optimization failed... (poses=%d, links=%d).", (int)posesIn.size(), (int)links.size());
8857  }
8858  std::string msg;
8859  if(poses.size())
8860  {
8861  float maxLinearError = 0.0f;
8862  float maxAngularError = 0.0f;
8863  graph::computeMaxGraphErrors(
8864  poses,
8865  links,
8866  maxLinearErrorRatio,
8867  maxAngularErrorRatio,
8868  maxLinearError,
8869  maxAngularError,
8870  &maxLinearLink,
8871  &maxAngularLink);
8872  if(maxLinearLink)
8873  {
8874  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()));
8875  if(maxLinearErrorRatio > maxOptimizationError)
8876  {
8877  msg = uFormat("Rejecting edge %d->%d because "
8878  "graph error is too large (abs=%f m) after optimization (ratio %f for edge %d->%d, stddev=%f m). "
8879  "\"%s\" is %f.",
8880  newLink.from(),
8881  newLink.to(),
8882  maxLinearError,
8883  maxLinearErrorRatio,
8884  maxLinearLink->from(),
8885  maxLinearLink->to(),
8886  sqrt(maxLinearLink->transVariance()),
8887  Parameters::kRGBDOptimizeMaxError().c_str(),
8888  maxOptimizationError);
8889  }
8890  }
8891  if(maxAngularLink)
8892  {
8893  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()));
8894  if(maxAngularErrorRatio > maxOptimizationError)
8895  {
8896  msg = uFormat("Rejecting edge %d->%d because "
8897  "graph error is too large (abs=%f deg) after optimization (ratio %f for edge %d->%d, stddev=%f deg). "
8898  "\"%s\" is %f.",
8899  newLink.from(),
8900  newLink.to(),
8901  maxAngularError*180.0f/CV_PI,
8902  maxAngularErrorRatio,
8903  maxAngularLink->from(),
8904  maxAngularLink->to(),
8905  sqrt(maxAngularLink->rotVariance()),
8906  Parameters::kRGBDOptimizeMaxError().c_str(),
8907  maxOptimizationError);
8908  }
8909  }
8910  }
8911  else
8912  {
8913  msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
8914  newLink.from(),
8915  newLink.to());
8916  }
8917  if(!msg.empty())
8918  {
8919  UWARN("%s", msg.c_str());
8920  if(!silent)
8921  {
8922  QMessageBox::warning(this,
8923  tr("Add link"),
8924  tr("%1").arg(msg.c_str()));
8925  }
8926 
8927  updateConstraints = false;
8928  }
8929 
8930  if(updateConstraints && silent && !graphes_.empty() && graphes_.back().size() == poses.size())
8931  {
8932  graphes_.back() = poses;
8933  }
8934  }
8935 
8936  if(updateConstraints)
8937  {
8938  if(containsLink(linksRemoved_, from, to))
8939  {
8940  //simply remove from linksRemoved
8941  linksRemoved_.erase(rtabmap::graph::findLink(linksRemoved_, from, to));
8942  }
8943  else
8944  {
8945  if(newLink.from() < newLink.to())
8946  {
8947  newLink = newLink.inverse();
8948  }
8949  linksAdded_.insert(std::make_pair(newLink.from(), newLink));
8950  }
8951  }
8952 
8953  if(!silent)
8954  {
8955  if(fromS && toS)
8956  {
8957  if((updateConstraints && newLink.from() > newLink.to()) || (!updateConstraints && switchedIds))
8958  {
8959  Signature * tmpS = fromS;
8960  fromS = toS;
8961  toS = tmpS;
8962  }
8963 
8964  if(updateConstraints)
8965  {
8966  updateLoopClosuresSlider(fromS->id(), toS->id());
8967  this->updateGraphView();
8968  this->updateConstraintView(newLink, false, *fromS, *toS);
8969  }
8970 
8971  std::multimap<int, cv::KeyPoint> keypointsFrom;
8972  std::multimap<int, cv::KeyPoint> keypointsTo;
8973  if(!fromS->getWordsKpts().empty())
8974  {
8975  for(std::map<int, int>::const_iterator iter=fromS->getWords().begin(); iter!=fromS->getWords().end(); ++iter)
8976  {
8977  keypointsFrom.insert(keypointsFrom.end(), std::make_pair(iter->first, fromS->getWordsKpts()[iter->second]));
8978  }
8979  }
8980  if(!toS->getWordsKpts().empty())
8981  {
8982  for(std::map<int, int>::const_iterator iter=toS->getWords().begin(); iter!=toS->getWords().end(); ++iter)
8983  {
8984  keypointsTo.insert(keypointsTo.end(), std::make_pair(iter->first, toS->getWordsKpts()[iter->second]));
8985  }
8986  }
8987  ui_->graphicsView_A->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
8988  ui_->graphicsView_B->setFeatures(keypointsTo, toS->sensorData().depthRaw());
8989  updateWordsMatching(info.inliersIDs);
8990  }
8991  else if(updateConstraints)
8992  {
8993  updateLoopClosuresSlider(from, to);
8994  this->updateGraphView();
8995  }
8996  }
8997 
8998  for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
8999  {
9000  delete *iter;
9001  }
9002 
9003  return updateConstraints;
9004 }
9005 
9006 void DatabaseViewer::resetConstraint()
9007 {
9008  int from = ids_.at(ui_->horizontalSlider_A->value());
9009  int to = ids_.at(ui_->horizontalSlider_B->value());
9010  if(ui_->label_type->text().toInt() == Link::kLandmark)
9011  {
9012  int position = ui_->horizontalSlider_loops->value();
9013  const rtabmap::Link & link = loopLinks_.at(position);
9014  from = link.from();
9015  to = link.to();
9016  }
9017 
9018  if(from < to)
9019  {
9020  int tmp = to;
9021  to = from;
9022  from = tmp;
9023  }
9024 
9025  if(from == to)
9026  {
9027  UWARN("Cannot reset link to same node");
9028  return;
9029  }
9030 
9031 
9032  std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, from, to);
9033  if(iter != linksRefined_.end())
9034  {
9035  linksRefined_.erase(iter);
9036  this->updateGraphView();
9037  }
9038 
9039  updateConstraintView();
9040 }
9041 
9042 void DatabaseViewer::rejectConstraint()
9043 {
9044  int priorId = sender() == ui_->toolButton_remove_priorA?ids_.at(ui_->horizontalSlider_A->value()):
9045  sender() == ui_->toolButton_remove_priorB?ids_.at(ui_->horizontalSlider_B->value()):0;
9046 
9047  int from = priorId>0?priorId:ids_.at(ui_->horizontalSlider_A->value());
9048  int to = priorId>0?priorId:ids_.at(ui_->horizontalSlider_B->value());
9049  if(priorId==0 && ui_->label_type->text().toInt() == Link::kLandmark)
9050  {
9051  int position = ui_->horizontalSlider_loops->value();
9052  const rtabmap::Link & link = loopLinks_.at(position);
9053  from = link.from();
9054  to = link.to();
9055  }
9056 
9057  if(from < to)
9058  {
9059  int tmp = to;
9060  to = from;
9061  from = tmp;
9062  }
9063 
9064  if(priorId==0 && from == to)
9065  {
9066  UWARN("Cannot reject link to same node");
9067  return;
9068  }
9069 
9070  bool removed = false;
9071 
9072  // find the original one
9073  std::multimap<int, Link>::iterator iter;
9074  iter = rtabmap::graph::findLink(links_, from, to);
9075  if(iter != links_.end())
9076  {
9077  if(iter->second.type() == Link::kNeighbor || iter->second.type() == Link::kNeighborMerged)
9078  {
9079  QMessageBox::StandardButton button = QMessageBox::warning(this, tr("Reject link"),
9080  tr("Removing the neighbor link %1->%2 will split the graph. Do you want to continue?").arg(from).arg(to),
9081  QMessageBox::Yes | QMessageBox::No, QMessageBox::No);
9082  if(button != QMessageBox::Yes)
9083  {
9084  return;
9085  }
9086  }
9087  linksRemoved_.insert(*iter);
9088  removed = true;
9089  }
9090 
9091  // remove from refined and added
9092  iter = rtabmap::graph::findLink(linksRefined_, from, to);
9093  if(iter != linksRefined_.end())
9094  {
9095  linksRefined_.erase(iter);
9096  removed = true;
9097  }
9098  iter = rtabmap::graph::findLink(linksAdded_, from, to);
9099  if(iter != linksAdded_.end())
9100  {
9101  linksAdded_.erase(iter);
9102  removed = true;
9103  }
9104  if(removed)
9105  {
9106  if(priorId==0)
9107  {
9108  this->updateGraphView();
9109  updateLoopClosuresSlider();
9110  }
9111  else
9112  {
9113  bool priorsIgnored = Parameters::defaultOptimizerPriorsIgnored();
9114  Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kOptimizerPriorsIgnored(), priorsIgnored);
9115  int indexA = ui_->horizontalSlider_A->value();
9116  int indexB = ui_->horizontalSlider_B->value();
9117  if(!priorsIgnored)
9118  {
9119  this->updateGraphView();
9120  }
9121  if(ui_->horizontalSlider_A->value() != indexA)
9122  ui_->horizontalSlider_A->setValue(indexA);
9123  else
9124  sliderAValueChanged(indexA);
9125  if(ui_->horizontalSlider_B->value() != indexB)
9126  ui_->horizontalSlider_B->setValue(indexB);
9127  else
9128  sliderBValueChanged(indexB);
9129  }
9130  }
9131 }
9132 
9133 std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
9134  const std::multimap<int, rtabmap::Link> & edgeConstraints)
9135 {
9136  UDEBUG("linksAdded_=%d linksRefined_=%d linksRemoved_=%d", (int)linksAdded_.size(), (int)linksRefined_.size(), (int)linksRemoved_.size());
9137 
9138  std::multimap<int, rtabmap::Link> links;
9139  for(std::multimap<int, rtabmap::Link>::const_iterator iter=edgeConstraints.begin();
9140  iter!=edgeConstraints.end();
9141  ++iter)
9142  {
9143  std::multimap<int, rtabmap::Link>::iterator findIter;
9144 
9145  findIter = rtabmap::graph::findLink(linksRemoved_, iter->second.from(), iter->second.to());
9146  if(findIter != linksRemoved_.end())
9147  {
9148  UDEBUG("Removed link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
9149  continue; // don't add this link
9150  }
9151 
9152  findIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
9153  if(findIter!=linksRefined_.end())
9154  {
9155  links.insert(*findIter); // add the refined link
9156  UDEBUG("Updated link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
9157  continue;
9158  }
9159 
9160  links.insert(*iter); // add original link
9161  }
9162 
9163  //look for added links
9164  for(std::multimap<int, rtabmap::Link>::const_iterator iter=linksAdded_.begin();
9165  iter!=linksAdded_.end();
9166  ++iter)
9167  {
9168  std::multimap<int, rtabmap::Link>::iterator findIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
9169  if(findIter!=linksRefined_.end())
9170  {
9171  links.insert(*findIter); // add the refined link
9172  UDEBUG("Added refined link (%d->%d, %d)", findIter->second.from(), findIter->second.to(), findIter->second.type());
9173  continue;
9174  }
9175 
9176  UDEBUG("Added link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
9177  links.insert(*iter);
9178  }
9179 
9180  return links;
9181 }
9182 
9183 void DatabaseViewer::updateLoopClosuresSlider(int from, int to)
9184 {
9185  UDEBUG("%d %d", from, to);
9186  loopLinks_.clear();
9187  std::multimap<int, Link> links = updateLinksWithModifications(links_);
9188  int position = ui_->horizontalSlider_loops->value();
9189  std::multimap<int, Link> linksSortedByParents;
9190  for(std::multimap<int, rtabmap::Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
9191  {
9192  if(iter->second.to() > iter->second.from())
9193  {
9194  linksSortedByParents.insert(std::make_pair(iter->second.to(), iter->second.inverse()));
9195  }
9196  else if(iter->second.to() != iter->second.from())
9197  {
9198  linksSortedByParents.insert(*iter);
9199  }
9200  }
9201  for(std::multimap<int, rtabmap::Link>::iterator iter = linksSortedByParents.begin(); iter!=linksSortedByParents.end(); ++iter)
9202  {
9203  if(!iter->second.transform().isNull())
9204  {
9205  if(iter->second.type() != rtabmap::Link::kNeighbor &&
9206  iter->second.type() != rtabmap::Link::kNeighborMerged)
9207  {
9208  if((iter->second.from() == from && iter->second.to() == to) ||
9209  (iter->second.to() == from && iter->second.from() == to))
9210  {
9211  position = loopLinks_.size();
9212  }
9213  loopLinks_.append(iter->second);
9214  }
9215  }
9216  else
9217  {
9218  UERROR("Transform null for link from %d to %d", iter->first, iter->second.to());
9219  }
9220  }
9221 
9222  if(loopLinks_.size())
9223  {
9224  if(loopLinks_.size() == 1)
9225  {
9226  // just to be able to move the cursor of the loop slider
9227  loopLinks_.push_back(loopLinks_.front());
9228  }
9229  ui_->horizontalSlider_loops->setMinimum(0);
9230  ui_->horizontalSlider_loops->setMaximum(loopLinks_.size()-1);
9231  ui_->horizontalSlider_loops->setEnabled(true);
9232  if(position != ui_->horizontalSlider_loops->value())
9233  {
9234  ui_->horizontalSlider_loops->setValue(position);
9235  }
9236  else
9237  {
9238  this->updateConstraintView(loopLinks_.at(position));
9239  }
9240  }
9241  else
9242  {
9243  ui_->horizontalSlider_loops->setEnabled(false);
9244  constraintsViewer_->removeAllClouds();
9245  constraintsViewer_->refreshView();
9246  updateConstraintButtons();
9247  }
9248 }
9249 
9250 void DatabaseViewer::notifyParametersChanged(const QStringList & parametersChanged)
9251 {
9252  bool updateStereo = false;
9253  bool updateGraphView = false;
9254  for(QStringList::const_iterator iter=parametersChanged.constBegin();
9255  iter!=parametersChanged.constEnd() && (!updateStereo || !updateGraphView);
9256  ++iter)
9257  {
9258  QString group = iter->split('/').first();
9259  if(!updateStereo && group == "Stereo")
9260  {
9261  updateStereo = true;
9262  continue;
9263  }
9264  if(!updateGraphView && group == "Optimize")
9265  {
9266  updateGraphView = true;
9267  continue;
9268  }
9269  }
9270 
9271  if(updateStereo)
9272  {
9273  this->updateStereo();
9274  }
9275  if(updateGraphView)
9276  {
9277  this->updateGraphView();
9278  }
9279  this->configModified();
9280 }
9281 
9282 } // namespace rtabmap
rtabmap::SensorData
Definition: SensorData.h:51
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
w
RowVector3d w
int
int
rtabmap::DBDriver::loadWords
void loadWords(const std::set< int > &wordIds, std::list< VisualWord * > &vws)
Definition: DBDriver.cpp:616
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:127
rtabmap::util3d::laserScanToPointCloudINormal
pcl::PointCloud< pcl::PointXYZINormal >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudINormal(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
Definition: util3d.cpp:2402
rtabmap::DatabaseViewer::updateLoggerLevel
void updateLoggerLevel()
Definition: DatabaseViewer.cpp:5693
rtabmap::CloudViewer::setCameraLockZ
void setCameraLockZ(bool enabled=true)
Definition: CloudViewer.cpp:3304
rtabmap::DatabaseViewer::refineLinks
void refineLinks()
Definition: DatabaseViewer.cpp:4444
rtabmap::util3d::cloudFromDepthRGB
RTABMAP_DEPRECATED pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT cloudFromDepthRGB(const cv::Mat &imageRgb, const cv::Mat &imageDepth, float cx, float cy, float fx, float fy, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0)
Definition: util3d.cpp:416
Compression.h
rtabmap::DBDriver::loadOptimizedPoses
std::map< int, Transform > loadOptimizedPoses(Transform *lastlocalizationPose=0) const
Definition: DBDriver.cpp:1197
rtabmap::DatabaseViewer::regenerateCurrentLocalMaps
void regenerateCurrentLocalMaps()
Definition: DatabaseViewer.cpp:3953
rtabmap::DBDriver::getNodeData
void getNodeData(int signatureId, SensorData &data, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const
Definition: DBDriver.cpp:685
rtabmap::SensorData::setLaserScan
void setLaserScan(const LaserScan &laserScan, bool clearPreviousData=true)
Definition: SensorData.cpp:381
rtabmap::DatabaseViewer::DatabaseViewer
DatabaseViewer(const QString &ini=QString(), QWidget *parent=0)
Definition: DatabaseViewer.cpp:109
rtabmap::CloudViewer::updateCameraTargetPosition
void updateCameraTargetPosition(const Transform &pose)
Definition: CloudViewer.cpp:2979
rtabmap::DatabaseViewer::generateGraph
void generateGraph()
Definition: DatabaseViewer.cpp:3701
rtabmap::DBDriver::getCalibration
bool getCalibration(int signatureId, std::vector< CameraModel > &models, std::vector< StereoCameraModel > &stereoModels) const
Definition: DBDriver.cpp:736
rtabmap::ProgressDialog
Definition: ProgressDialog.h:43
rtabmap::DBDriver::getNodesMemoryUsed
long getNodesMemoryUsed() const
Definition: DBDriver.cpp:119
UFile::getName
std::string getName()
Definition: UFile.h:135
gtsam::Values::insert
void insert(const Values &values)
rtabmap::Feature2D::create
static Feature2D * create(const ParametersMap &parameters=ParametersMap())
Definition: Features2d.cpp:569
save
save
rtabmap::DBDriver::updateLink
void updateLink(const Link &link)
Definition: DBDriver.cpp:477
UINFO
#define UINFO(...)
name
rtabmap::CloudViewer::isFrustumShown
bool isFrustumShown() const
Definition: CloudViewer.cpp:2471
rtabmap::StereoCameraModel
Definition: StereoCameraModel.h:35
rtabmap::DatabaseViewer::exportDatabase
void exportDatabase()
Definition: DatabaseViewer.cpp:1320
rtabmap::util3d::createPolygonIndexes
void RTABMAP_CORE_EXPORT createPolygonIndexes(const std::vector< pcl::Vertices > &polygons, int cloudSize, std::vector< std::set< int > > &neighborPolygons, std::vector< std::set< int > > &vertexPolygons)
Given a set of polygons, create two indexes: polygons to neighbor polygons and vertices to polygons.
Definition: util3d_surface.cpp:94
rtabmap::DBDriver::getLastParameters
ParametersMap getLastParameters() const
Definition: DBDriver.cpp:239
rtabmap::SensorData::uncompressData
void uncompressData()
Definition: SensorData.cpp:528
uMean
T uMean(const T *v, unsigned int size)
Definition: UMath.h:399
prior
prior
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:728
rtabmap::EditDepthArea::getModifiedImage
cv::Mat getModifiedImage() const
Definition: EditDepthArea.cpp:117
rtabmap::util3d::laserScanToPointCloud2
pcl::PCLPointCloud2::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloud2(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2230
rtabmap::DatabaseViewer::sliderBMoved
void sliderBMoved(int)
Definition: DatabaseViewer.cpp:5985
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:230
arg::arg
constexpr arg(const char *name=nullptr)
rtabmap::CloudViewer::removeOccupancyGridMap
void removeOccupancyGridMap()
Definition: CloudViewer.cpp:1595
rtabmap::DatabaseViewer::selectObstacleColor
void selectObstacleColor()
Definition: DatabaseViewer.cpp:2357
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:1270
rtabmap::LaserScan::rangeMax
float rangeMax() const
Definition: LaserScan.h:122
timer
rtabmap::DataRecorder::init
bool init(const QString &path, bool recordInRAM=true)
Definition: DataRecorder.cpp:67
s
RealScalar s
rtabmap::CloudViewer::addCloudTextureMesh
bool addCloudTextureMesh(const std::string &id, const pcl::TextureMesh::Ptr &textureMesh, const cv::Mat &texture, const Transform &pose=Transform::getIdentity())
Definition: CloudViewer.cpp:1074
rtabmap::DatabaseViewer::infoReducedGraph_
bool infoReducedGraph_
Definition: DatabaseViewer.h:252
rtabmap::DatabaseViewer::exportPoses
void exportPoses(int format)
Definition: DatabaseViewer.cpp:2451
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:996
rtabmap::DatabaseViewer::weights_
std::map< int, int > weights_
Definition: DatabaseViewer.h:217
rtabmap::DatabaseViewer::graphes_
std::list< std::map< int, rtabmap::Transform > > graphes_
Definition: DatabaseViewer.h:226
rtabmap::DatabaseViewer::editMapArea_
EditMapArea * editMapArea_
Definition: DatabaseViewer.h:245
b
Array< int, 3, 1 > b
rtabmap::DatabaseViewer::extractImages
void extractImages()
Definition: DatabaseViewer.cpp:1495
rtabmap::DBDriver::getImagesMemoryUsed
long getImagesMemoryUsed() const
Definition: DBDriver.cpp:135
rtabmap::DatabaseViewer::ui_
Ui_DatabaseViewer * ui_
Definition: DatabaseViewer.h:209
Horizontal
Horizontal
rtabmap::CloudViewer::addCloud
bool addCloud(const std::string &id, const pcl::PCLPointCloud2Ptr &binaryCloud, const Transform &pose, bool rgb, bool hasNormals, bool hasIntensity, const QColor &color=QColor(), int viewport=1)
Definition: CloudViewer.cpp:745
rtabmap::ExportCloudsDialog::forceAssembling
void forceAssembling(bool enabled)
Definition: ExportCloudsDialog.cpp:335
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:1463
rtabmap::CloudViewer::setPolygonPicking
void setPolygonPicking(bool enabled)
Definition: CloudViewer.cpp:2758
rtabmap::Registration
Definition: Registration.h:39
rtabmap::DBDriver::getLastNodesSize
int getLastNodesSize() const
Definition: DBDriver.cpp:207
rtabmap::CloudViewer::setCloudOpacity
void setCloudOpacity(const std::string &id, double opacity=1.0)
Definition: CloudViewer.cpp:3249
UFile::length
long length()
Definition: UFile.h:110
rtabmap::ExportDialog::sessionExported
int sessionExported() const
Definition: ExportDialog.cpp:140
rtabmap::ExportCloudsDialog::isGainRGB
bool isGainRGB() const
Definition: ExportCloudsDialog.cpp:1569
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:533
ExportDialog.h
rtabmap::DatabaseViewer::graphLinkSelected
void graphLinkSelected(int, int)
Definition: DatabaseViewer.cpp:4585
rtabmap::CloudViewer::removeAllLines
void removeAllLines()
Definition: CloudViewer.cpp:1866
rtabmap::RegistrationInfo
Definition: RegistrationInfo.h:34
UDirectory.h
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:411
size
Index size
rtabmap::CloudViewer::addOrUpdateGraph
void addOrUpdateGraph(const std::string &id, const pcl::PointCloud< pcl::PointXYZ >::Ptr &graph, const QColor &color=Qt::gray)
Definition: CloudViewer.cpp:2296
this
this
rtabmap::CameraModel
Definition: CameraModel.h:38
rtabmap::EditMapArea::getModifiedMap
cv::Mat getModifiedMap() const
Definition: EditMapArea.cpp:79
rtabmap::DBDriver::loadSignature
Signature * loadSignature(int id, bool *loadedFromTrash=0)
Definition: DBDriver.cpp:547
uStr2Bool
bool UTILITE_EXPORT uStr2Bool(const char *str)
Definition: UConversion.cpp:170
EditMapArea.h
rtabmap::OctoMap::createProjectionMap
cv::Mat createProjectionMap(float &xMin, float &yMin, float &gridCellSize, float minGridSize=0.0f, unsigned int treeDepth=0)
Definition: OctoMap.cpp:989
rtabmap::DatabaseViewer::notifyParametersChanged
void notifyParametersChanged(const QStringList &)
Definition: DatabaseViewer.cpp:9250
rtabmap::EditDepthArea::setColorMap
void setColorMap(uCvQtDepthColorMap type)
Definition: EditDepthArea.cpp:161
rtabmap::CloudViewer::removeGraph
void removeGraph(const std::string &id)
Definition: CloudViewer.cpp:2333
rtabmap::DatabaseViewer::updateGraphView
void updateGraphView()
Definition: DatabaseViewer.cpp:7345
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:232
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:613
rtabmap::DatabaseViewer::wmStates_
std::map< int, std::vector< int > > wmStates_
Definition: DatabaseViewer.h:218
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
rtabmap::DatabaseViewer::update
void update(int value, QLabel *labelIndex, QLabel *labelParents, QLabel *labelChildren, QLabel *weight, QLabel *label, QLabel *stamp, rtabmap::ImageView *view, QLabel *labelId, QLabel *labelMapId, QLabel *labelPose, QLabel *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:4647
GeodeticCoords.h
rtabmap::DatabaseViewer::generateLocalGraph
void generateLocalGraph()
Definition: DatabaseViewer.cpp:3716
rtabmap::DatabaseViewer::resetAllChanges
void resetAllChanges()
Definition: DatabaseViewer.cpp:4554
rtabmap::Optimizer::create
static Optimizer * create(const ParametersMap &parameters)
Definition: Optimizer.cpp:72
rtabmap::DatabaseViewer::closeEvent
virtual void closeEvent(QCloseEvent *event)
Definition: DatabaseViewer.cpp:1226
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:242
rtabmap::DatabaseViewer::selectFrontierColor
void selectFrontierColor()
Definition: DatabaseViewer.cpp:2381
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:1310
type
rtabmap::DatabaseViewer::showCloseButton
void showCloseButton(bool visible=true)
Definition: DatabaseViewer.cpp:534
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:6026
rtabmap::EnvSensor::kAmbientAirPressure
@ kAmbientAirPressure
Definition: EnvSensor.h:41
rtabmap::DatabaseViewer::sliderAMoved
void sliderAMoved(int)
Definition: DatabaseViewer.cpp:5972
rtabmap::EditConstraintDialog::getLinearVariance
double getLinearVariance() const
Definition: EditConstraintDialog.cpp:113
rtabmap::DatabaseViewer::constraintsViewer_
CloudViewer * constraintsViewer_
Definition: DatabaseViewer.h:210
rtabmap::GeodeticCoords
Definition: GeodeticCoords.h:52
rtabmap::DatabaseViewer::editDepthArea_
EditDepthArea * editDepthArea_
Definition: DatabaseViewer.h:243
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:249
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:2434
rtabmap::DatabaseViewer::updateOptimizedMesh
void updateOptimizedMesh()
Definition: DatabaseViewer.cpp:3568
rtabmap::DatabaseViewer::linksRemoved_
std::multimap< int, rtabmap::Link > linksRemoved_
Definition: DatabaseViewer.h:235
y
Matrix3f y
rtabmap::LocalGridCache
Definition: LocalGrid.h:56
rtabmap::ExportCloudsDialog::isExposeFusion
bool isExposeFusion() const
Definition: ExportCloudsDialog.cpp:1597
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:8031
rtabmap::KeypointItem
Definition: KeypointItem.h:41
rtabmap::DBDriver::loadNodeData
void loadNodeData(Signature *signature, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const
Definition: DBDriver.cpp:658
rtabmap::CloudMap::getMapGround
const pcl::PointCloud< pcl::PointXYZRGB >::Ptr & getMapGround() const
Definition: CloudMap.h:47
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:219
rtabmap::util3d::laserScanFromPointCloud
LaserScan laserScanFromPointCloud(const PointCloud2T &cloud, bool filterNaNs, bool is2D, const Transform &transform)
Definition: util3d.hpp:37
rtabmap::CloudViewer::removeAllFrustums
void removeAllFrustums(bool exceptCameraReference=false)
Definition: CloudViewer.cpp:2283
rtabmap::EnvSensor::kAmbientLight
@ kAmbientLight
Definition: EnvSensor.h:42
rtabmap::graph::calcRMSE
Transform RTABMAP_CORE_EXPORT calcRMSE(const std::map< int, Transform > &groundTruth, const std::map< int, Transform > &poses, float &translational_rmse, float &translational_mean, float &translational_median, float &translational_std, float &translational_min, float &translational_max, float &rotational_rmse, float &rotational_mean, float &rotational_median, float &rotational_std, float &rotational_min, float &rotational_max)
Definition: Graph.cpp:759
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:238
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:240
rtabmap::CloudViewer::addOctomap
bool addOctomap(const OctoMap *octomap, unsigned int treeDepth=0, bool volumeRepresentation=true)
Definition: CloudViewer.cpp:1113
rtabmap::GridMap
Definition: GridMap.h:42
rtabmap::CloudViewer::addCloudMesh
bool addCloudMesh(const std::string &id, const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const std::vector< pcl::Vertices > &polygons, const Transform &pose=Transform::getIdentity())
Definition: CloudViewer.cpp:923
rtabmap::DatabaseViewer::lastWmIds_
std::set< int > lastWmIds_
Definition: DatabaseViewer.h:215
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:539
rtabmap::DatabaseViewer::cloudViewer_
CloudViewer * cloudViewer_
Definition: DatabaseViewer.h:211
rtabmap::DatabaseViewer::groundTruthPoses_
std::map< int, rtabmap::Transform > groundTruthPoses_
Definition: DatabaseViewer.h:229
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:130
rtabmap::DatabaseViewer::mapIds_
std::map< int, int > mapIds_
Definition: DatabaseViewer.h:216
rtabmap::DatabaseViewer::sliderAValueChanged
void sliderAValueChanged(int)
Definition: DatabaseViewer.cpp:4593
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:500
rtabmap::ExportDialog::isOdomExported
bool isOdomExported() const
Definition: ExportDialog.cpp:160
rtabmap::SensorData::stereoCameraModels
const std::vector< StereoCameraModel > & stereoCameraModels() const
Definition: SensorData.h:241
rtabmap::DBDriver::save2DMap
void save2DMap(const cv::Mat &map, float xMin, float yMin, float cellSize) const
Definition: DBDriver.cpp:1205
indices
indices
rtabmap::Transform::getTranslationAndEulerAngles
void getTranslationAndEulerAngles(float &x, float &y, float &z, float &roll, float &pitch, float &yaw) const
Definition: Transform.cpp:258
rtabmap::util3d::organizedFastMesh
std::vector< pcl::Vertices > RTABMAP_CORE_EXPORT organizedFastMesh(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, double angleTolerance, bool quad, int trianglePixelSize, const Eigen::Vector3f &viewpoint=Eigen::Vector3f(0, 0, 0))
Definition: util3d_surface.cpp:168
rtabmap::CloudViewer::getFrustumColor
QColor getFrustumColor() const
Definition: CloudViewer.cpp:2481
rtabmap::EnvSensor::kAmbientTemperature
@ kAmbientTemperature
Definition: EnvSensor.h:40
rtabmap::DatabaseViewer::exportOptimizedMesh
void exportOptimizedMesh()
Definition: DatabaseViewer.cpp:3453
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:6194
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:348
rtabmap::DBDriver::getLastDictionarySize
int getLastDictionarySize() const
Definition: DBDriver.cpp:215
rtabmap::CloudViewer
Definition: CloudViewer.h:79
rtabmap::DatabaseViewer::updateAllNeighborCovariances
void updateAllNeighborCovariances()
Definition: DatabaseViewer.cpp:4327
rtabmap::EditConstraintDialog::getAngularVariance
double getAngularVariance() const
Definition: EditConstraintDialog.cpp:117
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:1143
rtabmap::ExportCloudsDialog::getTextureSize
int getTextureSize() const
Definition: ExportCloudsDialog.cpp:1548
rtabmap::util3d::mergeTextures
cv::Mat RTABMAP_CORE_EXPORT mergeTextures(pcl::TextureMesh &mesh, const std::map< int, cv::Mat > &images, const std::map< int, CameraModel > &calibrations, const Memory *memory=0, const DBDriver *dbDriver=0, int textureSize=4096, int textureCount=1, const std::vector< std::map< int, pcl::PointXY > > &vertexToPixels=std::vector< std::map< int, pcl::PointXY > >(), bool gainCompensation=true, float gainBeta=10.0f, bool gainRGB=true, bool blending=true, int blendingDecimation=0, int brightnessContrastRatioLow=0, int brightnessContrastRatioHigh=0, bool exposureFusion=false, const ProgressState *state=0, unsigned char blankValue=255, std::map< int, std::map< int, cv::Vec4d > > *gains=0, std::map< int, std::map< int, cv::Mat > > *blendingGains=0, std::pair< float, float > *contrastValues=0)
Definition: util3d_surface.cpp:1438
rtabmap::Parameters::getDefaultParameters
static const ParametersMap & getDefaultParameters()
Definition: Parameters.h:896
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:1280
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:649
uKeys
std::vector< K > uKeys(const std::multimap< K, V > &mm)
Definition: UStl.h:67
rtabmap::ExportCloudsDialog::getGainBeta
double getGainBeta() const
Definition: ExportCloudsDialog.cpp:1565
rtabmap::DatabaseViewer::~DatabaseViewer
virtual ~DatabaseViewer()
Definition: DatabaseViewer.cpp:509
rtabmap::DatabaseViewer::openDatabase
void openDatabase()
Definition: DatabaseViewer.cpp:811
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:240
rtabmap::util3d::cloudRGBFromSensorData
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT cloudRGBFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:1268
rtabmap::ProgressDialog::isCanceled
bool isCanceled() const
Definition: ProgressDialog.h:57
rtabmap::ProgressDialog::setCancelButtonVisible
void setCancelButtonVisible(bool visible)
Definition: ProgressDialog.cpp:97
RecoveryState.h
rtabmap::LocalGridMaker::getCellSize
float getCellSize() const
Definition: LocalGridMaker.h:49
rtabmap::DatabaseViewer::exportGPS_KML
void exportGPS_KML()
Definition: DatabaseViewer.cpp:2884
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:326
uStrNumCmp
int uStrNumCmp(const std::string &a, const std::string &b)
Definition: UStl.h:717
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:185
rtabmap::CloudViewer::addElevationMap
bool addElevationMap(const cv::Mat &map32FC1, float resolution, float xMin, float yMin, float opacity)
Definition: CloudViewer.cpp:1610
data
int data[]
rtabmap::DatabaseViewer::viewOptimizedMesh
void viewOptimizedMesh()
Definition: DatabaseViewer.cpp:3401
rtabmap::DatabaseViewer::updateConstraintButtons
void updateConstraintButtons()
Definition: DatabaseViewer.cpp:6859
rtabmap::Registration::isImageRequired
bool isImageRequired() const
Definition: Registration.cpp:90
rtabmap::CloudViewer::addOrUpdateFrustum
void addOrUpdateFrustum(const std::string &id, const Transform &pose, const Transform &localTransform, double scale, const QColor &color=QColor(), float fovX=1.1, float fovY=0.85)
Definition: CloudViewer.cpp:2146
delta
def delta(g0, g1)
util3d_transforms.h
rtabmap::DatabaseViewer::graphLinks_
std::multimap< int, rtabmap::Link > graphLinks_
Definition: DatabaseViewer.h:227
rtabmap::util3d::convertImage8U2Map
cv::Mat RTABMAP_CORE_EXPORT convertImage8U2Map(const cv::Mat &map8U, bool pgmFormat=false)
Definition: util3d_mapping.cpp:945
UDirectory::getDir
static std::string getDir(const std::string &filePath)
Definition: UDirectory.cpp:273
Features2d.h
rtabmap::DatabaseViewer::restoreDefaultSettings
void restoreDefaultSettings()
Definition: DatabaseViewer.cpp:751
rtabmap::DatabaseViewer::linksAdded_
std::multimap< int, rtabmap::Link > linksAdded_
Definition: DatabaseViewer.h:234
CloudMap.h
g
float g
j
std::ptrdiff_t j
rtabmap::DatabaseViewer::generate3DMap
void generate3DMap()
Definition: DatabaseViewer.cpp:4133
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:8004
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:2346
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:2442
rtabmap::LocalGridCache::clear
void clear(bool temporaryOnly=false)
Definition: LocalGrid.cpp:103
rtabmap::DatabaseViewer::ids_
QList< int > ids_
Definition: DatabaseViewer.h:214
rtabmap::DatabaseViewer::editMapDialog_
QDialog * editMapDialog_
Definition: DatabaseViewer.h:244
rtabmap::DatabaseViewer::updateStatistics
void updateStatistics()
Definition: DatabaseViewer.cpp:2310
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
rtabmap::CloudViewer::getFrustumScale
float getFrustumScale() const
Definition: CloudViewer.cpp:2476
CloudViewer.h
rtabmap::DatabaseViewer::updateStereo
void updateStereo()
Definition: DatabaseViewer.cpp:5701
rtabmap::GainCompensator
Definition: GainCompensator.h:44
rtabmap::DatabaseViewer::linksRefined_
std::multimap< int, rtabmap::Link > linksRefined_
Definition: DatabaseViewer.h:233
rtabmap::DatabaseViewer::keyPressEvent
virtual void keyPressEvent(QKeyEvent *event)
Definition: DatabaseViewer.cpp:1301
rtabmap::StereoDense
Definition: StereoDense.h:38
rtabmap::DatabaseViewer::updateInfo
void updateInfo()
Definition: DatabaseViewer.cpp:2176
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:1293
rtabmap::DatabaseViewer::savedMaximized_
bool savedMaximized_
Definition: DatabaseViewer.h:248
rtabmap::DatabaseViewer::recoverDatabase
void recoverDatabase()
Definition: DatabaseViewer.cpp:1197
rtabmap::GPS::bearing
const double & bearing() const
Definition: GPS.h:64
rtabmap::util3d::laserScanToPointCloudRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudRGB(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2322
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:133
rtabmap::CloudViewer::buildPickingLocator
void buildPickingLocator(bool enable)
Definition: CloudViewer.cpp:3571
Signature.h
rtabmap::DatabaseViewer::regenerateLocalMaps
void regenerateLocalMaps()
Definition: DatabaseViewer.cpp:3802
rtabmap::CloudViewer::removeCoordinate
void removeCoordinate(const std::string &id)
Definition: CloudViewer.cpp:1777
all
static const Eigen::internal::all_t all
GridMap.h
rtabmap::GPS::latitude
const double & latitude() const
Definition: GPS.h:61
rtabmap::DBDriver::getLastWordId
void getLastWordId(int &id) const
Definition: DBDriver.cpp:1003
rtabmap::GPS::longitude
const double & longitude() const
Definition: GPS.h:60
rtabmap::DBDriver::getUrl
const std::string & getUrl() const
Definition: DBDriver.h:72
arg
rtabmap::CloudViewer::addOrUpdateCoordinate
void addOrUpdateCoordinate(const std::string &id, const Transform &transform, double scale, bool foreground=false)
Definition: CloudViewer.cpp:1735
rtabmap::ExportCloudsDialog::getTextureBrightnessConstrastRatioLow
int getTextureBrightnessConstrastRatioLow() const
Definition: ExportCloudsDialog.cpp:1589
rtabmap::DatabaseViewer::exportPosesKML
void exportPosesKML()
Definition: DatabaseViewer.cpp:2446
rtabmap::util3d::isFinite
bool RTABMAP_CORE_EXPORT isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3327
Optimizer.h
rtabmap::DatabaseViewer::infoTotalOdom_
double infoTotalOdom_
Definition: DatabaseViewer.h:253
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:228
rtabmap::DatabaseViewer::graphNodeSelected
void graphNodeSelected(int)
Definition: DatabaseViewer.cpp:4572
rtabmap::GridMap::createTerrainMesh
pcl::PolygonMesh::Ptr createTerrainMesh() const
Definition: GridMap.cpp:161
rtabmap::DatabaseViewer::linkRefiningDialog_
LinkRefiningDialog * linkRefiningDialog_
Definition: DatabaseViewer.h:246
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:8561
rtabmap::util3d::laserScan2dFromPointCloud
LaserScan RTABMAP_CORE_EXPORT laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
Definition: util3d.cpp:1992
UPlot::addCurve
UPlotCurve * addCurve(const QString &curveName, const QColor &color=QColor())
Definition: UPlot.cpp:2134
rtabmap::DBDriver::getNodeInfo
bool getNodeInfo(int signatureId, Transform &pose, int &mapId, int &weight, std::string &label, double &stamp, Transform &groundTruthPose, std::vector< float > &velocity, GPS &gps, EnvSensors &sensors) const
Definition: DBDriver.cpp:786
info
else if n * info
rtabmap::ExportDialog::isUserDataExported
bool isUserDataExported() const
Definition: ExportDialog.cpp:165
rtabmap::graph::exportGPS
bool RTABMAP_CORE_EXPORT exportGPS(const std::string &filePath, const std::map< int, GPS > &gpsValues, unsigned int rgba=0xFFFFFFFF)
Definition: Graph.cpp:497
rtabmap::CloudViewer::addOrUpdateLine
void addOrUpdateLine(const std::string &id, const Transform &from, const Transform &to, const QColor &color, bool arrow=false, bool foreground=false)
Definition: CloudViewer.cpp:1810
UASSERT
#define UASSERT(condition)
rtabmap::DatabaseViewer::exportGPS
void exportGPS(int format)
Definition: DatabaseViewer.cpp:2889
params
SmartProjectionParams params(gtsam::HESSIAN, gtsam::ZERO_ON_DEGENERACY)
rtabmap::Feature2D
Definition: Features2d.h:106
z
z
rtabmap::DBDriver::getDatabaseVersion
std::string getDatabaseVersion() const
Definition: DBDriver.cpp:275
uValue
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
Definition: UStl.h:238
x
x
rtabmap::SensorData::setGroundTruth
void setGroundTruth(const Transform &pose)
Definition: SensorData.h:283
m
Matrix3f m
rtabmap::ExportDialog::isDepthExported
bool isDepthExported() const
Definition: ExportDialog.cpp:150
p
Point3_ p(2)
rtabmap::CloudViewer::setCloudColorIndex
void setCloudColorIndex(const std::string &id, int index)
Definition: CloudViewer.cpp:3241
rtabmap::DatabaseViewer::updateGrid
void updateGrid()
Definition: DatabaseViewer.cpp:7859
rtabmap::DatabaseViewer::getIniFilePath
QString getIniFilePath() const
Definition: DatabaseViewer.cpp:544
rtabmap::DBDriver::getLaserScanInfo
bool getLaserScanInfo(int signatureId, LaserScan &info) const
Definition: DBDriver.cpp:762
rtabmap::GPS::stamp
const double & stamp() const
Definition: GPS.h:59
rtabmap::CloudViewer::updateCameraFrustums
void updateCameraFrustums(const Transform &pose, const std::vector< CameraModel > &models)
Definition: CloudViewer.cpp:3115
rtabmap::OccupancyGrid::getMap
cv::Mat getMap(float &xMin, float &yMin) const
Definition: OccupancyGrid.cpp:99
rtabmap::GeodeticCoords::altitude
const double & altitude() const
Definition: GeodeticCoords.h:60
rtabmap::DBDriver::getAllStatistics
std::map< int, std::pair< std::map< std::string, float >, double > > getAllStatistics() const
Definition: DBDriver.cpp:257
Stereo.h
rtabmap::DBDriver::getWordsMemoryUsed
long getWordsMemoryUsed() const
Definition: DBDriver.cpp:183
DBDriver.h
rtabmap::Registration::isUserDataRequired
bool isUserDataRequired() const
Definition: Registration.cpp:110
rtabmap::Transform::getDistance
float getDistance(const Transform &t) const
Definition: Transform.cpp:293
rtabmap::Transform::setIdentity
void setIdentity()
Definition: Transform.cpp:157
rtabmap::EditConstraintDialog::getTransform
Transform getTransform() const
Definition: EditConstraintDialog.cpp:103
rtabmap::util3d::laserScanToPointCloudI
pcl::PointCloud< pcl::PointXYZI >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudI(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
Definition: util3d.cpp:2349
rtabmap::DBDriver::getCalibrationsMemoryUsed
long getCalibrationsMemoryUsed() const
Definition: DBDriver.cpp:151
rtabmap::ExportCloudsDialog::getBlendingDecimation
int getBlendingDecimation() const
Definition: ExportCloudsDialog.cpp:1577
rtabmap::Registration::isScanRequired
bool isScanRequired() const
Definition: Registration.cpp:100
util3d_mapping.h
rtabmap::DatabaseViewer::generatedLocalMaps_
LocalGridCache generatedLocalMaps_
Definition: DatabaseViewer.h:239
rtabmap::DatabaseViewer::stereoViewer_
CloudViewer * stereoViewer_
Definition: DatabaseViewer.h:212
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:213
time
#define time
rtabmap::DatabaseViewer::modifiedLaserScans_
std::map< int, LaserScan > modifiedLaserScans_
Definition: DatabaseViewer.h:236
rtabmap::ExportDialog
Definition: ExportDialog.h:40
rtabmap::util3d::cloudsRGBFromSensorData
std::vector< pcl::PointCloud< pcl::PointXYZRGB >::Ptr > RTABMAP_CORE_EXPORT cloudsRGBFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< pcl::IndicesPtr > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:1093
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
out
std::ofstream out("Result.txt")
rtabmap::graph::findLink
std::multimap< int, Link >::iterator RTABMAP_CORE_EXPORT findLink(std::multimap< int, Link > &links, int from, int to, bool checkBothWays=true, Link::Type type=Link::kUndef)
Definition: Graph.cpp:1024
rtabmap::LaserScan::angleMax
float angleMax() const
Definition: LaserScan.h:124
rtabmap::Optimizer::type
virtual Type type() const =0
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
rtabmap::OctoMap::hasColor
bool hasColor() const
Definition: global_map/OctoMap.h:203
graph
FactorGraph< FACTOR > * graph
rtabmap::SensorData::setFeatures
void setFeatures(const std::vector< cv::KeyPoint > &keypoints, const std::vector< cv::Point3f > &keypoints3D, const cv::Mat &descriptors)
Definition: SensorData.cpp:801
rtabmap::GainCompensator::apply
void apply(int id, pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, bool rgb=true) const
Definition: GainCompensator.cpp:426
rtabmap::DBDriver::loadLinks
void loadLinks(int signatureId, std::multimap< int, Link > &links, Link::Type type=Link::kUndef) const
Definition: DBDriver.cpp:825
rtabmap::Optimizer::optimize
std::map< int, Transform > optimize(int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints, std::list< std::map< int, Transform > > *intermediateGraphes=0, double *finalError=0, int *iterationsDone=0)
Definition: Optimizer.cpp:399
path
path
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
str
rtabmap::CloudViewer::removeOctomap
void removeOctomap()
Definition: CloudViewer.cpp:1324
rtabmap::LocalGridCache::add
void add(int nodeId, const cv::Mat &ground, const cv::Mat &obstacles, const cv::Mat &empty, float cellSize, const cv::Point3f &viewPoint=cv::Point3f(0, 0, 0))
Definition: LocalGrid.cpp:55
UWARN
#define UWARN(...)
rtabmap::ExportDialog::outputPath
QString outputPath() const
Definition: ExportDialog.cpp:125
UPlotCurve::addValue
void addValue(UPlotItem *data)
Definition: UPlot.cpp:423
rtabmap::EditDepthArea::setImage
void setImage(const cv::Mat &depth, const cv::Mat &rgb=cv::Mat())
Definition: EditDepthArea.cpp:78
rtabmap::DBDriver::getLastNodeIds
void getLastNodeIds(std::set< int > &ids) const
Definition: DBDriver.cpp:879
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
rtabmap::DatabaseViewer::exportDialog_
ExportCloudsDialog * exportDialog_
Definition: DatabaseViewer.h:241
rtabmap::CloudViewer::removeCloud
bool removeCloud(const std::string &id)
Definition: CloudViewer.cpp:2588
rtabmap::GridMap::createHeightMap
cv::Mat createHeightMap(float &xMin, float &yMin, float &cellSize) const
Definition: GridMap.cpp:57
rtabmap::DBDriver::getLinksMemoryUsed
long getLinksMemoryUsed() const
Definition: DBDriver.cpp:127
rtabmap::DatabaseViewer::updateAllLandmarkCovariances
void updateAllLandmarkCovariances()
Definition: DatabaseViewer.cpp:4343
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::DatabaseViewer::infoTotalTime_
double infoTotalTime_
Definition: DatabaseViewer.h:254
M_PI
#define M_PI
Definition: tango-gl/include/tango-gl/util.h:66
rtabmap::DatabaseViewer::lastSliderIndexBrowsed_
int lastSliderIndexBrowsed_
Definition: DatabaseViewer.h:222
rtabmap::DatabaseViewer::updateAllLoopClosureCovariances
void updateAllLoopClosureCovariances()
Definition: DatabaseViewer.cpp:4331
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:558
rtabmap::DatabaseViewer::exportPosesRGBDSLAMMotionCapture
void exportPosesRGBDSLAMMotionCapture()
Definition: DatabaseViewer.cpp:2422
rtabmap::DatabaseViewer::closeDatabase
bool closeDatabase()
Definition: DatabaseViewer.cpp:935
set::clear
void clear()
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:183
rtabmap::DatabaseViewer::iniFilePath_
QString iniFilePath_
Definition: DatabaseViewer.h:250
ULogger.h
ULogger class and convenient macros.
a
ArrayXXi a
rtabmap::DatabaseViewer::writeSettings
void writeSettings()
Definition: DatabaseViewer.cpp:652
UPlot.h
rtabmap::DatabaseViewer::updateCovariances
void updateCovariances(const QList< Link > &links)
Definition: DatabaseViewer.cpp:4356
rtabmap::util3d::assemblePolygonMesh
pcl::PolygonMesh::Ptr RTABMAP_CORE_EXPORT assemblePolygonMesh(const cv::Mat &cloudMat, const std::vector< std::vector< RTABMAP_PCL_INDEX > > &polygons)
Definition: util3d_surface.cpp:1395
rtabmap::Transform
Definition: Transform.h:41
rtabmap::util3d::cloudFromSensorData
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT cloudFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:1040
rtabmap::CloudViewer::addOccupancyGridMap
bool addOccupancyGridMap(const cv::Mat &map8U, float resolution, float xMin, float yMin, float opacity)
Definition: CloudViewer.cpp:1530
Memory.h
empty
rtabmap::DatabaseViewer::regenerateSavedMap
void regenerateSavedMap()
Definition: DatabaseViewer.cpp:3308
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:896
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:2373
rtabmap::ExportCloudsDialog::isBlending
bool isBlending() const
Definition: ExportCloudsDialog.cpp:1573
rtabmap::EnvSensors
std::map< EnvSensor::Type, EnvSensor > EnvSensors
Definition: EnvSensor.h:81
uStr2Int
int UTILITE_EXPORT uStr2Int(const std::string &str)
Definition: UConversion.cpp:125
glm::rotation
GLM_FUNC_DECL detail::tquat< T, P > rotation(detail::tvec3< T, P > const &orig, detail::tvec3< T, P > const &dest)
rtabmap::Parameters::readINI
static void readINI(const std::string &configFile, ParametersMap &parameters, bool modifiedOnly=false)
Definition: Parameters.cpp:1273
UPlotCurve
Definition: UPlot.h:93
rtabmap::DatabaseViewer::import2DMap
void import2DMap()
Definition: DatabaseViewer.cpp:3253
Graph.h
rtabmap::CloudViewer::clear
virtual void clear()
Definition: CloudViewer.cpp:261
rtabmap::ExportCloudsDialog::isGainCompensation
bool isGainCompensation() const
Definition: ExportCloudsDialog.cpp:1561
rtabmap::DatabaseViewer::gpsValues_
std::map< int, GPS > gpsValues_
Definition: DatabaseViewer.h:231
rtabmap::DatabaseViewer::exportGPS_TXT
void exportGPS_TXT()
Definition: DatabaseViewer.cpp:2880
rtabmap::DatabaseViewer::databaseFileName_
std::string databaseFileName_
Definition: DatabaseViewer.h:225
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
rtabmap::DatabaseViewer::resetConstraint
void resetConstraint()
Definition: DatabaseViewer.cpp:9006
values
leaf::MyValues values
rtabmap::DatabaseViewer::rejectConstraint
void rejectConstraint()
Definition: DatabaseViewer.cpp:9042
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:255
rtabmap::Transform::rotation
Transform rotation() const
Definition: Transform.cpp:195
rtabmap::DBDriver::loadOptimizedMesh
cv::Mat loadOptimizedMesh(std::vector< std::vector< std::vector< RTABMAP_PCL_INDEX > > > *polygons=0, std::vector< std::vector< Eigen::Vector2f > > *texCoords=0, cv::Mat *textures=0) const
Definition: DBDriver.cpp:1235
ULOGGER_ERROR
#define ULOGGER_ERROR(...)
Definition: ULogger.h:56
rtabmap::DatabaseViewer::exportPosesRaw
void exportPosesRaw()
Definition: DatabaseViewer.cpp:2418
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:2922
rtabmap::DatabaseViewer::selectGroundColor
void selectGroundColor()
Definition: DatabaseViewer.cpp:2365
rtabmap::Optimizer::kTypeTORO
@ kTypeTORO
Definition: Optimizer.h:66
rtabmap::DatabaseViewer::refineConstraint
void refineConstraint()
Definition: DatabaseViewer.cpp:8036
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:1094
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:1002
rtabmap::Parameters::getType
static std::string getType(const std::string &paramKey)
Definition: Parameters.cpp:470
rtabmap::ProgressDialog::incrementStep
void incrementStep(int steps=1)
Definition: ProgressDialog.cpp:138
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
RegistrationIcp.h
rtabmap::util3d::projectDisparityTo3D
cv::Point3f RTABMAP_CORE_EXPORT projectDisparityTo3D(const cv::Point2f &pt, float disparity, const StereoCameraModel &model)
Definition: util3d.cpp:2653
UTimer
Definition: UTimer.h:46
rtabmap::DatabaseViewer::updateWordsMatching
void updateWordsMatching(const std::vector< int > &inliers=std::vector< int >())
Definition: DatabaseViewer.cpp:5890
rtabmap::LaserScan::angleIncrement
float angleIncrement() const
Definition: LaserScan.h:125
rtabmap::DatabaseViewer::updateIds
void updateIds()
Definition: DatabaseViewer.cpp:1702
rtabmap::ExportCloudsDialog::getTextureBrightnessConstrastRatioHigh
int getTextureBrightnessConstrastRatioHigh() const
Definition: ExportCloudsDialog.cpp:1593
rtabmap::Stereo::create
static Stereo * create(const ParametersMap &parameters=ParametersMap())
Definition: Stereo.cpp:35
rtabmap::OccupancyGrid::getProbMap
cv::Mat getProbMap(float &xMin, float &yMin) const
Definition: OccupancyGrid.cpp:142
rtabmap::DBDriver::load2DMap
cv::Mat load2DMap(float &xMin, float &yMin, float &cellSize) const
Definition: DBDriver.cpp:1212
rtabmap::DatabaseViewer::sliderIterationsValueChanged
void sliderIterationsValueChanged(int)
Definition: DatabaseViewer.cpp:6929
rtabmap::ImageView
Definition: ImageView.h:49
rtabmap::CloudViewer::setCameraFree
void setCameraFree()
Definition: CloudViewer.cpp:3298
rtabmap::DatabaseViewer::loopLinks_
QList< rtabmap::Link > loopLinks_
Definition: DatabaseViewer.h:221
rtabmap::CloudViewer::removeAllClouds
void removeAllClouds()
Definition: CloudViewer.cpp:2575
rtabmap::DatabaseViewer::dbDriver_
rtabmap::DBDriver * dbDriver_
Definition: DatabaseViewer.h:223
float
float
rtabmap::DatabaseViewer::updateLoopClosuresSlider
void updateLoopClosuresSlider(int from=0, int to=0)
Definition: DatabaseViewer.cpp:9183
rtabmap::CloudViewer::removeElevationMap
void removeElevationMap()
Definition: CloudViewer.cpp:1727
rtabmap::util3d::laserScanToPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2269
rtabmap::DatabaseViewer::neighborLinks_
QList< rtabmap::Link > neighborLinks_
Definition: DatabaseViewer.h:220
rtabmap::StereoDense::create
static StereoDense * create(const ParametersMap &parameters)
Definition: StereoDense.cpp:33
rtabmap::RecoveryState
Definition: RecoveryState.h:39
rtabmap::DBDriver::updateLaserScan
void updateLaserScan(int nodeId, const LaserScan &scan)
Definition: DBDriver.cpp:524
rtabmap::Transform::toEigen3d
Eigen::Affine3d toEigen3d() const
Definition: Transform.cpp:396
rtabmap::DatabaseViewer::update3dView
void update3dView()
Definition: DatabaseViewer.cpp:5998
rtabmap::GPS::toGeodeticCoords
GeodeticCoords toGeodeticCoords() const
Definition: GPS.h:66
rtabmap::DBDriver::getLaserScansMemoryUsed
long getLaserScansMemoryUsed() const
Definition: DBDriver.cpp:167
DataRecorder.h
rtabmap::OctoMap
Definition: global_map/OctoMap.h:175
ULogger::Level
Level
Definition: ULogger.h:252
rtabmap::DBDriver::getAllNodeIds
void getAllNodeIds(std::set< int > &ids, bool ignoreChildren=false, bool ignoreBadSignatures=false, bool ignoreIntermediateNodes=false) const
Definition: DBDriver.cpp:886
uMax
T uMax(const T *v, unsigned int size, unsigned int &index)
Definition: UMath.h:92
Vector2::x
float x
rtabmap::GeodeticCoords::longitude
const double & longitude() const
Definition: GeodeticCoords.h:59
rtabmap::DatabaseViewer::updateGraphRotation
void updateGraphRotation()
Definition: DatabaseViewer.cpp:7331
rtabmap::LaserScan::hasIntensity
bool hasIntensity() const
Definition: LaserScan.h:136
rtabmap::DatabaseViewer::detectMoreLoopClosures
void detectMoreLoopClosures()
Definition: DatabaseViewer.cpp:4186
false
#define false
Definition: ConvertUTF.c:56
transform
EIGEN_DONT_INLINE void transform(const Quaternion< Scalar > &t, Data &data)
uIsDigit
bool uIsDigit(const char c)
Definition: UStl.h:620
OccupancyGrid.h
uCvMat2QImage
QImage uCvMat2QImage(const cv::Mat &image, bool isBgr=true, uCvQtDepthColorMap colorMap=uCvQtDepthWhiteToBlack, float depthMin=0, float depthMax=0)
Definition: UCv2Qt.h:47
rtabmap::DBDriver::updateDepthImage
void updateDepthImage(int nodeId, const cv::Mat &image)
Definition: DBDriver.cpp:515
uValues
std::vector< V > uValues(const std::multimap< K, V > &mm)
Definition: UStl.h:100
rtabmap::SensorData::depthOrRightRaw
const cv::Mat & depthOrRightRaw() const
Definition: SensorData.h:184
rtabmap::LaserScan::dataType
int dataType() const
Definition: LaserScan.h:132
UPlot
Definition: UPlot.h:492
rtabmap::DBDriver::generateGraph
void generateGraph(const std::string &fileName, const std::set< int > &ids=std::set< int >(), const std::map< int, Signature * > &otherSignatures=std::map< int, Signature * >())
Definition: DBDriver.cpp:1250
rtabmap::CloudViewer::refreshView
void refreshView()
Definition: CloudViewer.cpp:538
rtabmap::DatabaseViewer::odomMaxInf_
std::vector< double > odomMaxInf_
Definition: DatabaseViewer.h:237
rtabmap::SensorData::setGPS
void setGPS(const GPS &gps)
Definition: SensorData.h:290
rtabmap::util3d::convertMap2Image8U
cv::Mat RTABMAP_CORE_EXPORT convertMap2Image8U(const cv::Mat &map8S, bool pgmFormat=false)
Definition: util3d_mapping.cpp:904
rtabmap::Transform::clone
Transform clone() const
Definition: Transform.cpp:102
rtabmap::DBDriver::saveOptimizedPoses
void saveOptimizedPoses(const std::map< int, Transform > &optimizedPoses, const Transform &lastlocalizationPose) const
Definition: DBDriver.cpp:1191
rtabmap::util3d::fixTextureMeshForVisualization
void RTABMAP_CORE_EXPORT fixTextureMeshForVisualization(pcl::TextureMesh &textureMesh)
Definition: util3d_surface.cpp:2195
uStr2Float
float UTILITE_EXPORT uStr2Float(const std::string &str)
Definition: UConversion.cpp:138
rtabmap::DatabaseViewer::setupMainLayout
void setupMainLayout(bool vertical)
Definition: DatabaseViewer.cpp:518
rtabmap::DatabaseViewer::exportSaved2DMap
void exportSaved2DMap()
Definition: DatabaseViewer.cpp:3198
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:1351
rtabmap::DBDriver::getTotalDictionarySize
int getTotalDictionarySize() const
Definition: DBDriver.cpp:231
t
Point2 t(10, 10)
rtabmap::Optimizer::isCovarianceIgnored
bool isCovarianceIgnored() const
Definition: Optimizer.h:92
rtabmap::Transform::translation
Transform translation() const
Definition: Transform.cpp:203
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:138
rtabmap::util3d::laserScanToPointCloudRGBNormal
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudRGBNormal(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2376
rtabmap::ProgressDialog::setMaximumSteps
void setMaximumSteps(int steps)
Definition: ProgressDialog.cpp:133
rtabmap::DatabaseViewer::updateOctomapView
void updateOctomapView()
Definition: DatabaseViewer.cpp:7883
rtabmap::ExportCloudsDialog::saveSettings
void saveSettings(QSettings &settings, const QString &group="") const
Definition: ExportCloudsDialog.cpp:353
rtabmap::CloudViewer::setCloudPointSize
void setCloudPointSize(const std::string &id, int size)
Definition: CloudViewer.cpp:3276
rtabmap::ExportCloudsDialog::getMaxTextures
int getMaxTextures() const
Definition: ExportCloudsDialog.cpp:1557
UFile.h
rtabmap::util3d::transformFromXYZCorrespondencesSVD
Transform RTABMAP_CORE_EXPORT transformFromXYZCorrespondencesSVD(const pcl::PointCloud< pcl::PointXYZ > &cloud1, const pcl::PointCloud< pcl::PointXYZ > &cloud2)
Definition: util3d_registration.cpp:50
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::DBDriver::getGridsMemoryUsed
long getGridsMemoryUsed() const
Definition: DBDriver.cpp:159
rtabmap::LocalGridCache::size
size_t size() const
Definition: LocalGrid.h:76
rtabmap::Stereo::computeCorrespondences
virtual std::vector< cv::Point2f > computeCorrespondences(const cv::Mat &leftImage, const cv::Mat &rightImage, const std::vector< cv::Point2f > &leftCorners, std::vector< unsigned char > &status) const
Definition: Stereo.cpp:72
rtabmap::util3d::laserScanToPointCloudNormal
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudNormal(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2296
rtabmap::DatabaseViewer::editDepthImage
void editDepthImage()
Definition: DatabaseViewer.cpp:2389
rtabmap::DatabaseViewer::exportPosesTORO
void exportPosesTORO()
Definition: DatabaseViewer.cpp:2438
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:4620
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:6018
value
value
rtabmap::DBDriver::loadSignatures
void loadSignatures(const std::list< int > &ids, std::list< Signature * > &signatures, std::set< int > *loadedFromTrash=0)
Definition: DBDriver.cpp:564
i
int i
rtabmap::Parameters::writeINI
static void writeINI(const std::string &configFile, const ParametersMap &parameters)
Definition: Parameters.cpp:1287
rtabmap::SensorData::setEnvSensors
void setEnvSensors(const EnvSensors &sensors)
Definition: SensorData.h:296
rtabmap::DBDriver::getAllLinks
void getAllLinks(std::multimap< int, Link > &links, bool ignoreNullLinks=true, bool withLandmarks=false) const
Definition: DBDriver.cpp:925
StereoDense.h
rtabmap::DatabaseViewer::sliderNeighborValueChanged
void sliderNeighborValueChanged(int)
Definition: DatabaseViewer.cpp:6010
rtabmap::LaserScan::rangeMin
float rangeMin() const
Definition: LaserScan.h:121
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
rtabmap::EditMapArea::setMap
void setMap(const cv::Mat &map)
Definition: EditMapArea.cpp:67
rtabmap::CloudViewer::clearTrajectory
void clearTrajectory()
Definition: CloudViewer.cpp:2435
rtabmap::DatabaseViewer::pathDatabase_
QString pathDatabase_
Definition: DatabaseViewer.h:224
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:9133
rtabmap::Signature
Definition: Signature.h:48
rtabmap::util3d::cloudsFromSensorData
std::vector< pcl::PointCloud< pcl::PointXYZ >::Ptr > RTABMAP_CORE_EXPORT cloudsFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< pcl::IndicesPtr > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:854
Recovery.h
rtabmap::DatabaseViewer::exportPosesRGBDSLAMID
void exportPosesRGBDSLAMID()
Definition: DatabaseViewer.cpp:2430
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:4080
rtabmap::DBDriver::saveOptimizedMesh
void saveOptimizedMesh(const cv::Mat &cloud, const std::vector< std::vector< std::vector< RTABMAP_PCL_INDEX > > > &polygons=std::vector< std::vector< std::vector< RTABMAP_PCL_INDEX > > >(), const std::vector< std::vector< Eigen::Vector2f > > &texCoords=std::vector< std::vector< Eigen::Vector2f > >(), const cv::Mat &textures=cv::Mat()) const
Definition: DBDriver.cpp:1220
rtabmap::EditDepthArea
Definition: EditDepthArea.h:45
rtabmap::EditDepthArea::isModified
bool isModified() const
Definition: EditDepthArea.h:54
rtabmap::ProgressDialog::maximumSteps
int maximumSteps() const
Definition: ProgressDialog.cpp:129
v2
v2
rtabmap::EnvSensor::kWifiSignalStrength
@ kWifiSignalStrength
Definition: EnvSensor.h:39
msg
msg
rtabmap::ExportCloudsDialog::viewClouds
void viewClouds(const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, const std::map< int, int > &mapIds, const QMap< int, Signature > &cachedSignatures, const std::map< int, std::pair< pcl::PointCloud< pcl::PointXYZRGB >::Ptr, pcl::IndicesPtr > > &cachedClouds, const std::map< int, LaserScan > &cachedScans, const QString &workingDirectory, const ParametersMap &parameters)
Definition: ExportCloudsDialog.cpp:1205
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:1625
rtabmap::DBDriver::create
static DBDriver * create(const ParametersMap &parameters=ParametersMap())
Definition: DBDriver.cpp:41
rtabmap::DatabaseViewer::exportPosesRGBDSLAM
void exportPosesRGBDSLAM()
Definition: DatabaseViewer.cpp:2426
EditDepthArea.h

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:08