Fixed Stereo parameters not used for OdometryBOW. Saving parameters in Memory and Feature2D to re-use old parameters when underlaying algorithm strategies are re-created.

corelib/include/rtabmap/core/Features2d.h

@@ -148,6 +148,7 @@ public:
 			const std::vector<cv::KeyPoint> & keypoints) const;
 
 	virtual void parseParameters(const ParametersMap & parameters);
+	virtual const ParametersMap & getParameters() const {return parameters_;}
 	virtual Feature2D::Type getType() const = 0;
 
 protected:
@@ -158,6 +159,7 @@ private:
 	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const = 0;
 
 private:
+	ParametersMap parameters_;
 	int maxFeatures_;
 	float _wordsMaxDepth; // 0=inf
 	float _wordsMinDepth;

corelib/include/rtabmap/core/Memory.h

@@ -67,6 +67,7 @@ public:
 	virtual ~Memory();
 
 	virtual void parseParameters(const ParametersMap & parameters);
+	virtual const ParametersMap & getParameters() const {return parameters_;}
 	bool update(const SensorData & data,
 			Statistics * stats = 0);
 	bool update(const SensorData & data,
@@ -222,6 +223,7 @@ protected:
 
 private:
 	// parameters
+	ParametersMap parameters_;
 	float _similarityThreshold;
 	bool _rawDataKept;
 	bool _binDataKept;

corelib/include/rtabmap/core/Rtabmap.h

@@ -112,6 +112,7 @@ public:
 	void dumpPrediction() const;
 	void dumpData() const;
 	void parseParameters(const ParametersMap & parameters);
+	const ParametersMap & getParameters() const {return _parameters;}
 	void setWorkingDirectory(std::string path);
 	void rejectLoopClosure(int oldId, int newId);
 	void get3DMap(std::map<int, Signature> & signatures,
@@ -215,7 +216,7 @@ private:
 	EpipolarGeometry * _epipolarGeometry;
 	BayesFilter * _bayesFilter;
 	Optimizer * _graphOptimizer;
-	ParametersMap _modifiedParameters;
+	ParametersMap _parameters;
 
 	Memory * _memory;
 

corelib/src/Features2d.cpp

@@ -354,6 +354,8 @@ Feature2D::~Feature2D()
 }
 void Feature2D::parseParameters(const ParametersMap & parameters)
 {
+	uInsert(parameters_, parameters);
+
 	Parameters::parse(parameters, Parameters::kKpMaxFeatures(), maxFeatures_);
 	Parameters::parse(parameters, Parameters::kKpMaxDepth(), _wordsMaxDepth);
 	Parameters::parse(parameters, Parameters::kKpMinDepth(), _wordsMinDepth);
@@ -399,7 +401,7 @@ void Feature2D::parseParameters(const ParametersMap & parameters)
 	if((iter=parameters.find(Parameters::kStereoOpticalFlow())) != parameters.end())
 	{
 		delete _stereo;
-		_stereo = Stereo::create(parameters);
+		_stereo = Stereo::create(parameters_);
 	}
 	else
 	{

corelib/src/Memory.cpp

@@ -377,6 +377,8 @@ Memory::~Memory()
 
 void Memory::parseParameters(const ParametersMap & parameters)
 {
+	uInsert(parameters_, parameters);
+
 	UDEBUG("");
 	ParametersMap::const_iterator iter;
 
@@ -438,7 +440,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 			_feature2D = 0;
 		}
 
-		_feature2D = Feature2D::create(detectorStrategy, parameters);
+		_feature2D = Feature2D::create(detectorStrategy, parameters_);
 	}
 	else if(_feature2D)
 	{

corelib/src/OdometryBOW.cpp

@@ -96,14 +96,8 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
 	for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
 	{
 		std::string group = uSplit(iter->first, '/').front();
-		if(group.compare("SURF") == 0 ||
+		if(Parameters::isFeatureParameter(iter->first) ||
-			group.compare("SIFT") == 0 ||
+		   group.compare("Stereo") == 0)
-			group.compare("BRIEF") == 0 ||
-			group.compare("FAST") == 0 ||
-			group.compare("ORB") == 0 ||
-			group.compare("FREAK") == 0 ||
-			group.compare("GFTT") == 0 ||
-			group.compare("BRISK") == 0)
 		{
 			customParameters.insert(*iter);
 		}

corelib/src/RegistrationVis.cpp

@@ -60,15 +60,6 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters) :
 		_flowMaxLevel(Parameters::defaultVisCorFlowMaxLevel())
 {
 	_featureParameters = Parameters::getDefaultParameters();
-	uInsert(_featureParameters, ParametersPair(Parameters::kMemIncrementalMemory(), "true")); // make sure it is incremental
-	uInsert(_featureParameters, ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
-	uInsert(_featureParameters, ParametersPair(Parameters::kMemBinDataKept(), "false"));
-	uInsert(_featureParameters, ParametersPair(Parameters::kMemSTMSize(), "0"));
-	uInsert(_featureParameters, ParametersPair(Parameters::kKpIncrementalDictionary(), "true")); // make sure it is incremental
-	uInsert(_featureParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
-	uInsert(_featureParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0"));
-	uInsert(_featureParameters, ParametersPair(Parameters::kMemGenerateIds(), "true"));
-
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), _featureParameters.at(Parameters::kVisCorNNType())));
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), _featureParameters.at(Parameters::kVisCorNNDR())));
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpDetectorStrategy(), _featureParameters.at(Parameters::kVisFeatureType())));

