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 parameter "g2o/PixelVariance" (default 1)

Browse Source
This commit is contained in:
matlabbe
2016-04-06 15:24:36 -04:00
parent 000a2727ef
commit d2ebdea96d
10 changed files with 100 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/include/rtabmap/core/OptimizerG2O.h
View File

@@ -77,6 +77,7 @@ public:
private: private:
int solver_; int solver_;
int optimizer_; int optimizer_;
double pixelVariance_;
}; };
} /* namespace rtabmap */ } /* namespace rtabmap */

1
corelib/include/rtabmap/core/Parameters.h
View File

@@ -346,6 +346,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(g2o, Solver, int, 0, "0=csparse 1=pcg 2=cholmod"); RTABMAP_PARAM(g2o, Solver, int, 0, "0=csparse 1=pcg 2=cholmod");
RTABMAP_PARAM(g2o, Optimizer, int, 0, "0=Levenberg 1=GaussNewton"); RTABMAP_PARAM(g2o, Optimizer, int, 0, "0=Levenberg 1=GaussNewton");
RTABMAP_PARAM(g2o, PixelVariance, double, 1.0, "Pixel variance used for SBA.");
// Odometry // Odometry
RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F)"); RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F)");

6
corelib/src/OptimizerG2O.cpp
View File

@@ -112,6 +112,8 @@ void OptimizerG2O::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kg2oSolver(), solver_); Parameters::parse(parameters, Parameters::kg2oSolver(), solver_);
Parameters::parse(parameters, Parameters::kg2oOptimizer(), optimizer_); Parameters::parse(parameters, Parameters::kg2oOptimizer(), optimizer_);
Parameters::parse(parameters, Parameters::kg2oPixelVariance(), pixelVariance_);
UASSERT(pixelVariance_ > 0.0);
#ifndef G2O_HAVE_CHOLMOD #ifndef G2O_HAVE_CHOLMOD
if(solver_ == 2) if(solver_ == 2)
@@ -738,7 +740,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
e->setVertex(0, vpt3d); e->setVertex(0, vpt3d);
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(camId))); e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(camId)));
e->setMeasurement(obs); e->setMeasurement(obs);
e->setInformation(Eigen::Matrix2d::Identity()); e->setInformation(Eigen::Matrix2d::Identity() / pixelVariance_);
if(robustKernel) if(robustKernel)
{ {
@@ -772,7 +774,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
if(i>0 && (optimizer.activeRobustChi2() > 1000000000000.0 || !uIsFinite(optimizer.activeRobustChi2()))) if(i>0 && (optimizer.activeRobustChi2() > 1000000000000.0 || !uIsFinite(optimizer.activeRobustChi2())))
{ {
UWARN("g2o: Large optimimzation error detected (%f), aborting optimization!"); UWARN("g2o: Large optimization error detected (%f), aborting optimization!");
return optimizedPoses; return optimizedPoses;
} }

2
corelib/src/OptimizerGTSAM.cpp
View File

@@ -262,7 +262,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
} }
catch(gtsam::IndeterminantLinearSystemException & e) catch(gtsam::IndeterminantLinearSystemException & e)
{ {
UERROR("GTSAM exception catched: %s", e.what()); UERROR("GTSAM exception caught: %s", e.what());
return optimizedPoses; return optimizedPoses;
} }

2
guilib/src/MainWindow.cpp
View File

@@ -3887,6 +3887,7 @@ void MainWindow::postProcessing()
bool sba = _postProcessingDialog->isSBA(); bool sba = _postProcessingDialog->isSBA();
int sbaIterations = _postProcessingDialog->sbaIterations(); int sbaIterations = _postProcessingDialog->sbaIterations();
double sbaEpsilon = _postProcessingDialog->sbaEpsilon(); double sbaEpsilon = _postProcessingDialog->sbaEpsilon();
double sbaVariance = _postProcessingDialog->sbaVariance();
Optimizer::Type sbaType = _postProcessingDialog->sbaType(); Optimizer::Type sbaType = _postProcessingDialog->sbaType();
if(!detectMoreLoopClosures && !refineNeighborLinks && !refineLoopClosureLinks && !sba) if(!detectMoreLoopClosures && !refineNeighborLinks && !refineLoopClosureLinks && !sba)
@@ -4186,6 +4187,7 @@ void MainWindow::postProcessing()
ParametersMap parametersSBA = _preferencesDialog->getAllParameters(); ParametersMap parametersSBA = _preferencesDialog->getAllParameters();
uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(sbaIterations))); uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerIterations(), uNumber2Str(sbaIterations)));
uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerEpsilon(), uNumber2Str(sbaEpsilon))); uInsert(parametersSBA, std::make_pair(Parameters::kOptimizerEpsilon(), uNumber2Str(sbaEpsilon)));
uInsert(parametersSBA, std::make_pair(Parameters::kg2oPixelVariance(), uNumber2Str(sbaVariance)));
Optimizer * sba = Optimizer::create(sbaType, parametersSBA); Optimizer * sba = Optimizer::create(sbaType, parametersSBA);
std::map<int, Transform> newPoses = sba->optimizeBA(optimizedPoses.begin()->first, optimizedPoses, linksOut, _cachedSignatures.toStdMap()); std::map<int, Transform> newPoses = sba->optimizeBA(optimizedPoses.begin()->first, optimizedPoses, linksOut, _cachedSignatures.toStdMap());
delete sba; delete sba;

