i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

RtabmapThread: using stamps in odometry messages to throttle detection rate (if stamps are not set, system time is used)

Browse Source
This commit is contained in:
matlabbe
2016-01-14 11:37:39 -05:00
parent dc2746a365
commit 6cc511cb80
3 changed files with 11 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/include/rtabmap/core/RtabmapThread.h
View File

@@ -106,6 +106,7 @@ private:
float _rate;
bool _createIntermediateNodes;
UTimer * _frameRateTimer;
double _previousStamp;
Rtabmap * _rtabmap;
bool _paused;

8
corelib/src/CameraRGB.cpp
View File

@@ -405,12 +405,14 @@ SensorData CameraImages::captureImage()
{
if(this->getImageRate() > 0.0f)
{
UWARN("CameraImages: Cannot read images as fast as their timestamps (delay=%f s). Disable "
"source image rate or disable synchronization of capture time with timestamps.", _captureDelay);
UWARN("CameraImages: Cannot read images as fast as their timestamps (target delay=%fs, capture time=%fs). Disable "
"source image rate or disable synchronization of capture time with timestamps.",
_captureDelay, _captureTimer.getElapsedTime());
}
else
{
UWARN("CameraImages: Cannot read images as fast as their timestamps (delay=%f s).", _captureDelay);
UWARN("CameraImages: Cannot read images as fast as their timestamps (target delay=%fs, capture time=%fs).",
_captureDelay, _captureTimer.getElapsedTime());
}
}

6
corelib/src/RtabmapThread.cpp
View File

@@ -48,6 +48,7 @@ RtabmapThread::RtabmapThread(Rtabmap * rtabmap) :
_rate(Parameters::defaultRtabmapDetectionRate()),
_createIntermediateNodes(Parameters::defaultRtabmapCreateIntermediateNodes()),
_frameRateTimer(new UTimer()),
_previousStamp(0.0),
_rtabmap(rtabmap),
_paused(false),
lastPose_(Transform::getIdentity()),
@@ -91,6 +92,7 @@ void RtabmapThread::clearBufferedData()
lastPose_.setIdentity();
_rotVariance = 0;
_transVariance = 0;
_previousStamp = 0;
}
_dataMutex.unlock();
@@ -508,7 +510,8 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
bool ignoreFrame = false;
if(_rate>0.0f)
{
if(_frameRateTimer->getElapsedTime() < 1.0f/_rate)
if((_previousStamp>0.0 && odomEvent.data().stamp()>_previousStamp && odomEvent.data().stamp() - _previousStamp < 1.0f/_rate) ||
((_previousStamp<=0.0 || odomEvent.data().stamp()<=_previousStamp) && _frameRateTimer->getElapsedTime() < 1.0f/_rate))
{
ignoreFrame = true;
}
@@ -528,6 +531,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
else if(!ignoreFrame)
{
_frameRateTimer->start();
_previousStamp = odomEvent.data().stamp();
}
lastPose_ = odomEvent.pose();