Rtabmap: reset all parameters to default on close()

corelib/include/rtabmap/core/Rtabmap.h

@@ -197,7 +197,7 @@ private:
 	EpipolarGeometry * _epipolarGeometry;
 	BayesFilter * _bayesFilter;
 	graph::Optimizer * _graphOptimizer;
-	ParametersMap _lastParameters;
+	ParametersMap _modifiedParameters;
 
 	Memory * _memory;
 

corelib/src/Rtabmap.cpp

@@ -265,6 +265,8 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 	{
 		_databasePath = _wDir + "/" + Parameters::getDefaultDatabaseName();
 	}
+	UASSERT(UFile::getExtension(_databasePath).compare("db") == 0);
+
 	UINFO("Using database \"%s\".", _databasePath.c_str());
 	bool newDatabase = !UFile::exists(_databasePath);
 	this->parseParameters(parameters);
@@ -330,16 +332,12 @@ void Rtabmap::close()
 		_graphOptimizer = 0;
 	}
 	_databasePath.clear();
+	parseParameters(Parameters::getDefaultParameters()); // reset to default parameters
+	_modifiedParameters.clear();
 }
 
 void Rtabmap::parseParameters(const ParametersMap & parameters)
 {
-	if(_databasePath.empty())
-	{
-		UERROR("RTAB-Map is not initialized. Call rtabmap::init() instead.");
-		return;
-	}
-
 	ULOGGER_DEBUG("");
 	ParametersMap::const_iterator iter;
 	if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
@@ -421,10 +419,13 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	}
 
 	if(!_memory)
+	{
+		if(!_databasePath.empty())
 		{
 			_memory = new Memory(parameters);
 			_memory->init(_databasePath, false, parameters, true);
 		}
+	}
 	else
 	{
 		_memory->parseParameters(parameters);
@@ -462,7 +463,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 
 	for(ParametersMap::const_iterator iter = parameters.begin(); iter!=parameters.end(); ++iter)
 	{
-		uInsert(_lastParameters, ParametersPair(iter->first, iter->second));
+		uInsert(_modifiedParameters, ParametersPair(iter->first, iter->second));
 	}
 }
 
@@ -700,10 +701,11 @@ void Rtabmap::resetMemory()
 	_constraints.clear();
 	_mapCorrection.setIdentity();
 	_mapTransform.setIdentity();
+	this->clearPath();
 
 	if(_memory)
 	{
-		_memory->init(_databasePath, true, ParametersMap(), true);
+		_memory->init(_databasePath, true, _modifiedParameters, true);
 		if(_memory->getLastWorkingSignature())
 		{
 			optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, &_constraints);
@@ -1378,7 +1380,7 @@ bool Rtabmap::process(const SensorData & data)
 			std::string rejectedMsg;
 			if(_reextractLoopClosureFeatures)
 			{
-				ParametersMap customParameters = _lastParameters; // get BOW LCC parameters
+				ParametersMap customParameters = _modifiedParameters; // get BOW LCC parameters
 				// override some parameters
 				uInsert(customParameters, ParametersPair(Parameters::kMemIncrementalMemory(), "true")); // make sure it is incremental
 				uInsert(customParameters, ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
@@ -2052,6 +2054,9 @@ std::map<int, Transform> Rtabmap::getWMPosesInRadius(
 		float radius,
 		int maxDiffID, // 0 means ignore
 		int & nearestId) const
+{
+	std::map<int, Transform> poses;
+	if(_memory && fromId > 0)
 	{
 		UDEBUG("");
 		const Signature * fromS = _memory->getSignature(fromId);
@@ -2086,7 +2091,6 @@ std::map<int, Transform> Rtabmap::getWMPosesInRadius(
 		Transform fromT = _optimizedPoses.at(fromId);
 
 		nearestId = 0;
-	std::map<int, Transform> poses;
 		float minDistance = -1;
 		if(cloud->size())
 		{
@@ -2162,6 +2166,7 @@ std::map<int, Transform> Rtabmap::getWMPosesInRadius(
 			}
 		}
 		UDEBUG("nearestId = %d, minDistance=%f poses=%d", nearestId, minDistance, (int)poses.size());
+	}
 	return poses;
 }
 
@@ -2460,6 +2465,8 @@ bool Rtabmap::computePath(
 		int targetNode,
 		const std::map<int, Transform> & nodes,
 		const std::multimap<int, rtabmap::Link> & constraints)
+{
+	if(_memory)
 	{
 		if(!_memory->getLastWorkingSignature())
 		{
@@ -2523,6 +2530,8 @@ bool Rtabmap::computePath(
 
 		return _path.size()>0;
 	}
+	return false;
+}
 
 // return true if path is updated
 bool Rtabmap::computePath(int targetNode, bool global)
@@ -2670,7 +2679,7 @@ void Rtabmap::updateGoalIndex()
 		return;
 	}
 
-	if(_path.size())
+	if( _memory && _path.size())
 	{
 		// Make sure the next signatures on the path are linked together
 		float distanceSoFar = 0.0f;