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

Implemented OptimizerG2O::optimimzeBA(). Updated PostProcessingDialog (g2o sba option). Fixed words descriptors not filled in rtabmap::getMap3D(). CameraModelD(): return 5 null coeff distorsions if not set

Browse Source
This commit is contained in:
matlabbe
2016-03-28 18:19:17 -04:00
parent 18759e7197
commit 8030d89634
16 changed files with 556 additions and 293 deletions
Download Patch File Download Diff File Expand all files Collapse all files

107
guilib/src/MainWindow.cpp
View File

@@ -3879,7 +3879,6 @@ void MainWindow::postProcessing()
}
bool detectMoreLoopClosures = _postProcessingDialog->isDetectMoreLoopClosures();
bool reextractFeatures = _postProcessingDialog->isReextractFeatures();
bool refineNeighborLinks = _postProcessingDialog->isRefineNeighborLinks();
bool refineLoopClosureLinks = _postProcessingDialog->isRefineLoopClosureLinks();
double clusterRadius = _postProcessingDialog->clusterRadius();
@@ -3888,8 +3887,7 @@ void MainWindow::postProcessing()
bool sba = _postProcessingDialog->isSBA();
int sbaIterations = _postProcessingDialog->sbaIterations();
double sbaEpsilon = _postProcessingDialog->sbaEpsilon();
double sbaInlierDistance = _postProcessingDialog->sbaInlierDistance();
int sbaMinInliers = _postProcessingDialog->sbaMinInliers();
Optimizer::Type sbaType = _postProcessingDialog->sbaType();
if(!detectMoreLoopClosures && !refineNeighborLinks && !refineLoopClosureLinks && !sba)
{
@@ -3960,6 +3958,7 @@ void MainWindow::postProcessing()
bool warn = false;
int loopClosuresAdded = 0;
std::multimap<int, int> checkedLoopClosures;
if(detectMoreLoopClosures)
{
UDEBUG("");
@@ -3990,67 +3989,58 @@ void MainWindow::postProcessing()
to = iter->first;
}
// only add new links and one per cluster per iteration
if(addedLinks.find(from) == addedLinks.end() && addedLinks.find(to) == addedLinks.end() &&
rtabmap::graph::findLink(_currentLinksMap, from, to) == _currentLinksMap.end())
bool alreadyChecked = false;
for(std::multimap<int, int>::iterator jter = checkedLoopClosures.lower_bound(from);
!alreadyChecked && jter!=checkedLoopClosures.end() && jter->first == from;
++jter)
{
if(!_cachedSignatures.contains(from))
if(to == jter->second)
{
UERROR("Didn't find signature %d", from);
alreadyChecked = true;
}
else if(!_cachedSignatures.contains(to))
{
UERROR("Didn't find signature %d", to);
}
else
{
_initProgressDialog->incrementStep();
QApplication::processEvents();
}
Signature signatureFrom = _cachedSignatures[from];
Signature signatureTo = _cachedSignatures[to];
if(!alreadyChecked)
{
// only add new links and one per cluster per iteration
if(addedLinks.find(from) == addedLinks.end() &&
rtabmap::graph::findLink(_currentLinksMap, from, to) == _currentLinksMap.end())
{
checkedLoopClosures.insert(std::make_pair(from, to));
if(signatureFrom.getWeight() >= 0 &&
signatureTo.getWeight() >= 0) // ignore intermediate nodes
if(!_cachedSignatures.contains(from))
{
if(reextractFeatures)
UERROR("Didn't find signature %d", from);
}
else if(!_cachedSignatures.contains(to))
{
UERROR("Didn't find signature %d", to);
}
else
{
_initProgressDialog->incrementStep();
QApplication::processEvents();
Signature signatureFrom = _cachedSignatures[from];
Signature signatureTo = _cachedSignatures[to];
if(signatureFrom.getWeight() >= 0 &&
signatureTo.getWeight() >= 0) // ignore intermediate nodes
{
signatureFrom.setWords(std::multimap<int, cv::KeyPoint>());
signatureFrom.setWords3(std::multimap<int, cv::Point3f>());
signatureTo.setWords(std::multimap<int, cv::KeyPoint>());
signatureTo.setWords3(std::multimap<int, cv::Point3f>());
Transform transform;
RegistrationInfo info;
RegistrationVis registration(parameters);
transform = registration.computeTransformation(signatureFrom, signatureTo, Transform(), &info);
if(signatureFrom.sensorData().imageRaw().empty())
if(!transform.isNull())
{
signatureFrom.sensorData().uncompressData();
if(signatureFrom.sensorData().imageRaw().empty())
{
continue;
}
UINFO("Added new loop closure between %d and %d.", from, to);
addedLinks.insert(from);
addedLinks.insert(to);
_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, info.variance, info.variance)));
++loopClosuresAdded;
_initProgressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
}
if(signatureTo.sensorData().imageRaw().empty())
{
signatureTo.sensorData().uncompressData();
if(signatureTo.sensorData().imageRaw().empty())
{
continue;
}
}
}
Transform transform;
RegistrationInfo info;
RegistrationVis registration(parameters);
transform = registration.computeTransformation(signatureFrom, signatureTo, Transform(), &info);
if(!transform.isNull())
{
UINFO("Added new loop closure between %d and %d.", from, to);
addedLinks.insert(from);
addedLinks.insert(to);
_currentLinksMap.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, transform, info.variance, info.variance)));
++loopClosuresAdded;
_initProgressDialog->appendText(tr("Detected loop closure %1->%2! (%3/%4)").arg(from).arg(to).arg(i+1).arg(clusters.size()));
}
}
}
@@ -4186,7 +4176,7 @@ void MainWindow::postProcessing()
if(sba)
{
UASSERT(OptimizerCVSBA::available());
UASSERT(Optimizer::isAvailable(sbaType));
_initProgressDialog->appendText(tr("SBA (%1 nodes, %2 constraints, %3 iterations)...")
.arg(optimizedPoses.size()).arg(linksOut.size()).arg(sbaIterations));
QApplication::processEvents();
@@ -4196,10 +4186,9 @@ void MainWindow::postProcessing()
ParametersMap parametersSBA = _preferencesDialog->getAllParameters();
uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(sbaIterations)));
uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerEpsilon(), uNumber2Str(sbaEpsilon)));
OptimizerCVSBA cvsba = OptimizerCVSBA(parametersSBA);
cvsba.setInlierDistance(sbaInlierDistance);
cvsba.setMinInliers(sbaMinInliers);
std::map<int, Transform> newPoses = cvsba.optimizeBA(0, optimizedPoses, linksOut, _cachedSignatures.toStdMap());
Optimizer * sba = Optimizer::create(sbaType, parametersSBA);
std::map<int, Transform> newPoses = sba->optimizeBA(optimizedPoses.begin()->first, optimizedPoses, linksOut, _cachedSignatures.toStdMap());
delete sba;
if(newPoses.size())
{
optimizedPoses = newPoses;

60
guilib/src/PostProcessingDialog.cpp
View File

@@ -39,11 +39,21 @@ PostProcessingDialog::PostProcessingDialog(QWidget * parent) :
_ui = new Ui_PostProcessingDialog();
_ui->setupUi(this);
if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA))
if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA) && !Optimizer::isAvailable(Optimizer::kTypeG2O))
{
_ui->sba->setEnabled(false);
_ui->sba->setChecked(false);
}
else if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA))
{
_ui->comboBox_sbaType->setItemData(1, 0, Qt::UserRole - 1);
_ui->comboBox_sbaType->setCurrentIndex(0);
}
else if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
{
_ui->comboBox_sbaType->setItemData(0, 0, Qt::UserRole - 1);
_ui->comboBox_sbaType->setCurrentIndex(1);
}
restoreDefaults();
@@ -57,15 +67,13 @@ PostProcessingDialog::PostProcessingDialog(QWidget * parent) :
connect(_ui->clusterRadius, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->clusterAngle, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->iterations, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
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_epsilon, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->sba_minInlierDistance, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->sba_minInliers, SIGNAL(valueChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->comboBox_sbaType, SIGNAL(indexChanged(int)), this, SIGNAL(configChanged()));
}
PostProcessingDialog::~PostProcessingDialog()
@@ -83,14 +91,12 @@ void PostProcessingDialog::saveSettings(QSettings & settings, const QString & gr
settings.setValue("cluster_radius", this->clusterRadius());
settings.setValue("cluster_angle", this->clusterAngle());
settings.setValue("iterations", this->iterations());
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_epsilon", this->sbaEpsilon());
settings.setValue("sba_inlier_distance", this->sbaInlierDistance());
settings.setValue("sba_min_inliers", this->sbaMinInliers());
settings.setValue("sba_type", this->sbaType());
if(!group.isEmpty())
{
settings.endGroup();
@@ -107,14 +113,12 @@ void PostProcessingDialog::loadSettings(QSettings & settings, const QString & gr
this->setClusterRadius(settings.value("cluster_radius", this->clusterRadius()).toDouble());
this->setClusterAngle(settings.value("cluster_angle", this->clusterAngle()).toDouble());
this->setIterations(settings.value("iterations", this->iterations()).toInt());
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->setSBAEpsilon(settings.value("sba_epsilon", this->sbaEpsilon()).toDouble());
this->setSBAInlierDistance(settings.value("sba_inlier_distance", this->sbaInlierDistance()).toDouble());
this->setSBAMinInliers(settings.value("sba_min_inliers", this->sbaMinInliers()).toInt());
this->setSBAType((Optimizer::Type)settings.value("sba_type", this->sbaType()).toInt());
if(!group.isEmpty())
{
settings.endGroup();
@@ -127,14 +131,12 @@ void PostProcessingDialog::restoreDefaults()
setClusterRadius(0.5);
setClusterAngle(30);
setIterations(1);
setReextractFeatures(false);
setRefineNeighborLinks(false);
setRefineLoopClosureLinks(false);
setSBA(false);
setSBAIterations(20);
setSBAEpsilon(0.0001);
setSBAInlierDistance(0.05);
setSBAMinInliers(10);
setSBAType(!Optimizer::isAvailable(Optimizer::kTypeG2O)&&Optimizer::isAvailable(Optimizer::kTypeCVSBA)?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O);
}
void PostProcessingDialog::updateButtonBox()
@@ -163,11 +165,6 @@ int PostProcessingDialog::iterations() const
return _ui->iterations->value();
}
bool PostProcessingDialog::isReextractFeatures() const
{
return _ui->reextractFeatures->isChecked();
}
bool PostProcessingDialog::isRefineNeighborLinks() const
{
return _ui->refineNeighborLinks->isChecked();
@@ -191,13 +188,9 @@ double PostProcessingDialog::sbaEpsilon() const
{
return _ui->sba_epsilon->value();
}
double PostProcessingDialog::sbaInlierDistance() const
Optimizer::Type PostProcessingDialog::sbaType() const
{
return _ui->sba_minInlierDistance->value();
}
int PostProcessingDialog::sbaMinInliers() const
{
return _ui->sba_minInliers->value();
return _ui->comboBox_sbaType->currentIndex()==0?Optimizer::kTypeG2O:Optimizer::kTypeCVSBA;
}
//setters
@@ -217,10 +210,6 @@ void PostProcessingDialog::setIterations(int iterations)
{
_ui->iterations->setValue(iterations);
}
void PostProcessingDialog::setReextractFeatures(bool on)
{
_ui->reextractFeatures->setChecked(on);
}
void PostProcessingDialog::setRefineNeighborLinks(bool on)
{
_ui->refineNeighborLinks->setChecked(on);
@@ -241,13 +230,16 @@ void PostProcessingDialog::setSBAEpsilon(double epsilon)
{
_ui->sba_epsilon->setValue(epsilon);
}
void PostProcessingDialog::setSBAInlierDistance(double inlierDistance)
void PostProcessingDialog::setSBAType(Optimizer::Type type)
{
_ui->sba_minInlierDistance->setValue(inlierDistance);
}
void PostProcessingDialog::setSBAMinInliers(int minInliers)
{
_ui->sba_minInliers->setValue(minInliers);
if(type == Optimizer::kTypeCVSBA)
{
_ui->comboBox_sbaType->setCurrentIndex(1);
}
else
{
_ui->comboBox_sbaType->setCurrentIndex(0);
}
}

10
guilib/src/PostProcessingDialog.h
View File

@@ -31,6 +31,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <QDialog>
#include <QtCore/QSettings>
#include <rtabmap/core/Optimizer.h>
class Ui_PostProcessingDialog;
class QAbstractButton;
@@ -53,28 +55,24 @@ public:
double clusterRadius() const;
double clusterAngle() const;
int iterations() const;
bool isReextractFeatures() const;
bool isRefineNeighborLinks() const;
bool isRefineLoopClosureLinks() const;
bool isSBA() const;
int sbaIterations() const;
double sbaEpsilon() const;
double sbaInlierDistance() const;
int sbaMinInliers() const;
Optimizer::Type sbaType() const;
//setters
void setDetectMoreLoopClosures(bool on);
void setClusterRadius(double radius);
void setClusterAngle(double angle);
void setIterations(int iterations);
void setReextractFeatures(bool on);
void setRefineNeighborLinks(bool on);
void setRefineLoopClosureLinks(bool on);
void setSBA(bool on);
void setSBAIterations(int iterations);
void setSBAEpsilon(double epsilon);
void setSBAInlierDistance(double inlierDistance);
void setSBAMinInliers(int minInliers);
void setSBAType(Optimizer::Type type);
signals:
void configChanged();

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

@@ -87,26 +87,6 @@
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_5">
<property name="text">
<string>Re-extract features</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QCheckBox" name="reextractFeatures">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_6">
<property name="text">
@@ -201,72 +181,7 @@
<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="2" 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="2" 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="3" 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="3" 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>
<item row="0" column="0">
<item row="1" column="0">
<widget class="QSpinBox" name="sba_iterations">
<property name="minimum">
<number>1</number>
@@ -279,17 +194,21 @@
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_9">
<property name="text">
<string>Stop optimizing when the error improvement is less than this value.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<item row="0" column="0">
<widget class="QComboBox" name="comboBox_sbaType">
<item>
<property name="text">
<string>g2o</string>
</property>
</item>
<item>
<property name="text">
<string>cvsba</string>
</property>
</item>
</widget>
</item>
<item row="1" column="0">
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="sba_epsilon">
<property name="suffix">
<string/>
@@ -311,6 +230,36 @@
</property>
</widget>
</item>
<item row="1" 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="2" column="1">
<widget class="QLabel" name="label_9">
<property name="text">
<string>Stop optimizing when the error improvement is less than this value.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_10">
<property name="text">
<string>Type</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
</layout>