i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 10:00:23 +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()) if(_memory->getLastWorkingSignature())
{ {
const Transform & lastPose = _memory->getLastWorkingSignature()->getPose(); // use raw odometry const Transform & lastPose = _memory->getLastWorkingSignature()->getPose(); // use raw odometry
Transform lastPoseToNewPose = lastPose.inverse() * data.pose();
float x,y,z, roll,pitch,yaw; // look for identity
lastPoseToNewPose.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw); if(!lastPose.isIdentity() && data.pose().isIdentity())
if(_newMapOdomChangeDistance > 0.0 && (x*x + y*y + z*z) > _newMapOdomChangeDistance*_newMapOdomChangeDistance)
{ {
int mapId = triggerNewMap(); int mapId = triggerNewMap();
UWARN("Odometry is reset (large odometry change detected > %f). A new map (%d) is created! Last pose = %s, new pose = %s", UWARN("Odometry is reset (identity pose detected). Increment map id to %d!", mapId);
_newMapOdomChangeDistance, }
mapId, else if(_newMapOdomChangeDistance > 0.0)
lastPose.prettyPrint().c_str(), {
data.pose().prettyPrint().c_str()); // 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; 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!"); UWARN("Odometry is reset (identity pose detected). Increment map id!");
pushNewState(kStateTriggeringMap); pushNewState(kStateTriggeringMap);
_rotVariance = 0; _rotVariance = 0;
_transVariance = 0; _transVariance = 0;
} }
_frameRateTimer->start();
lastPose_ = sensorData.pose(); lastPose_ = sensorData.pose();
if(sensorData.poseRotVariance() > _rotVariance) if(sensorData.poseRotVariance() > _rotVariance)
@@ -536,15 +552,6 @@ void RtabmapThread::addData(const SensorData & sensorData)
_transVariance = sensorData.poseTransVariance(); _transVariance = sensorData.poseTransVariance();
} }
if(_rate>0.0f)
{
if(_frameRateTimer->getElapsedTime() < 1.0f/_rate)
{
return;
}
}
_frameRateTimer->start();
bool notify = true; bool notify = true;
_dataMutex.lock(); _dataMutex.lock();
{ {

26
guilib/src/MainWindow.cpp
View File

@@ -1821,15 +1821,31 @@ void MainWindow::processRtabmapEventInit(int status, const QString & info)
{ {
if(!_newDatabasePathOutput.isEmpty()) 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()); removed = QFile::remove(_newDatabasePathOutput);
UINFO(msg.c_str()); }
QMessageBox::information(this, tr("Database saved!"), QString(msg.c_str())); 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 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()); UERROR(msg.c_str());
QMessageBox::critical(this, tr("Closing failed!"), QString(msg.c_str())); QMessageBox::critical(this, tr("Closing failed!"), QString(msg.c_str()));
} }