Fixed #1439

corelib/src/Rtabmap.cpp

@@ -716,6 +716,36 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 
 	if(_memory)
 	{
+		bool isMemIncremental = _memory->isIncremental();
+		if(Parameters::parse(parameters, Parameters::kMemIncrementalMemory(), isMemIncremental) &&
+			isMemIncremental != _memory->isIncremental())
+		{
+			// Mode has changed from Mapping to Localization, cleanup the local graph
+			if(_memory->isGraphReduced() && _memory->isIncremental())
+			{
+				// Force reducing graph, then remove filtered nodes from the optimized poses
+				std::map<int, int> reducedIds;
+				_memory->incrementMapId(&reducedIds);
+				for(std::map<int, int>::iterator iter=reducedIds.begin(); iter!=reducedIds.end(); ++iter)
+				{
+					_optimizedPoses.erase(iter->first);
+				}
+			}
+
+			// In both cases, we save the latest optimized graph and latest localization pose
+			_memory->saveOptimizedPoses(_optimizedPoses, _lastLocalizationPose);
+
+			// Mode changed from Localization to Mapping, clear local graph
+			if(!_memory->isIncremental()) {
+				_optimizedPoses.clear();
+				_lastLocalizationPose.setNull();
+				_mapCorrection.setIdentity();
+				_mapCorrectionBackup.setNull();
+				_localizationCovariance = cv::Mat();
+				_lastLocalizationNodeId = 0;
+			}
+		}
+
 		_memory->parseParameters(parameters);
 		if(_memory->isIncremental() && !_globalScanMap.empty())
 		{
@@ -1720,6 +1750,7 @@ bool Rtabmap::process(
 					_constraints.erase(--_constraints.end());
 				}
 			}
+
 			_constraints.insert(std::make_pair(tmp.from(), tmp));
 		}
 		// Localization mode stuff