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 optimizeFromGraphEnd (default true) parameter. It can be used to keep the map referential from the oldest node in the current graph

Browse Source 
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1441 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-06-25 23:00:35 +00:00
parent bb3c8afd45
commit f34907f92f
9 changed files with 171 additions and 88 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -225,7 +225,8 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(RGBD, LinearUpdate, float, 0.0, "Min linear displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
RTABMAP_PARAM(RGBD, AngularUpdate, float, 0.0, "Min angular displacement to update the map. Rehearsal is done prior to this, so weights are still updated.");
RTABMAP_PARAM(RGBD, NewMapOdomChangeDistance, float, 1, "A new map is created if a change of odometry translation greater than X m is detected (0 m = disabled).");
RTABMAP_PARAM(RGBD, ToroIterations, int, 100, "TORO graph optimization iterations")
RTABMAP_PARAM(RGBD, ToroIterations, int, 100, "TORO graph optimization iterations");
RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd, bool, true, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest mode of the current graph, but it can be useful to preserve the map referential from the oldest node).");
// Local loop closure detection
RTABMAP_PARAM(RGBD, LocalLoopDetectionTime, bool, true, "Detection over all locations in STM.");

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

@@ -152,6 +152,7 @@ private:
int _localDetectMaxDiffID;
int _toroIterations;
std::string _databasePath;
bool _optimizeFromGraphEnd;
int _lcHypothesisId;
float _lcHypothesisValue;

31
corelib/src/Rtabmap.cpp
View File

@@ -91,6 +91,7 @@ Rtabmap::Rtabmap() :
_localDetectMaxDiffID(Parameters::defaultRGBDLocalLoopDetectionMaxDiffID()),
_toroIterations(Parameters::defaultRGBDToroIterations()),
_databasePath(""),
_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
_lcHypothesisId(0),
_lcHypothesisValue(0),
_retrievedId(0),
@@ -339,6 +340,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionNeighbors(), _localDetectMaxNeighbors);
Parameters::parse(parameters, Parameters::kRGBDLocalLoopDetectionMaxDiffID(), _localDetectMaxDiffID);
Parameters::parse(parameters, Parameters::kRGBDToroIterations(), _toroIterations);
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
// RGB-D SLAM stuff
if((iter=parameters.find(Parameters::kLccIcpType())) != parameters.end())
@@ -826,13 +828,6 @@ bool Rtabmap::process(const Image & image)
}
}
// Reset map correction if we are mapping!
if(_memory->isIncremental() && !_mapCorrection.isIdentity())
{
UWARN("Reset map correction because we are now mapping!");
_mapCorrection.setIdentity();
}
Transform newPose = _mapCorrection * signature->getPose();
_optimizedPoses.insert(std::make_pair(signature->id(), newPose));
@@ -1358,6 +1353,13 @@ bool Rtabmap::process(const Image & image)
UINFO("Update map correction: SLAM mode");
// SLAM mode!
optimizeCurrentMap(signature->id(), false, _optimizedPoses, &_constraints);
// Update map correction, it should be identify when optimizing from the last node
_mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse();
if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
{
UERROR("Map correction should be identity when optimizing from the last node. T=%s", _mapCorrection.prettyPrint().c_str());
}
}
else if(_lcHypothesisId > 0 || localSpaceClosureId > 0 || signaturesRetrieved.size())
{
@@ -1957,6 +1959,21 @@ void Rtabmap::optimizeCurrentMap(
std::map<int, int> ids = _memory->getNeighborsId(id, 0, lookInDatabase?-1:0, true);
UDEBUG("ids=%d", (int)ids.size());
if(!_optimizeFromGraphEnd && ids.size() > 1)
{
UTimer timer;
UDEBUG("Optimize from the first location (%d) instead of the last (%d) "
"in the local graph. Recomputing neighbors depth...");
int first = ids.begin()->first;
ids = _memory->getNeighborsId(first, 0, lookInDatabase?-1:0, true);
UDEBUG("Optimize from the first location (%d) instead of the last (%d) "
"in the local graph. Recomputing neighbors depth... time=%fs",
first,
id,
timer.ticks());
}
std::map<int, Transform> poses;
std::multimap<int, Link> edgeConstraints;