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

Fixed auto reset identity jump. If external guess, do not report lost on recovery after auto reset. (#1587)

Browse Source
This commit is contained in:
matlabbe
2025-09-30 16:22:54 -07:00
committed by GitHub
parent 0f37bafd66
commit 05f077b950
2 changed files with 14 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Odometry.cpp
+5 -3
View File
@@ -1016,7 +1016,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
--_resetCurrentCount;
if(_resetCurrentCount == 0)
{
if(!guess.isNull() && !guessFromMotion_) {
if(!guess.isNull() && !guessIn.isNull()) {
UWARN("Odometry automatically reset to latest pose (%s) + guess (%s)!", _pose.prettyPrint().c_str(), guess.prettyPrint().c_str());
this->reset(_pose * guess);
}
@@ -1029,14 +1029,15 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
{
*info = OdometryInfo();
}
return this->computeTransform(data, Transform(), info);
this->computeTransform(data, Transform(), info);
return _pose;
}
}
previousVelocities_.clear();
velocityGuess_.setNull();
previousStamp_ = 0;
}
return Transform();
}
@@ -1066,6 +1067,7 @@ void Odometry::initKalmanFilter(const Transform & initialPose, float vx, float v
0, 0, 0, 0, 0, 0.17 } };
static const boost::array<double, 36> STANDARD_TWIST_COVARIANCE =
{ { 0.05, 0, 0, 0, 0, 0,
}
0, 0.05, 0, 0, 0, 0,
0, 0, 0.05, 0, 0, 0,
0, 0, 0, 0.09, 0, 0,