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

Replaced all optimizeIncremental() by optimizeMultiSession() to fix GTSAM errors when merging multiple maps together. DBReader: always publish saved stamp. DBViewer: added actions to set covariance of all neighbor or loop closure links.

Browse Source
This commit is contained in:
matlabbe
2018-10-15 17:07:18 -04:00
parent b2c012d8bc
commit f1546c1fca
15 changed files with 285 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files

142
guilib/src/DatabaseViewer.cpp
View File

@@ -256,6 +256,8 @@ 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_->actionUpdate_all_neighbor_covariances, SIGNAL(triggered()), this, SLOT(updateAllNeighborCovariances()));
connect(ui_->actionUpdate_all_loop_closure_covariances, SIGNAL(triggered()), this, SLOT(updateAllLoopClosureCovariances()));
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()));
@@ -1466,6 +1468,7 @@ void DatabaseViewer::updateIds()
weights_.clear();
wmStates_.clear();
odomPoses_.clear();
dbOptimizedPoses_.clear();
groundTruthPoses_.clear();
gpsPoses_.clear();
gpsValues_.clear();
@@ -1626,6 +1629,8 @@ void DatabaseViewer::updateIds()
}
}
dbOptimizedPoses_ = dbDriver_->loadOptimizedPoses();
if(!groundTruthPoses_.empty() || !gpsPoses_.empty())
{
ui_->checkBox_alignPosesWithGroundTruth->setVisible(true);
@@ -3290,24 +3295,75 @@ void DatabaseViewer::detectMoreLoopClosures()
progressDialog->setValue(progressDialog->maximumSteps());
}
void DatabaseViewer::refineAllNeighborLinks()
void DatabaseViewer::updateAllNeighborCovariances()
{
if(neighborLinks_.size())
updateAllCovariances(neighborLinks_);
}
void DatabaseViewer::updateAllLoopClosureCovariances()
{
updateAllCovariances(loopLinks_);
}
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);
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;
if(!ok) return;
double angularVar = stddev*stddev;
rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
progressDialog->setAttribute(Qt::WA_DeleteOnClose);
progressDialog->setMaximumSteps(neighborLinks_.size());
progressDialog->setMaximumSteps(links.size());
progressDialog->setCancelButtonVisible(true);
progressDialog->setMinimumWidth(800);
progressDialog->show();
for(int i=0; i<neighborLinks_.size(); ++i)
{
int from = neighborLinks_[i].from();
int to = neighborLinks_[i].to();
this->refineConstraint(neighborLinks_[i].from(), neighborLinks_[i].to(), true);
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;
progressDialog->appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(neighborLinks_.size()));
for(int i=0; i<links.size(); ++i)
{
int from = links[i].from();
int to = links[i].to();
Link currentLink = findActiveLink(from, to);
if(!currentLink.isValid())
{
UERROR("Not found link! (%d->%d)", from, to);
return;
}
currentLink = Link(
currentLink.from(),
currentLink.to(),
currentLink.type(),
currentLink.transform(),
infMatrix.clone(),
currentLink.userDataCompressed());
bool updated = false;
std::multimap<int, Link>::iterator iter = linksRefined_.find(currentLink.from());
while(iter != linksRefined_.end() && iter->first == currentLink.from())
{
if(iter->second.to() == currentLink.to() &&
iter->second.type() == currentLink.type())
{
iter->second = currentLink;
updated = true;
break;
}
++iter;
}
if(!updated)
{
linksRefined_.insert(std::make_pair(currentLink.from(), currentLink));
}
progressDialog->appendText(tr("Updated link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(links.size()));
progressDialog->incrementStep();
QApplication::processEvents();
if(progressDialog->isCanceled())
@@ -3322,24 +3378,32 @@ void DatabaseViewer::refineAllNeighborLinks()
}
}
void DatabaseViewer::refineAllNeighborLinks()
{
refineAllLinks(neighborLinks_);
}
void DatabaseViewer::refineAllLoopClosureLinks()
{
if(loopLinks_.size())
refineAllLinks(loopLinks_);
}
void DatabaseViewer::refineAllLinks(const QList<Link> & links)
{
if(links.size())
{
rtabmap::ProgressDialog * progressDialog = new rtabmap::ProgressDialog(this);
progressDialog->setAttribute(Qt::WA_DeleteOnClose);
progressDialog->setMaximumSteps(loopLinks_.size());
progressDialog->setMaximumSteps(links.size());
progressDialog->setCancelButtonVisible(true);
progressDialog->setMinimumWidth(800);
progressDialog->show();
for(int i=0; i<loopLinks_.size(); ++i)
for(int i=0; i<links.size(); ++i)
{
int from = loopLinks_[i].from();
int to = loopLinks_[i].to();
this->refineConstraint(loopLinks_[i].from(), loopLinks_[i].to(), true);
int from = links[i].from();
int to = links[i].to();
this->refineConstraint(links[i].from(), links[i].to(), true);
progressDialog->appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(loopLinks_.size()));
progressDialog->appendText(tr("Refined link %1->%2 (%3/%4)").arg(from).arg(to).arg(i+1).arg(links.size()));
progressDialog->incrementStep();
QApplication::processEvents();
if(progressDialog->isCanceled())
@@ -5373,6 +5437,10 @@ void DatabaseViewer::updateGraphView()
{
optimizedGraphGuess = lastOptimizedGraph_;
}
else
{
optimizedGraphGuess = dbOptimizedPoses_;
}
graphes_.clear();
graphLinks_.clear();
@@ -5610,17 +5678,16 @@ void DatabaseViewer::updateGraphView()
time.start();
std::map<int, rtabmap::Transform> finalPoses = optimizer->optimize(fromId, posesOut, linksOut, ui_->checkBox_iterativeOptimization->isChecked()?&graphes_:0);
ui_->label_timeOptimization->setNum(double(time.elapsed())/1000.0);
graphes_.push_back(finalPoses);
graphLinks_ = linksOut;
ui_->label_poses->setNum((int)finalPoses.size());
if(posesOut.size() && finalPoses.empty())
{
UWARN("Optimization failed, trying incremental optimization instead... this may take a while (poses=%d, links=%d).", (int)posesOut.size(), (int)linksOut.size());
finalPoses = optimizer->optimizeIncremental(fromId, posesOut, linksOut, &graphes_);
UWARN("Optimization failed, trying multi-session optimization instead... (poses=%d, links=%d).", (int)posesOut.size(), (int)linksOut.size());
finalPoses = optimizer->optimizeMultiSession(fromId, posesOut, linksOut, &graphes_);
if(finalPoses.empty())
{
UWARN("Incremental optimization also failed.");
UWARN("Multi-session optimization also failed.");
if(!optimizer->isCovarianceIgnored() || optimizer->type() != Optimizer::kTypeTORO)
{
QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors. "
@@ -5631,17 +5698,8 @@ void DatabaseViewer::updateGraphView()
QMessageBox::warning(this, tr("Graph optimization error!"), tr("Graph optimization has failed. See the terminal for potential errors."));
}
}
else
{
UWARN("Incremental optimization succeeded!");
QMessageBox::information(this, tr("Incremental optimization succeeded!"), tr("Graph optimization has failed but "
"incremental optimization succeeded. Next optimizations will use the current "
"best optimized poses as first guess instead of odometry poses."));
useLastOptimizedGraphAsGuess_ = true;
lastOptimizedGraph_ = finalPoses;
}
}
graphes_.push_back(finalPoses);
delete optimizer;
}
if(graphes_.size())
@@ -6308,11 +6366,29 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
UASSERT_MSG(odomPoses_.find(newLink.from()) != odomPoses_.end(), uFormat("id=%d poses=%d links=%d", newLink.from(), (int)poses.size(), (int)links.size()).c_str());
UASSERT_MSG(odomPoses_.find(newLink.to()) != odomPoses_.end(), uFormat("id=%d poses=%d links=%d", newLink.to(), (int)poses.size(), (int)links.size()).c_str());
optimizer->getConnectedGraph(fromId, odomPoses_, linksIn, poses, links);
// use already optimized poses
if(graphes_.size())
{
const std::map<int, Transform> & optimizedPoses = graphes_.back();
for(std::map<int, Transform>::iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(optimizedPoses.find(iter->first) != optimizedPoses.end())
{
iter->second = optimizedPoses.at(iter->first);
}
}
}
UASSERT(poses.find(fromId) != poses.end());
UASSERT_MSG(poses.find(newLink.from()) != poses.end(), uFormat("id=%d poses=%d links=%d", newLink.from(), (int)poses.size(), (int)links.size()).c_str());
UASSERT_MSG(poses.find(newLink.to()) != poses.end(), uFormat("id=%d poses=%d links=%d", newLink.to(), (int)poses.size(), (int)links.size()).c_str());
UASSERT(graph::findLink(links, newLink.from(), newLink.to()) != links.end());
poses = optimizer->optimize(fromId, poses, links);
std::map<int, Transform> posesIn = poses;
poses = optimizer->optimize(fromId, posesIn, links);
if(posesIn.size() && poses.empty())
{
UWARN("Optimization failed, trying multi-session optimization instead... (poses=%d, links=%d).", (int)posesIn.size(), (int)links.size());
poses = optimizer->optimizeMultiSession(fromId, posesIn, links);
}
std::string msg;
if(poses.size())
{
@@ -6333,7 +6409,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
if(maxLinearErrorRatio > maxOptimizationError)
{
msg = uFormat("Rejecting edge %d->%d because "
"graph error is too large after optimization (ratio %f for edge %d->%d, stddev=%f). "
"graph error is too large after optimization (ratio %f for edge %d->%d, stddev=%f m). "
"\"%s\" is %f.",
newLink.from(),
newLink.to(),
@@ -6351,7 +6427,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
if(maxAngularErrorRatio > maxOptimizationError)
{
msg = uFormat("Rejecting edge %d->%d because "
"graph error is too large after optimization (ratio %f for edge %d->%d, stddev=%f). "
"graph error is too large after optimization (ratio %f for edge %d->%d, stddev=%f deg). "
"\"%s\" is %f.",
newLink.from(),
newLink.to(),

40
guilib/src/ui/DatabaseViewer.ui
View File

@@ -61,8 +61,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>327</width>
<height>222</height>
<width>301</width>
<height>242</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
@@ -253,8 +253,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>327</width>
<height>222</height>
<width>301</width>
<height>242</height>
</rect>
</property>
<layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
@@ -579,7 +579,7 @@
<x>0</x>
<y>0</y>
<width>1547</width>
<height>25</height>
<height>22</height>
</rect>
</property>
<widget class="QMenu" name="menuFile">
@@ -637,6 +637,8 @@
<addaction name="actionDetect_more_loop_closures"/>
<addaction name="actionRefine_all_neighbor_links"/>
<addaction name="actionRefine_all_loop_closure_links"/>
<addaction name="actionUpdate_all_neighbor_covariances"/>
<addaction name="actionUpdate_all_loop_closure_covariances"/>
<addaction name="separator"/>
<addaction name="actionRegenerate_local_grid_maps"/>
<addaction name="actionRegenerate_local_grid_maps_selected"/>
@@ -1228,8 +1230,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>312</width>
<height>200</height>
<width>318</width>
<height>197</height>
</rect>
</property>
<attribute name="label">
@@ -1389,9 +1391,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-167</y>
<width>441</width>
<height>764</height>
<y>0</y>
<width>519</width>
<height>791</height>
</rect>
</property>
<attribute name="label">
@@ -1924,8 +1926,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>200</width>
<height>111</height>
<width>205</width>
<height>114</height>
</rect>
</property>
<attribute name="label">
@@ -2024,8 +2026,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>179</width>
<height>452</height>
<width>185</width>
<height>485</height>
</rect>
</property>
<attribute name="label">
@@ -2696,6 +2698,16 @@
<string>Update optimized mesh...</string>
</property>
</action>
<action name="actionUpdate_all_neighbor_covariances">
<property name="text">
<string>Update all neighbor covariances...</string>
</property>
</action>
<action name="actionUpdate_all_loop_closure_covariances">
<property name="text">
<string>Update all loop closure covariances...</string>
</property>
</action>
</widget>
<customwidgets>
<customwidget>