i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-06 18:17:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added Optimizer/GravitySigma parameter (with GTSAM support).

Browse Source
This commit is contained in:
matlabbe
2019-03-31 15:51:50 -04:00
parent 35933cafba
commit fc9c762516
17 changed files with 565 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Optimizer.cpp
+7 -4
View File
@@ -210,7 +210,7 @@ void Optimizer::getConnectedGraph(
{
if(!uContains(posesOut, toId))
{
posesOut.insert(std::make_pair(toId, posesOut.at(fromId) * (kter->second.from()==fromId?kter->second.transform():kter->second.transform().inverse())));
posesOut.insert(*posesIn.find(toId));//std::make_pair(toId, posesOut.at(fromId) * (kter->second.from()==fromId?kter->second.transform():kter->second.transform().inverse())));
if(curentPoses.find(toId) == curentPoses.end())
{
nextPoses.insert(toId);
@@ -229,14 +229,15 @@ void Optimizer::getConnectedGraph(
}
}
Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust, bool priorsIgnored, bool landmarksIgnored) :
Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust, bool priorsIgnored, bool landmarksIgnored, float gravitySigma) :
iterations_(iterations),
slam2d_(slam2d),
covarianceIgnored_(covarianceIgnored),
epsilon_(epsilon),
robust_(robust),
priorsIgnored_(priorsIgnored),
landmarksIgnored_(landmarksIgnored)
landmarksIgnored_(landmarksIgnored),
gravitySigma_(gravitySigma)
{
}
@@ -247,7 +248,8 @@ Optimizer::Optimizer(const ParametersMap & parameters) :
epsilon_(Parameters::defaultOptimizerEpsilon()),
robust_(Parameters::defaultOptimizerRobust()),
priorsIgnored_(Parameters::defaultOptimizerPriorsIgnored()),
landmarksIgnored_(Parameters::defaultOptimizerLandmarksIgnored())
landmarksIgnored_(Parameters::defaultOptimizerLandmarksIgnored()),
gravitySigma_(Parameters::defaultOptimizerGravitySigma())
{
parseParameters(parameters);
}
@@ -261,6 +263,7 @@ void Optimizer::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kOptimizerRobust(), robust_);
Parameters::parse(parameters, Parameters::kOptimizerPriorsIgnored(), priorsIgnored_);
Parameters::parse(parameters, Parameters::kOptimizerLandmarksIgnored(), landmarksIgnored_);
Parameters::parse(parameters, Parameters::kOptimizerGravitySigma(), gravitySigma_);
}
std::map<int, Transform> Optimizer::optimizeIncremental(