i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-12 14:30:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

Supporting GTSAM bearing/range factors for landmarks (#1346)

Browse Source 
* Supporting GTSAM bearing/range factors for landmarks

* refreshing initial covariance after being modified in same sessions
This commit is contained in:
matlabbe
2024-09-21 18:14:52 -07:00
committed by GitHub
parent 0e85bd849c
commit 379e11bd66
6 changed files with 528 additions and 324 deletions
Download Patch File Download Diff File Expand all files Collapse all files
guilib/src/DatabaseViewer.cpp
+23 -64
View File
@@ -4357,13 +4357,19 @@ void DatabaseViewer::updateCovariances(const QList<Link> & links)
{
if(links.size())
{
bool ok = false;
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) 0=inf"), 1, 0.0, 90, 2, &ok)*M_PI/180.0;
if(!ok) return;
double angularVar = stddev*stddev;
cv::Mat infMatrix = links.first().infMatrix();
std::multimap<int, Link>::iterator findIter = rtabmap::graph::findLink(linksRefined_, links.first().from() ,links.first().to(), false, links.first().type());
if(findIter != linksRefined_.end())
{
infMatrix = findIter->second.infMatrix();
}
EditConstraintDialog dialog(Transform::getIdentity(), infMatrix.inv());
dialog.setPoseGroupVisible(false);
if(dialog.exec() != QDialog::Accepted)
{
return;
}
rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
progressDialog->setAttribute(Qt::WA_DeleteOnClose);
@@ -4372,23 +4378,7 @@ void DatabaseViewer::updateCovariances(const QList<Link> & links)
progressDialog->setMinimumWidth(800);
progressDialog->show();
cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1);
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;
}
infMatrix = dialog.getCovariance().inv();
for(int i=0; i<links.size(); ++i)
{
@@ -6074,6 +6064,11 @@ void DatabaseViewer::editConstraint()
else if(ui_->label_type->text().toInt() == Link::kLandmark)
{
link = loopLinks_.at(ui_->horizontalSlider_loops->value());
std::multimap<int, Link>::iterator findIter = rtabmap::graph::findLink(linksRefined_, link.from() ,link.to(), false, link.type());
if(findIter != linksRefined_.end())
{
link = findIter->second;
}
}
else
{
@@ -6083,28 +6078,10 @@ void DatabaseViewer::editConstraint()
if(link.isValid())
{
cv::Mat covBefore = link.infMatrix().inv();
EditConstraintDialog dialog(link.transform(),
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);
EditConstraintDialog dialog(link.transform(), covBefore);
if(dialog.exec() == QDialog::Accepted)
{
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();
}
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();
}
else
{
covariance(cv::Range(3,6), cv::Range(3,6)) *= 9999.9;
}
cv::Mat covariance = dialog.getCovariance();
Link newLink(link.from(), link.to(), link.type(), dialog.getTransform(), covariance.inv());
std::multimap<int, Link>::iterator iter = linksRefined_.find(link.from());
while(iter != linksRefined_.end() && iter->first == link.from())
@@ -6133,28 +6110,10 @@ void DatabaseViewer::editConstraint()
else
{
EditConstraintDialog dialog(
link.transform(),
priorId>0?0.001:1,
priorId>0?0.001:1);
link.transform(), cv::Mat::eye(6,6,CV_64FC1) * (priorId>0?0.00001:1));
if(dialog.exec() == QDialog::Accepted)
{
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();
}
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();
}
else
{
covariance(cv::Range(3,6), cv::Range(3,6)) *= 9999.9;
}
cv::Mat covariance = dialog.getCovariance();
int from = priorId>0?priorId:ids_.at(ui_->horizontalSlider_A->value());
int to = priorId>0?priorId:ids_.at(ui_->horizontalSlider_B->value());
Link newLink(