From ab5fd5018b8d6ab1b6be6782d1afc0065c4efa21 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 6 Oct 2021 11:34:40 -0400 Subject: [PATCH] DbViewer: added update all landmark covariances menu action, also enabled edit constraint on landmark links. Covariance can be set to 9999. --- guilib/include/rtabmap/gui/DatabaseViewer.h | 1 + .../rtabmap/gui/EditConstraintDialog.h | 2 +- guilib/src/DatabaseViewer.cpp | 139 +++++++++++++----- guilib/src/EditConstraintDialog.cpp | 8 +- guilib/src/ui/DatabaseViewer.ui | 10 +- guilib/src/ui/editConstraintDialog.ui | 2 +- 6 files changed, 120 insertions(+), 42 deletions(-) diff --git a/guilib/include/rtabmap/gui/DatabaseViewer.h b/guilib/include/rtabmap/gui/DatabaseViewer.h index f65b9780..0be873c7 100644 --- a/guilib/include/rtabmap/gui/DatabaseViewer.h +++ b/guilib/include/rtabmap/gui/DatabaseViewer.h @@ -122,6 +122,7 @@ private Q_SLOTS: void detectMoreLoopClosures(); void updateAllNeighborCovariances(); void updateAllLoopClosureCovariances(); + void updateAllLandmarkCovariances(); void refineAllNeighborLinks(); void refineAllLoopClosureLinks(); void resetAllChanges(); diff --git a/guilib/include/rtabmap/gui/EditConstraintDialog.h b/guilib/include/rtabmap/gui/EditConstraintDialog.h index 21e7dcb4..acf7eb08 100644 --- a/guilib/include/rtabmap/gui/EditConstraintDialog.h +++ b/guilib/include/rtabmap/gui/EditConstraintDialog.h @@ -42,7 +42,7 @@ class RTABMAPGUI_EXP EditConstraintDialog : public QDialog Q_OBJECT public: - EditConstraintDialog(const Transform & constraint, double linearSigma = 0, double angularSigma = 0, QWidget * parent = 0); + EditConstraintDialog(const Transform & constraint, double linearSigma = 1, double angularSigma = 1, QWidget * parent = 0); virtual ~EditConstraintDialog(); Transform getTransform() const; diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index 570601b3..b742b268 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -292,6 +292,7 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : connect(ui_->actionDetect_more_loop_closures, SIGNAL(triggered()), this, SLOT(detectMoreLoopClosures())); 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_all_neighbor_links, SIGNAL(triggered()), this, SLOT(refineAllNeighborLinks())); connect(ui_->actionRefine_all_loop_closure_links, SIGNAL(triggered()), this, SLOT(refineAllLoopClosureLinks())); connect(ui_->actionRegenerate_local_grid_maps, SIGNAL(triggered()), this, SLOT(regenerateLocalMaps())); @@ -4128,7 +4129,27 @@ void DatabaseViewer::updateAllNeighborCovariances() } void DatabaseViewer::updateAllLoopClosureCovariances() { - updateAllCovariances(loopLinks_); + QList links; + for(int i=0; i links; + for(int i=0; i & links) @@ -4136,10 +4157,10 @@ void DatabaseViewer::updateAllCovariances(const QList & links) if(links.size()) { bool ok = false; - double stddev = QInputDialog::getDouble(this, tr("Linear error"), tr("Std deviation (m)"), 0.01, 0.0001, 9, 4, &ok); + double stddev = QInputDialog::getDouble(this, tr("Linear error"), tr("Std deviation (m) 0=inf"), 0.01, 0.0, 9, 4, &ok); if(!ok) return; double linearVar = stddev*stddev; - stddev = QInputDialog::getDouble(this, tr("Angular error"), tr("Std deviation (deg)"), 1, 0.01, 45, 2, &ok)*M_PI/180.0; + stddev = QInputDialog::getDouble(this, tr("Angular error"), tr("Std deviation (deg) 0=inf"), 1, 0.0, 90, 2, &ok)*M_PI/180.0; if(!ok) return; double angularVar = stddev*stddev; @@ -4151,8 +4172,22 @@ void DatabaseViewer::updateAllCovariances(const QList & links) progressDialog->show(); cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1); - infMatrix(cv::Range(0,3), cv::Range(0,3))/=linearVar; - infMatrix(cv::Range(3,6), cv::Range(3,6))/=angularVar; + if(linearVar == 0.0) + { + infMatrix(cv::Range(0,3), cv::Range(0,3)) /= 9999.9; + } + else + { + infMatrix(cv::Range(0,3), cv::Range(0,3)) /= linearVar; + } + if(angularVar == 0.0) + { + infMatrix(cv::Range(3,6), cv::Range(3,6)) /= 9999.9; + } + else + { + infMatrix(cv::Range(3,6), cv::Range(3,6)) /= angularVar; + } for(int i=0; ifindActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value())); + Link link; + if(ui_->label_type->text().toInt() == Link::kLandmark) + { + int position = ui_->horizontalSlider_loops->value(); + link = loopLinks_.at(position); + } + else + { + link = this->findActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value())); + } if(link.isValid()) { cv::Mat covBefore = link.infMatrix().inv(); EditConstraintDialog dialog(link.transform(), - covBefore.at(0,0)!=1.0?std::sqrt(covBefore.at(0,0)):0, - covBefore.at(5,5)!=1.0?std::sqrt(covBefore.at(5,5)):0); + covBefore.at(0,0)<9999.0?std::sqrt(covBefore.at(0,0)):0.0, + covBefore.at(5,5)<9999.0?std::sqrt(covBefore.at(5,5)):0.0); if(dialog.exec() == QDialog::Accepted) { bool updated = false; cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1); if(dialog.getLinearVariance()>0) { - covariance(cv::Range(0,3), cv::Range(0,3)) *= dialog.getLinearVariance()*dialog.getLinearVariance(); + covariance(cv::Range(0,3), cv::Range(0,3)) *= dialog.getLinearVariance(); + } + else + { + covariance(cv::Range(0,3), cv::Range(0,3)) *= 9999.9; } if(dialog.getAngularVariance()>0) { - covariance(cv::Range(3,6), cv::Range(3,6)) *= dialog.getAngularVariance()*dialog.getAngularVariance(); + covariance(cv::Range(3,6), cv::Range(3,6)) *= dialog.getAngularVariance(); + } + else + { + covariance(cv::Range(3,6), cv::Range(3,6)) *= 9999.9; } Link newLink(link.from(), link.to(), link.type(), dialog.getTransform(), covariance.inv()); std::multimap::iterator iter = linksRefined_.find(link.from()); @@ -5654,11 +5706,19 @@ void DatabaseViewer::editConstraint() cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1); if(dialog.getLinearVariance()>0) { - covariance(cv::Range(0,3), cv::Range(0,3)) *= dialog.getLinearVariance()*dialog.getLinearVariance(); + covariance(cv::Range(0,3), cv::Range(0,3)) *= dialog.getLinearVariance(); + } + else + { + covariance(cv::Range(0,3), cv::Range(0,3)) *= 9999.9; } if(dialog.getAngularVariance()>0) { - covariance(cv::Range(3,6), cv::Range(3,6)) *= dialog.getAngularVariance()*dialog.getAngularVariance(); + covariance(cv::Range(3,6), cv::Range(3,6)) *= dialog.getAngularVariance(); + } + else + { + covariance(cv::Range(3,6), cv::Range(3,6)) *= 9999.9; } int from = ids_.at(ui_->horizontalSlider_A->value()); int to = ids_.at(ui_->horizontalSlider_B->value()); @@ -6315,29 +6375,36 @@ void DatabaseViewer::updateConstraintButtons() ui_->pushButton_reject->setEnabled(false); ui_->toolButton_constraint->setEnabled(false); + Link currentLink; + int from; + int to; if(ui_->label_type->text().toInt() == Link::kLandmark) { - ui_->pushButton_reject->setEnabled(true); - return; + //check for modified link + currentLink = loopLinks_.at(ui_->horizontalSlider_loops->value()); + from = currentLink.from(); + to = currentLink.to(); } - - int from = ids_.at(ui_->horizontalSlider_A->value()); - int to = ids_.at(ui_->horizontalSlider_B->value()); - if(from!=to && from && to && - odomPoses_.find(from) != odomPoses_.end() && - odomPoses_.find(to) != odomPoses_.end() && - (ui_->checkBox_enableForAll->isChecked() || - (weights_.find(from) != weights_.end() && weights_.at(from)>=0 && - weights_.find(to) != weights_.end() && weights_.at(to)>=0))) + else { - if((!containsLink(links_, from ,to) && !containsLink(linksAdded_, from ,to)) || - containsLink(linksRemoved_, from ,to)) + from = ids_.at(ui_->horizontalSlider_A->value()); + to = ids_.at(ui_->horizontalSlider_B->value()); + if(from!=to && from && to && + odomPoses_.find(from) != odomPoses_.end() && + odomPoses_.find(to) != odomPoses_.end() && + (ui_->checkBox_enableForAll->isChecked() || + (weights_.find(from) != weights_.end() && weights_.at(from)>=0 && + weights_.find(to) != weights_.end() && weights_.at(to)>=0))) { - ui_->pushButton_add->setEnabled(true); + if((!containsLink(links_, from ,to) && !containsLink(linksAdded_, from ,to)) || + containsLink(linksRemoved_, from ,to)) + { + ui_->pushButton_add->setEnabled(true); + } } - } - Link currentLink = findActiveLink(from ,to); + currentLink = findActiveLink(from ,to); + } if(currentLink.isValid() && ((currentLink.from() == from && currentLink.to() == to) || (currentLink.from() == to && currentLink.to() == from))) @@ -6348,19 +6415,13 @@ void DatabaseViewer::updateConstraintButtons() } //check for modified link - bool modified = false; std::multimap::iterator iter = rtabmap::graph::findLink(linksRefined_, currentLink.from(), currentLink.to()); if(iter != linksRefined_.end()) { currentLink = iter->second; ui_->pushButton_reset->setEnabled(true); - modified = true; } - if(!modified) - { - ui_->pushButton_reset->setEnabled(false); - } - ui_->pushButton_refine->setEnabled(currentLink.from()!=currentLink.to()); + ui_->pushButton_refine->setEnabled(currentLink.from()!=currentLink.to() && currentLink.type() != Link::kLandmark); ui_->toolButton_constraint->setEnabled(true); } } @@ -8194,6 +8255,14 @@ void DatabaseViewer::resetConstraint() { int from = ids_.at(ui_->horizontalSlider_A->value()); int to = ids_.at(ui_->horizontalSlider_B->value()); + if(ui_->label_type->text().toInt() == Link::kLandmark) + { + int position = ui_->horizontalSlider_loops->value(); + const rtabmap::Link & link = loopLinks_.at(position); + from = link.from(); + to = link.to(); + } + if(from < to) { int tmp = to; diff --git a/guilib/src/EditConstraintDialog.cpp b/guilib/src/EditConstraintDialog.cpp index 66d643aa..59229a46 100644 --- a/guilib/src/EditConstraintDialog.cpp +++ b/guilib/src/EditConstraintDialog.cpp @@ -48,11 +48,12 @@ EditConstraintDialog::EditConstraintDialog(const Transform & constraint, double _ui->roll->setValue(roll); _ui->pitch->setValue(pitch); _ui->yaw->setValue(yaw); + + _ui->checkBox_radians->setChecked(true); _ui->linear_sigma->setValue(linearSigma); _ui->angular_sigma->setValue(angularSigma); connect(_ui->checkBox_radians, SIGNAL(stateChanged(int)), this, SLOT(switchUnits())); - _ui->checkBox_radians->setChecked(false); } EditConstraintDialog::~EditConstraintDialog() @@ -111,7 +112,7 @@ Transform EditConstraintDialog::getTransform() const double EditConstraintDialog::getLinearVariance() const { - return _ui->linear_sigma->value(); + return _ui->linear_sigma->value()*_ui->linear_sigma->value(); } double EditConstraintDialog::getAngularVariance() const { @@ -120,7 +121,8 @@ double EditConstraintDialog::getAngularVariance() const { conversion = M_PI/180.0; } - return _ui->angular_sigma->value()*conversion; + double value = _ui->angular_sigma->value()*conversion; + return value*value; } } diff --git a/guilib/src/ui/DatabaseViewer.ui b/guilib/src/ui/DatabaseViewer.ui index 958a50b0..89ecc844 100644 --- a/guilib/src/ui/DatabaseViewer.ui +++ b/guilib/src/ui/DatabaseViewer.ui @@ -745,6 +745,7 @@ + @@ -1542,7 +1543,7 @@ 0 - -322 + 0 518 911 @@ -2137,7 +2138,7 @@ 0 0 - 298 + 226 192 @@ -3015,6 +3016,11 @@ RGBD-SLAM ID format (*.txt) + + + Update all landmark covariances... + + diff --git a/guilib/src/ui/editConstraintDialog.ui b/guilib/src/ui/editConstraintDialog.ui index fdeebcb2..afa25856 100644 --- a/guilib/src/ui/editConstraintDialog.ui +++ b/guilib/src/ui/editConstraintDialog.ui @@ -258,7 +258,7 @@ - <html><head/><body><p>Setting &sigma; to 0 will set identity covariance.</p></body></html> + <html><head/><body><p>Setting σ to 0 will set 9999 covariance.</p></body></html>