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

OdometryThread: Updated logic on how to sync imu and sensor data (to make sure odom has always all IMUs it needs prior to process images)

Browse Source
This commit is contained in:
matlabbe
2025-06-29 17:00:12 -07:00
parent c9292bea5b
commit 963ba42a1a
5 changed files with 51 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/src/Graph.cpp
View File

@@ -445,6 +445,10 @@ bool importPoses(
std::list<std::string> strList = uSplit(str);
if((strList.size() >= 8 && format!=11) || (strList.size() == 9 && format==11))
{
if(!uIsNumber(strList.front())) {
UWARN("Skipping \"%s\"", str.c_str());
continue;
}
double stamp = uStr2Double(strList.front());
strList.pop_front();
if(format==11)

2
corelib/src/Odometry.cpp
View File

@@ -322,7 +322,9 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
Transform previous = this->getPose();
Transform newFramePose = Transform(previous.x(), previous.y(), previous.z(), imuQuat.x(), imuQuat.y(), imuQuat.z(), imuQuat.w());
UWARN("Updated initial pose from %s to %s with IMU orientation", previous.prettyPrint().c_str(), newFramePose.prettyPrint().c_str());
std::map<double, rtabmap::Transform> imus = imus_;
this->reset(newFramePose);
imus_ = imus;
}
imus_.insert(std::make_pair(data.stamp(), imuT));

60
corelib/src/OdometryThread.cpp
View File

@@ -40,8 +40,8 @@ OdometryThread::OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSi
_dataBufferMaxSize(dataBufferMaxSize),
_resetOdometry(false),
_resetPose(Transform::getIdentity()),
_lastImuStamp(0.0),
_imuEstimatedDelay(0.0)
_oldestAsyncImuStamp(0.0),
_newestAsyncImuStamp(0.0)
{
UASSERT(_odometry != 0);
}
@@ -110,7 +110,8 @@ void OdometryThread::mainLoop()
UScopeMutex lock(_dataMutex);
_dataBuffer.clear();
_imuBuffer.clear();
_lastImuStamp = 0.0f;
_oldestAsyncImuStamp = 0.0;
_newestAsyncImuStamp = 0.0;
}
SensorData data;
@@ -161,22 +162,39 @@ void OdometryThread::addData(const SensorData & data)
!data.laserScanCompressed().empty() ||
data.imu().empty())
{
_dataBuffer.push_back(data);
while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
{
UDEBUG("Data buffer is full, the oldest data is removed to add the new one.");
_dataBuffer.erase(_dataBuffer.begin());
if(_oldestAsyncImuStamp > 0.0 && data.stamp() < _oldestAsyncImuStamp) {
UWARN("Received image/lidar with stamp (%f) older than oldest received imu "
"(%f), skipping that frame (imu buffer size=%ld). "
"When using async IMU, make sure IMU is published faster "
"than camera/lidar (assuming IMU latency is very small compared to camera/lidar).",
data.stamp(), _oldestAsyncImuStamp, _imuBuffer.size());
notify = false;
}
else if(_newestAsyncImuStamp > 0.0 && data.stamp()>=_newestAsyncImuStamp) {
UWARN("Received image/lidar with stamp (%f) newer than latest received imu "
"(%f), skipping that frame (imu buffer size=%ld). "
"When using async IMU, make sure IMU is published faster "
"than camera/lidar (assuming IMU latency is very small compared to camera/lidar).",
data.stamp(), _newestAsyncImuStamp, _imuBuffer.size());
notify = false;
}
else {
_dataBuffer.push_back(data);
while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
{
UDEBUG("Data buffer is full, the oldest data is removed to add the new one.");
_dataBuffer.erase(_dataBuffer.begin());
notify = false;
}
}
}
else
{
_imuBuffer.push_back(data);
if(_lastImuStamp != 0.0 && data.stamp() > _lastImuStamp)
{
_imuEstimatedDelay = data.stamp() - _lastImuStamp;
if(_oldestAsyncImuStamp == 0) {
_oldestAsyncImuStamp = data.stamp();
}
_lastImuStamp = data.stamp();
_newestAsyncImuStamp = data.stamp();
}
}
_dataMutex.unlock();
@@ -195,18 +213,14 @@ bool OdometryThread::getData(SensorData & data)
{
if(!_dataBuffer.empty())
{
if(!_imuBuffer.empty())
// Send IMU up to stamp greater than image (OpenVINS needs this).
while(!_imuBuffer.empty())
{
// Send IMU up to stamp greater than image (OpenVINS needs this).
while(!_imuBuffer.empty())
{
_odometry->process(_imuBuffer.front());
double stamp = _imuBuffer.front().stamp();
_imuBuffer.pop_front();
if(stamp > _dataBuffer.front().stamp())
{
break;
}
_odometry->process(_imuBuffer.front());
double stamp =_imuBuffer.front().stamp();
_imuBuffer.pop_front();
if(stamp > _dataBuffer.front().stamp()) {
break;
}
}