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

Localization: Revert mapCorrection transform to one before being lost (when odom can be computed again)

Browse Source
This commit is contained in:
matlabbe
2016-12-05 15:16:04 -05:00
parent e5524ddc1d
commit cd62e67011
3 changed files with 17 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
corelib/src/Rtabmap.cpp
View File

@@ -330,6 +330,7 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
_optimizedPoses.clear();
_constraints.clear();
_mapCorrection.setIdentity();
_mapCorrectionBackup.setNull();
_lastLocalizationPose.setNull();
_lastLocalizationNodeId = 0;
_distanceTravelled = 0.0f;
@@ -647,6 +648,8 @@ int Rtabmap::triggerNewMap()
_optimizedPoses.clear();
_constraints.clear();
_lastLocalizationNodeId = 0;
_mapCorrection.setIdentity();
_mapCorrectionBackup.setNull();
//Verify if there are nodes that were merged through graph reduction
if(reducedIds.size() && _path.size())
@@ -781,6 +784,7 @@ void Rtabmap::resetMemory()
_optimizedPoses.clear();
_constraints.clear();
_mapCorrection.setIdentity();
_mapCorrectionBackup.setNull();
_lastLocalizationPose.setNull();
_lastLocalizationNodeId = 0;
_distanceTravelled = 0.0f;
@@ -879,8 +883,15 @@ bool Rtabmap::process(
//============================================================
// If RGBD SLAM is enabled, a pose must be set.
//============================================================
bool fakeOdom = false;
if(_rgbdSlamMode)
{
if(!_memory->isIncremental() && !odomPose.isNull() && !_mapCorrectionBackup.isNull())
{
_mapCorrection = _mapCorrectionBackup;
_mapCorrectionBackup.setNull();
}
if(odomPose.isNull())
{
if(_memory->isIncremental())
@@ -895,6 +906,7 @@ bool Rtabmap::process(
{
_lastLocalizationPose = Transform::getIdentity();
}
fakeOdom = true;
odomPose = _mapCorrection.inverse() * _lastLocalizationPose;
}
}
@@ -2215,6 +2227,10 @@ bool Rtabmap::process(
// Update map correction, it should be identify when optimizing from the last node
UASSERT(_optimizedPoses.find(signature->id()) != _optimizedPoses.end());
if(fakeOdom && _mapCorrectionBackup.isNull())
{
_mapCorrectionBackup = _mapCorrection;
}
_mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse();
_lastLocalizationPose = _optimizedPoses.at(signature->id()); // update
if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)