i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +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

8
corelib/src/Odometry.cpp
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,

11
corelib/src/OdometryThread.cpp
View File

@@ -131,11 +131,18 @@ void OdometryThread::mainLoop()
if(!data.imageRaw().empty() || !data.laserScanRaw().empty() || (pose.isNull() && data.imu().empty()))
{
UDEBUG("Odom pose = %s", pose.prettyPrint().c_str());
// a null pose notify that odometry could not be computed
this->post(new OdometryEvent(data, pose, info));
if(!pose.isNull()) {
_previousGuessPose = event.info().odomPose;
if(!event.info().odomPose.isNull() && info.reg.covariance.at<double>(0,0) >= 9999)
{
// In case of external guess, keep reporting lost till we
// process the second frame with valid covariance. This way it
// won't trigger a new map.
pose = Transform();
}
}
// a null pose notify that odometry could not be computed
this->post(new OdometryEvent(data, pose, info));
}
}
}