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

Fixed "localization won't be corrected with gravity" warning in localization mode and imu data are not used (while Optimizer/GravitySigma>0)

Browse Source
This commit is contained in:
matlabbe
2021-05-02 19:42:59 -04:00
parent 40ab98a814
commit ff956a725e
1 changed files with 6 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
corelib/src/Rtabmap.cpp
View File

@@ -2900,9 +2900,10 @@ bool Rtabmap::process(
u = signature->getPose() * transform; u = signature->getPose() * transform;
} }
else else if(iterGravityLoop!=loopS->getLinks().end() ||
iterGravitySign!=signature->getLinks().end())
{ {
UWARN("Gravity link not found for %d and/or %d, localization won't be corrected with gravity.", loopId, signature->id()); UWARN("Gravity link not found for %d or %d, localization won't be corrected with gravity.", loopId, signature->id());
} }
} }
Transform up = u * oldPose.inverse(); Transform up = u * oldPose.inverse();
@@ -2955,9 +2956,10 @@ bool Rtabmap::process(
newPose = _optimizedPoses.at(loopId) * transform.inverse(); newPose = _optimizedPoses.at(loopId) * transform.inverse();
UDEBUG("newPose gravity=%s", newPose.prettyPrint().c_str()); UDEBUG("newPose gravity=%s", newPose.prettyPrint().c_str());
} }
else else if(iterGravityLoop!=loopS->getLinks().end() ||
iterGravitySign!=signature->getLinks().end())
{ {
UWARN("Gravity link not found for %d and/or %d, localization won't be corrected with gravity.", loopId, signature->id()); UWARN("Gravity link not found for %d or %d, localization won't be corrected with gravity.", loopId, signature->id());
} }
} }
_optimizedPoses.at(signature->id()) = newPose; _optimizedPoses.at(signature->id()) = newPose;