22
guilib/src/PostProcessingDialog.cpp
View File

@@ -74,6 +74,9 @@ PostProcessingDialog::PostProcessingDialog(QWidget * parent) :
connect(_ui->sba_iterations, SIGNAL(valueChanged(int)), 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_epsilon, SIGNAL(valueChanged(double)), this, SIGNAL(configChanged()));
connect(_ui->comboBox_sbaType, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged())); connect(_ui->comboBox_sbaType, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
connect(_ui->comboBox_sbaType, SIGNAL(currentIndexChanged(int)), this, SLOT(updateVisibility()));
updateVisibility();
} }
PostProcessingDialog::~PostProcessingDialog() PostProcessingDialog::~PostProcessingDialog()
@@ -81,6 +84,12 @@ PostProcessingDialog::~PostProcessingDialog()
delete _ui; delete _ui;
} }
void PostProcessingDialog::updateVisibility()
{
_ui->sba_variance->setVisible(_ui->comboBox_sbaType->currentIndex() == 0);
_ui->label_variance->setVisible(_ui->comboBox_sbaType->currentIndex() == 0);
}
void PostProcessingDialog::saveSettings(QSettings & settings, const QString & group) const void PostProcessingDialog::saveSettings(QSettings & settings, const QString & group) const
{ {
if(!group.isEmpty()) if(!group.isEmpty())
@@ -97,6 +106,7 @@ void PostProcessingDialog::saveSettings(QSettings & settings, const QString & gr
settings.setValue("sba_iterations", this->sbaIterations()); settings.setValue("sba_iterations", this->sbaIterations());
settings.setValue("sba_epsilon", this->sbaEpsilon()); settings.setValue("sba_epsilon", this->sbaEpsilon());
settings.setValue("sba_type", this->sbaType()); settings.setValue("sba_type", this->sbaType());
settings.setValue("sba_variance", this->sbaVariance());
if(!group.isEmpty()) if(!group.isEmpty())
{ {
settings.endGroup(); settings.endGroup();
@@ -119,6 +129,7 @@ void PostProcessingDialog::loadSettings(QSettings & settings, const QString & gr
this->setSBAIterations(settings.value("sba_iterations", this->sbaIterations()).toInt()); this->setSBAIterations(settings.value("sba_iterations", this->sbaIterations()).toInt());
this->setSBAEpsilon(settings.value("sba_epsilon", this->sbaEpsilon()).toDouble()); this->setSBAEpsilon(settings.value("sba_epsilon", this->sbaEpsilon()).toDouble());
this->setSBAType((Optimizer::Type)settings.value("sba_type", this->sbaType()).toInt()); this->setSBAType((Optimizer::Type)settings.value("sba_type", this->sbaType()).toInt());
this->setSBAVariance(settings.value("sba_variance", this->sbaVariance()).toDouble());
if(!group.isEmpty()) if(!group.isEmpty())
{ {
settings.endGroup(); settings.endGroup();
@@ -135,8 +146,9 @@ void PostProcessingDialog::restoreDefaults()
setRefineLoopClosureLinks(false); setRefineLoopClosureLinks(false);
setSBA(false); setSBA(false);
setSBAIterations(20); setSBAIterations(20);
setSBAEpsilon(0.0001); setSBAEpsilon(0.0);
setSBAType(!Optimizer::isAvailable(Optimizer::kTypeG2O)&&Optimizer::isAvailable(Optimizer::kTypeCVSBA)?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O); setSBAType(!Optimizer::isAvailable(Optimizer::kTypeG2O)&&Optimizer::isAvailable(Optimizer::kTypeCVSBA)?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O);
setSBAVariance(1.0);
} }
void PostProcessingDialog::updateButtonBox() void PostProcessingDialog::updateButtonBox()
@@ -188,6 +200,10 @@ double PostProcessingDialog::sbaEpsilon() const
{ {
return _ui->sba_epsilon->value(); return _ui->sba_epsilon->value();
} }
double PostProcessingDialog::sbaVariance() const
{
return _ui->sba_variance->value();
}
Optimizer::Type PostProcessingDialog::sbaType() const Optimizer::Type PostProcessingDialog::sbaType() const
{ {
return _ui->comboBox_sbaType->currentIndex()==0?Optimizer::kTypeG2O:Optimizer::kTypeCVSBA; return _ui->comboBox_sbaType->currentIndex()==0?Optimizer::kTypeG2O:Optimizer::kTypeCVSBA;
@@ -230,6 +246,10 @@ void PostProcessingDialog::setSBAEpsilon(double epsilon)
{ {
_ui->sba_epsilon->setValue(epsilon); _ui->sba_epsilon->setValue(epsilon);
} }
void PostProcessingDialog::setSBAVariance(double variance)
{
_ui->sba_variance->setValue(variance);
}
void PostProcessingDialog::setSBAType(Optimizer::Type type) void PostProcessingDialog::setSBAType(Optimizer::Type type)
{ {
if(type == Optimizer::kTypeCVSBA) if(type == Optimizer::kTypeCVSBA)

3
guilib/src/PostProcessingDialog.h
View File

@@ -60,6 +60,7 @@ public:
bool isSBA() const; bool isSBA() const;
int sbaIterations() const; int sbaIterations() const;
double sbaEpsilon() const; double sbaEpsilon() const;
double sbaVariance() const;
Optimizer::Type sbaType() const; Optimizer::Type sbaType() const;
//setters //setters
@@ -72,6 +73,7 @@ public:
void setSBA(bool on); void setSBA(bool on);
void setSBAIterations(int iterations); void setSBAIterations(int iterations);
void setSBAEpsilon(double epsilon); void setSBAEpsilon(double epsilon);
void setSBAVariance(double variance);
void setSBAType(Optimizer::Type type); void setSBAType(Optimizer::Type type);
signals: signals:
@@ -81,6 +83,7 @@ public slots:
void restoreDefaults(); void restoreDefaults();
private slots: private slots:
void updateVisibility();
void updateButtonBox(); void updateButtonBox();

1
guilib/src/PreferencesDialog.cpp
View File

@@ -658,6 +658,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->comboBox_g2o_solver->setObjectName(Parameters::kg2oSolver().c_str()); _ui->comboBox_g2o_solver->setObjectName(Parameters::kg2oSolver().c_str());
_ui->comboBox_g2o_optimizer->setObjectName(Parameters::kg2oOptimizer().c_str()); _ui->comboBox_g2o_optimizer->setObjectName(Parameters::kg2oOptimizer().c_str());
_ui->doubleSpinBox_g2o_variance->setObjectName(Parameters::kg2oPixelVariance().c_str());
_ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str()); _ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str());
_ui->graphPlan_goalsSavedInUserData->setObjectName(Parameters::kRGBDGoalsSavedInUserData().c_str()); _ui->graphPlan_goalsSavedInUserData->setObjectName(Parameters::kRGBDGoalsSavedInUserData().c_str());

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

@@ -187,7 +187,7 @@
<number>1</number> <number>1</number>
</property> </property>
<property name="maximum"> <property name="maximum">
<number>1000</number> <number>999999</number>
</property> </property>
<property name="value"> <property name="value">
<number>100</number> <number>100</number>
@@ -217,7 +217,7 @@
<number>4</number> <number>4</number>
</property> </property>
<property name="minimum"> <property name="minimum">
<double>0.000100000000000</double> <double>0.000000000000000</double>
</property> </property>
<property name="maximum"> <property name="maximum">
<double>1.000000000000000</double> <double>1.000000000000000</double>
@@ -260,6 +260,38 @@
</property> </property>
</widget> </widget>
</item> </item>
<item row="3" column="1">
<widget class="QLabel" name="label_variance">
<property name="text">
<string>Pixel variance used by g2o.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="sba_variance">
<property name="suffix">
<string/>
</property>
<property name="decimals">
<number>4</number>
</property>
<property name="minimum">
<double>0.000000000000000</double>
</property>
<property name="maximum">
<double>10.000000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
</item>
</layout> </layout>
</item> </item>
</layout> </layout>

35
guilib/src/ui/preferencesDialog.ui
View File

@@ -63,9 +63,9 @@
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>
<y>-1161</y> <y>0</y>
<width>681</width> <width>681</width>
<height>2010</height> <height>2044</height>
</rect> </rect>
</property> </property>
<layout class="QVBoxLayout" name="verticalLayout_16"> <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum> <enum>QFrame::Raised</enum>
</property> </property>
<property name="currentIndex"> <property name="currentIndex">
<number>3</number> <number>12</number>
</property> </property>
<widget class="QWidget" name="page_22"> <widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1"> <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -7060,6 +7060,35 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property> </property>
</widget> </widget>
</item> </item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="doubleSpinBox_g2o_variance">
<property name="enabled">
<bool>false</bool>
</property>
<property name="decimals">
<number>4</number>
</property>
<property name="minimum">
<double>0.000100000000000</double>
</property>
<property name="maximum">
<double>10.000000000000000</double>
</property>
<property name="value">
<double>1.000000000000000</double>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_148">
<property name="enabled">
<bool>false</bool>
</property>
<property name="text">
<string>Pixel variance for SBA.</string>
</property>
</widget>
</item>
</layout> </layout>
</widget> </widget>
</item> </item>