DatabaseViewer.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include "rtabmap/gui/DatabaseViewer.h"
29 #include "rtabmap/gui/CloudViewer.h"
30 #include "ui_DatabaseViewer.h"
31 #include <QMessageBox>
32 #include <QFileDialog>
33 #include <QInputDialog>
34 #include <QDesktopWidget>
35 #include <QColorDialog>
36 #include <QGraphicsLineItem>
37 #include <QtGui/QCloseEvent>
38 #include <QGraphicsOpacityEffect>
39 #include <QtCore/QBuffer>
40 #include <QtCore/QTextStream>
41 #include <QtCore/QDateTime>
42 #include <QtCore/QSettings>
43 #include <QThread>
44 #include <rtabmap/utilite/ULogger.h>
45 #include <rtabmap/utilite/UDirectory.h>
46 #include <rtabmap/utilite/UConversion.h>
47 #include <opencv2/core/core_c.h>
48 #include <opencv2/imgproc/types_c.h>
49 #include <opencv2/highgui/highgui.hpp>
50 #include <rtabmap/utilite/UTimer.h>
51 #include <rtabmap/utilite/UFile.h>
52 #include "rtabmap/utilite/UPlot.h"
53 #include "rtabmap/core/DBDriver.h"
54 #include "rtabmap/gui/KeypointItem.h"
55 #include "rtabmap/gui/CloudViewer.h"
56 #include "rtabmap/utilite/UCv2Qt.h"
57 #include "rtabmap/core/util3d.h"
58 #include "rtabmap/core/util3d_transforms.h"
59 #include "rtabmap/core/util3d_filtering.h"
60 #include "rtabmap/core/util3d_surface.h"
61 #include "rtabmap/core/util3d_registration.h"
62 #include "rtabmap/core/util3d_mapping.h"
63 #include "rtabmap/core/util2d.h"
64 #include "rtabmap/core/Signature.h"
65 #include "rtabmap/core/Memory.h"
66 #include "rtabmap/core/Features2d.h"
67 #include "rtabmap/core/Compression.h"
68 #include "rtabmap/core/Graph.h"
69 #include "rtabmap/core/Stereo.h"
70 #include "rtabmap/core/StereoDense.h"
71 #include "rtabmap/core/Optimizer.h"
72 #include "rtabmap/core/RegistrationVis.h"
73 #include "rtabmap/core/RegistrationIcp.h"
74 #include "rtabmap/core/OccupancyGrid.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/ProgressDialog.h"
87 #include "rtabmap/gui/ParametersToolBox.h"
88 #include "rtabmap/gui/RecoveryState.h"
89 #include <pcl/io/pcd_io.h>
90 #include <pcl/io/ply_io.h>
91 #include <pcl/io/obj_io.h>
92 #include <pcl/filters/voxel_grid.h>
93 #include <pcl/filters/crop_box.h>
94 #include <pcl/common/transforms.h>
95 #include <pcl/common/common.h>
96 
97 #ifdef RTABMAP_OCTOMAP
98 #include "rtabmap/core/OctoMap.h"
99 #endif
100 
101 namespace rtabmap {
102 
103 DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
104  QMainWindow(parent),
105  dbDriver_(0),
106  octomap_(0),
107  exportDialog_(new ExportCloudsDialog(this)),
108  editDepthDialog_(new QDialog(this)),
109  editMapDialog_(new QDialog(this)),
110  savedMaximized_(false),
111  firstCall_(true),
112  iniFilePath_(ini),
113  infoReducedGraph_(false),
114  infoTotalOdom_(0.0),
115  infoSessions_(0)
116 {
117  pathDatabase_ = QDir::homePath()+"/Documents/RTAB-Map"; //use home directory by default
118 
119  if(!UDirectory::exists(pathDatabase_.toStdString()))
120  {
121  pathDatabase_ = QDir::homePath();
122  }
123 
124  ui_ = new Ui_DatabaseViewer();
125  ui_->setupUi(this);
126  ui_->buttonBox->setVisible(false);
127  connect(ui_->buttonBox->button(QDialogButtonBox::Close), SIGNAL(clicked()), this, SLOT(close()));
128 
129  ui_->comboBox_logger_level->setVisible(parent==0);
130  ui_->label_logger_level->setVisible(parent==0);
131  connect(ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(int)), this, SLOT(updateLoggerLevel()));
132  connect(ui_->actionVertical_Layout, SIGNAL(toggled(bool)), this, SLOT(setupMainLayout(bool)));
133 
134  editDepthDialog_->resize(640, 480);
135  QVBoxLayout * vLayout = new QVBoxLayout(editDepthDialog_);
136  editDepthArea_ = new EditDepthArea(editDepthDialog_);
137  vLayout->setContentsMargins(0,0,0,0);
138  vLayout->setSpacing(0);
139  vLayout->addWidget(editDepthArea_, 1);
140  QDialogButtonBox * buttonBox = new QDialogButtonBox(QDialogButtonBox::Save | QDialogButtonBox::Cancel | QDialogButtonBox::Reset, Qt::Horizontal, editDepthDialog_);
141  vLayout->addWidget(buttonBox);
142  connect(buttonBox, SIGNAL(accepted()), editDepthDialog_, SLOT(accept()));
143  connect(buttonBox, SIGNAL(rejected()), editDepthDialog_, SLOT(reject()));
144  connect(buttonBox->button(QDialogButtonBox::Reset), SIGNAL(clicked()), editDepthArea_, SLOT(resetChanges()));
145  editDepthDialog_->setLayout(vLayout);
146  editDepthDialog_->setWindowTitle(tr("Edit Depth Image"));
147 
148  editMapDialog_->resize(640, 480);
149  vLayout = new QVBoxLayout(editMapDialog_);
150  editMapArea_ = new EditMapArea(editMapDialog_);
151  vLayout->setContentsMargins(0,0,0,0);
152  vLayout->setSpacing(0);
153  vLayout->addWidget(editMapArea_, 1);
154  buttonBox = new QDialogButtonBox(QDialogButtonBox::Save | QDialogButtonBox::Cancel | QDialogButtonBox::Reset, Qt::Horizontal, editMapDialog_);
155  vLayout->addWidget(buttonBox);
156  connect(buttonBox, SIGNAL(accepted()), editMapDialog_, SLOT(accept()));
157  connect(buttonBox, SIGNAL(rejected()), editMapDialog_, SLOT(reject()));
158  connect(buttonBox->button(QDialogButtonBox::Reset), SIGNAL(clicked()), editMapArea_, SLOT(resetChanges()));
159  editMapDialog_->setLayout(vLayout);
160  editMapDialog_->setWindowTitle(tr("Edit Optimized Map"));
161 
162  QString title("RTAB-Map Database Viewer[*]");
163  this->setWindowTitle(title);
164 
165  ui_->dockWidget_constraints->setVisible(false);
166  ui_->dockWidget_graphView->setVisible(false);
167  ui_->dockWidget_occupancyGridView->setVisible(false);
168  ui_->dockWidget_guiparameters->setVisible(false);
169  ui_->dockWidget_coreparameters->setVisible(false);
170  ui_->dockWidget_info->setVisible(false);
171  ui_->dockWidget_stereoView->setVisible(false);
172  ui_->dockWidget_view3d->setVisible(false);
173  ui_->dockWidget_statistics->setVisible(false);
174 
175  // Create cloud viewers
176  constraintsViewer_ = new CloudViewer(ui_->dockWidgetContents);
177  cloudViewer_ = new CloudViewer(ui_->dockWidgetContents_3dviews);
178  stereoViewer_ = new CloudViewer(ui_->dockWidgetContents_stereo);
179  occupancyGridViewer_ = new CloudViewer(ui_->dockWidgetContents_occupancyGrid);
180  constraintsViewer_->setObjectName("constraintsViewer");
181  cloudViewer_->setObjectName("cloudViewerA");
182  stereoViewer_->setObjectName("stereoViewer");
183  occupancyGridViewer_->setObjectName("occupancyGridView");
184  ui_->layout_constraintsViewer->addWidget(constraintsViewer_);
185  ui_->horizontalLayout_3dviews->addWidget(cloudViewer_, 1);
186  ui_->horizontalLayout_stereo->addWidget(stereoViewer_, 1);
187  ui_->layout_occupancyGridView->addWidget(occupancyGridViewer_, 1);
188 
189  constraintsViewer_->setCameraLockZ(false);
190  constraintsViewer_->setCameraFree();
191  occupancyGridViewer_->setCameraFree();
192 
193  ui_->graphicsView_stereo->setAlpha(255);
194 
195 #ifndef RTABMAP_OCTOMAP
196  ui_->checkBox_octomap->setEnabled(false);
197  ui_->checkBox_octomap->setChecked(false);
198 #endif
199 
200  ParametersMap parameters;
201  uInsert(parameters, Parameters::getDefaultParameters("SURF"));
202  uInsert(parameters, Parameters::getDefaultParameters("SIFT"));
203  uInsert(parameters, Parameters::getDefaultParameters("BRIEF"));
204  uInsert(parameters, Parameters::getDefaultParameters("FAST"));
205  uInsert(parameters, Parameters::getDefaultParameters("GFTT"));
206  uInsert(parameters, Parameters::getDefaultParameters("ORB"));
207  uInsert(parameters, Parameters::getDefaultParameters("FREAK"));
208  uInsert(parameters, Parameters::getDefaultParameters("BRISK"));
209  uInsert(parameters, Parameters::getDefaultParameters("KAZE"));
210  uInsert(parameters, Parameters::getDefaultParameters("SuperPoint"));
211  uInsert(parameters, Parameters::getDefaultParameters("Optimizer"));
212  uInsert(parameters, Parameters::getDefaultParameters("g2o"));
213  uInsert(parameters, Parameters::getDefaultParameters("GTSAM"));
214  uInsert(parameters, Parameters::getDefaultParameters("Reg"));
215  uInsert(parameters, Parameters::getDefaultParameters("Vis"));
216  uInsert(parameters, Parameters::getDefaultParameters("Icp"));
217  uInsert(parameters, Parameters::getDefaultParameters("PyMatcher"));
218  uInsert(parameters, Parameters::getDefaultParameters("Stereo"));
219  uInsert(parameters, Parameters::getDefaultParameters("StereoBM"));
220  uInsert(parameters, Parameters::getDefaultParameters("StereoSGBM"));
221  uInsert(parameters, Parameters::getDefaultParameters("Grid"));
222  uInsert(parameters, Parameters::getDefaultParameters("GridGlobal"));
223  uInsert(parameters, Parameters::getDefaultParameters("Marker"));
224  parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDOptimizeMaxError()));
225  parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDLoopClosureReextractFeatures()));
226  parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDLoopCovLimited()));
227  parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kRGBDProximityPathFilteringRadius()));
228  ui_->parameters_toolbox->setupUi(parameters);
229  exportDialog_->setObjectName("ExportCloudsDialog");
230  restoreDefaultSettings();
231  this->readSettings();
232 
233  setupMainLayout(ui_->actionVertical_Layout->isChecked());
234  ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked());
235  ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
236  ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
237  ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
238  ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
239  ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
240 
241  ui_->menuView->addAction(ui_->dockWidget_constraints->toggleViewAction());
242  ui_->menuView->addAction(ui_->dockWidget_graphView->toggleViewAction());
243  ui_->menuView->addAction(ui_->dockWidget_occupancyGridView->toggleViewAction());
244  ui_->menuView->addAction(ui_->dockWidget_stereoView->toggleViewAction());
245  ui_->menuView->addAction(ui_->dockWidget_view3d->toggleViewAction());
246  ui_->menuView->addAction(ui_->dockWidget_guiparameters->toggleViewAction());
247  ui_->menuView->addAction(ui_->dockWidget_coreparameters->toggleViewAction());
248  ui_->menuView->addAction(ui_->dockWidget_info->toggleViewAction());
249  ui_->menuView->addAction(ui_->dockWidget_statistics->toggleViewAction());
250  connect(ui_->dockWidget_graphView->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView()));
251  connect(ui_->dockWidget_occupancyGridView->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView()));
252  connect(ui_->dockWidget_statistics->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateStatistics()));
253  connect(ui_->dockWidget_info->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateInfo()));
254 
255 
256  connect(ui_->parameters_toolbox, SIGNAL(parametersChanged(const QStringList &)), this, SLOT(notifyParametersChanged(const QStringList &)));
257 
258  connect(ui_->actionQuit, SIGNAL(triggered()), this, SLOT(close()));
259 
260  ui_->actionOpen_database->setEnabled(true);
261  ui_->actionClose_database->setEnabled(false);
262 
263  // connect actions with custom slots
264  ui_->actionSave_config->setShortcut(QKeySequence::Save);
265  connect(ui_->actionSave_config, SIGNAL(triggered()), this, SLOT(writeSettings()));
266  connect(ui_->actionOpen_database, SIGNAL(triggered()), this, SLOT(openDatabase()));
267  connect(ui_->actionClose_database, SIGNAL(triggered()), this, SLOT(closeDatabase()));
268  connect(ui_->actionDatabase_recovery, SIGNAL(triggered()), this, SLOT(recoverDatabase()));
269  connect(ui_->actionExport, SIGNAL(triggered()), this, SLOT(exportDatabase()));
270  connect(ui_->actionExtract_images, SIGNAL(triggered()), this, SLOT(extractImages()));
271  connect(ui_->actionEdit_depth_image, SIGNAL(triggered()), this, SLOT(editDepthImage()));
272  connect(ui_->actionGenerate_graph_dot, SIGNAL(triggered()), this, SLOT(generateGraph()));
273  connect(ui_->actionGenerate_local_graph_dot, SIGNAL(triggered()), this, SLOT(generateLocalGraph()));
274  connect(ui_->actionRaw_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRaw()));
275  connect(ui_->actionRGBD_SLAM_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAM()));
276  connect(ui_->actionRGBD_SLAM_ID_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAMID()));
277  connect(ui_->actionRGBD_SLAM_motion_capture_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesRGBDSLAMMotionCapture()));
278  connect(ui_->actionKITTI_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesKITTI()));
279  connect(ui_->actionTORO_graph, SIGNAL(triggered()), this , SLOT(exportPosesTORO()));
280  connect(ui_->actionG2o_g2o, SIGNAL(triggered()), this , SLOT(exportPosesG2O()));
281  connect(ui_->actionPoses_KML, SIGNAL(triggered()), this , SLOT(exportPosesKML()));
282  connect(ui_->actionGPS_TXT, SIGNAL(triggered()), this , SLOT(exportGPS_TXT()));
283  connect(ui_->actionGPS_KML, SIGNAL(triggered()), this , SLOT(exportGPS_KML()));
284  connect(ui_->actionEdit_optimized_2D_map, SIGNAL(triggered()), this , SLOT(editSaved2DMap()));
285  connect(ui_->actionExport_saved_2D_map, SIGNAL(triggered()), this , SLOT(exportSaved2DMap()));
286  connect(ui_->actionImport_2D_map, SIGNAL(triggered()), this , SLOT(import2DMap()));
287  connect(ui_->actionRegenerate_optimized_2D_map, SIGNAL(triggered()), this , SLOT(regenerateSavedMap()));
288  connect(ui_->actionView_optimized_mesh, SIGNAL(triggered()), this , SLOT(viewOptimizedMesh()));
289  connect(ui_->actionExport_optimized_mesh, SIGNAL(triggered()), this , SLOT(exportOptimizedMesh()));
290  connect(ui_->actionUpdate_optimized_mesh, SIGNAL(triggered()), this , SLOT(updateOptimizedMesh()));
291  connect(ui_->actionView_3D_map, SIGNAL(triggered()), this, SLOT(view3DMap()));
292  connect(ui_->actionGenerate_3D_map_pcd, SIGNAL(triggered()), this, SLOT(generate3DMap()));
293  connect(ui_->actionDetect_more_loop_closures, SIGNAL(triggered()), this, SLOT(detectMoreLoopClosures()));
294  connect(ui_->actionUpdate_all_neighbor_covariances, SIGNAL(triggered()), this, SLOT(updateAllNeighborCovariances()));
295  connect(ui_->actionUpdate_all_loop_closure_covariances, SIGNAL(triggered()), this, SLOT(updateAllLoopClosureCovariances()));
296  connect(ui_->actionUpdate_all_landmark_covariances, SIGNAL(triggered()), this, SLOT(updateAllLandmarkCovariances()));
297  connect(ui_->actionRefine_all_neighbor_links, SIGNAL(triggered()), this, SLOT(refineAllNeighborLinks()));
298  connect(ui_->actionRefine_all_loop_closure_links, SIGNAL(triggered()), this, SLOT(refineAllLoopClosureLinks()));
299  connect(ui_->actionRegenerate_local_grid_maps, SIGNAL(triggered()), this, SLOT(regenerateLocalMaps()));
300  connect(ui_->actionRegenerate_local_grid_maps_selected, SIGNAL(triggered()), this, SLOT(regenerateCurrentLocalMaps()));
301  connect(ui_->actionReset_all_changes, SIGNAL(triggered()), this, SLOT(resetAllChanges()));
302  connect(ui_->actionRestore_default_GUI_settings, SIGNAL(triggered()), this, SLOT(restoreDefaultSettings()));
303 
304  //ICP buttons
305  connect(ui_->pushButton_refine, SIGNAL(clicked()), this, SLOT(refineConstraint()));
306  connect(ui_->pushButton_add, SIGNAL(clicked()), this, SLOT(addConstraint()));
307  connect(ui_->pushButton_reset, SIGNAL(clicked()), this, SLOT(resetConstraint()));
308  connect(ui_->pushButton_reject, SIGNAL(clicked()), this, SLOT(rejectConstraint()));
309  ui_->pushButton_refine->setEnabled(false);
310  ui_->pushButton_add->setEnabled(false);
311  ui_->pushButton_reset->setEnabled(false);
312  ui_->pushButton_reject->setEnabled(false);
313 
314  ui_->menuEdit->setEnabled(false);
315  ui_->actionGenerate_3D_map_pcd->setEnabled(false);
316  ui_->actionExport->setEnabled(false);
317  ui_->actionExtract_images->setEnabled(false);
318  ui_->menuExport_poses->setEnabled(false);
319  ui_->menuExport_GPS->setEnabled(false);
320  ui_->actionPoses_KML->setEnabled(false);
321  ui_->actionEdit_optimized_2D_map->setEnabled(false);
322  ui_->actionExport_saved_2D_map->setEnabled(false);
323  ui_->actionImport_2D_map->setEnabled(false);
324  ui_->actionRegenerate_optimized_2D_map->setEnabled(false);
325  ui_->actionView_optimized_mesh->setEnabled(false);
326  ui_->actionExport_optimized_mesh->setEnabled(false);
327  ui_->actionUpdate_optimized_mesh->setEnabled(false);
328 
329  ui_->horizontalSlider_A->setTracking(false);
330  ui_->horizontalSlider_B->setTracking(false);
331  ui_->horizontalSlider_A->setEnabled(false);
332  ui_->horizontalSlider_B->setEnabled(false);
333  connect(ui_->horizontalSlider_A, SIGNAL(valueChanged(int)), this, SLOT(sliderAValueChanged(int)));
334  connect(ui_->horizontalSlider_B, SIGNAL(valueChanged(int)), this, SLOT(sliderBValueChanged(int)));
335  connect(ui_->horizontalSlider_A, SIGNAL(sliderMoved(int)), this, SLOT(sliderAMoved(int)));
336  connect(ui_->horizontalSlider_B, SIGNAL(sliderMoved(int)), this, SLOT(sliderBMoved(int)));
337 
338  connect(ui_->spinBox_mesh_angleTolerance, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
339  connect(ui_->spinBox_mesh_minClusterSize, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
340  connect(ui_->spinBox_mesh_fillDepthHoles, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
341  connect(ui_->spinBox_mesh_depthError, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
342  connect(ui_->checkBox_mesh_quad, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
343  connect(ui_->spinBox_mesh_triangleSize, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
344  connect(ui_->checkBox_showWords, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
345  connect(ui_->checkBox_showCloud, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
346  connect(ui_->checkBox_showMesh, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
347  connect(ui_->checkBox_showScan, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
348  connect(ui_->checkBox_showMap, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
349  connect(ui_->checkBox_showGrid, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
350  connect(ui_->checkBox_odomFrame_3dview, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
351  connect(ui_->checkBox_gravity_3dview, SIGNAL(toggled(bool)), this, SLOT(update3dView()));
352 
353  ui_->horizontalSlider_neighbors->setTracking(false);
354  ui_->horizontalSlider_loops->setTracking(false);
355  ui_->horizontalSlider_neighbors->setEnabled(false);
356  ui_->horizontalSlider_loops->setEnabled(false);
357  connect(ui_->horizontalSlider_neighbors, SIGNAL(valueChanged(int)), this, SLOT(sliderNeighborValueChanged(int)));
358  connect(ui_->horizontalSlider_loops, SIGNAL(valueChanged(int)), this, SLOT(sliderLoopValueChanged(int)));
359  connect(ui_->horizontalSlider_neighbors, SIGNAL(sliderMoved(int)), this, SLOT(sliderNeighborValueChanged(int)));
360  connect(ui_->horizontalSlider_loops, SIGNAL(sliderMoved(int)), this, SLOT(sliderLoopValueChanged(int)));
361  connect(ui_->checkBox_showOptimized, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
362  connect(ui_->checkBox_show3Dclouds, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
363  connect(ui_->checkBox_show2DScans, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
364  connect(ui_->checkBox_show3DWords, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
365  connect(ui_->checkBox_odomFrame, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
366  ui_->checkBox_showOptimized->setEnabled(false);
367  connect(ui_->toolButton_constraint, SIGNAL(clicked(bool)), this, SLOT(editConstraint()));
368  connect(ui_->checkBox_enableForAll, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintButtons()));
369 
370  ui_->horizontalSlider_iterations->setTracking(false);
371  ui_->horizontalSlider_iterations->setEnabled(false);
372  ui_->spinBox_optimizationsFrom->setEnabled(false);
373  connect(ui_->horizontalSlider_iterations, SIGNAL(valueChanged(int)), this, SLOT(sliderIterationsValueChanged(int)));
374  connect(ui_->horizontalSlider_iterations, SIGNAL(sliderMoved(int)), this, SLOT(sliderIterationsValueChanged(int)));
375  connect(ui_->spinBox_optimizationsFrom, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
376  connect(ui_->checkBox_iterativeOptimization, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
377  connect(ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
378  connect(ui_->checkBox_wmState, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
379  connect(ui_->graphViewer, SIGNAL(mapShownRequested()), this, SLOT(updateGraphView()));
380  connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
381  connect(ui_->checkBox_ignorePoseCorrection, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
382  connect(ui_->checkBox_ignoreGlobalLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
383  connect(ui_->checkBox_ignoreLocalLoopSpace, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
384  connect(ui_->checkBox_ignoreLocalLoopTime, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
385  connect(ui_->checkBox_ignoreUserLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
386  connect(ui_->checkBox_ignoreLandmarks, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
387  connect(ui_->doubleSpinBox_optimizationScale, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
388  connect(ui_->checkBox_octomap, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
389  connect(ui_->checkBox_grid_2d, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
390  connect(ui_->comboBox_octomap_rendering_type, SIGNAL(currentIndexChanged(int)), this, SLOT(updateOctomapView()));
391  connect(ui_->spinBox_grid_depth, SIGNAL(valueChanged(int)), this, SLOT(updateOctomapView()));
392  connect(ui_->checkBox_grid_empty, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
393  connect(ui_->doubleSpinBox_gainCompensationRadius, SIGNAL(valueChanged(double)), this, SLOT(updateConstraintView()));
394  connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(updateConstraintView()));
395  connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(update3dView()));
396  connect(ui_->checkBox_cameraProjection, SIGNAL(stateChanged(int)), this, SLOT(update3dView()));
397  connect(ui_->checkBox_showDisparityInsteadOfRight, SIGNAL(stateChanged(int)), this, SLOT(update3dView()));
398  connect(ui_->spinBox_decimation, SIGNAL(valueChanged(int)), this, SLOT(updateConstraintView()));
399  connect(ui_->spinBox_decimation, SIGNAL(valueChanged(int)), this, SLOT(update3dView()));
400  connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(updateGraphView()));
401  connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
402  connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
403 
404  ui_->label_stereo_inliers_name->setStyleSheet("QLabel {color : blue; }");
405  ui_->label_stereo_flowOutliers_name->setStyleSheet("QLabel {color : red; }");
406  ui_->label_stereo_slopeOutliers_name->setStyleSheet("QLabel {color : yellow; }");
407  ui_->label_stereo_disparityOutliers_name->setStyleSheet("QLabel {color : magenta; }");
408 
409 
410  // connect configuration changed
411  connect(ui_->graphViewer, SIGNAL(configChanged()), this, SLOT(configModified()));
412  connect(ui_->graphicsView_A, SIGNAL(configChanged()), this, SLOT(configModified()));
413  connect(ui_->graphicsView_B, SIGNAL(configChanged()), this, SLOT(configModified()));
414  connect(ui_->comboBox_logger_level, SIGNAL(currentIndexChanged(int)), this, SLOT(configModified()));
415  connect(ui_->actionVertical_Layout, SIGNAL(toggled(bool)), this, SLOT(configModified()));
416  connect(ui_->checkBox_alignPosesWithGroundTruth, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
417  connect(ui_->checkBox_alignScansCloudsWithGroundTruth, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
418  connect(ui_->checkBox_ignoreIntermediateNodes, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
419  connect(ui_->checkBox_ignoreIntermediateNodes, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
420  connect(ui_->checkBox_timeStats, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
421  connect(ui_->checkBox_timeStats, SIGNAL(stateChanged(int)), this, SLOT(updateStatistics()));
422  // Graph view
423  connect(ui_->doubleSpinBox_gainCompensationRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
424  connect(ui_->doubleSpinBox_voxelSize, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
425  connect(ui_->checkBox_cameraProjection, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
426  connect(ui_->checkBox_showDisparityInsteadOfRight, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
427  connect(ui_->spinBox_decimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
428  connect(ui_->groupBox_posefiltering, SIGNAL(clicked(bool)), this, SLOT(configModified()));
429  connect(ui_->doubleSpinBox_posefilteringRadius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
430  connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
431 
432  connect(ui_->spinBox_icp_decimation, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
433  connect(ui_->doubleSpinBox_icp_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
434  connect(ui_->doubleSpinBox_icp_minDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
435  connect(ui_->checkBox_icp_from_depth, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
436 
437  connect(ui_->doubleSpinBox_detectMore_radius, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
438  connect(ui_->doubleSpinBox_detectMore_radiusMin, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
439  connect(ui_->doubleSpinBox_detectMore_angle, SIGNAL(valueChanged(double)), this, SLOT(configModified()));
440  connect(ui_->spinBox_detectMore_iterations, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
441  connect(ui_->checkBox_detectMore_intraSession, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
442  connect(ui_->checkBox_detectMore_interSession, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
443 
444  connect(ui_->lineEdit_obstacleColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified()));
445  connect(ui_->lineEdit_groundColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified()));
446  connect(ui_->lineEdit_emptyColor, SIGNAL(textChanged(const QString &)), this, SLOT(configModified()));
447  connect(ui_->lineEdit_obstacleColor, SIGNAL(textChanged(const QString &)), this, SLOT(updateGrid()));
448  connect(ui_->lineEdit_groundColor, SIGNAL(textChanged(const QString &)), this, SLOT(updateGrid()));
449  connect(ui_->lineEdit_emptyColor, SIGNAL(textChanged(const QString &)), this, SLOT(updateGrid()));
450  connect(ui_->toolButton_obstacleColor, SIGNAL(clicked(bool)), this, SLOT(selectObstacleColor()));
451  connect(ui_->toolButton_groundColor, SIGNAL(clicked(bool)), this, SLOT(selectGroundColor()));
452  connect(ui_->toolButton_emptyColor, SIGNAL(clicked(bool)), this, SLOT(selectEmptyColor()));
453  connect(ui_->spinBox_cropRadius, SIGNAL(valueChanged(int)), this, SLOT(configModified()));
454  connect(ui_->checkBox_grid_showProbMap, SIGNAL(stateChanged(int)), this, SLOT(configModified()));
455  connect(ui_->checkBox_grid_showProbMap, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
456 
457  connect(exportDialog_, SIGNAL(configChanged()), this, SLOT(configModified()));
458 
459  // dockwidget
460  QList<QDockWidget*> dockWidgets = this->findChildren<QDockWidget*>();
461  for(int i=0; i<dockWidgets.size(); ++i)
462  {
463  connect(dockWidgets[i], SIGNAL(dockLocationChanged(Qt::DockWidgetArea)), this, SLOT(configModified()));
464  connect(dockWidgets[i]->toggleViewAction(), SIGNAL(toggled(bool)), this, SLOT(configModified()));
465  }
466  ui_->dockWidget_constraints->installEventFilter(this);
467  ui_->dockWidget_graphView->installEventFilter(this);
468  ui_->dockWidget_occupancyGridView->installEventFilter(this);
469  ui_->dockWidget_stereoView->installEventFilter(this);
470  ui_->dockWidget_view3d->installEventFilter(this);
471  ui_->dockWidget_guiparameters->installEventFilter(this);
472  ui_->dockWidget_coreparameters->installEventFilter(this);
473  ui_->dockWidget_info->installEventFilter(this);
474  ui_->dockWidget_statistics->installEventFilter(this);
475 }
476 
477 DatabaseViewer::~DatabaseViewer()
478 {
479  delete ui_;
480  delete dbDriver_;
481 #ifdef RTABMAP_OCTOMAP
482  delete octomap_;
483 #endif
484 }
485 
486 void DatabaseViewer::setupMainLayout(bool vertical)
487 {
488  if(vertical)
489  {
490  qobject_cast<QHBoxLayout *>(ui_->horizontalLayout_imageViews->layout())->setDirection(QBoxLayout::TopToBottom);
491  }
492  else if(!vertical)
493  {
494  qobject_cast<QHBoxLayout *>(ui_->horizontalLayout_imageViews->layout())->setDirection(QBoxLayout::LeftToRight);
495  }
496  if(ids_.size())
497  {
498  sliderAValueChanged(ui_->horizontalSlider_A->value()); // update matching lines
499  }
500 }
501 
502 void DatabaseViewer::showCloseButton(bool visible)
503 {
504  ui_->buttonBox->setVisible(visible);
505 }
506 
507 void DatabaseViewer::configModified()
508 {
509  this->setWindowModified(true);
510 }
511 
512 QString DatabaseViewer::getIniFilePath() const
513 {
514  if(!iniFilePath_.isEmpty())
515  {
516  return iniFilePath_;
517  }
518  QString privatePath = QDir::homePath() + "/.rtabmap";
519  if(!QDir(privatePath).exists())
520  {
521  QDir::home().mkdir(".rtabmap");
522  }
523  return privatePath + "/rtabmap.ini";
524 }
525 
526 void DatabaseViewer::readSettings()
527 {
528  QString path = getIniFilePath();
529  QSettings settings(path, QSettings::IniFormat);
530  settings.beginGroup("DatabaseViewer");
531 
532  //load window state / geometry
533  QByteArray bytes;
534  bytes = settings.value("geometry", QByteArray()).toByteArray();
535  if(!bytes.isEmpty())
536  {
537  this->restoreGeometry(bytes);
538  }
539  bytes = settings.value("state", QByteArray()).toByteArray();
540  if(!bytes.isEmpty())
541  {
542  this->restoreState(bytes);
543  }
544  savedMaximized_ = settings.value("maximized", false).toBool();
545 
546  ui_->comboBox_logger_level->setCurrentIndex(settings.value("loggerLevel", ui_->comboBox_logger_level->currentIndex()).toInt());
547  ui_->actionVertical_Layout->setChecked(settings.value("verticalLayout", ui_->actionVertical_Layout->isChecked()).toBool());
548  ui_->checkBox_ignoreIntermediateNodes->setChecked(settings.value("ignoreIntermediateNodes", ui_->checkBox_ignoreIntermediateNodes->isChecked()).toBool());
549  ui_->checkBox_timeStats->setChecked(settings.value("timeStats", ui_->checkBox_timeStats->isChecked()).toBool());
550 
551  // GraphViewer settings
552  ui_->graphViewer->loadSettings(settings, "GraphView");
553 
554  settings.beginGroup("optimization");
555  ui_->doubleSpinBox_gainCompensationRadius->setValue(settings.value("gainCompensationRadius", ui_->doubleSpinBox_gainCompensationRadius->value()).toDouble());
556  ui_->doubleSpinBox_voxelSize->setValue(settings.value("voxelSize", ui_->doubleSpinBox_voxelSize->value()).toDouble());
557  ui_->spinBox_decimation->setValue(settings.value("decimation", ui_->spinBox_decimation->value()).toInt());
558  ui_->checkBox_cameraProjection->setChecked(settings.value("camProj", ui_->checkBox_cameraProjection->isChecked()).toBool());
559  ui_->checkBox_showDisparityInsteadOfRight->setChecked(settings.value("showDisp", ui_->checkBox_showDisparityInsteadOfRight->isChecked()).toBool());
560  settings.endGroup();
561 
562  settings.beginGroup("grid");
563  ui_->groupBox_posefiltering->setChecked(settings.value("poseFiltering", ui_->groupBox_posefiltering->isChecked()).toBool());
564  ui_->doubleSpinBox_posefilteringRadius->setValue(settings.value("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value()).toDouble());
565  ui_->doubleSpinBox_posefilteringAngle->setValue(settings.value("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value()).toDouble());
566  ui_->lineEdit_obstacleColor->setText(settings.value("colorObstacle", ui_->lineEdit_obstacleColor->text()).toString());
567  ui_->lineEdit_groundColor->setText(settings.value("colorGround", ui_->lineEdit_groundColor->text()).toString());
568  ui_->lineEdit_emptyColor->setText(settings.value("colorEmpty", ui_->lineEdit_emptyColor->text()).toString());
569  ui_->spinBox_cropRadius->setValue(settings.value("cropRadius", ui_->spinBox_cropRadius->value()).toInt());
570  ui_->checkBox_grid_showProbMap->setChecked(settings.value("probMap", ui_->checkBox_grid_showProbMap->isChecked()).toBool());
571  settings.endGroup();
572 
573  settings.beginGroup("mesh");
574  ui_->checkBox_mesh_quad->setChecked(settings.value("quad", ui_->checkBox_mesh_quad->isChecked()).toBool());
575  ui_->spinBox_mesh_angleTolerance->setValue(settings.value("angleTolerance", ui_->spinBox_mesh_angleTolerance->value()).toInt());
576  ui_->spinBox_mesh_minClusterSize->setValue(settings.value("minClusterSize", ui_->spinBox_mesh_minClusterSize->value()).toInt());
577  ui_->spinBox_mesh_fillDepthHoles->setValue(settings.value("fillDepthHolesSize", ui_->spinBox_mesh_fillDepthHoles->value()).toInt());
578  ui_->spinBox_mesh_depthError->setValue(settings.value("fillDepthHolesError", ui_->spinBox_mesh_depthError->value()).toInt());
579  ui_->spinBox_mesh_triangleSize->setValue(settings.value("triangleSize", ui_->spinBox_mesh_triangleSize->value()).toInt());
580  settings.endGroup();
581 
582  // ImageViews
583  ui_->graphicsView_A->loadSettings(settings, "ImageViewA");
584  ui_->graphicsView_B->loadSettings(settings, "ImageViewB");
585 
586  // ICP parameters
587  settings.beginGroup("icp");
588  ui_->spinBox_icp_decimation->setValue(settings.value("decimation", ui_->spinBox_icp_decimation->value()).toInt());
589  ui_->doubleSpinBox_icp_maxDepth->setValue(settings.value("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value()).toDouble());
590  ui_->doubleSpinBox_icp_minDepth->setValue(settings.value("minDepth", ui_->doubleSpinBox_icp_minDepth->value()).toDouble());
591  ui_->checkBox_icp_from_depth->setChecked(settings.value("icpFromDepth", ui_->checkBox_icp_from_depth->isChecked()).toBool());
592  settings.endGroup();
593 
594  settings.endGroup(); // DatabaseViewer
595 
596  // Use same parameters used by RTAB-Map
597  settings.beginGroup("Gui");
598  exportDialog_->loadSettings(settings, exportDialog_->objectName());
599  settings.beginGroup("PostProcessingDialog");
600  ui_->doubleSpinBox_detectMore_radius->setValue(settings.value("cluster_radius", ui_->doubleSpinBox_detectMore_radius->value()).toDouble());
601  ui_->doubleSpinBox_detectMore_radiusMin->setValue(settings.value("cluster_radius_min", ui_->doubleSpinBox_detectMore_radiusMin->value()).toDouble());
602  ui_->doubleSpinBox_detectMore_angle->setValue(settings.value("cluster_angle", ui_->doubleSpinBox_detectMore_angle->value()).toDouble());
603  ui_->spinBox_detectMore_iterations->setValue(settings.value("iterations", ui_->spinBox_detectMore_iterations->value()).toInt());
604  ui_->checkBox_detectMore_intraSession->setChecked(settings.value("intra_session", ui_->checkBox_detectMore_intraSession->isChecked()).toBool());
605  ui_->checkBox_detectMore_interSession->setChecked(settings.value("inter_session", ui_->checkBox_detectMore_interSession->isChecked()).toBool());
606  settings.endGroup();
607  settings.endGroup();
608 
609 
610  ParametersMap parameters;
611  Parameters::readINI(path.toStdString(), parameters);
612  for(ParametersMap::iterator iter = parameters.begin(); iter!= parameters.end(); ++iter)
613  {
614  ui_->parameters_toolbox->updateParameter(iter->first, iter->second);
615  }
616 }
617 
618 void DatabaseViewer::writeSettings()
619 {
620  QString path = getIniFilePath();
621  QSettings settings(path, QSettings::IniFormat);
622  settings.beginGroup("DatabaseViewer");
623 
624  //save window state / geometry
625  if(!this->isMaximized())
626  {
627  settings.setValue("geometry", this->saveGeometry());
628  }
629  settings.setValue("state", this->saveState());
630  settings.setValue("maximized", this->isMaximized());
631  savedMaximized_ = this->isMaximized();
632 
633  settings.setValue("loggerLevel", ui_->comboBox_logger_level->currentIndex());
634  settings.setValue("verticalLayout", ui_->actionVertical_Layout->isChecked());
635  settings.setValue("ignoreIntermediateNodes", ui_->checkBox_ignoreIntermediateNodes->isChecked());
636  settings.setValue("timeStats", ui_->checkBox_timeStats->isChecked());
637 
638  // save GraphViewer settings
639  ui_->graphViewer->saveSettings(settings, "GraphView");
640 
641  // save optimization settings
642  settings.beginGroup("optimization");
643  settings.setValue("gainCompensationRadius", ui_->doubleSpinBox_gainCompensationRadius->value());
644  settings.setValue("voxelSize", ui_->doubleSpinBox_voxelSize->value());
645  settings.setValue("decimation", ui_->spinBox_decimation->value());
646  settings.setValue("camProj", ui_->checkBox_cameraProjection->isChecked());
647  settings.setValue("showDisp", ui_->checkBox_showDisparityInsteadOfRight->isChecked());
648  settings.endGroup();
649 
650  // save Grid settings
651  settings.beginGroup("grid");
652  settings.setValue("poseFiltering", ui_->groupBox_posefiltering->isChecked());
653  settings.setValue("poseFilteringRadius", ui_->doubleSpinBox_posefilteringRadius->value());
654  settings.setValue("poseFilteringAngle", ui_->doubleSpinBox_posefilteringAngle->value());
655  settings.setValue("colorObstacle", ui_->lineEdit_obstacleColor->text());
656  settings.setValue("colorGround", ui_->lineEdit_groundColor->text());
657  settings.setValue("colorEmpty", ui_->lineEdit_emptyColor->text());
658  settings.setValue("cropRadius", ui_->spinBox_cropRadius->value());
659  settings.setValue("probMap", ui_->checkBox_grid_showProbMap->isChecked());
660  settings.endGroup();
661 
662  settings.beginGroup("mesh");
663  settings.setValue("quad", ui_->checkBox_mesh_quad->isChecked());
664  settings.setValue("angleTolerance", ui_->spinBox_mesh_angleTolerance->value());
665  settings.setValue("minClusterSize", ui_->spinBox_mesh_minClusterSize->value());
666  settings.setValue("fillDepthHolesSize", ui_->spinBox_mesh_fillDepthHoles->value());
667  settings.setValue("fillDepthHolesError", ui_->spinBox_mesh_depthError->value());
668  settings.setValue("triangleSize", ui_->spinBox_mesh_triangleSize->value());
669  settings.endGroup();
670 
671  // ImageViews
672  ui_->graphicsView_A->saveSettings(settings, "ImageViewA");
673  ui_->graphicsView_B->saveSettings(settings, "ImageViewB");
674 
675  // save ICP parameters
676  settings.beginGroup("icp");
677  settings.setValue("decimation", ui_->spinBox_icp_decimation->value());
678  settings.setValue("maxDepth", ui_->doubleSpinBox_icp_maxDepth->value());
679  settings.setValue("minDepth", ui_->doubleSpinBox_icp_minDepth->value());
680  settings.setValue("icpFromDepth", ui_->checkBox_icp_from_depth->isChecked());
681  settings.endGroup();
682 
683  settings.endGroup(); // DatabaseViewer
684 
685  // Use same parameters used by RTAB-Map
686  settings.beginGroup("Gui");
687  exportDialog_->saveSettings(settings, exportDialog_->objectName());
688  settings.beginGroup("PostProcessingDialog");
689  settings.setValue("cluster_radius", ui_->doubleSpinBox_detectMore_radius->value());
690  settings.setValue("cluster_radius_min", ui_->doubleSpinBox_detectMore_radiusMin->value());
691  settings.setValue("cluster_angle", ui_->doubleSpinBox_detectMore_angle->value());
692  settings.setValue("iterations", ui_->spinBox_detectMore_iterations->value());
693  settings.setValue("intra_session", ui_->checkBox_detectMore_intraSession->isChecked());
694  settings.setValue("inter_session", ui_->checkBox_detectMore_interSession->isChecked());
695  settings.endGroup();
696  settings.endGroup();
697 
698  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
699  for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end();)
700  {
701  if(!ui_->parameters_toolbox->getParameterWidget(iter->first.c_str()))
702  {
703  parameters.erase(iter++);
704  }
705  else
706  {
707  ++iter;
708  }
709  }
710  Parameters::writeINI(path.toStdString(), parameters);
711 
712  this->setWindowModified(false);
713 }
714 
715 void DatabaseViewer::restoreDefaultSettings()
716 {
717  // reset GUI parameters
718  ui_->comboBox_logger_level->setCurrentIndex(1);
719  ui_->checkBox_alignPosesWithGroundTruth->setChecked(true);
720  ui_->checkBox_alignScansCloudsWithGroundTruth->setChecked(false);
721  ui_->checkBox_ignoreIntermediateNodes->setChecked(false);
722  ui_->checkBox_timeStats->setChecked(true);
723 
724  ui_->checkBox_iterativeOptimization->setChecked(true);
725  ui_->checkBox_spanAllMaps->setChecked(true);
726  ui_->checkBox_wmState->setChecked(false);
727  ui_->checkBox_ignorePoseCorrection->setChecked(false);
728  ui_->checkBox_ignoreGlobalLoop->setChecked(false);
729  ui_->checkBox_ignoreLocalLoopSpace->setChecked(false);
730  ui_->checkBox_ignoreLocalLoopTime->setChecked(false);
731  ui_->checkBox_ignoreUserLoop->setChecked(false);
732  ui_->checkBox_ignoreLandmarks->setChecked(false);
733  ui_->doubleSpinBox_optimizationScale->setValue(1.0);
734  ui_->doubleSpinBox_gainCompensationRadius->setValue(0.0);
735  ui_->doubleSpinBox_voxelSize->setValue(0.0);
736  ui_->spinBox_decimation->setValue(1);
737  ui_->checkBox_cameraProjection->setChecked(false);
738  ui_->checkBox_showDisparityInsteadOfRight->setChecked(false);
739 
740  ui_->groupBox_posefiltering->setChecked(false);
741  ui_->doubleSpinBox_posefilteringRadius->setValue(0.1);
742  ui_->doubleSpinBox_posefilteringAngle->setValue(30);
743  ui_->checkBox_grid_empty->setChecked(true);
744  ui_->checkBox_octomap->setChecked(false);
745  ui_->lineEdit_obstacleColor->setText(QColor(Qt::red).name());
746  ui_->lineEdit_groundColor->setText(QColor(Qt::green).name());
747  ui_->lineEdit_emptyColor->setText(QColor(Qt::yellow).name());
748  ui_->spinBox_cropRadius->setValue(1);
749  ui_->checkBox_grid_showProbMap->setChecked(false);
750 
751  ui_->checkBox_mesh_quad->setChecked(true);
752  ui_->spinBox_mesh_angleTolerance->setValue(15);
753  ui_->spinBox_mesh_minClusterSize->setValue(0);
754  ui_->spinBox_mesh_fillDepthHoles->setValue(false);
755  ui_->spinBox_mesh_depthError->setValue(10);
756  ui_->spinBox_mesh_triangleSize->setValue(2);
757 
758  ui_->spinBox_icp_decimation->setValue(1);
759  ui_->doubleSpinBox_icp_maxDepth->setValue(0.0);
760  ui_->doubleSpinBox_icp_minDepth->setValue(0.0);
761  ui_->checkBox_icp_from_depth->setChecked(false);
762 
763  ui_->doubleSpinBox_detectMore_radius->setValue(1.0);
764  ui_->doubleSpinBox_detectMore_radiusMin->setValue(0.0);
765  ui_->doubleSpinBox_detectMore_angle->setValue(30.0);
766  ui_->spinBox_detectMore_iterations->setValue(5);
767  ui_->checkBox_detectMore_intraSession->setChecked(true);
768  ui_->checkBox_detectMore_interSession->setChecked(true);
769 }
770 
771 void DatabaseViewer::openDatabase()
772 {
773  QString path = QFileDialog::getOpenFileName(this, tr("Select file"), pathDatabase_, tr("Databases (*.db)"));
774  if(!path.isEmpty())
775  {
776  openDatabase(path);
777  }
778 }
779 
780 bool DatabaseViewer::openDatabase(const QString & path)
781 {
782  UDEBUG("Open database \"%s\"", path.toStdString().c_str());
783  if(QFile::exists(path))
784  {
785  if(QFileInfo(path).isFile())
786  {
787  std::string driverType = "sqlite3";
788 
789  dbDriver_ = DBDriver::create();
790 
791  if(!dbDriver_->openConnection(path.toStdString()))
792  {
793  ui_->actionClose_database->setEnabled(false);
794  ui_->actionOpen_database->setEnabled(true);
795  delete dbDriver_;
796  dbDriver_ = 0;
797  QMessageBox::warning(this, "Database error", tr("Can't open database \"%1\"").arg(path));
798  }
799  else
800  {
801  ui_->actionClose_database->setEnabled(true);
802  ui_->actionOpen_database->setEnabled(false);
803 
804  pathDatabase_ = UDirectory::getDir(path.toStdString()).c_str();
805  if(pathDatabase_.isEmpty() || pathDatabase_.compare(".") == 0)
806  {
807  pathDatabase_ = QDir::currentPath();
808  }
809  databaseFileName_ = UFile::getName(path.toStdString());
810  ui_->graphViewer->setWorkingDirectory(pathDatabase_);
811 
812  // look if there are saved parameters
813  ParametersMap parameters = dbDriver_->getLastParameters();
814 
815  if(parameters.size())
816  {
817  const ParametersMap & currentParameters = ui_->parameters_toolbox->getParameters();
818  ParametersMap differentParameters;
819  for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
820  {
821  ParametersMap::const_iterator jter = currentParameters.find(iter->first);
822  if(jter!=currentParameters.end() &&
823  ui_->parameters_toolbox->getParameterWidget(QString(iter->first.c_str())) != 0 &&
824  iter->second.compare(jter->second) != 0 &&
825  iter->first.compare(Parameters::kRtabmapWorkingDirectory()) != 0)
826  {
827  bool different = true;
828  if(Parameters::getType(iter->first).compare("double") ==0 ||
829  Parameters::getType(iter->first).compare("float") == 0)
830  {
831  if(uStr2Double(iter->second) == uStr2Double(jter->second))
832  {
833  different = false;
834  }
835  }
836  if(different)
837  {
838  differentParameters.insert(*iter);
839  QString msg = tr("Parameter \"%1\": database=\"%2\" Preferences=\"%3\"")
840  .arg(iter->first.c_str())
841  .arg(iter->second.c_str())
842  .arg(jter->second.c_str());
843  UWARN(msg.toStdString().c_str());
844  }
845  }
846  }
847 
848  if(differentParameters.size())
849  {
850  int r = QMessageBox::question(this,
851  tr("Update parameters..."),
852  tr("The database is using %1 different parameter(s) than "
853  "those currently set in Core parameters panel. Do you want "
854  "to use database's parameters?").arg(differentParameters.size()),
855  QMessageBox::Yes | QMessageBox::No,
856  QMessageBox::Yes);
857  if(r == QMessageBox::Yes)
858  {
859  QStringList str;
860  for(rtabmap::ParametersMap::const_iterator iter = differentParameters.begin(); iter!=differentParameters.end(); ++iter)
861  {
862  ui_->parameters_toolbox->updateParameter(iter->first, iter->second);
863  str.push_back(iter->first.c_str());
864  }
865  notifyParametersChanged(str);
866  }
867  }
868  }
869 
870  updateIds();
871  this->setWindowTitle("RTAB-Map Database Viewer - " + path + "[*]");
872  return true;
873  }
874  }
875  else // directory
876  {
877  pathDatabase_ = path;
878  if(pathDatabase_.isEmpty() || pathDatabase_.compare(".") == 0)
879  {
880  pathDatabase_ = QDir::currentPath();
881  }
882  ui_->graphViewer->setWorkingDirectory(pathDatabase_);
883  }
884  }
885  else
886  {
887  QMessageBox::warning(this, "Database error", tr("Database \"%1\" does not exist.").arg(path));
888  }
889  return false;
890 }
891 
892 bool DatabaseViewer::closeDatabase()
893 {
894  this->setWindowTitle("RTAB-Map Database Viewer[*]");
895  if(dbDriver_)
896  {
897  if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size())
898  {
899  QMessageBox::StandardButton button = QMessageBox::question(this,
900  tr("Links modified"),
901  tr("Some links are modified (%1 added, %2 refined, %3 removed), do you want to save them?")
902  .arg(linksAdded_.size()).arg(linksRefined_.size()).arg(linksRemoved_.size()),
903  QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
904  QMessageBox::Cancel);
905 
906  if(button == QMessageBox::Yes)
907  {
908  // Added links
909  for(std::multimap<int, rtabmap::Link>::iterator iter=linksAdded_.begin(); iter!=linksAdded_.end(); ++iter)
910  {
911  std::multimap<int, rtabmap::Link>::iterator refinedIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
912  if(refinedIter != linksRefined_.end())
913  {
914  dbDriver_->addLink(refinedIter->second);
915  dbDriver_->addLink(refinedIter->second.inverse());
916  }
917  else
918  {
919  dbDriver_->addLink(iter->second);
920  dbDriver_->addLink(iter->second.inverse());
921  }
922  }
923 
924  //Refined links
925  for(std::multimap<int, rtabmap::Link>::iterator iter=linksRefined_.begin(); iter!=linksRefined_.end(); ++iter)
926  {
927  if(!containsLink(linksAdded_, iter->second.from(), iter->second.to()))
928  {
929  dbDriver_->updateLink(iter->second);
930  dbDriver_->updateLink(iter->second.inverse());
931  }
932  }
933 
934  // Rejected links
935  for(std::multimap<int, rtabmap::Link>::iterator iter=linksRemoved_.begin(); iter!=linksRemoved_.end(); ++iter)
936  {
937  dbDriver_->removeLink(iter->second.to(), iter->second.from());
938  dbDriver_->removeLink(iter->second.from(), iter->second.to());
939  }
940  linksAdded_.clear();
941  linksRefined_.clear();
942  linksRemoved_.clear();
943 
944  // Clear the optimized poses, this will force rtabmap to re-optimize the graph on initialization
945  Transform lastLocalizationPose;
946  if(!dbDriver_->loadOptimizedPoses(&lastLocalizationPose).empty())
947  {
948  dbDriver_->saveOptimizedPoses(std::map<int, Transform>(), lastLocalizationPose);
949  }
950  // This will force rtabmap_ros to regenerate the global occupancy grid if there was one
951  dbDriver_->save2DMap(cv::Mat(), 0, 0, 0);
952  dbDriver_->saveOptimizedMesh(cv::Mat());
953  }
954 
955  if(button != QMessageBox::Yes && button != QMessageBox::No)
956  {
957  return false;
958  }
959  }
960 
961  if( generatedLocalMaps_.size() &&
962  uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.11.10") >= 0)
963  {
964  QMessageBox::StandardButton button = QMessageBox::question(this,
965  tr("Local occupancy grid maps modified"),
966  tr("%1 occupancy grid maps are modified, do you want to "
967  "save them? This will overwrite occupancy grids saved in the database.")
968  .arg(generatedLocalMaps_.size()),
969  QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
970  QMessageBox::Cancel);
971 
972  if(button == QMessageBox::Yes)
973  {
974  UASSERT(generatedLocalMaps_.size() == generatedLocalMapsInfo_.size());
975  std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat > >::iterator mapIter = generatedLocalMaps_.begin();
976  std::map<int, std::pair<float, cv::Point3f> >::iterator infoIter = generatedLocalMapsInfo_.begin();
977  for(; mapIter!=generatedLocalMaps_.end(); ++mapIter, ++infoIter)
978  {
979  UASSERT(mapIter->first == infoIter->first);
980  dbDriver_->updateOccupancyGrid(
981  mapIter->first,
982  mapIter->second.first.first,
983  mapIter->second.first.second,
984  mapIter->second.second,
985  infoIter->second.first,
986  infoIter->second.second);
987  }
988  generatedLocalMaps_.clear();
989  generatedLocalMapsInfo_.clear();
990  localMaps_.clear();
991  localMapsInfo_.clear();
992 
993  // This will force rtabmap_ros to regenerate the global occupancy grid if there was one
994  dbDriver_->save2DMap(cv::Mat(), 0, 0, 0);
995  }
996 
997  if(button != QMessageBox::Yes && button != QMessageBox::No)
998  {
999  return false;
1000  }
1001  }
1002 
1003  if(!modifiedLaserScans_.empty())
1004  {
1005  QMessageBox::StandardButton button = QMessageBox::question(this,
1006  tr("Laser scans modified"),
1007  tr("%1 laser scans are modified, do you want to "
1008  "save them? This will overwrite laser scans saved in the database.")
1009  .arg(modifiedLaserScans_.size()),
1010  QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
1011  QMessageBox::Cancel);
1012 
1013  if(button == QMessageBox::Yes)
1014  {
1015  for(std::map<int, LaserScan>::iterator iter=modifiedLaserScans_.begin(); iter!=modifiedLaserScans_.end(); ++iter)
1016  {
1017  dbDriver_->updateLaserScan(iter->first, iter->second);
1018  }
1019  modifiedLaserScans_.clear();
1020  }
1021 
1022  if(button != QMessageBox::Yes && button != QMessageBox::No)
1023  {
1024  return false;
1025  }
1026  }
1027 
1028  delete dbDriver_;
1029  dbDriver_ = 0;
1030  ids_.clear();
1031  idToIndex_.clear();
1032  neighborLinks_.clear();
1033  loopLinks_.clear();
1034  graphes_.clear();
1035  graphLinks_.clear();
1036  odomPoses_.clear();
1037  groundTruthPoses_.clear();
1038  gpsPoses_.clear();
1039  gpsValues_.clear();
1040  lastWmIds_.clear();
1041  mapIds_.clear();
1042  weights_.clear();
1043  wmStates_.clear();
1044  links_.clear();
1045  linksAdded_.clear();
1046  linksRefined_.clear();
1047  linksRemoved_.clear();
1048  localMaps_.clear();
1049  localMapsInfo_.clear();
1050  generatedLocalMaps_.clear();
1051  generatedLocalMapsInfo_.clear();
1052  modifiedLaserScans_.clear();
1053  ui_->graphViewer->clearAll();
1054  occupancyGridViewer_->clear();
1055  ui_->menuEdit->setEnabled(false);
1056  ui_->actionGenerate_3D_map_pcd->setEnabled(false);
1057  ui_->actionExport->setEnabled(false);
1058  ui_->actionExtract_images->setEnabled(false);
1059  ui_->menuExport_poses->setEnabled(false);
1060  ui_->menuExport_GPS->setEnabled(false);
1061  ui_->actionPoses_KML->setEnabled(false);
1062  ui_->actionEdit_optimized_2D_map->setEnabled(false);
1063  ui_->actionExport_saved_2D_map->setEnabled(false);
1064  ui_->actionImport_2D_map->setEnabled(false);
1065  ui_->actionRegenerate_optimized_2D_map->setEnabled(false);
1066  ui_->actionView_optimized_mesh->setEnabled(false);
1067  ui_->actionExport_optimized_mesh->setEnabled(false);
1068  ui_->actionUpdate_optimized_mesh->setEnabled(false);
1069  ui_->checkBox_showOptimized->setEnabled(false);
1070  ui_->toolBox_statistics->clear();
1071  databaseFileName_.clear();
1072  ui_->checkBox_alignPosesWithGroundTruth->setVisible(false);
1073  ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(false);
1074  ui_->doubleSpinBox_optimizationScale->setVisible(false);
1075  ui_->label_scale_title->setVisible(false);
1076  ui_->label_rmse->setVisible(false);
1077  ui_->label_rmse_title->setVisible(false);
1078  ui_->checkBox_ignoreIntermediateNodes->setVisible(false);
1079  ui_->label_ignoreINtermediateNdoes->setVisible(false);
1080  ui_->label_alignPosesWithGroundTruth->setVisible(false);
1081  ui_->label_alignScansCloudsWithGroundTruth->setVisible(false);
1082  ui_->label_optimizeFrom->setText(tr("Root"));
1083  ui_->textEdit_info->clear();
1084 
1085  ui_->pushButton_refine->setEnabled(false);
1086  ui_->pushButton_add->setEnabled(false);
1087  ui_->pushButton_reset->setEnabled(false);
1088  ui_->pushButton_reject->setEnabled(false);
1089 
1090  ui_->horizontalSlider_loops->setEnabled(false);
1091  ui_->horizontalSlider_loops->setMaximum(0);
1092  ui_->horizontalSlider_iterations->setEnabled(false);
1093  ui_->horizontalSlider_iterations->setMaximum(0);
1094  ui_->horizontalSlider_neighbors->setEnabled(false);
1095  ui_->horizontalSlider_neighbors->setMaximum(0);
1096  ui_->label_constraint->clear();
1097  ui_->label_constraint_opt->clear();
1098  ui_->label_variance->clear();
1099  ui_->lineEdit_covariance->clear();
1100  ui_->label_type->clear();
1101  ui_->label_type_name->clear();
1102  ui_->checkBox_showOptimized->setEnabled(false);
1103 
1104  ui_->horizontalSlider_A->setEnabled(false);
1105  ui_->horizontalSlider_A->setMaximum(0);
1106  ui_->horizontalSlider_B->setEnabled(false);
1107  ui_->horizontalSlider_B->setMaximum(0);
1108  ui_->label_idA->setText("NaN");
1109  ui_->label_idB->setText("NaN");
1110  sliderAValueChanged(0);
1111  sliderBValueChanged(0);
1112 
1113  constraintsViewer_->clear();
1114  constraintsViewer_->refreshView();
1115 
1116  cloudViewer_->clear();
1117  cloudViewer_->refreshView();
1118 
1119  occupancyGridViewer_->clear();
1120  occupancyGridViewer_->refreshView();
1121 
1122  ui_->graphViewer->clearAll();
1123  ui_->label_loopClosures->clear();
1124  ui_->label_timeOptimization->clear();
1125  ui_->label_pathLength->clear();
1126  ui_->label_poses->clear();
1127  ui_->label_rmse->clear();
1128  ui_->spinBox_optimizationsFrom->setEnabled(false);
1129 
1130  ui_->graphicsView_A->clear();
1131  ui_->graphicsView_B->clear();
1132 
1133  ui_->graphicsView_stereo->clear();
1134  stereoViewer_->clear();
1135  stereoViewer_->refreshView();
1136 
1137  ui_->toolBox_statistics->clear();
1138  }
1139 
1140  ui_->actionClose_database->setEnabled(dbDriver_ != 0);
1141  ui_->actionOpen_database->setEnabled(dbDriver_ == 0);
1142 
1143  return dbDriver_ == 0;
1144 }
1145 
1146 
1147 void DatabaseViewer::recoverDatabase()
1148 {
1149  QString path = QFileDialog::getOpenFileName(this, tr("Select file"), pathDatabase_, tr("Databases (*.db)"));
1150  if(!path.isEmpty())
1151  {
1152  if(path.compare(pathDatabase_+QDir::separator()+databaseFileName_.c_str()) == 0)
1153  {
1154  QMessageBox::information(this, "Database recovery", tr("The selected database is already opened, close it first."));
1155  return;
1156  }
1157  std::string errorMsg;
1158  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
1159  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
1160  progressDialog->setMaximumSteps(100);
1161  progressDialog->show();
1162  progressDialog->setCancelButtonVisible(true);
1163  RecoveryState state(progressDialog);
1164  if(databaseRecovery(path.toStdString(), false, &errorMsg, &state))
1165  {
1166  QMessageBox::information(this, "Database recovery", tr("Database \"%1\" recovered! Try opening it again.").arg(path));
1167  }
1168  else
1169  {
1170  QMessageBox::warning(this, "Database recovery", tr("Database recovery failed: \"%1\".").arg(errorMsg.c_str()));
1171  }
1172  progressDialog->setValue(progressDialog->maximumSteps());
1173  }
1174 }
1175 
1176 void DatabaseViewer::closeEvent(QCloseEvent* event)
1177 {
1178  //write settings before quit?
1179  bool save = false;
1180  if(this->isWindowModified())
1181  {
1182  QMessageBox::Button b=QMessageBox::question(this,
1183  tr("Database Viewer"),
1184  tr("There are unsaved changed settings. Save them?"),
1185  QMessageBox::Save | QMessageBox::Cancel | QMessageBox::Discard);
1186  if(b == QMessageBox::Save)
1187  {
1188  save = true;
1189  }
1190  else if(b != QMessageBox::Discard)
1191  {
1192  event->ignore();
1193  return;
1194  }
1195  }
1196 
1197  if(save)
1198  {
1199  writeSettings();
1200  }
1201 
1202  event->accept();
1203 
1204  if(!closeDatabase())
1205  {
1206  event->ignore();
1207  }
1208 
1209  if(event->isAccepted())
1210  {
1211  ui_->toolBox_statistics->closeFigures();
1212  if(dbDriver_)
1213  {
1214  delete dbDriver_;
1215  dbDriver_ = 0;
1216  }
1217  }
1218 }
1219 
1220 void DatabaseViewer::showEvent(QShowEvent* anEvent)
1221 {
1222  this->setWindowModified(false);
1223 
1224  if((ui_->graphViewer->isVisible() || ui_->dockWidget_occupancyGridView->isVisible()) && graphes_.size() && localMaps_.size()==0)
1225  {
1226  sliderIterationsValueChanged((int)graphes_.size()-1);
1227  }
1228 }
1229 
1230 void DatabaseViewer::moveEvent(QMoveEvent* anEvent)
1231 {
1232  if(this->isVisible())
1233  {
1234  // HACK, there is a move event when the window is shown the first time.
1235  if(!firstCall_)
1236  {
1237  this->configModified();
1238  }
1239  firstCall_ = false;
1240  }
1241 }
1242 
1243 void DatabaseViewer::resizeEvent(QResizeEvent* anEvent)
1244 {
1245  if(this->isVisible())
1246  {
1247  this->configModified();
1248  }
1249 }
1250 
1251 void DatabaseViewer::keyPressEvent(QKeyEvent *event)
1252 {
1253  //catch ctrl-s to save settings
1254  if((event->modifiers() & Qt::ControlModifier) && event->key() == Qt::Key_S)
1255  {
1256  this->writeSettings();
1257  }
1258 }
1259 
1260 bool DatabaseViewer::eventFilter(QObject *obj, QEvent *event)
1261 {
1262  if (event->type() == QEvent::Resize && qobject_cast<QDockWidget*>(obj))
1263  {
1264  this->setWindowModified(true);
1265  }
1266  return QWidget::eventFilter(obj, event);
1267 }
1268 
1269 
1270 void DatabaseViewer::exportDatabase()
1271 {
1272  if(!dbDriver_ || ids_.size() == 0)
1273  {
1274  return;
1275  }
1276 
1277  rtabmap::ExportDialog dialog;
1278 
1279  if(dialog.exec())
1280  {
1281  if(!dialog.outputPath().isEmpty())
1282  {
1283  int framesIgnored = dialog.framesIgnored();
1284  double frameRate = dialog.targetFramerate();
1285  int sessionExported = dialog.sessionExported();
1286  QString path = dialog.outputPath();
1287  rtabmap::DataRecorder recorder;
1288  QList<int> ids;
1289 
1290  double previousStamp = 0;
1291  std::vector<double> delays(ids_.size());
1292  int oi=0;
1293  std::map<int, Transform> poses;
1294  std::map<int, double> stamps;
1295  std::map<int, Transform> groundTruths;
1296  std::map<int, GPS> gpsValues;
1297  std::map<int, EnvSensors> sensorsValues;
1298  for(int i=0; i<ids_.size(); i+=1+framesIgnored)
1299  {
1300  Transform odomPose, groundTruth;
1301  int weight = -1;
1302  int mapId = -1;
1303  std::string label;
1304  double stamp = 0;
1305  std::vector<float> velocity;
1306  GPS gps;
1307  EnvSensors sensors;
1308  if(dbDriver_->getNodeInfo(ids_[i], odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors))
1309  {
1310  if(frameRate == 0 ||
1311  previousStamp == 0 ||
1312  stamp == 0 ||
1313  stamp - previousStamp >= 1.0/frameRate)
1314  {
1315  if(sessionExported < 0 || sessionExported == mapId)
1316  {
1317  ids.push_back(ids_[i]);
1318 
1319  if(previousStamp && stamp)
1320  {
1321  delays[oi++] = stamp - previousStamp;
1322  }
1323  previousStamp = stamp;
1324 
1325  poses.insert(std::make_pair(ids_[i], odomPose));
1326  stamps.insert(std::make_pair(ids_[i], stamp));
1327  groundTruths.insert(std::make_pair(ids_[i], groundTruth));
1328  if(gps.stamp() > 0.0)
1329  {
1330  gpsValues.insert(std::make_pair(ids_[i], gps));
1331  }
1332  if(sensors.size())
1333  {
1334  sensorsValues.insert(std::make_pair(ids_[i], sensors));
1335  }
1336  }
1337  }
1338  if(sessionExported >= 0 && mapId > sessionExported)
1339  {
1340  break;
1341  }
1342  }
1343  }
1344  delays.resize(oi);
1345 
1346  if(recorder.init(path, false))
1347  {
1348  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
1349  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
1350  progressDialog->setMaximumSteps(ids.size());
1351  progressDialog->show();
1352  progressDialog->setCancelButtonVisible(true);
1353  UINFO("Decompress: rgb=%d depth=%d scan=%d userData=%d",
1354  dialog.isRgbExported()?1:0,
1355  dialog.isDepthExported()?1:0,
1356  dialog.isDepth2dExported()?1:0,
1357  dialog.isUserDataExported()?1:0);
1358 
1359  for(int i=0; i<ids.size() && !progressDialog->isCanceled(); ++i)
1360  {
1361  int id = ids.at(i);
1362 
1363  SensorData data;
1364  dbDriver_->getNodeData(id, data);
1365  cv::Mat depth, rgb, userData;
1366  LaserScan scan;
1367  data.uncompressDataConst(
1368  !dialog.isRgbExported()?0:&rgb,
1369  !dialog.isDepthExported()?0:&depth,
1370  !dialog.isDepth2dExported()?0:&scan,
1371  !dialog.isUserDataExported()?0:&userData);
1372  cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
1373  if(dialog.isOdomExported())
1374  {
1375  std::multimap<int, Link> links;
1376  dbDriver_->loadLinks(id, links, Link::kNeighbor);
1377  if(links.size() && links.begin()->first < id)
1378  {
1379  covariance = links.begin()->second.infMatrix().inv();
1380  }
1381  }
1382 
1383  rtabmap::SensorData sensorData;
1384  if(data.cameraModels().size())
1385  {
1386  sensorData = rtabmap::SensorData(
1387  scan,
1388  rgb,
1389  depth,
1390  data.cameraModels(),
1391  id,
1392  stamps.at(id),
1393  userData);
1394  }
1395  else
1396  {
1397  sensorData = rtabmap::SensorData(
1398  scan,
1399  rgb,
1400  depth,
1401  data.stereoCameraModels(),
1402  id,
1403  stamps.at(id),
1404  userData);
1405  }
1406  if(groundTruths.find(id)!=groundTruths.end())
1407  {
1408  sensorData.setGroundTruth(groundTruths.at(id));
1409  }
1410  if(gpsValues.find(id)!=gpsValues.end())
1411  {
1412  sensorData.setGPS(gpsValues.at(id));
1413  }
1414  if(sensorsValues.find(id)!=sensorsValues.end())
1415  {
1416  sensorData.setEnvSensors(sensorsValues.at(id));
1417  }
1418 
1419  recorder.addData(sensorData, dialog.isOdomExported()?poses.at(id):Transform(), covariance);
1420 
1421  progressDialog->appendText(tr("Exported node %1").arg(id));
1422  progressDialog->incrementStep();
1423  QApplication::processEvents();
1424  }
1425  progressDialog->setValue(progressDialog->maximumSteps());
1426  if(delays.size())
1427  {
1428  progressDialog->appendText(tr("Average frame rate=%1 Hz (Min=%2, Max=%3)")
1429  .arg(1.0/uMean(delays)).arg(1.0/uMax(delays)).arg(1.0/uMin(delays)));
1430  }
1431  progressDialog->appendText(tr("Export finished to \"%1\"!").arg(path));
1432  }
1433  else
1434  {
1435  UERROR("DataRecorder init failed?!");
1436  }
1437  }
1438  else
1439  {
1440  QMessageBox::warning(this, tr("Cannot export database"), tr("An output path must be set!"));
1441  }
1442  }
1443 }
1444 
1445 void DatabaseViewer::extractImages()
1446 {
1447  if(!dbDriver_ || ids_.size() == 0)
1448  {
1449  return;
1450  }
1451 
1452  QStringList formats;
1453  formats.push_back("id.jpg");
1454  formats.push_back("id.png");
1455  formats.push_back("timestamp.jpg");
1456  formats.push_back("timestamp.png");
1457  bool ok;
1458  QString format = QInputDialog::getItem(this, tr("Which RGB format?"), tr("Format:"), formats, 0, false, &ok);
1459  if(!ok)
1460  {
1461  return;
1462  }
1463 
1464  QString ext = format.split('.').back();
1465  bool useStamp = format.split('.').front().compare("timestamp") == 0;
1466  bool directoriesCreated = false;
1467  QString path = QFileDialog::getExistingDirectory(this, tr("Select directory where to save images..."), pathDatabase_);
1468  if(!path.isEmpty())
1469  {
1470  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
1471  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
1472  progressDialog->setMaximumSteps(ids_.size());
1473  progressDialog->setCancelButtonVisible(true);
1474  progressDialog->appendText(tr("Saving %1 images to %2...").arg(ids_.size()).arg(path));
1475  progressDialog->show();
1476 
1477  int imagesExported = 0;
1478  for(int i=0; i<ids_.size(); ++i)
1479  {
1480  QString id = QString::number(ids_.at(i));
1481 
1482  SensorData data;
1483  dbDriver_->getNodeData(ids_.at(i), data);
1484  data.uncompressData();
1485 
1486  if(!directoriesCreated)
1487  {
1488  //stereo
1489  if(!data.imageRaw().empty() && !data.rightRaw().empty())
1490  {
1491  QDir dir;
1492  dir.mkdir(QString("%1/left").arg(path));
1493  dir.mkdir(QString("%1/right").arg(path));
1494  dir.mkdir(QString("%1/calib").arg(path));
1495  directoriesCreated = true;
1496  }
1497  else if(!data.imageRaw().empty())
1498  {
1499  if(!data.depthRaw().empty())
1500  {
1501  QDir dir;
1502  dir.mkdir(QString("%1/rgb").arg(path));
1503  dir.mkdir(QString("%1/depth").arg(path));
1504  dir.mkdir(QString("%1/calib").arg(path));
1505  directoriesCreated = true;
1506  }
1507  }
1508  }
1509 
1510  if(!data.imageRaw().empty() && useStamp)
1511  {
1512  Transform p,gt;
1513  int m,w;
1514  std::string l;
1515  double stamp;
1516  std::vector<float> v;
1517  GPS gps;
1518  EnvSensors s;
1519  dbDriver_->getNodeInfo(ids_.at(i), p, m, w, l, stamp, gt, v, gps, s);
1520  if(stamp == 0.0)
1521  {
1522  UWARN("Node %d has null timestamp! Using id instead!", ids_.at(i));
1523  }
1524  else
1525  {
1526  id = QString::number(stamp, 'f');
1527  }
1528  }
1529 
1530  if(!data.imageRaw().empty())
1531  {
1532  if(!data.rightRaw().empty())
1533  {
1534  if(!cv::imwrite(QString("%1/left/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw()))
1535  UWARN("Failed saving \"%s\"", QString("%1/left/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
1536  if(!cv::imwrite(QString("%1/right/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.rightRaw()))
1537  UWARN("Failed saving \"%s\"", QString("%1/right/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
1538  UINFO(QString("Saved left/%1.%2 and right/%1.%2").arg(id).arg(ext).toStdString().c_str());
1539 
1540  if(databaseFileName_.empty())
1541  {
1542  UERROR("Cannot save calibration file, database name is empty!");
1543  }
1544  else if(data.stereoCameraModels().size()>=1 && data.stereoCameraModels().front().isValidForProjection())
1545  {
1546  for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
1547  {
1548  std::string cameraName = id.toStdString();
1549  if(data.stereoCameraModels().size()>1)
1550  {
1551  cameraName+="_"+uNumber2Str((int)i);
1552  }
1553  StereoCameraModel model(
1554  cameraName,
1555  data.imageRaw().size(),
1556  data.stereoCameraModels()[i].left().K(),
1557  data.stereoCameraModels()[i].left().D(),
1558  data.stereoCameraModels()[i].left().R(),
1559  data.stereoCameraModels()[i].left().P(),
1560  data.rightRaw().size(),
1561  data.stereoCameraModels()[i].right().K(),
1562  data.stereoCameraModels()[i].right().D(),
1563  data.stereoCameraModels()[i].right().R(),
1564  data.stereoCameraModels()[i].right().P(),
1565  data.stereoCameraModels()[i].R(),
1566  data.stereoCameraModels()[i].T(),
1567  data.stereoCameraModels()[i].E(),
1568  data.stereoCameraModels()[i].F(),
1569  data.stereoCameraModels()[i].left().localTransform());
1570  if(model.save(path.toStdString() + "/calib"))
1571  {
1572  UINFO("Saved stereo calibration \"%s\"", (path.toStdString()+"/calib/"+cameraName+".yaml").c_str());
1573  }
1574  else
1575  {
1576  UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/calib/"+cameraName+".yaml").c_str());
1577  }
1578  }
1579  }
1580  }
1581  else
1582  {
1583  if(!data.depthRaw().empty())
1584  {
1585  if(!cv::imwrite(QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw()))
1586  UWARN("Failed saving \"%s\"", QString("%1/rgb/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
1587  if(!cv::imwrite(QString("%1/depth/%2.png").arg(path).arg(id).toStdString(), data.depthRaw().type()==CV_32FC1?util2d::cvtDepthFromFloat(data.depthRaw()):data.depthRaw()))
1588  UWARN("Failed saving \"%s\"", QString("%1/depth/%2.png").arg(path).arg(id).toStdString().c_str());
1589  UINFO(QString("Saved rgb/%1.%2 and depth/%1.png").arg(id).arg(ext).toStdString().c_str());
1590  }
1591  else
1592  {
1593  if(!cv::imwrite(QString("%1/%2.%3").arg(path).arg(id).arg(ext).toStdString(), data.imageRaw()))
1594  UWARN("Failed saving \"%s\"", QString("%1/%2.%3").arg(path).arg(id).arg(ext).toStdString().c_str());
1595  else
1596  UINFO(QString("Saved %1.%2").arg(id).arg(ext).toStdString().c_str());
1597  }
1598 
1599  if(databaseFileName_.empty())
1600  {
1601  UERROR("Cannot save calibration file, database name is empty!");
1602  }
1603  else if(data.cameraModels().size() >= 1 && data.cameraModels().front().isValidForProjection())
1604  {
1605  for(size_t i=0; i<data.cameraModels().size(); ++i)
1606  {
1607  std::string cameraName = id.toStdString();
1608  if(data.cameraModels().size()>1)
1609  {
1610  cameraName+="_"+uNumber2Str((int)i);
1611  }
1612  CameraModel model(cameraName,
1613  data.imageRaw().size(),
1614  data.cameraModels()[i].K(),
1615  data.cameraModels()[i].D(),
1616  data.cameraModels()[i].R(),
1617  data.cameraModels()[i].P(),
1618  data.cameraModels()[i].localTransform());
1619  std::string dirPrefix = "";
1620  if(!data.depthRaw().empty())
1621  {
1622  dirPrefix = "/calib";
1623  }
1624  if(model.save(path.toStdString()+dirPrefix))
1625  {
1626  UINFO("Saved calibration \"%s\"", (path.toStdString()+dirPrefix+"/"+cameraName+".yaml").c_str());
1627  }
1628  else
1629  {
1630  UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/"+cameraName+".yaml").c_str());
1631  }
1632  }
1633  }
1634  }
1635  ++imagesExported;
1636  }
1637  progressDialog->incrementStep();
1638  QApplication::processEvents();
1639  if(progressDialog->isCanceled())
1640  {
1641  progressDialog->appendText("Cancel!");
1642  break;
1643  }
1644  }
1645  progressDialog->appendText("Done!");
1646  progressDialog->setValue(progressDialog->maximumSteps());
1647 
1648  QMessageBox::information(this, tr("Exporting"), tr("%1 images exported!").arg(imagesExported));
1649  }
1650 }
1651 
1652 void DatabaseViewer::updateIds()
1653 {
1654  if(!dbDriver_)
1655  {
1656  return;
1657  }
1658 
1659  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
1660  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
1661  int progressSteps = 5;
1662  if(ui_->graphViewer->isVisible() || ui_->dockWidget_occupancyGridView->isVisible())
1663  {
1664  ++progressSteps;
1665  }
1666  if(ui_->textEdit_info->isVisible())
1667  {
1668  ++progressSteps;
1669  }
1670  if(ui_->toolBox_statistics->isVisible())
1671  {
1672  ++progressSteps;
1673  }
1674  progressDialog->setMaximumSteps(progressSteps);
1675  progressDialog->show();
1676  progressDialog->setCancelButtonVisible(false);
1677 
1678  progressDialog->appendText(tr("Loading all ids..."));
1679  QApplication::processEvents();
1680  uSleep(100);
1681  QApplication::processEvents();
1682 
1683  UINFO("Loading all IDs...");
1684  std::set<int> ids;
1685  dbDriver_->getAllNodeIds(ids);
1686  ids_ = QList<int>::fromStdList(std::list<int>(ids.begin(), ids.end()));
1687  lastWmIds_.clear();
1688  dbDriver_->getLastNodeIds(lastWmIds_);
1689  idToIndex_.clear();
1690  mapIds_.clear();
1691  weights_.clear();
1692  wmStates_.clear();
1693  odomPoses_.clear();
1694  groundTruthPoses_.clear();
1695  gpsPoses_.clear();
1696  gpsValues_.clear();
1697  lastSliderIndexBrowsed_ = 0;
1698  ui_->checkBox_wmState->setVisible(false);
1699  ui_->checkBox_alignPosesWithGroundTruth->setVisible(false);
1700  ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(false);
1701  ui_->doubleSpinBox_optimizationScale->setVisible(false);
1702  ui_->label_scale_title->setVisible(false);
1703  ui_->label_rmse->setVisible(false);
1704  ui_->label_rmse_title->setVisible(false);
1705  ui_->checkBox_ignoreIntermediateNodes->setVisible(false);
1706  ui_->label_ignoreINtermediateNdoes->setVisible(false);
1707  ui_->label_alignPosesWithGroundTruth->setVisible(false);
1708  ui_->label_alignScansCloudsWithGroundTruth->setVisible(false);
1709  ui_->menuEdit->setEnabled(true);
1710  ui_->actionGenerate_3D_map_pcd->setEnabled(true);
1711  ui_->actionExport->setEnabled(true);
1712  ui_->actionExtract_images->setEnabled(true);
1713  ui_->menuExport_poses->setEnabled(false);
1714  ui_->menuExport_GPS->setEnabled(false);
1715  ui_->actionPoses_KML->setEnabled(false);
1716  ui_->actionEdit_optimized_2D_map->setEnabled(false);
1717  ui_->actionExport_saved_2D_map->setEnabled(false);
1718  ui_->actionImport_2D_map->setEnabled(false);
1719  ui_->actionRegenerate_optimized_2D_map->setEnabled(false);
1720  ui_->actionView_optimized_mesh->setEnabled(false);
1721  ui_->actionExport_optimized_mesh->setEnabled(false);
1722  ui_->actionUpdate_optimized_mesh->setEnabled(uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.13.0") >= 0);
1723  links_.clear();
1724  linksAdded_.clear();
1725  linksRefined_.clear();
1726  linksRemoved_.clear();
1727  ui_->toolBox_statistics->clear();
1728  ui_->label_optimizeFrom->setText(tr("Root"));
1729 
1730  progressDialog->appendText(tr("%1 ids loaded!").arg(ids.size()));
1731  progressDialog->incrementStep();
1732  progressDialog->appendText(tr("Loading all links..."));
1733  QApplication::processEvents();
1734  uSleep(100);
1735  QApplication::processEvents();
1736 
1737  std::multimap<int, Link> unilinks;
1738  dbDriver_->getAllLinks(unilinks, true, true);
1739  UDEBUG("%d total links loaded", (int)unilinks.size());
1740  // add both direction links
1741  std::multimap<int, Link> links;
1742  for(std::multimap<int, Link>::iterator iter=unilinks.begin(); iter!=unilinks.end(); ++iter)
1743  {
1744  links.insert(*iter);
1745  if(graph::findLink(unilinks, iter->second.to(), iter->second.from(), false) == unilinks.end())
1746  {
1747  links.insert(std::make_pair(iter->second.to(), iter->second.inverse()));
1748  }
1749  }
1750 
1751  progressDialog->appendText(tr("%1 links loaded!").arg(unilinks.size()));
1752  progressDialog->incrementStep();
1753  progressDialog->appendText("Loading Working Memory state...");
1754  QApplication::processEvents();
1755  uSleep(100);
1756  QApplication::processEvents();
1757 
1758  infoTotalOdom_ = 0.0;
1759  Transform previousPose;
1760  infoSessions_ = ids_.size()?1:0;
1761  infoTotalTime_ = 0.0;
1762  double previousStamp = 0.0;
1763  infoReducedGraph_ = false;
1764  std::map<int, std::vector<int> > wmStates = dbDriver_->getAllStatisticsWmStates();
1765 
1766  progressDialog->appendText("Loading Working Memory state... done!");
1767  progressDialog->incrementStep();
1768  progressDialog->appendText("Loading info for all nodes...");
1769  QApplication::processEvents();
1770  uSleep(100);
1771  QApplication::processEvents();
1772 
1773  int lastValidNodeId = 0;
1774  for(int i=0; i<ids_.size(); ++i)
1775  {
1776  idToIndex_.insert(ids_[i], i);
1777 
1778  Transform p, g;
1779  int w;
1780  std::string l;
1781  double s;
1782  int mapId;
1783  std::vector<float> v;
1784  GPS gps;
1785  EnvSensors sensors;
1786  dbDriver_->getNodeInfo(ids_[i], p, mapId, w, l, s, g, v, gps, sensors);
1787  mapIds_.insert(std::make_pair(ids_[i], mapId));
1788  weights_.insert(std::make_pair(ids_[i], w));
1789  if(w>=0)
1790  {
1791  for(std::multimap<int, Link>::iterator iter=links.find(ids_[i]); iter!=links.end() && iter->first==ids_[i]; ++iter)
1792  {
1793  // Make compatible with old databases, when "weight=-1" was not yet introduced to identify ignored nodes
1794  if(iter->second.type() == Link::kNeighbor || iter->second.type() == Link::kNeighborMerged)
1795  {
1796  lastValidNodeId = ids_[i];
1797  }
1798  }
1799  }
1800  if(wmStates.find(ids_[i]) != wmStates.end())
1801  {
1802  wmStates_.insert(std::make_pair(ids_[i], wmStates.at(ids_[i])));
1803  ui_->checkBox_wmState->setVisible(true);
1804  }
1805  if(w < 0)
1806  {
1807  ui_->checkBox_ignoreIntermediateNodes->setVisible(true);
1808  ui_->label_ignoreINtermediateNdoes->setVisible(true);
1809  }
1810  if(i>0)
1811  {
1812  if(mapIds_.at(ids_[i-1]) == mapId)
1813  {
1814  if(!p.isNull() && !previousPose.isNull())
1815  {
1816  infoTotalOdom_ += p.getDistance(previousPose);
1817  }
1818 
1819  if(previousStamp > 0.0 && s > 0.0)
1820  {
1821  infoTotalTime_ += s-previousStamp;
1822  }
1823  }
1824  else
1825  {
1826  ++infoSessions_;
1827  }
1828  }
1829  previousStamp=s;
1830  previousPose=p;
1831 
1832  //links
1833  for(std::multimap<int, Link>::iterator jter=links.find(ids_[i]); jter!=links.end() && jter->first == ids_[i]; ++jter)
1834  {
1835  if(jter->second.type() == Link::kNeighborMerged)
1836  {
1837  infoReducedGraph_ = true;
1838  }
1839 
1840  std::multimap<int, Link>::iterator invertedLinkIter = graph::findLink(links, jter->second.to(), jter->second.from(), false, jter->second.type());
1841  if( jter->second.isValid() && // null transform means a rehearsed location
1842  ids.find(jter->second.from()) != ids.end() &&
1843  (ids.find(jter->second.to()) != ids.end() || jter->second.to()<0) && // to add landmark links
1844  graph::findLink(links_, jter->second.from(), jter->second.to(), false, jter->second.type()) == links_.end() &&
1845  invertedLinkIter != links.end() &&
1846  w != -9)
1847  {
1848  // check if user_data is set in opposite direction
1849  if(jter->second.userDataCompressed().cols == 0 &&
1850  invertedLinkIter->second.userDataCompressed().cols != 0)
1851  {
1852  links_.insert(std::make_pair(invertedLinkIter->second.from(), invertedLinkIter->second));
1853  }
1854  else
1855  {
1856  links_.insert(std::make_pair(ids_[i], jter->second));
1857  }
1858  }
1859  }
1860  // Add pose
1861  odomPoses_.insert(std::make_pair(ids_[i], p));
1862  if(!g.isNull())
1863  {
1864  groundTruthPoses_.insert(std::make_pair(ids_[i], g));
1865  }
1866  if(gps.stamp() > 0.0)
1867  {
1868  gpsValues_.insert(std::make_pair(ids_[i], gps));
1869 
1870  cv::Point3f p(0.0f,0.0f,0.0f);
1871  if(!gpsPoses_.empty())
1872  {
1873  GeodeticCoords coords = gps.toGeodeticCoords();
1874  GPS originGPS = gpsValues_.begin()->second;
1875  p = coords.toENU_WGS84(originGPS.toGeodeticCoords());
1876  }
1877  Transform pose(p.x, p.y, p.z, 0.0f, 0.0f, (float)((-(gps.bearing()-90))*M_PI/180.0));
1878  gpsPoses_.insert(std::make_pair(ids_[i], pose));
1879  }
1880  }
1881 
1882  progressDialog->appendText("Loading info for all nodes... done!");
1883  progressDialog->incrementStep();
1884  progressDialog->appendText("Loading optimized poses and maps...");
1885  QApplication::processEvents();
1886  uSleep(100);
1887  QApplication::processEvents();
1888 
1889  if(!groundTruthPoses_.empty() || !gpsPoses_.empty())
1890  {
1891  ui_->checkBox_alignPosesWithGroundTruth->setVisible(true);
1892  ui_->doubleSpinBox_optimizationScale->setVisible(true);
1893  ui_->label_scale_title->setVisible(true);
1894  ui_->label_rmse->setVisible(true);
1895  ui_->label_rmse_title->setVisible(true);
1896  ui_->label_alignPosesWithGroundTruth->setVisible(true);
1897 
1898  if(!groundTruthPoses_.empty())
1899  {
1900  ui_->label_alignPosesWithGroundTruth->setText(tr("Align poses with ground truth"));
1901  ui_->checkBox_alignScansCloudsWithGroundTruth->setVisible(true);
1902  ui_->label_alignScansCloudsWithGroundTruth->setVisible(true);
1903  }
1904  else
1905  {
1906  ui_->label_alignPosesWithGroundTruth->setText(tr("Align poses with GPS"));
1907  }
1908  }
1909  if(!gpsValues_.empty())
1910  {
1911  ui_->menuExport_GPS->setEnabled(true);
1912  ui_->actionPoses_KML->setEnabled(true);
1913  }
1914 
1915  float xMin, yMin, cellSize;
1916  bool hasMap = !dbDriver_->load2DMap(xMin, yMin, cellSize).empty();
1917  ui_->actionEdit_optimized_2D_map->setEnabled(hasMap);
1918  ui_->actionExport_saved_2D_map->setEnabled(hasMap);
1919  ui_->actionImport_2D_map->setEnabled(hasMap);
1920  ui_->actionRegenerate_optimized_2D_map->setEnabled(uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.17.0") >= 0);
1921 
1922  if(!dbDriver_->loadOptimizedMesh().empty())
1923  {
1924  ui_->actionView_optimized_mesh->setEnabled(true);
1925  ui_->actionExport_optimized_mesh->setEnabled(true);
1926  }
1927 
1928  UINFO("Loaded %d ids, %d poses and %d links", (int)ids_.size(), (int)odomPoses_.size(), (int)links_.size());
1929 
1930  progressDialog->appendText("Loading optimized poses and maps... done!");
1931  progressDialog->incrementStep();
1932  QApplication::processEvents();
1933  uSleep(100);
1934  QApplication::processEvents();
1935 
1936  if(ids_.size() && ui_->toolBox_statistics->isVisible())
1937  {
1938  progressDialog->appendText("Loading statistics...");
1939  QApplication::processEvents();
1940  uSleep(100);
1941  QApplication::processEvents();
1942 
1943  UINFO("Update statistics...");
1944  updateStatistics();
1945 
1946  progressDialog->appendText("Loading statistics... done!");
1947  progressDialog->incrementStep();
1948  QApplication::processEvents();
1949  uSleep(100);
1950  QApplication::processEvents();
1951  }
1952 
1953 
1954  ui_->textEdit_info->clear();
1955  if(ui_->textEdit_info->isVisible())
1956  {
1957  progressDialog->appendText("Update database info...");
1958  QApplication::processEvents();
1959  uSleep(100);
1960  QApplication::processEvents();
1961 
1962  updateInfo();
1963 
1964  progressDialog->appendText("Update database info... done!");
1965  progressDialog->incrementStep();
1966  QApplication::processEvents();
1967  uSleep(100);
1968  QApplication::processEvents();
1969  }
1970 
1971  if(ids.size())
1972  {
1973  if(odomPoses_.size())
1974  {
1975  bool nullPoses = odomPoses_.begin()->second.isNull();
1976  for(std::map<int,Transform>::iterator iter=odomPoses_.begin(); iter!=odomPoses_.end(); ++iter)
1977  {
1978  if((!iter->second.isNull() && nullPoses) ||
1979  (iter->second.isNull() && !nullPoses))
1980  {
1981  if(iter->second.isNull())
1982  {
1983  UWARN("Pose %d is null!", iter->first);
1984  }
1985  UWARN("Mixed valid and null poses! Ignoring graph...");
1986  odomPoses_.clear();
1987  links_.clear();
1988  break;
1989  }
1990  }
1991  if(nullPoses)
1992  {
1993  odomPoses_.clear();
1994  links_.clear();
1995  }
1996 
1997  if(odomPoses_.size())
1998  {
1999  ui_->spinBox_optimizationsFrom->setRange(odomPoses_.begin()->first, odomPoses_.rbegin()->first);
2000  ui_->spinBox_optimizationsFrom->setValue(odomPoses_.begin()->first);
2001  ui_->label_optimizeFrom->setText(tr("Root [%1, %2]").arg(odomPoses_.begin()->first).arg(odomPoses_.rbegin()->first));
2002  }
2003  }
2004 
2005  if(lastValidNodeId>0)
2006  {
2007  // find full connected graph from last node in working memory
2008  Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
2009 
2010  std::map<int, rtabmap::Transform> posesOut;
2011  std::multimap<int, rtabmap::Link> linksOut;
2012  UINFO("Get connected graph from %d (%d poses, %d links)", lastValidNodeId, (int)odomPoses_.size(), (int)links_.size());
2013  optimizer->getConnectedGraph(
2014  lastValidNodeId,
2015  odomPoses_,
2016  links_,
2017  posesOut,
2018  linksOut);
2019 
2020  if(!posesOut.empty())
2021  {
2022  bool optimizeFromGraphEnd = Parameters::defaultRGBDOptimizeFromGraphEnd();
2023  Parameters::parse(dbDriver_->getLastParameters(), Parameters::kRGBDOptimizeFromGraphEnd(), optimizeFromGraphEnd);
2024  if(optimizeFromGraphEnd)
2025  {
2026  ui_->spinBox_optimizationsFrom->setValue(posesOut.rbegin()->first);
2027  }
2028  else
2029  {
2030  ui_->spinBox_optimizationsFrom->setValue(posesOut.lower_bound(1)->first);
2031  }
2032  }
2033 
2034  delete optimizer;
2035  }
2036  }
2037 
2038  ui_->menuExport_poses->setEnabled(!odomPoses_.empty());
2039  graphes_.clear();
2040  graphLinks_.clear();
2041  neighborLinks_.clear();
2042  loopLinks_.clear();
2043  for(std::multimap<int, rtabmap::Link>::iterator iter = links_.begin(); iter!=links_.end(); ++iter)
2044  {
2045  if(!iter->second.transform().isNull())
2046  {
2047  if(iter->second.type() == rtabmap::Link::kNeighbor ||
2048  iter->second.type() == rtabmap::Link::kNeighborMerged)
2049  {
2050  neighborLinks_.append(iter->second);
2051  }
2052  else if(iter->second.from()!=iter->second.to())
2053  {
2054  loopLinks_.append(iter->second);
2055  }
2056  }
2057  else
2058  {
2059  UERROR("Transform null for link from %d to %d", iter->first, iter->second.to());
2060  }
2061  }
2062 
2063  if(ids_.size())
2064  {
2065  ui_->horizontalSlider_A->setMinimum(0);
2066  ui_->horizontalSlider_B->setMinimum(0);
2067  ui_->horizontalSlider_A->setMaximum(ids_.size()-1);
2068  ui_->horizontalSlider_B->setMaximum(ids_.size()-1);
2069  ui_->horizontalSlider_A->setEnabled(true);
2070  ui_->horizontalSlider_B->setEnabled(true);
2071  ui_->horizontalSlider_A->setSliderPosition(0);
2072  ui_->horizontalSlider_B->setSliderPosition(0);
2073  sliderAValueChanged(0);
2074  sliderBValueChanged(0);
2075  }
2076  else
2077  {
2078  ui_->horizontalSlider_A->setEnabled(false);
2079  ui_->horizontalSlider_B->setEnabled(false);
2080  ui_->label_idA->setText("NaN");
2081  ui_->label_idB->setText("NaN");
2082  }
2083 
2084  if(neighborLinks_.size())
2085  {
2086  ui_->horizontalSlider_neighbors->setMinimum(0);
2087  ui_->horizontalSlider_neighbors->setMaximum(neighborLinks_.size()-1);
2088  ui_->horizontalSlider_neighbors->setEnabled(true);
2089  ui_->horizontalSlider_neighbors->setSliderPosition(0);
2090  }
2091  else
2092  {
2093  ui_->horizontalSlider_neighbors->setEnabled(false);
2094  }
2095 
2096  if(ids_.size())
2097  {
2098  updateLoopClosuresSlider();
2099  if(ui_->graphViewer->isVisible() || ui_->dockWidget_occupancyGridView->isVisible())
2100  {
2101  progressDialog->appendText("Updating Graph View...");
2102  QApplication::processEvents();
2103  uSleep(100);
2104  QApplication::processEvents();
2105 
2106  updateGraphView();
2107 
2108  progressDialog->appendText("Updating Graph View... done!");
2109  progressDialog->incrementStep();
2110  QApplication::processEvents();
2111  uSleep(100);
2112  QApplication::processEvents();
2113  }
2114  }
2115  progressDialog->setValue(progressDialog->maximumSteps());
2116 }
2117 
2118 void DatabaseViewer::updateInfo()
2119 {
2120  UINFO("Update database info...");
2121  if(dbDriver_)
2122  {
2123  if(ui_->textEdit_info->toPlainText().isEmpty())
2124  {
2125  ui_->textEdit_info->append(tr("Path:\t\t%1").arg(dbDriver_->getUrl().c_str()));
2126  ui_->textEdit_info->append(tr("Version:\t\t%1").arg(dbDriver_->getDatabaseVersion().c_str()));
2127  ui_->textEdit_info->append(tr("Sessions:\t\t%1").arg(infoSessions_));
2128  if(infoReducedGraph_)
2129  {
2130  ui_->textEdit_info->append(tr("Total odometry length:\t%1 m (approx. as graph has been reduced)").arg(infoTotalOdom_));
2131  }
2132  else
2133  {
2134  ui_->textEdit_info->append(tr("Total odometry length:\t%1 m").arg(infoTotalOdom_));
2135  }
2136 
2137  int lastWordIdId = 0;
2138  int wordsDim = 0;
2139  int wordsType = 0;
2140  dbDriver_->getLastWordId(lastWordIdId);
2141  if(lastWordIdId>0)
2142  {
2143  std::set<int> ids;
2144  ids.insert(lastWordIdId);
2145  std::list<VisualWord *> vws;
2146  dbDriver_->loadWords(ids, vws);
2147  if(!vws.empty())
2148  {
2149  wordsDim = vws.front()->getDescriptor().cols;
2150  wordsType = vws.front()->getDescriptor().type();
2151  delete vws.front();
2152  vws.clear();
2153  }
2154  }
2155 
2156  ui_->textEdit_info->append(tr("Total time:\t\t%1").arg(QDateTime::fromMSecsSinceEpoch(infoTotalTime_*1000).toUTC().toString("hh:mm:ss.zzz")));
2157  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()));
2158  ui_->textEdit_info->append(tr("WM:\t\t%1 nodes and %2 words").arg(dbDriver_->getLastNodesSize()).arg(dbDriver_->getLastDictionarySize()));
2159  ui_->textEdit_info->append(tr("Global graph:\t%1 poses and %2 links").arg(odomPoses_.size()).arg(links_.size()));
2160  ui_->textEdit_info->append(tr("Ground truth:\t%1 poses").arg(groundTruthPoses_.size()));
2161  ui_->textEdit_info->append(tr("GPS:\t%1 poses").arg(gpsValues_.size()));
2162  ui_->textEdit_info->append("");
2163  long total = 0;
2164  long dbSize = UFile::length(dbDriver_->getUrl());
2165  long mem = dbSize;
2166  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"));
2167  mem = dbDriver_->getNodesMemoryUsed();
2168  total+=mem;
2169  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"));
2170  mem = dbDriver_->getLinksMemoryUsed();
2171  total+=mem;
2172  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"));
2173  mem = dbDriver_->getImagesMemoryUsed();
2174  total+=mem;
2175  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"));
2176  mem = dbDriver_->getDepthImagesMemoryUsed();
2177  total+=mem;
2178  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"));
2179  mem = dbDriver_->getCalibrationsMemoryUsed();
2180  total+=mem;
2181  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"));
2182  mem = dbDriver_->getGridsMemoryUsed();
2183  total+=mem;
2184  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"));
2185  mem = dbDriver_->getLaserScansMemoryUsed();
2186  total+=mem;
2187  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"));
2188  mem = dbDriver_->getUserDataMemoryUsed();
2189  total+=mem;
2190  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"));
2191  mem = dbDriver_->getWordsMemoryUsed();
2192  total+=mem;
2193  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"));
2194  mem = dbDriver_->getFeaturesMemoryUsed();
2195  total+=mem;
2196  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"));
2197  mem = dbDriver_->getStatisticsMemoryUsed();
2198  total+=mem;
2199  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"));
2200  mem = dbSize - total;
2201  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"));
2202  ui_->textEdit_info->append("");
2203  std::set<int> idsWithoutBad;
2204  dbDriver_->getAllNodeIds(idsWithoutBad, false, true);
2205  int infoBadcountInLTM = 0;
2206  int infoBadCountInGraph = 0;
2207  for(int i=0; i<ids_.size(); ++i)
2208  {
2209  if(idsWithoutBad.find(ids_[i]) == idsWithoutBad.end())
2210  {
2211  ++infoBadcountInLTM;
2212  if(odomPoses_.find(ids_[i]) != odomPoses_.end())
2213  {
2214  ++infoBadCountInGraph;
2215  }
2216  }
2217  }
2218  ui_->textEdit_info->append(tr("%1 bad signatures in LTM").arg(infoBadcountInLTM));
2219  ui_->textEdit_info->append(tr("%1 bad signatures in the global graph").arg(infoBadCountInGraph));
2220  ui_->textEdit_info->append("");
2221  ParametersMap parameters = dbDriver_->getLastParameters();
2222  QFontMetrics metrics(ui_->textEdit_info->font());
2223  int tabW = ui_->textEdit_info->tabStopWidth();
2224  for(ParametersMap::iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
2225  {
2226  int strW = metrics.width(QString(iter->first.c_str()) + "=");
2227  ui_->textEdit_info->append(tr("%1=%2%3")
2228  .arg(iter->first.c_str())
2229  .arg(strW < tabW?"\t\t\t\t":strW < tabW*2?"\t\t\t":strW < tabW*3?"\t\t":"\t")
2230  .arg(iter->second.c_str()));
2231  }
2232 
2233  // move back the cursor at the beginning
2234  ui_->textEdit_info->moveCursor(QTextCursor::Start) ;
2235  ui_->textEdit_info->ensureCursorVisible() ;
2236  }
2237  }
2238  else
2239  {
2240  ui_->textEdit_info->clear();
2241  }
2242 }
2243 
2244 void DatabaseViewer::updateStatistics()
2245 {
2246  UDEBUG("");
2247  if(dbDriver_)
2248  {
2249  ui_->toolBox_statistics->clear();
2250  double firstStamp = 0.0;
2251  std::map<int, std::pair<std::map<std::string, float>, double> > allStats = dbDriver_->getAllStatistics();
2252 
2253  std::map<std::string, std::pair<std::vector<qreal>, std::vector<qreal> > > allData;
2254  std::map<std::string, int > allDataOi;
2255 
2256  for(std::map<int, std::pair<std::map<std::string, float>, double> >::iterator jter=allStats.begin(); jter!=allStats.end(); ++jter)
2257  {
2258  double stamp=jter->second.second;
2259  std::map<std::string, float> & statistics = jter->second.first;
2260  if(firstStamp==0.0)
2261  {
2262  firstStamp = stamp;
2263  }
2264  for(std::map<std::string, float>::iterator iter=statistics.begin(); iter!=statistics.end(); ++iter)
2265  {
2266  if(allData.find(iter->first) == allData.end())
2267  {
2268  //initialize data vectors
2269  allData.insert(std::make_pair(iter->first, std::make_pair(std::vector<qreal>(allStats.size(), 0.0f), std::vector<qreal>(allStats.size(), 0.0f) )));
2270  allDataOi.insert(std::make_pair(iter->first, 0));
2271  }
2272 
2273  int & oi = allDataOi.at(iter->first);
2274  allData.at(iter->first).first[oi] = ui_->checkBox_timeStats->isChecked()?qreal(stamp-firstStamp):jter->first;
2275  allData.at(iter->first).second[oi] = iter->second;
2276  ++oi;
2277  }
2278  }
2279 
2280  for(std::map<std::string, std::pair<std::vector<qreal>, std::vector<qreal> > >::iterator iter=allData.begin(); iter!=allData.end(); ++iter)
2281  {
2282  int oi = allDataOi.at(iter->first);
2283  iter->second.first.resize(oi);
2284  iter->second.second.resize(oi);
2285  ui_->toolBox_statistics->updateStat(iter->first.c_str(), iter->second.first, iter->second.second, true);
2286  }
2287  }
2288  UDEBUG("");
2289 }
2290 
2291 void DatabaseViewer::selectObstacleColor()
2292 {
2293  QColor c = QColorDialog::getColor(ui_->lineEdit_obstacleColor->text(), this);
2294  if(c.isValid())
2295  {
2296  ui_->lineEdit_obstacleColor->setText(c.name());
2297  }
2298 }
2299 void DatabaseViewer::selectGroundColor()
2300 {
2301  QColor c = QColorDialog::getColor(ui_->lineEdit_groundColor->text(), this);
2302  if(c.isValid())
2303  {
2304  ui_->lineEdit_groundColor->setText(c.name());
2305  }
2306 }
2307 void DatabaseViewer::selectEmptyColor()
2308 {
2309  QColor c = QColorDialog::getColor(ui_->lineEdit_emptyColor->text(), this);
2310  if(c.isValid())
2311  {
2312  ui_->lineEdit_emptyColor->setText(c.name());
2313  }
2314 }
2315 void DatabaseViewer::editDepthImage()
2316 {
2317  if(dbDriver_ && ids_.size())
2318  {
2319  if(lastSliderIndexBrowsed_>= ids_.size())
2320  {
2321  lastSliderIndexBrowsed_ = ui_->horizontalSlider_A->value();
2322  }
2323  int id = ids_.at(lastSliderIndexBrowsed_);
2324  SensorData data;
2325  dbDriver_->getNodeData(id, data, true, false, false, false);
2326  data.uncompressData();
2327  if(!data.depthRaw().empty())
2328  {
2329  editDepthArea_->setColorMap(lastSliderIndexBrowsed_ == ui_->horizontalSlider_B->value()?ui_->graphicsView_B->getDepthColorMap():ui_->graphicsView_A->getDepthColorMap());
2330  editDepthArea_->setImage(data.depthRaw(), data.imageRaw());
2331  if(editDepthDialog_->exec() == QDialog::Accepted && editDepthArea_->isModified())
2332  {
2333  cv::Mat depth = editDepthArea_->getModifiedImage();
2334  UASSERT(data.depthRaw().type() == depth.type());
2335  UASSERT(data.depthRaw().cols == depth.cols);
2336  UASSERT(data.depthRaw().rows == depth.rows);
2337  dbDriver_->updateDepthImage(id, depth);
2338  this->update3dView();
2339  }
2340  }
2341  }
2342 }
2343 
2344 void DatabaseViewer::exportPosesRaw()
2345 {
2346  exportPoses(0);
2347 }
2348 void DatabaseViewer::exportPosesRGBDSLAMMotionCapture()
2349 {
2350  exportPoses(1);
2351 }
2352 void DatabaseViewer::exportPosesRGBDSLAM()
2353 {
2354  exportPoses(10);
2355 }
2356 void DatabaseViewer::exportPosesRGBDSLAMID()
2357 {
2358  exportPoses(11);
2359 }
2360 void DatabaseViewer::exportPosesKITTI()
2361 {
2362  exportPoses(2);
2363 }
2364 void DatabaseViewer::exportPosesTORO()
2365 {
2366  exportPoses(3);
2367 }
2368 void DatabaseViewer::exportPosesG2O()
2369 {
2370  exportPoses(4);
2371 }
2372 void DatabaseViewer::exportPosesKML()
2373 {
2374  exportPoses(5);
2375 }
2376 
2377 void DatabaseViewer::exportPoses(int format)
2378 {
2379  QStringList types;
2380  types.push_back("Map's graph (see Graph View)");
2381  types.push_back("Odometry");
2382  if(!groundTruthPoses_.empty())
2383  {
2384  types.push_back("Ground Truth");
2385  }
2386  bool ok;
2387  QString type = QInputDialog::getItem(this, tr("Which poses?"), tr("Poses:"), types, 0, false, &ok);
2388  if(!ok)
2389  {
2390  return;
2391  }
2392  bool odometry = type.compare("Odometry") == 0;
2393  bool groundTruth = type.compare("Ground Truth") == 0;
2394 
2395  if(groundTruth && groundTruthPoses_.empty())
2396  {
2397  QMessageBox::warning(this, tr("Cannot export poses"), tr("No ground truth poses in database?!"));
2398  return;
2399  }
2400  else if(!odometry && graphes_.empty())
2401  {
2402  this->updateGraphView();
2403  if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
2404  {
2405  QMessageBox::warning(this, tr("Cannot export poses"), tr("No graph in database?!"));
2406  return;
2407  }
2408  }
2409  else if(odometry && odomPoses_.empty())
2410  {
2411  QMessageBox::warning(this, tr("Cannot export poses"), tr("No odometry poses in database?!"));
2412  return;
2413  }
2414 
2415  if(format == 5)
2416  {
2417  if(gpsValues_.empty() || gpsPoses_.empty())
2418  {
2419  QMessageBox::warning(this, tr("Cannot export poses"), tr("No GPS in database?!"));
2420  }
2421  else
2422  {
2423  std::map<int, rtabmap::Transform> graph;
2424  if(groundTruth)
2425  {
2426  graph = groundTruthPoses_;
2427  }
2428  else if(odometry)
2429  {
2430  graph = odomPoses_;
2431  }
2432  else
2433  {
2434  graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
2435  }
2436 
2437 
2438  //align with ground truth for more meaningful results
2439  pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
2440  cloud1.resize(graph.size());
2441  cloud2.resize(graph.size());
2442  int oi = 0;
2443  int idFirst = 0;
2444  for(std::map<int, Transform>::const_iterator iter=gpsPoses_.begin(); iter!=gpsPoses_.end(); ++iter)
2445  {
2446  std::map<int, Transform>::iterator iter2 = graph.find(iter->first);
2447  if(iter2!=graph.end())
2448  {
2449  if(oi==0)
2450  {
2451  idFirst = iter->first;
2452  }
2453  cloud1[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
2454  cloud2[oi++] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z());
2455  }
2456  }
2457 
2458  Transform t = Transform::getIdentity();
2459  if(oi>5)
2460  {
2461  cloud1.resize(oi);
2462  cloud2.resize(oi);
2463 
2464  t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
2465  }
2466  else if(idFirst)
2467  {
2468  t = gpsPoses_.at(idFirst) * graph.at(idFirst).inverse();
2469  }
2470 
2471  std::map<int, GPS> values;
2472  GeodeticCoords origin = gpsValues_.begin()->second.toGeodeticCoords();
2473  for(std::map<int, Transform>::iterator iter=graph.begin(); iter!=graph.end(); ++iter)
2474  {
2475  iter->second = t * iter->second;
2476 
2477  GeodeticCoords coord;
2478  coord.fromENU_WGS84(cv::Point3d(iter->second.x(), iter->second.y(), iter->second.z()), origin);
2479  double bearing = -(iter->second.theta()*180.0/M_PI-90.0);
2480  if(bearing < 0)
2481  {
2482  bearing += 360;
2483  }
2484 
2485  Transform p, g;
2486  int w;
2487  std::string l;
2488  double stamp=0.0;
2489  int mapId;
2490  std::vector<float> v;
2491  GPS gps;
2492  EnvSensors sensors;
2493  dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors);
2494  values.insert(std::make_pair(iter->first, GPS(stamp, coord.longitude(), coord.latitude(), coord.altitude(), 0, 0)));
2495  }
2496 
2497  QString output = pathDatabase_ + QDir::separator() + "poses.kml";
2498  QString path = QFileDialog::getSaveFileName(
2499  this,
2500  tr("Save File"),
2501  output,
2502  tr("Google Earth file (*.kml)"));
2503 
2504  if(!path.isEmpty())
2505  {
2506  bool saved = graph::exportGPS(path.toStdString(), values, ui_->graphViewer->getNodeColor().rgba());
2507 
2508  if(saved)
2509  {
2510  QMessageBox::information(this,
2511  tr("Export poses..."),
2512  tr("GPS coordinates saved to \"%1\".")
2513  .arg(path));
2514  }
2515  else
2516  {
2517  QMessageBox::information(this,
2518  tr("Export poses..."),
2519  tr("Failed to save GPS coordinates to \"%1\"!")
2520  .arg(path));
2521  }
2522  }
2523  }
2524  return;
2525  }
2526 
2527  std::map<int, Transform> optimizedPoses;
2528  if(groundTruth)
2529  {
2530  optimizedPoses = groundTruthPoses_;
2531  }
2532  else
2533  {
2534  if(odometry)
2535  {
2536  optimizedPoses = odomPoses_;
2537  }
2538  else
2539  {
2540  optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
2541  }
2542 
2543  if(ui_->checkBox_alignPosesWithGroundTruth->isChecked())
2544  {
2545  std::map<int, Transform> refPoses = groundTruthPoses_;
2546  if(refPoses.empty())
2547  {
2548  refPoses = gpsPoses_;
2549  }
2550 
2551  // Log ground truth statistics (in TUM's RGBD-SLAM format)
2552  if(refPoses.size())
2553  {
2554  float translational_rmse = 0.0f;
2555  float translational_mean = 0.0f;
2556  float translational_median = 0.0f;
2557  float translational_std = 0.0f;
2558  float translational_min = 0.0f;
2559  float translational_max = 0.0f;
2560  float rotational_rmse = 0.0f;
2561  float rotational_mean = 0.0f;
2562  float rotational_median = 0.0f;
2563  float rotational_std = 0.0f;
2564  float rotational_min = 0.0f;
2565  float rotational_max = 0.0f;
2566 
2567  Transform gtToMap = graph::calcRMSE(
2568  refPoses,
2569  optimizedPoses,
2570  translational_rmse,
2571  translational_mean,
2572  translational_median,
2573  translational_std,
2574  translational_min,
2575  translational_max,
2576  rotational_rmse,
2577  rotational_mean,
2578  rotational_median,
2579  rotational_std,
2580  rotational_min,
2581  rotational_max);
2582 
2583  if(ui_->checkBox_alignPosesWithGroundTruth->isChecked() && !gtToMap.isIdentity())
2584  {
2585  for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
2586  {
2587  iter->second = gtToMap * iter->second;
2588  }
2589  }
2590  }
2591  }
2592  }
2593 
2594  if(optimizedPoses.size())
2595  {
2596  std::map<int, Transform> localTransforms;
2597  QStringList items;
2598  items.push_back("Robot (base frame)");
2599  items.push_back("Camera");
2600  items.push_back("Scan");
2601  bool ok;
2602  QString item = QInputDialog::getItem(this, tr("Export Poses"), tr("Frame: "), items, 0, false, &ok);
2603  if(!ok || item.isEmpty())
2604  {
2605  return;
2606  }
2607  if(item.compare("Robot (base frame)") != 0)
2608  {
2609  bool cameraFrame = item.compare("Camera") == 0;
2610  for(std::map<int, Transform>::iterator iter=optimizedPoses.begin(); iter!=optimizedPoses.end(); ++iter)
2611  {
2612  Transform localTransform;
2613  if(cameraFrame)
2614  {
2615  std::vector<CameraModel> models;
2616  std::vector<StereoCameraModel> stereoModels;
2617  if(dbDriver_->getCalibration(iter->first, models, stereoModels))
2618  {
2619  if((models.size() == 1 &&
2620  !models.at(0).localTransform().isNull()))
2621  {
2622  localTransform = models.at(0).localTransform();
2623  }
2624  else if(stereoModels.size() == 1 &&
2625  !stereoModels[0].localTransform().isNull())
2626  {
2627  localTransform = stereoModels[0].localTransform();
2628  }
2629  else if(models.size()>1)
2630  {
2631  UWARN("Multi-camera is not supported (node %d)", iter->first);
2632  }
2633  else
2634  {
2635  UWARN("Calibration not valid for node %d", iter->first);
2636  }
2637  }
2638  else
2639  {
2640  UWARN("Missing calibration for node %d", iter->first);
2641  }
2642  }
2643  else
2644  {
2645  LaserScan info;
2646  if(dbDriver_->getLaserScanInfo(iter->first, info))
2647  {
2648  localTransform = info.localTransform();
2649  }
2650  else
2651  {
2652  UWARN("Missing scan info for node %d", iter->first);
2653  }
2654 
2655  }
2656  if(!localTransform.isNull())
2657  {
2658  localTransforms.insert(std::make_pair(iter->first, localTransform));
2659  }
2660  }
2661  if(localTransforms.empty())
2662  {
2663  QMessageBox::warning(this,
2664  tr("Export Poses"),
2665  tr("Could not find any \"%1\" frame, exporting in Robot frame instead.").arg(item));
2666  }
2667  }
2668 
2669  std::map<int, Transform> poses;
2670  std::multimap<int, Link> links;
2671  if(localTransforms.empty())
2672  {
2673  poses = optimizedPoses;
2674  if(!odometry)
2675  {
2676  links = graphLinks_;
2677  }
2678  }
2679  else
2680  {
2681  //adjust poses and links
2682  for(std::map<int, Transform>::iterator iter=localTransforms.begin(); iter!=localTransforms.end(); ++iter)
2683  {
2684  poses.insert(std::make_pair(iter->first, optimizedPoses.at(iter->first) * iter->second));
2685  }
2686  if(!odometry)
2687  {
2688  for(std::multimap<int, Link>::iterator iter=graphLinks_.begin(); iter!=graphLinks_.end(); ++iter)
2689  {
2690  if(uContains(poses, iter->second.from()) && uContains(poses, iter->second.to()))
2691  {
2692  std::multimap<int, Link>::iterator inserted = links.insert(*iter);
2693  int from = iter->second.from();
2694  int to = iter->second.to();
2695  inserted->second.setTransform(localTransforms.at(from).inverse()*iter->second.transform()*localTransforms.at(to));
2696  }
2697  }
2698  }
2699  }
2700 
2701  if(format != 4 && !poses.empty() && poses.begin()->first<0) // not g2o, landmark not supported
2702  {
2703  UWARN("Only g2o format (4) can export landmarks, they are ignored with format %d", format);
2704  std::map<int, Transform>::iterator iter=poses.begin();
2705  while(iter!=poses.end() && iter->first < 0)
2706  {
2707  poses.erase(iter++);
2708  }
2709  }
2710 
2711  std::map<int, double> stamps;
2712  if(format == 1 || format == 10 || format == 11)
2713  {
2714  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
2715  {
2716  Transform p, g;
2717  int w;
2718  std::string l;
2719  double stamp=0.0;
2720  int mapId;
2721  std::vector<float> v;
2722  GPS gps;
2723  EnvSensors sensors;
2724  if(dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps, sensors))
2725  {
2726  stamps.insert(std::make_pair(iter->first, stamp));
2727  }
2728  }
2729  if(stamps.size()!=poses.size())
2730  {
2731  QMessageBox::warning(this, tr("Export poses..."), tr("Poses (%1) and stamps (%2) have not the same size! Cannot export in RGB-D SLAM format.")
2732  .arg(poses.size()).arg(stamps.size()));
2733  return;
2734  }
2735  }
2736 
2737  QString output = pathDatabase_ + QDir::separator() + (format==3?"toro%1.graph":format==4?"poses%1.g2o":"poses%1.txt");
2738  QString suffix = odometry?"_odom":groundTruth?"_gt":"";
2739  output = output.arg(suffix);
2740 
2741  QString path = QFileDialog::getSaveFileName(
2742  this,
2743  tr("Save File"),
2744  output,
2745  format == 3?tr("TORO file (*.graph)"):format==4?tr("g2o file (*.g2o)"):tr("Text file (*.txt)"));
2746 
2747  if(!path.isEmpty())
2748  {
2749  if(QFileInfo(path).suffix() == "")
2750  {
2751  if(format == 3)
2752  {
2753  path += ".graph";
2754  }
2755  else if(format==4)
2756  {
2757  path += ".g2o";
2758  }
2759  else
2760  {
2761  path += ".txt";
2762  }
2763  }
2764 
2765  bool saved = graph::exportPoses(path.toStdString(), format, poses, links, stamps, ui_->parameters_toolbox->getParameters());
2766 
2767  if(saved)
2768  {
2769  QMessageBox::information(this,
2770  tr("Export poses..."),
2771  tr("%1 saved to \"%2\".")
2772  .arg(format == 3?"TORO graph":format == 4?"g2o graph":"Poses")
2773  .arg(path));
2774  }
2775  else
2776  {
2777  QMessageBox::information(this,
2778  tr("Export poses..."),
2779  tr("Failed to save %1 to \"%2\"!")
2780  .arg(format == 3?"TORO graph":format == 4?"g2o graph":"poses")
2781  .arg(path));
2782  }
2783  }
2784  }
2785 }
2786 
2787 void DatabaseViewer::exportGPS_TXT()
2788 {
2789  exportGPS(0);
2790 }
2791 void DatabaseViewer::exportGPS_KML()
2792 {
2793  exportGPS(1);
2794 }
2795 
2796 void DatabaseViewer::exportGPS(int format)
2797 {
2798  if(!gpsValues_.empty())
2799  {
2800  QString output = pathDatabase_ + QDir::separator() + (format==0?"gps.txt":"gps.kml");
2801  QString path = QFileDialog::getSaveFileName(
2802  this,
2803  tr("Save File"),
2804  output,
2805  format==0?tr("Raw format (*.txt)"):tr("Google Earth file (*.kml)"));
2806 
2807  if(!path.isEmpty())
2808  {
2809  bool saved = graph::exportGPS(path.toStdString(), gpsValues_, ui_->graphViewer->getGPSColor().rgba());
2810 
2811  if(saved)
2812  {
2813  QMessageBox::information(this,
2814  tr("Export poses..."),
2815  tr("GPS coordinates saved to \"%1\".")
2816  .arg(path));
2817  }
2818  else
2819  {
2820  QMessageBox::information(this,
2821  tr("Export poses..."),
2822  tr("Failed to save GPS coordinates to \"%1\"!")
2823  .arg(path));
2824  }
2825  }
2826  }
2827 }
2828 
2829 void DatabaseViewer::editSaved2DMap()
2830 {
2831  if(!dbDriver_)
2832  {
2833  QMessageBox::warning(this, tr("Cannot edit 2D map"), tr("A database must must loaded first...\nUse File->Open database."));
2834  return;
2835  }
2836 
2837  if(uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.17.0") < 0)
2838  {
2839  QMessageBox::warning(this, tr("Cannot edit 2D map"),
2840  tr("The database has too old version (%1) to saved "
2841  "optimized map. Version 0.17.0 minimum required.").arg(dbDriver_->getDatabaseVersion().c_str()));
2842  return;
2843  }
2844 
2845  if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size() || generatedLocalMaps_.size())
2846  {
2847  QMessageBox::warning(this, tr("Cannot edit 2D map"),
2848  tr("The database has modified links and/or modified local "
2849  "occupancy grids, the 2D optimized map cannot be modified."));
2850  return;
2851  }
2852 
2853  float xMin, yMin, cellSize;
2854  cv::Mat map = dbDriver_->load2DMap(xMin, yMin, cellSize);
2855  if(map.empty())
2856  {
2857  QMessageBox::warning(this, tr("Cannot export 2D map"), tr("The database doesn't contain a saved 2D map."));
2858  return;
2859  }
2860 
2861  cv::Mat map8U = rtabmap::util3d::convertMap2Image8U(map, false);
2862  cv::Mat map8UFlip, map8URotated;
2863  cv::flip(map8U, map8UFlip, 0);
2864  if(!ui_->graphViewer->isOrientationENU())
2865  {
2866  //ROTATE_90_COUNTERCLOCKWISE
2867  cv::transpose(map8UFlip, map8URotated);
2868  cv::flip(map8URotated, map8URotated, 0);
2869  }
2870  else
2871  {
2872  map8URotated = map8UFlip;
2873  }
2874 
2875  editMapArea_->setMap(map8URotated);
2876 
2877  if(editMapDialog_->exec() == QDialog::Accepted)
2878  {
2879  cv::Mat mapModified = editMapArea_->getModifiedMap();
2880 
2881  if(!ui_->graphViewer->isOrientationENU())
2882  {
2883  //ROTATE_90_CLOCKWISE
2884  cv::transpose(mapModified, map8URotated);
2885  cv::flip(map8URotated, map8URotated, 1);
2886  }
2887  else
2888  {
2889  map8URotated = mapModified;
2890  }
2891  cv::flip(map8URotated, map8UFlip, 0);
2892 
2893  UASSERT(map8UFlip.type() == map8U.type());
2894  UASSERT(map8UFlip.cols == map8U.cols);
2895  UASSERT(map8UFlip.rows == map8U.rows);
2896 
2897  cv::Mat map8S = rtabmap::util3d::convertImage8U2Map(map8UFlip, false);
2898 
2899  if(editMapArea_->isModified())
2900  {
2901  dbDriver_->save2DMap(map8S, xMin, yMin, cellSize);
2902  QMessageBox::information(this, tr("Edit 2D map"), tr("Map updated!"));
2903  }
2904 
2905  int cropRadius = ui_->spinBox_cropRadius->value();
2906  QMessageBox::StandardButton b = QMessageBox::question(this,
2907  tr("Crop empty space"),
2908  tr("Do you want to clear empty space from local occupancy grids and laser scans?\n\n"
2909  "Advantages:\n"
2910  " * 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"
2911  " * 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"
2912  "Disadvantage:\n"
2913  " * Cropping the laser scans cannot be reverted after the viewer is closed and changes have been saved.\n\n"
2914  "Parameter(s):\n"
2915  " Crop radius = %1 pixels\n\n"
2916  "Press \"Yes\" to filter only grids.\n"
2917  "Press \"Yes to All\" to filter both grids and laser scans.\n").arg(cropRadius),
2918  QMessageBox::Yes | QMessageBox::YesToAll | QMessageBox::No, QMessageBox::No);
2919  if(b == QMessageBox::Yes || b == QMessageBox::YesToAll)
2920  {
2921  std::map<int, Transform> poses = dbDriver_->loadOptimizedPoses(); // poses should match the grid map
2922 
2923  modifiedLaserScans_.clear();
2924 
2925  rtabmap::ProgressDialog progressDialog(this);
2926  progressDialog.setMaximumSteps(poses.size()+1);
2927  progressDialog.show();
2928  progressDialog.setCancelButtonVisible(true);
2929  progressDialog.appendText(QString("Cropping empty space... %1 scans to filter").arg(poses.size()));
2930  progressDialog.setMinimumWidth(800);
2931  QApplication::processEvents();
2932 
2933  UINFO("Cropping empty space... poses=%d cropRadius=%d", poses.size(), cropRadius);
2934  UASSERT(cropRadius>=0);
2935  for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end() && !progressDialog.isCanceled(); ++iter)
2936  {
2937  // local grid
2938  cv::Mat gridGround;
2939  cv::Mat gridObstacles;
2940  cv::Mat gridEmpty;
2941 
2942  // scan
2943  SensorData data;
2944  dbDriver_->getNodeData(iter->first, data);
2945  LaserScan scan;
2946  data.uncompressData(0,0,&scan,0,&gridGround,&gridObstacles,&gridEmpty);
2947 
2948  if(generatedLocalMaps_.find(iter->first) != generatedLocalMaps_.end())
2949  {
2950  gridObstacles = generatedLocalMaps_.find(iter->first)->second.first.second;
2951  }
2952  if(!gridObstacles.empty())
2953  {
2954  cv::Mat filtered = cv::Mat(1, gridObstacles.cols, gridObstacles.type());
2955  int oi = 0;
2956  for(int i=0; i<gridObstacles.cols; ++i)
2957  {
2958  const float * ptr = gridObstacles.ptr<float>(0, i);
2959  cv::Point3f pt(ptr[0], ptr[1], gridObstacles.channels()==2?0:ptr[2]);
2960  pt = util3d::transformPoint(pt, iter->second);
2961 
2962  int x = int((pt.x - xMin) / cellSize + 0.5f);
2963  int y = int((pt.y - yMin) / cellSize + 0.5f);
2964 
2965  if(x>=0 && x<map8S.cols &&
2966  y>=0 && y<map8S.rows)
2967  {
2968  bool obstacleDetected = false;
2969 
2970  for(int j=-cropRadius; j<=cropRadius && !obstacleDetected; ++j)
2971  {
2972  for(int k=-cropRadius; k<=cropRadius && !obstacleDetected; ++k)
2973  {
2974  if(x+j>=0 && x+j<map8S.cols &&
2975  y+k>=0 && y+k<map8S.rows &&
2976  map8S.at<unsigned char>(y+k,x+j) == 100)
2977  {
2978  obstacleDetected = true;
2979  }
2980  }
2981  }
2982 
2983  if(map8S.at<unsigned char>(y,x) != 0 || obstacleDetected)
2984  {
2985  // Verify that we don't have an obstacle on neighbor cells
2986  cv::Mat(gridObstacles, cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(filtered, cv::Range::all(), cv::Range(oi,oi+1)));
2987  ++oi;
2988  }
2989  }
2990  }
2991 
2992  if(oi != gridObstacles.cols)
2993  {
2994  progressDialog.appendText(QString("Grid %1 filtered %2 pts -> %3 pts").arg(iter->first).arg(gridObstacles.cols).arg(oi));
2995  UINFO("Grid %d filtered %d -> %d", iter->first, gridObstacles.cols, oi);
2996 
2997  // update
2998  std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> value;
2999  if(generatedLocalMaps_.find(iter->first) != generatedLocalMaps_.end())
3000  {
3001  value = generatedLocalMaps_.at(iter->first);
3002  }
3003  else
3004  {
3005  value.first.first = gridGround;
3006  value.second = gridEmpty;
3007  uInsert(generatedLocalMapsInfo_, std::make_pair(data.id(), std::make_pair(data.gridCellSize(), data.gridViewPoint())));
3008  }
3009  value.first.second = cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi));
3010  uInsert(generatedLocalMaps_, std::make_pair(iter->first, value));
3011  }
3012  }
3013 
3014  if(b == QMessageBox::YesToAll && !scan.isEmpty())
3015  {
3016  Transform mapToScan = iter->second * scan.localTransform();
3017 
3018  cv::Mat filtered = cv::Mat(1, scan.size(), scan.dataType());
3019  int oi = 0;
3020  for(int i=0; i<scan.size(); ++i)
3021  {
3022  const float * ptr = scan.data().ptr<float>(0, i);
3023  cv::Point3f pt(ptr[0], ptr[1], scan.is2d()?0:ptr[2]);
3024  pt = util3d::transformPoint(pt, mapToScan);
3025 
3026  int x = int((pt.x - xMin) / cellSize + 0.5f);
3027  int y = int((pt.y - yMin) / cellSize + 0.5f);
3028 
3029  if(x>=0 && x<map8S.cols &&
3030  y>=0 && y<map8S.rows)
3031  {
3032  bool obstacleDetected = false;
3033 
3034  for(int j=-cropRadius; j<=cropRadius && !obstacleDetected; ++j)
3035  {
3036  for(int k=-cropRadius; k<=cropRadius && !obstacleDetected; ++k)
3037  {
3038  if(x+j>=0 && x+j<map8S.cols &&
3039  y+k>=0 && y+k<map8S.rows &&
3040  map8S.at<unsigned char>(y+k,x+j) == 100)
3041  {
3042  obstacleDetected = true;
3043  }
3044  }
3045  }
3046 
3047  if(map8S.at<unsigned char>(y,x) != 0 || obstacleDetected)
3048  {
3049  // Verify that we don't have an obstacle on neighbor cells
3050  cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(filtered, cv::Range::all(), cv::Range(oi,oi+1)));
3051  ++oi;
3052  }
3053  }
3054  }
3055 
3056  if(oi != scan.size())
3057  {
3058  progressDialog.appendText(QString("Scan %1 filtered %2 pts -> %3 pts").arg(iter->first).arg(scan.size()).arg(oi));
3059  UINFO("Scan %d filtered %d -> %d", iter->first, scan.size(), oi);
3060 
3061  // update
3062  if(scan.angleIncrement()!=0)
3063  {
3064  // copy meta data
3065  scan = LaserScan(
3066  cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi)),
3067  scan.format(),
3068  scan.rangeMin(),
3069  scan.rangeMax(),
3070  scan.angleMin(),
3071  scan.angleMax(),
3072  scan.angleIncrement(),
3073  scan.localTransform());
3074  }
3075  else
3076  {
3077  // copy meta data
3078  scan = LaserScan(
3079  cv::Mat(filtered, cv::Range::all(), cv::Range(0, oi)),
3080  scan.maxPoints(),
3081  scan.rangeMax(),
3082  scan.format(),
3083  scan.localTransform());
3084  }
3085  uInsert(modifiedLaserScans_, std::make_pair(iter->first, scan));
3086  }
3087  }
3088  progressDialog.incrementStep();
3089  QApplication::processEvents();
3090  }
3091  if(progressDialog.isCanceled())
3092  {
3093  modifiedLaserScans_.clear();
3094  }
3095  else
3096  {
3097  update3dView();
3098  updateGraphView();
3099  }
3100  progressDialog.setValue(progressDialog.maximumSteps());
3101  }
3102  }
3103 }
3104 
3105 void DatabaseViewer::exportSaved2DMap()
3106 {
3107  if(!dbDriver_)
3108  {
3109  QMessageBox::warning(this, tr("Cannot export 2D map"), tr("A database must must loaded first...\nUse File->Open database."));
3110  return;
3111  }
3112 
3113  float xMin, yMin, cellSize;
3114  cv::Mat map = dbDriver_->load2DMap(xMin, yMin, cellSize);
3115  if(map.empty())
3116  {
3117  QMessageBox::warning(this, tr("Cannot export 2D map"), tr("The database doesn't contain a saved 2D map."));
3118  }
3119  else
3120  {
3121  cv::Mat map8U = rtabmap::util3d::convertMap2Image8U(map, true);
3122  QString name = QFileInfo(databaseFileName_.c_str()).baseName();
3123  QString path = QFileDialog::getSaveFileName(
3124  this,
3125  tr("Save File"),
3126  pathDatabase_+"/" + name + ".pgm",
3127  tr("Map (*.pgm)"));
3128 
3129  if(!path.isEmpty())
3130  {
3131  if(QFileInfo(path).suffix() == "")
3132  {
3133  path += ".pgm";
3134  }
3135  cv::imwrite(path.toStdString(), map8U);
3136  QMessageBox::information(this, tr("Export 2D map"), tr("Exported %1!").arg(path));
3137  }
3138  }
3139 }
3140 
3141 void DatabaseViewer::import2DMap()
3142 {
3143  if(!dbDriver_)
3144  {
3145  QMessageBox::warning(this, tr("Cannot import 2D map"), tr("A database must must loaded first...\nUse File->Open database."));
3146  return;
3147  }
3148 
3149  if(uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.17.0") < 0)
3150  {
3151  QMessageBox::warning(this, tr("Cannot edit 2D map"),
3152  tr("The database has too old version (%1) to saved "
3153  "optimized map. Version 0.17.0 minimum required.").arg(dbDriver_->getDatabaseVersion().c_str()));
3154  return;
3155  }
3156 
3157  if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size() || generatedLocalMaps_.size())
3158  {
3159  QMessageBox::warning(this, tr("Cannot import 2D map"),
3160  tr("The database has modified links and/or modified local "
3161  "occupancy grids, the 2D optimized map cannot be modified."));
3162  return;
3163  }
3164 
3165  float xMin, yMin, cellSize;
3166  cv::Mat mapOrg = dbDriver_->load2DMap(xMin, yMin, cellSize);
3167  if(mapOrg.empty())
3168  {
3169  QMessageBox::warning(this, tr("Cannot import 2D map"), tr("The database doesn't contain a saved 2D map."));
3170  }
3171  else
3172  {
3173  QString path = QFileDialog::getOpenFileName(
3174  this,
3175  tr("Open File"),
3176  pathDatabase_,
3177  tr("Map (*.pgm)"));
3178  if(!path.isEmpty())
3179  {
3180  cv::Mat map8U = cv::imread(path.toStdString(), cv::IMREAD_UNCHANGED);
3181  cv::Mat map = rtabmap::util3d::convertImage8U2Map(map8U, true);
3182 
3183  if(mapOrg.cols == map.cols && mapOrg.rows == map8U.rows)
3184  {
3185  dbDriver_->save2DMap(map, xMin, yMin, cellSize);
3186  QMessageBox::information(this, tr("Import 2D map"), tr("Imported %1!").arg(path));
3187  }
3188  else
3189  {
3190  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));
3191  }
3192  }
3193  }
3194 }
3195 
3196 void DatabaseViewer::regenerateSavedMap()
3197 {
3198  if(!dbDriver_)
3199  {
3200  QMessageBox::warning(this, tr("Cannot import 2D map"), tr("A database must must loaded first...\nUse File->Open database."));
3201  return;
3202  }
3203 
3204  if(uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.17.0") < 0)
3205  {
3206  QMessageBox::warning(this, tr("Cannot edit 2D map"),
3207  tr("The database has too old version (%1) to saved "
3208  "optimized map. Version 0.17.0 minimum required.").arg(dbDriver_->getDatabaseVersion().c_str()));
3209  return;
3210  }
3211 
3212  if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size() || generatedLocalMaps_.size())
3213  {
3214  QMessageBox::warning(this, tr("Cannot import 2D map"),
3215  tr("The database has modified links and/or modified local "
3216  "occupancy grids, the 2D optimized map cannot be modified. Try "
3217  "closing the database and re-open it to save the changes."));
3218  return;
3219  }
3220 
3221  if((int)graphes_.empty() || localMaps_.empty())
3222  {
3223  QMessageBox::warning(this, tr("Cannot regenerate 2D map"),
3224  tr("Graph is empty, make sure you opened the "
3225  "Graph View and there is a map shown."));
3226  return;
3227  }
3228 
3229  //
3230 #ifdef RTABMAP_OCTOMAP
3231  QStringList types;
3232  types.push_back("Default occupancy grid");
3233  types.push_back("From OctoMap projection");
3234  bool ok;
3235  QString type = QInputDialog::getItem(this, tr("Which type?"), tr("Type:"), types, 0, false, &ok);
3236  if(!ok)
3237  {
3238  return;
3239  }
3240 #endif
3241 
3242  //update scans
3243  UINFO("Update local maps list...");
3244  float xMin, yMin;
3245  cv::Mat map;
3246  float gridCellSize = Parameters::defaultGridCellSize();
3247  Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kGridCellSize(), gridCellSize);
3248 #ifdef RTABMAP_OCTOMAP
3249  if(type.compare("From OctoMap projection") == 0)
3250  {
3251  //create local octomap
3252  OctoMap octomap(ui_->parameters_toolbox->getParameters());
3253  for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end(); ++iter)
3254  {
3255  if(iter->second.first.first.channels() == 2 || iter->second.first.second.channels() == 2)
3256  {
3257  QMessageBox::warning(this, tr(""),
3258  tr("Some local occupancy grids are 2D, but OctoMap requires 3D local "
3259  "occupancy grids. Select default occupancy grid or generate "
3260  "3D local occupancy grids (\"Grid/3D\" core parameter)."));
3261  return;
3262  }
3263  octomap.addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second, localMapsInfo_.at(iter->first).second);
3264  }
3265 
3266  octomap.update(graphes_.back());
3267  map = octomap.createProjectionMap(xMin, yMin, gridCellSize, 0);
3268  }
3269  else
3270 #endif
3271  {
3272  OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
3273  for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end(); ++iter)
3274  {
3275  grid.addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second);
3276  }
3277  grid.update(graphes_.back());
3278  map = grid.getMap(xMin, yMin);
3279  }
3280 
3281  if(map.empty())
3282  {
3283  QMessageBox::information(this, tr("Regenerate 2D map"), tr("Failed to renegerate the map, resulting map is empty!"));
3284  }
3285  else
3286  {
3287  dbDriver_->save2DMap(map, xMin, yMin, gridCellSize);
3288  Transform lastlocalizationPose;
3289  dbDriver_->loadOptimizedPoses(&lastlocalizationPose);
3290  if(lastlocalizationPose.isNull() && !graphes_.back().empty())
3291  {
3292  // use last pose by default
3293  lastlocalizationPose = graphes_.back().rbegin()->second;
3294  }
3295  dbDriver_->saveOptimizedPoses(graphes_.back(), lastlocalizationPose);
3296  // reset optimized mesh as poses have changed
3297  dbDriver_->saveOptimizedMesh(cv::Mat());
3298  QMessageBox::information(this, tr("Regenerate 2D map"), tr("Map regenerated!"));
3299  ui_->actionEdit_optimized_2D_map->setEnabled(true);
3300  ui_->actionExport_saved_2D_map->setEnabled(true);
3301  ui_->actionImport_2D_map->setEnabled(true);
3302  }
3303 }
3304 
3305 void DatabaseViewer::viewOptimizedMesh()
3306 {
3307  if(!dbDriver_)
3308  {
3309  QMessageBox::warning(this, tr("Cannot view optimized mesh"), tr("A database must must loaded first...\nUse File->Open database."));
3310  return;
3311  }
3312 
3313  std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
3314 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
3315  std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
3316 #else
3317  std::vector<std::vector<Eigen::Vector2f> > texCoords;
3318 #endif
3319  cv::Mat textures;
3320  cv::Mat cloudMat = dbDriver_->loadOptimizedMesh(&polygons, &texCoords, &textures);
3321  if(cloudMat.empty())
3322  {
3323  QMessageBox::warning(this, tr("Cannot view optimized mesh"), tr("The database doesn't contain a saved optimized mesh."));
3324  }
3325  else
3326  {
3327  CloudViewer * viewer = new CloudViewer(this);
3328  viewer->setWindowFlags(Qt::Window);
3329  viewer->setAttribute(Qt::WA_DeleteOnClose);
3330  viewer->buildPickingLocator(true);
3331  if(!textures.empty())
3332  {
3333  pcl::TextureMeshPtr mesh = util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
3334  util3d::fixTextureMeshForVisualization(*mesh);
3335  viewer->setWindowTitle("Optimized Textured Mesh");
3336  viewer->setPolygonPicking(true);
3337  viewer->addCloudTextureMesh("mesh", mesh, textures);
3338  }
3339  else if(polygons.size() == 1)
3340  {
3341  pcl::PolygonMeshPtr mesh = util3d::assemblePolygonMesh(cloudMat, polygons.at(0));
3342  viewer->setWindowTitle("Optimized Mesh");
3343  viewer->setPolygonPicking(true);
3344  viewer->addCloudMesh("mesh", mesh);
3345  }
3346  else
3347  {
3348  LaserScan scan = LaserScan::backwardCompatibility(cloudMat);
3349  pcl::PCLPointCloud2::Ptr cloud = util3d::laserScanToPointCloud2(scan);
3350  viewer->setWindowTitle("Optimized Point Cloud");
3351  viewer->addCloud("mesh", cloud, Transform::getIdentity(), scan.hasRGB(), scan.hasNormals(), scan.hasIntensity());
3352  }
3353  viewer->show();
3354  }
3355 }
3356 
3357 void DatabaseViewer::exportOptimizedMesh()
3358 {
3359  if(!dbDriver_)
3360  {
3361  QMessageBox::warning(this, tr("Cannot export optimized mesh"), tr("A database must must loaded first...\nUse File->Open database."));
3362  return;
3363  }
3364 
3365  std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
3366 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
3367  std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
3368 #else
3369  std::vector<std::vector<Eigen::Vector2f> > texCoords;
3370 #endif
3371  cv::Mat textures;
3372  cv::Mat cloudMat = dbDriver_->loadOptimizedMesh(&polygons, &texCoords, &textures);
3373  if(cloudMat.empty())
3374  {
3375  QMessageBox::warning(this, tr("Cannot export optimized mesh"), tr("The database doesn't contain a saved optimized mesh."));
3376  }
3377  else
3378  {
3379  QString name = QFileInfo(databaseFileName_.c_str()).baseName();
3380 
3381  if(!textures.empty())
3382  {
3383  pcl::TextureMeshPtr mesh = util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures);
3384  QString path = QFileDialog::getSaveFileName(
3385  this,
3386  tr("Save File"),
3387  pathDatabase_+"/" + name + ".obj",
3388  tr("Mesh (*.obj)"));
3389 
3390  if(!path.isEmpty())
3391  {
3392  if(QFileInfo(path).suffix() == "")
3393  {
3394  path += ".obj";
3395  }
3396  QString baseName = QFileInfo(path).baseName();
3397  if(mesh->tex_materials.size() == 1)
3398  {
3399  mesh->tex_materials.at(0).tex_file = baseName.toStdString() + ".png";
3400  cv::imwrite((QFileInfo(path).absoluteDir().absolutePath()+QDir::separator()+baseName).toStdString() + ".png", textures);
3401  }
3402  else
3403  {
3404  for(unsigned int i=0; i<mesh->tex_materials.size(); ++i)
3405  {
3406  mesh->tex_materials.at(i).tex_file = (baseName+QDir::separator()+QString::number(i)+".png").toStdString();
3407  UASSERT((i+1)*textures.rows <= (unsigned int)textures.cols);
3408  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)));
3409  }
3410  }
3411  pcl::io::saveOBJFile(path.toStdString(), *mesh);
3412 
3413  QMessageBox::information(this, tr("Export Textured Mesh"), tr("Exported %1!").arg(path));
3414  }
3415  }
3416  else if(polygons.size() == 1)
3417  {
3418  pcl::PolygonMeshPtr mesh = util3d::assemblePolygonMesh(cloudMat, polygons.at(0));
3419  QString path = QFileDialog::getSaveFileName(
3420  this,
3421  tr("Save File"),
3422  pathDatabase_+"/" + name + ".ply",
3423  tr("Mesh (*.ply)"));
3424 
3425  if(!path.isEmpty())
3426  {
3427  if(QFileInfo(path).suffix() == "")
3428  {
3429  path += ".ply";
3430  }
3431  pcl::io::savePLYFileBinary(path.toStdString(), *mesh);
3432  QMessageBox::information(this, tr("Export Mesh"), tr("Exported %1!").arg(path));
3433  }
3434  }
3435  else
3436  {
3437  QString path = QFileDialog::getSaveFileName(
3438  this,
3439  tr("Save File"),
3440  pathDatabase_+"/" + name + ".ply",
3441  tr("Point cloud data (*.ply *.pcd)"));
3442 
3443  if(!path.isEmpty())
3444  {
3445  if(QFileInfo(path).suffix() == "")
3446  {
3447  path += ".ply";
3448  }
3449  bool success = false;
3450  pcl::PCLPointCloud2::Ptr cloud = util3d::laserScanToPointCloud2(LaserScan::backwardCompatibility(cloudMat));
3451  if(QFileInfo(path).suffix() == "pcd")
3452  {
3453  success = pcl::io::savePCDFile(path.toStdString(), *cloud) == 0;
3454  }
3455  else
3456  {
3457  success = pcl::io::savePLYFile(path.toStdString(), *cloud) == 0;
3458  }
3459  if(success)
3460  {
3461  QMessageBox::information(this, tr("Export Point Cloud"), tr("Exported %1!").arg(path));
3462  }
3463  else
3464  {
3465  QMessageBox::critical(this, tr("Export Point Cloud"), tr("Failed exporting %1!").arg(path));
3466  }
3467  }
3468  }
3469  }
3470 }
3471 
3472 void DatabaseViewer::updateOptimizedMesh()
3473 {
3474  if(!ids_.size() || !dbDriver_)
3475  {
3476  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
3477  return;
3478  }
3479 
3480  if(graphes_.empty())
3481  {
3482  this->updateGraphView();
3483  if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
3484  {
3485  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
3486  return;
3487  }
3488  }
3489 
3490  std::map<int, Transform> optimizedPoses;
3491  if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty())
3492  {
3493  optimizedPoses = groundTruthPoses_;
3494  }
3495  else
3496  {
3497  optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
3498  }
3499  if(ui_->groupBox_posefiltering->isChecked())
3500  {
3501  optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
3502  ui_->doubleSpinBox_posefilteringRadius->value(),
3503  ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
3504  }
3505  Transform lastlocalizationPose;
3506  dbDriver_->loadOptimizedPoses(&lastlocalizationPose);
3507  //optimized poses have changed, reset 2d map
3508  dbDriver_->save2DMap(cv::Mat(), 0, 0, 0);
3509  if(optimizedPoses.size() > 0)
3510  {
3511  exportDialog_->setDBDriver(dbDriver_);
3512  exportDialog_->forceAssembling(true);
3513  exportDialog_->setOkButton();
3514 
3515  std::map<int, pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr> clouds;
3516  std::map<int, pcl::PolygonMesh::Ptr> meshes;
3517  std::map<int, pcl::TextureMesh::Ptr> textureMeshes;
3518  std::vector<std::map<int, pcl::PointXY> > textureVertexToPixels;
3519 
3520  if(exportDialog_->getExportedClouds(
3521  optimizedPoses,
3522  updateLinksWithModifications(links_),
3523  mapIds_,
3524  QMap<int, Signature>(),
3525  std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
3526  std::map<int, LaserScan>(),
3527  pathDatabase_,
3528  ui_->parameters_toolbox->getParameters(),
3529  clouds,
3530  meshes,
3531  textureMeshes,
3532  textureVertexToPixels))
3533  {
3534  if(textureMeshes.size())
3535  {
3536  dbDriver_->saveOptimizedPoses(optimizedPoses, lastlocalizationPose);
3537 
3538  cv::Mat globalTextures;
3539  pcl::TextureMeshPtr textureMesh = textureMeshes.at(0);
3540  if(textureMesh->tex_materials.size()>1)
3541  {
3542  globalTextures = util3d::mergeTextures(
3543  *textureMesh,
3544  std::map<int, cv::Mat>(),
3545  std::map<int, std::vector<CameraModel> >(),
3546  0,
3547  dbDriver_,
3548  exportDialog_->getTextureSize(),
3549  exportDialog_->getMaxTextures(),
3550  textureVertexToPixels,
3551  exportDialog_->isGainCompensation(),
3552  exportDialog_->getGainBeta(),
3553  exportDialog_->isGainRGB(),
3554  exportDialog_->isBlending(),
3555  exportDialog_->getBlendingDecimation(),
3556  exportDialog_->getTextureBrightnessConstrastRatioLow(),
3557  exportDialog_->getTextureBrightnessConstrastRatioHigh(),
3558  exportDialog_->isExposeFusion());
3559  }
3560  dbDriver_->saveOptimizedMesh(
3561  util3d::laserScanFromPointCloud(textureMesh->cloud, false).data(),
3562  util3d::convertPolygonsFromPCL(textureMesh->tex_polygons),
3563  textureMesh->tex_coordinates,
3564  globalTextures);
3565  QMessageBox::information(this, tr("Update Optimized Textured Mesh"), tr("Updated!"));
3566  ui_->actionView_optimized_mesh->setEnabled(true);
3567  ui_->actionExport_optimized_mesh->setEnabled(true);
3568  this->viewOptimizedMesh();
3569  }
3570  else if(meshes.size())
3571  {
3572  dbDriver_->saveOptimizedPoses(optimizedPoses, lastlocalizationPose);
3573  std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons(1);
3574  polygons.at(0) = util3d::convertPolygonsFromPCL(meshes.at(0)->polygons);
3575  dbDriver_->saveOptimizedMesh(util3d::laserScanFromPointCloud(meshes.at(0)->cloud, false).data(), polygons);
3576  QMessageBox::information(this, tr("Update Optimized Mesh"), tr("Updated!"));
3577  ui_->actionView_optimized_mesh->setEnabled(true);
3578  ui_->actionExport_optimized_mesh->setEnabled(true);
3579  this->viewOptimizedMesh();
3580  }
3581  else if(clouds.size())
3582  {
3583  dbDriver_->saveOptimizedPoses(optimizedPoses, lastlocalizationPose);
3584  dbDriver_->saveOptimizedMesh(util3d::laserScanFromPointCloud(*clouds.at(0)).data());
3585  QMessageBox::information(this, tr("Update Optimized PointCloud"), tr("Updated!"));
3586  ui_->actionView_optimized_mesh->setEnabled(true);
3587  ui_->actionExport_optimized_mesh->setEnabled(true);
3588  this->viewOptimizedMesh();
3589  }
3590  else
3591  {
3592  QMessageBox::critical(this, tr("Update Optimized Mesh"), tr("Nothing to save!"));
3593  }
3594  }
3595  exportDialog_->setProgressDialogToMax();
3596  }
3597  else
3598  {
3599  QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
3600  }
3601 }
3602 
3603 void DatabaseViewer::generateGraph()
3604 {
3605  if(!dbDriver_)
3606  {
3607  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("A database must must loaded first...\nUse File->Open database."));
3608  return;
3609  }
3610 
3611  QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/Graph.dot", tr("Graphiz file (*.dot)"));
3612  if(!path.isEmpty())
3613  {
3614  dbDriver_->generateGraph(path.toStdString());
3615  }
3616 }
3617 
3618 void DatabaseViewer::generateLocalGraph()
3619 {
3620  if(!ids_.size() || !dbDriver_)
3621  {
3622  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
3623  return;
3624  }
3625  bool ok = false;
3626  int id = QInputDialog::getInt(this, tr("Around which location?"), tr("Location ID"), ids_.first(), ids_.first(), ids_.last(), 1, &ok);
3627 
3628  if(ok)
3629  {
3630  int margin = QInputDialog::getInt(this, tr("Depth around the location?"), tr("Margin"), 4, 1, 100, 1, &ok);
3631  if(ok)
3632  {
3633  QString path = QFileDialog::getSaveFileName(this, tr("Save File"), pathDatabase_+"/Graph" + QString::number(id) + ".dot", tr("Graphiz file (*.dot)"));
3634  if(!path.isEmpty() && id>0)
3635  {
3636  std::map<int, int> ids;
3637  std::list<int> curentMarginList;
3638  std::set<int> currentMargin;
3639  std::set<int> nextMargin;
3640  nextMargin.insert(id);
3641  int m = 0;
3642  while((margin == 0 || m < margin) && nextMargin.size())
3643  {
3644  curentMarginList = std::list<int>(nextMargin.rbegin(), nextMargin.rend());
3645  nextMargin.clear();
3646 
3647  for(std::list<int>::iterator jter = curentMarginList.begin(); jter!=curentMarginList.end(); ++jter)
3648  {
3649  if(ids.find(*jter) == ids.end())
3650  {
3651  std::multimap<int, Link> links;
3652  ids.insert(std::pair<int, int>(*jter, m));
3653 
3654  UTimer timer;
3655  dbDriver_->loadLinks(*jter, links);
3656 
3657  // links
3658  for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
3659  {
3660  if( !uContains(ids, iter->first))
3661  {
3662  UASSERT(iter->second.type() != Link::kUndef);
3663  if(iter->second.type() == Link::kNeighbor ||
3664  iter->second.type() == Link::kNeighborMerged)
3665  {
3666  nextMargin.insert(iter->first);
3667  }
3668  else
3669  {
3670  // loop closures are on same margin
3671  if(currentMargin.insert(iter->first).second)
3672  {
3673  curentMarginList.push_back(iter->first);
3674  }
3675  }
3676  }
3677  }
3678  }
3679  }
3680  ++m;
3681  }
3682 
3683  if(ids.size() > 0)
3684  {
3685  ids.insert(std::pair<int,int>(id, 0));
3686  std::set<int> idsSet;
3687  for(std::map<int, int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
3688  {
3689  idsSet.insert(idsSet.end(), iter->first);
3690  UINFO("Node %d", iter->first);
3691  }
3692  UINFO("idsSet=%d", idsSet.size());
3693  dbDriver_->generateGraph(path.toStdString(), idsSet);
3694  }
3695  else
3696  {
3697  QMessageBox::critical(this, tr("Error"), tr("No neighbors found for signature %1.").arg(id));
3698  }
3699  }
3700  }
3701  }
3702 }
3703 
3704 void DatabaseViewer::regenerateLocalMaps()
3705 {
3706  OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
3707 
3708  generatedLocalMaps_.clear();
3709  generatedLocalMapsInfo_.clear();
3710 
3711  rtabmap::ProgressDialog progressDialog(this);
3712  progressDialog.setMaximumSteps(ids_.size());
3713  progressDialog.show();
3714  progressDialog.setCancelButtonVisible(true);
3715 
3716  UPlot * plot = new UPlot(this);
3717  plot->setWindowFlags(Qt::Window);
3718  plot->setWindowTitle("Local Occupancy Grid Generation Time (ms)");
3719  plot->setAttribute(Qt::WA_DeleteOnClose);
3720  UPlotCurve * decompressionCurve = plot->addCurve("Decompression");
3721  UPlotCurve * gridCreationCurve = plot->addCurve("Grid Creation");
3722  plot->show();
3723 
3724  UPlot * plotCells = new UPlot(this);
3725  plotCells->setWindowFlags(Qt::Window);
3726  plotCells->setWindowTitle("Occupancy Cells");
3727  plotCells->setAttribute(Qt::WA_DeleteOnClose);
3728  UPlotCurve * totalCurve = plotCells->addCurve("Total");
3729  UPlotCurve * emptyCurve = plotCells->addCurve("Empty");
3730  UPlotCurve * obstaclesCurve = plotCells->addCurve("Obstacles");
3731  UPlotCurve * groundCurve = plotCells->addCurve("Ground");
3732  plotCells->show();
3733 
3734  double decompressionTime = 0;
3735  double gridCreationTime = 0;
3736 
3737  for(int i =0; i<ids_.size() && !progressDialog.isCanceled(); ++i)
3738  {
3739  UTimer timer;
3740  SensorData data;
3741  dbDriver_->getNodeData(ids_.at(i), data);
3742  data.uncompressData();
3743  decompressionTime = timer.ticks()*1000.0;
3744 
3745  int mapId, weight;
3746  Transform odomPose, groundTruth;
3747  std::string label;
3748  double stamp;
3749  QString msg;
3750  std::vector<float> velocity;
3751  GPS gps;
3752  EnvSensors sensors;
3753  if(dbDriver_->getNodeInfo(data.id(), odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors))
3754  {
3755  Signature s = data;
3756  s.setPose(odomPose);
3757  cv::Mat ground, obstacles, empty;
3758  cv::Point3f viewpoint;
3759  timer.ticks();
3760 
3761  if(ui_->checkBox_grid_regenerateFromSavedGrid->isChecked() && s.sensorData().gridCellSize() > 0.0f)
3762  {
3763  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridObstacleCellsRaw()));
3764  *cloud+=*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridGroundCellsRaw()));
3765 
3766  if(cloud->size())
3767  {
3768  // update viewpoint
3769  if(s.sensorData().cameraModels().size())
3770  {
3771  // average of all local transforms
3772  float sum = 0;
3773  for(unsigned int i=0; i<s.sensorData().cameraModels().size(); ++i)
3774  {
3775  const Transform & t = s.sensorData().cameraModels()[i].localTransform();
3776  if(!t.isNull())
3777  {
3778  viewpoint.x += t.x();
3779  viewpoint.y += t.y();
3780  viewpoint.z += t.z();
3781  sum += 1.0f;
3782  }
3783  }
3784  if(sum > 0.0f)
3785  {
3786  viewpoint.x /= sum;
3787  viewpoint.y /= sum;
3788  viewpoint.z /= sum;
3789  }
3790  }
3791  else if(s.sensorData().cameraModels().size())
3792  {
3793  // average of all local transforms
3794  float sum = 0;
3795  for(unsigned int i=0; i<s.sensorData().stereoCameraModels().size(); ++i)
3796  {
3797  const Transform & t = s.sensorData().stereoCameraModels()[i].localTransform();
3798  if(!t.isNull())
3799  {
3800  viewpoint.x += t.x();
3801  viewpoint.y += t.y();
3802  viewpoint.z += t.z();
3803  sum += 1.0f;
3804  }
3805  }
3806  if(sum > 0.0f)
3807  {
3808  viewpoint.x /= sum;
3809  viewpoint.y /= sum;
3810  viewpoint.z /= sum;
3811  }
3812  }
3813 
3814  grid.createLocalMap(util3d::laserScanFromPointCloud(*cloud), s.getPose(), ground, obstacles, empty, viewpoint);
3815  }
3816  }
3817  else
3818  {
3819  grid.createLocalMap(s, ground, obstacles, empty, viewpoint);
3820  }
3821 
3822  gridCreationTime = timer.ticks()*1000.0;
3823  uInsert(generatedLocalMaps_, std::make_pair(data.id(), std::make_pair(std::make_pair(ground, obstacles), empty)));
3824  uInsert(generatedLocalMapsInfo_, std::make_pair(data.id(), std::make_pair(grid.getCellSize(), viewpoint)));
3825  msg = QString("Generated local occupancy grid map %1/%2").arg(i+1).arg((int)ids_.size());
3826 
3827  totalCurve->addValue(ids_.at(i), obstacles.cols+ground.cols+empty.cols);
3828  emptyCurve->addValue(ids_.at(i), empty.cols);
3829  obstaclesCurve->addValue(ids_.at(i), obstacles.cols);
3830  groundCurve->addValue(ids_.at(i), ground.cols);
3831  }
3832 
3833  progressDialog.appendText(msg);
3834  progressDialog.incrementStep();
3835 
3836  decompressionCurve->addValue(ids_.at(i), decompressionTime);
3837  gridCreationCurve->addValue(ids_.at(i), gridCreationTime);
3838 
3839  if(ids_.size() < 50 || (i+1) % 25 == 0)
3840  {
3841  QApplication::processEvents();
3842  }
3843  }
3844  progressDialog.setValue(progressDialog.maximumSteps());
3845 
3846  if(graphes_.size())
3847  {
3848  update3dView();
3849  sliderIterationsValueChanged((int)graphes_.size()-1);
3850  }
3851  else
3852  {
3853  updateGrid();
3854  }
3855 }
3856 
3857 void DatabaseViewer::regenerateCurrentLocalMaps()
3858 {
3859  UTimer time;
3860  OccupancyGrid grid(ui_->parameters_toolbox->getParameters());
3861 
3862  if(ids_.size() == 0)
3863  {
3864  UWARN("ids_ is empty!");
3865  return;
3866  }
3867 
3868  QSet<int> idsSet;
3869  idsSet.insert(ids_.at(ui_->horizontalSlider_A->value()));
3870  idsSet.insert(ids_.at(ui_->horizontalSlider_B->value()));
3871  QList<int> ids = idsSet.toList();
3872 
3873  rtabmap::ProgressDialog progressDialog(this);
3874  progressDialog.setMaximumSteps(ids.size());
3875  progressDialog.show();
3876 
3877  for(int i =0; i<ids.size(); ++i)
3878  {
3879  generatedLocalMaps_.erase(ids.at(i));
3880  generatedLocalMapsInfo_.erase(ids.at(i));
3881 
3882  SensorData data;
3883  dbDriver_->getNodeData(ids.at(i), data);
3884  data.uncompressData();
3885 
3886  int mapId, weight;
3887  Transform odomPose, groundTruth;
3888  std::string label;
3889  double stamp;
3890  QString msg;
3891  std::vector<float> velocity;
3892  GPS gps;
3893  EnvSensors sensors;
3894  if(dbDriver_->getNodeInfo(data.id(), odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps, sensors))
3895  {
3896  Signature s = data;
3897  s.setPose(odomPose);
3898  cv::Mat ground, obstacles, empty;
3899  cv::Point3f viewpoint;
3900 
3901  if(ui_->checkBox_grid_regenerateFromSavedGrid->isChecked() && s.sensorData().gridCellSize() > 0.0f)
3902  {
3903  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridObstacleCellsRaw()));
3904  *cloud+=*util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(s.sensorData().gridGroundCellsRaw()));
3905 
3906  if(cloud->size())
3907  {
3908  // update viewpoint
3909  if(s.sensorData().cameraModels().size())
3910  {
3911  // average of all local transforms
3912  float sum = 0;
3913  for(unsigned int i=0; i<s.sensorData().cameraModels().size(); ++i)
3914  {
3915  const Transform & t = s.sensorData().cameraModels()[i].localTransform();
3916  if(!t.isNull())
3917  {
3918  viewpoint.x += t.x();
3919  viewpoint.y += t.y();
3920  viewpoint.z += t.z();
3921  sum += 1.0f;
3922  }
3923  }
3924  if(sum > 0.0f)
3925  {
3926  viewpoint.x /= sum;
3927  viewpoint.y /= sum;
3928  viewpoint.z /= sum;
3929  }
3930  }
3931  else if(s.sensorData().stereoCameraModels().size())
3932  {
3933  // average of all local transforms
3934  float sum = 0;
3935  for(unsigned int i=0; i<s.sensorData().stereoCameraModels().size(); ++i)
3936  {
3937  const Transform & t = s.sensorData().stereoCameraModels()[i].localTransform();
3938  if(!t.isNull())
3939  {
3940  viewpoint.x += t.x();
3941  viewpoint.y += t.y();
3942  viewpoint.z += t.z();
3943  sum += 1.0f;
3944  }
3945  }
3946  if(sum > 0.0f)
3947  {
3948  viewpoint.x /= sum;
3949  viewpoint.y /= sum;
3950  viewpoint.z /= sum;
3951  }
3952  }
3953 
3954  grid.createLocalMap(util3d::laserScanFromPointCloud(*cloud), s.getPose(), ground, obstacles, empty, viewpoint);
3955  }
3956  }
3957  else
3958  {
3959  grid.createLocalMap(s, ground, obstacles, empty, viewpoint);
3960  }
3961 
3962 
3963  uInsert(generatedLocalMaps_, std::make_pair(data.id(), std::make_pair(std::make_pair(ground, obstacles),empty)));
3964  uInsert(generatedLocalMapsInfo_, std::make_pair(data.id(), std::make_pair(grid.getCellSize(), viewpoint)));
3965  msg = QString("Generated local occupancy grid map %1/%2 (%3s)").arg(i+1).arg((int)ids.size()).arg(time.ticks());
3966  }
3967 
3968  progressDialog.appendText(msg);
3969  progressDialog.incrementStep();
3970  QApplication::processEvents();
3971  }
3972  progressDialog.setValue(progressDialog.maximumSteps());
3973 
3974  if(graphes_.size())
3975  {
3976  update3dView();
3977  sliderIterationsValueChanged((int)graphes_.size()-1);
3978  }
3979  else
3980  {
3981  updateGrid();
3982  }
3983 }
3984 
3985 void DatabaseViewer::view3DMap()
3986 {
3987  if(!ids_.size() || !dbDriver_)
3988  {
3989  QMessageBox::warning(this, tr("Cannot view 3D map"), tr("The database is empty..."));
3990  return;
3991  }
3992 
3993  if(graphes_.empty())
3994  {
3995  this->updateGraphView();
3996  if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
3997  {
3998  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
3999  return;
4000  }
4001  }
4002 
4003  std::map<int, Transform> optimizedPoses;
4004  if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty())
4005  {
4006  optimizedPoses = groundTruthPoses_;
4007  }
4008  else
4009  {
4010  optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
4011  }
4012  if(ui_->groupBox_posefiltering->isChecked())
4013  {
4014  optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
4015  ui_->doubleSpinBox_posefilteringRadius->value(),
4016  ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
4017  }
4018  if(optimizedPoses.size() > 0)
4019  {
4020  exportDialog_->setDBDriver(dbDriver_);
4021  exportDialog_->viewClouds(optimizedPoses,
4022  updateLinksWithModifications(links_),
4023  mapIds_,
4024  QMap<int, Signature>(),
4025  std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
4026  std::map<int, LaserScan>(),
4027  pathDatabase_,
4028  ui_->parameters_toolbox->getParameters());
4029  }
4030  else
4031  {
4032  QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
4033  }
4034 }
4035 
4036 void DatabaseViewer::generate3DMap()
4037 {
4038  if(!ids_.size() || !dbDriver_)
4039  {
4040  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty..."));
4041  return;
4042  }
4043 
4044  if(graphes_.empty())
4045  {
4046  this->updateGraphView();
4047  if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
4048  {
4049  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
4050  return;
4051  }
4052  }
4053 
4054  std::map<int, Transform> optimizedPoses;
4055  if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty())
4056  {
4057  optimizedPoses = groundTruthPoses_;
4058  }
4059  else
4060  {
4061  optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
4062  }
4063  if(ui_->groupBox_posefiltering->isChecked())
4064  {
4065  optimizedPoses = graph::radiusPosesFiltering(optimizedPoses,
4066  ui_->doubleSpinBox_posefilteringRadius->value(),
4067  ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
4068  }
4069  if(optimizedPoses.size() > 0)
4070  {
4071  exportDialog_->setDBDriver(dbDriver_);
4072  exportDialog_->exportClouds(optimizedPoses,
4073  updateLinksWithModifications(links_),
4074  mapIds_,
4075  QMap<int, Signature>(),
4076  std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> >(),
4077  std::map<int, LaserScan>(),
4078  pathDatabase_,
4079  ui_->parameters_toolbox->getParameters());
4080  }
4081  else
4082  {
4083  QMessageBox::critical(this, tr("Error"), tr("No neighbors found for node %1.").arg(ui_->spinBox_optimizationsFrom->value()));
4084  }
4085 }
4086 
4087 void DatabaseViewer::detectMoreLoopClosures()
4088 {
4089  if(graphes_.empty())
4090  {
4091  this->updateGraphView();
4092  if(graphes_.empty() || ui_->horizontalSlider_iterations->maximum() != (int)graphes_.size()-1)
4093  {
4094  QMessageBox::warning(this, tr("Cannot generate a graph"), tr("No graph in database?!"));
4095  return;
4096  }
4097  }
4098 
4099  std::map<int, Transform> optimizedPoses = graphes_.back();
4100 
4101  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
4102  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
4103  progressDialog->setMaximumSteps(1);
4104  progressDialog->setCancelButtonVisible(true);
4105  progressDialog->setMinimumWidth(800);
4106  progressDialog->show();
4107 
4108  const ParametersMap & parameters = ui_->parameters_toolbox->getParameters();
4109  bool loopCovLimited = Parameters::defaultRGBDLoopCovLimited();
4110  Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), loopCovLimited);
4111  if(loopCovLimited)
4112  {
4113  odomMaxInf_ = graph::getMaxOdomInf(updateLinksWithModifications(links_));
4114  }
4115 
4116  int iterations = ui_->spinBox_detectMore_iterations->value();
4117  UASSERT(iterations > 0);
4118  int added = 0;
4119  std::multimap<int, int> checkedLoopClosures;
4120  std::pair<int, int> lastAdded(0,0);
4121  bool intraSession = ui_->checkBox_detectMore_intraSession->isChecked();
4122  bool interSession = ui_->checkBox_detectMore_interSession->isChecked();
4123  if(!interSession && !intraSession)
4124  {
4125  QMessageBox::warning(this, tr("Cannot detect more loop closures"), tr("Intra and inter session parameters are disabled! Enable one or both."));
4126  return;
4127  }
4128 
4129  for(int n=0; n<iterations; ++n)
4130  {
4131  UINFO("iteration %d/%d", n+1, iterations);
4132  std::multimap<int, int> clusters = rtabmap::graph::radiusPosesClustering(
4133  std::map<int, Transform>(optimizedPoses.upper_bound(0), optimizedPoses.end()),
4134  ui_->doubleSpinBox_detectMore_radius->value(),
4135  ui_->doubleSpinBox_detectMore_angle->value()*CV_PI/180.0);
4136 
4137  progressDialog->setMaximumSteps(progressDialog->maximumSteps()+(int)clusters.size());
4138  progressDialog->appendText(tr("Looking for more loop closures, %1 clusters found.").arg(clusters.size()));
4139  QApplication::processEvents();
4140  if(progressDialog->isCanceled())
4141  {
4142  break;
4143  }
4144 
4145  std::set<int> addedLinks;
4146  int i=0;
4147  for(std::multimap<int, int>::iterator iter=clusters.begin(); iter!= clusters.end() && !progressDialog->isCanceled(); ++iter, ++i)
4148  {
4149  int from = iter->first;
4150  int to = iter->second;
4151  if(from < to)
4152  {
4153  from = iter->second;
4154  to = iter->first;
4155  }
4156 
4157  int mapIdFrom = uValue(mapIds_, from, 0);
4158  int mapIdTo = uValue(mapIds_, to, 0);
4159 
4160  if((interSession && mapIdFrom != mapIdTo) ||
4161  (intraSession && mapIdFrom == mapIdTo))
4162  {
4163  // only add new links and one per cluster per iteration
4164  if(rtabmap::graph::findLink(checkedLoopClosures, from, to) == checkedLoopClosures.end())
4165  {
4166  if(!findActiveLink(from, to).isValid() && !containsLink(linksRemoved_, from, to) &&
4167  addedLinks.find(from) == addedLinks.end() &&
4168  addedLinks.find(to) == addedLinks.end())
4169  {
4170  // Reverify if in the bounds with the current optimized graph
4171  Transform delta = optimizedPoses.at(from).inverse() * optimizedPoses.at(to);
4172  if(delta.getNorm() < ui_->doubleSpinBox_detectMore_radius->value() &&
4173  delta.getNorm() >= ui_->doubleSpinBox_detectMore_radiusMin->value())
4174  {
4175  checkedLoopClosures.insert(std::make_pair(from, to));
4176  if(addConstraint(from, to, true))
4177  {
4178  UINFO("Added new loop closure between %d and %d.", from, to);
4179  ++added;
4180  addedLinks.insert(from);
4181  addedLinks.insert(to);
4182  lastAdded.first = from;
4183  lastAdded.second = to;
4184 
4185  progressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
4186  QApplication::processEvents();
4187 
4188  optimizedPoses = graphes_.back();
4189  }
4190  }
4191  }
4192  }
4193  }
4194  progressDialog->incrementStep();
4195  if(i%100)
4196  {
4197  QApplication::processEvents();
4198  }
4199  }
4200  UINFO("Iteration %d/%d: added %d loop closures.", n+1, iterations, (int)addedLinks.size()/2);
4201  progressDialog->appendText(tr("Iteration %1/%2: Detected %3 loop closures!").arg(n+1).arg(iterations).arg(addedLinks.size()/2));
4202  if(addedLinks.size() == 0)
4203  {
4204  break;
4205  }
4206  if(n+1<iterations)
4207  {
4208  // Re-optimize the map before doing next iterations
4209  this->updateGraphView();
4210  optimizedPoses = graphes_.back();
4211  }
4212  }
4213 
4214  odomMaxInf_.clear();
4215 
4216  if(added)
4217  {
4218  this->updateGraphView();
4219  this->updateLoopClosuresSlider(lastAdded.first, lastAdded.second);
4220  }
4221  UINFO("Total added %d loop closures.", added);
4222 
4223  progressDialog->appendText(tr("Total new loop closures detected=%1").arg(added));
4224  progressDialog->setValue(progressDialog->maximumSteps());
4225 }
4226 
4227 void DatabaseViewer::updateAllNeighborCovariances()
4228 {
4229  updateAllCovariances(neighborLinks_);
4230 }
4231 void DatabaseViewer::updateAllLoopClosureCovariances()
4232 {
4233  QList<rtabmap::Link> links;
4234  for(int i=0; i<loopLinks_.size(); ++i)
4235  {
4236  if(loopLinks_.at(i).type() != Link::kLandmark)
4237  {
4238  links.push_back(loopLinks_.at(i));
4239  }
4240  }
4241  updateAllCovariances(links);
4242 }
4243 void DatabaseViewer::updateAllLandmarkCovariances()
4244 {
4245  QList<rtabmap::Link> links;
4246  for(int i=0; i<loopLinks_.size(); ++i)
4247  {
4248  if(loopLinks_.at(i).type() == Link::kLandmark)
4249  {
4250  links.push_back(loopLinks_.at(i));
4251  }
4252  }
4253  updateAllCovariances(links);
4254 }
4255 
4256 void DatabaseViewer::updateAllCovariances(const QList<Link> & links)
4257 {
4258  if(links.size())
4259  {
4260  bool ok = false;
4261  double stddev = QInputDialog::getDouble(this, tr("Linear error"), tr("Std deviation (m) 0=inf"), 0.01, 0.0, 9, 4, &ok);
4262  if(!ok) return;
4263  double linearVar = stddev*stddev;
4264  stddev = QInputDialog::getDouble(this, tr("Angular error"), tr("Std deviation (deg) 0=inf"), 1, 0.0, 90, 2, &ok)*M_PI/180.0;
4265  if(!ok) return;
4266  double angularVar = stddev*stddev;
4267 
4268  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
4269  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
4270  progressDialog->setMaximumSteps(links.size());
4271  progressDialog->setCancelButtonVisible(true);
4272  progressDialog->setMinimumWidth(800);
4273  progressDialog->show();
4274 
4275  cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
4276  if(linearVar == 0.0)
4277  {
4278  infMatrix(cv::Range(0,3), cv::Range(0,3)) /= 9999.9;
4279  }
4280  else
4281  {
4282  infMatrix(cv::Range(0,3), cv::Range(0,3)) /= linearVar;
4283  }
4284  if(angularVar == 0.0)
4285  {
4286  infMatrix(cv::Range(3,6), cv::Range(3,6)) /= 9999.9;
4287  }
4288  else
4289  {
4290  infMatrix(cv::Range(3,6), cv::Range(3,6)) /= angularVar;
4291  }
4292 
4293  for(int i=0; i<links.size(); ++i)
4294  {
4295  int from = links[i].from();
4296  int to = links[i].to();
4297 
4298  Link currentLink = findActiveLink(from, to);
4299  if(!currentLink.isValid())
4300  {
4301  UERROR("Not found link! (%d->%d)", from, to);
4302  return;
4303  }
4304  currentLink = Link(
4305  currentLink.from(),
4306  currentLink.to(),
4307  currentLink.type(),
4308  currentLink.transform(),
4309  infMatrix.clone(),
4310  currentLink.userDataCompressed());
4311  bool updated = false;
4312  std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
4313  while(iter != linksRefined_.end() && iter->first == currentLink.from())
4314  {
4315  if(iter->second.to() == currentLink.to() &&
4316  iter->second.type() == currentLink.type())
4317  {
4318  iter->second = currentLink;
4319  updated = true;
4320  break;
4321  }
4322  ++iter;
4323  }
4324  if(!updated)
4325  {
4326  linksRefined_.insert(std::make_pair(currentLink.from(), currentLink));
4327  }
4328 
4329  progressDialog->appendText(tr("Updated link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(links.size()));
4330  progressDialog->incrementStep();
4331  QApplication::processEvents();
4332  if(progressDialog->isCanceled())
4333  {
4334  break;
4335  }
4336  }
4337  this->updateGraphView();
4338 
4339  progressDialog->setValue(progressDialog->maximumSteps());
4340  progressDialog->appendText("Refining links finished!");
4341  }
4342 }
4343 
4344 void DatabaseViewer::refineAllNeighborLinks()
4345 {
4346  refineAllLinks(neighborLinks_);
4347 }
4348 void DatabaseViewer::refineAllLoopClosureLinks()
4349 {
4350  refineAllLinks(loopLinks_);
4351 }
4352 void DatabaseViewer::refineAllLinks(const QList<Link> & links)
4353 {
4354  if(links.size())
4355  {
4356  rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
4357  progressDialog->setAttribute(Qt::WA_DeleteOnClose);
4358  progressDialog->setMaximumSteps(links.size());
4359  progressDialog->setCancelButtonVisible(true);
4360  progressDialog->setMinimumWidth(800);
4361  progressDialog->show();
4362 
4363  for(int i=0; i<links.size(); ++i)
4364  {
4365  int from = links[i].from();
4366  int to = links[i].to();
4367  if(from > 0 && to > 0)
4368  {
4369  this->refineConstraint(links[i].from(), links[i].to(), true);
4370  progressDialog->appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(links.size()));
4371  }
4372  else
4373  {
4374  progressDialog->appendText(tr("Ignored link %1->%2 (landmark)").arg(from).arg(to));
4375  }
4376  progressDialog->incrementStep();
4377  QApplication::processEvents();
4378  if(progressDialog->isCanceled())
4379  {
4380  break;
4381  }
4382  }
4383  this->updateGraphView();
4384 
4385  progressDialog->setValue(progressDialog->maximumSteps());
4386  progressDialog->appendText("Refining links finished!");
4387  }
4388 }
4389 
4390 void DatabaseViewer::resetAllChanges()
4391 {
4392  if(QMessageBox::question(this,
4393  tr("Reset all changes"),
4394  tr("You are about to reset all changes you've made so far, do you want to continue?"),
4395  QMessageBox::Yes | QMessageBox::No,
4396  QMessageBox::No) == QMessageBox::Yes)
4397  {
4398  linksAdded_.clear();
4399  linksRefined_.clear();
4400  linksRemoved_.clear();
4401  generatedLocalMaps_.clear();
4402  generatedLocalMapsInfo_.clear();
4403  modifiedLaserScans_.clear();
4404  updateLoopClosuresSlider();
4405  this->updateGraphView();
4406  }
4407 }
4408 
4409 void DatabaseViewer::sliderAValueChanged(int value)
4410 {
4411  this->update(value,
4412  ui_->label_indexA,
4413  ui_->label_parentsA,
4414  ui_->label_childrenA,
4415  ui_->label_weightA,
4416  ui_->label_labelA,
4417  ui_->label_stampA,
4418  ui_->graphicsView_A,
4419  ui_->label_idA,
4420  ui_->label_mapA,
4421  ui_->label_poseA,
4422  ui_->label_optposeA,
4423  ui_->label_velA,
4424  ui_->label_calibA,
4425  ui_->label_scanA,
4426  ui_->label_gravityA,
4427  ui_->label_priorA,
4428  ui_->label_gpsA,
4429  ui_->label_gtA,
4430  ui_->label_sensorsA,
4431  true);
4432 }
4433 
4434 void DatabaseViewer::sliderBValueChanged(int value)
4435 {
4436  this->update(value,
4437  ui_->label_indexB,
4438  ui_->label_parentsB,
4439  ui_->label_childrenB,
4440  ui_->label_weightB,
4441  ui_->label_labelB,
4442  ui_->label_stampB,
4443  ui_->graphicsView_B,
4444  ui_->label_idB,
4445  ui_->label_mapB,
4446  ui_->label_poseB,
4447  ui_->label_optposeB,
4448  ui_->label_velB,
4449  ui_->label_calibB,
4450  ui_->label_scanB,
4451  ui_->label_gravityB,
4452  ui_->label_priorB,
4453  ui_->label_gpsB,
4454  ui_->label_gtB,
4455  ui_->label_sensorsB,
4456  true);
4457 }
4458 
4459 void DatabaseViewer::update(int value,
4460  QLabel * labelIndex,
4461  QLabel * labelParents,
4462  QLabel * labelChildren,
4463  QLabel * weight,
4464  QLabel * label,
4465  QLabel * stamp,
4466  rtabmap::ImageView * view,
4467  QLabel * labelId,
4468  QLabel * labelMapId,
4469  QLabel * labelPose,
4470  QLabel * labelOptPose,
4471  QLabel * labelVelocity,
4472  QLabel * labelCalib,
4473  QLabel * labelScan,
4474  QLabel * labelGravity,
4475  QLabel * labelPrior,
4476  QLabel * labelGps,
4477  QLabel * labelGt,
4478  QLabel * labelSensors,
4479  bool updateConstraintView)
4480 {
4481  lastSliderIndexBrowsed_ = value;
4482 
4483  UTimer timer;
4484  labelIndex->setText(QString::number(value));
4485  labelParents->clear();
4486  labelChildren->clear();
4487  weight->clear();
4488  label->clear();
4489  labelMapId->clear();
4490  labelPose->clear();
4491  labelOptPose->clear();
4492  labelVelocity->clear();
4493  stamp->clear();
4494  labelCalib->clear();
4495  labelScan ->clear();
4496  labelGravity->clear();
4497  labelPrior->clear();
4498  labelGps->clear();
4499  labelGt->clear();
4500  labelSensors->clear();
4501  if(value >= 0 && value < ids_.size())
4502  {
4503  view->clear();
4504  int id = ids_.at(value);
4505  int mapId = -1;
4506  labelId->setText(QString::number(id));
4507  if(id>0)
4508  {
4509  //image
4510  QImage img;
4511  cv::Mat imgDepth;
4512  if(dbDriver_)
4513  {
4514  SensorData data;
4515  dbDriver_->getNodeData(id, data);
4516  data.uncompressData();
4517  if(!data.imageRaw().empty())
4518  {
4519  img = uCvMat2QImage(data.imageRaw());
4520  }
4521  if(!data.depthOrRightRaw().empty())
4522  {
4523  cv::Mat depth =data.depthOrRightRaw();
4524  if(!data.depthRaw().empty())
4525  {
4526  if(ui_->spinBox_mesh_fillDepthHoles->value() > 0)
4527  {
4528  depth = util2d::fillDepthHoles(depth, ui_->spinBox_mesh_fillDepthHoles->value(), float(ui_->spinBox_mesh_depthError->value())/100.0f);
4529  }
4530  }
4531  if( !data.imageRaw().empty() &&
4532  !data.rightRaw().empty() &&
4533  data.stereoCameraModels().size()==1 && // Multiple stereo cameras not implemented
4534  data.stereoCameraModels()[0].isValidForProjection() &&
4535  ui_->checkBox_showDisparityInsteadOfRight->isChecked())
4536  {
4537  rtabmap::StereoDense * denseStereo = rtabmap::StereoDense::create(ui_->parameters_toolbox->getParameters());
4538  depth = util2d::depthFromDisparity(denseStereo->computeDisparity(data.imageRaw(), data.rightRaw()), data.stereoCameraModels()[0].left().fx(), data.stereoCameraModels()[0].baseline(), CV_32FC1);
4539  delete denseStereo;
4540  }
4541  imgDepth = depth;
4542  }
4543 
4544  QRectF rect;
4545  if(!img.isNull())
4546  {
4547  view->setImage(img);
4548  rect = img.rect();
4549  }
4550  else
4551  {
4552  ULOGGER_DEBUG("Image is empty");
4553  }
4554 
4555  if(!imgDepth.empty())
4556  {
4557  view->setImageDepth(imgDepth);
4558  if(img.isNull())
4559  {
4560  rect.setWidth(imgDepth.cols);
4561  rect.setHeight(imgDepth.rows);
4562  }
4563  }
4564  else
4565  {
4566  ULOGGER_DEBUG("Image depth is empty");
4567  }
4568  if(rect.isValid())
4569  {
4570  view->setSceneRect(rect);
4571  }
4572 
4573  std::list<int> ids;
4574  ids.push_back(id);
4575  std::list<Signature*> signatures;
4576  dbDriver_->loadSignatures(ids, signatures);
4577 
4578  if(signatures.size() && signatures.front()!=0 && !signatures.front()->getWordsKpts().empty())
4579  {
4580  std::multimap<int, cv::KeyPoint> keypoints;
4581  for(std::map<int, int>::const_iterator iter=signatures.front()->getWords().begin(); iter!=signatures.front()->getWords().end(); ++iter)
4582  {
4583  keypoints.insert(std::make_pair(iter->first, signatures.front()->getWordsKpts()[iter->second]));
4584  }
4585  view->setFeatures(keypoints, data.depthOrRightRaw().type() == CV_8UC1?cv::Mat():data.depthOrRightRaw(), Qt::yellow);
4586  }
4587 
4588  Transform odomPose, g;
4589  int w;
4590  std::string l;
4591  double s;
4592  std::vector<float> v;
4593  GPS gps;
4594  EnvSensors sensors;
4595  dbDriver_->getNodeInfo(id, odomPose, mapId, w, l, s, g, v, gps, sensors);
4596 
4597  weight->setNum(w);
4598  label->setText(l.c_str());
4599  float x,y,z,roll,pitch,yaw;
4600  odomPose.getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw);
4601  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));
4602  if(graphes_.size())
4603  {
4604  if(graphes_.back().find(id) == graphes_.back().end())
4605  {
4606  labelOptPose->setText("<Not in optimized graph>");
4607  }
4608  else
4609  {
4610  graphes_.back().find(id)->second.getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw);
4611  labelOptPose->setText(QString("xyz=(%1,%2,%3)\nrpy=(%4,%5,%6)").arg(x).arg(y).arg(z).arg(roll).arg(pitch).arg(yaw));
4612  }
4613  }
4614  if(s!=0.0)
4615  {
4616  stamp->setText(QString::number(s, 'f'));
4617  stamp->setToolTip(QDateTime::fromMSecsSinceEpoch(s*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
4618  }
4619  if(v.size()==6)
4620  {
4621  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]));
4622  }
4623 
4624  std::multimap<int, Link> gravityLink;
4625  dbDriver_->loadLinks(id, gravityLink, Link::kGravity);
4626  if(!gravityLink.empty())
4627  {
4628  float roll,pitch,yaw;
4629  gravityLink.begin()->second.transform().getEulerAngles(roll, pitch, yaw);
4630  Eigen::Vector3d v = Transform(0,0,0,roll,pitch,0).toEigen3d() * -Eigen::Vector3d::UnitZ();
4631  labelGravity->setText(QString("x=%1 y=%2 z=%3").arg(v[0]).arg(v[1]).arg(v[2]));
4632  labelGravity->setToolTip(QString("roll=%1 pitch=%2 yaw=%3").arg(roll).arg(pitch).arg(yaw));
4633  }
4634 
4635  std::multimap<int, Link> priorLink;
4636  dbDriver_->loadLinks(id, priorLink, Link::kPosePrior);
4637  if(!priorLink.empty())
4638  {
4639  priorLink.begin()->second.transform().getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw);
4640  labelPrior->setText(QString("xyz=(%1,%2,%3)\nrpy=(%4,%5,%6)").arg(x).arg(y).arg(z).arg(roll).arg(pitch).arg(yaw));
4641  std::stringstream out;
4642  out << priorLink.begin()->second.infMatrix().inv();
4643  labelPrior->setToolTip(QString("%1").arg(out.str().c_str()));
4644  }
4645 
4646  if(gps.stamp()>0.0)
4647  {
4648  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()));
4649  labelGps->setToolTip(QDateTime::fromMSecsSinceEpoch(gps.stamp()*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
4650  }
4651  if(!g.isNull())
4652  {
4653  labelGt->setText(QString("%1").arg(g.prettyPrint().c_str()));
4654  }
4655  if(sensors.size())
4656  {
4657  QString sensorsStr;
4658  QString tooltipStr;
4659  for(EnvSensors::iterator iter=sensors.begin(); iter!=sensors.end(); ++iter)
4660  {
4661  if(iter != sensors.begin())
4662  {
4663  sensorsStr += " | ";
4664  tooltipStr += " | ";
4665  }
4666 
4667  if(iter->first == EnvSensor::kWifiSignalStrength)
4668  {
4669  sensorsStr += uFormat("%.1f dbm", iter->second.value()).c_str();
4670  tooltipStr += "Wifi signal strength";
4671  }
4672  else if(iter->first == EnvSensor::kAmbientTemperature)
4673  {
4674  sensorsStr += uFormat("%.1f \u00B0C", iter->second.value()).c_str();
4675  tooltipStr += "Ambient Temperature";
4676  }
4677  else if(iter->first == EnvSensor::kAmbientAirPressure)
4678  {
4679  sensorsStr += uFormat("%.1f hPa", iter->second.value()).c_str();
4680  tooltipStr += "Ambient Air Pressure";
4681  }
4682  else if(iter->first == EnvSensor::kAmbientLight)
4683  {
4684  sensorsStr += uFormat("%.0f lx", iter->second.value()).c_str();
4685  tooltipStr += "Ambient Light";
4686  }
4687  else if(iter->first == EnvSensor::kAmbientRelativeHumidity)
4688  {
4689  sensorsStr += uFormat("%.0f %%", iter->second.value()).c_str();
4690  tooltipStr += "Ambient Relative Humidity";
4691  }
4692  else
4693  {
4694  sensorsStr += uFormat("%.2f", iter->second.value()).c_str();
4695  tooltipStr += QString("Type %1").arg((int)iter->first);
4696  }
4697 
4698  }
4699  labelSensors->setText(sensorsStr);
4700  labelSensors->setToolTip(tooltipStr);
4701  }
4702  if(data.cameraModels().size() || data.stereoCameraModels().size())
4703  {
4704  std::stringstream calibrationDetails;
4705  if(data.cameraModels().size())
4706  {
4707  if(!data.depthRaw().empty() && data.depthRaw().cols!=data.imageRaw().cols && data.imageRaw().cols)
4708  {
4709  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]")
4710  .arg(data.cameraModels().size())
4711  .arg(data.cameraModels()[0].imageWidth()>0?data.cameraModels()[0].imageWidth():data.imageRaw().cols/data.cameraModels().size())
4712  .arg(data.cameraModels()[0].imageHeight()>0?data.cameraModels()[0].imageHeight():data.imageRaw().rows)
4713  .arg(data.cameraModels()[0].fx())
4714  .arg(data.cameraModels()[0].fy())
4715  .arg(data.cameraModels()[0].cx())
4716  .arg(data.cameraModels()[0].cy())
4717  .arg(data.depthRaw().cols/data.cameraModels().size())
4718  .arg(data.depthRaw().rows)
4719  .arg(data.cameraModels()[0].localTransform().prettyPrint().c_str())
4720  .arg(data.cameraModels()[0].localTransform().r11()).arg(data.cameraModels()[0].localTransform().r12()).arg(data.cameraModels()[0].localTransform().r13()).arg(data.cameraModels()[0].localTransform().o14())
4721  .arg(data.cameraModels()[0].localTransform().r21()).arg(data.cameraModels()[0].localTransform().r22()).arg(data.cameraModels()[0].localTransform().r23()).arg(data.cameraModels()[0].localTransform().o24())
4722  .arg(data.cameraModels()[0].localTransform().r31()).arg(data.cameraModels()[0].localTransform().r32()).arg(data.cameraModels()[0].localTransform().r33()).arg(data.cameraModels()[0].localTransform().o34()));
4723  }
4724  else
4725  {
4726  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]")
4727  .arg(data.cameraModels().size())
4728  .arg(data.cameraModels()[0].imageWidth()>0?data.cameraModels()[0].imageWidth():data.imageRaw().cols/data.cameraModels().size())
4729  .arg(data.cameraModels()[0].imageHeight()>0?data.cameraModels()[0].imageHeight():data.imageRaw().rows)
4730  .arg(data.cameraModels()[0].fx())
4731  .arg(data.cameraModels()[0].fy())
4732  .arg(data.cameraModels()[0].cx())
4733  .arg(data.cameraModels()[0].cy())
4734  .arg(data.cameraModels()[0].localTransform().prettyPrint().c_str())
4735  .arg(data.cameraModels()[0].localTransform().r11()).arg(data.cameraModels()[0].localTransform().r12()).arg(data.cameraModels()[0].localTransform().r13()).arg(data.cameraModels()[0].localTransform().o14())
4736  .arg(data.cameraModels()[0].localTransform().r21()).arg(data.cameraModels()[0].localTransform().r22()).arg(data.cameraModels()[0].localTransform().r23()).arg(data.cameraModels()[0].localTransform().o24())
4737  .arg(data.cameraModels()[0].localTransform().r31()).arg(data.cameraModels()[0].localTransform().r32()).arg(data.cameraModels()[0].localTransform().r33()).arg(data.cameraModels()[0].localTransform().o34()));
4738  }
4739 
4740  for(unsigned int i=0; i<data.cameraModels().size();++i)
4741  {
4742  if(i!=0) calibrationDetails << std::endl;
4743  calibrationDetails << "Id: " << i << " Size=" << data.cameraModels()[i].imageWidth() << "x" << data.cameraModels()[i].imageHeight() << std::endl;
4744  if( data.cameraModels()[i].K_raw().total()) calibrationDetails << "K=" << data.cameraModels()[i].K_raw() << std::endl;
4745  if( data.cameraModels()[i].D_raw().total()) calibrationDetails << "D=" << data.cameraModels()[i].D_raw() << std::endl;
4746  if( data.cameraModels()[i].R().total()) calibrationDetails << "R=" << data.cameraModels()[i].R() << std::endl;
4747  if( data.cameraModels()[i].P().total()) calibrationDetails << "P=" << data.cameraModels()[i].P() << std::endl;
4748  }
4749 
4750  }
4751  else if(data.stereoCameraModels().size())
4752  {
4753  //stereo
4754  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]")
4755  .arg(data.stereoCameraModels()[0].left().imageWidth()>0?data.stereoCameraModels()[0].left().imageWidth():data.imageRaw().cols)
4756  .arg(data.stereoCameraModels()[0].left().imageHeight()>0?data.stereoCameraModels()[0].left().imageHeight():data.imageRaw().rows)
4757  .arg(data.stereoCameraModels()[0].left().fx())
4758  .arg(data.stereoCameraModels()[0].left().fy())
4759  .arg(data.stereoCameraModels()[0].left().cx())
4760  .arg(data.stereoCameraModels()[0].left().cy())
4761  .arg(data.stereoCameraModels()[0].baseline())
4762  .arg(data.stereoCameraModels()[0].localTransform().prettyPrint().c_str())
4763  .arg(data.stereoCameraModels()[0].localTransform().r11()).arg(data.stereoCameraModels()[0].localTransform().r12()).arg(data.stereoCameraModels()[0].localTransform().r13()).arg(data.stereoCameraModels()[0].localTransform().o14())
4764  .arg(data.stereoCameraModels()[0].localTransform().r21()).arg(data.stereoCameraModels()[0].localTransform().r22()).arg(data.stereoCameraModels()[0].localTransform().r23()).arg(data.stereoCameraModels()[0].localTransform().o24())
4765  .arg(data.stereoCameraModels()[0].localTransform().r31()).arg(data.stereoCameraModels()[0].localTransform().r32()).arg(data.stereoCameraModels()[0].localTransform().r33()).arg(data.stereoCameraModels()[0].localTransform().o34()));
4766 
4767  for(unsigned int i=0; i<data.stereoCameraModels().size();++i)
4768  {
4769  calibrationDetails << "Id: " << i << std::endl;
4770  calibrationDetails << " Left:" << " Size=" << data.stereoCameraModels()[i].left().imageWidth() << "x" << data.stereoCameraModels()[i].left().imageHeight() << std::endl;
4771  if( data.stereoCameraModels()[i].left().K_raw().total()) calibrationDetails << " K=" << data.stereoCameraModels()[i].left().K_raw() << std::endl;
4772  if( data.stereoCameraModels()[i].left().D_raw().total()) calibrationDetails << " D=" << data.stereoCameraModels()[i].left().D_raw() << std::endl;
4773  if( data.stereoCameraModels()[i].left().R().total()) calibrationDetails << " R=" << data.stereoCameraModels()[i].left().R() << std::endl;
4774  if( data.stereoCameraModels()[i].left().P().total()) calibrationDetails << " P=" << data.stereoCameraModels()[i].left().P() << std::endl;
4775  calibrationDetails << " Right:" << " Size=" << data.stereoCameraModels()[i].right().imageWidth() << "x" << data.stereoCameraModels()[i].right().imageHeight() << std::endl;
4776  if( data.stereoCameraModels()[i].right().K_raw().total()) calibrationDetails << " K=" << data.stereoCameraModels()[i].right().K_raw() << std::endl;
4777  if( data.stereoCameraModels()[i].right().D_raw().total()) calibrationDetails << " D=" << data.stereoCameraModels()[i].right().D_raw() << std::endl;
4778  if( data.stereoCameraModels()[i].right().R().total()) calibrationDetails << " R=" << data.stereoCameraModels()[i].right().R() << std::endl;
4779  if( data.stereoCameraModels()[i].right().P().total()) calibrationDetails << " P=" << data.stereoCameraModels()[i].right().P() << std::endl;
4780  if( data.stereoCameraModels()[i].R().total()) calibrationDetails << " R=" << data.stereoCameraModels()[i].R() << std::endl;
4781  if( data.stereoCameraModels()[i].T().total()) calibrationDetails << " T=" << data.stereoCameraModels()[i].T() << std::endl;
4782  if( data.stereoCameraModels()[i].F().total()) calibrationDetails << " F=" << data.stereoCameraModels()[i].F() << std::endl;
4783  if( data.stereoCameraModels()[i].E().total()) calibrationDetails << " E=" << data.stereoCameraModels()[i].E() << std::endl;
4784  }
4785  }
4786  labelCalib->setToolTip(calibrationDetails.str().c_str());
4787 
4788  }
4789  else
4790  {
4791  labelCalib->setText("NA");
4792  }
4793 
4794  if(data.laserScanRaw().size())
4795  {
4796  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")
4797  .arg(data.laserScanRaw().formatName().c_str())
4798  .arg(data.laserScanRaw().size())
4799  .arg(data.laserScanRaw().maxPoints())
4800  .arg(data.laserScanRaw().rangeMin())
4801  .arg(data.laserScanRaw().rangeMax())
4802  .arg(data.laserScanRaw().angleMin())
4803  .arg(data.laserScanRaw().angleMax())
4804  .arg(data.laserScanRaw().angleIncrement())
4805  .arg(data.laserScanRaw().hasRGB()?1:0)
4806  .arg(data.laserScanRaw().is2d()?1:0)
4807  .arg(data.laserScanRaw().hasNormals()?1:0)
4808  .arg(data.laserScanRaw().hasIntensity()?1:0)
4809  .arg(data.laserScanRaw().localTransform().prettyPrint().c_str()));
4810  }
4811 
4812  //stereo
4813  if(!data.depthOrRightRaw().empty() && data.depthOrRightRaw().type() == CV_8UC1)
4814  {
4815  this->updateStereo(&data);
4816  }
4817  else
4818  {
4819  stereoViewer_->clear();
4820  ui_->graphicsView_stereo->clear();
4821  }
4822 
4823  // 3d view
4824  if(cloudViewer_->isVisible())
4825  {
4826  cloudViewer_->removeAllLines();
4827  cloudViewer_->removeAllFrustums();
4828  cloudViewer_->removeCloud("mesh");
4829  cloudViewer_->removeCloud("cloud");
4830  cloudViewer_->removeCloud("scan");
4831  cloudViewer_->removeCloud("map");
4832  cloudViewer_->removeCloud("ground");
4833  cloudViewer_->removeCloud("obstacles");
4834  cloudViewer_->removeCloud("empty_cells");
4835  cloudViewer_->removeCloud("words");
4836  cloudViewer_->removeOctomap();
4837 
4838  Transform pose = Transform::getIdentity();
4839  if(signatures.size() && ui_->checkBox_odomFrame_3dview->isChecked())
4840  {
4841  float x, y, z, roll, pitch, yaw;
4842  (*signatures.begin())->getPose().getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
4843  pose = Transform(0,0,z,roll,pitch,0);
4844  }
4845 
4846  if(!gravityLink.empty() && ui_->checkBox_gravity_3dview->isChecked())
4847  {
4848  Transform gravityT = gravityLink.begin()->second.transform();
4849  Eigen::Vector3f gravity(0,0,-1);
4850  if(pose.isIdentity())
4851  {
4852  gravityT = gravityT.inverse();
4853  }
4854  gravity = (gravityT.rotation()*(pose).rotation().inverse()).toEigen3f()*gravity;
4855  cloudViewer_->addOrUpdateLine("gravity", pose, (pose).translation()*Transform(gravity[0], gravity[1], gravity[2], 0, 0, 0)*pose.rotation().inverse(), Qt::yellow, true, false);
4856  }
4857 
4858  //add scan
4859  LaserScan laserScanRaw = data.laserScanRaw();
4860  if(modifiedLaserScans_.find(id)!=modifiedLaserScans_.end())
4861  {
4862  laserScanRaw = modifiedLaserScans_.at(id);
4863  }
4864  if(ui_->checkBox_showScan->isChecked() && laserScanRaw.size())
4865  {
4866  if(laserScanRaw.hasRGB() && laserScanRaw.hasNormals())
4867  {
4868  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr scan = util3d::laserScanToPointCloudRGBNormal(laserScanRaw, laserScanRaw.localTransform());
4869  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
4870  }
4871  else if(laserScanRaw.hasIntensity() && laserScanRaw.hasNormals())
4872  {
4873  pcl::PointCloud<pcl::PointXYZINormal>::Ptr scan = util3d::laserScanToPointCloudINormal(laserScanRaw, laserScanRaw.localTransform());
4874  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
4875  }
4876  else if(laserScanRaw.hasNormals())
4877  {
4878  pcl::PointCloud<pcl::PointNormal>::Ptr scan = util3d::laserScanToPointCloudNormal(laserScanRaw, laserScanRaw.localTransform());
4879  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
4880  }
4881  else if(laserScanRaw.hasRGB())
4882  {
4883  pcl::PointCloud<pcl::PointXYZRGB>::Ptr scan = util3d::laserScanToPointCloudRGB(laserScanRaw, laserScanRaw.localTransform());
4884  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
4885  }
4886  else if(laserScanRaw.hasIntensity())
4887  {
4888  pcl::PointCloud<pcl::PointXYZI>::Ptr scan = util3d::laserScanToPointCloudI(laserScanRaw, laserScanRaw.localTransform());
4889  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
4890  }
4891  else
4892  {
4893  pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(laserScanRaw, laserScanRaw.localTransform());
4894  cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
4895  }
4896  }
4897 
4898  // add RGB-D cloud
4899  if(ui_->checkBox_showCloud->isChecked() && ui_->checkBox_cameraProjection->isChecked() &&
4900  !data.imageRaw().empty() && !laserScanRaw.empty() && !laserScanRaw.is2d())
4901  {
4902  pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud = util3d::laserScanToPointCloudINormal(laserScanRaw, laserScanRaw.localTransform());
4903  std::vector<CameraModel> models = data.cameraModels();
4904  if(!data.stereoCameraModels().empty())
4905  {
4906  models.clear();
4907  for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
4908  {
4909  models.push_back(data.stereoCameraModels()[i].left());
4910  }
4911  }
4912 
4913  if(!models.empty() && !models[0].isValidForProjection())
4914  {
4915  models.clear();
4916  }
4917 
4918  if(!models.empty() && models[0].imageWidth() != 0 && models[0].imageHeight() != 0)
4919  {
4920  std::map<int, Transform> cameraPoses;
4921  std::map<int, std::vector<CameraModel> > cameraModels;
4922  cameraPoses.insert(std::make_pair(data.id(), Transform::getIdentity()));
4923  cameraModels.insert(std::make_pair(data.id(), models));
4924 
4925  std::vector<std::pair< std::pair<int, int>, pcl::PointXY> > pointToPixel;
4926  pointToPixel = util3d::projectCloudToCameras(
4927  *cloud,
4928  cameraPoses,
4929  cameraModels);
4930  // color the cloud
4931  UASSERT(pointToPixel.empty() || pointToPixel.size() == cloud->size());
4932  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudValidPoints(new pcl::PointCloud<pcl::PointXYZRGB>);
4933  cloudValidPoints->resize(cloud->size());
4934  int oi=0;
4935  for(size_t i=0; i<pointToPixel.size(); ++i)
4936  {
4937  pcl::PointXYZINormal & pt = cloud->at(i);
4938  pcl::PointXYZRGB ptColor;
4939  int nodeID = pointToPixel[i].first.first;
4940  int cameraIndex = pointToPixel[i].first.second;
4941  if(nodeID>0 && cameraIndex>=0)
4942  {
4943  cv::Mat image = data.imageRaw();
4944 
4945  int subImageWidth = image.cols / cameraModels.at(nodeID).size();
4946  image = image(cv::Range::all(), cv::Range(cameraIndex*subImageWidth, (cameraIndex+1)*subImageWidth));
4947 
4948 
4949  int x = pointToPixel[i].second.x * (float)image.cols;
4950  int y = pointToPixel[i].second.y * (float)image.rows;
4951  UASSERT(x>=0 && x<image.cols);
4952  UASSERT(y>=0 && y<image.rows);
4953 
4954  if(image.type()==CV_8UC3)
4955  {
4956  cv::Vec3b bgr = image.at<cv::Vec3b>(y, x);
4957  ptColor.b = bgr[0];
4958  ptColor.g = bgr[1];
4959  ptColor.r = bgr[2];
4960  }
4961  else
4962  {
4963  UASSERT(image.type()==CV_8UC1);
4964  ptColor.r = ptColor.g = ptColor.b = image.at<unsigned char>(pointToPixel[i].second.y * image.rows, pointToPixel[i].second.x * image.cols);
4965  }
4966 
4967  ptColor.x = pt.x;
4968  ptColor.y = pt.y;
4969  ptColor.z = pt.z;
4970  cloudValidPoints->at(oi++) = ptColor;
4971  }
4972  }
4973  cloudValidPoints->resize(oi);
4974  if(!cloudValidPoints->empty())
4975  {
4976  if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
4977  {
4978  cloudValidPoints = util3d::voxelize(cloudValidPoints, ui_->doubleSpinBox_voxelSize->value());
4979  }
4980 
4981  cloudViewer_->addCloud("cloud", cloudValidPoints, pose);
4982  }
4983  else
4984  {
4985  UWARN("Camera projection to scan returned an empty cloud, no visible points from cameras...");
4986  }
4987  }
4988  else
4989  {
4990  UERROR("Node has invalid camera models, camera projection on scan cannot be done!.");
4991  }
4992  }
4993  else if(ui_->checkBox_showCloud->isChecked() || ui_->checkBox_showMesh->isChecked())
4994  {
4995  if(!data.depthOrRightRaw().empty())
4996  {
4997  if(!data.imageRaw().empty())
4998  {
4999  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
5000  pcl::IndicesPtr indices(new std::vector<int>);
5001  if(!data.depthRaw().empty() && data.cameraModels().size()==1)
5002  {
5003  cv::Mat depth = data.depthRaw();
5004  if(ui_->spinBox_mesh_fillDepthHoles->value() > 0)
5005  {
5006  depth = util2d::fillDepthHoles(depth, ui_->spinBox_mesh_fillDepthHoles->value(), float(ui_->spinBox_mesh_depthError->value())/100.0f);
5007  }
5008  cloud = util3d::cloudFromDepthRGB(
5009  data.imageRaw(),
5010  depth,
5011  data.cameraModels()[0],
5012  ui_->spinBox_decimation->value(),0,0,indices.get());
5013  if(indices->size())
5014  {
5015  cloud = util3d::transformPointCloud(cloud, data.cameraModels()[0].localTransform());
5016  }
5017 
5018  }
5019  else
5020  {
5021  cloud = util3d::cloudRGBFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, indices.get(), ui_->parameters_toolbox->getParameters());
5022  }
5023  if(indices->size())
5024  {
5025  if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
5026  {
5027  cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value());
5028  }
5029 
5030  if(ui_->checkBox_showMesh->isChecked() && !cloud->is_dense)
5031  {
5032  Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
5033  if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
5034  {
5035  viewpoint[0] = data.cameraModels()[0].localTransform().x();
5036  viewpoint[1] = data.cameraModels()[0].localTransform().y();
5037  viewpoint[2] = data.cameraModels()[0].localTransform().z();
5038  }
5039  else if(data.stereoCameraModels().size() && !data.stereoCameraModels()[0].localTransform().isNull())
5040  {
5041  viewpoint[0] = data.stereoCameraModels()[0].localTransform().x();
5042  viewpoint[1] = data.stereoCameraModels()[0].localTransform().y();
5043  viewpoint[2] = data.stereoCameraModels()[0].localTransform().z();
5044  }
5045  std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
5046  cloud,
5047  float(ui_->spinBox_mesh_angleTolerance->value())*M_PI/180.0f,
5048  ui_->checkBox_mesh_quad->isChecked(),
5049  ui_->spinBox_mesh_triangleSize->value(),
5050  viewpoint);
5051 
5052  if(ui_->spinBox_mesh_minClusterSize->value())
5053  {
5054  // filter polygons
5055  std::vector<std::set<int> > neighbors;
5056  std::vector<std::set<int> > vertexToPolygons;
5057  util3d::createPolygonIndexes(polygons,
5058  cloud->size(),
5059  neighbors,
5060  vertexToPolygons);
5061  std::list<std::list<int> > clusters = util3d::clusterPolygons(
5062  neighbors,
5063  ui_->spinBox_mesh_minClusterSize->value());
5064  std::vector<pcl::Vertices> filteredPolygons(polygons.size());
5065  int oi=0;
5066  for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
5067  {
5068  for(std::list<int>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
5069  {
5070  filteredPolygons[oi++] = polygons.at(*jter);
5071  }
5072  }
5073  filteredPolygons.resize(oi);
5074  polygons = filteredPolygons;
5075  }
5076 
5077  cloudViewer_->addCloudMesh("mesh", cloud, polygons, pose);
5078  }
5079  if(ui_->checkBox_showCloud->isChecked())
5080  {
5081  cloudViewer_->addCloud("cloud", cloud, pose);
5082  }
5083  }
5084  }
5085  else if(ui_->checkBox_showCloud->isChecked())
5086  {
5087  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
5088  pcl::IndicesPtr indices(new std::vector<int>);
5089  cloud = util3d::cloudFromSensorData(data, ui_->spinBox_decimation->value(), 0, 0, indices.get(), ui_->parameters_toolbox->getParameters());
5090  if(indices->size())
5091  {
5092  if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
5093  {
5094  cloud = util3d::voxelize(cloud, indices, ui_->doubleSpinBox_voxelSize->value());
5095  }
5096 
5097  cloudViewer_->addCloud("cloud", cloud, pose);
5098  }
5099  }
5100  }
5101  }
5102 
5103  //frustums
5104  if(cloudViewer_->isFrustumShown())
5105  {
5106  if(data.cameraModels().size())
5107  {
5108  cloudViewer_->updateCameraFrustums(pose, data.cameraModels());
5109  }
5110  else
5111  {
5112  cloudViewer_->updateCameraFrustums(pose, data.stereoCameraModels());
5113  }
5114  }
5115 
5116  //words
5117  if(ui_->checkBox_showWords->isChecked() &&
5118  !signatures.empty() &&
5119  !(*signatures.begin())->getWords3().empty())
5120  {
5121  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
5122  cloud->resize((*signatures.begin())->getWords3().size());
5123  int i=0;
5124  for(std::multimap<int, int>::const_iterator iter=(*signatures.begin())->getWords().begin();
5125  iter!=(*signatures.begin())->getWords().end();
5126  ++iter)
5127  {
5128  const cv::Point3f & pt = (*signatures.begin())->getWords3()[iter->second];
5129  cloud->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
5130  }
5131 
5132  if(cloud->size())
5133  {
5134  cloud = rtabmap::util3d::removeNaNFromPointCloud(cloud);
5135  }
5136 
5137  if(cloud->size())
5138  {
5139  cloudViewer_->addCloud("words", cloud, pose, Qt::red);
5140  }
5141  }
5142 
5143  //add occupancy grid
5144  if(ui_->checkBox_showMap->isChecked() || ui_->checkBox_showGrid->isChecked())
5145  {
5146  std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > localMaps;
5147  std::map<int, std::pair<float, cv::Point3f> > localMapsInfo;
5148  if(generatedLocalMaps_.find(data.id()) != generatedLocalMaps_.end())
5149  {
5150  localMaps.insert(*generatedLocalMaps_.find(data.id()));
5151  localMapsInfo.insert(*generatedLocalMapsInfo_.find(data.id()));
5152  }
5153  else if(!data.gridGroundCellsRaw().empty() || !data.gridObstacleCellsRaw().empty())
5154  {
5155  localMaps.insert(std::make_pair(data.id(), std::make_pair(std::make_pair(data.gridGroundCellsRaw(), data.gridObstacleCellsRaw()), data.gridEmptyCellsRaw())));
5156  localMapsInfo.insert(std::make_pair(data.id(), std::make_pair(data.gridCellSize(), data.gridViewPoint())));
5157  }
5158  if(!localMaps.empty())
5159  {
5160  std::map<int, Transform> poses;
5161  poses.insert(std::make_pair(data.id(), pose));
5162 
5163 #ifdef RTABMAP_OCTOMAP
5164  OctoMap * octomap = 0;
5165  if(ui_->checkBox_octomap->isChecked() &&
5166  (!localMaps.begin()->second.first.first.empty() || !localMaps.begin()->second.first.second.empty()) &&
5167  (localMaps.begin()->second.first.first.empty() || localMaps.begin()->second.first.first.channels() > 2) &&
5168  (localMaps.begin()->second.first.second.empty() || localMaps.begin()->second.first.second.channels() > 2) &&
5169  (localMaps.begin()->second.second.empty() || localMaps.begin()->second.second.channels() > 2) &&
5170  localMapsInfo.begin()->second.first > 0.0f)
5171  {
5172  //create local octomap
5173  ParametersMap params;
5174  params.insert(ParametersPair(Parameters::kGridCellSize(), uNumber2Str(localMapsInfo.begin()->second.first)));
5175  octomap = new OctoMap(params);
5176  octomap->addToCache(data.id(), localMaps.begin()->second.first.first, localMaps.begin()->second.first.second, localMaps.begin()->second.second, localMapsInfo.begin()->second.second);
5177  octomap->update(poses);
5178  }
5179 #endif
5180 
5181  if(ui_->checkBox_showMap->isChecked())
5182  {
5183  float xMin=0.0f, yMin=0.0f;
5184  cv::Mat map8S;
5185  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
5186  float gridCellSize = Parameters::defaultGridCellSize();
5187  Parameters::parse(parameters, Parameters::kGridCellSize(), gridCellSize);
5188  parameters = Parameters::filterParameters(parameters, "GridGlobal", true);
5189 #ifdef RTABMAP_OCTOMAP
5190  if(octomap)
5191  {
5192  map8S = octomap->createProjectionMap(xMin, yMin, gridCellSize, 0);
5193  }
5194  else
5195 #endif
5196  {
5197  OccupancyGrid grid(parameters);
5198  grid.setCellSize(gridCellSize);
5199  grid.addToCache(data.id(), localMaps.begin()->second.first.first, localMaps.begin()->second.first.second, localMaps.begin()->second.second);
5200  grid.update(poses);
5201  map8S = grid.getMap(xMin, yMin);
5202  }
5203  if(!map8S.empty())
5204  {
5205  //convert to gray scaled map
5206  cloudViewer_->addOccupancyGridMap(util3d::convertMap2Image8U(map8S), gridCellSize, xMin, yMin, 1);
5207  }
5208  }
5209 
5210  if(ui_->checkBox_showGrid->isChecked())
5211  {
5212 #ifdef RTABMAP_OCTOMAP
5213  if(octomap)
5214  {
5215  if(ui_->comboBox_octomap_rendering_type->currentIndex()== 0)
5216  {
5217  pcl::IndicesPtr obstacles(new std::vector<int>);
5218  pcl::IndicesPtr empty(new std::vector<int>);
5219  pcl::IndicesPtr ground(new std::vector<int>);
5220  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap->createCloud(ui_->spinBox_grid_depth->value(), obstacles.get(), empty.get(), ground.get());
5221  if(octomap->hasColor())
5222  {
5223  pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
5224  pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
5225  cloudViewer_->addCloud("obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
5226  cloudViewer_->setCloudPointSize("obstacles", 5);
5227 
5228  pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
5229  pcl::copyPointCloud(*cloud, *ground, *groundCloud);
5230  cloudViewer_->addCloud("ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
5231  cloudViewer_->setCloudPointSize("ground", 5);
5232  }
5233  else
5234  {
5235  pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
5236  pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
5237  cloudViewer_->addCloud("obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
5238  cloudViewer_->setCloudPointSize("obstacles", 5);
5239 
5240  pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
5241  pcl::copyPointCloud(*cloud, *ground, *groundCloud);
5242  cloudViewer_->addCloud("ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
5243  cloudViewer_->setCloudPointSize("ground", 5);
5244  }
5245 
5246  if(ui_->checkBox_grid_empty->isChecked())
5247  {
5248  pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZ>);
5249  pcl::copyPointCloud(*cloud, *empty, *emptyCloud);
5250  cloudViewer_->addCloud("empty_cells", emptyCloud, Transform::getIdentity(), QColor(ui_->lineEdit_emptyColor->text()));
5251  cloudViewer_->setCloudOpacity("empty_cells", 0.5);
5252  cloudViewer_->setCloudPointSize("empty_cells", 5);
5253  }
5254  }
5255  else
5256  {
5257  cloudViewer_->addOctomap(octomap, ui_->spinBox_grid_depth->value(), ui_->comboBox_octomap_rendering_type->currentIndex()>1);
5258  }
5259  }
5260  else
5261 #endif
5262  {
5263  // occupancy cloud
5264  LaserScan scan = LaserScan::backwardCompatibility(localMaps.begin()->second.first.first);
5265  if(scan.hasRGB())
5266  {
5267  cloudViewer_->addCloud("ground", util3d::laserScanToPointCloudRGB(scan), pose, QColor(ui_->lineEdit_groundColor->text()));
5268  }
5269  else
5270  {
5271  cloudViewer_->addCloud("ground", util3d::laserScanToPointCloud(scan), pose, QColor(ui_->lineEdit_groundColor->text()));
5272  }
5273  scan = LaserScan::backwardCompatibility(localMaps.begin()->second.first.second);
5274  if(scan.hasRGB())
5275  {
5276  cloudViewer_->addCloud("obstacles", util3d::laserScanToPointCloudRGB(scan), pose, QColor(ui_->lineEdit_obstacleColor->text()));
5277  }
5278  else
5279  {
5280  cloudViewer_->addCloud("obstacles", util3d::laserScanToPointCloud(scan), pose, QColor(ui_->lineEdit_obstacleColor->text()));
5281  }
5282 
5283  cloudViewer_->setCloudPointSize("ground", 5);
5284  cloudViewer_->setCloudPointSize("obstacles", 5);
5285 
5286  if(ui_->checkBox_grid_empty->isChecked())
5287  {
5288  cloudViewer_->addCloud("empty_cells",
5289  util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(localMaps.begin()->second.second)),
5290  pose,
5291  QColor(ui_->lineEdit_emptyColor->text()));
5292  cloudViewer_->setCloudPointSize("empty_cells", 5);
5293  cloudViewer_->setCloudOpacity("empty_cells", 0.5);
5294  }
5295  }
5296  }
5297 #ifdef RTABMAP_OCTOMAP
5298  if(octomap)
5299  {
5300  delete octomap;
5301  }
5302 #endif
5303  }
5304  }
5305  cloudViewer_->updateCameraTargetPosition(pose);
5306  cloudViewer_->clearTrajectory();
5307  cloudViewer_->refreshView();
5308  }
5309 
5310  if(signatures.size())
5311  {
5312  UASSERT(signatures.front() != 0 && signatures.size() == 1);
5313  delete signatures.front();
5314  signatures.clear();
5315  }
5316  }
5317 
5318  // loops
5319  std::multimap<int, rtabmap::Link> links;
5320  dbDriver_->loadLinks(id, links);
5321  if(links.size())
5322  {
5323  QString strParents, strChildren;
5324  for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
5325  {
5326  if(iter->second.type() != Link::kNeighbor &&
5327  iter->second.type() != Link::kNeighborMerged)
5328  {
5329  if(iter->first < id)
5330  {
5331  strChildren.append(QString("%1 ").arg(iter->first));
5332  }
5333  else
5334  {
5335  strParents.append(QString("%1 ").arg(iter->first));
5336  }
5337  }
5338  }
5339  labelParents->setText(strParents);
5340  labelChildren->setText(strChildren);
5341  }
5342  }
5343 
5344  if(mapId>=0)
5345  {
5346  labelMapId->setText(QString::number(mapId));
5347  }
5348  }
5349  else if(value != 0)
5350  {
5351  ULOGGER_ERROR("Slider index out of range ?");
5352  }
5353 
5354  updateConstraintButtons();
5355  updateWordsMatching();
5356 
5357  if(updateConstraintView && ui_->dockWidget_constraints->isVisible())
5358  {
5359  // update constraint view
5360  int from = ids_.at(ui_->horizontalSlider_A->value());
5361  int to = ids_.at(ui_->horizontalSlider_B->value());
5362  bool set = false;
5363  for(int i=0; i<loopLinks_.size() || i<neighborLinks_.size(); ++i)
5364  {
5365  if(i < loopLinks_.size())
5366  {
5367  if((loopLinks_[i].from() == from && loopLinks_[i].to() == to) ||
5368  (loopLinks_[i].from() == to && loopLinks_[i].to() == from))
5369  {
5370  if(i != ui_->horizontalSlider_loops->value())
5371  {
5372  ui_->horizontalSlider_loops->blockSignals(true);
5373  ui_->horizontalSlider_loops->setValue(i);
5374  ui_->horizontalSlider_loops->blockSignals(false);
5375  this->updateConstraintView(loopLinks_[i].from() == from?loopLinks_.at(i):loopLinks_.at(i).inverse(), false);
5376  }
5377  ui_->horizontalSlider_neighbors->blockSignals(true);
5378  ui_->horizontalSlider_neighbors->setValue(0);
5379  ui_->horizontalSlider_neighbors->blockSignals(false);
5380  set = true;
5381  break;
5382  }
5383  }
5384  if(i < neighborLinks_.size())
5385  {
5386  if((neighborLinks_[i].from() == from && neighborLinks_[i].to() == to) ||
5387  (neighborLinks_[i].from() == to && neighborLinks_[i].to() == from))
5388  {
5389  if(i != ui_->horizontalSlider_neighbors->value())
5390  {
5391  ui_->horizontalSlider_neighbors->blockSignals(true);
5392  ui_->horizontalSlider_neighbors->setValue(i);
5393  ui_->horizontalSlider_neighbors->blockSignals(false);
5394  this->updateConstraintView(neighborLinks_[i].from() == from?neighborLinks_.at(i):neighborLinks_.at(i).inverse(), false);
5395  }
5396  ui_->horizontalSlider_loops->blockSignals(true);
5397  ui_->horizontalSlider_loops->setValue(0);
5398  ui_->horizontalSlider_loops->blockSignals(false);
5399  set = true;
5400  break;
5401  }
5402  }
5403  }
5404  if(!set)
5405  {
5406  ui_->horizontalSlider_loops->blockSignals(true);
5407  ui_->horizontalSlider_neighbors->blockSignals(true);
5408  ui_->horizontalSlider_loops->setValue(0);
5409  ui_->horizontalSlider_neighbors->setValue(0);
5410  ui_->horizontalSlider_loops->blockSignals(false);
5411  ui_->horizontalSlider_neighbors->blockSignals(false);
5412 
5413  constraintsViewer_->removeAllClouds();
5414 
5415  Link link = this->findActiveLink(from, to);
5416  bool constraintViewUpdated = false;
5417  if(link.isValid() && link.type() != Link::kGravity)
5418  {
5419  this->updateConstraintView(link, false);
5420  constraintViewUpdated = true;
5421  }
5422  else if(graphes_.size())
5423  {
5424  // make a fake link using globally optimized poses
5425  std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
5426  if(optimizedPoses.size() > 0)
5427  {
5428  std::map<int, Transform>::iterator fromIter = optimizedPoses.find(from);
5429  std::map<int, Transform>::iterator toIter = optimizedPoses.find(to);
5430  if(fromIter != optimizedPoses.end() &&
5431  toIter != optimizedPoses.end())
5432  {
5433  Link link(from, to, Link::kUndef, fromIter->second.inverse() * toIter->second);
5434  this->updateConstraintView(link, false);
5435  constraintViewUpdated = true;
5436  }
5437  }
5438  }
5439  if(!constraintViewUpdated)
5440  {
5441  ui_->label_constraint->clear();
5442  ui_->label_constraint_opt->clear();
5443  ui_->label_variance->clear();
5444  ui_->lineEdit_covariance->clear();
5445  ui_->label_type->clear();
5446  ui_->label_type_name->clear();
5447  ui_->checkBox_showOptimized->setEnabled(false);
5448  }
5449  constraintsViewer_->refreshView();
5450 
5451  }
5452  }
5453 }
5454 
5455 void DatabaseViewer::updateLoggerLevel()
5456 {
5457  if(this->parent() == 0)
5458  {
5459  ULogger::setLevel((ULogger::Level)ui_->comboBox_logger_level->currentIndex());
5460  }
5461 }
5462 
5463 void DatabaseViewer::updateStereo()
5464 {
5465  if(ui_->horizontalSlider_A->maximum())
5466  {
5467  int id = ids_.at(ui_->horizontalSlider_A->value());
5468  SensorData data;
5469  dbDriver_->getNodeData(id, data);
5470  data.uncompressData();
5471  updateStereo(&data);
5472  }
5473 }
5474 
5475 void DatabaseViewer::updateStereo(const SensorData * data)
5476 {
5477  if(data &&
5478  ui_->dockWidget_stereoView->isVisible() &&
5479  !data->imageRaw().empty() &&
5480  !data->depthOrRightRaw().empty() &&
5481  data->depthOrRightRaw().type() == CV_8UC1 &&
5482  data->stereoCameraModels().size()==1 && // Not implemented for multiple stereo cameras
5483  data->stereoCameraModels()[0].isValidForProjection())
5484  {
5485  cv::Mat leftMono;
5486  if(data->imageRaw().channels() == 3)
5487  {
5488  cv::cvtColor(data->imageRaw(), leftMono, CV_BGR2GRAY);
5489  }
5490  else
5491  {
5492  leftMono = data->imageRaw();
5493  }
5494 
5495  UTimer timer;
5496  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
5497  Stereo * stereo = Stereo::create(parameters);
5498 
5499  // generate kpts
5500  std::vector<cv::KeyPoint> kpts;
5501  uInsert(parameters, ParametersPair(Parameters::kKpMaxFeatures(), parameters.at(Parameters::kVisMaxFeatures())));
5502  uInsert(parameters, ParametersPair(Parameters::kKpMinDepth(), parameters.at(Parameters::kVisMinDepth())));
5503  uInsert(parameters, ParametersPair(Parameters::kKpMaxDepth(), parameters.at(Parameters::kVisMaxDepth())));
5504  uInsert(parameters, ParametersPair(Parameters::kKpDetectorStrategy(), parameters.at(Parameters::kVisFeatureType())));
5505  uInsert(parameters, ParametersPair(Parameters::kKpRoiRatios(), parameters.at(Parameters::kVisRoiRatios())));
5506  uInsert(parameters, ParametersPair(Parameters::kKpSubPixEps(), parameters.at(Parameters::kVisSubPixEps())));
5507  uInsert(parameters, ParametersPair(Parameters::kKpSubPixIterations(), parameters.at(Parameters::kVisSubPixIterations())));
5508  uInsert(parameters, ParametersPair(Parameters::kKpSubPixWinSize(), parameters.at(Parameters::kVisSubPixWinSize())));
5509  Feature2D * kptDetector = Feature2D::create(parameters);
5510  kpts = kptDetector->generateKeypoints(leftMono);
5511  delete kptDetector;
5512 
5513  float timeKpt = timer.ticks();
5514 
5515  std::vector<cv::Point2f> leftCorners;
5516  cv::KeyPoint::convert(kpts, leftCorners);
5517 
5518  // Find features in the new right image
5519  std::vector<unsigned char> status;
5520  std::vector<cv::Point2f> rightCorners;
5521 
5522  rightCorners = stereo->computeCorrespondences(
5523  leftMono,
5524  data->rightRaw(),
5525  leftCorners,
5526  status);
5527  delete stereo;
5528 
5529  float timeStereo = timer.ticks();
5530 
5531  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
5532  cloud->resize(kpts.size());
5533  float bad_point = std::numeric_limits<float>::quiet_NaN ();
5534  UASSERT(status.size() == kpts.size());
5535  int oi = 0;
5536  int inliers = 0;
5537  int flowOutliers= 0;
5538  int slopeOutliers= 0;
5539  int negativeDisparityOutliers = 0;
5540  for(unsigned int i=0; i<status.size(); ++i)
5541  {
5542  cv::Point3f pt(bad_point, bad_point, bad_point);
5543  if(status[i])
5544  {
5545  float disparity = leftCorners[i].x - rightCorners[i].x;
5546  if(disparity > 0.0f)
5547  {
5548  cv::Point3f tmpPt = util3d::projectDisparityTo3D(
5549  leftCorners[i],
5550  disparity,
5551  data->stereoCameraModels()[0]);
5552 
5553  if(util3d::isFinite(tmpPt))
5554  {
5555  pt = util3d::transformPoint(tmpPt, data->stereoCameraModels()[0].left().localTransform());
5556  status[i] = 100; //blue
5557  ++inliers;
5558  cloud->at(oi++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
5559  }
5560  }
5561  else
5562  {
5563  status[i] = 102; //magenta
5564  ++negativeDisparityOutliers;
5565  }
5566  }
5567  else
5568  {
5569  ++flowOutliers;
5570  }
5571  }
5572  cloud->resize(oi);
5573 
5574  UINFO("correspondences = %d/%d (%f) (time kpt=%fs stereo=%fs)",
5575  (int)cloud->size(), (int)leftCorners.size(), float(cloud->size())/float(leftCorners.size()), timeKpt, timeStereo);
5576 
5577  stereoViewer_->updateCameraTargetPosition(Transform::getIdentity());
5578  stereoViewer_->addCloud("stereo", cloud);
5579  stereoViewer_->refreshView();
5580 
5581  ui_->label_stereo_inliers->setNum(inliers);
5582  ui_->label_stereo_flowOutliers->setNum(flowOutliers);
5583  ui_->label_stereo_slopeOutliers->setNum(slopeOutliers);
5584  ui_->label_stereo_disparityOutliers->setNum(negativeDisparityOutliers);
5585 
5586  std::vector<cv::KeyPoint> rightKpts;
5587  cv::KeyPoint::convert(rightCorners, rightKpts);
5588  std::vector<cv::DMatch> good_matches(kpts.size());
5589  for(unsigned int i=0; i<good_matches.size(); ++i)
5590  {
5591  good_matches[i].trainIdx = i;
5592  good_matches[i].queryIdx = i;
5593  }
5594 
5595 
5596  //
5597  //cv::Mat imageMatches;
5598  //cv::drawMatches( leftMono, kpts, data->getDepthRaw(), rightKpts,
5599  // good_matches, imageMatches, cv::Scalar::all(-1), cv::Scalar::all(-1),
5600  // std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
5601 
5602  //ui_->graphicsView_stereo->setImage(uCvMat2QImage(imageMatches));
5603 
5604  ui_->graphicsView_stereo->clear();
5605  ui_->graphicsView_stereo->setLinesShown(true);
5606  ui_->graphicsView_stereo->setFeaturesShown(false);
5607  ui_->graphicsView_stereo->setImageDepthShown(true);
5608 
5609  ui_->graphicsView_stereo->setImage(uCvMat2QImage(data->imageRaw()));
5610  ui_->graphicsView_stereo->setImageDepth(data->depthOrRightRaw());
5611 
5612  // Draw lines between corresponding features...
5613  for(unsigned int i=0; i<kpts.size(); ++i)
5614  {
5615  if(rightKpts[i].pt.x > 0 && rightKpts[i].pt.y > 0)
5616  {
5617  QColor c = Qt::green;
5618  if(status[i] == 0)
5619  {
5620  c = Qt::red;
5621  }
5622  else if(status[i] == 100)
5623  {
5624  c = Qt::blue;
5625  }
5626  else if(status[i] == 101)
5627  {
5628  c = Qt::yellow;
5629  }
5630  else if(status[i] == 102)
5631  {
5632  c = Qt::magenta;
5633  }
5634  else if(status[i] == 110)
5635  {
5636  c = Qt::cyan;
5637  }
5638  ui_->graphicsView_stereo->addLine(
5639  kpts[i].pt.x,
5640  kpts[i].pt.y,
5641  rightKpts[i].pt.x,
5642  rightKpts[i].pt.y,
5643  c,
5644  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));
5645  }
5646  }
5647  ui_->graphicsView_stereo->update();
5648  }
5649 }
5650 
5651 void DatabaseViewer::updateWordsMatching(const std::vector<int> & inliers)
5652 {
5653  int from = ids_.at(ui_->horizontalSlider_A->value());
5654  int to = ids_.at(ui_->horizontalSlider_B->value());
5655  if(from && to)
5656  {
5657  ui_->graphicsView_A->clearLines();
5658  ui_->graphicsView_A->setFeaturesColor(ui_->graphicsView_A->getDefaultFeatureColor());
5659  ui_->graphicsView_B->clearLines();
5660  ui_->graphicsView_B->setFeaturesColor(ui_->graphicsView_B->getDefaultFeatureColor());
5661 
5662  const QMultiMap<int, KeypointItem*> & wordsA = ui_->graphicsView_A->getFeatures();
5663  const QMultiMap<int, KeypointItem*> & wordsB = ui_->graphicsView_B->getFeatures();
5664  std::set<int> inliersSet(inliers.begin(), inliers.end());
5665  if(wordsA.size() && wordsB.size())
5666  {
5667  QList<int> ids = wordsA.uniqueKeys();
5668  for(int i=0; i<ids.size(); ++i)
5669  {
5670  if(ids[i] > 0 && wordsA.count(ids[i]) == 1 && wordsB.count(ids[i]) == 1)
5671  {
5672  // Add lines
5673  // Draw lines between corresponding features...
5674  float scaleAX = ui_->graphicsView_A->viewScale();
5675  float scaleBX = ui_->graphicsView_B->viewScale();
5676 
5677  float marginAX = (ui_->graphicsView_A->width() - ui_->graphicsView_A->sceneRect().width()*scaleAX)/2.0f;
5678  float marginAY = (ui_->graphicsView_A->height() - ui_->graphicsView_A->sceneRect().height()*scaleAX)/2.0f;
5679  float marginBX = (ui_->graphicsView_B->width() - ui_->graphicsView_B->sceneRect().width()*scaleBX)/2.0f;
5680  float marginBY = (ui_->graphicsView_B->height() - ui_->graphicsView_B->sceneRect().height()*scaleBX)/2.0f;
5681 
5682  float deltaX = 0;
5683  float deltaY = 0;
5684 
5685  if(ui_->actionVertical_Layout->isChecked())
5686  {
5687  deltaY = ui_->graphicsView_A->height();
5688  }
5689  else
5690  {
5691  deltaX = ui_->graphicsView_A->width();
5692  }
5693 
5694  const KeypointItem * kptA = wordsA.value(ids[i]);
5695  const KeypointItem * kptB = wordsB.value(ids[i]);
5696 
5697  QColor cA = ui_->graphicsView_A->getDefaultMatchingLineColor();
5698  QColor cB = ui_->graphicsView_B->getDefaultMatchingLineColor();
5699  if(inliersSet.find(ids[i])!=inliersSet.end())
5700  {
5701  cA = ui_->graphicsView_A->getDefaultMatchingFeatureColor();
5702  cB = ui_->graphicsView_B->getDefaultMatchingFeatureColor();
5703  ui_->graphicsView_A->setFeatureColor(ids[i], ui_->graphicsView_A->getDefaultMatchingFeatureColor());
5704  ui_->graphicsView_B->setFeatureColor(ids[i], ui_->graphicsView_B->getDefaultMatchingFeatureColor());
5705  }
5706  else
5707  {
5708  ui_->graphicsView_A->setFeatureColor(ids[i], ui_->graphicsView_A->getDefaultMatchingLineColor());
5709  ui_->graphicsView_B->setFeatureColor(ids[i], ui_->graphicsView_B->getDefaultMatchingLineColor());
5710  }
5711 
5712  ui_->graphicsView_A->addLine(
5713  kptA->keypoint().pt.x,
5714  kptA->keypoint().pt.y,
5715  (kptB->keypoint().pt.x*scaleBX+marginBX+deltaX-marginAX)/scaleAX,
5716  (kptB->keypoint().pt.y*scaleBX+marginBY+deltaY-marginAY)/scaleAX,
5717  cA);
5718 
5719  ui_->graphicsView_B->addLine(
5720  (kptA->keypoint().pt.x*scaleAX+marginAX-deltaX-marginBX)/scaleBX,
5721  (kptA->keypoint().pt.y*scaleAX+marginAY-deltaY-marginBY)/scaleBX,
5722  kptB->keypoint().pt.x,
5723  kptB->keypoint().pt.y,
5724  cB);
5725  }
5726  }
5727  ui_->graphicsView_A->update();
5728  ui_->graphicsView_B->update();
5729  }
5730  }
5731 }
5732 
5733 void DatabaseViewer::sliderAMoved(int value)
5734 {
5735  ui_->label_indexA->setText(QString::number(value));
5736  if(value>=0 && value < ids_.size())
5737  {
5738  ui_->label_idA->setText(QString::number(ids_.at(value)));
5739  }
5740  else
5741  {
5742  ULOGGER_ERROR("Slider index out of range ?");
5743  }
5744 }
5745 
5746 void DatabaseViewer::sliderBMoved(int value)
5747 {
5748  ui_->label_indexB->setText(QString::number(value));
5749  if(value>=0 && value < ids_.size())
5750  {
5751  ui_->label_idB->setText(QString::number(ids_.at(value)));
5752  }
5753  else
5754  {
5755  ULOGGER_ERROR("Slider index out of range ?");
5756  }
5757 }
5758 
5759 void DatabaseViewer::update3dView()
5760 {
5761  if(lastSliderIndexBrowsed_ == ui_->horizontalSlider_B->value())
5762  {
5763  sliderBValueChanged(ui_->horizontalSlider_B->value());
5764  }
5765  else
5766  {
5767  sliderAValueChanged(ui_->horizontalSlider_A->value());
5768  }
5769 }
5770 
5771 void DatabaseViewer::sliderNeighborValueChanged(int value)
5772 {
5773  if(value < neighborLinks_.size())
5774  {
5775  this->updateConstraintView(neighborLinks_.at(value));
5776  }
5777 }
5778 
5779 void DatabaseViewer::sliderLoopValueChanged(int value)
5780 {
5781  if(value < loopLinks_.size())
5782  {
5783  this->updateConstraintView(loopLinks_.at(value));
5784  }
5785 }
5786 
5787 void DatabaseViewer::editConstraint()
5788 {
5789  if(ids_.size())
5790  {
5791  Link link;
5792  if(ui_->label_type->text().toInt() == Link::kLandmark)
5793  {
5794  int position = ui_->horizontalSlider_loops->value();
5795  link = loopLinks_.at(position);
5796  }
5797  else
5798  {
5799  link = this->findActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value()));
5800  }
5801  if(link.isValid())
5802  {
5803  cv::Mat covBefore = link.infMatrix().inv();
5804  EditConstraintDialog dialog(link.transform(),
5805  covBefore.at<double>(0,0)<9999.0?std::sqrt(covBefore.at<double>(0,0)):0.0,
5806  covBefore.at<double>(5,5)<9999.0?std::sqrt(covBefore.at<double>(5,5)):0.0);
5807  if(dialog.exec() == QDialog::Accepted)
5808  {
5809  bool updated = false;
5810  cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1);
5811  if(dialog.getLinearVariance()>0)
5812  {
5813  covariance(cv::Range(0,3), cv::Range(0,3)) *= dialog.getLinearVariance();
5814  }
5815  else
5816  {
5817  covariance(cv::Range(0,3), cv::Range(0,3)) *= 9999.9;
5818  }
5819  if(dialog.getAngularVariance()>0)
5820  {
5821  covariance(cv::Range(3,6), cv::Range(3,6)) *= dialog.getAngularVariance();
5822  }
5823  else
5824  {
5825  covariance(cv::Range(3,6), cv::Range(3,6)) *= 9999.9;
5826  }
5827  Link newLink(link.from(), link.to(), link.type(), dialog.getTransform(), covariance.inv());
5828  std::multimap<int, Link>::iterator iter = linksRefined_.find(link.from());
5829  while(iter != linksRefined_.end() && iter->first == link.from())
5830  {
5831  if(iter->second.to() == link.to() &&
5832  iter->second.type() == link.type())
5833  {
5834  iter->second = newLink;
5835  updated = true;
5836  break;
5837  }
5838  ++iter;
5839  }
5840  if(!updated)
5841  {
5842  linksRefined_.insert(std::make_pair(newLink.from(), newLink));
5843  updated = true;
5844  }
5845  if(updated)
5846  {
5847  this->updateGraphView();
5848  updateConstraintView();
5849  }
5850  }
5851  }
5852  else
5853  {
5854  EditConstraintDialog dialog(Transform::getIdentity());
5855  if(dialog.exec() == QDialog::Accepted)
5856  {
5857  cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1);
5858  if(dialog.getLinearVariance()>0)
5859  {
5860  covariance(cv::Range(0,3), cv::Range(0,3)) *= dialog.getLinearVariance();
5861  }
5862  else
5863  {
5864  covariance(cv::Range(0,3), cv::Range(0,3)) *= 9999.9;
5865  }
5866  if(dialog.getAngularVariance()>0)
5867  {
5868  covariance(cv::Range(3,6), cv::Range(3,6)) *= dialog.getAngularVariance();
5869  }
5870  else
5871  {
5872  covariance(cv::Range(3,6), cv::Range(3,6)) *= 9999.9;
5873  }
5874  int from = ids_.at(ui_->horizontalSlider_A->value());
5875  int to = ids_.at(ui_->horizontalSlider_B->value());
5876  Link newLink(
5877  from,
5878  to,
5879  Link::kUserClosure,
5880  dialog.getTransform(),
5881  covariance.inv());
5882  if(newLink.from() < newLink.to())
5883  {
5884  newLink = newLink.inverse();
5885  }
5886  linksAdded_.insert(std::make_pair(newLink.from(), newLink));
5887  this->updateGraphView();
5888  updateLoopClosuresSlider(from, to);
5889  }
5890  }
5891  }
5892 }
5893 
5894 // only called when ui_->checkBox_showOptimized state changed
5895 void DatabaseViewer::updateConstraintView()
5896 {
5897  if(ids_.size())
5898  {
5899  Link link = this->findActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value()));
5900  if(link.isValid())
5901  {
5902  UDEBUG("link %d->%d type=%d", link.from(), link.to(), link.type());
5903  if((link.type() == Link::kNeighbor ||
5904  link.type() == Link::kNeighborMerged) &&
5905  ui_->horizontalSlider_neighbors->value() >= 0 && ui_->horizontalSlider_neighbors->value() < neighborLinks_.size())
5906  {
5907  this->updateConstraintView(neighborLinks_.at(ui_->horizontalSlider_neighbors->value()), false);
5908  }
5909  else if((link.type() != Link::kPosePrior || link.type() != Link::kGravity) &&
5910  ui_->horizontalSlider_loops->value() >= 0 && ui_->horizontalSlider_loops->value() < loopLinks_.size())
5911  {
5912  this->updateConstraintView(loopLinks_.at(ui_->horizontalSlider_loops->value()), false);
5913  }
5914  else
5915  {
5916  this->updateConstraintView(link, false);
5917  }
5918  }
5919  }
5920 }
5921 
5922 void DatabaseViewer::updateConstraintView(
5923  const rtabmap::Link & linkIn,
5924  bool updateImageSliders,
5925  const Signature & signatureFrom,
5926  const Signature & signatureTo)
5927 {
5928  UDEBUG("%d -> %d", linkIn.from(), linkIn.to());
5929  std::multimap<int, Link>::iterator iterLink = rtabmap::graph::findLink(linksRefined_, linkIn.from(), linkIn.to());
5930  rtabmap::Link link = linkIn;
5931 
5932  if(iterLink != linksRefined_.end())
5933  {
5934  if(iterLink->second.from() == link.to())
5935  {
5936  link = iterLink->second.inverse();
5937  }
5938  else
5939  {
5940  link = iterLink->second;
5941  }
5942  }
5943  else if(ui_->checkBox_ignorePoseCorrection->isChecked())
5944  {
5945  if(link.type() == Link::kNeighbor ||
5946  link.type() == Link::kNeighborMerged)
5947  {
5948  Transform poseFrom = uValue(odomPoses_, link.from(), Transform());
5949  Transform poseTo = uValue(odomPoses_, link.to(), Transform());
5950  if(!poseFrom.isNull() && !poseTo.isNull())
5951  {
5952  // recompute raw odom transformation and
5953  // reset to identity covariance
5954  link = Link(link.from(),
5955  link.to(),
5956  link.type(),
5957  poseFrom.inverse() * poseTo);
5958  }
5959  }
5960  }
5961  UDEBUG("%d -> %d", link.from(), link.to());
5962  rtabmap::Transform t = link.transform();
5963  if(link.type() == Link::kGravity)
5964  {
5965  // remove yaw, keep only roll and pitch
5966  float roll, pitch, yaw;
5967  t.getEulerAngles(roll, pitch, yaw);
5968  t = Transform(0,0,0,roll,pitch,0);
5969  }
5970 
5971  ui_->label_constraint->clear();
5972  ui_->label_constraint_opt->clear();
5973  ui_->checkBox_showOptimized->setEnabled(false);
5974  UASSERT(!t.isNull() && dbDriver_);
5975 
5976  ui_->label_type->setText(QString::number(link.type()));
5977  ui_->label_type_name->setText(tr("(%1)")
5978  .arg(link.type()==Link::kNeighbor?"Neighbor":
5979  link.type()==Link::kNeighborMerged?"Merged neighbor":
5980  link.type()==Link::kGlobalClosure?"Loop closure":
5981  link.type()==Link::kLocalSpaceClosure?"Space proximity link":
5982  link.type()==Link::kLocalTimeClosure?"Time proximity link":
5983  link.type()==Link::kUserClosure?"User link":
5984  link.type()==Link::kLandmark?"Landmark "+QString::number(-link.to()):
5985  link.type()==Link::kVirtualClosure?"Virtual link":
5986  link.type()==Link::kGravity?"Gravity link":"Undefined"));
5987  ui_->label_variance->setText(QString("%1, %2")
5988  .arg(sqrt(link.transVariance()))
5989  .arg(sqrt(link.rotVariance())));
5990  std::stringstream out;
5991  out << link.infMatrix().inv();
5992  ui_->lineEdit_covariance->setText(out.str().c_str());
5993  ui_->label_constraint->setText(QString("%1").arg(t.prettyPrint().c_str()).replace(" ", "\n"));
5994  if(graphes_.size() &&
5995  (int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
5996  {
5997  std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
5998 
5999  std::map<int, rtabmap::Transform>::iterator iterFrom = graph.find(link.from());
6000  std::map<int, rtabmap::Transform>::iterator iterTo = graph.find(link.to());
6001  if(iterFrom != graph.end() && iterTo != graph.end())
6002  {
6003  ui_->checkBox_showOptimized->setEnabled(true);
6004  Transform topt = iterFrom->second.inverse()*iterTo->second;
6005  float diff = topt.getDistance(t);
6006  Transform v1 = t.rotation()*Transform(1,0,0,0,0,0);
6007  Transform v2 = topt.rotation()*Transform(1,0,0,0,0,0);
6008  float a = pcl::getAngle3D(Eigen::Vector4f(v1.x(), v1.y(), v1.z(), 0), Eigen::Vector4f(v2.x(), v2.y(), v2.z(), 0));
6009  a = (a *180.0f) / CV_PI;
6010  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));
6011 
6012  if(ui_->checkBox_showOptimized->isChecked())
6013  {
6014  t = topt;
6015  }
6016  }
6017  }
6018 
6019  if(updateImageSliders)
6020  {
6021  ui_->horizontalSlider_A->blockSignals(true);
6022  ui_->horizontalSlider_B->blockSignals(true);
6023  // set from on left and to on right
6024  if(link.from()>0)
6025  ui_->horizontalSlider_A->setValue(idToIndex_.value(link.from()));
6026  if(link.to() > 0)
6027  ui_->horizontalSlider_B->setValue(idToIndex_.value(link.to()));
6028  ui_->horizontalSlider_A->blockSignals(false);
6029  ui_->horizontalSlider_B->blockSignals(false);
6030  if(link.from()>0)
6031  this->update(idToIndex_.value(link.from()),
6032  ui_->label_indexA,
6033  ui_->label_parentsA,
6034  ui_->label_childrenA,
6035  ui_->label_weightA,
6036  ui_->label_labelA,
6037  ui_->label_stampA,
6038  ui_->graphicsView_A,
6039  ui_->label_idA,
6040  ui_->label_mapA,
6041  ui_->label_poseA,
6042  ui_->label_optposeA,
6043  ui_->label_velA,
6044  ui_->label_calibA,
6045  ui_->label_scanA,
6046  ui_->label_gravityA,
6047  ui_->label_priorA,
6048  ui_->label_gpsA,
6049  ui_->label_gtA,
6050  ui_->label_sensorsA,
6051  false); // don't update constraints view!
6052  if(link.to()>0)
6053  {
6054  this->update(idToIndex_.value(link.to()),
6055  ui_->label_indexB,
6056  ui_->label_parentsB,
6057  ui_->label_childrenB,
6058  ui_->label_weightB,
6059  ui_->label_labelB,
6060  ui_->label_stampB,
6061  ui_->graphicsView_B,
6062  ui_->label_idB,
6063  ui_->label_mapB,
6064  ui_->label_poseB,
6065  ui_->label_optposeB,
6066  ui_->label_velB,
6067  ui_->label_calibB,
6068  ui_->label_scanB,
6069  ui_->label_gravityB,
6070  ui_->label_priorB,
6071  ui_->label_gpsB,
6072  ui_->label_gtB,
6073  ui_->label_sensorsB,
6074  false); // don't update constraints view!
6075  }
6076  }
6077 
6078  if(constraintsViewer_->isVisible())
6079  {
6080  SensorData dataFrom, dataTo;
6081 
6082  if(signatureFrom.id()>0)
6083  {
6084  dataFrom = signatureFrom.sensorData();
6085  }
6086  else
6087  {
6088  dbDriver_->getNodeData(link.from(), dataFrom);
6089  }
6090  dataFrom.uncompressData();
6091  UASSERT(dataFrom.imageRaw().empty() || dataFrom.imageRaw().type()==CV_8UC3 || dataFrom.imageRaw().type() == CV_8UC1);
6092  UASSERT(dataFrom.depthOrRightRaw().empty() || dataFrom.depthOrRightRaw().type()==CV_8UC1 || dataFrom.depthOrRightRaw().type() == CV_16UC1 || dataFrom.depthOrRightRaw().type() == CV_32FC1);
6093 
6094  if(signatureTo.id()>0)
6095  {
6096  dataTo = signatureTo.sensorData();
6097  }
6098  else if(link.to()>0)
6099  {
6100  dbDriver_->getNodeData(link.to(), dataTo);
6101  }
6102  dataTo.uncompressData();
6103  UASSERT(dataTo.imageRaw().empty() || dataTo.imageRaw().type()==CV_8UC3 || dataTo.imageRaw().type() == CV_8UC1);
6104  UASSERT(dataTo.depthOrRightRaw().empty() || dataTo.depthOrRightRaw().type()==CV_8UC1 || dataTo.depthOrRightRaw().type() == CV_16UC1 || dataTo.depthOrRightRaw().type() == CV_32FC1);
6105 
6106  // get odom pose
6107  Transform pose = Transform::getIdentity();
6108  if(ui_->checkBox_odomFrame->isChecked())
6109  {
6110  int m,w;
6111  std::string l;
6112  double s;
6113  Transform p,g;
6114  std::vector<float> v;
6115  GPS gps;
6116  EnvSensors sensors;
6117  dbDriver_->getNodeInfo(link.from(), p, m, w, l, s, g, v, gps, sensors);
6118  if(!p.isNull())
6119  {
6120  // keep just the z and roll/pitch rotation
6121  float x, y, z, roll, pitch, yaw;
6122  p.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
6123  pose = Transform(0,0,z,roll,pitch,0);
6124  }
6125  }
6126 
6127  constraintsViewer_->removeCloud("cloud0");
6128  constraintsViewer_->removeCloud("cloud1");
6129  //cloud 3d
6130  if(ui_->checkBox_show3Dclouds->isChecked())
6131  {
6132  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom, cloudTo;
6133  pcl::IndicesPtr indicesFrom(new std::vector<int>);
6134  pcl::IndicesPtr indicesTo(new std::vector<int>);
6135  if(!dataFrom.imageRaw().empty() && !dataFrom.depthOrRightRaw().empty())
6136  {
6137  cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, ui_->spinBox_decimation->value(), 0, 0, indicesFrom.get(), ui_->parameters_toolbox->getParameters());
6138  }
6139  if(!dataTo.imageRaw().empty() && !dataTo.depthOrRightRaw().empty())
6140  {
6141  cloudTo=util3d::cloudRGBFromSensorData(dataTo, ui_->spinBox_decimation->value(), 0, 0, indicesTo.get(), ui_->parameters_toolbox->getParameters());
6142  }
6143 
6144  if(cloudTo.get() && indicesTo->size())
6145  {
6146  cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t);
6147  }
6148 
6149  // Gain compensation
6150  if(ui_->doubleSpinBox_gainCompensationRadius->value()>0.0 &&
6151  cloudFrom.get() && indicesFrom->size() &&
6152  cloudTo.get() && indicesTo->size())
6153  {
6154  UTimer t;
6155  GainCompensator compensator(ui_->doubleSpinBox_gainCompensationRadius->value());
6156  compensator.feed(cloudFrom, indicesFrom, cloudTo, indicesTo, Transform::getIdentity());
6157  compensator.apply(0, cloudFrom, indicesFrom);
6158  compensator.apply(1, cloudTo, indicesTo);
6159  UINFO("Gain compensation time = %fs", t.ticks());
6160  }
6161 
6162  if(cloudFrom.get() && indicesFrom->size())
6163  {
6164  if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
6165  {
6166  cloudFrom = util3d::voxelize(cloudFrom, indicesFrom, ui_->doubleSpinBox_voxelSize->value());
6167  }
6168  constraintsViewer_->addCloud("cloud0", cloudFrom, pose, Qt::red);
6169  }
6170  if(cloudTo.get() && indicesTo->size())
6171  {
6172  if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
6173  {
6174  cloudTo = util3d::voxelize(cloudTo, indicesTo, ui_->doubleSpinBox_voxelSize->value());
6175  }
6176  constraintsViewer_->addCloud("cloud1", cloudTo, pose, Qt::cyan);
6177  }
6178  }
6179 
6180  constraintsViewer_->removeCloud("words0");
6181  constraintsViewer_->removeCloud("words1");
6182  if(ui_->checkBox_show3DWords->isChecked())
6183  {
6184  std::list<int> ids;
6185  ids.push_back(link.from());
6186  if(link.to()>0)
6187  {
6188  ids.push_back(link.to());
6189  }
6190  std::list<Signature*> signatures;
6191  dbDriver_->loadSignatures(ids, signatures);
6192  if(signatures.size() == 2 || (link.to()<0 && signatures.size()==1))
6193  {
6194  const Signature * sFrom = signatureFrom.id()>0?&signatureFrom:signatures.front();
6195  const Signature * sTo = 0;
6196  if(signatures.size()==2)
6197  {
6198  sTo = signatureTo.id()>0?&signatureTo:signatures.back();
6199  UASSERT(sTo);
6200  }
6201  UASSERT(sFrom);
6202  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom(new pcl::PointCloud<pcl::PointXYZ>);
6203  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo(new pcl::PointCloud<pcl::PointXYZ>);
6204  cloudFrom->resize(sFrom->getWords3().size());
6205  if(sTo)
6206  {
6207  cloudTo->resize(sTo->getWords3().size());
6208  }
6209  int i=0;
6210  if(!sFrom->getWords3().empty())
6211  {
6212  for(std::multimap<int, int>::const_iterator iter=sFrom->getWords().begin();
6213  iter!=sFrom->getWords().end();
6214  ++iter)
6215  {
6216  const cv::Point3f & pt = sFrom->getWords3()[iter->second];
6217  cloudFrom->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
6218  }
6219  }
6220  i=0;
6221  if(sTo && !sTo->getWords3().empty())
6222  {
6223  for(std::multimap<int, int>::const_iterator iter=sTo->getWords().begin();
6224  iter!=sTo->getWords().end();
6225  ++iter)
6226  {
6227  const cv::Point3f & pt = sTo->getWords3()[iter->second];
6228  cloudTo->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
6229  }
6230  }
6231 
6232  if(cloudFrom->size())
6233  {
6234  cloudFrom = rtabmap::util3d::removeNaNFromPointCloud(cloudFrom);
6235  }
6236  if(cloudTo->size())
6237  {
6238  cloudTo = rtabmap::util3d::removeNaNFromPointCloud(cloudTo);
6239  if(cloudTo->size())
6240  {
6241  cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t);
6242  }
6243  }
6244 
6245  if(cloudFrom->size())
6246  {
6247  constraintsViewer_->addCloud("words0", cloudFrom, pose, Qt::red);
6248  }
6249  else
6250  {
6251  UWARN("Empty 3D words for node %d", link.from());
6252  constraintsViewer_->removeCloud("words0");
6253  }
6254  if(cloudTo->size())
6255  {
6256  constraintsViewer_->addCloud("words1", cloudTo, pose, Qt::cyan);
6257  }
6258  else
6259  {
6260  if(sTo)
6261  {
6262  UWARN("Empty 3D words for node %d", link.to());
6263  }
6264  constraintsViewer_->removeCloud("words1");
6265  }
6266  }
6267  else
6268  {
6269  UERROR("Not found signature %d or %d in RAM", link.from(), link.to());
6270  constraintsViewer_->removeCloud("words0");
6271  constraintsViewer_->removeCloud("words1");
6272  }
6273  //cleanup
6274  for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
6275  {
6276  delete *iter;
6277  }
6278  }
6279 
6280  constraintsViewer_->removeCloud("scan2");
6281  constraintsViewer_->removeGraph("scan2graph");
6282  constraintsViewer_->removeCloud("scan0");
6283  constraintsViewer_->removeCloud("scan1");
6284  if(ui_->checkBox_show2DScans->isChecked())
6285  {
6286  //cloud 2d
6287  if(link.type() == Link::kLocalSpaceClosure &&
6288  !link.userDataCompressed().empty() &&
6289  signatureTo.id()==0)
6290  {
6291  std::vector<int> ids;
6292  cv::Mat userData = link.uncompressUserDataConst();
6293  if(userData.type() == CV_8SC1 &&
6294  userData.rows == 1 &&
6295  userData.cols >= 8 && // including null str ending
6296  userData.at<char>(userData.cols-1) == 0 &&
6297  memcmp(userData.data, "SCANS:", 6) == 0)
6298  {
6299  std::string scansStr = (const char *)userData.data;
6300  UINFO("Detected \"%s\" in links's user data", scansStr.c_str());
6301  if(!scansStr.empty())
6302  {
6303  std::list<std::string> strs = uSplit(scansStr, ':');
6304  if(strs.size() == 2)
6305  {
6306  std::list<std::string> strIds = uSplit(strs.rbegin()->c_str(), ';');
6307  for(std::list<std::string>::iterator iter=strIds.begin(); iter!=strIds.end(); ++iter)
6308  {
6309  ids.push_back(atoi(iter->c_str()));
6310  if(ids.back() == link.from())
6311  {
6312  ids.pop_back();
6313  }
6314  }
6315  }
6316  }
6317  }
6318  if(ids.size())
6319  {
6320  //add other scans matching
6321  //optimize the path's poses locally
6322 
6323  std::map<int, rtabmap::Transform> poses;
6324  for(unsigned int i=0; i<ids.size(); ++i)
6325  {
6326  if(uContains(odomPoses_, ids[i]))
6327  {
6328  poses.insert(*odomPoses_.find(ids[i]));
6329  }
6330  else
6331  {
6332  UERROR("Not found %d node!", ids[i]);
6333  }
6334  }
6335  if(poses.size())
6336  {
6337  Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
6338 
6339  UASSERT(uContains(poses, link.to()));
6340  std::map<int, rtabmap::Transform> posesOut;
6341  std::multimap<int, rtabmap::Link> linksOut;
6342  optimizer->getConnectedGraph(
6343  link.to(),
6344  poses,
6345  updateLinksWithModifications(links_),
6346  posesOut,
6347  linksOut);
6348 
6349  if(poses.size() != posesOut.size())
6350  {
6351  UWARN("Scan poses input and output are different! %d vs %d", (int)poses.size(), (int)posesOut.size());
6352  }
6353 
6354  UDEBUG("Input poses: ");
6355  for(std::map<int, Transform>::iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter)
6356  {
6357  UDEBUG(" %d=%s", iter->first, iter->second.prettyPrint().c_str());
6358  }
6359  UDEBUG("Input links: ");
6360  for(std::multimap<int, Link>::iterator iter=linksOut.begin(); iter!=linksOut.end(); ++iter)
6361  {
6362  UDEBUG(" %d->%d (type=%s) %s", iter->second.from(), iter->second.to(), iter->second.typeName().c_str(), iter->second.transform().prettyPrint().c_str());
6363  }
6364 
6365  std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(link.to(), posesOut, linksOut);
6366  delete optimizer;
6367 
6368  UDEBUG("Output poses: ");
6369  for(std::map<int, Transform>::iterator iter=finalPoses.begin(); iter!=finalPoses.end(); ++iter)
6370  {
6371  UDEBUG(" %d=%s", iter->first, iter->second.prettyPrint().c_str());
6372  }
6373 
6374  // transform local poses in loop referential
6375  Transform u = t * finalPoses.at(link.to()).inverse();
6376  pcl::PointCloud<pcl::PointXYZ>::Ptr assembledScans(new pcl::PointCloud<pcl::PointXYZ>);
6377  pcl::PointCloud<pcl::PointNormal>::Ptr assembledNormalScans(new pcl::PointCloud<pcl::PointNormal>);
6378  pcl::PointCloud<pcl::PointXYZI>::Ptr assembledIScans(new pcl::PointCloud<pcl::PointXYZI>);
6379  pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembledINormalScans(new pcl::PointCloud<pcl::PointXYZINormal>);
6380  pcl::PointCloud<pcl::PointXYZ>::Ptr graph(new pcl::PointCloud<pcl::PointXYZ>);
6381  for(std::map<int, Transform>::iterator iter=finalPoses.begin(); iter!=finalPoses.end(); ++iter)
6382  {
6383  iter->second = u * iter->second;
6384  if(iter->first != link.to()) // already added to view
6385  {
6386  //create scan
6387  SensorData data;
6388  dbDriver_->getNodeData(iter->first, data);
6389  LaserScan scan;
6390  data.uncompressDataConst(0, 0, &scan, 0);
6391  if(!scan.isEmpty())
6392  {
6393  if(scan.hasNormals() && scan.hasIntensity())
6394  {
6395  *assembledINormalScans += *util3d::laserScanToPointCloudINormal(scan, iter->second*scan.localTransform());
6396  }
6397  else if(scan.hasNormals())
6398  {
6399  *assembledNormalScans += *util3d::laserScanToPointCloudNormal(scan, iter->second*scan.localTransform());
6400  }
6401  else if(scan.hasIntensity())
6402  {
6403  *assembledIScans += *util3d::laserScanToPointCloudI(scan, iter->second*scan.localTransform());
6404  }
6405  else
6406  {
6407  *assembledScans += *util3d::laserScanToPointCloud(scan, iter->second*scan.localTransform());
6408  }
6409  }
6410  }
6411  graph->push_back(util3d::transformPoint(pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()), pose));
6412  }
6413 
6414  if(assembledNormalScans->size())
6415  {
6416  constraintsViewer_->addCloud("scan2", assembledNormalScans, pose, Qt::cyan);
6417  constraintsViewer_->setCloudColorIndex("scan2", 2);
6418  }
6419  if(assembledScans->size())
6420  {
6421  constraintsViewer_->addCloud("scan2", assembledScans, pose, Qt::cyan);
6422  constraintsViewer_->setCloudColorIndex("scan2", 2);
6423  }
6424  if(assembledINormalScans->size())
6425  {
6426  constraintsViewer_->addCloud("scan2", assembledINormalScans, pose, Qt::cyan);
6427  constraintsViewer_->setCloudColorIndex("scan2", 2);
6428  }
6429  if(assembledIScans->size())
6430  {
6431  constraintsViewer_->addCloud("scan2", assembledIScans, pose, Qt::cyan);
6432  constraintsViewer_->setCloudColorIndex("scan2", 2);
6433  }
6434  if(graph->size())
6435  {
6436  constraintsViewer_->addOrUpdateGraph("scan2graph", graph, Qt::cyan);
6437  }
6438  }
6439  }
6440  }
6441 
6442  // Added loop closure scans
6443  constraintsViewer_->removeCloud("scan0");
6444  constraintsViewer_->removeCloud("scan1");
6445  if(!dataFrom.laserScanRaw().isEmpty())
6446  {
6447  if(dataFrom.laserScanRaw().hasNormals() && dataFrom.laserScanRaw().hasIntensity())
6448  {
6449  pcl::PointCloud<pcl::PointXYZINormal>::Ptr scan;
6450  scan = rtabmap::util3d::laserScanToPointCloudINormal(dataFrom.laserScanRaw(), dataFrom.laserScanRaw().localTransform());
6451  constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
6452  constraintsViewer_->setCloudColorIndex("scan0", 2);
6453  }
6454  else if(dataFrom.laserScanRaw().hasNormals())
6455  {
6456  pcl::PointCloud<pcl::PointNormal>::Ptr scan;
6457  scan = rtabmap::util3d::laserScanToPointCloudNormal(dataFrom.laserScanRaw(), dataFrom.laserScanRaw().localTransform());
6458  constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
6459  constraintsViewer_->setCloudColorIndex("scan0", 2);
6460  }
6461  else if(dataFrom.laserScanRaw().hasIntensity())
6462  {
6463  pcl::PointCloud<pcl::PointXYZI>::Ptr scan;
6464  scan = rtabmap::util3d::laserScanToPointCloudI(dataFrom.laserScanRaw(), dataFrom.laserScanRaw().localTransform());
6465  constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
6466  constraintsViewer_->setCloudColorIndex("scan0", 2);
6467  }
6468  else
6469  {
6470  pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
6471  scan = rtabmap::util3d::laserScanToPointCloud(dataFrom.laserScanRaw(), dataFrom.laserScanRaw().localTransform());
6472  constraintsViewer_->addCloud("scan0", scan, pose, Qt::yellow);
6473  constraintsViewer_->setCloudColorIndex("scan0", 2);
6474  }
6475  }
6476  if(!dataTo.laserScanRaw().isEmpty())
6477  {
6478  if(dataTo.laserScanRaw().hasNormals() && dataTo.laserScanRaw().hasIntensity())
6479  {
6480  pcl::PointCloud<pcl::PointXYZINormal>::Ptr scan;
6481  scan = rtabmap::util3d::laserScanToPointCloudINormal(dataTo.laserScanRaw(), t*dataTo.laserScanRaw().localTransform());
6482  constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
6483  constraintsViewer_->setCloudColorIndex("scan1", 2);
6484  }
6485  else if(dataTo.laserScanRaw().hasNormals())
6486  {
6487  pcl::PointCloud<pcl::PointNormal>::Ptr scan;
6488  scan = rtabmap::util3d::laserScanToPointCloudNormal(dataTo.laserScanRaw(), t*dataTo.laserScanRaw().localTransform());
6489  constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
6490  constraintsViewer_->setCloudColorIndex("scan1", 2);
6491  }
6492  else if(dataTo.laserScanRaw().hasIntensity())
6493  {
6494  pcl::PointCloud<pcl::PointXYZI>::Ptr scan;
6495  scan = rtabmap::util3d::laserScanToPointCloudI(dataTo.laserScanRaw(), t*dataTo.laserScanRaw().localTransform());
6496  constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
6497  constraintsViewer_->setCloudColorIndex("scan1", 2);
6498  }
6499  else
6500  {
6501  pcl::PointCloud<pcl::PointXYZ>::Ptr scan;
6502  scan = rtabmap::util3d::laserScanToPointCloud(dataTo.laserScanRaw(), t*dataTo.laserScanRaw().localTransform());
6503  constraintsViewer_->addCloud("scan1", scan, pose, Qt::magenta);
6504  constraintsViewer_->setCloudColorIndex("scan1", 2);
6505  }
6506  }
6507  }
6508 
6509  //frustums
6510  constraintsViewer_->removeAllFrustums();
6511  if(constraintsViewer_->isFrustumShown())
6512  {
6513  CameraModel model;
6514  if(dataFrom.cameraModels().size())
6515  {
6516  model = dataFrom.cameraModels()[0];
6517  }
6518  else if(dataFrom.stereoCameraModels().size())
6519  {
6520  model = dataFrom.stereoCameraModels()[0].left();
6521  }
6522  constraintsViewer_->addOrUpdateFrustum("frustum_from", pose, model.localTransform(), constraintsViewer_->getFrustumScale(), constraintsViewer_->getFrustumColor(), model.fovX(), model.fovY());
6523  model = CameraModel();
6524  if(dataTo.cameraModels().size())
6525  {
6526  model = dataTo.cameraModels()[0];
6527  }
6528  else if(dataTo.stereoCameraModels().size())
6529  {
6530  model = dataTo.stereoCameraModels()[0].left();
6531  }
6532  constraintsViewer_->addOrUpdateFrustum("frustum_to", pose*t, model.localTransform(), constraintsViewer_->getFrustumScale(), constraintsViewer_->getFrustumColor(), model.fovX(), model.fovY());
6533  }
6534 
6535  //update coordinate
6536  constraintsViewer_->addOrUpdateCoordinate("from_coordinate", pose, 0.2);
6537 #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
6538  constraintsViewer_->addOrUpdateCoordinate("to_coordinate", pose*t, 0.2);
6539  constraintsViewer_->removeCoordinate("to_coordinate_gt");
6540  if(uContains(groundTruthPoses_, link.from()) && uContains(groundTruthPoses_, link.to()))
6541  {
6542  constraintsViewer_->addOrUpdateCoordinate("to_coordinate_gt",
6543  pose*(groundTruthPoses_.at(link.from()).inverse()*groundTruthPoses_.at(link.to())), 0.1);
6544  }
6545 #endif
6546 
6547  constraintsViewer_->clearTrajectory();
6548 
6549  constraintsViewer_->refreshView();
6550  }
6551 
6552  // update buttons
6553  updateConstraintButtons();
6554 }
6555 
6556 void DatabaseViewer::updateConstraintButtons()
6557 {
6558  ui_->pushButton_refine->setEnabled(false);
6559  ui_->pushButton_reset->setEnabled(false);
6560  ui_->pushButton_add->setEnabled(false);
6561  ui_->pushButton_reject->setEnabled(false);
6562  ui_->toolButton_constraint->setEnabled(false);
6563 
6564  Link currentLink;
6565  int from;
6566  int to;
6567  if(ui_->label_type->text().toInt() == Link::kLandmark)
6568  {
6569  //check for modified link
6570  currentLink = loopLinks_.at(ui_->horizontalSlider_loops->value());
6571  from = currentLink.from();
6572  to = currentLink.to();
6573  }
6574  else
6575  {
6576  from = ids_.at(ui_->horizontalSlider_A->value());
6577  to = ids_.at(ui_->horizontalSlider_B->value());
6578  if(from!=to && from && to &&
6579  odomPoses_.find(from) != odomPoses_.end() &&
6580  odomPoses_.find(to) != odomPoses_.end() &&
6581  (ui_->checkBox_enableForAll->isChecked() ||
6582  (weights_.find(from) != weights_.end() && weights_.at(from)>=0 &&
6583  weights_.find(to) != weights_.end() && weights_.at(to)>=0)))
6584  {
6585  if((!containsLink(links_, from ,to) && !containsLink(linksAdded_, from ,to)) ||
6586  containsLink(linksRemoved_, from ,to))
6587  {
6588  ui_->pushButton_add->setEnabled(true);
6589  }
6590  }
6591  else if(ui_->checkBox_enableForAll->isChecked())
6592  {
6593  if(odomPoses_.find(from) == odomPoses_.end())
6594  {
6595  UWARN("Button \"Add\" cannot be enabled even if \"all\" checkbox is checked, as node %d doesn't have odometry set.", from);
6596  }
6597  else if(odomPoses_.find(to) == odomPoses_.end())
6598  {
6599  UWARN("Button \"Add\" cannot be enabled even if \"all\" checkbox is checked, as node %d doesn't have odometry set.", to);
6600  }
6601  }
6602 
6603  currentLink = findActiveLink(from ,to);
6604  }
6605 
6606  if(currentLink.isValid() &&
6607  ((currentLink.from() == from && currentLink.to() == to) || (currentLink.from() == to && currentLink.to() == from)))
6608  {
6609  if(!containsLink(linksRemoved_, from ,to))
6610  {
6611  ui_->pushButton_reject->setEnabled(true);
6612  }
6613 
6614  //check for modified link
6615  std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, currentLink.from(), currentLink.to());
6616  if(iter != linksRefined_.end())
6617  {
6618  currentLink = iter->second;
6619  ui_->pushButton_reset->setEnabled(true);
6620  }
6621  ui_->pushButton_refine->setEnabled(currentLink.from()!=currentLink.to() && currentLink.type() != Link::kLandmark);
6622  ui_->toolButton_constraint->setEnabled(true);
6623  }
6624 }
6625 
6626 void DatabaseViewer::sliderIterationsValueChanged(int value)
6627 {
6628  if(dbDriver_ && value >=0 && value < (int)graphes_.size())
6629  {
6630  std::map<int, rtabmap::Transform> graph = uValueAt(graphes_, value);
6631 
6632  std::map<int, Transform> refPoses = groundTruthPoses_;
6633  if(refPoses.empty())
6634  {
6635  refPoses = gpsPoses_;
6636  }
6637 
6638  // Log ground truth statistics (in TUM's RGBD-SLAM format)
6639  if(refPoses.size())
6640  {
6641  // compute KITTI statistics before aligning the poses
6642  float length = graph::computePathLength(graph);
6643  if(refPoses.size() == graph.size() && length >= 100.0f)
6644  {
6645  float t_err = 0.0f;
6646  float r_err = 0.0f;
6647  graph::calcKittiSequenceErrors(uValues(refPoses), uValues(graph), t_err, r_err);
6648  UINFO("KITTI t_err = %f %%", t_err);
6649  UINFO("KITTI r_err = %f deg/m", r_err);
6650  }
6651 
6652  float translational_rmse = 0.0f;
6653  float translational_mean = 0.0f;
6654  float translational_median = 0.0f;
6655  float translational_std = 0.0f;
6656  float translational_min = 0.0f;
6657  float translational_max = 0.0f;
6658  float rotational_rmse = 0.0f;
6659  float rotational_mean = 0.0f;
6660  float rotational_median = 0.0f;
6661  float rotational_std = 0.0f;
6662  float rotational_min = 0.0f;
6663  float rotational_max = 0.0f;
6664 
6665  Transform gtToMap = graph::calcRMSE(
6666  refPoses,
6667  graph,
6668  translational_rmse,
6669  translational_mean,
6670  translational_median,
6671  translational_std,
6672  translational_min,
6673  translational_max,
6674  rotational_rmse,
6675  rotational_mean,
6676  rotational_median,
6677  rotational_std,
6678  rotational_min,
6679  rotational_max);
6680 
6681  // ground truth live statistics
6682  ui_->label_rmse->setNum(translational_rmse);
6683  UINFO("translational_rmse=%f", translational_rmse);
6684  UINFO("translational_mean=%f", translational_mean);
6685  UINFO("translational_median=%f", translational_median);
6686  UINFO("translational_std=%f", translational_std);
6687  UINFO("translational_min=%f", translational_min);
6688  UINFO("translational_max=%f", translational_max);
6689 
6690  UINFO("rotational_rmse=%f", rotational_rmse);
6691  UINFO("rotational_mean=%f", rotational_mean);
6692  UINFO("rotational_median=%f", rotational_median);
6693  UINFO("rotational_std=%f", rotational_std);
6694  UINFO("rotational_min=%f", rotational_min);
6695  UINFO("rotational_max=%f", rotational_max);
6696 
6697  if(ui_->checkBox_alignPosesWithGroundTruth->isChecked() && !gtToMap.isIdentity())
6698  {
6699  for(std::map<int, Transform>::iterator iter=graph.begin(); iter!=graph.end(); ++iter)
6700  {
6701  iter->second = gtToMap * iter->second;
6702  }
6703  }
6704  }
6705 
6706  std::map<int, rtabmap::Transform> graphFiltered;
6707  if(ui_->checkBox_alignScansCloudsWithGroundTruth->isChecked() && !groundTruthPoses_.empty())
6708  {
6709  graphFiltered = groundTruthPoses_;
6710  }
6711  else
6712  {
6713  graphFiltered = graph;
6714  }
6715  if(ui_->groupBox_posefiltering->isChecked())
6716  {
6717  graphFiltered = graph::radiusPosesFiltering(graph,
6718  ui_->doubleSpinBox_posefilteringRadius->value(),
6719  ui_->doubleSpinBox_posefilteringAngle->value()*CV_PI/180.0);
6720  }
6721  std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> > localMaps;
6722  std::map<int, std::pair<float, cv::Point3f> > localMapsInfo;
6723 #ifdef RTABMAP_OCTOMAP
6724  if(octomap_)
6725  {
6726  delete octomap_;
6727  octomap_ = 0;
6728  }
6729 #endif
6730  if(ui_->dockWidget_graphView->isVisible() || ui_->dockWidget_occupancyGridView->isVisible())
6731  {
6732  //update scans
6733  UINFO("Update local maps list...");
6734  std::vector<int> ids = uKeys(graphFiltered);
6735  for(unsigned int i=0; i<ids.size(); ++i)
6736  {
6737  if(generatedLocalMaps_.find(ids[i]) != generatedLocalMaps_.end())
6738  {
6739  localMaps.insert(*generatedLocalMaps_.find(ids[i]));
6740  localMapsInfo.insert(*generatedLocalMapsInfo_.find(ids[i]));
6741  }
6742  else if(localMaps_.find(ids[i]) != localMaps_.end())
6743  {
6744  if(!localMaps_.find(ids[i])->second.first.first.empty() || !localMaps_.find(ids[i])->second.first.second.empty())
6745  {
6746  localMaps.insert(*localMaps_.find(ids.at(i)));
6747  localMapsInfo.insert(*localMapsInfo_.find(ids[i]));
6748  }
6749  }
6750  else if(ids.at(i)>0)
6751  {
6752  SensorData data;
6753  dbDriver_->getNodeData(ids.at(i), data, false, false, false);
6754  cv::Mat ground, obstacles, empty;
6755  data.uncompressData(0, 0, 0, 0, &ground, &obstacles, &empty);
6756  localMaps_.insert(std::make_pair(ids.at(i), std::make_pair(std::make_pair(ground, obstacles), empty)));
6757  localMapsInfo_.insert(std::make_pair(ids.at(i), std::make_pair(data.gridCellSize(), data.gridViewPoint())));
6758  if(!ground.empty() || !obstacles.empty())
6759  {
6760  localMaps.insert(std::make_pair(ids.at(i), std::make_pair(std::make_pair(ground, obstacles), empty)));
6761  localMapsInfo.insert(std::make_pair(ids.at(i), std::make_pair(data.gridCellSize(), data.gridViewPoint())));
6762  }
6763  }
6764  }
6765  //cleanup
6766  for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps_.begin(); iter!=localMaps_.end();)
6767  {
6768  if(graphFiltered.find(iter->first) == graphFiltered.end())
6769  {
6770  localMapsInfo_.erase(iter->first);
6771  localMaps_.erase(iter++);
6772  }
6773  else
6774  {
6775  ++iter;
6776  }
6777  }
6778  UINFO("Update local maps list... done (%d local maps, graph size=%d)", (int)localMaps.size(), (int)graph.size());
6779  }
6780 
6781  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
6782  float cellSize = Parameters::defaultGridCellSize();
6783  Parameters::parse(parameters, Parameters::kGridCellSize(), cellSize);
6784 
6785  ui_->graphViewer->updateGTGraph(groundTruthPoses_);
6786  ui_->graphViewer->updateGPSGraph(gpsPoses_, gpsValues_);
6787  ui_->graphViewer->updateGraph(graph, graphLinks_, mapIds_, weights_);
6788  if(!ui_->checkBox_wmState->isChecked())
6789  {
6790  bool allNodesAreInWM = true;
6791  std::map<int, float> colors;
6792  for(std::map<int, rtabmap::Transform>::iterator iter=graph.begin(); iter!=graph.end(); ++iter)
6793  {
6794  if(lastWmIds_.find(iter->first) != lastWmIds_.end())
6795  {
6796  colors.insert(std::make_pair(iter->first, 1));
6797  }
6798  else
6799  {
6800  allNodesAreInWM = false;
6801  }
6802  }
6803  if(!allNodesAreInWM)
6804  {
6805  ui_->graphViewer->updatePosterior(colors, 1, 1);
6806  }
6807  }
6808  QGraphicsRectItem * rectScaleItem = 0;
6809  ui_->graphViewer->clearMap();
6810  occupancyGridViewer_->clear();
6811  if(graph.size() && localMaps.size() &&
6812  (ui_->graphViewer->isGridMapVisible() || ui_->dockWidget_occupancyGridView->isVisible()))
6813  {
6814  QTime time;
6815  time.start();
6816 
6817 #ifdef RTABMAP_OCTOMAP
6818  if(ui_->checkBox_octomap->isChecked())
6819  {
6820  octomap_ = new OctoMap(parameters);
6821  bool updateAborted = false;
6822  for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter)
6823  {
6824  if(iter->second.first.first.channels() == 2 || iter->second.first.second.channels() == 2)
6825  {
6826  QMessageBox::warning(this, tr(""),
6827  tr("Some local occupancy grids are 2D, but OctoMap requires 3D local "
6828  "occupancy grids. Uncheck OctoMap under GUI parameters or generate "
6829  "3D local occupancy grids (\"Grid/3D\" core parameter)."));
6830  updateAborted = true;
6831  break;
6832  }
6833  octomap_->addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second, localMapsInfo.at(iter->first).second);
6834  }
6835  if(!updateAborted)
6836  {
6837  octomap_->update(graphFiltered);
6838  }
6839  }
6840 #endif
6841 
6842  // Generate 2d grid map?
6843  if((ui_->dockWidget_graphView->isVisible() && ui_->graphViewer->isGridMapVisible()) ||
6844  (ui_->dockWidget_occupancyGridView->isVisible() && ui_->checkBox_grid_2d->isChecked()))
6845  {
6846  bool eroded = Parameters::defaultGridGlobalEroded();
6847  Parameters::parse(parameters, Parameters::kGridGlobalEroded(), eroded);
6848  float xMin, yMin;
6849  cv::Mat map;
6850 
6851 #ifdef RTABMAP_OCTOMAP
6852  if(ui_->checkBox_octomap->isChecked())
6853  {
6854  map = octomap_->createProjectionMap(xMin, yMin, cellSize, 0, ui_->spinBox_grid_depth->value());
6855  }
6856  else
6857 #endif
6858  {
6859  if(eroded)
6860  {
6861  uInsert(parameters, ParametersPair(Parameters::kGridGlobalEroded(), "true"));
6862  }
6863  OccupancyGrid grid(parameters);
6864  grid.setCellSize(cellSize);
6865  for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter)
6866  {
6867  grid.addToCache(iter->first, iter->second.first.first, iter->second.first.second, iter->second.second);
6868  }
6869  grid.update(graphFiltered);
6870  if(ui_->checkBox_grid_showProbMap->isChecked())
6871  {
6872  map = grid.getProbMap(xMin, yMin);
6873  }
6874  else
6875  {
6876  map = grid.getMap(xMin, yMin);
6877  }
6878  }
6879 
6880  ui_->label_timeGrid->setNum(double(time.elapsed())/1000.0);
6881 
6882  if(!map.empty())
6883  {
6884  cv::Mat map8U = rtabmap::util3d::convertMap2Image8U(map);
6885  if(ui_->dockWidget_graphView->isVisible() && ui_->graphViewer->isGridMapVisible())
6886  {
6887  ui_->graphViewer->updateMap(map8U, cellSize, xMin, yMin);
6888  }
6889  if(ui_->dockWidget_occupancyGridView->isVisible() && ui_->checkBox_grid_2d->isChecked())
6890  {
6891  occupancyGridViewer_->addOccupancyGridMap(map8U, cellSize, xMin, yMin, 1.0f);
6892  occupancyGridViewer_->refreshView();
6893  }
6894 
6895  // Zoom to ignore unknowns
6896  int xFirst = 0;
6897  int yFirst = 0;
6898  int xLast = map.cols;
6899  int yLast = map.rows;
6900  bool firstSet = false;
6901  bool lastSet = false;
6902  for(int x=0; x<map.cols && (!firstSet || !lastSet); ++x)
6903  {
6904  for(int y=0; y<map.rows; ++y)
6905  {
6906  // check for first
6907  if(!firstSet && map.at<char>(y, x) != -1)
6908  {
6909  xFirst = x;
6910  firstSet = true;
6911  }
6912  // check for last
6913  int opp = map.cols-(x+1);
6914  if(!lastSet && map.at<char>(y, opp) != -1)
6915  {
6916  xLast = opp;
6917  lastSet = true;
6918  }
6919  }
6920  }
6921  firstSet = false;
6922  lastSet = false;
6923  for(int y=0; y<map.rows && (!firstSet || !lastSet); ++y)
6924  {
6925  for(int x=0; x<map.cols; ++x)
6926  {
6927  // check for first
6928  if(!firstSet && map.at<char>(y, x) != -1)
6929  {
6930  yFirst = y;
6931  firstSet = true;
6932  }
6933  // check for last
6934  int opp = map.rows-(y+1);
6935  if(!lastSet && map.at<char>(map.rows-(y+1), x) != -1)
6936  {
6937  yLast = opp;
6938  lastSet = true;
6939  }
6940  }
6941  }
6942  // Only zoom if there are significant unknowns
6943  if( (xLast > xFirst && yLast > yFirst) &&
6944  (xFirst > 50 ||
6945  xLast < map.cols-50 ||
6946  yFirst > 50 ||
6947  yLast < map.rows-50))
6948  {
6949  rectScaleItem = ui_->graphViewer->scene()->addRect(
6950  xFirst-25,
6951  yFirst-25,
6952  xLast-xFirst+50,
6953  yLast-yFirst+50);
6954  rectScaleItem->setTransform(QTransform::fromScale(cellSize*100.0f, -cellSize*100.0f), true);
6955  rectScaleItem->setRotation(90);
6956  rectScaleItem->setPos(-yMin*100.0f, -xMin*100.0f);
6957  }
6958  }
6959  }
6960 
6961  // Generate 3d grid map?
6962  if(ui_->dockWidget_occupancyGridView->isVisible())
6963  {
6964 #ifdef RTABMAP_OCTOMAP
6965  if(ui_->checkBox_octomap->isChecked())
6966  {
6967  updateOctomapView();
6968  }
6969  else
6970 #endif
6971  {
6972  pcl::PointCloud<pcl::PointXYZ>::Ptr groundXYZ(new pcl::PointCloud<pcl::PointXYZ>);
6973  pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesXYZ(new pcl::PointCloud<pcl::PointXYZ>);
6974  pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCellsXYZ(new pcl::PointCloud<pcl::PointXYZ>);
6975  pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
6976  pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
6977  pcl::PointCloud<pcl::PointXYZRGB>::Ptr emptyCellsRGB(new pcl::PointCloud<pcl::PointXYZRGB>);
6978 
6979  for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator iter=localMaps.begin(); iter!=localMaps.end(); ++iter)
6980  {
6981  Transform pose = graphFiltered.at(iter->first);
6982  float x,y,z,roll,pitch,yaw;
6983  pose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
6984  Transform pose2d(x,y, 0, 0, 0, yaw);
6985  if(!iter->second.first.first.empty())
6986  {
6987  if(iter->second.first.first.channels() == 4)
6988  {
6989  *groundRGB += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(iter->second.first.first), pose);
6990  }
6991  else
6992  {
6993  *groundXYZ += *util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(iter->second.first.first), iter->second.first.first.channels()==2?pose2d:pose);
6994  }
6995  }
6996  if(!iter->second.first.second.empty())
6997  {
6998  if(iter->second.first.second.channels() == 4)
6999  {
7000  *obstaclesRGB += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(iter->second.first.second), pose);
7001  }
7002  else
7003  {
7004  *obstaclesXYZ += *util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(iter->second.first.second), iter->second.first.second.channels()==2?pose2d:pose);
7005  }
7006  }
7007  if(ui_->checkBox_grid_empty->isChecked())
7008  {
7009  if(!iter->second.second.empty())
7010  {
7011  if(iter->second.second.channels() == 4)
7012  {
7013  *emptyCellsRGB += *util3d::laserScanToPointCloudRGB(LaserScan::backwardCompatibility(iter->second.second), pose);
7014  }
7015  else
7016  {
7017  *emptyCellsXYZ += *util3d::laserScanToPointCloud(LaserScan::backwardCompatibility(iter->second.second), iter->second.second.channels()==2?pose2d:pose);
7018  }
7019  }
7020  }
7021  }
7022  // occupancy cloud
7023  if(groundRGB->size())
7024  {
7025  groundRGB = util3d::voxelize(groundRGB, cellSize);
7026  occupancyGridViewer_->addCloud("groundRGB",
7027  groundRGB,
7028  Transform::getIdentity(),
7029  QColor(ui_->lineEdit_groundColor->text()));
7030  occupancyGridViewer_->setCloudPointSize("groundRGB", 5);
7031  }
7032  if(groundXYZ->size())
7033  {
7034  groundXYZ = util3d::voxelize(groundXYZ, cellSize);
7035  occupancyGridViewer_->addCloud("groundXYZ",
7036  groundXYZ,
7037  Transform::getIdentity(),
7038  QColor(ui_->lineEdit_groundColor->text()));
7039  occupancyGridViewer_->setCloudPointSize("groundXYZ", 5);
7040  }
7041  if(obstaclesRGB->size())
7042  {
7043  obstaclesRGB = util3d::voxelize(obstaclesRGB, cellSize);
7044  occupancyGridViewer_->addCloud("obstaclesRGB",
7045  obstaclesRGB,
7046  Transform::getIdentity(),
7047  QColor(ui_->lineEdit_obstacleColor->text()));
7048  occupancyGridViewer_->setCloudPointSize("obstaclesRGB", 5);
7049  }
7050  if(obstaclesXYZ->size())
7051  {
7052  obstaclesXYZ = util3d::voxelize(obstaclesXYZ, cellSize);
7053  occupancyGridViewer_->addCloud("obstaclesXYZ",
7054  obstaclesXYZ,
7055  Transform::getIdentity(),
7056  QColor(ui_->lineEdit_obstacleColor->text()));
7057  occupancyGridViewer_->setCloudPointSize("obstaclesXYZ", 5);
7058  }
7059  if(emptyCellsRGB->size())
7060  {
7061  emptyCellsRGB = util3d::voxelize(emptyCellsRGB, cellSize);
7062  occupancyGridViewer_->addCloud("emptyCellsRGB",
7063  emptyCellsRGB,
7064  Transform::getIdentity(),
7065  QColor(ui_->lineEdit_emptyColor->text()));
7066  occupancyGridViewer_->setCloudPointSize("emptyCellsRGB", 5);
7067  occupancyGridViewer_->setCloudOpacity("emptyCellsRGB", 0.5);
7068  }
7069  if(emptyCellsXYZ->size())
7070  {
7071  emptyCellsXYZ = util3d::voxelize(emptyCellsXYZ, cellSize);
7072  occupancyGridViewer_->addCloud("emptyCellsXYZ",
7073  emptyCellsXYZ,
7074  Transform::getIdentity(),
7075  QColor(ui_->lineEdit_emptyColor->text()));
7076  occupancyGridViewer_->setCloudPointSize("emptyCellsXYZ", 5);
7077  occupancyGridViewer_->setCloudOpacity("emptyCellsXYZ", 0.5);
7078  }
7079  occupancyGridViewer_->refreshView();
7080  }
7081  }
7082  }
7083  ui_->graphViewer->fitInView(ui_->graphViewer->scene()->itemsBoundingRect(), Qt::KeepAspectRatio);
7084  if(rectScaleItem != 0)
7085  {
7086  ui_->graphViewer->fitInView(rectScaleItem, Qt::KeepAspectRatio);
7087  ui_->graphViewer->scene()->removeItem(rectScaleItem);
7088  delete rectScaleItem;
7089  }
7090 
7091  ui_->graphViewer->update();
7092  ui_->label_iterations->setNum(value);
7093 
7094  //compute total length (neighbor links)
7095  float length = 0.0f;
7096  for(std::multimap<int, rtabmap::Link>::const_iterator iter=graphLinks_.begin(); iter!=graphLinks_.end(); ++iter)
7097  {
7098  std::map<int, rtabmap::Transform>::const_iterator jterA = graph.find(iter->first);
7099  std::map<int, rtabmap::Transform>::const_iterator jterB = graph.find(iter->second.to());
7100  if(jterA != graph.end() && jterB != graph.end())
7101  {
7102  const rtabmap::Transform & poseA = jterA->second;
7103  const rtabmap::Transform & poseB = jterB->second;
7104  if(iter->second.type() == rtabmap::Link::kNeighbor ||
7105  iter->second.type() == rtabmap::Link::kNeighborMerged)
7106  {
7107  Eigen::Vector3f vA, vB;
7108  float x,y,z;
7109  poseA.getTranslation(x,y,z);
7110  vA[0] = x; vA[1] = y; vA[2] = z;
7111  poseB.getTranslation(x,y,z);
7112  vB[0] = x; vB[1] = y; vB[2] = z;
7113  length += (vB - vA).norm();
7114  }
7115  }
7116  }
7117  ui_->label_pathLength->setNum(length);
7118  }
7119 }
7120 void DatabaseViewer::updateGraphView()
7121 {
7122  ui_->label_loopClosures->clear();
7123  ui_->label_poses->clear();
7124  ui_->label_rmse->clear();
7125 
7126  if(odomPoses_.size())
7127  {
7128  int fromId = ui_->spinBox_optimizationsFrom->value();
7129  if(!uContains(odomPoses_, fromId))
7130  {
7131  QMessageBox::warning(this, tr(""), tr("Graph optimization from id (%1) for which node is not linked to graph.\n Minimum=%2, Maximum=%3")
7132  .arg(fromId)
7133  .arg(odomPoses_.begin()->first)
7134  .arg(odomPoses_.rbegin()->first));
7135  return;
7136  }
7137 
7138  graphes_.clear();
7139  graphLinks_.clear();
7140 
7141  std::map<int, rtabmap::Transform> poses = odomPoses_;
7142  if(ui_->checkBox_wmState->isChecked() && uContains(wmStates_, fromId))
7143  {
7144  std::map<int, rtabmap::Transform> wmPoses;
7145  std::vector<int> & wmState = wmStates_.at(fromId);
7146  for(unsigned int i=0; i<wmState.size(); ++i)
7147  {
7148  std::map<int, rtabmap::Transform>::iterator iter = poses.find(wmState[i]);
7149  if(iter!=poses.end())
7150  {
7151  wmPoses.insert(*iter);
7152  }
7153  }
7154  if(!wmPoses.empty())
7155  {
7156  poses = wmPoses;
7157  }
7158  else
7159  {
7160  UWARN("Empty WM poses!? Ignoring WM state... (root id=%d, wmState=%d)", fromId, wmState.size());
7161  }
7162  }
7163 
7164  // filter current map if not spanning to all maps
7165  if(!ui_->checkBox_spanAllMaps->isChecked() && uContains(mapIds_, fromId) && mapIds_.at(fromId) >= 0)
7166  {
7167  int currentMapId = mapIds_.at(fromId);
7168  for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end();)
7169  {
7170  if(iter->first>0 && (!uContains(mapIds_, iter->first) || mapIds_.at(iter->first) != currentMapId))
7171  {
7172  poses.erase(iter++);
7173  }
7174  else
7175  {
7176  ++iter;
7177  }
7178  }
7179  }
7180 
7181  ui_->menuExport_poses->setEnabled(true);
7182  std::multimap<int, rtabmap::Link> links = links_;
7183  loopLinks_.clear();
7184 
7185  // filter current map if not spanning to all maps
7186  if(!ui_->checkBox_spanAllMaps->isChecked() && uContains(mapIds_, fromId) && mapIds_.at(fromId) >= 0)
7187  {
7188  int currentMapId = mapIds_.at(fromId);
7189  for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end();)
7190  {
7191  if((iter->second.from()>0 && (!uContains(mapIds_, iter->second.from()) || mapIds_.at(iter->second.from()) != currentMapId)) ||
7192  (iter->second.to()>0 && (!uContains(mapIds_, iter->second.to()) || mapIds_.at(iter->second.to()) != currentMapId)))
7193  {
7194  links.erase(iter++);
7195  }
7196  else
7197  {
7198  ++iter;
7199  }
7200  }
7201  }
7202 
7203  links = updateLinksWithModifications(links);
7204  if(ui_->checkBox_ignorePoseCorrection->isChecked())
7205  {
7206  for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
7207  {
7208  if(iter->second.type() == Link::kNeighbor ||
7209  iter->second.type() == Link::kNeighborMerged)
7210  {
7211  Transform poseFrom = uValue(poses, iter->second.from(), Transform());
7212  Transform poseTo = uValue(poses, iter->second.to(), Transform());
7213  if(!poseFrom.isNull() && !poseTo.isNull())
7214  {
7215  // reset to identity covariance
7216  iter->second = Link(
7217  iter->second.from(),
7218  iter->second.to(),
7219  iter->second.type(),
7220  poseFrom.inverse() * poseTo);
7221  }
7222  }
7223  }
7224  }
7225 
7226  // Marker priors parameters
7227  double markerPriorsLinearVariance = Parameters::defaultMarkerPriorsVarianceLinear();
7228  double markerPriorsAngularVariance = Parameters::defaultMarkerPriorsVarianceAngular();
7229  std::map<int, Transform> markerPriors;
7230  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
7231  Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), markerPriorsLinearVariance);
7232  UASSERT(markerPriorsLinearVariance>0.0f);
7233  Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceAngular(), markerPriorsAngularVariance);
7234  UASSERT(markerPriorsAngularVariance>0.0f);
7235  std::string markerPriorsStr;
7236  if(Parameters::parse(parameters, Parameters::kMarkerPriors(), markerPriorsStr))
7237  {
7238  std::list<std::string> strList = uSplit(markerPriorsStr, '|');
7239  for(std::list<std::string>::iterator iter=strList.begin(); iter!=strList.end(); ++iter)
7240  {
7241  std::string markerStr = *iter;
7242  while(!markerStr.empty() && !uIsDigit(markerStr[0]))
7243  {
7244  markerStr.erase(markerStr.begin());
7245  }
7246  if(!markerStr.empty())
7247  {
7248  std::string idStr = uSplitNumChar(markerStr).front();
7249  int id = uStr2Int(idStr);
7250  Transform prior = Transform::fromString(markerStr.substr(idStr.size()));
7251  if(!prior.isNull() && id>0)
7252  {
7253  markerPriors.insert(std::make_pair(-id, prior));
7254  UDEBUG("Added landmark prior %d: %s", id, prior.prettyPrint().c_str());
7255  }
7256  else
7257  {
7258  UERROR("Failed to parse element \"%s\" in parameter %s", markerStr.c_str(), Parameters::kMarkerPriors().c_str());
7259  }
7260  }
7261  else if(!iter->empty())
7262  {
7263  UERROR("Failed to parse parameter %s, value=\"%s\"", Parameters::kMarkerPriors().c_str(), iter->c_str());
7264  }
7265  }
7266  }
7267 
7268 
7269  // filter links
7270  int totalNeighbor = 0;
7271  int totalNeighborMerged = 0;
7272  int totalGlobal = 0;
7273  int totalLocalTime = 0;
7274  int totalLocalSpace = 0;
7275  int totalUser = 0;
7276  int totalPriors = 0;
7277  int totalLandmarks = 0;
7278  int totalGravity = 0;
7279  for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end();)
7280  {
7281  if(iter->second.type() == Link::kNeighbor)
7282  {
7283  ++totalNeighbor;
7284  }
7285  else if(iter->second.type() == Link::kNeighborMerged)
7286  {
7287  ++totalNeighborMerged;
7288  }
7289  else if(iter->second.type() == Link::kGlobalClosure)
7290  {
7291  if(ui_->checkBox_ignoreGlobalLoop->isChecked())
7292  {
7293  links.erase(iter++);
7294  continue;
7295  }
7296  loopLinks_.push_back(iter->second);
7297  ++totalGlobal;
7298  }
7299  else if(iter->second.type() == Link::kLocalSpaceClosure)
7300  {
7301  if(ui_->checkBox_ignoreLocalLoopSpace->isChecked())
7302  {
7303  links.erase(iter++);
7304  continue;
7305  }
7306  loopLinks_.push_back(iter->second);
7307  ++totalLocalSpace;
7308  }
7309  else if(iter->second.type() == Link::kLocalTimeClosure)
7310  {
7311  if(ui_->checkBox_ignoreLocalLoopTime->isChecked())
7312  {
7313  links.erase(iter++);
7314  continue;
7315  }
7316  loopLinks_.push_back(iter->second);
7317  ++totalLocalTime;
7318  }
7319  else if(iter->second.type() == Link::kUserClosure)
7320  {
7321  if(ui_->checkBox_ignoreUserLoop->isChecked())
7322  {
7323  links.erase(iter++);
7324  continue;
7325  }
7326  loopLinks_.push_back(iter->second);
7327  ++totalUser;
7328  }
7329  else if(iter->second.type() == Link::kLandmark)
7330  {
7331  if(ui_->checkBox_ignoreLandmarks->isChecked())
7332  {
7333  links.erase(iter++);
7334  continue;
7335  }
7336  UASSERT(iter->second.from() > 0 && iter->second.to() < 0);
7337  if(poses.find(iter->second.from()) != poses.end() && poses.find(iter->second.to()) == poses.end())
7338  {
7339  poses.insert(std::make_pair(iter->second.to(), poses.at(iter->second.from())*iter->second.transform()));
7340  }
7341  loopLinks_.push_back(iter->second);
7342  ++totalLandmarks;
7343 
7344  // add landmark priors if there are some
7345  int markerId = iter->second.to();
7346  if(markerPriors.find(markerId) != markerPriors.end())
7347  {
7348  cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64FC1);
7349  infMatrix(cv::Range(0,3), cv::Range(0,3)) /= markerPriorsLinearVariance;
7350  infMatrix(cv::Range(3,6), cv::Range(3,6)) /= markerPriorsAngularVariance;
7351  links.insert(std::make_pair(markerId, Link(markerId, markerId, Link::kPosePrior, markerPriors.at(markerId), infMatrix)));
7352  UDEBUG("Added prior %d : %s (variance: lin=%f ang=%f)", markerId, markerPriors.at(markerId).prettyPrint().c_str(),
7353  markerPriorsLinearVariance, markerPriorsAngularVariance);
7354  ++totalPriors;
7355  }
7356  }
7357  else if(iter->second.type() == Link::kPosePrior)
7358  {
7359  ++totalPriors;
7360  }
7361  else if(iter->second.type() == Link::kGravity)
7362  {
7363  ++totalGravity;
7364  }
7365  else
7366  {
7367  loopLinks_.push_back(iter->second);
7368  }
7369  ++iter;
7370  }
7371  updateLoopClosuresSlider();
7372 
7373  ui_->label_loopClosures->setText(tr("(%1, %2, %3, %4, %5, %6, %7, %8, %9)")
7374  .arg(totalNeighbor)
7375  .arg(totalNeighborMerged)
7376  .arg(totalGlobal)
7377  .arg(totalLocalSpace)
7378  .arg(totalLocalTime)
7379  .arg(totalUser)
7380  .arg(totalPriors)
7381  .arg(totalLandmarks)
7382  .arg(totalGravity));
7383 
7384  // remove intermediate nodes?
7385  if(ui_->checkBox_ignoreIntermediateNodes->isVisible() &&
7386  ui_->checkBox_ignoreIntermediateNodes->isChecked())
7387  {
7388  for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
7389  {
7390  if(iter->second.type() == Link::kNeighbor ||
7391  iter->second.type() == Link::kNeighborMerged)
7392  {
7393  Link link = iter->second;
7394  while(uContains(weights_, link.to()) && weights_.at(link.to()) < 0)
7395  {
7396  std::multimap<int, Link>::iterator uter = links.find(link.to());
7397  if(uter != links.end())
7398  {
7399  UASSERT(links.count(link.to()) == 1);
7400  poses.erase(link.to());
7401  link = link.merge(uter->second, uter->second.type());
7402  links.erase(uter);
7403  }
7404  else
7405  {
7406  break;
7407  }
7408  }
7409 
7410  iter->second = link;
7411  }
7412  }
7413  }
7414 
7415  graphes_.push_back(poses);
7416 
7417  Optimizer * optimizer = Optimizer::create(parameters);
7418 
7419  std::map<int, rtabmap::Transform> posesOut;
7420  std::multimap<int, rtabmap::Link> linksOut;
7421  UINFO("Get connected graph from %d (%d poses, %d links)", fromId, (int)poses.size(), (int)links.size());
7422  optimizer->getConnectedGraph(
7423  fromId,
7424  poses,
7425  links,
7426  posesOut,
7427  linksOut);
7428  UINFO("Connected graph of %d poses and %d links", (int)posesOut.size(), (int)linksOut.size());
7429  QTime time;
7430  time.start();
7431  std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(fromId, posesOut, linksOut, ui_->checkBox_iterativeOptimization->isChecked()?&graphes_:0);
7432  ui_->label_timeOptimization->setNum(double(time.elapsed())/1000.0);
7433  graphLinks_ = linksOut;
7434  if(posesOut.size() && finalPoses.empty())
7435  {
7436  UWARN("Optimization failed... (poses=%d, links=%d).", (int)posesOut.size(), (int)linksOut.size());
7437  if(!optimizer->isCovarianceIgnored() || optimizer->type() != Optimizer::kTypeTORO)
7438  {
7439  QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors. "
7440  "Give it a try with %1=0 and %2=true.").arg(Parameters::kOptimizerStrategy().c_str()).arg(Parameters::kOptimizerVarianceIgnored().c_str()));
7441  }
7442  else
7443  {
7444  QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors."));
7445  }
7446  }
7447  ui_->label_poses->setNum((int)finalPoses.size());
7448  graphes_.push_back(finalPoses);
7449  delete optimizer;
7450  }
7451  if(graphes_.size())
7452  {
7453  if(ui_->doubleSpinBox_optimizationScale->value()!=-1.0)
7454  {
7455  // scale all poses
7456  for(std::list<std::map<int, Transform> >::iterator iter=graphes_.begin(); iter!=graphes_.end(); ++iter)
7457  {
7458  for(std::map<int, Transform>::iterator jter=iter->begin(); jter!=iter->end(); ++jter)
7459  {
7460  jter->second = jter->second.clone();
7461  jter->second.x() *= ui_->doubleSpinBox_optimizationScale->value();
7462  jter->second.y() *= ui_->doubleSpinBox_optimizationScale->value();
7463  jter->second.z() *= ui_->doubleSpinBox_optimizationScale->value();
7464  }
7465  }
7466  }
7467 
7468  ui_->horizontalSlider_iterations->setMaximum((int)graphes_.size()-1);
7469  ui_->horizontalSlider_iterations->setValue((int)graphes_.size()-1);
7470  ui_->horizontalSlider_iterations->setEnabled(true);
7471  ui_->spinBox_optimizationsFrom->setEnabled(true);
7472  sliderIterationsValueChanged((int)graphes_.size()-1);
7473  }
7474  else
7475  {
7476  ui_->horizontalSlider_iterations->setEnabled(false);
7477  ui_->spinBox_optimizationsFrom->setEnabled(false);
7478  }
7479 }
7480 
7481 void DatabaseViewer::updateGrid()
7482 {
7483  if(sender() == ui_->checkBox_grid_2d && !ui_->checkBox_grid_2d->isChecked())
7484  {
7485  //just remove map in occupancy grid view
7486  occupancyGridViewer_->removeOccupancyGridMap();
7487  occupancyGridViewer_->refreshView();
7488  }
7489  else
7490  {
7491  ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked());
7492  ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
7493  ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7494  ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
7495  ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
7496  ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7497 
7498  update3dView();
7499  updateGraphView();
7500  }
7501 }
7502 
7503 void DatabaseViewer::updateOctomapView()
7504 {
7505 #ifdef RTABMAP_OCTOMAP
7506  ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked());
7507  ui_->spinBox_grid_depth->setVisible(ui_->checkBox_octomap->isChecked());
7508  ui_->checkBox_grid_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7509  ui_->label_octomap_cubes->setVisible(ui_->checkBox_octomap->isChecked());
7510  ui_->label_octomap_depth->setVisible(ui_->checkBox_octomap->isChecked());
7511  ui_->label_octomap_empty->setVisible(!ui_->checkBox_octomap->isChecked() || ui_->comboBox_octomap_rendering_type->currentIndex()==0);
7512 
7513  if(ui_->checkBox_octomap->isChecked())
7514  {
7515  if(octomap_)
7516  {
7517  occupancyGridViewer_->removeOctomap();
7518  occupancyGridViewer_->removeCloud("octomap_obstacles");
7519  occupancyGridViewer_->removeCloud("octomap_empty");
7520  if(ui_->comboBox_octomap_rendering_type->currentIndex()>0)
7521  {
7522  occupancyGridViewer_->addOctomap(octomap_, ui_->spinBox_grid_depth->value(), ui_->comboBox_octomap_rendering_type->currentIndex()>1);
7523  }
7524  else
7525  {
7526  pcl::IndicesPtr obstacles(new std::vector<int>);
7527  pcl::IndicesPtr empty(new std::vector<int>);
7528  pcl::IndicesPtr ground(new std::vector<int>);
7529  std::vector<double> prob;
7530  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = octomap_->createCloud(
7531  ui_->spinBox_grid_depth->value(),
7532  obstacles.get(),
7533  empty.get(),
7534  ground.get(),
7535  true,
7536  0,
7537  &prob);
7538 
7539  if(octomap_->hasColor())
7540  {
7541  pcl::PointCloud<pcl::PointXYZRGB>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
7542  pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
7543  occupancyGridViewer_->addCloud("octomap_obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
7544  occupancyGridViewer_->setCloudPointSize("octomap_obstacles", 5);
7545 
7546  pcl::PointCloud<pcl::PointXYZRGB>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
7547  pcl::copyPointCloud(*cloud, *ground, *groundCloud);
7548  occupancyGridViewer_->addCloud("octomap_ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
7549  occupancyGridViewer_->setCloudPointSize("octomap_ground", 5);
7550  }
7551  else
7552  {
7553  pcl::PointCloud<pcl::PointXYZ>::Ptr obstaclesCloud(new pcl::PointCloud<pcl::PointXYZ>);
7554  pcl::copyPointCloud(*cloud, *obstacles, *obstaclesCloud);
7555  occupancyGridViewer_->addCloud("octomap_obstacles", obstaclesCloud, Transform::getIdentity(), QColor(ui_->lineEdit_obstacleColor->text()));
7556  occupancyGridViewer_->setCloudPointSize("octomap_obstacles", 5);
7557 
7558  pcl::PointCloud<pcl::PointXYZ>::Ptr groundCloud(new pcl::PointCloud<pcl::PointXYZ>);
7559  pcl::copyPointCloud(*cloud, *ground, *groundCloud);
7560  occupancyGridViewer_->addCloud("octomap_ground", groundCloud, Transform::getIdentity(), QColor(ui_->lineEdit_groundColor->text()));
7561  occupancyGridViewer_->setCloudPointSize("octomap_ground", 5);
7562  }
7563 
7564  if(ui_->checkBox_grid_empty->isChecked())
7565  {
7566  if(prob.size()==cloud->size())
7567  {
7568  float occThr = Parameters::defaultGridGlobalOccupancyThr();
7569  Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kGridGlobalOccupancyThr(), occThr);
7570  pcl::PointCloud<pcl::PointXYZRGB>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
7571  emptyCloud->resize(empty->size());
7572  for(unsigned int i=0;i<empty->size(); ++i)
7573  {
7574  emptyCloud->points.at(i).x = cloud->points.at(empty->at(i)).x;
7575  emptyCloud->points.at(i).y = cloud->points.at(empty->at(i)).y;
7576  emptyCloud->points.at(i).z = cloud->points.at(empty->at(i)).z;
7577  QColor hsv = QColor::fromHsv(int(prob.at(empty->at(i))/occThr*240.0), 255, 255, 255);
7578  QRgb color = hsv.rgb();
7579  emptyCloud->points.at(i).r = qRed(color);
7580  emptyCloud->points.at(i).g = qGreen(color);
7581  emptyCloud->points.at(i).b = qBlue(color);
7582  }
7583  occupancyGridViewer_->addCloud("octomap_empty", emptyCloud, Transform::getIdentity(), QColor(ui_->lineEdit_emptyColor->text()));
7584  occupancyGridViewer_->setCloudOpacity("octomap_empty", 0.5);
7585  occupancyGridViewer_->setCloudPointSize("octomap_empty", 5);
7586  }
7587  else
7588  {
7589  pcl::PointCloud<pcl::PointXYZ>::Ptr emptyCloud(new pcl::PointCloud<pcl::PointXYZ>);
7590  pcl::copyPointCloud(*cloud, *empty, *emptyCloud);
7591  occupancyGridViewer_->addCloud("octomap_empty", emptyCloud, Transform::getIdentity(), QColor(ui_->lineEdit_emptyColor->text()));
7592  occupancyGridViewer_->setCloudOpacity("octomap_empty", 0.5);
7593  occupancyGridViewer_->setCloudPointSize("octomap_empty", 5);
7594  }
7595  }
7596  }
7597  occupancyGridViewer_->refreshView();
7598  }
7599  if(ui_->dockWidget_view3d->isVisible() && ui_->checkBox_showGrid->isChecked())
7600  {
7601  this->update3dView();
7602  }
7603  }
7604 #endif
7605 }
7606 
7607 Link DatabaseViewer::findActiveLink(int from, int to)
7608 {
7609  Link link;
7610  std::multimap<int, Link>::iterator findIter = rtabmap::graph::findLink(linksRefined_, from ,to);
7611  if(findIter != linksRefined_.end())
7612  {
7613  link = findIter->second;
7614  }
7615  else
7616  {
7617  findIter = rtabmap::graph::findLink(linksAdded_, from ,to);
7618  if(findIter != linksAdded_.end())
7619  {
7620  link = findIter->second;
7621  }
7622  else if(!containsLink(linksRemoved_, from ,to))
7623  {
7624  findIter = rtabmap::graph::findLink(links_, from ,to);
7625  if(findIter != links_.end())
7626  {
7627  link = findIter->second;
7628  }
7629  }
7630  }
7631  return link;
7632 }
7633 
7634 bool DatabaseViewer::containsLink(std::multimap<int, Link> & links, int from, int to)
7635 {
7636  return rtabmap::graph::findLink(links, from, to) != links.end();
7637 }
7638 
7639 void DatabaseViewer::refineConstraint()
7640 {
7641  int from = ids_.at(ui_->horizontalSlider_A->value());
7642  int to = ids_.at(ui_->horizontalSlider_B->value());
7643  refineConstraint(from, to, false);
7644 }
7645 
7646 void DatabaseViewer::refineConstraint(int from, int to, bool silent)
7647 {
7648  UDEBUG("%d -> %d", from, to);
7649  bool switchedIds = false;
7650  if(from == to)
7651  {
7652  UWARN("Cannot refine link to same node");
7653  return;
7654  }
7655 
7656 
7657  Link currentLink = findActiveLink(from, to);
7658  if(!currentLink.isValid())
7659  {
7660  UERROR("Not found link! (%d->%d)", from, to);
7661  return;
7662  }
7663 
7664  UDEBUG("%d -> %d (type=%d)", currentLink.from(), currentLink.to(), currentLink.type());
7665  Transform t = currentLink.transform();
7666  if(ui_->checkBox_showOptimized->isChecked() &&
7667  (currentLink.type() == Link::kNeighbor || currentLink.type() == Link::kNeighborMerged) &&
7668  graphes_.size() &&
7669  (int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
7670  {
7671  std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
7672  if(currentLink.type() == Link::kNeighbor || currentLink.type() == Link::kNeighborMerged)
7673  {
7674  std::map<int, rtabmap::Transform>::iterator iterFrom = graph.find(currentLink.from());
7675  std::map<int, rtabmap::Transform>::iterator iterTo = graph.find(currentLink.to());
7676  if(iterFrom != graph.end() && iterTo != graph.end())
7677  {
7678  Transform topt = iterFrom->second.inverse()*iterTo->second;
7679  t = topt;
7680  }
7681  }
7682  }
7683  else if(ui_->checkBox_ignorePoseCorrection->isChecked() &&
7684  graph::findLink(linksRefined_, currentLink.from(), currentLink.to()) == linksRefined_.end())
7685  {
7686  if(currentLink.type() == Link::kNeighbor ||
7687  currentLink.type() == Link::kNeighborMerged)
7688  {
7689  Transform poseFrom = uValue(odomPoses_, currentLink.from(), Transform());
7690  Transform poseTo = uValue(odomPoses_, currentLink.to(), Transform());
7691  if(!poseFrom.isNull() && !poseTo.isNull())
7692  {
7693  t = poseFrom.inverse() * poseTo; // recompute raw odom transformation
7694  }
7695  }
7696  }
7697 
7698  Transform transform;
7699  RegistrationInfo info;
7700  Signature * fromS = 0;
7701  Signature * toS = 0;
7702 
7703  fromS = dbDriver_->loadSignature(currentLink.from());
7704  if(fromS == 0)
7705  {
7706  UERROR("Signature %d not found!", currentLink.from());
7707  return;
7708  }
7709 
7710  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
7711 
7712  UTimer timer;
7713 
7714  // Is it a multi-scan proximity detection?
7715  cv::Mat userData = currentLink.uncompressUserDataConst();
7716  std::map<int, rtabmap::Transform> scanPoses;
7717 
7718  if(currentLink.type() == Link::kLocalSpaceClosure &&
7719  !currentLink.userDataCompressed().empty() &&
7720  userData.type() == CV_8SC1 &&
7721  userData.rows == 1 &&
7722  userData.cols >= 8 && // including null str ending
7723  userData.at<char>(userData.cols-1) == 0 &&
7724  memcmp(userData.data, "SCANS:", 6) == 0 &&
7725  currentLink.from() > currentLink.to())
7726  {
7727  std::string scansStr = (const char *)userData.data;
7728  UINFO("Detected \"%s\" in links's user data", scansStr.c_str());
7729  if(!scansStr.empty())
7730  {
7731  std::list<std::string> strs = uSplit(scansStr, ':');
7732  if(strs.size() == 2)
7733  {
7734  std::list<std::string> strIds = uSplit(strs.rbegin()->c_str(), ';');
7735  for(std::list<std::string>::iterator iter=strIds.begin(); iter!=strIds.end(); ++iter)
7736  {
7737  int id = atoi(iter->c_str());
7738  if(uContains(odomPoses_, id))
7739  {
7740  scanPoses.insert(*odomPoses_.find(id));
7741  }
7742  else
7743  {
7744  UERROR("Not found %d node!", id);
7745  }
7746  }
7747  }
7748  }
7749  }
7750  if(scanPoses.size()>1)
7751  {
7752  //optimize the path's poses locally
7753  Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
7754 
7755  UASSERT(uContains(scanPoses, currentLink.to()));
7756  std::map<int, rtabmap::Transform> posesOut;
7757  std::multimap<int, rtabmap::Link> linksOut;
7758  optimizer->getConnectedGraph(
7759  currentLink.to(),
7760  scanPoses,
7761  updateLinksWithModifications(links_),
7762  posesOut,
7763  linksOut);
7764 
7765  if(scanPoses.size() != posesOut.size())
7766  {
7767  UWARN("Scan poses input and output are different! %d vs %d", (int)scanPoses.size(), (int)posesOut.size());
7768  UWARN("Input poses: ");
7769  for(std::map<int, Transform>::iterator iter=scanPoses.begin(); iter!=scanPoses.end(); ++iter)
7770  {
7771  UWARN(" %d", iter->first);
7772  }
7773  UWARN("Input links: ");
7774  std::multimap<int, Link> modifiedLinks = updateLinksWithModifications(links_);
7775  for(std::multimap<int, Link>::iterator iter=modifiedLinks.begin(); iter!=modifiedLinks.end(); ++iter)
7776  {
7777  UWARN(" %d->%d", iter->second.from(), iter->second.to());
7778  }
7779  }
7780 
7781  scanPoses = optimizer->optimize(currentLink.to(), posesOut, linksOut);
7782  delete optimizer;
7783 
7784  std::map<int, Transform> filteredScanPoses = scanPoses;
7785  float proximityFilteringRadius = 0.0f;
7786  Parameters::parse(parameters, Parameters::kRGBDProximityPathFilteringRadius(), proximityFilteringRadius);
7787  if(scanPoses.size() > 2 && proximityFilteringRadius > 0.0f)
7788  {
7789  // path filtering
7790  filteredScanPoses = graph::radiusPosesFiltering(scanPoses, proximityFilteringRadius, 0, true);
7791  // make sure the current pose is still here
7792  filteredScanPoses.insert(*scanPoses.find(currentLink.to()));
7793  }
7794 
7795  Transform toPoseInv = filteredScanPoses.at(currentLink.to()).inverse();
7796  dbDriver_->loadNodeData(fromS, !silent, true, !silent, !silent);
7797  fromS->sensorData().uncompressData();
7798  LaserScan fromScan = fromS->sensorData().laserScanRaw();
7799  int maxPoints = fromScan.size();
7800  if(maxPoints == 0)
7801  {
7802  UWARN("From scan %d is empty!", fromS->id());
7803  }
7804  pcl::PointCloud<pcl::PointXYZ>::Ptr assembledToClouds(new pcl::PointCloud<pcl::PointXYZ>);
7805  pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(new pcl::PointCloud<pcl::PointNormal>);
7806  pcl::PointCloud<pcl::PointXYZI>::Ptr assembledToIClouds(new pcl::PointCloud<pcl::PointXYZI>);
7807  pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembledToNormalIClouds(new pcl::PointCloud<pcl::PointXYZINormal>);
7808  pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledToRGBClouds(new pcl::PointCloud<pcl::PointXYZRGB>);
7809  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledToNormalRGBClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
7810  for(std::map<int, Transform>::const_iterator iter = filteredScanPoses.begin(); iter!=filteredScanPoses.end(); ++iter)
7811  {
7812  if(iter->first != currentLink.from())
7813  {
7814  SensorData data;
7815  dbDriver_->getNodeData(iter->first, data);
7816  if(!data.laserScanCompressed().isEmpty())
7817  {
7818  LaserScan scan;
7819  data.uncompressData(0, 0, &scan);
7820  if(!scan.isEmpty() && fromScan.format() == scan.format())
7821  {
7822  if(scan.hasIntensity())
7823  {
7824  if(scan.hasNormals())
7825  {
7826  *assembledToNormalIClouds += *util3d::laserScanToPointCloudINormal(scan,
7827  toPoseInv * iter->second * scan.localTransform());
7828  }
7829  else
7830  {
7831  *assembledToIClouds += *util3d::laserScanToPointCloudI(scan,
7832  toPoseInv * iter->second * scan.localTransform());
7833  }
7834  }
7835  else if(scan.hasRGB())
7836  {
7837  if(scan.hasNormals())
7838  {
7839  *assembledToNormalRGBClouds += *util3d::laserScanToPointCloudRGBNormal(scan,
7840  toPoseInv * iter->second * scan.localTransform());
7841  }
7842  else
7843  {
7844  *assembledToRGBClouds += *util3d::laserScanToPointCloudRGB(scan,
7845  toPoseInv * iter->second * scan.localTransform());
7846  }
7847  }
7848  else
7849  {
7850  if(scan.hasNormals())
7851  {
7852  *assembledToNormalClouds += *util3d::laserScanToPointCloudNormal(scan,
7853  toPoseInv * iter->second * scan.localTransform());
7854  }
7855  else
7856  {
7857  *assembledToClouds += *util3d::laserScanToPointCloud(scan,
7858  toPoseInv * iter->second * scan.localTransform());
7859  }
7860  }
7861 
7862  if(scan.size() > maxPoints)
7863  {
7864  maxPoints = scan.size();
7865  }
7866  }
7867  else
7868  {
7869  UWARN("scan format of %d is not the same than from scan %d: %d vs %d", data.id(), fromS->id(), scan.format(), fromScan.format());
7870  }
7871  }
7872  else
7873  {
7874  UWARN("Laser scan not found for signature %d", iter->first);
7875  }
7876  }
7877  }
7878 
7879  LaserScan assembledScan;
7880  if(assembledToNormalClouds->size())
7881  {
7882  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToNormalClouds):util3d::laserScanFromPointCloud(*assembledToNormalClouds);
7883  }
7884  else if(assembledToClouds->size())
7885  {
7886  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToClouds):util3d::laserScanFromPointCloud(*assembledToClouds);
7887  }
7888  else if(assembledToNormalIClouds->size())
7889  {
7890  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToNormalIClouds):util3d::laserScanFromPointCloud(*assembledToNormalIClouds);
7891  }
7892  else if(assembledToIClouds->size())
7893  {
7894  assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToIClouds):util3d::laserScanFromPointCloud(*assembledToIClouds);
7895  }
7896  else if(assembledToNormalRGBClouds->size())
7897  {
7898  UASSERT(!fromScan.is2d());
7899  assembledScan = util3d::laserScanFromPointCloud(*assembledToNormalRGBClouds);
7900  }
7901  else if(assembledToRGBClouds->size())
7902  {
7903  UASSERT(!fromScan.is2d());
7904  assembledScan = util3d::laserScanFromPointCloud(*assembledToRGBClouds);
7905  }
7906  else
7907  {
7908  UWARN("Assembled scan is empty!");
7909  }
7910  SensorData assembledData(cv::Mat(), to);
7911  // scans are in base frame but for 2d scans, set the height so that correspondences matching works
7912  assembledData.setLaserScan(LaserScan(
7913  assembledScan,
7914  fromScan.maxPoints()?fromScan.maxPoints():maxPoints,
7915  fromScan.rangeMax(),
7916  fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
7917 
7918  toS = new Signature(assembledData);
7919  RegistrationIcp registrationIcp(parameters);
7920  transform = registrationIcp.computeTransformationMod(*fromS, *toS, currentLink.transform(), &info);
7921  if(!transform.isNull())
7922  {
7923  // local scan matching proximity detection should have higher variance (see Rtabmap::process())
7924  info.covariance*=100.0;
7925  }
7926  }
7927  else
7928  {
7929  toS = dbDriver_->loadSignature(currentLink.to());
7930  if(toS == 0)
7931  {
7932  UERROR("Signature %d not found!", currentLink.to());
7933  delete fromS;
7934  return;
7935  }
7936 
7937  bool reextractVisualFeatures = uStr2Bool(parameters.at(Parameters::kRGBDLoopClosureReextractFeatures()));
7938  Registration * reg = Registration::create(parameters);
7939  if( reg->isScanRequired() ||
7940  reg->isUserDataRequired() ||
7941  reextractVisualFeatures ||
7942  !silent)
7943  {
7944  dbDriver_->loadNodeData(fromS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
7945  dbDriver_->loadNodeData(toS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
7946 
7947  if(!silent)
7948  {
7949  fromS->sensorData().uncompressData();
7950  toS->sensorData().uncompressData();
7951  }
7952  }
7953 
7954  if(reextractVisualFeatures)
7955  {
7956  fromS->removeAllWords();
7957  fromS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
7958  toS->removeAllWords();
7959  toS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
7960  }
7961 
7962  if(reg->isScanRequired())
7963  {
7964  if(ui_->checkBox_icp_from_depth->isChecked())
7965  {
7966  // generate laser scans from depth image
7967  cv::Mat tmpA, tmpB, tmpC, tmpD;
7968  fromS->sensorData().uncompressData(&tmpA, &tmpB, 0);
7969  toS->sensorData().uncompressData(&tmpC, &tmpD, 0);
7970  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom = util3d::cloudFromSensorData(
7971  fromS->sensorData(),
7972  ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
7973  ui_->doubleSpinBox_icp_maxDepth->value(),
7974  ui_->doubleSpinBox_icp_minDepth->value(),
7975  0,
7976  ui_->parameters_toolbox->getParameters());
7977  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo = util3d::cloudFromSensorData(
7978  toS->sensorData(),
7979  ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
7980  ui_->doubleSpinBox_icp_maxDepth->value(),
7981  ui_->doubleSpinBox_icp_minDepth->value(),
7982  0,
7983  ui_->parameters_toolbox->getParameters());
7984 
7985  if(cloudFrom->empty() && cloudTo->empty())
7986  {
7987  std::string msg = "Option to generate scan from depth is checked (GUI Parameters->Refine), but "
7988  "resulting clouds from depth are empty. Transformation estimation will likely "
7989  "fails. Uncheck the parameter to use laser scans.";
7990  UWARN(msg.c_str());
7991  if(!silent)
7992  {
7993  QMessageBox::warning(this,
7994  tr("Refine link"),
7995  tr("%1").arg(msg.c_str()));
7996  }
7997  }
7998  else if(!fromS->sensorData().laserScanCompressed().isEmpty() || !toS->sensorData().laserScanCompressed().isEmpty())
7999  {
8000  UWARN("There are laser scans in data, but generate laser scan from "
8001  "depth image option is activated (GUI Parameters->Refine). "
8002  "Ignoring saved laser scans...");
8003  }
8004 
8005  int maxLaserScans = cloudFrom->size();
8006  fromS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0));
8007  toS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0));
8008  }
8009  else
8010  {
8011  LaserScan tmpA, tmpB;
8012  fromS->sensorData().uncompressData(0, 0, &tmpA);
8013  toS->sensorData().uncompressData(0, 0, &tmpB);
8014  }
8015  }
8016 
8017  if(reg->isImageRequired() && reextractVisualFeatures)
8018  {
8019  cv::Mat tmpA, tmpB, tmpC, tmpD;
8020  fromS->sensorData().uncompressData(&tmpA, &tmpB, 0);
8021  toS->sensorData().uncompressData(&tmpC, &tmpD, 0);
8022  }
8023 
8024  UINFO("Uncompress time: %f s", timer.ticks());
8025 
8026  if(fromS->id() < toS->id())
8027  {
8028  transform = reg->computeTransformationMod(*fromS, *toS, t, &info);
8029  }
8030  else
8031  {
8032  transform = reg->computeTransformationMod(*toS, *fromS, t.isNull()?t:t.inverse(), &info);
8033  switchedIds = true;
8034  }
8035 
8036  delete reg;
8037  }
8038  UINFO("(%d ->%d) Registration time: %f s", currentLink.from(), currentLink.to(), timer.ticks());
8039 
8040  if(!transform.isNull())
8041  {
8042  if(!transform.isIdentity())
8043  {
8044  if(info.covariance.at<double>(0,0)<=0.0)
8045  {
8046  info.covariance = cv::Mat::eye(6,6,CV_64FC1)*0.0001; // epsilon if exact transform
8047  }
8048  }
8049  if(switchedIds)
8050  {
8051  transform = transform.inverse();
8052  }
8053 
8054  Link newLink(currentLink.from(), currentLink.to(), currentLink.type(), transform, info.covariance.inv(), currentLink.userDataCompressed());
8055 
8056  bool updated = false;
8057  std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
8058  while(iter != linksRefined_.end() && iter->first == currentLink.from())
8059  {
8060  if(iter->second.to() == currentLink.to() &&
8061  iter->second.type() == currentLink.type())
8062  {
8063  iter->second = newLink;
8064  updated = true;
8065  break;
8066  }
8067  ++iter;
8068  }
8069  if(!updated)
8070  {
8071  linksRefined_.insert(std::make_pair(newLink.from(), newLink));
8072  updated = true;
8073  }
8074 
8075  if(updated && !silent)
8076  {
8077  this->updateGraphView();
8078  }
8079 
8080  if(!silent && ui_->dockWidget_constraints->isVisible())
8081  {
8082  if(toS && fromS->id() > 0 && toS->id() > 0)
8083  {
8084  updateLoopClosuresSlider(fromS->id(), toS->id());
8085  std::multimap<int, cv::KeyPoint> keypointsFrom;
8086  std::multimap<int, cv::KeyPoint> keypointsTo;
8087  if(!fromS->getWordsKpts().empty())
8088  {
8089  for(std::map<int, int>::const_iterator iter=fromS->getWords().begin(); iter!=fromS->getWords().end(); ++iter)
8090  {
8091  keypointsFrom.insert(keypointsFrom.end(), std::make_pair(iter->first, fromS->getWordsKpts()[iter->second]));
8092  }
8093  }
8094  if(!toS->getWordsKpts().empty())
8095  {
8096  for(std::map<int, int>::const_iterator iter=toS->getWords().begin(); iter!=toS->getWords().end(); ++iter)
8097  {
8098  keypointsTo.insert(keypointsTo.end(), std::make_pair(iter->first, toS->getWordsKpts()[iter->second]));
8099  }
8100  }
8101  if(newLink.type() != Link::kNeighbor && fromS->id() < toS->id())
8102  {
8103  this->updateConstraintView(newLink.inverse(), true, *toS, *fromS);
8104  ui_->graphicsView_A->setFeatures(keypointsTo, toS->sensorData().depthRaw());
8105  ui_->graphicsView_B->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
8106  }
8107  else
8108  {
8109  this->updateConstraintView(newLink, true, *fromS, *toS);
8110  ui_->graphicsView_A->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
8111  ui_->graphicsView_B->setFeatures(keypointsTo, toS->sensorData().depthRaw());
8112  }
8113 
8114  updateWordsMatching(info.inliersIDs);
8115  }
8116  else
8117  {
8118  this->updateConstraintView();
8119  }
8120  }
8121  }
8122  else if(!silent)
8123  {
8124  if(toS && fromS->id() > 0 && toS->id() > 0)
8125  {
8126  // just update matches in the views
8127  std::multimap<int, cv::KeyPoint> keypointsFrom;
8128  std::multimap<int, cv::KeyPoint> keypointsTo;
8129  if(!fromS->getWordsKpts().empty())
8130  {
8131  for(std::map<int, int>::const_iterator iter=fromS->getWords().begin(); iter!=fromS->getWords().end(); ++iter)
8132  {
8133  keypointsFrom.insert(keypointsFrom.end(), std::make_pair(iter->first, fromS->getWordsKpts()[iter->second]));
8134  }
8135  }
8136  if(!toS->getWordsKpts().empty())
8137  {
8138  for(std::map<int, int>::const_iterator iter=toS->getWords().begin(); iter!=toS->getWords().end(); ++iter)
8139  {
8140  keypointsTo.insert(keypointsTo.end(), std::make_pair(iter->first, toS->getWordsKpts()[iter->second]));
8141  }
8142  }
8143  if(currentLink.type() != Link::kNeighbor && fromS->id() < toS->id())
8144  {
8145  ui_->graphicsView_A->setFeatures(keypointsTo, toS->sensorData().depthRaw());
8146  ui_->graphicsView_B->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
8147  }
8148  else
8149  {
8150  ui_->graphicsView_A->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
8151  ui_->graphicsView_B->setFeatures(keypointsTo, toS->sensorData().depthRaw());
8152  }
8153  updateWordsMatching(info.inliersIDs);
8154  }
8155 
8156  QMessageBox::warning(this,
8157  tr("Refine link"),
8158  tr("Cannot find a transformation between nodes %1 and %2: %3").arg(currentLink.from()).arg(currentLink.to()).arg(info.rejectedMsg.c_str()));
8159  }
8160  delete fromS;
8161  delete toS;
8162 }
8163 
8164 void DatabaseViewer::addConstraint()
8165 {
8166  int from = ids_.at(ui_->horizontalSlider_A->value());
8167  int to = ids_.at(ui_->horizontalSlider_B->value());
8168  addConstraint(from, to, false);
8169 }
8170 
8171 bool DatabaseViewer::addConstraint(int from, int to, bool silent)
8172 {
8173  bool switchedIds = false;
8174  if(from == to)
8175  {
8176  UWARN("Cannot add link to same node");
8177  return false;
8178  }
8179  else if(from > to)
8180  {
8181  int tmp = from;
8182  from = to;
8183  to = tmp;
8184  switchedIds = true;
8185  }
8186  std::list<Signature*> signatures;
8187  Signature * fromS=0;
8188  Signature * toS=0;
8189 
8190  Link newLink;
8191  RegistrationInfo info;
8192  if(!containsLink(linksAdded_, from, to) &&
8193  !containsLink(links_, from, to))
8194  {
8195  UASSERT(!containsLink(linksRemoved_, from, to));
8196  UASSERT(!containsLink(linksRefined_, from, to));
8197 
8198  ParametersMap parameters = ui_->parameters_toolbox->getParameters();
8199  Registration * reg = Registration::create(parameters);
8200 
8201  bool loopCovLimited = Parameters::defaultRGBDLoopCovLimited();
8202  Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), loopCovLimited);
8203  std::vector<double> odomMaxInf = odomMaxInf_;
8204  if(loopCovLimited && odomMaxInf_.empty())
8205  {
8206  odomMaxInf = graph::getMaxOdomInf(updateLinksWithModifications(links_));
8207  }
8208 
8209  Transform t;
8210 
8211  std::list<int> ids;
8212  ids.push_back(from);
8213  ids.push_back(to);
8214  dbDriver_->loadSignatures(ids, signatures);
8215  if(signatures.size() != 2)
8216  {
8217  for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
8218  {
8219  delete *iter;
8220  }
8221  return false;
8222  }
8223  fromS = *signatures.begin();
8224  toS = *signatures.rbegin();
8225 
8226  bool reextractVisualFeatures = uStr2Bool(parameters.at(Parameters::kRGBDLoopClosureReextractFeatures()));
8227  if(reg->isScanRequired() ||
8228  reg->isUserDataRequired() ||
8229  reextractVisualFeatures ||
8230  !silent)
8231  {
8232  // Add sensor data to generate features
8233  dbDriver_->loadNodeData(fromS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
8234  fromS->sensorData().uncompressData();
8235  dbDriver_->loadNodeData(toS, reextractVisualFeatures || !silent || (reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked()), reg->isScanRequired() || !silent, reg->isUserDataRequired() || !silent, !silent);
8236  toS->sensorData().uncompressData();
8237  if(reextractVisualFeatures)
8238  {
8239  fromS->removeAllWords();
8240  fromS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
8241  toS->removeAllWords();
8242  toS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
8243  }
8244  if(reg->isScanRequired() && ui_->checkBox_icp_from_depth->isChecked())
8245  {
8246  // generate laser scans from depth image
8247  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom = util3d::cloudFromSensorData(
8248  fromS->sensorData(),
8249  ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
8250  ui_->doubleSpinBox_icp_maxDepth->value(),
8251  ui_->doubleSpinBox_icp_minDepth->value(),
8252  0,
8253  ui_->parameters_toolbox->getParameters());
8254  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo = util3d::cloudFromSensorData(
8255  toS->sensorData(),
8256  ui_->spinBox_icp_decimation->value()==0?1:ui_->spinBox_icp_decimation->value(),
8257  ui_->doubleSpinBox_icp_maxDepth->value(),
8258  ui_->doubleSpinBox_icp_minDepth->value(),
8259  0,
8260  ui_->parameters_toolbox->getParameters());
8261 
8262  if(cloudFrom->empty() && cloudTo->empty())
8263  {
8264  std::string msg = "Option to generate scan from depth is checked (GUI Parameters->Refine), but "
8265  "resulting clouds from depth are empty. Transformation estimation will likely "
8266  "fails. Uncheck the parameter to use laser scans.";
8267  UWARN(msg.c_str());
8268  if(!silent)
8269  {
8270  QMessageBox::warning(this,
8271  tr("Add link"),
8272  tr("%1").arg(msg.c_str()));
8273  }
8274  }
8275  else if(!fromS->sensorData().laserScanCompressed().isEmpty() || !toS->sensorData().laserScanCompressed().isEmpty())
8276  {
8277  UWARN("There are laser scans in data, but generate laser scan from "
8278  "depth image option is activated (GUI Parameters->Refine). Ignoring saved laser scans...");
8279  }
8280 
8281  int maxLaserScans = cloudFrom->size();
8282  fromS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudFrom), Transform()), maxLaserScans, 0));
8283  toS->sensorData().setLaserScan(LaserScan(util3d::laserScanFromPointCloud(*util3d::removeNaNFromPointCloud(cloudTo), Transform()), maxLaserScans, 0));
8284  }
8285  }
8286  else if(!reextractVisualFeatures && fromS->getWords().empty() && toS->getWords().empty())
8287  {
8288  std::string msg = uFormat("\"%s\" is false and signatures (%d and %d) don't have words, "
8289  "registration will not be possible. Set \"%s\" to true.",
8290  Parameters::kRGBDLoopClosureReextractFeatures().c_str(),
8291  fromS->id(),
8292  toS->id(),
8293  Parameters::kRGBDLoopClosureReextractFeatures().c_str());
8294  UWARN(msg.c_str());
8295  if(!silent)
8296  {
8297  QMessageBox::warning(this,
8298  tr("Add link"),
8299  tr("%1").arg(msg.c_str()));
8300  }
8301  }
8302 
8303  Transform guess;
8304  bool guessFromGraphRejected = false;
8305  if(!reg->isImageRequired())
8306  {
8307  // make a fake guess using globally optimized poses
8308  if(graphes_.size())
8309  {
8310  std::map<int, Transform> optimizedPoses = graphes_.back();
8311  if(optimizedPoses.size() > 0)
8312  {
8313  std::map<int, Transform>::iterator fromIter = optimizedPoses.find(from);
8314  std::map<int, Transform>::iterator toIter = optimizedPoses.find(to);
8315  if(fromIter != optimizedPoses.end() &&
8316  toIter != optimizedPoses.end())
8317  {
8318  if(!silent)
8319  {
8320  if(QMessageBox::question(this,
8321  tr("Add constraint from optimized graph"),
8322  tr("Registration is done without vision (see %1 parameter), "
8323  "do you want to use a guess from the optimized graph?"
8324  ""
8325  "\n\nOtherwise, if "
8326  "the database has images, it is recommended to use %2=2 instead so that "
8327  "the guess can be found visually.")
8328  .arg(Parameters::kRegStrategy().c_str()).arg(Parameters::kRegStrategy().c_str()),
8329  QMessageBox::Yes | QMessageBox::No,
8330  QMessageBox::Yes) == QMessageBox::Yes)
8331  {
8332  guess = fromIter->second.inverse() * toIter->second;
8333  }
8334  else
8335  {
8336  guessFromGraphRejected = true;
8337  }
8338  }
8339  else
8340  {
8341  guess = fromIter->second.inverse() * toIter->second;
8342  }
8343  }
8344  }
8345  }
8346  if(guess.isNull() && !silent && !guessFromGraphRejected)
8347  {
8348  if(QMessageBox::question(this,
8349  tr("Add constraint without guess"),
8350  tr("Registration is done without vision (see %1 parameter) and we cannot (or we don't want to) find relative "
8351  "transformation between the nodes with the current graph. Do you want to use an identity "
8352  "transform for ICP guess? "
8353  ""
8354  "\n\nOtherwise, if the database has images, it is recommended to use %2=2 "
8355  "instead so that the guess can be found visually.")
8356  .arg(Parameters::kRegStrategy().c_str()).arg(Parameters::kRegStrategy().c_str()),
8357  QMessageBox::Yes | QMessageBox::Abort,
8358  QMessageBox::Abort) == QMessageBox::Yes)
8359  {
8360  guess.setIdentity();
8361  }
8362  else
8363  {
8364  guessFromGraphRejected = true;
8365  }
8366  }
8367  }
8368 
8369  if(switchedIds)
8370  {
8371  t = reg->computeTransformationMod(*toS, *fromS, guess.isNull()?guess:guess.inverse(), &info);
8372  if(!t.isNull())
8373  {
8374  t = t.inverse();
8375  }
8376  }
8377  else
8378  {
8379  t = reg->computeTransformationMod(*fromS, *toS, guess, &info);
8380  }
8381  delete reg;
8382  UDEBUG("");
8383 
8384  if(!t.isNull())
8385  {
8386  cv::Mat information = info.covariance.inv();
8387  if(odomMaxInf.size() == 6 && information.cols==6 && information.rows==6)
8388  {
8389  for(int i=0; i<6; ++i)
8390  {
8391  if(information.at<double>(i,i) > odomMaxInf[i])
8392  {
8393  information.at<double>(i,i) = odomMaxInf[i];
8394  }
8395  }
8396  }
8397 
8398  newLink = Link(from, to, Link::kUserClosure, t, information);
8399  }
8400  else if(!silent && !guessFromGraphRejected)
8401  {
8402  QMessageBox::StandardButton button = QMessageBox::warning(this,
8403  tr("Add link"),
8404  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()),
8405  QMessageBox::Yes | QMessageBox::No,
8406  QMessageBox::No);
8407  if(button == QMessageBox::Yes)
8408  {
8409  editConstraint();
8410  silent = true;
8411  }
8412  }
8413  }
8414  else if(containsLink(linksRemoved_, from, to))
8415  {
8416  newLink = rtabmap::graph::findLink(linksRemoved_, from, to)->second;
8417  }
8418 
8419  bool updateConstraints = newLink.isValid();
8420  float maxOptimizationError = uStr2Float(ui_->parameters_toolbox->getParameters().at(Parameters::kRGBDOptimizeMaxError()));
8421  if(newLink.isValid() &&
8422  maxOptimizationError > 0.0f &&
8423  uStr2Int(ui_->parameters_toolbox->getParameters().at(Parameters::kOptimizerIterations())) > 0.0f)
8424  {
8425  int fromId = newLink.from();
8426  std::multimap<int, Link> linksIn = updateLinksWithModifications(links_);
8427  linksIn.insert(std::make_pair(newLink.from(), newLink));
8428  const Link * maxLinearLink = 0;
8429  const Link * maxAngularLink = 0;
8430  float maxLinearErrorRatio = 0.0f;
8431  float maxAngularErrorRatio = 0.0f;
8432  Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());
8433  std::map<int, Transform> poses;
8434  std::multimap<int, Link> links;
8435  UASSERT(odomPoses_.find(fromId) != odomPoses_.end());
8436  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());
8437  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());
8438  optimizer->getConnectedGraph(fromId, odomPoses_, linksIn, poses, links);
8439  // use already optimized poses
8440  if(graphes_.size())
8441  {
8442  const std::map<int, Transform> & optimizedPoses = graphes_.back();
8443  for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
8444  {
8445  if(optimizedPoses.find(iter->first) != optimizedPoses.end())
8446  {
8447  iter->second = optimizedPoses.at(iter->first);
8448  }
8449  }
8450  }
8451  UASSERT(poses.find(fromId) != poses.end());
8452  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());
8453  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());
8454  UASSERT(graph::findLink(links, newLink.from(), newLink.to()) != links.end());
8455  std::map<int, Transform> posesIn = poses;
8456  poses = optimizer->optimize(fromId, posesIn, links);
8457  if(posesIn.size() && poses.empty())
8458  {
8459  UWARN("Optimization failed... (poses=%d, links=%d).", (int)posesIn.size(), (int)links.size());
8460  }
8461  std::string msg;
8462  if(poses.size())
8463  {
8464  float maxLinearError = 0.0f;
8465  float maxAngularError = 0.0f;
8466  graph::computeMaxGraphErrors(
8467  poses,
8468  links,
8469  maxLinearErrorRatio,
8470  maxAngularErrorRatio,
8471  maxLinearError,
8472  maxAngularError,
8473  &maxLinearLink,
8474  &maxAngularLink);
8475  if(maxLinearLink)
8476  {
8477  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()));
8478  if(maxLinearErrorRatio > maxOptimizationError)
8479  {
8480  msg = uFormat("Rejecting edge %d->%d because "
8481  "graph error is too large (abs=%f m) after optimization (ratio %f for edge %d->%d, stddev=%f m). "
8482  "\"%s\" is %f.",
8483  newLink.from(),
8484  newLink.to(),
8485  maxLinearError,
8486  maxLinearErrorRatio,
8487  maxLinearLink->from(),
8488  maxLinearLink->to(),
8489  sqrt(maxLinearLink->transVariance()),
8490  Parameters::kRGBDOptimizeMaxError().c_str(),
8491  maxOptimizationError);
8492  }
8493  }
8494  if(maxAngularLink)
8495  {
8496  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()));
8497  if(maxAngularErrorRatio > maxOptimizationError)
8498  {
8499  msg = uFormat("Rejecting edge %d->%d because "
8500  "graph error is too large (abs=%f deg) after optimization (ratio %f for edge %d->%d, stddev=%f deg). "
8501  "\"%s\" is %f.",
8502  newLink.from(),
8503  newLink.to(),
8504  maxAngularError*180.0f/CV_PI,
8505  maxAngularErrorRatio,
8506  maxAngularLink->from(),
8507  maxAngularLink->to(),
8508  sqrt(maxAngularLink->rotVariance()),
8509  Parameters::kRGBDOptimizeMaxError().c_str(),
8510  maxOptimizationError);
8511  }
8512  }
8513  }
8514  else
8515  {
8516  msg = uFormat("Rejecting edge %d->%d because graph optimization has failed!",
8517  newLink.from(),
8518  newLink.to());
8519  }
8520  if(!msg.empty())
8521  {
8522  UWARN("%s", msg.c_str());
8523  if(!silent)
8524  {
8525  QMessageBox::warning(this,
8526  tr("Add link"),
8527  tr("%1").arg(msg.c_str()));
8528  }
8529 
8530  updateConstraints = false;
8531  }
8532 
8533  if(updateConstraints && silent && !graphes_.empty() && graphes_.back().size() == poses.size())
8534  {
8535  graphes_.back() = poses;
8536  }
8537  }
8538 
8539  if(updateConstraints)
8540  {
8541  if(containsLink(linksRemoved_, from, to))
8542  {
8543  //simply remove from linksRemoved
8544  linksRemoved_.erase(rtabmap::graph::findLink(linksRemoved_, from, to));
8545  }
8546  else
8547  {
8548  if(newLink.from() < newLink.to())
8549  {
8550  newLink = newLink.inverse();
8551  }
8552  linksAdded_.insert(std::make_pair(newLink.from(), newLink));
8553  }
8554  }
8555 
8556  if(!silent)
8557  {
8558  if(fromS && toS)
8559  {
8560  if((updateConstraints && newLink.from() > newLink.to()) || (!updateConstraints && switchedIds))
8561  {
8562  Signature * tmpS = fromS;
8563  fromS = toS;
8564  toS = tmpS;
8565  }
8566 
8567  if(updateConstraints)
8568  {
8569  updateLoopClosuresSlider(fromS->id(), toS->id());
8570  this->updateGraphView();
8571  this->updateConstraintView(newLink, false, *fromS, *toS);
8572  }
8573 
8574  std::multimap<int, cv::KeyPoint> keypointsFrom;
8575  std::multimap<int, cv::KeyPoint> keypointsTo;
8576  if(!fromS->getWordsKpts().empty())
8577  {
8578  for(std::map<int, int>::const_iterator iter=fromS->getWords().begin(); iter!=fromS->getWords().end(); ++iter)
8579  {
8580  keypointsFrom.insert(keypointsFrom.end(), std::make_pair(iter->first, fromS->getWordsKpts()[iter->second]));
8581  }
8582  }
8583  if(!toS->getWordsKpts().empty())
8584  {
8585  for(std::map<int, int>::const_iterator iter=toS->getWords().begin(); iter!=toS->getWords().end(); ++iter)
8586  {
8587  keypointsTo.insert(keypointsTo.end(), std::make_pair(iter->first, toS->getWordsKpts()[iter->second]));
8588  }
8589  }
8590  ui_->graphicsView_A->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
8591  ui_->graphicsView_B->setFeatures(keypointsTo, toS->sensorData().depthRaw());
8592  updateWordsMatching(info.inliersIDs);
8593  }
8594  else if(updateConstraints)
8595  {
8596  updateLoopClosuresSlider(from, to);
8597  this->updateGraphView();
8598  }
8599  }
8600 
8601  for(std::list<Signature*>::iterator iter=signatures.begin(); iter!=signatures.end(); ++iter)
8602  {
8603  delete *iter;
8604  }
8605 
8606  return updateConstraints;
8607 }
8608 
8609 void DatabaseViewer::resetConstraint()
8610 {
8611  int from = ids_.at(ui_->horizontalSlider_A->value());
8612  int to = ids_.at(ui_->horizontalSlider_B->value());
8613  if(ui_->label_type->text().toInt() == Link::kLandmark)
8614  {
8615  int position = ui_->horizontalSlider_loops->value();
8616  const rtabmap::Link & link = loopLinks_.at(position);
8617  from = link.from();
8618  to = link.to();
8619  }
8620 
8621  if(from < to)
8622  {
8623  int tmp = to;
8624  to = from;
8625  from = tmp;
8626  }
8627 
8628  if(from == to)
8629  {
8630  UWARN("Cannot reset link to same node");
8631  return;
8632  }
8633 
8634 
8635  std::multimap<int, Link>::iterator iter = rtabmap::graph::findLink(linksRefined_, from, to);
8636  if(iter != linksRefined_.end())
8637  {
8638  linksRefined_.erase(iter);
8639  this->updateGraphView();
8640  }
8641 
8642  updateConstraintView();
8643 }
8644 
8645 void DatabaseViewer::rejectConstraint()
8646 {
8647  int from = ids_.at(ui_->horizontalSlider_A->value());
8648  int to = ids_.at(ui_->horizontalSlider_B->value());
8649  if(ui_->label_type->text().toInt() == Link::kLandmark)
8650  {
8651  int position = ui_->horizontalSlider_loops->value();
8652  const rtabmap::Link & link = loopLinks_.at(position);
8653  from = link.from();
8654  to = link.to();
8655  }
8656 
8657  if(from < to)
8658  {
8659  int tmp = to;
8660  to = from;
8661  from = tmp;
8662  }
8663 
8664  if(from == to)
8665  {
8666  UWARN("Cannot reject link to same node");
8667  return;
8668  }
8669 
8670  bool removed = false;
8671 
8672  // find the original one
8673  std::multimap<int, Link>::iterator iter;
8674  iter = rtabmap::graph::findLink(links_, from, to);
8675  if(iter != links_.end())
8676  {
8677  if(iter->second.type() == Link::kNeighbor || iter->second.type() == Link::kNeighborMerged)
8678  {
8679  QMessageBox::StandardButton button = QMessageBox::warning(this, tr("Reject link"),
8680  tr("Removing the neighbor link %1->%2 will split the graph. Do you want to continue?").arg(from).arg(to),
8681  QMessageBox::Yes | QMessageBox::No, QMessageBox::No);
8682  if(button != QMessageBox::Yes)
8683  {
8684  return;
8685  }
8686  }
8687  linksRemoved_.insert(*iter);
8688  removed = true;
8689  }
8690 
8691  // remove from refined and added
8692  iter = rtabmap::graph::findLink(linksRefined_, from, to);
8693  if(iter != linksRefined_.end())
8694  {
8695  linksRefined_.erase(iter);
8696  removed = true;
8697  }
8698  iter = rtabmap::graph::findLink(linksAdded_, from, to);
8699  if(iter != linksAdded_.end())
8700  {
8701  linksAdded_.erase(iter);
8702  removed = true;
8703  }
8704  if(removed)
8705  {
8706  this->updateGraphView();
8707  }
8708  updateLoopClosuresSlider();
8709 }
8710 
8711 std::multimap<int, rtabmap::Link> DatabaseViewer::updateLinksWithModifications(
8712  const std::multimap<int, rtabmap::Link> & edgeConstraints)
8713 {
8714  UDEBUG("linksAdded_=%d linksRefined_=%d linksRemoved_=%d", (int)linksAdded_.size(), (int)linksRefined_.size(), (int)linksRemoved_.size());
8715 
8716  std::multimap<int, rtabmap::Link> links;
8717  for(std::multimap<int, rtabmap::Link>::const_iterator iter=edgeConstraints.begin();
8718  iter!=edgeConstraints.end();
8719  ++iter)
8720  {
8721  std::multimap<int, rtabmap::Link>::iterator findIter;
8722 
8723  findIter = rtabmap::graph::findLink(linksRemoved_, iter->second.from(), iter->second.to());
8724  if(findIter != linksRemoved_.end())
8725  {
8726  UDEBUG("Removed link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
8727  continue; // don't add this link
8728  }
8729 
8730  findIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
8731  if(findIter!=linksRefined_.end())
8732  {
8733  links.insert(*findIter); // add the refined link
8734  UDEBUG("Updated link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
8735  continue;
8736  }
8737 
8738  links.insert(*iter); // add original link
8739  }
8740 
8741  //look for added links
8742  for(std::multimap<int, rtabmap::Link>::const_iterator iter=linksAdded_.begin();
8743  iter!=linksAdded_.end();
8744  ++iter)
8745  {
8746  std::multimap<int, rtabmap::Link>::iterator findIter = rtabmap::graph::findLink(linksRefined_, iter->second.from(), iter->second.to());
8747  if(findIter!=linksRefined_.end())
8748  {
8749  links.insert(*findIter); // add the refined link
8750  UDEBUG("Added refined link (%d->%d, %d)", findIter->second.from(), findIter->second.to(), findIter->second.type());
8751  continue;
8752  }
8753 
8754  UDEBUG("Added link (%d->%d, %d)", iter->second.from(), iter->second.to(), iter->second.type());
8755  links.insert(*iter);
8756  }
8757 
8758  return links;
8759 }
8760 
8761 void DatabaseViewer::updateLoopClosuresSlider(int from, int to)
8762 {
8763  UDEBUG("%d %d", from, to);
8764  loopLinks_.clear();
8765  std::multimap<int, Link> links = updateLinksWithModifications(links_);
8766  int position = ui_->horizontalSlider_loops->value();
8767  std::multimap<int, Link> linksSortedByParents;
8768  for(std::multimap<int, rtabmap::Link>::iterator iter = links.begin(); iter!=links.end(); ++iter)
8769  {
8770  if(iter->second.to() > iter->second.from())
8771  {
8772  linksSortedByParents.insert(std::make_pair(iter->second.to(), iter->second.inverse()));
8773  }
8774  else if(iter->second.to() != iter->second.from())
8775  {
8776  linksSortedByParents.insert(*iter);
8777  }
8778  }
8779  for(std::multimap<int, rtabmap::Link>::iterator iter = linksSortedByParents.begin(); iter!=linksSortedByParents.end(); ++iter)
8780  {
8781  if(!iter->second.transform().isNull())
8782  {
8783  if(iter->second.type() != rtabmap::Link::kNeighbor &&
8784  iter->second.type() != rtabmap::Link::kNeighborMerged)
8785  {
8786  if((iter->second.from() == from && iter->second.to() == to) ||
8787  (iter->second.to() == from && iter->second.from() == to))
8788  {
8789  position = loopLinks_.size();
8790  }
8791  loopLinks_.append(iter->second);
8792  }
8793  }
8794  else
8795  {
8796  UERROR("Transform null for link from %d to %d", iter->first, iter->second.to());
8797  }
8798  }
8799 
8800  if(loopLinks_.size())
8801  {
8802  if(loopLinks_.size() == 1)
8803  {
8804  // just to be able to move the cursor of the loop slider
8805  loopLinks_.push_back(loopLinks_.front());
8806  }
8807  ui_->horizontalSlider_loops->setMinimum(0);
8808  ui_->horizontalSlider_loops->setMaximum(loopLinks_.size()-1);
8809  ui_->horizontalSlider_loops->setEnabled(true);
8810  if(position != ui_->horizontalSlider_loops->value())
8811  {
8812  ui_->horizontalSlider_loops->setValue(position);
8813  }
8814  else
8815  {
8816  this->updateConstraintView(loopLinks_.at(position));
8817  }
8818  }
8819  else
8820  {
8821  ui_->horizontalSlider_loops->setEnabled(false);
8822  constraintsViewer_->removeAllClouds();
8823  constraintsViewer_->refreshView();
8824  updateConstraintButtons();
8825  }
8826 }
8827 
8828 void DatabaseViewer::notifyParametersChanged(const QStringList & parametersChanged)
8829 {
8830  bool updateStereo = false;
8831  bool updateGraphView = false;
8832  for(QStringList::const_iterator iter=parametersChanged.constBegin();
8833  iter!=parametersChanged.constEnd() && (!updateStereo || !updateGraphView);
8834  ++iter)
8835  {
8836  QString group = iter->split('/').first();
8837  if(!updateStereo && group == "Stereo")
8838  {
8839  updateStereo = true;
8840  continue;
8841  }
8842  if(!updateGraphView && group == "Optimize")
8843  {
8844  updateGraphView = true;
8845  continue;
8846  }
8847  }
8848 
8849  if(updateStereo)
8850  {
8851  this->updateStereo();
8852  }
8853  if(updateGraphView)
8854  {
8855  this->updateGraphView();
8856  }
8857  this->configModified();
8858 }
8859 
8860 } // namespace rtabmap
uStr2Int
int UTILITE_EXP uStr2Int(const std::string &str)
Definition: UConversion.cpp:125
rtabmap::CloudViewer::removeGraph
void removeGraph(const std::string &id)
Definition: CloudViewer.cpp:2194
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
rtabmap::util3d::isFinite
bool RTABMAP_EXP isFinite(const cv::Point3f &pt)
Definition: util3d.cpp:3194
rtabmap::EditDepthArea::setImage
void setImage(const cv::Mat &depth, const cv::Mat &rgb=cv::Mat())
Definition: EditDepthArea.cpp:77
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:497
rtabmap::DatabaseViewer::updateLinksWithModifications
std::multimap< int, rtabmap::Link > updateLinksWithModifications(const std::multimap< int, rtabmap::Link > &edgeConstraints)
Definition: DatabaseViewer.cpp:8711
rtabmap::DatabaseViewer::odomMaxInf_
std::vector< double > odomMaxInf_
Definition: DatabaseViewer.h:233
rtabmap::DatabaseViewer::generatedLocalMapsInfo_
std::map< int, std::pair< float, cv::Point3f > > generatedLocalMapsInfo_
Definition: DatabaseViewer.h:231
rtabmap::OccupancyGrid::update
bool update(const std::map< int, Transform > &poses)
Definition: OccupancyGrid.cpp:782
rtabmap::ProgressDialog::incrementStep
void incrementStep(int steps=1)
Definition: ProgressDialog.cpp:138
rtabmap::Registration::computeTransformationMod
Transform computeTransformationMod(Signature &from, Signature &to, Transform guess=Transform::getIdentity(), RegistrationInfo *info=0) const
Definition: Registration.cpp:189
rtabmap::DBDriver::getWordsMemoryUsed
long getWordsMemoryUsed() const
Definition: DBDriver.cpp:183
rtabmap::GPS::error
const double & error() const
Definition: GPS.h:63
rtabmap::util3d::transformFromXYZCorrespondencesSVD
Transform RTABMAP_EXP transformFromXYZCorrespondencesSVD(const pcl::PointCloud< pcl::PointXYZ > &cloud1, const pcl::PointCloud< pcl::PointXYZ > &cloud2)
Definition: util3d_registration.cpp:50
rtabmap::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::DBDriver::load2DMap
cv::Mat load2DMap(float &xMin, float &yMin, float &cellSize) const
Definition: DBDriver.cpp:1202
UTimer
Definition: UTimer.h:46
rtabmap::OctoMap::hasColor
bool hasColor() const
Definition: OctoMap.h:218
rtabmap::GeodeticCoords::latitude
const double & latitude() const
Definition: GeodeticCoords.h:58
rtabmap::DBDriver::save2DMap
void save2DMap(const cv::Mat &map, float xMin, float yMin, float cellSize) const
Definition: DBDriver.cpp:1195
rtabmap::DatabaseViewer::ui_
Ui_DatabaseViewer * ui_
Definition: DatabaseViewer.h:201
uMean
T uMean(const T *v, unsigned int size)
Definition: UMath.h:401
rtabmap::util3d::projectDisparityTo3D
cv::Point3f RTABMAP_EXP projectDisparityTo3D(const cv::Point2f &pt, float disparity, const StereoCameraModel &model)
Definition: util3d.cpp:2521
rtabmap::ExportDialog::sessionExported
int sessionExported() const
Definition: ExportDialog.cpp:140
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:1054
UFile::getName
std::string getName()
Definition: UFile.h:135
rtabmap::GPS::stamp
const double & stamp() const
Definition: GPS.h:59
UFile::length
long length()
Definition: UFile.h:110
rtabmap::graph::radiusPosesFiltering
std::map< int, Transform > RTABMAP_EXP radiusPosesFiltering(const std::map< int, Transform > &poses, float radius, float angle, bool keepLatest=true)
Definition: Graph.cpp:1253
rtabmap::graph::findLink
std::multimap< int, Link >::iterator RTABMAP_EXP findLink(std::multimap< int, Link > &links, int from, int to, bool checkBothWays=true, Link::Type type=Link::kUndef)
Definition: Graph.cpp:992
rtabmap::graph::computeMaxGraphErrors
void RTABMAP_EXP computeMaxGraphErrors(const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, float &maxLinearErrorRatio, float &maxAngularErrorRatio, float &maxLinearError, float &maxAngularError, const Link **maxLinearErrorLink=0, const Link **maxAngularErrorLink=0, bool for3DoF=false)
Definition: Graph.cpp:896
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::KeypointItem::keypoint
const cv::KeyPoint & keypoint() const
Definition: KeypointItem.h:48
rtabmap::ProgressDialog::setCancelButtonVisible
void setCancelButtonVisible(bool visible)
Definition: ProgressDialog.cpp:97
rtabmap::StereoDense::create
static StereoDense * create(const ParametersMap &parameters)
Definition: StereoDense.cpp:33
rtabmap::DBDriver::loadSignature
Signature * loadSignature(int id, bool *loadedFromTrash=0)
Definition: DBDriver.cpp:537
uKeys
std::vector< K > uKeys(const std::multimap< K, V > &mm)
Definition: UStl.h:67
rtabmap::LaserScan::maxPoints
int maxPoints() const
Definition: LaserScan.h:116
rtabmap::DBDriver::loadWords
void loadWords(const std::set< int > &wordIds, std::list< VisualWord *> &vws)
Definition: DBDriver.cpp:606
rtabmap::DBDriver::getStatisticsMemoryUsed
long getStatisticsMemoryUsed() const
Definition: DBDriver.cpp:199
uStr2Double
double UTILITE_EXP uStr2Double(const std::string &str)
Definition: UConversion.cpp:147
uIsDigit
bool uIsDigit(const char c)
Definition: UStl.h:622
rtabmap::DBDriver::updateLaserScan
void updateLaserScan(int nodeId, const LaserScan &scan)
Definition: DBDriver.cpp:514
rtabmap::util3d::transformPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
Definition: util3d_transforms.cpp:107
rtabmap::SensorData::setLaserScan
void setLaserScan(const LaserScan &laserScan, bool clearPreviousData=true)
Definition: SensorData.cpp:381
uValueAt
V & uValueAt(std::list< V > &list, const unsigned int &pos)
Definition: UStl.h:382
rtabmap::DatabaseViewer::resizeEvent
virtual void resizeEvent(QResizeEvent *anEvent)
Definition: DatabaseViewer.cpp:1243
rtabmap::CameraModel::fovX
double fovX() const
Definition: CameraModel.cpp:676
f
f
rtabmap::SensorData::laserScanCompressed
const LaserScan & laserScanCompressed() const
Definition: SensorData.h:181
x
x
rtabmap::LaserScan::angleMin
float angleMin() const
Definition: LaserScan.h:119
rtabmap::OccupancyGrid::getProbMap
cv::Mat getProbMap(float &xMin, float &yMin) const
Definition: OccupancyGrid.cpp:735
rtabmap::CloudViewer::addOrUpdateCoordinate
void addOrUpdateCoordinate(const std::string &id, const Transform &transform, double scale, bool foreground=false)
Definition: CloudViewer.cpp:1596
rtabmap::DatabaseViewer::gpsPoses_
std::map< int, rtabmap::Transform > gpsPoses_
Definition: DatabaseViewer.h:222
rtabmap::graph::exportGPS
bool RTABMAP_EXP exportGPS(const std::string &filePath, const std::map< int, GPS > &gpsValues, unsigned int rgba=0xFFFFFFFF)
Definition: Graph.cpp:497
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
rtabmap::DatabaseViewer::closeEvent
virtual void closeEvent(QCloseEvent *event)
Definition: DatabaseViewer.cpp:1176
uStrNumCmp
int uStrNumCmp(const std::string &a, const std::string &b)
Definition: UStl.h:719
UDirectory::getDir
static std::string getDir(const std::string &filePath)
Definition: UDirectory.cpp:273
rtabmap::DatabaseViewer::showEvent
virtual void showEvent(QShowEvent *anEvent)
Definition: DatabaseViewer.cpp:1220
rtabmap::util3d::laserScanToPointCloudRGBNormal
pcl::PointCloud< pcl::PointXYZRGBNormal >::Ptr RTABMAP_EXP laserScanToPointCloudRGBNormal(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2268
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
rtabmap::DatabaseViewer::graphes_
std::list< std::map< int, rtabmap::Transform > > graphes_
Definition: DatabaseViewer.h:218
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:401
rtabmap::DatabaseViewer::constraintsViewer_
CloudViewer * constraintsViewer_
Definition: DatabaseViewer.h:202
rtabmap::DataRecorder::addData
void addData(const rtabmap::SensorData &data, const Transform &pose=Transform(), const cv::Mat &infMatrix=cv::Mat::eye(6, 6, CV_64FC1))
Definition: DataRecorder.cpp:126
uStr2Bool
bool UTILITE_EXP uStr2Bool(const char *str)
Definition: UConversion.cpp:170
rtabmap::CloudViewer::removeCloud
bool removeCloud(const std::string &id)
Definition: CloudViewer.cpp:2449
rtabmap::LaserScan::channels
static int channels(const Format &format)
Definition: LaserScan.cpp:75
rtabmap::SensorData::depthOrRightRaw
const cv::Mat & depthOrRightRaw() const
Definition: SensorData.h:184
data
data
rtabmap::util3d::removeNaNFromPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP removeNaNFromPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud)
Definition: util3d_filtering.cpp:992
uCvMat2QImage
QImage uCvMat2QImage(const cv::Mat &image, bool isBgr=true, uCvQtDepthColorMap colorMap=uCvQtDepthWhiteToBlack, float depthMin=0, float depthMax=0)
Definition: UCv2Qt.h:47
rtabmap::util3d::laserScanToPointCloud2
pcl::PCLPointCloud2::Ptr RTABMAP_EXP laserScanToPointCloud2(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2158
rtabmap::util2d::depthFromDisparity
cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat &disparity, float fx, float baseline, int type=CV_32FC1)
Definition: util2d.cpp:763
rtabmap::DBDriver::saveOptimizedMesh
void saveOptimizedMesh(const cv::Mat &cloud, const std::vector< std::vector< std::vector< RTABMAP_PCL_INDEX > > > &polygons=std::vector< std::vector< std::vector< RTABMAP_PCL_INDEX > > >(), const std::vector< std::vector< Eigen::Vector2f > > &texCoords=std::vector< std::vector< Eigen::Vector2f > >(), const cv::Mat &textures=cv::Mat()) const
Definition: DBDriver.cpp:1210
rtabmap::EditMapArea::setMap
void setMap(const cv::Mat &map)
Definition: EditMapArea.cpp:66
rtabmap::EditDepthArea::setColorMap
void setColorMap(uCvQtDepthColorMap type)
Definition: EditDepthArea.cpp:160
uStr2Float
float UTILITE_EXP uStr2Float(const std::string &str)
Definition: UConversion.cpp:138
rtabmap::DatabaseViewer::exportGPS
void exportGPS(int format)
Definition: DatabaseViewer.cpp:2796
glm::all
GLM_FUNC_DECL bool all(vecType< bool, P > const &v)
rtabmap::CloudViewer::addOccupancyGridMap
bool addOccupancyGridMap(const cv::Mat &map8U, float resolution, float xMin, float yMin, float opacity)
Definition: CloudViewer.cpp:1516
rtabmap::SensorData::stereoCameraModels
const std::vector< StereoCameraModel > & stereoCameraModels() const
Definition: SensorData.h:237
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:1172
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:1591
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:112
rtabmap::DBDriver::getFeaturesMemoryUsed
long getFeaturesMemoryUsed() const
Definition: DBDriver.cpp:191
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:113
rtabmap::DatabaseViewer::getIniFilePath
QString getIniFilePath() const
Definition: DatabaseViewer.cpp:512
rtabmap::Signature::getWords3
const std::vector< cv::Point3f > & getWords3() const
Definition: Signature.h:131
rtabmap::Parameters::writeINI
static void writeINI(const std::string &configFile, const ParametersMap &parameters)
Definition: Parameters.cpp:1248
rtabmap::CloudViewer::clear
virtual void clear()
Definition: CloudViewer.cpp:251
rtabmap::OccupancyGrid::setCellSize
void setCellSize(float cellSize)
Definition: OccupancyGrid.cpp:260
rtabmap::SensorData::setGPS
void setGPS(const GPS &gps)
Definition: SensorData.h:286
rtabmap::RegistrationInfo::inliersIDs
std::vector< int > inliersIDs
Definition: RegistrationInfo.h:84
rtabmap::Transform::getNorm
float getNorm() const
Definition: Transform.cpp:273
UConversion.h
Some conversion functions.
UPlotCurve::addValue
void addValue(UPlotItem *data)
Definition: UPlot.cpp:422
rtabmap::ImageView::setImage
void setImage(const QImage &image)
Definition: ImageView.cpp:1143
rtabmap::DBDriver::loadLinks
void loadLinks(int signatureId, std::multimap< int, Link > &links, Link::Type type=Link::kUndef) const
Definition: DBDriver.cpp:815
rtabmap::util3d::assembleTextureMesh
pcl::TextureMesh::Ptr RTABMAP_EXP assembleTextureMesh(const cv::Mat &cloudMat, const std::vector< std::vector< std::vector< RTABMAP_PCL_INDEX > > > &polygons, const std::vector< std::vector< Eigen::Vector2f > > &texCoords, cv::Mat &textures, bool mergeTextures=false)
Definition: util3d_surface.cpp:1272
rtabmap::CloudViewer::setPolygonPicking
void setPolygonPicking(bool enabled)
Definition: CloudViewer.cpp:2619
rtabmap::DatabaseViewer::linksAdded_
std::multimap< int, rtabmap::Link > linksAdded_
Definition: DatabaseViewer.h:226
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
rtabmap::CloudViewer::buildPickingLocator
void buildPickingLocator(bool enable)
Definition: CloudViewer.cpp:3442
rtabmap::DatabaseViewer::neighborLinks_
QList< rtabmap::Link > neighborLinks_
Definition: DatabaseViewer.h:212
rtabmap::DatabaseViewer::findActiveLink
Link findActiveLink(int from, int to)
Definition: DatabaseViewer.cpp:7607
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:125
rtabmap::DatabaseViewer::links_
std::multimap< int, rtabmap::Link > links_
Definition: DatabaseViewer.h:224
rtabmap::DBDriver::getNodesMemoryUsed
long getNodesMemoryUsed() const
Definition: DBDriver.cpp:119
rtabmap::CloudViewer::isFrustumShown
bool isFrustumShown() const
Definition: CloudViewer.cpp:2332
rtabmap::LaserScan::is2d
bool is2d() const
Definition: LaserScan.h:128
rtabmap::DatabaseViewer::notifyParametersChanged
void notifyParametersChanged(const QStringList &)
Definition: DatabaseViewer.cpp:8828
rtabmap::Transform::getTranslationAndEulerAngles
void getTranslationAndEulerAngles(float &x, float &y, float &z, float &roll, float &pitch, float &yaw) const
Definition: Transform.cpp:248
rtabmap::DatabaseViewer::exportDialog_
ExportCloudsDialog * exportDialog_
Definition: DatabaseViewer.h:235
ULogger::setLevel
static void setLevel(ULogger::Level level)
Definition: ULogger.h:339
rtabmap::EditMapArea::getModifiedMap
cv::Mat getModifiedMap() const
Definition: EditMapArea.cpp:78
rtabmap::DatabaseViewer::containsLink
bool containsLink(std::multimap< int, Link > &links, int from, int to)
Definition: DatabaseViewer.cpp:7634
rtabmap::Feature2D::generateKeypoints
std::vector< cv::KeyPoint > generateKeypoints(const cv::Mat &image, const cv::Mat &mask=cv::Mat())
Definition: Features2d.cpp:670
rtabmap::util3d::laserScanToPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP laserScanToPointCloud(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2197
util3d_registration.h
rtabmap::DatabaseViewer::idToIndex_
QMap< int, int > idToIndex_
Definition: DatabaseViewer.h:211
uMin
T uMin(const T *v, unsigned int size, unsigned int &index)
Definition: UMath.h:159
rtabmap::LaserScan::rangeMax
float rangeMax() const
Definition: LaserScan.h:118
prior
prior
rtabmap::ExportCloudsDialog::setDBDriver
void setDBDriver(const DBDriver *dbDriver)
Definition: ExportCloudsDialog.h:66
rtabmap::DatabaseViewer::modifiedLaserScans_
std::map< int, LaserScan > modifiedLaserScans_
Definition: DatabaseViewer.h:232
rtabmap::DBDriver::generateGraph
void generateGraph(const std::string &fileName, const std::set< int > &ids=std::set< int >(), const std::map< int, Signature *> &otherSignatures=std::map< int, Signature *>())
Definition: DBDriver.cpp:1240
rtabmap::DBDriver::loadOptimizedPoses
std::map< int, Transform > loadOptimizedPoses(Transform *lastlocalizationPose=0) const
Definition: DBDriver.cpp:1187
rtabmap::DBDriver::saveOptimizedPoses
void saveOptimizedPoses(const std::map< int, Transform > &optimizedPoses, const Transform &lastlocalizationPose) const
Definition: DBDriver.cpp:1181
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:1063
rtabmap::SensorData::gridCellSize
float gridCellSize() const
Definition: SensorData.h:266
rtabmap::LaserScan::hasRGB
bool hasRGB() const
Definition: LaserScan.h:130
rtabmap::SensorData::rightRaw
cv::Mat rightRaw() const
Definition: SensorData.h:211
rtabmap::util3d::cloudFromDepthRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP cloudFromDepthRGB(const cv::Mat &imageRgb, const cv::Mat &imageDepth, const CameraModel &model, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0)
Definition: util3d.cpp:429
UPlot
Definition: UPlot.h:489
rtabmap::DatabaseViewer::editDepthArea_
EditDepthArea * editDepthArea_
Definition: DatabaseViewer.h:237
rtabmap::ExportDialog::isOdomExported
bool isOdomExported() const
Definition: ExportDialog.cpp:160
util3d_mapping.h
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:1671
rtabmap::DatabaseViewer::linksRefined_
std::multimap< int, rtabmap::Link > linksRefined_
Definition: DatabaseViewer.h:225
rtabmap::DBDriver::openConnection
bool openConnection(const std::string &url, bool overwritten=false)
Definition: DBDriver.cpp:86
rtabmap::DBDriver::getAllLinks
void getAllLinks(std::multimap< int, Link > &links, bool ignoreNullLinks=true, bool withLandmarks=false) const
Definition: DBDriver.cpp:915
rtabmap::Signature::getWordsKpts
const std::vector< cv::KeyPoint > & getWordsKpts() const
Definition: Signature.h:112
rtabmap::DBDriver::getLaserScansMemoryUsed
long getLaserScansMemoryUsed() const
Definition: DBDriver.cpp:167
rtabmap::DBDriver::getLastNodeIds
void getLastNodeIds(std::set< int > &ids) const
Definition: DBDriver.cpp:869
rtabmap::ExportCloudsDialog::loadSettings
void loadSettings(QSettings &settings, const QString &group="")
Definition: ExportCloudsDialog.cpp:516
UASSERT
#define UASSERT(condition)
rtabmap::OctoMap::update
bool update(const std::map< int, Transform > &poses)
Definition: OctoMap.cpp:512
uSplit
std::list< std::string > uSplit(const std::string &str, char separator=' ')
Definition: UStl.h:566
rtabmap::GPS::altitude
const double & altitude() const
Definition: GPS.h:62
rtabmap::DatabaseViewer::setupMainLayout
void setupMainLayout(bool vertical)
Definition: DatabaseViewer.cpp:486
rtabmap::GPS::bearing
const double & bearing() const
Definition: GPS.h:64
rtabmap::OctoMap::addToCache
void addToCache(int nodeId, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &ground, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &obstacles, const pcl::PointXYZ &viewPoint)
Definition: OctoMap.cpp:363
rtabmap::ExportDialog::isDepthExported
bool isDepthExported() const
Definition: ExportDialog.cpp:150
rtabmap::util3d::laserScanToPointCloudRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP laserScanToPointCloudRGB(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2232
rtabmap::SensorData::id
int id() const
Definition: SensorData.h:174
rtabmap::ImageView::setSceneRect
void setSceneRect(const QRectF &rect)
Definition: ImageView.cpp:1310
rtabmap::LaserScan::hasNormals
bool hasNormals() const
Definition: LaserScan.h:129
rtabmap::EditMapArea::isModified
bool isModified() const
Definition: EditMapArea.h:54
rtabmap::DBDriver::getTotalDictionarySize
int getTotalDictionarySize() const
Definition: DBDriver.cpp:231
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:236
rtabmap::util3d::clusterPolygons
std::list< std::list< int > > RTABMAP_EXP clusterPolygons(const std::vector< std::set< int > > &neighborPolygons, int minClusterSize=0)
Definition: util3d_surface.cpp:131
rtabmap::ExportDialog::targetFramerate
double targetFramerate() const
Definition: ExportDialog.cpp:135
true
#define true
Definition: ConvertUTF.c:57
rtabmap::GeodeticCoords::toENU_WGS84
cv::Point3d toENU_WGS84(const GeodeticCoords &origin) const
Definition: GeodeticCoords.cpp:108
rtabmap::Feature2D::create
static Feature2D * create(const ParametersMap &parameters=ParametersMap())
Definition: Features2d.cpp:511
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, QLabel *labelGps, QLabel *labelGt, QLabel *labelSensors, bool updateConstraintView)
Definition: DatabaseViewer.cpp:4459
rtabmap::Parameters::filterParameters
static ParametersMap filterParameters(const ParametersMap &parameters, const std::string &group, bool remove=false)
Definition: Parameters.cpp:217
rtabmap::StereoDense::computeDisparity
virtual cv::Mat computeDisparity(const cv::Mat &leftImage, const cv::Mat &rightImage) const =0
rtabmap::Registration::isScanRequired
bool isScanRequired() const
Definition: Registration.cpp:100
rtabmap::DatabaseViewer::mapIds_
std::map< int, int > mapIds_
Definition: DatabaseViewer.h:208
ULOGGER_DEBUG
#define ULOGGER_DEBUG(...)
Definition: ULogger.h:53
rtabmap::DatabaseViewer::updateWordsMatching
void updateWordsMatching(const std::vector< int > &inliers=std::vector< int >())
Definition: DatabaseViewer.cpp:5651
rtabmap::ExportDialog::isDepth2dExported
bool isDepth2dExported() const
Definition: ExportDialog.cpp:155
rtabmap::DatabaseViewer::gpsValues_
std::map< int, GPS > gpsValues_
Definition: DatabaseViewer.h:223
rtabmap::DBDriver::getNodeInfo
bool getNodeInfo(int signatureId, Transform &pose, int &mapId, int &weight, std::string &label, double &stamp, Transform &groundTruthPose, std::vector< float > &velocity, GPS &gps, EnvSensors &sensors) const
Definition: DBDriver.cpp:776
rtabmap::DatabaseViewer::graphLinks_
std::multimap< int, rtabmap::Link > graphLinks_
Definition: DatabaseViewer.h:219
rtabmap::DatabaseViewer::loopLinks_
QList< rtabmap::Link > loopLinks_
Definition: DatabaseViewer.h:213
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
UPlot::addCurve
UPlotCurve * addCurve(const QString &curveName, const QColor &color=QColor())
Definition: UPlot.cpp:2102
rtabmap::SensorData::depthRaw
cv::Mat depthRaw() const
Definition: SensorData.h:210
name
string name
rtabmap::OccupancyGrid::addToCache
void addToCache(int nodeId, const cv::Mat &ground, const cv::Mat &obstacles, const cv::Mat &empty)
Definition: OccupancyGrid.cpp:767
rtabmap::util3d::laserScanToPointCloudINormal
pcl::PointCloud< pcl::PointXYZINormal >::Ptr RTABMAP_EXP laserScanToPointCloudINormal(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
Definition: util3d.cpp:2285
rtabmap::ImageView::setImageDepth
void setImageDepth(const cv::Mat &imageDepth)
Definition: ImageView.cpp:1170
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:183
util3d_filtering.h
rtabmap::DBDriver::getCalibration
bool getCalibration(int signatureId, std::vector< CameraModel > &models, std::vector< StereoCameraModel > &stereoModels) const
Definition: DBDriver.cpp:726
rtabmap::DBDriver::getAllNodeIds
void getAllNodeIds(std::set< int > &ids, bool ignoreChildren=false, bool ignoreBadSignatures=false, bool ignoreIntermediateNodes=false) const
Definition: DBDriver.cpp:876
rtabmap::GeodeticCoords::altitude
const double & altitude() const
Definition: GeodeticCoords.h:60
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:406
rtabmap::graph::calcRMSE
Transform RTABMAP_EXP calcRMSE(const std::map< int, Transform > &groundTruth, const std::map< int, Transform > &poses, float &translational_rmse, float &translational_mean, float &translational_median, float &translational_std, float &translational_min, float &translational_max, float &rotational_rmse, float &rotational_mean, float &rotational_median, float &rotational_std, float &rotational_min, float &rotational_max)
Definition: Graph.cpp:759
rtabmap::util3d::projectCloudToCameras
std::vector< std::pair< std::pair< int, int >, pcl::PointXY > > RTABMAP_EXP 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)
rtabmap::graph::getMaxOdomInf
std::vector< double > RTABMAP_EXP getMaxOdomInf(const std::multimap< int, Link > &links)
Definition: Graph.cpp:964
rtabmap::util3d::cloudRGBFromSensorData
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_EXP cloudRGBFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:1056
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:316
rtabmap::DatabaseViewer::wmStates_
std::map< int, std::vector< int > > wmStates_
Definition: DatabaseViewer.h:210
rtabmap::CloudViewer::setCloudPointSize
void setCloudPointSize(const std::string &id, int size)
Definition: CloudViewer.cpp:3147
rtabmap::DBDriver::getNodeData
void getNodeData(int signatureId, SensorData &data, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const
Definition: DBDriver.cpp:675
rtabmap::Registration::isImageRequired
bool isImageRequired() const
Definition: Registration.cpp:90
rtabmap::LaserScan::empty
bool empty() const
Definition: LaserScan.h:124
rtabmap::DBDriver::getGridsMemoryUsed
long getGridsMemoryUsed() const
Definition: DBDriver.cpp:159
rtabmap::ProgressDialog::setMaximumSteps
void setMaximumSteps(int steps)
Definition: ProgressDialog.cpp:133
rtabmap::CameraModel::localTransform
const Transform & localTransform() const
Definition: CameraModel.h:116
rtabmap::util3d::convertImage8U2Map
cv::Mat RTABMAP_EXP convertImage8U2Map(const cv::Mat &map8U, bool pgmFormat=false)
Definition: util3d_mapping.cpp:949
rtabmap::DatabaseViewer::localMapsInfo_
std::map< int, std::pair< float, cv::Point3f > > localMapsInfo_
Definition: DatabaseViewer.h:229
rtabmap::CloudViewer::updateCameraTargetPosition
void updateCameraTargetPosition(const Transform &pose)
Definition: CloudViewer.cpp:2842
rtabmap::DBDriver::updateLink
void updateLink(const Link &link)
Definition: DBDriver.cpp:477
rtabmap::OccupancyGrid::getMap
cv::Mat getMap(float &xMin, float &yMin) const
Definition: OccupancyGrid.cpp:692
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:2007
rtabmap::util3d::laserScanFromPointCloud
LaserScan laserScanFromPointCloud(const PointCloud2T &cloud, bool filterNaNs, bool is2D, const Transform &transform)
Definition: util3d.hpp:37
translation
translation
rtabmap::DatabaseViewer::weights_
std::map< int, int > weights_
Definition: DatabaseViewer.h:209
rtabmap::DatabaseViewer::linksRemoved_
std::multimap< int, rtabmap::Link > linksRemoved_
Definition: DatabaseViewer.h:227
rtabmap::DBDriver::updateDepthImage
void updateDepthImage(int nodeId, const cv::Mat &image)
Definition: DBDriver.cpp:505
rtabmap::LaserScan::rangeMin
float rangeMin() const
Definition: LaserScan.h:117
rtabmap::CloudViewer::getFrustumColor
QColor getFrustumColor() const
Definition: CloudViewer.cpp:2342
uSplitNumChar
std::list< std::string > uSplitNumChar(const std::string &str)
Definition: UStl.h:672
util3d_transforms.h
rtabmap::util3d::organizedFastMesh
std::vector< pcl::Vertices > RTABMAP_EXP organizedFastMesh(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, double angleTolerance, bool quad, int trianglePixelSize, const Eigen::Vector3f &viewpoint=Eigen::Vector3f(0, 0, 0))
Definition: util3d_surface.cpp:168
rtabmap::ExportCloudsDialog::saveSettings
void saveSettings(QSettings &settings, const QString &group="") const
Definition: ExportCloudsDialog.cpp:344
rtabmap::DatabaseViewer::exportPoses
void exportPoses(int format)
Definition: DatabaseViewer.cpp:2377
params
params
rtabmap::SensorData::setEnvSensors
void setEnvSensors(const EnvSensors &sensors)
Definition: SensorData.h:292
rtabmap::DatabaseViewer::odomPoses_
std::map< int, rtabmap::Transform > odomPoses_
Definition: DatabaseViewer.h:220
rtabmap::DBDriver::getLaserScanInfo
bool getLaserScanInfo(int signatureId, LaserScan &info) const
Definition: DBDriver.cpp:752
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, bool adjustPosesWithConstraints=true) const
Definition: Optimizer.cpp:188
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:409
rtabmap::GPS::toGeodeticCoords
GeodeticCoords toGeodeticCoords() const
Definition: GPS.h:66
rtabmap::graph::radiusPosesClustering
std::multimap< int, int > RTABMAP_EXP radiusPosesClustering(const std::map< int, Transform > &poses, float radius, float angle)
Definition: Graph.cpp:1365
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:1110
rtabmap::DBDriver::getAllStatistics
std::map< int, std::pair< std::map< std::string, float >, double > > getAllStatistics() const
Definition: DBDriver.cpp:257
rtabmap::graph::exportPoses
bool RTABMAP_EXP exportPoses(const std::string &filePath, int format, const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints=std::multimap< int, Link >(), const std::map< int, double > &stamps=std::map< int, double >(), const ParametersMap &parameters=ParametersMap())
Definition: Graph.cpp:54
state
RecoveryProgressState state
Definition: tools/Recovery/main.cpp:56
rtabmap::CloudViewer::setCameraLockZ
void setCameraLockZ(bool enabled=true)
Definition: CloudViewer.cpp:3175
rtabmap::Signature::setPose
void setPose(const Transform &pose)
Definition: Signature.h:119
rtabmap::DBDriver::create
static DBDriver * create(const ParametersMap &parameters=ParametersMap())
Definition: DBDriver.cpp:41
rtabmap::DatabaseViewer::groundTruthPoses_
std::map< int, rtabmap::Transform > groundTruthPoses_
Definition: DatabaseViewer.h:221
glm::rotation
GLM_FUNC_DECL detail::tquat< T, P > rotation(detail::tvec3< T, P > const &orig, detail::tvec3< T, P > const &dest)
rtabmap::Stereo::create
static Stereo * create(const ParametersMap &parameters=ParametersMap())
Definition: Stereo.cpp:35
rtabmap::DBDriver::getCalibrationsMemoryUsed
long getCalibrationsMemoryUsed() const
Definition: DBDriver.cpp:151
rtabmap::DatabaseViewer::stereoViewer_
CloudViewer * stereoViewer_
Definition: DatabaseViewer.h:204
rtabmap::Transform::rotation
Transform rotation() const
Definition: Transform.cpp:195
rtabmap::Transform::getDistance
float getDistance(const Transform &t) const
Definition: Transform.cpp:283
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::util3d::mergeTextures
cv::Mat RTABMAP_EXP mergeTextures(pcl::TextureMesh &mesh, const std::map< int, cv::Mat > &images, const std::map< int, CameraModel > &calibrations, const Memory *memory=0, const DBDriver *dbDriver=0, int textureSize=4096, int textureCount=1, const std::vector< std::map< int, pcl::PointXY > > &vertexToPixels=std::vector< std::map< int, pcl::PointXY > >(), bool gainCompensation=true, float gainBeta=10.0f, bool gainRGB=true, bool blending=true, int blendingDecimation=0, int brightnessContrastRatioLow=0, int brightnessContrastRatioHigh=0, bool exposureFusion=false, const ProgressState *state=0, unsigned char blankValue=255, std::map< int, std::map< int, cv::Vec4d > > *gains=0, std::map< int, std::map< int, cv::Mat > > *blendingGains=0, std::pair< float, float > *contrastValues=0)
Definition: util3d_surface.cpp:1438
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:734
rtabmap::EditDepthArea::isModified
bool isModified() const
Definition: EditDepthArea.h:54
rtabmap::Parameters::getType
static std::string getType(const std::string &paramKey)
Definition: Parameters.cpp:467
rtabmap::DBDriver::getLastNodesSize
int getLastNodesSize() const
Definition: DBDriver.cpp:207
rtabmap::LaserScan::angleMax
float angleMax() const
Definition: LaserScan.h:120
uMax
T uMax(const T *v, unsigned int size, unsigned int &index)
Definition: UMath.h:94
false
#define false
Definition: ConvertUTF.c:56
rtabmap::ExportDialog::outputPath
QString outputPath() const
Definition: ExportDialog.cpp:125
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
rtabmap::util3d::convertPolygonsFromPCL
std::vector< std::vector< RTABMAP_PCL_INDEX > > RTABMAP_EXP convertPolygonsFromPCL(const std::vector< pcl::Vertices > &polygons)
Definition: util3d_surface.cpp:1227
rtabmap::DatabaseViewer::refineAllLinks
void refineAllLinks(const QList< Link > &links)
Definition: DatabaseViewer.cpp:4352
rtabmap::DatabaseViewer::dbDriver_
rtabmap::DBDriver * dbDriver_
Definition: DatabaseViewer.h:215
rtabmap::ExportDialog::isRgbExported
bool isRgbExported() const
Definition: ExportDialog.cpp:145
rtabmap::util3d::convertMap2Image8U
cv::Mat RTABMAP_EXP convertMap2Image8U(const cv::Mat &map8S, bool pgmFormat=false)
Definition: util3d_mapping.cpp:908
rtabmap::Parameters::getDefaultParameters
static const ParametersMap & getDefaultParameters()
Definition: Parameters.h:807
rtabmap::util3d::fixTextureMeshForVisualization
void RTABMAP_EXP fixTextureMeshForVisualization(pcl::TextureMesh &textureMesh)
Definition: util3d_surface.cpp:2195
rtabmap::SensorData::gridViewPoint
const cv::Point3f & gridViewPoint() const
Definition: SensorData.h:267
rtabmap::ExportDialog::isUserDataExported
bool isUserDataExported() const
Definition: ExportDialog.cpp:165
rtabmap::CloudViewer::addOrUpdateGraph
void addOrUpdateGraph(const std::string &id, const pcl::PointCloud< pcl::PointXYZ >::Ptr &graph, const QColor &color=Qt::gray)
Definition: CloudViewer.cpp:2157
rtabmap::CloudViewer::getFrustumScale
float getFrustumScale() const
Definition: CloudViewer.cpp:2337
uValues
std::vector< V > uValues(const std::multimap< K, V > &mm)
Definition: UStl.h:100
rtabmap::util3d::cloudFromSensorData
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP cloudFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:853
rtabmap::Optimizer::type
virtual Type type() const =0
rtabmap::Registration::create
static Registration * create(const ParametersMap &parameters)
Definition: Registration.cpp:39
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:185
rtabmap::DBDriver::getLastWordId
void getLastWordId(int &id) const
Definition: DBDriver.cpp:993
UDEBUG
#define UDEBUG(...)
rtabmap::Signature::sensorData
SensorData & sensorData()
Definition: Signature.h:137
rtabmap::EditDepthArea::getModifiedImage
cv::Mat getModifiedImage() const
Definition: EditDepthArea.cpp:116
rtabmap::DBDriver::loadNodeData
void loadNodeData(Signature *signature, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const
Definition: DBDriver.cpp:648
rtabmap::CloudViewer::updateCameraFrustums
void updateCameraFrustums(const Transform &pose, const std::vector< CameraModel > &models)
Definition: CloudViewer.cpp:2986
rtabmap::DatabaseViewer::showCloseButton
void showCloseButton(bool visible=true)
Definition: DatabaseViewer.cpp:502
rtabmap::Registration::isUserDataRequired
bool isUserDataRequired() const
Definition: Registration.cpp:110
rtabmap::DBDriver::loadSignatures
void loadSignatures(const std::list< int > &ids, std::list< Signature *> &signatures, std::set< int > *loadedFromTrash=0)
Definition: DBDriver.cpp:554
rtabmap::GPS::longitude
const double & longitude() const
Definition: GPS.h:60
rtabmap::databaseRecovery
bool RTABMAP_EXP databaseRecovery(const std::string &corruptedDatabase, bool keepCorruptedDatabase=true, std::string *errorMsg=0, ProgressState *progressState=0)
Definition: Recovery.cpp:39
rtabmap::util3d::laserScanToPointCloudNormal
pcl::PointCloud< pcl::PointNormal >::Ptr RTABMAP_EXP laserScanToPointCloudNormal(const LaserScan &laserScan, const Transform &transform=Transform())
Definition: util3d.cpp:2215
rtabmap::util3d::createPolygonIndexes
void RTABMAP_EXP createPolygonIndexes(const std::vector< pcl::Vertices > &polygons, int cloudSize, std::vector< std::set< int > > &neighborPolygons, std::vector< std::set< int > > &vertexPolygons)
Given a set of polygons, create two indexes: polygons to neighbor polygons and vertices to polygons...
Definition: util3d_surface.cpp:94
UERROR
#define UERROR(...)
rtabmap::Transform::getTranslation
void getTranslation(float &x, float &y, float &z) const
Definition: Transform.cpp:259
rtabmap::DatabaseViewer::updateLoopClosuresSlider
void updateLoopClosuresSlider(int from=0, int to=0)
Definition: DatabaseViewer.cpp:8761
rtabmap::SensorData::gridEmptyCellsRaw
const cv::Mat & gridEmptyCellsRaw() const
Definition: SensorData.h:264
rtabmap::util2d::fillDepthHoles
cv::Mat RTABMAP_EXP fillDepthHoles(const cv::Mat &depth, int maximumHoleSize=1, float errorRatio=0.02f)
Definition: util2d.cpp:1439
rtabmap::DBDriver::updateOccupancyGrid
void updateOccupancyGrid(int nodeId, const cv::Mat &ground, const cv::Mat &obstacles, const cv::Mat &empty, float cellSize, const cv::Point3f &viewpoint)
Definition: DBDriver.cpp:483
rtabmap::DatabaseViewer::keyPressEvent
virtual void keyPressEvent(QKeyEvent *event)
Definition: DatabaseViewer.cpp:1251
rtabmap::Signature::getPose
const Transform & getPose() const
Definition: Signature.h:132
ULogger.h
ULogger class and convenient macros.
rtabmap::SensorData::gridGroundCellsRaw
const cv::Mat & gridGroundCellsRaw() const
Definition: SensorData.h:260
UWARN
#define UWARN(...)
rtabmap::DBDriver::getDepthImagesMemoryUsed
long getDepthImagesMemoryUsed() const
Definition: DBDriver.cpp:143
rtabmap::DatabaseViewer::lastWmIds_
std::set< int > lastWmIds_
Definition: DatabaseViewer.h:207
rtabmap::Signature::id
int id() const
Definition: Signature.h:70
rtabmap::GainCompensator::feed
void feed(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloudA, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloudB, const Transform &transformB)
Definition: GainCompensator.cpp:57
rtabmap::GeodeticCoords::longitude
const double & longitude() const
Definition: GeodeticCoords.h:59
diff
diff
rtabmap::DBDriver::getLastParameters
ParametersMap getLastParameters() const
Definition: DBDriver.cpp:239
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
rtabmap::DBDriver::loadOptimizedMesh
cv::Mat loadOptimizedMesh(std::vector< std::vector< std::vector< RTABMAP_PCL_INDEX > > > *polygons=0, std::vector< std::vector< Eigen::Vector2f > > *texCoords=0, cv::Mat *textures=0) const
Definition: DBDriver.cpp:1225
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
UDirectory::exists
static bool exists(const std::string &dirPath)
Definition: UDirectory.cpp:249
rtabmap::Parameters::readINI
static void readINI(const std::string &configFile, ParametersMap &parameters, bool modifiedOnly=false)
Definition: Parameters.cpp:1161
rtabmap::CloudViewer::setCloudColorIndex
void setCloudColorIndex(const std::string &id, int index)
Definition: CloudViewer.cpp:3112
trace.model
model
Definition: trace.py:4
rtabmap::CloudViewer::removeCoordinate
void removeCoordinate(const std::string &id)
Definition: CloudViewer.cpp:1638
rtabmap::LaserScan::hasIntensity
bool hasIntensity() const
Definition: LaserScan.h:131
rtabmap::DataRecorder::init
bool init(const QString &path, bool recordInRAM=true)
Definition: DataRecorder.cpp:67
rtabmap::DatabaseViewer::localMaps_
std::map< int, std::pair< std::pair< cv::Mat, cv::Mat >, cv::Mat > > localMaps_
Definition: DatabaseViewer.h:228
rtabmap::DBDriver::getImagesMemoryUsed
long getImagesMemoryUsed() const
Definition: DBDriver.cpp:135
rtabmap::SensorData::setGroundTruth
void setGroundTruth(const Transform &pose)
Definition: SensorData.h:279
rtabmap::util2d::cvtDepthFromFloat
cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat &depth32F)
Definition: util2d.cpp:892
rtabmap::DBDriver::getDatabaseVersion
std::string getDatabaseVersion() const
Definition: DBDriver.cpp:275
rtabmap::DBDriver::getLastDictionarySize
int getLastDictionarySize() const
Definition: DBDriver.cpp:215
rtabmap::SensorData::gridObstacleCellsRaw
const cv::Mat & gridObstacleCellsRaw() const
Definition: SensorData.h:262
rtabmap::DatabaseViewer::cloudViewer_
CloudViewer * cloudViewer_
Definition: DatabaseViewer.h:203
rtabmap::GPS::latitude
const double & latitude() const
Definition: GPS.h:61
rtabmap::EnvSensors
std::map< EnvSensor::Type, EnvSensor > EnvSensors
Definition: EnvSensor.h:81
rtabmap::LaserScan::angleIncrement
float angleIncrement() const
Definition: LaserScan.h:121
rtabmap::OctoMap::createProjectionMap
cv::Mat createProjectionMap(float &xMin, float &yMin, float &gridCellSize, float minGridSize=0.0f, unsigned int treeDepth=0)
Definition: OctoMap.cpp:1218
rtabmap::LaserScan::formatName
static std::string formatName(const Format &format)
Definition: LaserScan.cpp:34
rtabmap::CloudViewer::addOctomap
bool addOctomap(const OctoMap *octomap, unsigned int treeDepth=0, bool volumeRepresentation=true)
Definition: CloudViewer.cpp:1102
rtabmap::DatabaseViewer::updateAllCovariances
void updateAllCovariances(const QList< Link > &links)
Definition: DatabaseViewer.cpp:4256
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:912
rtabmap::util3d::laserScanToPointCloudI
pcl::PointCloud< pcl::PointXYZI >::Ptr RTABMAP_EXP laserScanToPointCloudI(const LaserScan &laserScan, const Transform &transform=Transform(), float intensity=0.0f)
Definition: util3d.cpp:2250
rtabmap::DatabaseViewer::DatabaseViewer
DatabaseViewer(const QString &ini=QString(), QWidget *parent=0)
Definition: DatabaseViewer.cpp:103
rtabmap::DBDriver::getUserDataMemoryUsed
long getUserDataMemoryUsed() const
Definition: DBDriver.cpp:175
glm::length
GLM_FUNC_DECL genType::value_type length(genType const &x)
rtabmap::Optimizer::isCovarianceIgnored
bool isCovarianceIgnored() const
Definition: Optimizer.h:93
rtabmap::DBDriver::addLink
void addLink(const Link &link)
Definition: DBDriver.cpp:467
rtabmap::CameraModel::fovY
double fovY() const
Definition: CameraModel.cpp:680
rtabmap::ProgressDialog::appendText
void appendText(const QString &text, const QColor &color=Qt::black)
Definition: ProgressDialog.cpp:102
rtabmap::DatabaseViewer::eventFilter
virtual bool eventFilter(QObject *obj, QEvent *event)
Definition: DatabaseViewer.cpp:1260
ULOGGER_ERROR
#define ULOGGER_ERROR(...)
Definition: ULogger.h:56
rtabmap::util3d::voxelize
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_EXP voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
Definition: util3d_filtering.cpp:603
rtabmap::DatabaseViewer::moveEvent
virtual void moveEvent(QMoveEvent *anEvent)
Definition: DatabaseViewer.cpp:1230
rtabmap::Transform::fromString
static Transform fromString(const std::string &string)
Definition: Transform.cpp:465
rtabmap::DBDriver::removeLink
void removeLink(int from, int to)
Definition: DBDriver.cpp:473
rtabmap::graph::computePathLength
float RTABMAP_EXP computePathLength(const std::vector< std::pair< int, Transform > > &path, unsigned int fromIndex=0, unsigned int toIndex=0)
Definition: Graph.cpp:2248
rtabmap::LaserScan::dataType
int dataType() const
Definition: LaserScan.h:127
uFormat
std::string UTILITE_EXP uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::DBDriver::getLinksMemoryUsed
long getLinksMemoryUsed() const
Definition: DBDriver.cpp:127
uNumber2Str
std::string UTILITE_EXP uNumber2Str(unsigned int number)
Definition: UConversion.cpp:91
rtabmap::LaserScan::backwardCompatibility
static LaserScan backwardCompatibility(const cv::Mat &oldScanFormat, int maxPoints=0, int maxRange=0, const Transform &localTransform=Transform::getIdentity())
Definition: LaserScan.cpp:125
rtabmap::SensorData::uncompressDataConst
void uncompressDataConst(cv::Mat *imageRaw, cv::Mat *depthOrRightRaw, LaserScan *laserScanRaw=0, cv::Mat *userDataRaw=0, cv::Mat *groundCellsRaw=0, cv::Mat *obstacleCellsRaw=0, cv::Mat *emptyCellsRaw=0) const
Definition: SensorData.cpp:623
rtabmap::Transform::clone
Transform clone() const
Definition: Transform.cpp:102
rtabmap::DatabaseViewer::occupancyGridViewer_
CloudViewer * occupancyGridViewer_
Definition: DatabaseViewer.h:205
rtabmap::DatabaseViewer::generatedLocalMaps_
std::map< int, std::pair< std::pair< cv::Mat, cv::Mat >, cv::Mat > > generatedLocalMaps_
Definition: DatabaseViewer.h:230
rtabmap::CloudViewer::removeAllFrustums
void removeAllFrustums(bool exceptCameraReference=false)
Definition: CloudViewer.cpp:2144
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:122
rtabmap::GeodeticCoords::fromENU_WGS84
void fromENU_WGS84(const cv::Point3d &enu, const GeodeticCoords &origin)
Definition: GeodeticCoords.cpp:178
rtabmap::Optimizer::create
static Optimizer * create(const ParametersMap &parameters)
Definition: Optimizer.cpp:72
rtabmap::util3d::laserScan2dFromPointCloud
LaserScan RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud< pcl::PointXYZ > &cloud, const Transform &transform=Transform(), bool filterNaNs=true)
Definition: util3d.cpp:1920
rtabmap::ImageView::setFeatures
void setFeatures(const std::multimap< int, cv::KeyPoint > &refWords, const cv::Mat &depth=cv::Mat(), const QColor &color=Qt::yellow)
Definition: ImageView.cpp:1045
uValue
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
Definition: UStl.h:240
rtabmap::LaserScan::size
int size() const
Definition: LaserScan.h:126
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::DBDriver::getUrl
const std::string & getUrl() const
Definition: DBDriver.h:72
rtabmap::DBDriver::getAllStatisticsWmStates
std::map< int, std::vector< int > > getAllStatisticsWmStates() const
Definition: DBDriver.cpp:266
rtabmap::util3d::assemblePolygonMesh
pcl::PolygonMesh::Ptr RTABMAP_EXP assemblePolygonMesh(const cv::Mat &cloudMat, const std::vector< std::vector< RTABMAP_PCL_INDEX > > &polygons)
Definition: util3d_surface.cpp:1395
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
rtabmap::DatabaseViewer::editMapArea_
EditMapArea * editMapArea_
Definition: DatabaseViewer.h:239
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
rtabmap::CloudViewer::setCloudOpacity
void setCloudOpacity(const std::string &id, double opacity=1.0)
Definition: CloudViewer.cpp:3120
rtabmap::graph::calcKittiSequenceErrors
void RTABMAP_EXP calcKittiSequenceErrors(const std::vector< Transform > &poses_gt, const std::vector< Transform > &poses_result, float &t_err, float &r_err)
Definition: Graph.cpp:649
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:443
rtabmap::Signature::getWords
const std::multimap< int, int > & getWords() const
Definition: Signature.h:111
util3d_surface.h
rtabmap::Transform::toEigen3d
Eigen::Affine3d toEigen3d() const
Definition: Transform.cpp:386
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
UINFO
#define UINFO(...)

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:28