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

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

29
corelib/src/Rtabmap.cpp
View File

@@ -851,17 +851,28 @@ bool Rtabmap::process(const SensorData & data)
if(_memory->getLastWorkingSignature())
{
const Transform & lastPose = _memory->getLastWorkingSignature()->getPose(); // use raw odometry
Transform lastPoseToNewPose = lastPose.inverse() * data.pose();
float x,y,z, roll,pitch,yaw;
lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
if(_newMapOdomChangeDistance > 0.0 && (x*x + y*y + z*z) > _newMapOdomChangeDistance*_newMapOdomChangeDistance)
// look for identity
if(!lastPose.isIdentity() && data.pose().isIdentity())
{
int mapId = triggerNewMap();
UWARN("Odometry is reset (large odometry change detected > %f). A new map (%d) is created! Last pose = %s, new pose = %s",
_newMapOdomChangeDistance,
mapId,
lastPose.prettyPrint().c_str(),
data.pose().prettyPrint().c_str());
UWARN("Odometry is reset (identity pose detected). Increment map id to %d!", mapId);
}
else if(_newMapOdomChangeDistance > 0.0)
{
// look for large change
Transform lastPoseToNewPose = lastPose.inverse() * data.pose();
float x,y,z, roll,pitch,yaw;
lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
if((x*x + y*y + z*z) > _newMapOdomChangeDistance*_newMapOdomChangeDistance)
{
int mapId = triggerNewMap();
UWARN("Odometry is reset (large odometry change detected > %f). A new map (%d) is created! Last pose = %s, new pose = %s",
_newMapOdomChangeDistance,
mapId,
lastPose.prettyPrint().c_str(),
data.pose().prettyPrint().c_str());
}
}
}
}

27
corelib/src/RtabmapThread.cpp
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();
{