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

@@ -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,);

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<int, int> 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<int, Transform> 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<int, Transform>::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<int, Transform> 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<int, Transform>::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);

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<QColor>());
 	

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)

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))