From 6029b6283dd79fb4cc56fd8559baa9c6480c9b98 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 8 Jun 2020 10:14:21 -0400 Subject: [PATCH] Rtabmap: Refactored how fake odom is generated in localization mode when no odom is provided. Trigger new map in localization mode also clears last localization pose if RGBD/SavedLocalizationIgnored is true. --- corelib/include/rtabmap/core/Statistics.h | 1 + corelib/src/Rtabmap.cpp | 87 ++++++++++++++--------- guilib/src/CloudViewer.cpp | 2 - tools/Export/main.cpp | 2 +- tools/Reprocess/main.cpp | 5 +- 5 files changed, 56 insertions(+), 41 deletions(-) diff --git a/corelib/include/rtabmap/core/Statistics.h b/corelib/include/rtabmap/core/Statistics.h index b29ae4e2..093058d9 100644 --- a/corelib/include/rtabmap/core/Statistics.h +++ b/corelib/include/rtabmap/core/Statistics.h @@ -61,6 +61,7 @@ class RTABMAP_EXP Statistics RTABMAP_STATS(Loop, Reactivate_id,); RTABMAP_STATS(Loop, Hypothesis_ratio,); RTABMAP_STATS(Loop, Hypothesis_reactivated,); + RTABMAP_STATS(Loop, Map_id,); RTABMAP_STATS(Loop, Visual_words,); RTABMAP_STATS(Loop, Visual_inliers,); RTABMAP_STATS(Loop, Visual_matches,); diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 7f007341..c2f54dc2 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -729,11 +729,17 @@ int Rtabmap::triggerNewMap() int mapId = -1; if(_memory) { + _lastLocalizationNodeId = 0; + _odomCachePoses.clear(); + _odomCacheConstraints.clear(); + if(!_memory->isIncremental()) { - UWARN("Memory is not incremental (%s=false), ignoring creating a new map as we " - "should be already processing new nodes in a new session.", - Parameters::kMemIncrementalMemory().c_str()); + if(_savedLocalizationIgnored) + { + _mapCorrection.setIdentity(); + _lastLocalizationPose.setIdentity(); + } return mapId; } std::map reducedIds; @@ -741,9 +747,6 @@ int Rtabmap::triggerNewMap() UINFO("New map triggered, new map = %d", mapId); _optimizedPoses.clear(); _constraints.clear(); - _lastLocalizationNodeId = 0; - _odomCachePoses.clear(); - _odomCacheConstraints.clear(); if(_bayesFilter) { @@ -1053,36 +1056,34 @@ bool Rtabmap::process( bool fakeOdom = false; if(_rgbdSlamMode) { - if(!_memory->isIncremental() && !odomPose.isNull()) + if(!_memory->isIncremental() && + !odomPose.isNull() && + _optimizedPoses.size() && + _mapCorrection.isIdentity() && + !_lastLocalizationPose.isNull() && + !_lastLocalizationPose.isIdentity() && + _lastLocalizationNodeId == 0) { - if(!_mapCorrectionBackup.isNull()) + // Localization mode + if(!_optimizeFromGraphEnd) { - _mapCorrection = _mapCorrectionBackup; - _mapCorrectionBackup.setNull(); + //set map->odom so that odom is moved back to last saved localization + _mapCorrection = _lastLocalizationPose * odomPose.inverse(); + std::map nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end()); + _lastLocalizationNodeId = graph::findNearestNode(nodesOnly, _lastLocalizationPose); + UWARN("Update map correction based on last localization saved in database! correction = %s, nearest id = %d of last pose = %s, odom = %s", + _mapCorrection.prettyPrint().c_str(), + _lastLocalizationNodeId, + _lastLocalizationPose.prettyPrint().c_str(), + odomPose.prettyPrint().c_str()); } - else if(_optimizedPoses.size() && _mapCorrection.isIdentity() && !_lastLocalizationPose.isNull() && _lastLocalizationNodeId == 0) + else { - // Localization mode - if(!_optimizeFromGraphEnd) + //move optimized poses accordingly to last saved localization + Transform mapCorrectionInv = odomPose * _lastLocalizationPose.inverse(); + for(std::map::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter) { - //set map->odom so that odom is moved back to last saved localization - _mapCorrection = _lastLocalizationPose * odomPose.inverse(); - std::map nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end()); - _lastLocalizationNodeId = graph::findNearestNode(nodesOnly, _lastLocalizationPose); - UWARN("Update map correction based on last localization saved in database! correction = %s, nearest id = %d of last pose = %s, odom = %s", - _mapCorrection.prettyPrint().c_str(), - _lastLocalizationNodeId, - _lastLocalizationPose.prettyPrint().c_str(), - odomPose.prettyPrint().c_str()); - } - else - { - //move optimized poses accordingly to last saved localization - Transform mapCorrectionInv = odomPose * _lastLocalizationPose.inverse(); - for(std::map::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter) - { - iter->second = mapCorrectionInv * iter->second; - } + iter->second = mapCorrectionInv * iter->second; } } } @@ -1097,12 +1098,20 @@ bool Rtabmap::process( } else // fake localization { + if(!_mapCorrectionBackup.isNull()) + { + _mapCorrection = _mapCorrectionBackup; + _mapCorrectionBackup.setNull(); + } if(_lastLocalizationPose.isNull()) { _lastLocalizationPose = Transform::getIdentity(); } fakeOdom = true; odomPose = _mapCorrection.inverse() * _lastLocalizationPose; + UDEBUG("Map correction = %s", _mapCorrection.prettyPrint().c_str()); + UDEBUG("Last localization pose: %s", _lastLocalizationPose.prettyPrint().c_str()); + UDEBUG("Fake odom: %s", odomPose.prettyPrint().c_str()); } } else if(_memory->isIncremental()) // only in mapping mode @@ -1351,6 +1360,7 @@ bool Rtabmap::process( _constraints.insert(std::make_pair(iter->first, iter->second.inverse())); } } + // only in mapping mode we add a neighbor link if(signature->getLinks().size() && signature->getLinks().begin()->second.type() == Link::kNeighbor) { @@ -1381,6 +1391,11 @@ bool Rtabmap::process( _lastLocalizationPose = newPose; // keep in cache the latest corrected pose if(!_memory->isIncremental()) { + if(!_odomCacheAddLink.empty()) + { + float odomDistance = (_odomCacheAddLink.rbegin()->second.inverse() * signature->getPose()).getNorm(); + _distanceTravelled += odomDistance; + } _odomCacheAddLink.insert(std::make_pair(signature->id(), signature->getPose())); while(!_odomCacheAddLink.empty() && (int)_odomCacheAddLink.size() > _maxOdomCacheSize+1) { @@ -1415,7 +1430,6 @@ bool Rtabmap::process( odomCovariance.inv()))); _odomCachePoses.insert(std::make_pair(signature->id(), signature->getPose())); // keep odometry poses } - } } @@ -2815,10 +2829,12 @@ bool Rtabmap::process( else { Transform newPose = _optimizedPoses.at(localizationLinks.begin()->first) * localizationLinks.begin()->second.transform().inverse(); + UDEBUG("newPose=%s", newPose.prettyPrint().c_str()); if(_graphOptimizer->isSlam2d()) { // in case of 3d landmarks, transform constraint to 2D newPose = newPose.to3DoF(); + UDEBUG("newPose 2D=%s", newPose.prettyPrint().c_str()); } else if(_graphOptimizer->gravitySigma() > 0) { @@ -2851,6 +2867,7 @@ bool Rtabmap::process( transform *= error; newPose = _optimizedPoses.at(loopId) * transform.inverse(); + UDEBUG("newPose gravity=%s", newPose.prettyPrint().c_str()); } else { @@ -3132,6 +3149,7 @@ bool Rtabmap::process( UINFO("Set loop closure transform = %s", loopIter->second.transform().prettyPrint().c_str()); statistics_.setLoopClosureTransform(loopIter->second.transform()); + statistics_.addStatistic(Statistics::kLoopMap_id(), sLoop->mapId()); statistics_.addStatistic(Statistics::kLoopVisual_words(), sLoop->getWords().size()); // if ground truth exists, compute localization error @@ -3153,8 +3171,8 @@ bool Rtabmap::process( statistics_.addStatistic(Statistics::kLoopMapToOdom_pitch(), pitch*180.0f/M_PI); statistics_.addStatistic(Statistics::kLoopMapToOdom_yaw(), yaw*180.0f/M_PI); - // Odom correction (actual odometry pose change) - if(!odomPose.isNull() && !previousMapCorrection.isNull()) + // Odom correction (actual odometry pose change), ignore correction from first localization + if(!odomPose.isNull() && !previousMapCorrection.isNull() && !previousMapCorrection.isIdentity()) { Transform odomCorrection = (previousMapCorrection*odomPose).inverse()*_mapCorrection*odomPose; statistics_.addStatistic(Statistics::kLoopOdom_correction_norm(), odomCorrection.getNorm()); @@ -3177,6 +3195,7 @@ bool Rtabmap::process( statistics_.addStatistic(Statistics::kLoopMapToBase_roll(), roll*180.0f/M_PI); statistics_.addStatistic(Statistics::kLoopMapToBase_pitch(), pitch*180.0f/M_PI); statistics_.addStatistic(Statistics::kLoopMapToBase_yaw(), yaw*180.0f/M_PI); + UINFO("Localization pose = %s", _lastLocalizationPose.prettyPrint().c_str()); } statistics_.setMapCorrection(_mapCorrection); diff --git a/guilib/src/CloudViewer.cpp b/guilib/src/CloudViewer.cpp index b3226517..d99bf57e 100644 --- a/guilib/src/CloudViewer.cpp +++ b/guilib/src/CloudViewer.cpp @@ -415,7 +415,6 @@ void CloudViewer::saveSettings(QSettings & settings, const QString & group) cons settings.setValue("camera_target_follow", this->isCameraTargetFollow()); settings.setValue("camera_free", this->isCameraFree()); settings.setValue("camera_lockZ", this->isCameraLockZ()); - settings.setValue("camera_ortho", this->isCameraOrtho()); settings.setValue("bg_color", this->getDefaultBackgroundColor()); settings.setValue("rendering_rate", this->getRenderingRate()); @@ -468,7 +467,6 @@ void CloudViewer::loadSettings(QSettings & settings, const QString & group) this->setCameraFree(); } this->setCameraLockZ(settings.value("camera_lockZ", this->isCameraLockZ()).toBool()); - this->setCameraOrtho(settings.value("camera_ortho", this->isCameraOrtho()).toBool()); this->setDefaultBackgroundColor(settings.value("bg_color", this->getDefaultBackgroundColor()).value()); diff --git a/tools/Export/main.cpp b/tools/Export/main.cpp index c8f19da2..e72ddcb5 100644 --- a/tools/Export/main.cpp +++ b/tools/Export/main.cpp @@ -169,7 +169,7 @@ int main(int argc, char * argv[]) #ifdef RTABMAP_ALICE_VISION multiband = true; #else - printf("\"--multiband\" option cannot be used vecause RTAB-Map is not built with AliceVision support. Ignoring multiband...\n"); + printf("\"--multiband\" option cannot be used because RTAB-Map is not built with AliceVision support. Ignoring multiband...\n"); #endif } else if(std::strcmp(argv[i], "--poisson_depth") == 0) diff --git a/tools/Reprocess/main.cpp b/tools/Reprocess/main.cpp index ac419ca2..38bedfe6 100644 --- a/tools/Reprocess/main.cpp +++ b/tools/Reprocess/main.cpp @@ -399,10 +399,7 @@ int main(int argc, char * argv[]) showLocalizationStats(); lastLocalizationOdomPose = info.odomPose; } - if(incrementalMemory) - { - rtabmap.triggerNewMap(); - } + rtabmap.triggerNewMap(); } UTimer t; if(!rtabmap.process(data, info.odomPose, info.odomCovariance, info.odomVelocity, globalMapStats))