i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +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

7
guilib/src/AboutDialog.cpp
View File

@@ -61,6 +61,13 @@ AboutDialog::AboutDialog(QWidget * parent) :
_ui->label_flycapture2->setText(CameraStereoFlyCapture2::available()?"Yes":"No");
_ui->label_g2o->setText(graph::G2OOptimizer::available()?"Yes":"No");
#ifdef WITH_CVSBA
_ui->label_cvsba->setText("Yes");
#else
_ui->label_cvsba->setText("No");
#endif
}
AboutDialog::~AboutDialog()

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(),

62
guilib/src/PostProcessingDialog.cpp
View File

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "ui_postProcessingDialog.h"
#include <QPushButton>
#include <rtabmap/core/Graph.h>
namespace rtabmap {
@@ -38,6 +39,13 @@ PostProcessingDialog::PostProcessingDialog(QWidget * parent) :
_ui = new Ui_PostProcessingDialog();
_ui->setupUi(this);
if(!graph::CVSBAOptimizer::available())
{
_ui->sba->setEnabled(false);
}
restoreDefaults();
connect(_ui->detectMoreLoopClosures, SIGNAL(clicked(bool)), this, SLOT(updateButtonBox()));
connect(_ui->refineNeighborLinks, SIGNAL(stateChanged(int)), this, SLOT(updateButtonBox()));
connect(_ui->refineLoopClosureLinks, SIGNAL(stateChanged(int)), this, SLOT(updateButtonBox()));
@@ -50,6 +58,11 @@ PostProcessingDialog::PostProcessingDialog(QWidget * parent) :
connect(_ui->reextractFeatures, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->refineNeighborLinks, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->refineLoopClosureLinks, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->sba, SIGNAL(clicked(bool)), this, SIGNAL(configChanged()));
connect(_ui->sba_iterations, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->sba_minInlierDistance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->sba_minInliers, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
}
PostProcessingDialog::~PostProcessingDialog()
@@ -70,6 +83,10 @@ void PostProcessingDialog::saveSettings(QSettings & settings, const QString & gr
settings.setValue("reextract_features", this->isReextractFeatures());
settings.setValue("refine_neigbors", this->isRefineNeighborLinks());
settings.setValue("refine_lc", this->isRefineLoopClosureLinks());
settings.setValue("sba", this->isSBA());
settings.setValue("sba_iterations", this->sbaIterations());
settings.setValue("sba_inlier_distance", this->sbaInlierDistance());
settings.setValue("sba_min_inliers", this->sbaMinInliers());
if(!group.isEmpty())
{
settings.endGroup();
@@ -89,6 +106,10 @@ void PostProcessingDialog::loadSettings(QSettings & settings, const QString & gr
this->setReextractFeatures(settings.value("reextract_features", this->isReextractFeatures()).toBool());
this->setRefineNeighborLinks(settings.value("refine_neigbors", this->isRefineNeighborLinks()).toBool());
this->setRefineLoopClosureLinks(settings.value("refine_lc", this->isRefineLoopClosureLinks()).toBool());
this->setSBA(settings.value("sba", this->isSBA()).toBool());
this->setSBAIterations(settings.value("sba_iterations", this->sbaIterations()).toInt());
this->setSBAInlierDistance(settings.value("sba_inlier_distance", this->sbaInlierDistance()).toDouble());
this->setSBAMinInliers(settings.value("sba_min_inliers", this->sbaMinInliers()).toInt());
if(!group.isEmpty())
{
settings.endGroup();
@@ -104,12 +125,16 @@ void PostProcessingDialog::restoreDefaults()
setReextractFeatures(false);
setRefineNeighborLinks(false);
setRefineLoopClosureLinks(false);
setSBA(false);
setSBAIterations(150);
setSBAInlierDistance(0.02);
setSBAMinInliers(10);
}
void PostProcessingDialog::updateButtonBox()
{
_ui->buttonBox->button(QDialogButtonBox::Ok)->setEnabled(
isDetectMoreLoopClosures() || isRefineNeighborLinks() || isRefineLoopClosureLinks());
isDetectMoreLoopClosures() || isRefineNeighborLinks() || isRefineLoopClosureLinks() || isSBA());
}
bool PostProcessingDialog::isDetectMoreLoopClosures() const
@@ -147,6 +172,24 @@ bool PostProcessingDialog::isRefineLoopClosureLinks() const
return _ui->refineLoopClosureLinks->isChecked();
}
bool PostProcessingDialog::isSBA() const
{
return _ui->sba->isChecked();
}
int PostProcessingDialog::sbaIterations() const
{
return _ui->sba_iterations->value();
}
double PostProcessingDialog::sbaInlierDistance() const
{
return _ui->sba_minInlierDistance->value();
}
int PostProcessingDialog::sbaMinInliers() const
{
return _ui->sba_minInliers->value();
}
//setters
void PostProcessingDialog::setDetectMoreLoopClosures(bool on)
{
@@ -176,5 +219,22 @@ void PostProcessingDialog::setRefineLoopClosureLinks(bool on)
{
_ui->refineLoopClosureLinks->setChecked(on);
}
void PostProcessingDialog::setSBA(bool on)
{
_ui->sba->setChecked(graph::CVSBAOptimizer::available() && on);
}
void PostProcessingDialog::setSBAIterations(int iterations)
{
_ui->sba_iterations->setValue(iterations);
}
void PostProcessingDialog::setSBAInlierDistance(double inlierDistance)
{
_ui->sba_minInlierDistance->setValue(inlierDistance);
}
void PostProcessingDialog::setSBAMinInliers(int minInliers)
{
_ui->sba_minInliers->setValue(minInliers);
}
}

8
guilib/src/PostProcessingDialog.h
View File

@@ -56,6 +56,10 @@ public:
bool isReextractFeatures() const;
bool isRefineNeighborLinks() const;
bool isRefineLoopClosureLinks() const;
bool isSBA() const;
int sbaIterations() const;
double sbaInlierDistance() const;
int sbaMinInliers() const;
//setters
void setDetectMoreLoopClosures(bool on);
@@ -65,6 +69,10 @@ public:
void setReextractFeatures(bool on);
void setRefineNeighborLinks(bool on);
void setRefineLoopClosureLinks(bool on);
void setSBA(bool on);
void setSBAIterations(int iterations);
void setSBAInlierDistance(double inlierDistance);
void setSBAMinInliers(int minInliers);
signals:
void configChanged();

33
guilib/src/ui/aboutDialog.ui
View File

@@ -7,7 +7,7 @@
<x>0</x>
<y>0</y>
<width>824</width>
<height>507</height>
<height>581</height>
</rect>
</property>
<property name="sizePolicy">
@@ -82,6 +82,13 @@ p, li { white-space: pre-wrap; }
</item>
<item>
<layout class="QGridLayout" name="gridLayout" columnstretch="0,1">
<item row="11" column="0">
<widget class="QLabel" name="label_13">
<property name="text">
<string>With OpenNI2 :</string>
</property>
</widget>
</item>
<item row="13" column="0">
<widget class="QLabel" name="label_16">
<property name="text">
@@ -205,13 +212,6 @@ p, li { white-space: pre-wrap; }
</property>
</widget>
</item>
<item row="11" column="0">
<widget class="QLabel" name="label_13">
<property name="text">
<string>With OpenNI2 :</string>
</property>
</widget>
</item>
<item row="15" column="0">
<widget class="QLabel" name="label_14">
<property name="text">
@@ -293,6 +293,23 @@ p, li { white-space: pre-wrap; }
</property>
</widget>
</item>
<item row="16" column="0">
<widget class="QLabel" name="label_18">
<property name="text">
<string>With cvsba :</string>
</property>
</widget>
</item>
<item row="16" column="1">
<widget class="QLabel" name="label_cvsba">
<property name="text">
<string/>
</property>
<property name="alignment">
<set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
</property>
</widget>
</item>
</layout>
</item>
<item>

2
guilib/src/ui/mainWindow.ui
View File

@@ -27,7 +27,7 @@
<x>0</x>
<y>0</y>
<width>1012</width>
<height>22</height>
<height>25</height>
</rect>
</property>
<widget class="QMenu" name="menuFile">

101
guilib/src/ui/postProcessingDialog.ui
View File

@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>486</width>
<height>425</height>
<width>552</width>
<height>540</height>
</rect>
</property>
<property name="windowTitle">
@@ -187,6 +187,103 @@
</layout>
</widget>
</item>
<item>
<widget class="QGroupBox" name="sba">
<property name="title">
<string>Sparse Bundle Adjustment (SBA)</string>
</property>
<property name="checkable">
<bool>true</bool>
</property>
<property name="checked">
<bool>false</bool>
</property>
<layout class="QVBoxLayout" name="verticalLayout_4">
<item>
<layout class="QGridLayout" name="gridLayout_2" columnstretch="0,1">
<item row="0" column="1">
<widget class="QLabel" name="label_8">
<property name="text">
<string>Iterations</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QSpinBox" name="sba_iterations">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>1000</number>
</property>
<property name="value">
<number>100</number>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_10">
<property name="text">
<string>Inlier distance of corresponding 3D words</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QDoubleSpinBox" name="sba_minInlierDistance">
<property name="suffix">
<string> m</string>
</property>
<property name="decimals">
<number>3</number>
</property>
<property name="minimum">
<double>0.001000000000000</double>
</property>
<property name="maximum">
<double>10.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>0.020000000000000</double>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_11">
<property name="text">
<string>Minimum inliers</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QSpinBox" name="sba_minInliers">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>1000</number>
</property>
<property name="value">
<number>10</number>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>
</item>
<item>
<spacer name="verticalSpacer">
<property name="orientation">