i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +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:
int solver_;
int optimizer_;
double pixelVariance_;
};
} /* 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, Optimizer, int, 0, "0=Levenberg 1=GaussNewton");
RTABMAP_PARAM(g2o, PixelVariance, double, 1.0, "Pixel variance used for SBA.");
// Odometry
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::kg2oOptimizer(), optimizer_);
Parameters::parse(parameters, Parameters::kg2oPixelVariance(), pixelVariance_);
UASSERT(pixelVariance_ > 0.0);
#ifndef G2O_HAVE_CHOLMOD
if(solver_ == 2)
@@ -738,7 +740,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
e->setVertex(0, vpt3d);
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(camId)));
e->setMeasurement(obs);
e->setInformation(Eigen::Matrix2d::Identity());
e->setInformation(Eigen::Matrix2d::Identity() / pixelVariance_);
if(robustKernel)
{
@@ -772,7 +774,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
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;
}

2
corelib/src/OptimizerGTSAM.cpp
View File

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