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

GTSAM: added optimizer approach option #172

Browse Source
This commit is contained in:
matlabbe
2017-03-02 16:46:15 -05:00
parent 9b3b7b09a0
commit 300e37124c
5 changed files with 92 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
corelib/include/rtabmap/core/OptimizerGTSAM.h
View File

@@ -40,20 +40,18 @@ public:
static bool available();
public:
OptimizerGTSAM(
int iterations = Parameters::defaultOptimizerIterations(),
bool slam2d = Parameters::defaultRegForce3DoF(),
bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
double epsilon = Parameters::defaultOptimizerEpsilon(),
bool robust = Parameters::defaultOptimizerRobust()) :
Optimizer(iterations, slam2d, covarianceIgnored, epsilon, robust) {}
OptimizerGTSAM(const ParametersMap & parameters) :
Optimizer(parameters) {}
OptimizerGTSAM(const ParametersMap & parameters = ParametersMap()) :
Optimizer(parameters),
optimizer_(Parameters::defaultGTSAMOptimizer())
{
parseParameters(parameters);
}
virtual ~OptimizerGTSAM() {}
virtual Type type() const {return kTypeGTSAM;}
virtual void parseParameters(const ParametersMap & parameters);
virtual std::map<int, Transform> optimize(
int rootId,
const std::map<int, Transform> & poses,
@@ -61,6 +59,9 @@ public:
std::list<std::map<int, Transform> > * intermediateGraphes = 0,
double * finalError = 0,
int * iterationsDone = 0);
private:
int optimizer_;
};
} /* namespace rtabmap */

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

@@ -358,6 +358,8 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(g2o, RobustKernelDelta, double, 8, "Robust kernel delta used for bundle adjustment (0 means don't use robust kernel). Observations with chi2 over this threshold will be ignored in the second optimization pass.");
RTABMAP_PARAM(g2o, Baseline, double, 0.075, "When doing bundle adjustment with RGB-D data, we can set a fake baseline (m) to do stereo bundle adjustment (if 0, mono bundle adjustment is done). For stereo data, the baseline in the calibration is used directly.");
RTABMAP_PARAM(GTSAM, Optimizer, int, 1, "0=Levenberg 1=GaussNewton 2=Dogleg");
// Odometry
RTABMAP_PARAM(Odom, Strategy, int, 0, "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F)");
RTABMAP_PARAM(Odom, ResetCountdown, int, 0, "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");

53
corelib/src/OptimizerGTSAM.cpp
View File

@@ -69,6 +69,12 @@ bool OptimizerGTSAM::available()
#endif
}
void OptimizerGTSAM::parseParameters(const ParametersMap & parameters)
{
Optimizer::parseParameters(parameters);
Parameters::parse(parameters, Parameters::kGTSAMOptimizer(), optimizer_);
}
std::map<int, Transform> OptimizerGTSAM::optimize(
int rootId,
const std::map<int, Transform> & poses,
@@ -215,18 +221,29 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
UDEBUG("create optimizer");
gtsam::GaussNewtonParams parameters;
parameters.relativeErrorTol = epsilon();
parameters.maxIterations = iterations();
gtsam::GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters);
//gtsam::LevenbergMarquardtParams parametersLev;
//parametersLev.relativeErrorTol = epsilon();
//parametersLev.maxIterations = iterations();
//gtsam::LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parametersLev);
//gtsam::DoglegParams parametersDogleg;
//parametersDogleg.relativeErrorTol = epsilon();
//parametersDogleg.maxIterations = iterations();
//gtsam::DoglegOptimizer optimizer(graph, initialEstimate, parametersDogleg);
gtsam::NonlinearOptimizer * optimizer;
if(optimizer_ == 2)
{
gtsam::DoglegParams parameters;
parameters.relativeErrorTol = epsilon();
parameters.maxIterations = iterations();
optimizer = new gtsam::DoglegOptimizer(graph, initialEstimate, parameters);
}
else if(optimizer_ == 1)
{
gtsam::GaussNewtonParams parameters;
parameters.relativeErrorTol = epsilon();
parameters.maxIterations = iterations();
optimizer = new gtsam::GaussNewtonOptimizer(graph, initialEstimate, parameters);
}
else
{
gtsam::LevenbergMarquardtParams parameters;
parameters.relativeErrorTol = epsilon();
parameters.maxIterations = iterations();
optimizer = new gtsam::LevenbergMarquardtOptimizer(graph, initialEstimate, parameters);
}
UINFO("GTSAM optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
UTimer timer;
@@ -237,7 +254,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
if(intermediateGraphes && i > 0)
{
std::map<int, Transform> tmpPoses;
for(gtsam::Values::const_iterator iter=optimizer.values().begin(); iter!=optimizer.values().end(); ++iter)
for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
{
if(iter->value.dim() > 1)
{
@@ -257,17 +274,18 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
try
{
optimizer.iterate();
optimizer->iterate();
++it;
}
catch(gtsam::IndeterminantLinearSystemException & e)
{
UERROR("GTSAM exception caught: %s", e.what());
delete optimizer;
return optimizedPoses;
}
// early stop condition
double error = optimizer.error();
double error = optimizer->error();
UDEBUG("iteration %d error =%f", i+1, error);
double errorDelta = lastError - error;
if(i>0 && errorDelta < this->epsilon())
@@ -297,9 +315,9 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
{
*iterationsDone = it;
}
UINFO("GTSAM optimizing end (%d iterations done, error=%f (initial=%f final=%f), time=%f s)", optimizer.iterations(), optimizer.error(), graph.error(initialEstimate), graph.error(optimizer.values()), timer.ticks());
UINFO("GTSAM optimizing end (%d iterations done, error=%f (initial=%f final=%f), time=%f s)", optimizer->iterations(), optimizer->error(), graph.error(initialEstimate), graph.error(optimizer->values()), timer.ticks());
for(gtsam::Values::const_iterator iter=optimizer.values().begin(); iter!=optimizer.values().end(); ++iter)
for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
{
if(iter->value.dim() > 1)
{
@@ -315,6 +333,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
}
}
}
delete optimizer;
}
else if(poses.size() == 1 || iterations() <= 0)
{