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

15
corelib/src/Odometry.cpp
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)
{

61
corelib/src/OdometryThread.cpp
View File

@@ -63,7 +63,7 @@ bool OdometryThread::handleEvent(UEvent * event)
SensorEvent * sensorEvent = (SensorEvent*)event;
if(sensorEvent->getCode() == SensorEvent::kCodeData)
{
this->addData(sensorEvent->data());
this->addData(*sensorEvent);
}
}
else if(event->getClassName().compare("IMUEvent") == 0)
@@ -112,31 +112,42 @@ void OdometryThread::mainLoop()
_imuBuffer.clear();
_oldestAsyncImuStamp = 0.0;
_newestAsyncImuStamp = 0.0;
_previousGuessPose.setNull();
}
SensorData data;
if(getData(data))
SensorEvent event;
if(getData(event))
{
OdometryInfo info;
UDEBUG("Processing data...");
Transform pose = _odometry->process(data, &info);
Transform guess;
UDEBUG("event.info().odomPose=%s", event.info().odomPose.prettyPrint().c_str());
if(!_previousGuessPose.isNull() && !event.info().odomPose.isNull()) {
guess = _previousGuessPose.inverse() * event.info().odomPose;
}
SensorData data = event.data();
Transform pose = _odometry->process(data, guess , &info);
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;
}
}
}
}
void OdometryThread::addData(const SensorData & data)
void OdometryThread::addData(const SensorEvent & event)
{
if(data.imu().empty())
if(event.data().imu().empty())
{
if(dynamic_cast<OdometryMono*>(_odometry) == 0)
{
if((data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().empty() && data.stereoCameraModels().empty())) &&
data.laserScanRaw().empty())
if((event.data().imageRaw().empty() || event.data().depthOrRightRaw().empty() || (event.data().cameraModels().empty() && event.data().stereoCameraModels().empty())) &&
event.data().laserScanRaw().empty())
{
ULOGGER_ERROR("Missing some information (images/scans empty or missing calibration)!?");
return;
@@ -145,7 +156,7 @@ void OdometryThread::addData(const SensorData & data)
else
{
// Mono can accept RGB only
if(data.imageRaw().empty() || (data.cameraModels().empty() && data.stereoCameraModels().empty()))
if(event.data().imageRaw().empty() || (event.data().cameraModels().empty() && event.data().stereoCameraModels().empty()))
{
ULOGGER_ERROR("Missing some information (image empty or missing calibration)!?");
return;
@@ -156,30 +167,30 @@ void OdometryThread::addData(const SensorData & data)
bool notify = true;
_dataMutex.lock();
{
if( !data.imageRaw().empty() ||
!data.imageCompressed().empty() ||
!data.laserScanRaw().isEmpty() ||
!data.laserScanCompressed().empty() ||
data.imu().empty())
if( !event.data().imageRaw().empty() ||
!event.data().imageCompressed().empty() ||
!event.data().laserScanRaw().isEmpty() ||
!event.data().laserScanCompressed().empty() ||
event.data().imu().empty())
{
if(_oldestAsyncImuStamp > 0.0 && data.stamp() < _oldestAsyncImuStamp) {
if(_oldestAsyncImuStamp > 0.0 && event.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());
event.data().stamp(), _oldestAsyncImuStamp, _imuBuffer.size());
notify = false;
}
else if(_newestAsyncImuStamp > 0.0 && data.stamp()>=_newestAsyncImuStamp) {
else if(_newestAsyncImuStamp > 0.0 && event.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());
event.data().stamp(), _newestAsyncImuStamp, _imuBuffer.size());
notify = false;
}
else {
_dataBuffer.push_back(data);
_dataBuffer.push_back(event);
while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
{
UDEBUG("Data buffer is full, the oldest data is removed to add the new one.");
@@ -190,11 +201,11 @@ void OdometryThread::addData(const SensorData & data)
}
else
{
_imuBuffer.push_back(data);
_imuBuffer.push_back(event.data());
if(_oldestAsyncImuStamp == 0) {
_oldestAsyncImuStamp = data.stamp();
_oldestAsyncImuStamp = event.data().stamp();
}
_newestAsyncImuStamp = data.stamp();
_newestAsyncImuStamp = event.data().stamp();
}
}
_dataMutex.unlock();
@@ -205,7 +216,7 @@ void OdometryThread::addData(const SensorData & data)
}
}
bool OdometryThread::getData(SensorData & data)
bool OdometryThread::getData(SensorEvent & event)
{
bool dataFilled = false;
_dataAdded.acquire();
@@ -219,12 +230,12 @@ bool OdometryThread::getData(SensorData & data)
_odometry->process(_imuBuffer.front());
double stamp =_imuBuffer.front().stamp();
_imuBuffer.pop_front();
if(stamp > _dataBuffer.front().stamp()) {
if(stamp > _dataBuffer.front().data().stamp()) {
break;
}
}
data = _dataBuffer.front();
event = _dataBuffer.front();
_dataBuffer.pop_front();
dataFilled = true;
}