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

Added source odometry guess option in standalone app

Browse Source
This commit is contained in:
matlabbe
2025-09-28 16:31:42 -07:00
parent a552bdfb2f
commit 0f37bafd66
7 changed files with 389 additions and 336 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Odometry.cpp
+13 -2
View File
@@ -625,6 +625,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
if(!guessIn.isNull())
{
guess = guessIn;
UDEBUG("Using provided guess %s", guessIn.prettyPrint().c_str());
}
else if(!imus_.empty())
{
@@ -641,12 +642,16 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
{
guess = guess.to3DoF();
}
UDEBUG("Adjusting guess from motion with IMU %s", guess.prettyPrint().c_str());
}
else if(!imuLastTransform_.isNull())
{
UWARN("Could not find imu transform at %f", data.stamp());
}
}
else if(!guess.isNull()) {
UDEBUG("Using guess from motion %s", guess.prettyPrint().c_str());
}
UTimer time;
@@ -1011,8 +1016,14 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
--_resetCurrentCount;
if(_resetCurrentCount == 0)
{
UWARN("Odometry automatically reset to latest pose!");
this->reset(_pose);
if(!guess.isNull() && !guessFromMotion_) {
UWARN("Odometry automatically reset to latest pose (%s) + guess (%s)!", _pose.prettyPrint().c_str(), guess.prettyPrint().c_str());
this->reset(_pose * guess);
}
else {
UWARN("Odometry automatically reset to latest pose (%s)!", _pose.prettyPrint().c_str());
this->reset(_pose);
}
_resetCurrentCount = _resetCountdown;
if(info)
{