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

Added cvsba support for post-processing bundle adjustment (SBA)

Browse Source
This commit is contained in:
matlabbe
2015-08-12 20:59:21 -04:00
parent a8a3566fed
commit 31d374e141
13 changed files with 543 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
guilib/src/MainWindow.cpp
View File

@@ -3250,8 +3250,12 @@ void MainWindow::postProcessing()
double clusterRadius = _postProcessingDialog->clusterRadius();
double clusterAngle = _postProcessingDialog->clusterAngle();
int detectLoopClosureIterations = _postProcessingDialog->iterations();
bool sba = _postProcessingDialog->isSBA();
int sbaIterations = _postProcessingDialog->sbaIterations();
double sbaInlierDistance = _postProcessingDialog->sbaInlierDistance();
int sbaMinInliers = _postProcessingDialog->sbaMinInliers();
if(!detectMoreLoopClosures && !refineNeighborLinks && !refineLoopClosureLinks)
if(!detectMoreLoopClosures && !refineNeighborLinks && !refineLoopClosureLinks && !sba)
{
UWARN("No post-processing selection...");
return;
@@ -3331,6 +3335,10 @@ void MainWindow::postProcessing()
{
totalSteps+=(int)_currentLinksMap.size() - (int)odomPoses.size();
}
if(sba)
{
totalSteps+=1;
}
_initProgressDialog->setMaximumSteps(totalSteps);
_initProgressDialog->show();
@@ -3676,6 +3684,31 @@ void MainWindow::postProcessing()
_initProgressDialog->appendText(tr("Optimizing graph with updated links... done!"));
_initProgressDialog->incrementStep();
if(sba)
{
_initProgressDialog->appendText(tr("SBA (%1 nodes, %2 constraints)...")
.arg(optimizedPoses.size()).arg(linksOut.size()));
QApplication::processEvents();
QApplication::processEvents();
ParametersMap parametersSBA = _preferencesDialog->getAllParameters();
uInsert(parametersSBA, std::make_pair(Parameters::kRGBDOptimizeIterations(), uNumber2Str(sbaIterations)));
graph::CVSBAOptimizer cvsba = graph::CVSBAOptimizer(parameters);
cvsba.setInlierDistance(sbaInlierDistance);
cvsba.setMinInliers(sbaMinInliers);
std::map<int, Transform> newPoses = cvsba.optimizeBA(0, optimizedPoses, linksOut, _cachedSignatures.toStdMap());
if(newPoses.size())
{
optimizedPoses = newPoses;
_initProgressDialog->appendText(tr("SBA... done!"));
}
else
{
_initProgressDialog->appendText(tr("SBA... failed!"));
}
_initProgressDialog->incrementStep();
}
_initProgressDialog->appendText(tr("Updating map..."));
this->updateMapCloud(
optimizedPoses, Transform(),