i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-09 03:07:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed new map trigger from Identity odom when DetectionRate=0 and BufferSize=0. MainWindow: fixed overwritting database on close

Browse Source
This commit is contained in:
Mathieu Labbe
2015-05-21 09:16:13 -04:00
parent 9c8fb09df3
commit 8e26219e92
3 changed files with 58 additions and 24 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/RtabmapThread.cpp
+17 -10
View File
@@ -518,13 +518,29 @@ void RtabmapThread::addData(const SensorData & sensorData)
return;
}
if(!lastPose_.isIdentity() && sensorData.pose().isIdentity())
if(_rate>0.0f)
{
if(_frameRateTimer->getElapsedTime() < 1.0f/_rate)
{
if(!lastPose_.isIdentity() && sensorData.pose().isIdentity())
{
UWARN("Odometry is reset (identity pose detected). Increment map id!");
pushNewState(kStateTriggeringMap);
_rotVariance = 0;
_transVariance = 0;
}
return;
}
}
if(_dataBufferMaxSize > 0 && !lastPose_.isIdentity() && sensorData.pose().isIdentity())
{
UWARN("Odometry is reset (identity pose detected). Increment map id!");
pushNewState(kStateTriggeringMap);
_rotVariance = 0;
_transVariance = 0;
}
_frameRateTimer->start();
lastPose_ = sensorData.pose();
if(sensorData.poseRotVariance() > _rotVariance)
@@ -536,15 +552,6 @@ void RtabmapThread::addData(const SensorData & sensorData)
_transVariance = sensorData.poseTransVariance();
}
if(_rate>0.0f)
{
if(_frameRateTimer->getElapsedTime() < 1.0f/_rate)
{
return;
}
}
_frameRateTimer->start();
bool notify = true;
_dataMutex.lock();
{