diff --git a/guilib/include/rtabmap/gui/DatabaseViewer.h b/guilib/include/rtabmap/gui/DatabaseViewer.h index bcd6735b..e6e97f62 100644 --- a/guilib/include/rtabmap/gui/DatabaseViewer.h +++ b/guilib/include/rtabmap/gui/DatabaseViewer.h @@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -71,6 +72,24 @@ class EditMapArea; class LinkRefiningDialog; class Registration; class RegistrationIcp; +class ExportDialog; + +class GraphComponent +{ +public: + GraphComponent() : id(0), rootId(0), firstNodeId(0), lastNodeId(0), rotationValue(0), viewScale(-1.0f), optimizationFlavor(0) {} + + int id; + int rootId; + int firstNodeId; + int lastNodeId; + int rotationValue; + QPointF rootCoordinates; + float viewScale; + std::set nodeIds; + std::set selectedNodeIds; + int optimizationFlavor; +}; class RTABMAP_GUI_EXPORT DatabaseViewer : public QMainWindow { @@ -85,6 +104,10 @@ public: protected: virtual void showEvent(QShowEvent* anEvent); + +private: + void backupCurrentComponent(); + virtual void moveEvent(QMoveEvent* anEvent); virtual void resizeEvent(QResizeEvent* anEvent); virtual void keyPressEvent(QKeyEvent *event); @@ -92,6 +115,7 @@ protected: virtual bool eventFilter(QObject *obj, QEvent *event); private Q_SLOTS: + void updateGraphInteractionMode(); void writeSettings(); void restoreDefaultSettings(); void configModified(); @@ -131,6 +155,7 @@ private Q_SLOTS: void view3DMap(); void generate3DMap(); void detectMoreLoopClosures(); + void mergeComponents(); void updateAllNeighborCovariances(); void updateAllLoopClosureCovariances(); void updateAllLandmarkCovariances(); @@ -138,6 +163,7 @@ private Q_SLOTS: void resetAllChanges(); void graphNodeSelected(int); void graphLinkSelected(int, int); + void zoomToNode(); void sliderAValueChanged(int); void sliderBValueChanged(int); void sliderAMoved(int); @@ -146,6 +172,7 @@ private Q_SLOTS: void sliderNeighborValueChanged(int); void sliderLoopValueChanged(int); void sliderIterationsValueChanged(int); + void updateGraphViewRootId(); void editConstraint(); void updateGrid(); void updateOctomapView(); @@ -161,6 +188,12 @@ private Q_SLOTS: void notifyParametersChanged(const QStringList &); void setupMainLayout(bool vertical); void updateConstraintButtons(); + void optimizationIgnoredStateChanged(); + void fitInViewClicked(); + void tabBarComponentsValueChanged(int); + void regenerateGraphComponents(); + void updateComponentSelectedNodes(); + void clearAllSelectedNodes(); private: QString getIniFilePath() const; @@ -175,7 +208,7 @@ private: QLabel * label, QLabel * stamp, rtabmap::ImageView * view, - QLabel * labelId, + QToolButton * buttonNodeId, QLabel * labelMapId, QLabel * labelPose, QLabel * labelOptPose, @@ -205,7 +238,8 @@ private: std::multimap updateLinksWithModifications( const std::multimap & edgeConstraints); void updateNeighborsSlider(int from = 0, int to = 0); - void updateLoopClosuresSlider(int from = 0, int to = 0); + void updateLoopClosuresSlider(int from, int to); + void updateLoopClosuresSlider(); void updateCovariances(const QList & links); void refineLinks(const QList & links); void refineConstraint(int from, int to, Registration * reg, RegistrationIcp * regIcp, bool silent); @@ -234,6 +268,9 @@ private: std::string databaseFileName_; std::list > graphes_; std::multimap graphLinks_; + std::vector components_; + int activeComponentIndex_; + int lastValidNodeId_; std::map odomPoses_; std::map groundTruthPoses_; std::map gpsPoses_; @@ -253,6 +290,7 @@ private: QDialog * editMapDialog_; EditMapArea * editMapArea_; LinkRefiningDialog * linkRefiningDialog_; + ExportDialog * exportDataDialog_; bool savedMaximized_; bool firstCall_; diff --git a/guilib/include/rtabmap/gui/ExportDialog.h b/guilib/include/rtabmap/gui/ExportDialog.h index 2db6f992..28a036af 100644 --- a/guilib/include/rtabmap/gui/ExportDialog.h +++ b/guilib/include/rtabmap/gui/ExportDialog.h @@ -53,6 +53,8 @@ public: int framesIgnored() const; double targetFramerate() const; int sessionExported() const; + bool currentComponentOnly() const; + bool selectedNodesOnly() const; bool isRgbExported() const; bool isDepthExported() const; bool isDepth2dExported() const; diff --git a/guilib/include/rtabmap/gui/GraphViewer.h b/guilib/include/rtabmap/gui/GraphViewer.h index 75d35135..8ce6746f 100644 --- a/guilib/include/rtabmap/gui/GraphViewer.h +++ b/guilib/include/rtabmap/gui/GraphViewer.h @@ -57,6 +57,7 @@ class RTABMAP_GUI_EXPORT GraphViewer : public QGraphicsView { public: enum ViewPlane {XY, XZ, YZ}; + enum InteractionMode {HandMode, SelectionMode}; public: GraphViewer(QWidget * parent = 0); @@ -172,14 +173,21 @@ public: void setOdomCacheOverlayVisible(bool visible); void setOrientationENU(bool enabled); void setViewPlane(ViewPlane plane); + void setInteractionMode(InteractionMode mode); + InteractionMode getInteractionMode() const {return _interactionMode;} + std::set getSelectedNodeIds(); + void selectNodesFromIds(const std::set & ids); void setEnsureFrameVisible(bool visible); void setFastZoomMinNodes(int value); + QPointF getNodeScenePosition(int id) const; + void centerOnNode(int id); Q_SIGNALS: void configChanged(); void mapShownRequested(); void nodeSelected(int); void linkSelected(int, int); + void nodesSelected(); // Nodes selected by rubber band, call getSelectedNodeIds() to get ids selected public Q_SLOTS: void restoreDefaults(); @@ -253,6 +261,7 @@ private: bool _orientationENU; bool _mouseTracking; ViewPlane _viewPlane; + InteractionMode _interactionMode; bool _ensureFrameVisible; QPoint _previousMousePos; QPoint _initialMousePos; diff --git a/guilib/include/rtabmap/gui/LinkRefiningDialog.h b/guilib/include/rtabmap/gui/LinkRefiningDialog.h index 67afc4dc..548ce702 100644 --- a/guilib/include/rtabmap/gui/LinkRefiningDialog.h +++ b/guilib/include/rtabmap/gui/LinkRefiningDialog.h @@ -52,9 +52,13 @@ public: int nodeIdMax, int mapIdMin, int mapIdMax); + + void setRangesEnabled(bool selectedNodes, bool currentComponent); Link::Type getLinkType() const; void getIntraInterSessions(bool & intra, bool & inter) const; + bool isRangeByCurrentComponent() const; + bool isRangeBySelectedNodeIds() const; bool isRangeByNodeId() const; bool isRangeByMapId() const; void getRangeNodeId(int & from, int & to) const; diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 63ec3694..b1475d3e 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -110,11 +110,14 @@ namespace rtabmap { DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : QMainWindow(parent), dbDriver_(0), + activeComponentIndex_(0), + lastValidNodeId_(0), octomap_(0), exportDialog_(new ExportCloudsDialog(this)), editDepthDialog_(new QDialog(this)), editMapDialog_(new QDialog(this)), linkRefiningDialog_(new LinkRefiningDialog(this)), + exportDataDialog_(new ExportDialog(this)), savedMaximized_(false), firstCall_(true), iniFilePath_(ini), @@ -243,8 +246,10 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : parameters.insert(*Parameters::getDefaultParameters().find(Parameters::kMemSTMSize())); ui_->parameters_toolbox->setupUi(parameters); exportDialog_->setObjectName("ExportCloudsDialog"); + exportDataDialog_->setObjectName("ExportDataDialog"); restoreDefaultSettings(); this->readSettings(); + updateGraphInteractionMode(); setupMainLayout(ui_->actionVertical_Layout->isChecked()); ui_->comboBox_octomap_rendering_type->setVisible(ui_->checkBox_octomap->isChecked()); @@ -272,6 +277,9 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : connect(ui_->graphViewer, SIGNAL(nodeSelected(int)), this , SLOT(graphNodeSelected(int))); connect(ui_->graphViewer, SIGNAL(linkSelected(int,int)), this , SLOT(graphLinkSelected(int,int))); + connect(ui_->graphViewer, SIGNAL(nodesSelected()), this , SLOT(updateComponentSelectedNodes())); + connect(ui_->radioButton_graphHand, SIGNAL(toggled(bool)), this, SLOT(updateGraphInteractionMode())); + connect(ui_->radioButton_graphSelection, SIGNAL(toggled(bool)), this, SLOT(updateGraphInteractionMode())); connect(ui_->parameters_toolbox, SIGNAL(parametersChanged(const QStringList &)), this, SLOT(notifyParametersChanged(const QStringList &))); @@ -311,12 +319,14 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : connect(ui_->actionView_3D_map, SIGNAL(triggered()), this, SLOT(view3DMap())); connect(ui_->actionGenerate_3D_map_pcd, SIGNAL(triggered()), this, SLOT(generate3DMap())); connect(ui_->actionDetect_more_loop_closures, SIGNAL(triggered()), this, SLOT(detectMoreLoopClosures())); + connect(ui_->actionMerge_components_using_selected_nodes, SIGNAL(triggered()), this, SLOT(mergeComponents())); connect(ui_->actionUpdate_all_neighbor_covariances, SIGNAL(triggered()), this, SLOT(updateAllNeighborCovariances())); connect(ui_->actionUpdate_all_loop_closure_covariances, SIGNAL(triggered()), this, SLOT(updateAllLoopClosureCovariances())); connect(ui_->actionUpdate_all_landmark_covariances, SIGNAL(triggered()), this, SLOT(updateAllLandmarkCovariances())); connect(ui_->actionRefine_links, SIGNAL(triggered()), this, SLOT(refineLinks())); connect(ui_->actionRegenerate_local_grid_maps, SIGNAL(triggered()), this, SLOT(regenerateLocalMaps())); connect(ui_->actionRegenerate_local_grid_maps_selected, SIGNAL(triggered()), this, SLOT(regenerateCurrentLocalMaps())); + connect(ui_->actionClear_all_selected_nodes, SIGNAL(triggered()), this, SLOT(clearAllSelectedNodes())); connect(ui_->actionReset_all_changes, SIGNAL(triggered()), this, SLOT(resetAllChanges())); connect(ui_->actionRestore_default_GUI_settings, SIGNAL(triggered()), this, SLOT(restoreDefaultSettings())); @@ -357,6 +367,8 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : ui_->spinBox_indexB->setEnabled(false); connect(ui_->spinBox_indexA, SIGNAL(valueChanged(int)), ui_->horizontalSlider_A, SLOT(setValue(int))); connect(ui_->spinBox_indexB, SIGNAL(valueChanged(int)), ui_->horizontalSlider_B, SLOT(setValue(int))); + connect(ui_->button_nodeIdA, SIGNAL(clicked(bool)), this, SLOT(zoomToNode())); + connect(ui_->button_nodeIdB, SIGNAL(clicked(bool)), this, SLOT(zoomToNode())); connect(ui_->toolButton_edit_priorA, SIGNAL(clicked(bool)), this, SLOT(editConstraint())); connect(ui_->toolButton_edit_priorB, SIGNAL(clicked(bool)), this, SLOT(editConstraint())); @@ -395,13 +407,15 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : ui_->checkBox_showOptimized->setEnabled(false); connect(ui_->toolButton_constraint, SIGNAL(clicked(bool)), this, SLOT(editConstraint())); connect(ui_->checkBox_enableForAll, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintButtons())); + connect(ui_->checkBox_optimize_on_all_changes, SIGNAL(stateChanged(int)), this, SLOT(optimizationIgnoredStateChanged())); ui_->horizontalSlider_iterations->setTracking(true); ui_->horizontalSlider_iterations->setEnabled(false); ui_->spinBox_optimizationsFrom->setEnabled(false); connect(ui_->horizontalSlider_iterations, SIGNAL(valueChanged(int)), this, SLOT(sliderIterationsValueChanged(int))); - connect(ui_->spinBox_optimizationsFrom, SIGNAL(editingFinished()), this, SLOT(updateGraphView())); + connect(ui_->spinBox_optimizationsFrom, SIGNAL(editingFinished()), this, SLOT(updateGraphViewRootId())); connect(ui_->comboBox_optimizationFlavor, SIGNAL(activated(int)), this, SLOT(updateGraphView())); + connect(ui_->tabBar_components, SIGNAL(currentChanged(int)), this, SLOT(tabBarComponentsValueChanged(int))); connect(ui_->checkBox_spanAllMaps, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); connect(ui_->checkBox_wmState, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); connect(ui_->graphViewer, SIGNAL(mapShownRequested()), this, SLOT(updateGraphView())); @@ -471,6 +485,8 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : connect(ui_->doubleSpinBox_posefilteringAngle, SIGNAL(valueChanged(double)), this, SLOT(configModified())); connect(ui_->horizontalSlider_rotation, SIGNAL(valueChanged(int)), this, SLOT(updateGraphRotation())); connect(ui_->pushButton_applyRotation, SIGNAL(clicked()), this, SLOT(updateGraphView())); + connect(ui_->checkBox_fit_in_view, SIGNAL(stateChanged(int)), this, SLOT(configModified())); + connect(ui_->checkBox_fit_in_view, SIGNAL(stateChanged(int)), this, SLOT(fitInViewClicked())); connect(ui_->spinBox_icp_decimation, SIGNAL(valueChanged(int)), this, SLOT(configModified())); connect(ui_->doubleSpinBox_icp_maxDepth, SIGNAL(valueChanged(double)), this, SLOT(configModified())); @@ -503,6 +519,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : connect(ui_->checkBox_grid_showProbMap, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView())); connect(exportDialog_, SIGNAL(configChanged()), this, SLOT(configModified())); + connect(exportDataDialog_, SIGNAL(configChanged()), this, SLOT(configModified())); // dockwidget QList dockWidgets = this->findChildren(); @@ -578,6 +595,18 @@ void DatabaseViewer::configModified() this->setWindowModified(true); } +void DatabaseViewer::updateGraphInteractionMode() +{ + if(ui_->radioButton_graphSelection->isChecked()) + { + ui_->graphViewer->setInteractionMode(GraphViewer::SelectionMode); + } + else + { + ui_->graphViewer->setInteractionMode(GraphViewer::HandMode); + } +} + QString DatabaseViewer::getIniFilePath() const { if(!iniFilePath_.isEmpty()) @@ -637,6 +666,7 @@ void DatabaseViewer::readSettings() ui_->actionVertical_Layout->setChecked(settings.value("verticalLayout", ui_->actionVertical_Layout->isChecked()).toBool()); ui_->actionConcise_Layout->setChecked(settings.value("conciseLayout", ui_->actionConcise_Layout->isChecked()).toBool()); ui_->checkBox_ignoreIntermediateNodes->setChecked(settings.value("ignoreIntermediateNodes", ui_->checkBox_ignoreIntermediateNodes->isChecked()).toBool()); + ui_->checkBox_fit_in_view->setChecked(settings.value("fitInView", ui_->checkBox_fit_in_view->isChecked()).toBool()); ui_->checkBox_timeStats->setChecked(settings.value("timeStats", ui_->checkBox_timeStats->isChecked()).toBool()); // GraphViewer settings @@ -694,6 +724,7 @@ void DatabaseViewer::readSettings() // Use same parameters used by RTAB-Map settings.beginGroup("Gui"); exportDialog_->loadSettings(settings, exportDialog_->objectName()); + exportDataDialog_->loadSettings(settings, exportDataDialog_->objectName()); settings.beginGroup("PostProcessingDialog"); ui_->doubleSpinBox_detectMore_radius->setValue(settings.value("cluster_radius", ui_->doubleSpinBox_detectMore_radius->value()).toDouble()); ui_->doubleSpinBox_detectMore_radiusMin->setValue(settings.value("cluster_radius_min", ui_->doubleSpinBox_detectMore_radiusMin->value()).toDouble()); @@ -734,6 +765,7 @@ void DatabaseViewer::writeSettings() settings.setValue("verticalLayout", ui_->actionVertical_Layout->isChecked()); settings.setValue("conciseLayout", ui_->actionConcise_Layout->isChecked()); settings.setValue("ignoreIntermediateNodes", ui_->checkBox_ignoreIntermediateNodes->isChecked()); + settings.setValue("fitInView", ui_->checkBox_fit_in_view->isChecked()); settings.setValue("timeStats", ui_->checkBox_timeStats->isChecked()); // save GraphViewer settings @@ -792,6 +824,7 @@ void DatabaseViewer::writeSettings() // Use same parameters used by RTAB-Map settings.beginGroup("Gui"); exportDialog_->saveSettings(settings, exportDialog_->objectName()); + exportDataDialog_->saveSettings(settings, exportDataDialog_->objectName()); settings.beginGroup("PostProcessingDialog"); settings.setValue("cluster_radius", ui_->doubleSpinBox_detectMore_radius->value()); settings.setValue("cluster_radius_min", ui_->doubleSpinBox_detectMore_radiusMin->value()); @@ -829,6 +862,7 @@ void DatabaseViewer::restoreDefaultSettings() ui_->checkBox_alignPosesWithGroundTruth->setChecked(true); ui_->checkBox_alignScansCloudsWithGroundTruth->setChecked(false); ui_->checkBox_ignoreIntermediateNodes->setChecked(false); + ui_->checkBox_fit_in_view->setChecked(true); ui_->checkBox_timeStats->setChecked(true); ui_->comboBox_optimizationFlavor->setCurrentIndex(0); @@ -883,6 +917,8 @@ void DatabaseViewer::restoreDefaultSettings() ui_->checkBox_opt_graph_as_guess->setChecked(true); ui_->spinBox_fromToMapId->setValue(-1); ui_->spinBox_minGraphDistance->setValue(10); + ui_->radioButton_graphHand->setChecked(true); + ui_->checkBox_optimize_on_all_changes->setChecked(true); } void DatabaseViewer::openDatabase() @@ -1166,7 +1202,6 @@ bool DatabaseViewer::closeDatabase() localMaps_.clear(); generatedLocalMaps_.clear(); modifiedLaserScans_.clear(); - ui_->graphViewer->clearAll(); occupancyGridViewer_->clear(); ui_->menuEdit->setEnabled(false); ui_->actionGenerate_3D_map_pcd->setEnabled(false); @@ -1214,6 +1249,9 @@ bool DatabaseViewer::closeDatabase() ui_->horizontalSlider_iterations->setMaximum(0); ui_->horizontalSlider_neighbors->setEnabled(false); ui_->horizontalSlider_neighbors->setMaximum(0); + components_.clear(); // should be called before ui_->graphViewer->clearAll() to ignore selection events + activeComponentIndex_ = 0; + ui_->radioButton_graphSelection->setText(tr("Selection")); // clear count ui_->label_constraint->clear(); ui_->label_constraint_opt->clear(); ui_->label_variance->clear(); @@ -1226,8 +1264,8 @@ bool DatabaseViewer::closeDatabase() ui_->horizontalSlider_A->setMaximum(0); ui_->horizontalSlider_B->setEnabled(false); ui_->horizontalSlider_B->setMaximum(0); - ui_->label_idA->setText("NaN"); - ui_->label_idB->setText("NaN"); + ui_->button_nodeIdA->setText("NaN"); + ui_->button_nodeIdB->setText("NaN"); sliderAValueChanged(0); sliderBValueChanged(0); @@ -1404,16 +1442,58 @@ void DatabaseViewer::exportDatabase() return; } - rtabmap::ExportDialog dialog; - - if(dialog.exec()) + if(exportDataDialog_->exec()) { - if(!dialog.outputPath().isEmpty()) + if(!exportDataDialog_->outputPath().isEmpty()) { - int framesIgnored = dialog.framesIgnored(); - double frameRate = dialog.targetFramerate(); - int sessionExported = dialog.sessionExported(); - QString path = dialog.outputPath(); + int framesIgnored = exportDataDialog_->framesIgnored(); + double frameRate = exportDataDialog_->targetFramerate(); + int sessionExported = exportDataDialog_->sessionExported(); + QString path = exportDataDialog_->outputPath(); + std::set selectedIds; + if(exportDataDialog_->currentComponentOnly()) { + if(exportDataDialog_->selectedNodesOnly()) { + selectedIds = ui_->graphViewer->getSelectedNodeIds(); + if(selectedIds.empty()) + { + QMessageBox::warning(this, + tr("Cannot export database"), + tr("Exporting only selected nodes is requested but none is selected for the current component of the Graph View.")); + return; + } + } + else { + if(components_.empty()) + { + QMessageBox::warning(this, + tr("Cannot export database"), + tr("Exporting only the current component of the Graph View is requested, but the graph is not initialized. Open the Graph View to initialize the components." )); + return; + } + UASSERT(activeComponentIndex_>=0 && activeComponentIndex_<(int)components_.size()); + selectedIds = components_[activeComponentIndex_].nodeIds; + if(selectedIds.empty()) + { + QMessageBox::warning(this, + tr("Cannot export database"), + tr("Exporting only current component of the Graph View, but the current component is empty?!" )); + return; + } + } + } + else if(exportDataDialog_->selectedNodesOnly()) + { + for(const auto & comp: components_) + { + selectedIds.insert(comp.selectedNodeIds.begin(), comp.selectedNodeIds.end()); + } + if(selectedIds.empty()) + { + QMessageBox::warning(this, tr("Cannot export database"), tr("Exporting only selected nodes is requested but none is selected in the Graph View.")); + return; + } + } + rtabmap::DataRecorder recorder; QList ids; @@ -1442,7 +1522,8 @@ void DatabaseViewer::exportDatabase() stamp == 0 || stamp - previousStamp >= 1.0/frameRate) { - if(sessionExported < 0 || sessionExported == mapId) + if((sessionExported < 0 || sessionExported == mapId) && + (selectedIds.empty() || selectedIds.find(ids_[i]) != selectedIds.end())) { ids.push_back(ids_[i]); @@ -1481,10 +1562,10 @@ void DatabaseViewer::exportDatabase() progressDialog->show(); progressDialog->setCancelButtonVisible(true); UINFO("Decompress: rgb=%d depth=%d scan=%d userData=%d", - dialog.isRgbExported()?1:0, - dialog.isDepthExported()?1:0, - dialog.isDepth2dExported()?1:0, - dialog.isUserDataExported()?1:0); + exportDataDialog_->isRgbExported()?1:0, + exportDataDialog_->isDepthExported()?1:0, + exportDataDialog_->isDepth2dExported()?1:0, + exportDataDialog_->isUserDataExported()?1:0); for(int i=0; iisCanceled(); ++i) { @@ -1495,12 +1576,12 @@ void DatabaseViewer::exportDatabase() cv::Mat depth, rgb, userData; LaserScan scan; data.uncompressDataConst( - !dialog.isRgbExported()?0:&rgb, - !dialog.isDepthExported()?0:&depth, - !dialog.isDepth2dExported()?0:&scan, - !dialog.isUserDataExported()?0:&userData); + !exportDataDialog_->isRgbExported()?0:&rgb, + !exportDataDialog_->isDepthExported()?0:&depth, + !exportDataDialog_->isDepth2dExported()?0:&scan, + !exportDataDialog_->isUserDataExported()?0:&userData); cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1); - if(dialog.isOdomExported()) + if(exportDataDialog_->isOdomExported()) { std::multimap links; dbDriver_->loadLinks(id, links, Link::kNeighbor); @@ -1546,7 +1627,7 @@ void DatabaseViewer::exportDatabase() sensorData.setEnvSensors(sensorsValues.at(id)); } - recorder.addData(sensorData, dialog.isOdomExported()?poses.at(id):Transform(), covariance); + recorder.addData(sensorData, exportDataDialog_->isOdomExported()?poses.at(id):Transform(), covariance); progressDialog->appendText(tr("Exported node %1").arg(id)); progressDialog->incrementStep(); @@ -1876,6 +1957,9 @@ void DatabaseViewer::updateIds() ui_->actionView_optimized_mesh->setEnabled(false); ui_->actionExport_optimized_mesh->setEnabled(false); ui_->actionUpdate_optimized_mesh->setEnabled(uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.13.0") >= 0); + ui_->pushButton_applyRotation->setEnabled(true); + Qt::ItemFlags enableFlags = Qt::ItemFlags(Qt::ItemIsEnabled) | Qt::ItemIsSelectable; + ui_->comboBox_optimizationFlavor->setItemData(2, QVariant(static_cast(enableFlags)), Qt::UserRole - 1); links_.clear(); linksAdded_.clear(); linksRefined_.clear(); @@ -1926,7 +2010,7 @@ void DatabaseViewer::updateIds() uSleep(100); QApplication::processEvents(); - int lastValidNodeId = 0; + lastValidNodeId_ = 0; for(int i=0; isecond.type() == Link::kNeighbor || iter->second.type() == Link::kNeighborMerged) { - lastValidNodeId = ids_[i]; + lastValidNodeId_ = ids_[i]; } } } @@ -2066,17 +2150,14 @@ void DatabaseViewer::updateIds() } float xMin, yMin, cellSize; - bool hasMap = !dbDriver_->load2DMap(xMin, yMin, cellSize).empty(); + bool hasMap = !dbDriver_->load2DMap(xMin, yMin, cellSize).empty(); + bool hasMesh = !dbDriver_->loadOptimizedMesh().empty(); ui_->actionEdit_optimized_2D_map->setEnabled(hasMap); ui_->actionExport_saved_2D_map->setEnabled(hasMap); ui_->actionImport_2D_map->setEnabled(hasMap); ui_->actionRegenerate_optimized_2D_map->setEnabled(uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.17.0") >= 0); - - if(!dbDriver_->loadOptimizedMesh().empty()) - { - ui_->actionView_optimized_mesh->setEnabled(true); - ui_->actionExport_optimized_mesh->setEnabled(true); - } + ui_->actionView_optimized_mesh->setEnabled(hasMesh); + ui_->actionExport_optimized_mesh->setEnabled(hasMesh); UINFO("Loaded %d ids, %d poses and %d links", (int)ids_.size(), (int)odomPoses_.size(), (int)links_.size()); @@ -2155,16 +2236,16 @@ void DatabaseViewer::updateIds() } } - if(lastValidNodeId>0) + if(lastValidNodeId_>0) { // find full connected graph from last node in working memory Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters()); std::map posesOut; std::multimap linksOut; - UINFO("Get connected graph from %d (%d poses, %d links)", lastValidNodeId, (int)odomPoses_.size(), (int)links_.size()); + UINFO("Get connected graph from %d (%d poses, %d links)", lastValidNodeId_, (int)odomPoses_.size(), (int)links_.size()); optimizer->getConnectedGraph( - lastValidNodeId, + lastValidNodeId_, odomPoses_, links_, posesOut, @@ -2220,8 +2301,8 @@ void DatabaseViewer::updateIds() ui_->spinBox_indexA->setEnabled(false); ui_->spinBox_indexB->setEnabled(false); - ui_->label_idA->setText("NaN"); - ui_->label_idB->setText("NaN"); + ui_->button_nodeIdA->setText("NaN"); + ui_->button_nodeIdB->setText("NaN"); } if(ids_.size()) @@ -2487,6 +2568,11 @@ void DatabaseViewer::editDepthImage() this->update3dView(); } } + else + { + QMessageBox::warning(this, tr("Cannot edit depth"), tr("Node %1 has no depth image!").arg(id)); + return; + } } } @@ -3336,6 +3422,10 @@ void DatabaseViewer::import2DMap() void DatabaseViewer::regenerateSavedMap() { + // Make sure this action is disabled if + // current active component is not main + UASSERT(activeComponentIndex_ == 0); + if(!dbDriver_) { QMessageBox::warning(this, tr("Cannot import 2D map"), tr("A database must must loaded first...\nUse File->Open database.")); @@ -3602,6 +3692,10 @@ void DatabaseViewer::exportOptimizedMesh() void DatabaseViewer::updateOptimizedMesh() { + // Make sure this action is disabled if + // current active component is not main + UASSERT(activeComponentIndex_ == 0); + if(!ids_.size() || !dbDriver_) { QMessageBox::warning(this, tr("Cannot generate a graph"), tr("The database is empty...")); @@ -4000,9 +4094,18 @@ void DatabaseViewer::regenerateCurrentLocalMaps() return; } + std::set selectedIds = ui_->graphViewer->getSelectedNodeIds(); QSet idsSet; - idsSet.insert(ids_.at(ui_->horizontalSlider_A->value())); - idsSet.insert(ids_.at(ui_->horizontalSlider_B->value())); + for(auto id : selectedIds) + { + idsSet.insert(id); + } + if(idsSet.empty()) + { + // legacy behavior: used currently shown nodes + idsSet.insert(ids_.at(ui_->horizontalSlider_A->value())); + idsSet.insert(ids_.at(ui_->horizontalSlider_B->value())); + } #if QT_VERSION >= QT_VERSION_CHECK(5, 15, 3) QList ids(idsSet.begin(), idsSet.end()); #else @@ -4153,6 +4256,21 @@ void DatabaseViewer::view3DMap() } if(optimizedPoses.size() > 0) { + std::set selectedIds = ui_->graphViewer->getSelectedNodeIds(); + if(!selectedIds.empty()) + { + UINFO("Viewing %ld/%ld selected nodes of the current component.", selectedIds.size(), optimizedPoses.size()); + for(std::map::iterator iter = optimizedPoses.begin(); iter != optimizedPoses.end();) + { + if(selectedIds.find(iter->first) == selectedIds.end()) { + iter = optimizedPoses.erase(iter); + } + else { + ++iter; + } + } + } + exportDialog_->setDBDriver(dbDriver_); exportDialog_->viewClouds(optimizedPoses, updateLinksWithModifications(links_), @@ -4272,6 +4390,21 @@ void DatabaseViewer::generate3DMap() } if(optimizedPoses.size() > 0) { + std::set selectedIds = ui_->graphViewer->getSelectedNodeIds(); + if(!selectedIds.empty()) + { + UINFO("Exporting %ld/%ld selected nodes of the current component.", selectedIds.size(), optimizedPoses.size()); + for(std::map::iterator iter = optimizedPoses.begin(); iter != optimizedPoses.end();) + { + if(selectedIds.find(iter->first) == selectedIds.end()) { + iter = optimizedPoses.erase(iter); + } + else { + ++iter; + } + } + } + exportDialog_->setDBDriver(dbDriver_); exportDialog_->exportClouds(optimizedPoses, updateLinksWithModifications(links_), @@ -4327,11 +4460,14 @@ void DatabaseViewer::detectMoreLoopClosures() bool intraSession = ui_->checkBox_detectMore_intraSession->isChecked(); bool interSession = ui_->checkBox_detectMore_interSession->isChecked(); bool useOptimizedGraphAsGuess = ui_->checkBox_opt_graph_as_guess->isChecked(); + std::set selectedIds = ui_->graphViewer->getSelectedNodeIds(); int fromToMapId = ui_->spinBox_fromToMapId->value(); int minimumGraphDistance = ui_->spinBox_minGraphDistance->value(); - if(!interSession && !intraSession) + if(!interSession && !intraSession && selectedIds.empty()) { - QMessageBox::warning(this, tr("Cannot detect more loop closures"), tr("Intra and inter session parameters are disabled! Enable one or both.")); + QMessageBox::warning(this, + tr("Cannot detect more loop closures"), + tr("Intra and inter session parameters are disabled! Enable one or both. You can also select specific nodes of the current component instead.")); return; } @@ -4351,6 +4487,21 @@ void DatabaseViewer::detectMoreLoopClosures() break; } + if(!selectedIds.empty()) + { + progressDialog->appendText(tr("Looking for more loop closures for %1/%2 selected nodes of the current component.").arg(selectedIds.size()).arg(optimizedPoses.size())); + for(std::map::iterator iter = optimizedPoses.begin(); iter != optimizedPoses.end();) + { + if(selectedIds.find(iter->first) == selectedIds.end()) { + iter = optimizedPoses.erase(iter); + } + else { + ++iter; + } + } + } + + progressDialog->appendText(tr("Looking for more loop closures: %1 clusters found.").arg(clusters.size())); if(fromToMapId >=0) { @@ -4422,8 +4573,7 @@ void DatabaseViewer::detectMoreLoopClosures() int mapIdFrom = uValue(mapIds_, from, 0); int mapIdTo = uValue(mapIds_, to, 0); - if((interSession && mapIdFrom != mapIdTo) || - (intraSession && mapIdFrom == mapIdTo)) + if((((interSession && mapIdFrom != mapIdTo) || (intraSession && mapIdFrom == mapIdTo)) && selectedIds.empty()) || !selectedIds.empty()) { // only add new links and one per cluster per iteration if(rtabmap::graph::findLink(checkedLoopClosures, from, to) == checkedLoopClosures.end()) @@ -4457,7 +4607,15 @@ void DatabaseViewer::detectMoreLoopClosures() } } progressDialog->incrementStep(); - if(i%100) + if(clusters.size() <= 100) + { + QApplication::processEvents(); + } + else if(clusters.size() > 100 && i%10 == 0) + { + QApplication::processEvents(); + } + else if(clusters.size() > 1000 && i%100 == 0) { QApplication::processEvents(); } @@ -4489,6 +4647,127 @@ void DatabaseViewer::detectMoreLoopClosures() progressDialog->setValue(progressDialog->maximumSteps()); } +void DatabaseViewer::mergeComponents() +{ + std::shared_ptr reg(Registration::create(ui_->parameters_toolbox->getParameters())); + if(!reg->isImageRequired()) + { + QMessageBox::warning(this, + tr("Merge Components"), + tr("Registration is done without vision (see %1 parameter), so we cannot compute a " + "transformation guess between the nodes belonging to different components." + "\n\nIf the database has images, it is recommended to use %2 = 0 or 2 " + "instead so that the guess can be found visually. Open the \"Core Parameters\" panel " + "and the parameter is under Reg group.") + .arg(Parameters::kRegStrategy().c_str()).arg(Parameters::kRegStrategy().c_str())); + return; + } + + if(components_.size() < 2) + { + QMessageBox::warning(this, + tr("Merge Components"), + tr("A minimum of two components are required to merge them. Make sure Graph View " + "shows two disjoint components and select nodes from both components that should " + "be close by when merged.")); + return; + } + + int fromCompIndex = activeComponentIndex_; + int toCompIndex = -1; + + std::set activeSelection = ui_->graphViewer->getSelectedNodeIds(); + if(activeSelection.empty()) + { + QMessageBox::warning(this, tr("Merge Components"), tr("Please select nodes in the active component.")); + return; + } + + for(size_t i = 0; i < components_.size(); ++i) + { + if((int)i != fromCompIndex && !components_[i].selectedNodeIds.empty()) + { + if(toCompIndex == -1) + { + toCompIndex = (int)i; + } + else + { + QMessageBox::warning(this, tr("Merge Components"), tr("Please select nodes from exactly two different components.")); + return; + } + } + } + + if(toCompIndex == -1) + { + QMessageBox::warning(this, tr("Merge Components"), tr("Please select nodes in exactly one other component to merge with the active one.")); + return; + } + + std::vector N1(activeSelection.begin(), activeSelection.end()); + std::vector N2(components_[toCompIndex].selectedNodeIds.begin(), components_[toCompIndex].selectedNodeIds.end()); + + int totalPairs = (int)(N1.size() * N2.size()); + + rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this); + progressDialog->setAttribute(Qt::WA_DeleteOnClose); + progressDialog->setMaximumSteps(totalPairs); + progressDialog->setCancelButtonVisible(true); + progressDialog->setMinimumWidth(800); + progressDialog->show(); + progressDialog->appendText(tr("Looking for loop closure between %1 possible pairs...").arg(totalPairs)); + + bool loopFound = false; + int foundFrom = 0; + int foundTo = 0; + int step = 0; + + for(size_t i = 0; i < N1.size() && !loopFound && !progressDialog->isCanceled(); ++i) + { + for(size_t j = 0; j < N2.size() && !loopFound && !progressDialog->isCanceled(); ++j) + { + int from = N1[i]; + int to = N2[j]; + progressDialog->appendText(tr("Testing pair %1 and %2... (%3/%4)").arg(from).arg(to).arg(step+1).arg(totalPairs)); + + QApplication::processEvents(); + + if(addConstraint(from, to, reg.get(), true, false)) + { + loopFound = true; + foundFrom = from; + foundTo = to; + progressDialog->appendText(tr("Loop closure found between %1 and %2!").arg(from).arg(to)); + } + + ++step; + progressDialog->incrementStep(); + QApplication::processEvents(); + } + } + + progressDialog->setValue(progressDialog->maximumSteps()); + + if(loopFound) + { + QMessageBox::information(this, tr("Merge Components"), tr("Successfully merged components! Loop closure found between %1 and %2.").arg(foundFrom).arg(foundTo)); + + regenerateGraphComponents(); + this->updateGraphView(); + + // Combine both selections so the user can easily run detectMoreLoopClosures if they want + std::set finalSelection = activeSelection; + finalSelection.insert(N2.begin(), N2.end()); + ui_->graphViewer->selectNodesFromIds(finalSelection); + ui_->graphViewer->centerOnNode(foundFrom); + } + else if (!progressDialog->isCanceled()) + { + QMessageBox::information(this, tr("Merge Components"), tr("No loop closure found between the selected nodes of the two components.")); + } +} + void DatabaseViewer::updateAllNeighborCovariances() { std::multimap allLinks = updateLinksWithModifications(links_); @@ -4638,6 +4917,11 @@ void DatabaseViewer::refineLinks() maxNodeId, minMapId, maxMapId); + + std::set selectedNodeIds = ui_->graphViewer->getSelectedNodeIds(); + linkRefiningDialog_->setRangesEnabled( + !selectedNodeIds.empty(), + activeComponentIndex_>=0 && activeComponentIndex_<(int)components_.size() && !components_[activeComponentIndex_].nodeIds.empty()); if(linkRefiningDialog_->exec() == QDialog::Accepted) { @@ -4647,6 +4931,10 @@ void DatabaseViewer::refineLinks() linkRefiningDialog_->getRangeNodeId(minMapId, maxMapId); bool intra, inter; linkRefiningDialog_->getIntraInterSessions(intra, inter); + if(linkRefiningDialog_->isRangeByCurrentComponent()) + { + selectedNodeIds = components_[activeComponentIndex_].nodeIds; + } for(std::multimap::iterator iter=allLinks.begin(); iter!=allLinks.end(); ++iter) { if(iter->second.type() < Link::kPosePrior && @@ -4656,7 +4944,10 @@ void DatabaseViewer::refineLinks() int to = iter->second.to(); int mapFrom = uValue(mapIds_, from, 0); int mapTo = uValue(mapIds_, to, 0); - if(((linkRefiningDialog_->isRangeByNodeId() && + if((((linkRefiningDialog_->isRangeBySelectedNodeIds() || linkRefiningDialog_->isRangeByCurrentComponent()) && + (selectedNodeIds.find(from) != selectedNodeIds.end() && + selectedNodeIds.find(to) != selectedNodeIds.end())) || + (linkRefiningDialog_->isRangeByNodeId() && ((from >= minNodeId && from <= maxNodeId) || (to >= minNodeId && to <= maxNodeId))) || (linkRefiningDialog_->isRangeByMapId() && @@ -4745,6 +5036,42 @@ void DatabaseViewer::resetAllChanges() } } +void DatabaseViewer::zoomToNode() +{ + int value = -1; + if(sender() == ui_->button_nodeIdA) + { + value = ui_->spinBox_indexA->value(); + } + else if(sender() == ui_->button_nodeIdB) + { + value = ui_->spinBox_indexB->value(); + } + if(value >= 0 && value < ids_.size()) + { + int nodeId = ids_.at(value); + int componentIndex = -1; + for(int i = 0; i < (int)components_.size(); ++i) + { + if(components_[i].nodeIds.find(nodeId) != components_[i].nodeIds.end()) + { + componentIndex = i; + break; + } + } + if(componentIndex >= 0) + { + if(componentIndex != activeComponentIndex_) { + ui_->tabBar_components->setCurrentIndex(componentIndex); + } + else { + ui_->graphViewer->highlightNode(nodeId, sender()==ui_->button_nodeIdB?1:0); + } + ui_->graphViewer->centerOnNode(nodeId); + } + } +} + void DatabaseViewer::graphNodeSelected(int id) { if(id>0 && idToIndex_.contains(id)) @@ -4783,7 +5110,7 @@ void DatabaseViewer::sliderAValueChanged(int value) ui_->label_labelA, ui_->label_stampA, ui_->graphicsView_A, - ui_->label_idA, + ui_->button_nodeIdA, ui_->label_mapA, ui_->label_poseA, ui_->label_optposeA, @@ -4810,7 +5137,7 @@ void DatabaseViewer::sliderBValueChanged(int value) ui_->label_labelB, ui_->label_stampB, ui_->graphicsView_B, - ui_->label_idB, + ui_->button_nodeIdB, ui_->label_mapB, ui_->label_poseB, ui_->label_optposeB, @@ -4835,7 +5162,7 @@ void DatabaseViewer::update(int value, QLabel * label, QLabel * stamp, rtabmap::ImageView * view, - QLabel * labelId, + QToolButton * buttonNodeId, QLabel * labelMapId, QLabel * labelPose, QLabel * labelOptPose, @@ -4859,6 +5186,7 @@ void DatabaseViewer::update(int value, spinBoxIndex->blockSignals(false); labelParents->clear(); labelChildren->clear(); + buttonNodeId->setEnabled(false); weight->clear(); label->clear(); labelMapId->clear(); @@ -4880,7 +5208,7 @@ void DatabaseViewer::update(int value, view->clear(); int id = ids_.at(value); int mapId = -1; - labelId->setText(QString::number(id)); + buttonNodeId->setText(QString::number(id)); if(id>0) { if(ui_->dockWidget_graphView->isVisible()) { @@ -5001,6 +5329,7 @@ void DatabaseViewer::update(int value, weight->setNum(w); label->setText(l.c_str()); + buttonNodeId->setEnabled(w>=0 || w==-1); float x,y,z,roll,pitch,yaw; odomPose.getTranslationAndEulerAngles(x,y,z,roll, pitch,yaw); 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)); @@ -6252,7 +6581,7 @@ void DatabaseViewer::sliderAMoved(int value) ui_->spinBox_indexA->blockSignals(false); if(value>=0 && value < ids_.size()) { - ui_->label_idA->setText(QString::number(ids_.at(value))); + ui_->button_nodeIdA->setText(QString::number(ids_.at(value))); } else { @@ -6267,7 +6596,7 @@ void DatabaseViewer::sliderBMoved(int value) ui_->spinBox_indexB->blockSignals(false); if(value>=0 && value < ids_.size()) { - ui_->label_idB->setText(QString::number(ids_.at(value))); + ui_->button_nodeIdB->setText(QString::number(ids_.at(value))); } else { @@ -6371,6 +6700,7 @@ void DatabaseViewer::editConstraint() } if(priorId==0) { + regenerateGraphComponents(); this->updateGraphView(); updateConstraintView(); } @@ -6399,6 +6729,7 @@ void DatabaseViewer::editConstraint() updated = true; if(priorId==0) { + regenerateGraphComponents(); this->updateGraphView(); updateLoopClosuresSlider(from, to); } @@ -6421,20 +6752,12 @@ void DatabaseViewer::editConstraint() ui_->parameters_toolbox->updateParameter(Parameters::kOptimizerPriorsIgnored(), "false"); } } - int indexA = ui_->horizontalSlider_A->value(); - int indexB = ui_->horizontalSlider_B->value(); + if(!priorsIgnored) { + regenerateGraphComponents(); this->updateGraphView(); } - if(ui_->horizontalSlider_A->value() != indexA) - ui_->horizontalSlider_A->setValue(indexA); - else - sliderAValueChanged(indexA); - if(ui_->horizontalSlider_B->value() != indexB) - ui_->horizontalSlider_B->setValue(indexB); - else - sliderBValueChanged(indexB); } } } @@ -6585,7 +6908,7 @@ void DatabaseViewer::updateConstraintView( ui_->label_labelA, ui_->label_stampA, ui_->graphicsView_A, - ui_->label_idA, + ui_->button_nodeIdA, ui_->label_mapA, ui_->label_poseA, ui_->label_optposeA, @@ -6610,7 +6933,7 @@ void DatabaseViewer::updateConstraintView( ui_->label_labelB, ui_->label_stampB, ui_->graphicsView_B, - ui_->label_idB, + ui_->button_nodeIdB, ui_->label_mapB, ui_->label_poseB, ui_->label_optposeB, @@ -7202,6 +7525,22 @@ void DatabaseViewer::updateConstraintButtons() } } +void DatabaseViewer::optimizationIgnoredStateChanged() +{ + if(ui_->checkBox_optimize_on_all_changes->isChecked()) + { + this->updateGraphView(); + } +} + +void DatabaseViewer::fitInViewClicked() +{ + if(ui_->checkBox_fit_in_view->isChecked()) + { + sliderIterationsValueChanged(ui_->horizontalSlider_iterations->value()); + } +} + void DatabaseViewer::sliderIterationsValueChanged(int value) { UDEBUG("sender=%s value=%d currentValue = %d", sender()?sender()->objectName().toStdString().c_str():"NA", value, ui_->horizontalSlider_iterations->value()); @@ -7522,7 +7861,8 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) } } // Only zoom if there are significant unknowns - if( (xLast > xFirst && yLast > yFirst) && + if( ui_->checkBox_fit_in_view->isChecked() && + (xLast > xFirst && yLast > yFirst) && (xFirst > 50 || xLast < map.cols-50 || yFirst > 50 || @@ -7619,7 +7959,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) ui_->graphViewer->scene()->removeItem(rectScaleItem); delete rectScaleItem; } - else { + else if(ui_->checkBox_fit_in_view->isChecked()) { ui_->graphViewer->fitInView(ui_->graphViewer->sceneRect(), Qt::KeepAspectRatio); } @@ -7667,8 +8007,241 @@ void DatabaseViewer::updateGraphRotation() } } +void DatabaseViewer::backupCurrentComponent() +{ + UDEBUG(""); + if(activeComponentIndex_ < 0 || activeComponentIndex_ >= (int)components_.size()) return; + + GraphComponent & component = components_[activeComponentIndex_]; + + if(!ui_->graphViewer || component.rootId <= 0 || component.nodeIds.find(component.rootId) == component.nodeIds.end()) + { + component.viewScale = -1.0f; + component.rootCoordinates = QPointF(); + return; + } + + if(ui_->horizontalSlider_rotation->isEnabled()) + { + component.rotationValue = ui_->horizontalSlider_rotation->value(); + } + + component.viewScale = ui_->graphViewer->transform().m11(); + component.rootCoordinates = ui_->graphViewer->mapFromScene(ui_->graphViewer->getNodeScenePosition(component.rootId)); + component.selectedNodeIds = ui_->graphViewer->getSelectedNodeIds(); + component.optimizationFlavor = ui_->comboBox_optimizationFlavor->currentIndex(); +} + +void DatabaseViewer::regenerateGraphComponents() +{ + UDEBUG(""); + if(odomPoses_.empty()) { + components_.clear(); + ui_->radioButton_graphSelection->setText(tr("Selection")); + ui_->tabBar_components->setEnabled(false); + return; + } + + std::multimap links = updateLinksWithModifications(links_); + + backupCurrentComponent(); + + std::vector oldComponents = components_; + components_.clear(); + + std::map remainingPoses = odomPoses_; + + std::shared_ptr optimizer(Optimizer::create(ui_->parameters_toolbox->getParameters())); + bool optimizeFromGraphEnd = Parameters::defaultRGBDOptimizeFromGraphEnd(); + Parameters::parse(dbDriver_->getLastParameters(), Parameters::kRGBDOptimizeFromGraphEnd(), optimizeFromGraphEnd); + + /* Extracting new components */ + while(!remainingPoses.empty()) + { + int rootId = remainingPoses.begin()->first; + + // Ignore nodes with weight < 0 + if(weights_.find(rootId) == weights_.end() || weights_.at(rootId) < 0) + { + remainingPoses.erase(rootId); + continue; + } + + if( components_.empty() && + lastValidNodeId_>0 && + remainingPoses.find(lastValidNodeId_)!=remainingPoses.end() && + weights_.find(lastValidNodeId_) != weights_.end() && + weights_.at(lastValidNodeId_) >= 0) + { + // First component is the main one (the one linked to latest valid id in working memory) + rootId = lastValidNodeId_; + } + + std::map posesOut; + std::multimap linksOut; + optimizer->getConnectedGraph( + rootId, + remainingPoses, + links, + posesOut, + linksOut); + + if(!posesOut.empty()) + { + GraphComponent comp; + + if(posesOut.find(ui_->spinBox_optimizationsFrom->value()) != posesOut.end()) + { + comp.rootId = ui_->spinBox_optimizationsFrom->value(); + } + else if(optimizeFromGraphEnd) + { + comp.rootId = posesOut.rbegin()->first; + } + else + { + comp.rootId = posesOut.lower_bound(1)->first; + } + for(std::map::const_iterator iter=posesOut.begin(); iter!=posesOut.end(); ++iter) + { + comp.nodeIds.insert(comp.nodeIds.end(), iter->first); + remainingPoses.erase(iter->first); + } + comp.firstNodeId = posesOut.lower_bound(1)->first; + comp.lastNodeId = posesOut.rbegin()->first; + + components_.push_back(comp); + } + else { + //Failed, just remove it + remainingPoses.erase(rootId); + } + } + + /* Mapping old components and new ones */ + int newActiveIndex = -1; + if (!oldComponents.empty()) + { + for(size_t i = 0; i < oldComponents.size(); ++i) + { + const GraphComponent & oldComp = oldComponents[i]; + // Check if the old component still exist, + // if so, check if it has been modified + bool found = false; + for(size_t j = 0; j < components_.size() && !found; ++j) + { + GraphComponent & newComp = components_[j]; + for(const auto & id: oldComp.nodeIds) + { + if(newComp.nodeIds.find(id) != newComp.nodeIds.end()) + { + // If same size, copy old component's state + if(oldComp.nodeIds.size() == newComp.nodeIds.size()) + { + if(activeComponentIndex_ == (int)i) + { + newActiveIndex = (int)j; + } + newComp.rotationValue = oldComp.rotationValue; + newComp.viewScale = oldComp.viewScale; + newComp.rootId = oldComp.rootId; + newComp.rootCoordinates = oldComp.rootCoordinates; + newComp.selectedNodeIds = oldComp.selectedNodeIds; + } + found = true; + break; + } + } + } + } + } + + if (newActiveIndex != -1) + { + activeComponentIndex_ = newActiveIndex; + } + else + { + activeComponentIndex_ = 0; + for(size_t i = 0; i < components_.size(); ++i) { + if (ui_->spinBox_optimizationsFrom->value() == components_[i].rootId) { + activeComponentIndex_ = (int)i; + break; + } + } + } + + ui_->tabBar_components->blockSignals(true); + + while(ui_->tabBar_components->count() > 0) { + ui_->tabBar_components->removeTab(0); + } + + int total = 0; + for(size_t i = 0; i < components_.size(); ++i) { + QString tabLabel = QString("%1%2 (%3)%4") + .arg(i==0 ? "Main" : "") + .arg(i>0?char('A'+char(i-1)):'\0') + .arg(components_[i].nodeIds.size()) + .arg(components_[i].selectedNodeIds.empty()?"":QString(" [%1]").arg(components_[i].selectedNodeIds.size())); + ui_->tabBar_components->addTab(tabLabel); + total+=components_[i].selectedNodeIds.size(); + } + ui_->radioButton_graphSelection->setText(total==0?tr("Selection"):tr("Selection [%1]").arg(total)); + + ui_->tabBar_components->setCurrentIndex(activeComponentIndex_); + ui_->tabBar_components->setEnabled(components_.size() > 1); + ui_->tabBar_components->setVisible(components_.size() > 1); + ui_->graphComponentText->setVisible(components_.size() > 1); + ui_->graphComponentText->setText(components_.size() == 1 ? QString("") : QString("Components:")); + ui_->tabBar_components->blockSignals(false); +} + +void DatabaseViewer::updateComponentSelectedNodes() +{ + UDEBUG(""); + if(activeComponentIndex_>=0 && activeComponentIndex_<(int)components_.size()) + { + UASSERT(ui_->tabBar_components->count() == (int)components_.size()); + UDEBUG("Active component=%d selected before=%ld after=%ld", + activeComponentIndex_, + components_[activeComponentIndex_].selectedNodeIds.size(), + ui_->graphViewer->getSelectedNodeIds().size()); + components_[activeComponentIndex_].selectedNodeIds = ui_->graphViewer->getSelectedNodeIds(); + QString tabLabel = QString("%1%2 (%3)%4") + .arg(activeComponentIndex_==0 ? "Main" : "") + .arg(activeComponentIndex_>0?char('A'+char(activeComponentIndex_-1)):'\0') + .arg(components_[activeComponentIndex_].nodeIds.size()) + .arg(components_[activeComponentIndex_].selectedNodeIds.empty()?"":QString(" [%1]").arg(components_[activeComponentIndex_].selectedNodeIds.size())); + ui_->tabBar_components->setTabText(activeComponentIndex_, tabLabel); + } + + int total = 0; + for(auto & component: components_) + { + total += component.selectedNodeIds.size(); + } + ui_->radioButton_graphSelection->setText(total==0?tr("Selection"):tr("Selection [%1]").arg(total)); +} + +void DatabaseViewer::clearAllSelectedNodes() +{ + UASSERT(ui_->tabBar_components->count() == (int)components_.size()); + for(size_t i = 0; i < components_.size(); ++i) { + QString tabLabel = QString("%1%2 (%3)") + .arg(i==0 ? "Main" : "") + .arg(i>0?char('A'+char(i-1)):'\0') + .arg(components_[i].nodeIds.size()); + components_[i].selectedNodeIds.clear(); + ui_->tabBar_components->setTabText(i, tabLabel); + } + ui_->radioButton_graphSelection->setText(tr("Selection")); + ui_->graphViewer->scene()->clearSelection(); +} + void DatabaseViewer::updateGraphView() { + UDEBUG(""); ui_->label_loopClosures->clear(); ui_->label_poses->clear(); ui_->label_rmse->clear(); @@ -7684,6 +8257,8 @@ void DatabaseViewer::updateGraphView() if(odomPoses_.size()) { + if (components_.size() == 0) regenerateGraphComponents(); + int fromId = ui_->spinBox_optimizationsFrom->value(); if(!uContains(odomPoses_, fromId)) { @@ -8142,7 +8717,7 @@ void DatabaseViewer::updateGraphView() else { // Global map re-optimized - ui_->pushButton_applyRotation->setEnabled(true); + ui_->pushButton_applyRotation->setEnabled(activeComponentIndex_==0); ui_->horizontalSlider_rotation->setEnabled(true); ui_->spinBox_optimizationsFrom->setEnabled(true); ui_->checkBox_spanAllMaps->setEnabled(true); @@ -8184,6 +8759,115 @@ void DatabaseViewer::updateGraphView() } } +void DatabaseViewer::updateGraphViewRootId() +{ + int id = ui_->spinBox_optimizationsFrom->value(); + if(uContains(odomPoses_, id)) + { + int fromIdComponentIndex = -1; + for(int i = 0; i < (int)components_.size(); ++i) + { + if(components_[i].nodeIds.find(id) != components_[i].nodeIds.end()) + { + fromIdComponentIndex = i; + if(components_[i].rootId != id) + { + components_[i].rootCoordinates = QPointF(); + } + components_[i].rootId = id; + break; + } + } + + if (fromIdComponentIndex >= 0 && fromIdComponentIndex != activeComponentIndex_) { + // updateGraphView() is called inside tabBarComponentsValueChanged() slot + ui_->tabBar_components->setCurrentIndex(fromIdComponentIndex); + } + else { + updateGraphView(); + } + } + else + { + QMessageBox::warning(this, tr(""), tr("Graph optimization from id (%1) for which node is not linked to graph.\n Minimum=%2, Maximum=%3") + .arg(id) + .arg(odomPoses_.begin()->first) + .arg(odomPoses_.rbegin()->first)); + return; + } +} + +void DatabaseViewer::tabBarComponentsValueChanged(int value) +{ + UDEBUG(""); + if(value < 0 || value >= (int)components_.size() || value == activeComponentIndex_) { + return; + } + backupCurrentComponent(); + activeComponentIndex_ = value; + GraphComponent & comp = components_[activeComponentIndex_]; + + ui_->horizontalSlider_rotation->blockSignals(true); + ui_->horizontalSlider_rotation->setValue(comp.rotationValue); + ui_->label_rotation->setText(QString::number(float(-ui_->horizontalSlider_rotation->value())/10.0f, 'f', 1) + " deg"); + ui_->horizontalSlider_rotation->blockSignals(false); + + ui_->pushButton_applyRotation->setEnabled(value==0); + if(value==0) + { + Qt::ItemFlags enableFlags = Qt::ItemFlags(Qt::ItemIsEnabled) | Qt::ItemIsSelectable; + ui_->comboBox_optimizationFlavor->setItemData(2, QVariant(static_cast(enableFlags)), Qt::UserRole - 1); + } + else + { + ui_->comboBox_optimizationFlavor->setItemData(2, 0, Qt::UserRole - 1); + } + ui_->comboBox_optimizationFlavor->setCurrentIndex(value==0 || comp.optimizationFlavor<=1?comp.optimizationFlavor:0); + ui_->spinBox_optimizationsFrom->setValue(comp.rootId); + ui_->label_optimizeFrom->setText(tr("Root [%1, %2]").arg(comp.firstNodeId).arg(comp.lastNodeId)); + + // File menu + ui_->actionRegenerate_optimized_2D_map->setEnabled(value==0 && uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.17.0") >= 0); + ui_->actionUpdate_optimized_mesh->setEnabled(value==0 && uStrNumCmp(dbDriver_->getDatabaseVersion(), "0.13.0") >= 0); + + disconnect(ui_->graphViewer, SIGNAL(nodesSelected()), this , SLOT(updateComponentSelectedNodes())); + updateGraphView(); + connect(ui_->graphViewer, SIGNAL(nodesSelected()), this , SLOT(updateComponentSelectedNodes())); + + if(!ui_->radioButton_graphSelection->isChecked()) + { + comp.selectedNodeIds.clear(); + } + + // restore component's view + if(comp.viewScale > 0.0f && + comp.rootId > 0 && + comp.rootCoordinates != QPointF() && + comp.nodeIds.find(comp.rootId) != comp.nodeIds.end()) + { + QPointF rootScenePos = ui_->graphViewer->getNodeScenePosition(comp.rootId); + + ui_->graphViewer->resetTransform(); + ui_->graphViewer->scale(comp.viewScale, comp.viewScale); + + QPointF viewportCenter = ui_->graphViewer->viewport()->rect().center(); + QPointF viewportDelta = viewportCenter - comp.rootCoordinates; + QPointF sceneDelta = viewportDelta / comp.viewScale; + + updateGraphRotation(); + + ui_->graphViewer->centerOn(rootScenePos + sceneDelta); + ui_->graphViewer->selectNodesFromIds(comp.selectedNodeIds); + ui_->graphViewer->update(); + } + + updateComponentSelectedNodes(); // To update selection count in labels + + // Update info of the nodes currently shown + sliderAValueChanged(ui_->spinBox_indexA->value()); + sliderBValueChanged(ui_->spinBox_indexB->value()); +} + void DatabaseViewer::updateGrid() { if(sender() == ui_->checkBox_grid_2d && !ui_->checkBox_grid_2d->isChecked()) @@ -8796,7 +9480,7 @@ void DatabaseViewer::refineConstraint(int from, int to, Registration * reg, Regi updated = true; } - if(updated && !silent) + if(updated && !silent && ui_->checkBox_optimize_on_all_changes->isChecked()) { this->updateGraphView(); } @@ -9279,8 +9963,24 @@ bool DatabaseViewer::addConstraint(int from, int to, Registration * reg, bool si if(updateConstraints) { - updateLoopClosuresSlider(fromS->id(), toS->id()); - this->updateGraphView(); + int oldRootId = !components_.empty()?components_[activeComponentIndex_].rootId:0; + regenerateGraphComponents(); + std::map currentGraphNodes = uValueAt(graphes_, ui_->horizontalSlider_iterations->value()); + std::set currentComponentNodes = !components_.empty() ? components_[activeComponentIndex_].nodeIds : std::set(); + bool componentsChanged = currentGraphNodes.empty() || oldRootId == 0 || currentGraphNodes.size() != currentComponentNodes.size() || oldRootId != components_[activeComponentIndex_].rootId; + if(!ui_->checkBox_optimize_on_all_changes->isChecked() && !componentsChanged) { + // just add the link and update graph viewer + graphLinks_.insert(std::make_pair(newLink.from(), newLink)); + ui_->graphViewer->updateGraph(currentGraphNodes, graphLinks_, mapIds_, weights_); + updateLoopClosuresSlider(fromS->id(), toS->id()); + } + else { + this->updateGraphView(); + // graph would have been updated, re-highlight the nodes + ui_->graphViewer->highlightNode(ids_.at(ui_->horizontalSlider_A->value()), 0); + ui_->graphViewer->highlightNode(ids_.at(ui_->horizontalSlider_B->value()), 1); + } + this->updateConstraintView(newLink, false, *fromS, *toS); } @@ -9307,6 +10007,7 @@ bool DatabaseViewer::addConstraint(int from, int to, Registration * reg, bool si else if(updateConstraints) { updateLoopClosuresSlider(from, to); + regenerateGraphComponents(); this->updateGraphView(); } } @@ -9349,7 +10050,9 @@ void DatabaseViewer::resetConstraint() if(iter != linksRefined_.end()) { linksRefined_.erase(iter); - this->updateGraphView(); + if(ui_->checkBox_optimize_on_all_changes->isChecked()) { + this->updateGraphView(); + } } updateConstraintView(); @@ -9421,26 +10124,34 @@ void DatabaseViewer::rejectConstraint() { if(priorId==0) { - this->updateGraphView(); + int oldRootId = !components_.empty()?components_[activeComponentIndex_].rootId:0; + regenerateGraphComponents(); + std::map currentGraphNodes = uValueAt(graphes_, ui_->horizontalSlider_iterations->value()); + std::set currentComponentNodes = !components_.empty() ? components_[activeComponentIndex_].nodeIds : std::set(); + bool componentsChanged = currentGraphNodes.empty() || oldRootId == 0 || currentGraphNodes.size() != currentComponentNodes.size() || oldRootId != components_[activeComponentIndex_].rootId; + if(!ui_->checkBox_optimize_on_all_changes->isChecked() && !componentsChanged) { + // just remove the link and update graph viewer + graphLinks_.erase(graph::findLink(graphLinks_, from, to)); + ui_->graphViewer->updateGraph(currentGraphNodes, graphLinks_, mapIds_, weights_); + updateLoopClosuresSlider(); + } + else { + this->updateGraphView(); + // graph would have been updated, re-highlight the nodes + ui_->graphViewer->highlightNode(ids_.at(ui_->horizontalSlider_A->value()), 0); + ui_->graphViewer->highlightNode(ids_.at(ui_->horizontalSlider_B->value()), 1); + } } - else + else if(ui_->checkBox_optimize_on_all_changes->isChecked()) // removed prior links would not change anything visually if we don't re-optimize { bool priorsIgnored = Parameters::defaultOptimizerPriorsIgnored(); Parameters::parse(ui_->parameters_toolbox->getParameters(), Parameters::kOptimizerPriorsIgnored(), priorsIgnored); - int indexA = ui_->horizontalSlider_A->value(); - int indexB = ui_->horizontalSlider_B->value(); + if(!priorsIgnored) { + regenerateGraphComponents(); this->updateGraphView(); } - if(ui_->horizontalSlider_A->value() != indexA) - ui_->horizontalSlider_A->setValue(indexA); - else - sliderAValueChanged(indexA); - if(ui_->horizontalSlider_B->value() != indexB) - ui_->horizontalSlider_B->setValue(indexB); - else - sliderBValueChanged(indexB); } } } @@ -9580,6 +10291,7 @@ void DatabaseViewer::updateLoopClosuresSlider(int from, int to) loopLinks_.clear(); std::multimap links = updateLinksWithModifications(links_); int position = ui_->horizontalSlider_loops->value(); + int orgSliderMaximum = ui_->horizontalSlider_loops->maximum(); std::multimap linksSortedByParents; for(std::multimap::iterator iter = links.begin(); iter!=links.end(); ++iter) { @@ -9623,13 +10335,16 @@ void DatabaseViewer::updateLoopClosuresSlider(int from, int to) ui_->horizontalSlider_loops->setMinimum(0); ui_->horizontalSlider_loops->setMaximum(loopLinks_.size()-1); ui_->horizontalSlider_loops->setEnabled(true); - if(position != ui_->horizontalSlider_loops->value()) + if(position>=0) { - ui_->horizontalSlider_loops->setValue(position); - } - else - { - this->updateConstraintView(loopLinks_.at(position)); + if(position != ui_->horizontalSlider_loops->value()) + { + ui_->horizontalSlider_loops->setValue(position); + } + else if(orgSliderMaximum != ui_->horizontalSlider_loops->maximum()) + { + this->updateConstraintView(loopLinks_.at(position)); + } } } else @@ -9641,6 +10356,11 @@ void DatabaseViewer::updateLoopClosuresSlider(int from, int to) } } +void DatabaseViewer::updateLoopClosuresSlider() +{ + updateLoopClosuresSlider(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value())); +} + void DatabaseViewer::notifyParametersChanged(const QStringList & parametersChanged) { bool updateStereo = false; diff --git a/guilib/src/ExportDialog.cpp b/guilib/src/ExportDialog.cpp index 2ffc38d2..b00e10d6 100644 --- a/guilib/src/ExportDialog.cpp +++ b/guilib/src/ExportDialog.cpp @@ -47,6 +47,7 @@ ExportDialog::ExportDialog(QWidget * parent) : connect(_ui->spinBox_ignored, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged())); connect(_ui->doubleSpinBox_framerate, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged())); connect(_ui->spinBox_session, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged())); + connect(_ui->checkBox_selected_nodes, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); connect(_ui->checkBox_rgb, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); connect(_ui->checkBox_depth, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); connect(_ui->checkBox_depth2d, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged())); @@ -70,6 +71,8 @@ void ExportDialog::saveSettings(QSettings & settings, const QString & group) con settings.setValue("framesIgnored", this->framesIgnored()); settings.setValue("targetFramerate", this->targetFramerate()); settings.setValue("sessionExported", this->sessionExported()); + settings.setValue("currentComponentOnly", this->currentComponentOnly()); + settings.setValue("selectedNodesOnly", this->selectedNodesOnly()); settings.setValue("rgbExported", this->isRgbExported()); settings.setValue("depthExported", this->isDepthExported()); settings.setValue("depth2dExported", this->isDepth2dExported()); @@ -90,6 +93,8 @@ void ExportDialog::loadSettings(QSettings & settings, const QString & group) _ui->spinBox_ignored->setValue(settings.value("framesIgnored", this->framesIgnored()).toInt()); _ui->doubleSpinBox_framerate->setValue(settings.value("targetFramerate", this->targetFramerate()).toDouble()); _ui->spinBox_session->setValue(settings.value("sessionExported", this->sessionExported()).toInt()); + _ui->checkBox_current_component->setChecked(settings.value("currentComponentOnly", this->currentComponentOnly()).toBool()); + _ui->checkBox_selected_nodes->setChecked(settings.value("selectedNodesOnly", this->selectedNodesOnly()).toBool()); _ui->checkBox_rgb->setChecked(settings.value("rgbExported", this->isRgbExported()).toBool()); _ui->checkBox_depth->setChecked(settings.value("depthExported", this->isDepthExported()).toBool()); _ui->checkBox_depth2d->setChecked(settings.value("depth2dExported", this->isDepth2dExported()).toBool()); @@ -106,6 +111,8 @@ void ExportDialog::restoreDefaults() _ui->spinBox_ignored->setValue(0); _ui->doubleSpinBox_framerate->setValue(0); _ui->spinBox_session->setValue(-1); + _ui->checkBox_current_component->setChecked(false); + _ui->checkBox_selected_nodes->setChecked(false); _ui->checkBox_rgb->setChecked(true); _ui->checkBox_depth->setChecked(true); _ui->checkBox_depth2d->setChecked(true); @@ -142,6 +149,16 @@ int ExportDialog::sessionExported() const return _ui->spinBox_session->value(); } +bool ExportDialog::currentComponentOnly() const +{ + return _ui->checkBox_current_component->isChecked(); +} + +bool ExportDialog::selectedNodesOnly() const +{ + return _ui->checkBox_selected_nodes->isChecked(); +} + bool ExportDialog::isRgbExported() const { return _ui->checkBox_rgb->isChecked(); diff --git a/guilib/src/GraphViewer.cpp b/guilib/src/GraphViewer.cpp index c0a9a8de..21cf7b1c 100644 --- a/guilib/src/GraphViewer.cpp +++ b/guilib/src/GraphViewer.cpp @@ -342,6 +342,7 @@ GraphViewer::GraphViewer(QWidget * parent) : _orientationENU(false), _mouseTracking(false), _viewPlane(XY), + _interactionMode(HandMode), _ensureFrameVisible(true), _previousMousePos(-1,-1), _initialMousePos(-1,-1), @@ -388,6 +389,7 @@ void GraphViewer::setupGraphicsScene() _zoomOverlayItem->setVisible(false); _zoomOverlayItem->setAcceptedMouseButtons(Qt::NoButton); _zoomOverlayItem->setAcceptHoverEvents(false); + connect(this->scene(), &QGraphicsScene::selectionChanged, this, &GraphViewer::nodesSelected); _world = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001)); _root = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001)); @@ -664,6 +666,7 @@ void GraphViewer::updateGraph(const std::map & poses, item->setZValue(iter->first<0?21:20); item->setColor(color); item->setParentItem(_graphRoot); + item->setFlag(QGraphicsItem::ItemIsSelectable, iter->first>0 && _interactionMode == SelectionMode); item->show(); _nodeItems.insert(iter->first, item); ++iter; @@ -674,6 +677,7 @@ void GraphViewer::updateGraph(const std::map & poses, // NodeItem exists for the pose, copy data and increase both iterators UASSERT(iter->first == nter.key()); nter.value()->setColor(color); // reset color + nter.value()->setFlag(QGraphicsItem::ItemIsSelectable, iter->first > 0 && _interactionMode == SelectionMode); nter.value()->setToolTipInfo(QString()); nter.value()->setZValue(iter->first<0?21:20); nter.value()->setPose(iter->second, _viewPlane); @@ -937,6 +941,7 @@ void GraphViewer::updateGTGraph(const std::map & poses) item->setZValue(20); item->setColor(_gtPathColor); item->setParentItem(_gtGraphRoot); + item->setFlag(QGraphicsItem::ItemIsSelectable, false); item->setVisible(_nodeVisible); _gtNodeItems.insert(iter->first, item); } @@ -1071,6 +1076,7 @@ void GraphViewer::updateGPSGraph( item->setZValue(20); item->setColor(_gpsPathColor); item->setParentItem(_gpsGraphRoot); + item->setFlag(QGraphicsItem::ItemIsSelectable, false); item->setVisible(_nodeVisible); _gpsNodeItems.insert(iter->first, item); } @@ -1453,6 +1459,48 @@ void GraphViewer::highlightNode(int nodeId, int highlightIndex) } } +std::set GraphViewer::getSelectedNodeIds() +{ + std::set ids; + for(QMap::iterator iter = _nodeItems.begin(); iter != _nodeItems.end(); iter++) + { + if(iter.value() && iter.value()->isSelected()) + ids.insert(iter.key()); + } + return ids; +} + +void GraphViewer::selectNodesFromIds(const std::set & ids) +{ + this->scene()->blockSignals(true); + + for (QMap::iterator iter = _nodeItems.begin(); iter != _nodeItems.end(); ++iter) + { + if(iter.value() && iter.key()>0) + iter.value()->setSelected(ids.find(iter.key()) != ids.end()); + } + + this->scene()->blockSignals(false); +} + +void GraphViewer::setInteractionMode(InteractionMode mode) +{ + if(_interactionMode == mode) + { + return; + } + _interactionMode = mode; + this->setDragMode(mode == HandMode ? QGraphicsView::ScrollHandDrag : QGraphicsView::RubberBandDrag); + this->setRubberBandSelectionMode(Qt::IntersectsItemShape); + for(QMap::iterator iter = _nodeItems.begin(); iter != _nodeItems.end(); iter++) + { + if(iter.key()>0 && iter.value()) + { + iter.value()->setFlag(QGraphicsItem::ItemIsSelectable, mode == SelectionMode); + } + } +} + void GraphViewer::clearGraph() { qDeleteAll(_nodeItems); @@ -1566,6 +1614,7 @@ void GraphViewer::saveSettings(QSettings & settings, const QString & group) cons settings.setValue("odom_cache_overlay", this->isOdomCacheOverlayVisible()); settings.setValue("orientation_ENU", this->isOrientationENU()); settings.setValue("view_plane", (int)this->getViewPlane()); + settings.setValue("interaction_mode", (int)this->getInteractionMode()); settings.setValue("ensure_frame_visible", (int)this->isEnsureFrameVisible()); settings.setValue("fast_zoom_min_nodes", this->getFastZoomMinNodes()); if(!group.isEmpty()) @@ -1618,6 +1667,7 @@ void GraphViewer::loadSettings(QSettings & settings, const QString & group) this->setOdomCacheOverlayVisible(settings.value("odom_cache_overlay", this->isOdomCacheOverlayVisible()).toBool()); this->setOrientationENU(settings.value("orientation_ENU", this->isOrientationENU()).toBool()); this->setViewPlane((ViewPlane)settings.value("view_plane", (int)this->getViewPlane()).toInt()); + this->setInteractionMode((InteractionMode)settings.value("interaction_mode", (int)this->getInteractionMode()).toInt()); this->setEnsureFrameVisible(settings.value("ensure_frame_visible", this->isEnsureFrameVisible()).toBool()); this->setFastZoomMinNodes(settings.value("fast_zoom_min_nodes", this->getFastZoomMinNodes()).toInt()); if(!group.isEmpty()) @@ -2087,6 +2137,7 @@ void GraphViewer::restoreDefaults() setLocalPathVisible(true); setGtGraphVisible(true); setFastZoomMinNodes(0); + setInteractionMode(HandMode); } void GraphViewer::wheelEvent(QWheelEvent * event) @@ -2123,7 +2174,7 @@ void GraphViewer::mouseMoveEvent(QMouseEvent * event) { QToolTip::hideText(); } - if (event->modifiers() & (Qt::ShiftModifier | Qt::ControlModifier) && event->buttons() & Qt::LeftButton) { + if (_interactionMode == HandMode && event->modifiers() & (Qt::ShiftModifier | Qt::ControlModifier) && event->buttons() & Qt::LeftButton) { // same modifiers than 3D view, change zoom if(_previousMousePos.y()!=0) { qreal factor = (event->pos().y() - _previousMousePos.y() > 0) ? 0.98 : 1.02; @@ -2200,6 +2251,25 @@ QIcon createIcon(const QColor & color) return QIcon(pixmap); } +QPointF GraphViewer::getNodeScenePosition(int id) const +{ + NodeItem * item = _nodeItems.value(id, 0); + if(item) + { + return item->sceneBoundingRect().center(); + } + return QPointF(); +} + +void GraphViewer::centerOnNode(int id) +{ + QPointF center = this->getNodeScenePosition(id); + if(!center.isNull()) + { + this->centerOn(center); + } +} + void GraphViewer::contextMenuEvent(QContextMenuEvent * event) { QMenu menu; diff --git a/guilib/src/LinkRefiningDialog.cpp b/guilib/src/LinkRefiningDialog.cpp index 5763aebf..7181344b 100644 --- a/guilib/src/LinkRefiningDialog.cpp +++ b/guilib/src/LinkRefiningDialog.cpp @@ -87,6 +87,20 @@ void LinkRefiningDialog::setMinMax( restoreDefaults(); } } +void LinkRefiningDialog::setRangesEnabled(bool selectedNodes, bool currentComponent) +{ + ui_->radioButton_selected_nodes->setEnabled(selectedNodes); + if(!selectedNodes && ui_->radioButton_selected_nodes->isChecked()) + { + ui_->radioButton_nodes->setChecked(true); + } + + ui_->radioButton_current_component->setEnabled(currentComponent); + if(!currentComponent && ui_->radioButton_current_component->isChecked()) + { + ui_->radioButton_nodes->setChecked(true); + } +} Link::Type LinkRefiningDialog::getLinkType() const { @@ -103,6 +117,16 @@ void LinkRefiningDialog::getIntraInterSessions(bool & intra, bool & inter) const inter = ui_->comboBox_link_inter_intra->currentIndex() == 0 || ui_->comboBox_link_inter_intra->currentIndex() == 2; } +bool LinkRefiningDialog::isRangeByCurrentComponent() const +{ + return ui_->radioButton_current_component->isChecked(); +} + +bool LinkRefiningDialog::isRangeBySelectedNodeIds() const +{ + return ui_->radioButton_selected_nodes->isChecked(); +} + bool LinkRefiningDialog::isRangeByNodeId() const { return ui_->radioButton_nodes->isChecked(); diff --git a/guilib/src/ui/DatabaseViewer.ui b/guilib/src/ui/DatabaseViewer.ui index cc7918fb..8b4b95de 100644 --- a/guilib/src/ui/DatabaseViewer.ui +++ b/guilib/src/ui/DatabaseViewer.ui @@ -61,7 +61,7 @@ 0 0 - 364 + 344 389 @@ -392,7 +392,7 @@ 0 0 - 364 + 343 389 @@ -752,10 +752,19 @@ - + + + Click to locate in Graph View + idA + + Qt::ToolButtonTextOnly + + + true + @@ -835,10 +844,19 @@ - + + + Click to locate in Graph View + idB + + Qt::ToolButtonTextOnly + + + true + @@ -914,10 +932,10 @@ - + @@ -937,6 +955,7 @@ + @@ -948,6 +967,7 @@ + @@ -1198,7 +1218,17 @@ - + + + + + Re-optimize the graph right away after using any of these actions + + + + + + @@ -1217,20 +1247,44 @@ - - - Add + + + QFrame::StyledPanel - - - - - - Enable "Add" for all nodes (even not in graph) - - - + + QFrame::Raised + + + 0 + + + 0 + + + 6 + + + 0 + + + + + Add + + + + + + + Enable "Add" for all nodes (even not in graph) + + + + + + + @@ -1247,7 +1301,7 @@ - 0 + 13 20 @@ -1267,9 +1321,74 @@ + + + + + + Interaction mode: + + + + + + + Hand / Move + + + true + + + + + + + Selection + + + + + + + Qt::Horizontal + + + + 0 + 20 + + + + + + + + + + + + Graph component + + + + + + + true + + + true + + + false + + + + + @@ -1292,6 +1411,9 @@ + + 6 + @@ -1337,7 +1459,13 @@ - + + + + + + 6 + @@ -1380,10 +1508,23 @@ + + + + After any graph changes, re-zoom to fit the whole map in the view + + + Fit in view + + + + + 6 + @@ -1467,6 +1608,9 @@ + + 6 + @@ -1722,7 +1866,7 @@ - 2 + 0 @@ -1737,8 +1881,25 @@ Graph optimization - - + + + + Ignore pose correction + + + + + + + + + + false + + + + + @@ -1754,16 +1915,6 @@ - - - - - - - false - - - @@ -1771,15 +1922,8 @@ - - - - Ignore pose correction - - - - - + + @@ -1788,13 +1932,10 @@ - - + + - - - - false + Ignore landmarks @@ -1811,8 +1952,15 @@ - - + + + + Ignore local loop closures (space) + + + + + @@ -1828,20 +1976,6 @@ - - - - Ignore local loop closures (space) - - - - - - - Scale - - - @@ -1861,15 +1995,25 @@ - - + + - Ignore landmarks + + + + false - - + + + + Scale + + + + + @@ -2604,7 +2748,7 @@ 0 - -175 + 0 403 319 @@ -2613,54 +2757,13 @@ Detect more loop closures - - + + - Angle + Iterations - - - - Inter-session - - - - - - - - - - - - - - Radius Max - - - - - - - Intra-session - - - - - - - Qt::Vertical - - - - 20 - 40 - - - - @@ -2680,10 +2783,29 @@ - - - - Radius Min + + + + -1 + + + 9999 + + + -1 + + + + + + + 1 + + + 9999 + + + 10 @@ -2700,6 +2822,39 @@ + + + + + + + + + + + m + + + 2 + + + 10.000000000000000 + + + 0.100000000000000 + + + 1.000000000000000 + + + + + + + Inter-session + + + @@ -2707,20 +2862,23 @@ - - - - Iterations + + + + Qt::Vertical - + + + 20 + 40 + + + - - + + - - - - true + Radius Max @@ -2750,6 +2908,16 @@ + + + + + + + true + + + @@ -2760,38 +2928,6 @@ - - - - m - - - 2 - - - 10.000000000000000 - - - 0.100000000000000 - - - 1.000000000000000 - - - - - - - -1 - - - 9999 - - - -1 - - - @@ -2799,16 +2935,24 @@ - - - - 1 + + + + Angle - - 9999 + + + + + + Intra-session - - 10 + + + + + + Radius Min @@ -3021,7 +3165,7 @@ - 40 + 0 20 @@ -3391,7 +3535,7 @@ - Export 3D map (*.ply *.pcd) ... + Export 3D map (*.ply *.pcd)... @@ -3557,6 +3701,9 @@ Update optimized mesh... + + Available only for the Main component + @@ -3585,6 +3732,9 @@ Regenerate optimized 2D map... + + Available only for the Main component + @@ -3596,6 +3746,16 @@ Update all landmark covariances... + + + Merge components... + + + + + Clear all selected nodes + + @@ -3620,6 +3780,11 @@
../include/rtabmap/gui/ParametersToolBox.h
1 + + QTabBar + QWidget +
qtabbar.h
+ diff --git a/guilib/src/ui/exportDialog.ui b/guilib/src/ui/exportDialog.ui index 1f034c37..1987896c 100644 --- a/guilib/src/ui/exportDialog.ui +++ b/guilib/src/ui/exportDialog.ui @@ -6,8 +6,8 @@ 0 0 - 382 - 331 + 333 + 520 @@ -15,178 +15,199 @@ - - - - - - - - - - - - Frames ignored - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - - - Hz - - - 1 - - - - - - - Target frame rate - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - - - - - - -1 - - - 999 - - - -1 - - - - - - - Mapping session (-1 = all) - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - RGB images - - - true + + + Selection + + + + + + + + -1 + + + 999 + + + -1 + + + + + + + Mapping session (-1 = all) + + + + + + + + + + false + + + + + + + Selected nodes + + + true + + + + + + + Current component only + + + true + + + + + + + + + + false + + + + - - - Depth images - - - true + + + Filtering + + + + + + + + + + + + Nodes ignored + + + + + + + Hz + + + 1 + + + + + + + Target frame rate + + + + - - - Laser scans - - - true + + + Data + + + + + RGB images + + + true + + + + + + + Depth images + + + true + + + + + + + Laser scans + + + true + + + + + + + Odometry + + + true + + + + + + + User data (planned goals) + + + false + + + + - - - Odometry - - - true + + + Output Path + + + + + true + + + + + + + ... + + + + - - - - User data (planned goals) - - - false - - - - - - - - - true - - - - - - - ... - - - - - diff --git a/guilib/src/ui/linkRefiningDialog.ui b/guilib/src/ui/linkRefiningDialog.ui index 3bc36249..e6d843aa 100644 --- a/guilib/src/ui/linkRefiningDialog.ui +++ b/guilib/src/ui/linkRefiningDialog.ui @@ -6,8 +6,8 @@ 0 0 - 333 - 286 + 369 + 344 @@ -51,28 +51,21 @@ Range - + From - - + + - To + Maps: - - - - Nodes: - - - - + 1 @@ -82,7 +75,7 @@ - + 1 @@ -92,14 +85,21 @@ - - + + - Maps: + Nodes selected - + + + + To + + + + 0 @@ -112,7 +112,7 @@ - + 0 @@ -125,6 +125,20 @@ + + + + Nodes: + + + + + + + Current component + + +