i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +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/Rtabmap.cpp
+20 -9
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());
}
}
}
}
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();
{
guilib/src/MainWindow.cpp
+21 -5
View File
@@ -1821,15 +1821,31 @@ void MainWindow::processRtabmapEventInit(int status, const QString & info)
{
if(!_newDatabasePathOutput.isEmpty())
{
if(QFile::rename(_newDatabasePath, _newDatabasePathOutput))
bool removed = true;
if(QFile::exists(_newDatabasePathOutput))
{
std::string msg = uFormat("Database saved to \"%s\".", _newDatabasePathOutput.toStdString().c_str());
UINFO(msg.c_str());
QMessageBox::information(this, tr("Database saved!"), QString(msg.c_str()));
removed = QFile::remove(_newDatabasePathOutput);
}
if(removed)
{
if(QFile::rename(_newDatabasePath, _newDatabasePathOutput))
{
std::string msg = uFormat("Database saved to \"%s\".", _newDatabasePathOutput.toStdString().c_str());
UINFO(msg.c_str());
QMessageBox::information(this, tr("Database saved!"), QString(msg.c_str()));
}
else
{
std::string msg = uFormat("Failed to rename temporary database from \"%s\" to \"%s\".",
_newDatabasePath.toStdString().c_str(), _newDatabasePathOutput.toStdString().c_str());
UERROR(msg.c_str());
QMessageBox::critical(this, tr("Closing failed!"), QString(msg.c_str()));
}
}
else
{
std::string msg = uFormat("Failed to rename temporary database from \"%s\" to \"%s\".", _newDatabasePath.toStdString().c_str(), _newDatabasePathOutput.toStdString().c_str());
std::string msg = uFormat("Failed to overwrite the database \"%s\". The temporary database is still correctly saved at \"%s\".",
_newDatabasePathOutput.toStdString().c_str(), _newDatabasePath.toStdString().c_str());
UERROR(msg.c_str());
QMessageBox::critical(this, tr("Closing failed!"), QString(msg.c_str()));
}