i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-07 02:27:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

DbViewer: added update all landmark covariances menu action, also enabled edit constraint on landmark links. Covariance can be set to 9999.

Browse Source
This commit is contained in:
matlabbe
2021-10-06 11:34:40 -04:00
parent 44810a14e0
commit ab5fd5018b
6 changed files with 120 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
guilib/src/DatabaseViewer.cpp
+104 -35
View File
@@ -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<rtabmap::Link> links;
for(int i=0; i<loopLinks_.size(); ++i)
{
if(loopLinks_.at(i).type() != Link::kLandmark)
{
links.push_back(loopLinks_.at(i));
}
}
updateAllCovariances(links);
}
void DatabaseViewer::updateAllLandmarkCovariances()
{
QList<rtabmap::Link> links;
for(int i=0; i<loopLinks_.size(); ++i)
{
if(loopLinks_.at(i).type() == Link::kLandmark)
{
links.push_back(loopLinks_.at(i));
}
}
updateAllCovariances(links);
}
void DatabaseViewer::updateAllCovariances(const QList<Link> & links)
@@ -4136,10 +4157,10 @@ void DatabaseViewer::updateAllCovariances(const QList<Link> & 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<Link> & 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; i<links.size(); ++i)
{
@@ -5602,24 +5637,41 @@ void DatabaseViewer::editConstraint()
{
if(ids_.size())
{
Link link = this->findActiveLink(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<double>(0,0)!=1.0?std::sqrt(covBefore.at<double>(0,0)):0,
covBefore.at<double>(5,5)!=1.0?std::sqrt(covBefore.at<double>(5,5)):0);
covBefore.at<double>(0,0)<9999.0?std::sqrt(covBefore.at<double>(0,0)):0.0,
covBefore.at<double>(5,5)<9999.0?std::sqrt(covBefore.at<double>(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<int, Link>::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<int, Link>::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;