corelib/src/Rtabmap.cpp

@@ -362,11 +362,13 @@ void Rtabmap::close(bool databaseSaved)
 	}
 	_databasePath.clear();
 	parseParameters(Parameters::getDefaultParameters()); // reset to default parameters
-	_modifiedParameters.clear();
+	_parameters.clear();
 }
 
 void Rtabmap::parseParameters(const ParametersMap & parameters)
 {
+	uInsert(_parameters, parameters);
+
 	ULOGGER_DEBUG("");
 	ParametersMap::const_iterator iter;
 	if((iter=parameters.find(Parameters::kRtabmapWorkingDirectory())) != parameters.end())
@@ -481,7 +483,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 
 	for(ParametersMap::const_iterator iter = parameters.begin(); iter!=parameters.end(); ++iter)
 	{
-		uInsert(_modifiedParameters, ParametersPair(iter->first, iter->second));
+		uInsert(_parameters, ParametersPair(iter->first, iter->second));
 	}
 }
 
@@ -776,7 +778,7 @@ void Rtabmap::resetMemory()
 
 	if(_memory)
 	{
-		_memory->init(_databasePath, true, _modifiedParameters, true);
+		_memory->init(_databasePath, true, _parameters, true);
 		if(_memory->getLastWorkingSignature())
 		{
 			optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), false, _optimizedPoses, &_constraints);

corelib/src/util2d.cpp

@@ -225,7 +225,6 @@ std::vector<cv::Point2f> calcStereoCorrespondences(
 				{
 					if(level>0)
 					{
-						UDEBUG("");
 						tmpMaxDisparity = tmpMinDisparity+(bestScoreIndex+1)*(1<<level);
 						tmpMaxDisparity+=tmpMaxDisparity%level;
 						if(tmpMaxDisparity > maxDisparity)
@@ -247,7 +246,6 @@ std::vector<cv::Point2f> calcStereoCorrespondences(
 
 		if(bestScoreIndex>=0)
 		{
-			UDEBUG("");
 			//subpixel refining
 			int d = -(tmpMinDisparity+bestScoreIndex);
 

guilib/src/DatabaseViewer.cpp

@@ -3669,7 +3669,7 @@ void DatabaseViewer::refineConstraintVisually(int from, int to, bool silent, boo
 	RegistrationVis reg(parameters);
 	Signature fromS(dataFrom);
 	Signature toS(dataTo);
-	t = reg.computeTransformationMod(fromS, toS, Transform::getIdentity(), &rejectedMsg, &inliers, &variance);
+	t = reg.computeTransformationMod(fromS, toS, currentLink.transform(), &rejectedMsg, &inliers, &variance);
 	UDEBUG("");
 
 	if(!silent)

utilite/include/rtabmap/utilite/UStl.h

@@ -449,6 +449,23 @@ inline void uInsert(std::map<K, V> & map, const std::pair<K, V> & pair)
 	}
 }
 
+/**
+ * Insert items in the map. Contrary to the insert in the STL,
+ * if the key already exists, the value will be replaced by the new one.
+ */
+template<class K, class V>
+inline void uInsert(std::map<K, V> & map, const std::map<K, V> & items)
+{
+	for(typename std::map<K, V>::const_iterator iter=items.begin(); iter!=items.end(); ++iter)
+	{
+		std::pair<typename std::map<K, V>::iterator, bool> inserted = map.insert(*iter);
+		if(inserted.second == false)
+		{
+			inserted.first->second = iter->second;
+		}
+	}
+}
+
 /**
  * Convert a std::list to a std::vector.
  * @param list the list