Parameters renamed: "OdomLocalMap" group is now "OdomF2M" for Frame to Map odometry. Visual registration feature matching: using guess transform to limit the radius of correspondences "Vis/CorGuessWinSize=16"

2026-09-13 15:00:21 +08:00 · 2016-02-22 16:13:14 -05:00
parent 0d68f74d80
commit ccc4b4a6c2
22 changed files with 844 additions and 638 deletions
@@ -25,39 +25,37 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

-#ifndef ODOMETRYLOCALMAP_H_
-#define ODOMETRYLOCALMAP_H_
+#ifndef ODOMETRYF2M_H_
+#define ODOMETRYF2M_H_

 #include <rtabmap/core/Odometry.h>

 namespace rtabmap {

-class Memory;
+class Signature;
 class RegistrationVis;

-class RTABMAP_EXP OdometryLocalMap : public Odometry
+class RTABMAP_EXP OdometryF2M : public Odometry
 {
 public:
-	OdometryLocalMap(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
-	virtual ~OdometryLocalMap();
+	OdometryF2M(const rtabmap::ParametersMap & parameters = rtabmap::ParametersMap());
+	virtual ~OdometryF2M();

 	virtual void reset(const Transform & initialPose = Transform::getIdentity());
-	const std::multimap<int, cv::Point3f> & getLocalMap() const {return localMap_;}
-	const Memory * getMemory() const {return memory_;}
+	const std::multimap<int, cv::Point3f> & getLocalMap() const;

 private:
 	virtual Transform computeTransform(const SensorData & image, OdometryInfo * info = 0);

 private:
 	//Parameters
-	int localHistoryMaxSize_;
-	std::string fixedLocalMapPath_;
+	int maximumMapSize_;
+	std::string fixedMapPath_;

-	Memory * memory_;
 	RegistrationVis * regVis_;
-	std::multimap<int, cv::Point3f> localMap_;
+	Signature * map_;
 };

 }

-#endif /* ODOMETRYLOCALMAP_H_ */
+#endif /* ODOMETRYF2M_H_ */
@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #define ODOMETRYINFO_H_

 #include <map>
+#include "rtabmap/core/Transform.h"

 namespace rtabmap {

@@ -348,10 +348,11 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Odom, ParticleLambdaR, 		float, 100,       "Lambda of rotational components (roll,pitch,yaw).");
 	RTABMAP_PARAM(Odom, KalmanProcessNoise, 	float, 0.001,     "Process noise covariance value.");
 	RTABMAP_PARAM(Odom, KalmanMeasurementNoise, float, 0.01,      "Process measurement covariance value.");
+	RTABMAP_PARAM(Odom, GuessMotion,            bool, true,       "Guess next transformation from the last motion computed.");

 	// Odometry Bag-of-words
-	RTABMAP_PARAM(OdomLocalMap, HistorySize,       int, 1000,   "Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
-	RTABMAP_PARAM_STR(OdomLocalMap, FixedMapPath,   "",         "Path to a fixed map (RTAB-Map's database) to be used for odometry. Odometry will be constraint to this map. RGB-only images can be used if odometry PnP estimation is used.")
+	RTABMAP_PARAM(OdomF2M, MaxSize,             int, 1000,   "Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
+	RTABMAP_PARAM_STR(OdomF2M, FixedMapPath,    "",         "Path to a fixed map (RTAB-Map's database) to be used for odometry. Odometry will be constraint to this map. RGB-only images can be used if odometry PnP estimation is used.")

 	// Odometry Mono
 	RTABMAP_PARAM(OdomMono, InitMinFlow,            float, 100,  "Minimum optical flow required for the initialization step.");
@@ -360,8 +361,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(OdomMono, MaxVariance,            float, 0.01, "Maximum variance to add new points to local map.");

 	// Odometry Optical Flow
-	RTABMAP_PARAM(OdomF2F, KeyFrameThr,           int, 0,        "Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.");
-	RTABMAP_PARAM(OdomF2F, GuessMotion,           bool, true,    "Guess next transformation from the last motion computed.");
+	RTABMAP_PARAM(OdomF2F, KeyFrameThr,           int, 100,      "Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.");

 	// Common registration parameters
 	RTABMAP_PARAM(Reg, VarianceFromInliersCount, bool, false,   "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
@@ -373,7 +373,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Vis, ForwardEstOnly,           bool, true, 	"Forward estimation only (A->B). If false, a transformation is also computed in backward direction (B->A), then the two resulting transforms are merged (middle interpolation between the transforms).");
 	RTABMAP_PARAM(Vis, InlierDistance,           float, 0.1,    "[Vis/EstimationType = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.");
 	RTABMAP_PARAM(Vis, RefineIterations,         int, 10,       "[Vis/EstimationType = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
-	RTABMAP_PARAM(Vis, PnPReprojError, 	         float, 2.0,   "[Vis/EstimationType = 1] PnP reprojection error.");
+	RTABMAP_PARAM(Vis, PnPReprojError, 	         float, 2.0,    "[Vis/EstimationType = 1] PnP reprojection error.");
 	RTABMAP_PARAM(Vis, PnPFlags,                 int, 1,        "[Vis/EstimationType = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
 	RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 1,        "[Vis/EstimationType = 1] Refine iterations.");
 	RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.02,   "[Vis/EstimationType = 2] Epipolar geometry maximum variance to accept the transformation.");
@@ -386,15 +386,16 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM_STR(Vis, RoiRatios,        "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
    RTABMAP_PARAM(Vis, SubPixWinSize,            int, 3,        "See cv::cornerSubPix().");
 	RTABMAP_PARAM(Vis, SubPixIterations,         int, 0,        "See cv::cornerSubPix(). 0 disables sub pixel refining.");
-	RTABMAP_PARAM(Vis, SubPixEps,                float, 0.02,  "See cv::cornerSubPix().");
+	RTABMAP_PARAM(Vis, SubPixEps,                float, 0.02,   "See cv::cornerSubPix().");
 	RTABMAP_PARAM(Vis, CorType,                  int, 0,        "Correspondences computation approach: 0=Features Matching, 1=Optical Flow");
 	RTABMAP_PARAM(Vis, CorNNType, 	             int, 3,        "[Vis/CorrespondenceType=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach.");
 	RTABMAP_PARAM(Vis, CorNNDR,                  float, 0.8,    "[Vis/CorrespondenceType=0] NNDR: nearest neighbor distance ratio. Used for features matching approach.");
+	RTABMAP_PARAM(Vis, CorGuessWinSize,          int, 16,       "[Vis/CorrespondenceType=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences.");
 	RTABMAP_PARAM(Vis, CorFlowWinSize,           int, 16,       "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
 	RTABMAP_PARAM(Vis, CorFlowIterations,        int, 30,       "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
-	RTABMAP_PARAM(Vis, CorFlowEps,               float, 0.01,  "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
+	RTABMAP_PARAM(Vis, CorFlowEps,               float, 0.01,   "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
 	RTABMAP_PARAM(Vis, CorFlowMaxLevel,          int, 3,        "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
-	RTABMAP_PARAM(Vis, UseDepthAsMask,           bool, true,   "Use depth image as mask for features detection.");
+	RTABMAP_PARAM(Vis, UseDepthAsMask,           bool, true,    "Use depth image as mask for features detection.");

 	// ICP registration parameters
 	RTABMAP_PARAM(Icp, MaxTranslation,            float, 0.2,   "Maximum ICP translation correction accepted (m).");
@@ -35,6 +35,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 namespace rtabmap {

+class Feature2D;
+
 // Visual registration
 class RTABMAP_EXP RegistrationVis : public Registration
 {
@@ -49,6 +51,8 @@ public:
 	int getIterations() const {return _iterations;}
 	int getMinInliers() const {return _minInliers;}

+	Feature2D * createFeatureDetector() const; // for convenience
+
 protected:
 	virtual Transform computeTransformationImpl(
 			Signature & from,
@@ -75,6 +79,8 @@ private:
 	int _flowIterations;
 	float _flowEps;
 	int _flowMaxLevel;
+	float _nndr;
+	int _guessWinSize;
 	bool _useDepthAsMask;

 	ParametersMap _featureParameters;
@@ -69,7 +69,8 @@ public:
 			int signatureId);
 	virtual void addWord(VisualWord * vw);

-	virtual std::vector<int> findNN(const std::list<VisualWord *> & vws) const;
+	std::vector<int> findNN(const std::list<VisualWord *> & vws) const;
+	std::vector<int> findNN(const cv::Mat & descriptors) const;

 	void addWordRef(int wordId, int signatureId);
 	void removeAllWordRef(int wordId, int signatureId);
@@ -57,7 +57,7 @@ SET(SRC_FILES
 	
 	Odometry.cpp
 	OdometryThread.cpp
-	OdometryLocalMap.cpp
+	OdometryF2M.cpp
 	OdometryMono.cpp
 	OdometryF2F.cpp
 	
@@ -2056,7 +2056,6 @@ Transform Memory::computeTransform(

 	if(fromS && toS)
 	{
-		UWARN("%d=%d %d=%d", fromId, fromS->sensorData().cameraModels().size(), toId, toS->sensorData().cameraModels().size());
 		// make sure we have all data needed
 		if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired()) ||
 		   (_registrationPipeline->isScanRequired()) ||
@@ -25,9 +25,9 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

+#include <rtabmap/core/OdometryF2M.h>
 #include "rtabmap/core/Odometry.h"
 #include "rtabmap/core/OdometryF2F.h"
-#include "rtabmap/core/OdometryLocalMap.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
@@ -54,7 +54,7 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
 		odometry = new OdometryF2F(parameters);
 		break;
 	default:
-		odometry = new OdometryLocalMap(parameters);
+		odometry = new OdometryF2M(parameters);
 		type = Odometry::kTypeLocalMap;
 		break;
 	}
@@ -37,12 +37,12 @@ namespace rtabmap {
 OdometryF2F::OdometryF2F(const ParametersMap & parameters) :
 	Odometry(parameters),
 	keyFrameThr_(Parameters::defaultOdomF2FKeyFrameThr()),
-	guessFromMotion_(Parameters::defaultOdomF2FGuessMotion()),
+	guessFromMotion_(Parameters::defaultOdomGuessMotion()),
 	motionSinceLastKeyFrame_(Transform::getIdentity())
 {
 	registrationPipeline_ = Registration::create(parameters);
 	Parameters::parse(parameters, Parameters::kOdomF2FKeyFrameThr(), keyFrameThr_);
-	Parameters::parse(parameters, Parameters::kOdomF2FGuessMotion(), guessFromMotion_);
+	Parameters::parse(parameters, Parameters::kOdomGuessMotion(), guessFromMotion_);
 }

 OdometryF2F::~OdometryF2F()
@@ -130,25 +130,28 @@ Transform OdometryF2F::computeTransform(
 		if(keyFrameThr_ <= 0 || (int)regInfo.inliers <= keyFrameThr_)
 		{
 			UDEBUG("Update key frame");
-			Signature newRefFrame(data);
-
-			int features = 0;
-			if(registrationPipeline_->getMinVisualCorrespondences()>0)
+			int features = newFrame.sensorData().keypoints().size();
+			if(features == 0)
 			{
+				newFrame = Signature(data);
 				// this will generate features only for the first frame
 				Signature dummy;
 				registrationPipeline_->computeTransformationMod(
-						newRefFrame,
+						newFrame,
 						dummy);
-				features = (int)newRefFrame.getWords().size();
+				features = (int)newFrame.sensorData().keypoints().size();
 			}

 			if((features >= registrationPipeline_->getMinVisualCorrespondences()) &&
 			   (registrationPipeline_->getMinGeometryCorrespondencesRatio()==0.0f ||
-					   (newRefFrame.sensorData().laserScanRaw().cols &&
-					   (newRefFrame.sensorData().laserScanMaxPts() == 0 || float(newRefFrame.sensorData().laserScanRaw().cols)/float(newRefFrame.sensorData().laserScanMaxPts())>=registrationPipeline_->getMinGeometryCorrespondencesRatio()))))
+					   (newFrame.sensorData().laserScanRaw().cols &&
+					   (newFrame.sensorData().laserScanMaxPts() == 0 || float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanMaxPts())>=registrationPipeline_->getMinGeometryCorrespondencesRatio()))))
 			{
-				refFrame_ = newRefFrame;
+				refFrame_ = newFrame;
+
+				refFrame_.setWords(std::multimap<int, cv::KeyPoint>());
+				refFrame_.setWords3(std::multimap<int, cv::Point3f>());
+				refFrame_.setWordsDescriptors(std::multimap<int, cv::Mat>());

 				//reset motion
 				motionSinceLastKeyFrame_.setIdentity();
@@ -160,13 +163,13 @@ Transform OdometryF2F::computeTransform(
 					UWARN("Too low 2D features (%d), keeping last key frame...", features);
 				}

-				if(registrationPipeline_->getMinGeometryCorrespondencesRatio()>0.0f && newRefFrame.sensorData().laserScanRaw().cols==0)
+				if(registrationPipeline_->getMinGeometryCorrespondencesRatio()>0.0f && newFrame.sensorData().laserScanRaw().cols==0)
 				{
-					UWARN("Too low scan points (%d), keeping last key frame...", newRefFrame.sensorData().laserScanRaw().cols);
+					UWARN("Too low scan points (%d), keeping last key frame...", newFrame.sensorData().laserScanRaw().cols);
 				}
-				else if(registrationPipeline_->getMinGeometryCorrespondencesRatio()>0.0f && newRefFrame.sensorData().laserScanMaxPts() != 0 && float(newRefFrame.sensorData().laserScanRaw().cols)/float(newRefFrame.sensorData().laserScanMaxPts())<registrationPipeline_->getMinGeometryCorrespondencesRatio())
+				else if(registrationPipeline_->getMinGeometryCorrespondencesRatio()>0.0f && newFrame.sensorData().laserScanMaxPts() != 0 && float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanMaxPts())<registrationPipeline_->getMinGeometryCorrespondencesRatio())
 				{
-					UWARN("Too low scan points ratio (%d < %d), keeping last key frame...", float(newRefFrame.sensorData().laserScanRaw().cols)/float(newRefFrame.sensorData().laserScanMaxPts()), registrationPipeline_->getMinGeometryCorrespondencesRatio());
+					UWARN("Too low scan points ratio (%d < %d), keeping last key frame...", float(newFrame.sensorData().laserScanRaw().cols)/float(newFrame.sensorData().laserScanMaxPts()), registrationPipeline_->getMinGeometryCorrespondencesRatio());
 				}
 			}
 		}
@@ -189,7 +192,7 @@ Transform OdometryF2F::computeTransform(
 			timer.elapsed(),
 			output.isNull()?"true":"false",
 			(int)regInfo.inliers,
-			(int)refFrame_.getWords().size(),
+			(int)refFrame_.sensorData().keypoints().size(),
 			!output.isNull()?"true":"false");

 	return output;
@@ -0,0 +1,328 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/core/OdometryInfo.h"
+#include "rtabmap/core/Memory.h"
+#include "rtabmap/core/VisualWord.h"
+#include "rtabmap/core/Signature.h"
+#include "rtabmap/core/RegistrationVis.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/util3d_registration.h"
+#include "rtabmap/core/util3d_correspondences.h"
+#include "rtabmap/core/util3d_motion_estimation.h"
+#include "rtabmap/core/Optimizer.h"
+#include "rtabmap/core/VWDictionary.h"
+#include "rtabmap/core/util3d.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/utilite/UTimer.h"
+#include "rtabmap/utilite/UConversion.h"
+#include <opencv2/calib3d/calib3d.hpp>
+#include <rtabmap/core/OdometryF2M.h>
+
+#if _MSC_VER
+	#define ISFINITE(value) _finite(value)
+#else
+	#define ISFINITE(value) std::isfinite(value)
+#endif
+
+namespace rtabmap {
+
+OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
+	Odometry(parameters),
+	maximumMapSize_(Parameters::defaultOdomF2MMaxSize()),
+	fixedMapPath_(Parameters::defaultOdomF2MFixedMapPath()),
+	regVis_(new RegistrationVis(parameters)),
+	map_(new Signature(-1))
+{
+	UDEBUG("");
+	Parameters::parse(parameters, Parameters::kOdomF2MMaxSize(), maximumMapSize_);
+	Parameters::parse(parameters, Parameters::kOdomF2MFixedMapPath(), fixedMapPath_);
+
+	if(!fixedMapPath_.empty())
+	{
+		UINFO("Init odometry from a fixed database: \"%s\"", fixedMapPath_.c_str());
+		// init the local map with a all 3D features contained in the database
+		ParametersMap customParameters;
+		customParameters.insert(ParametersPair(Parameters::kMemIncrementalMemory(), "false"));
+		customParameters.insert(ParametersPair(Parameters::kMemInitWMWithAllNodes(), "true"));
+		customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
+		Memory memory(customParameters);
+		if(!memory.init(fixedMapPath_, false, ParametersMap()))
+		{
+			UERROR("Error initializing the memory for BOW Odometry.");
+		}
+		else
+		{
+			// get the graph
+			std::map<int, int> ids = memory.getNeighborsId(memory.getLastSignatureId(), 0, -1);
+			std::map<int, Transform> poses;
+			std::multimap<int, Link> links;
+			memory.getMetricConstraints(uKeysSet(ids), poses, links, true);
+
+			if(poses.size())
+			{
+				//optimize the graph
+				Optimizer * optimizer = Optimizer::create(parameters);
+				std::map<int, Transform> optimizedPoses = optimizer->optimize(poses.begin()->first, poses, links);
+				delete optimizer;
+
+				std::multimap<int, cv::Point3f> words3D;
+				std::multimap<int, cv::Mat> wordsDescriptors;
+
+				// fill the local map
+				for(std::map<int, Transform>::iterator posesIter=optimizedPoses.begin();
+					posesIter!=optimizedPoses.end();
+					++posesIter)
+				{
+					const Signature * s = memory.getSignature(posesIter->first);
+					if(s)
+					{
+						// Transform 3D points accordingly to pose and add them to local map
+						for(std::multimap<int, cv::Point3f>::const_iterator pointsIter=s->getWords3().begin();
+							pointsIter!=s->getWords3().end();
+							++pointsIter)
+						{
+							if(!uContains(words3D, pointsIter->first))
+							{
+								words3D.insert(std::make_pair(pointsIter->first, util3d::transformPoint(pointsIter->second, posesIter->second)));
+
+								if(s->getWordsDescriptors().size() == s->getWords3().size())
+								{
+									UASSERT(uContains(s->getWordsDescriptors(), pointsIter->first));
+									wordsDescriptors.insert(std::make_pair(pointsIter->first, s->getWordsDescriptors().find(pointsIter->first)->second));
+								}
+								else // load descriptor from dictionary
+								{
+									UASSERT(memory.getVWDictionary()->getWord(pointsIter->first) != 0);
+									wordsDescriptors.insert(std::make_pair(pointsIter->first, memory.getVWDictionary()->getWord(pointsIter->first)->getDescriptor()));
+								}
+							}
+						}
+					}
+				}
+				UASSERT(words3D.size() == wordsDescriptors.size());
+				map_->setWords3(words3D);
+				map_->setWordsDescriptors(wordsDescriptors);
+			}
+			else
+			{
+				UERROR("No pose loaded from database \"%s\"", fixedMapPath_.c_str());
+			}
+		}
+		if((int)map_->getWords3().size() < regVis_->getMinInliers() || map_->getWords3().size() == 0)
+		{
+			UERROR("The loaded fixed map from \"%s\" is too small! Only %d unique features loaded. Odometry won't be computed!",
+					fixedMapPath_.c_str(), (int)map_->getWords3().size());
+		}
+	}
+}
+
+OdometryF2M::~OdometryF2M()
+{
+	delete map_;
+	UDEBUG("");
+}
+
+
+void OdometryF2M::reset(const Transform & initialPose)
+{
+	if(fixedMapPath_.empty())
+	{
+		Odometry::reset(initialPose);
+		map_->sensorData() = SensorData();
+	}
+	else
+	{
+		UWARN("Odometry cannot be reset when a fixed local map is set.");
+	}
+}
+
+const std::multimap<int, cv::Point3f> & OdometryF2M::getLocalMap() const
+{
+	return map_->getWords3();
+}
+
+// return not null transform if odometry is correctly computed
+Transform OdometryF2M::computeTransform(
+		const SensorData & data,
+		OdometryInfo * info)
+{
+	UTimer timer;
+	Transform output;
+
+	if(info)
+	{
+		info->type = 0;
+	}
+
+	RegistrationInfo regInfo;
+	int nFeatures = 0;
+
+	// Generate keypoints from the new data
+	if(data.isValid())
+	{
+		Signature newSignature(data);
+		if(map_->getWords3().size() && newSignature.sensorData().isValid())
+		{
+			Transform guess = this->previousTransform().isIdentity()||this->previousTransform().isNull()?Transform():this->getPose()*this->previousTransform();
+			Transform transform = regVis_->computeTransformationMod(*map_, newSignature, guess, &regInfo);
+
+			if(!transform.isNull())
+			{
+				// make it incremental
+				transform = this->getPose().inverse() * transform;
+			}
+			else if(!regInfo.rejectedMsg.empty())
+			{
+				UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
+			}
+			else
+			{
+				UWARN("Unknown registration error");
+			}
+
+			if(fixedMapPath_.empty())
+			{
+				output = transform;
+
+				int added = 0;
+				int removed = 0;
+
+				// update local map
+				std::multimap<int, cv::Point3f> mapPoints = map_->getWords3();
+				std::multimap<int, cv::Mat> mapDescriptors = map_->getWordsDescriptors();
+				Transform t = this->getPose()*output;
+				UASSERT(mapPoints.size() == mapDescriptors.size());
+				UASSERT(newSignature.getWordsDescriptors().size() == newSignature.getWords3().size());
+				std::list<int> newIds = uUniqueKeys(newSignature.getWordsDescriptors());
+				for(std::list<int>::iterator iter=newIds.begin(); iter!=newIds.end(); ++iter)
+				{
+					if(mapPoints.find(*iter) == mapPoints.end())
+					{
+						mapPoints.insert(std::make_pair(*iter, util3d::transformPoint(newSignature.getWords3().find(*iter)->second, t)));
+						mapDescriptors.insert(std::make_pair(*iter, newSignature.getWordsDescriptors().find(*iter)->second));
+						++added;
+					}
+				}
+
+				// remove words in map if max size is reached
+				if(mapPoints.size() > maximumMapSize_)
+				{
+					// remove oldest first, keep matched features
+					std::set<int> matches(regInfo.matchesIDs.begin(), regInfo.matchesIDs.end());
+					std::multimap<int, cv::Mat>::iterator iterMapWords = mapDescriptors.begin();
+					for(std::multimap<int, cv::Point3f>::iterator iter = mapPoints.begin();
+						iter!=mapPoints.end() && (int)mapPoints.size() > maximumMapSize_ && mapPoints.size() >= newIds.size();)
+					{
+						if(matches.find(iter->first) == matches.end())
+						{
+							iter = mapPoints.erase(iter);
+							iterMapWords = mapDescriptors.erase(iterMapWords);
+							++removed;
+						}
+						else
+						{
+							++iter;
+							++iterMapWords;
+						}
+					}
+				}
+
+				map_->setWords3(mapPoints);
+				map_->setWordsDescriptors(mapDescriptors);
+
+				UINFO("Updated map: %d added %d removed (new map size=%d)", added, removed, (int)mapPoints.size());
+			}
+			else
+			{
+				// fixed local map, don't update with the new signature
+				output = transform;
+			}
+		}
+		else
+		{
+			// just generate keypoints for the new signature
+			Signature dummy;
+			regVis_->computeTransformationMod(
+					newSignature,
+					dummy);
+
+			if(fixedMapPath_.empty() && (int)newSignature.getWords3().size() >= regVis_->getMinInliers())
+			{
+				output.setIdentity();
+				// a very high variance tells that the new pose is not linked with the previous one
+				regInfo.variance = 9999;
+
+				Transform t = this->getPose(); // initial pose may be not identity...
+				std::multimap<int, cv::Point3f> transformedPoints;
+				for(std::multimap<int, cv::Point3f>::const_iterator iter = newSignature.getWords3().begin(); iter!=newSignature.getWords3().end(); ++iter)
+				{
+					transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, t)));
+				}
+
+				map_->setWords3(transformedPoints);
+				map_->setWordsDescriptors(newSignature.getWordsDescriptors());
+				map_->sensorData().setCameraModels(newSignature.sensorData().cameraModels());
+				map_->sensorData().setStereoCameraModel(newSignature.sensorData().stereoCameraModel());
+			}
+		}
+
+		map_->sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat()); // clear sensorData features
+
+		if(this->isInfoDataFilled() && info)
+		{
+			info->words = newSignature.getWords();
+		}
+	}
+
+	if(info)
+	{
+		info->variance = regInfo.variance;
+		info->inliers = regInfo.inliers;
+		info->matches = regInfo.matches;
+		info->features = nFeatures;
+		info->localMapSize = (int)map_->getWords3().size();
+
+		if(this->isInfoDataFilled())
+		{
+			info->wordMatches = regInfo.matchesIDs;
+			info->wordInliers = regInfo.inliersIDs;
+		}
+	}
+
+	UINFO("Odom update time = %fs lost=%s features=%d inliers=%d/%d variance=%f local_map=%d",
+			timer.elapsed(),
+			output.isNull()?"true":"false",
+			nFeatures,
+			regInfo.inliers,
+			regInfo.matches,
+			regInfo.variance,
+			(int)map_->getWords3().size());
+	return output;
+}
+
+} // namespace rtabmap
@@ -1,395 +0,0 @@
-/*
-Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
-All rights reserved.
-
-Redistribution and use in source and binary forms, with or without
-modification, are permitted provided that the following conditions are met:
-    * Redistributions of source code must retain the above copyright
-      notice, this list of conditions and the following disclaimer.
-    * Redistributions in binary form must reproduce the above copyright
-      notice, this list of conditions and the following disclaimer in the
-      documentation and/or other materials provided with the distribution.
-    * Neither the name of the Universite de Sherbrooke nor the
-      names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
-
-THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
-DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
-(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
-ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
-(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
-SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-*/
-
-#include "rtabmap/core/OdometryLocalMap.h"
-#include "rtabmap/core/OdometryInfo.h"
-#include "rtabmap/core/Memory.h"
-#include "rtabmap/core/Signature.h"
-#include "rtabmap/core/RegistrationVis.h"
-#include "rtabmap/core/util3d_transforms.h"
-#include "rtabmap/core/util3d_registration.h"
-#include "rtabmap/core/util3d_correspondences.h"
-#include "rtabmap/core/util3d_motion_estimation.h"
-#include "rtabmap/core/Optimizer.h"
-#include "rtabmap/core/VWDictionary.h"
-#include "rtabmap/core/util3d.h"
-#include "rtabmap/utilite/ULogger.h"
-#include "rtabmap/utilite/UTimer.h"
-#include "rtabmap/utilite/UConversion.h"
-#include <opencv2/calib3d/calib3d.hpp>
-
-#if _MSC_VER
-	#define ISFINITE(value) _finite(value)
-#else
-	#define ISFINITE(value) std::isfinite(value)
-#endif
-
-namespace rtabmap {
-
-OdometryLocalMap::OdometryLocalMap(const ParametersMap & parameters) :
-	Odometry(parameters),
-	localHistoryMaxSize_(Parameters::defaultOdomLocalMapHistorySize()),
-	fixedLocalMapPath_(Parameters::defaultOdomLocalMapFixedMapPath()),
-	memory_(0),
-	regVis_(new RegistrationVis(parameters))
-{
-	UDEBUG("");
-	Parameters::parse(parameters, Parameters::kOdomLocalMapHistorySize(), localHistoryMaxSize_);
-	Parameters::parse(parameters, Parameters::kOdomLocalMapFixedMapPath(), fixedLocalMapPath_);
-
-	ParametersMap customParameters;
-	float minDepth = Parameters::defaultVisMinDepth();
-	float maxDepth = Parameters::defaultVisMaxDepth();
-	std::string roi = Parameters::defaultVisRoiRatios();
-	bool useDepthAsMask = Parameters::defaultVisUseDepthAsMask();
-	Parameters::parse(parameters, Parameters::kVisMinDepth(), minDepth);
-	Parameters::parse(parameters, Parameters::kVisMaxDepth(), maxDepth);
-	Parameters::parse(parameters, Parameters::kVisRoiRatios(), roi);
-	Parameters::parse(parameters, Parameters::kVisUseDepthAsMask(), useDepthAsMask);
-	customParameters.insert(ParametersPair(Parameters::kKpMinDepth(), uNumber2Str(minDepth)));
-	customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(maxDepth)));
-	customParameters.insert(ParametersPair(Parameters::kKpRoiRatios(), roi));
-	customParameters.insert(ParametersPair(Parameters::kMemUseDepthAsMask(), uBool2Str(useDepthAsMask)));
-	customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
-	customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
-	customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
-	customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
-	customParameters.insert(ParametersPair(Parameters::kMemSaveDepth16Format(), "false"));
-	int nn = Parameters::defaultVisCorNNType();
-	float nndr = Parameters::defaultVisCorNNDR();
-	int featureType = Parameters::defaultVisFeatureType();
-	int maxFeatures = Parameters::defaultVisMaxFeatures();
-	Parameters::parse(parameters, Parameters::kVisCorNNType(), nn);
-	Parameters::parse(parameters, Parameters::kVisCorNNDR(), nndr);
-	Parameters::parse(parameters, Parameters::kVisFeatureType(), featureType);
-	Parameters::parse(parameters, Parameters::kVisMaxFeatures(), maxFeatures);
-	customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(nn)));
-	customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
-	customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(featureType)));
-	customParameters.insert(ParametersPair(Parameters::kKpMaxFeatures(), uNumber2Str(maxFeatures)));
-
-	// Memory's stereo parameters, copy from Odometry
-	int subPixWinSize = Parameters::defaultVisSubPixWinSize();
-	int subPixIterations = Parameters::defaultVisSubPixIterations();
-	double subPixEps = Parameters::defaultVisSubPixEps();
-	Parameters::parse(parameters, Parameters::kVisSubPixWinSize(), subPixWinSize);
-	Parameters::parse(parameters, Parameters::kVisSubPixIterations(), subPixIterations);
-	Parameters::parse(parameters, Parameters::kVisSubPixEps(), subPixEps);
-	customParameters.insert(ParametersPair(Parameters::kKpSubPixWinSize(), uNumber2Str(subPixWinSize)));
-	customParameters.insert(ParametersPair(Parameters::kKpSubPixIterations(), uNumber2Str(subPixIterations)));
-	customParameters.insert(ParametersPair(Parameters::kKpSubPixEps(), uNumber2Str(subPixEps)));
-
-	// add only feature stuff
-	for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
-	{
-		std::string group = uSplit(iter->first, '/').front();
-		if(Parameters::isFeatureParameter(iter->first) ||
-		   group.compare("Stereo") == 0)
-		{
-			customParameters.insert(*iter);
-		}
-	}
-
-	if(fixedLocalMapPath_.empty())
-	{
-		memory_ = new Memory(customParameters);
-		if(!memory_->init("", false, ParametersMap()))
-		{
-			UERROR("Error initializing the memory for BOW Odometry.");
-		}
-	}
-	else
-	{
-		UINFO("Init odometry from a fixed database: \"%s\"", fixedLocalMapPath_.c_str());
-		// init the local map with a all 3D features contained in the database
-		customParameters.insert(ParametersPair(Parameters::kMemIncrementalMemory(), "false"));
-		customParameters.insert(ParametersPair(Parameters::kMemInitWMWithAllNodes(), "true"));
-		memory_ = new Memory(customParameters);
-		if(!memory_->init(fixedLocalMapPath_, false, ParametersMap()))
-		{
-			UERROR("Error initializing the memory for BOW Odometry.");
-		}
-		else
-		{
-			// get the graph
-			std::map<int, int> ids = memory_->getNeighborsId(memory_->getLastSignatureId(), 0, -1);
-			std::map<int, Transform> poses;
-			std::multimap<int, Link> links;
-			memory_->getMetricConstraints(uKeysSet(ids), poses, links, true);
-
-			if(poses.size())
-			{
-				//optimize the graph
-				Optimizer * optimizer = Optimizer::create(parameters);
-				std::map<int, Transform> optimizedPoses = optimizer->optimize(poses.begin()->first, poses, links);
-				delete optimizer;
-
-				// fill the local map
-				for(std::map<int, Transform>::iterator posesIter=optimizedPoses.begin();
-					posesIter!=optimizedPoses.end();
-					++posesIter)
-				{
-					const Signature * s = memory_->getSignature(posesIter->first);
-					if(s)
-					{
-						// Transform 3D points accordingly to pose and add them to local map
-						const std::multimap<int, cv::Point3f> & words3D = s->getWords3();
-						for(std::multimap<int, cv::Point3f>::const_iterator pointsIter=words3D.begin();
-							pointsIter!=words3D.end();
-							++pointsIter)
-						{
-							if(!uContains(localMap_, pointsIter->first))
-							{
-								localMap_.insert(std::make_pair(pointsIter->first, util3d::transformPoint(pointsIter->second, posesIter->second)));
-							}
-						}
-					}
-				}
-			}
-			else
-			{
-				UERROR("No pose loaded from database \"%s\"", fixedLocalMapPath_.c_str());
-			}
-		}
-		if((int)localMap_.size() < regVis_->getMinInliers() || localMap_.size() == 0)
-		{
-			UERROR("The loaded fixed map from \"%s\" is too small! Only %d unique features loaded. Odometry won't be computed!",
-					fixedLocalMapPath_.c_str(), (int)localMap_.size());
-		}
-	}
-}
-
-OdometryLocalMap::~OdometryLocalMap()
-{
-	delete memory_;
-	UDEBUG("");
-}
-
-
-void OdometryLocalMap::reset(const Transform & initialPose)
-{
-	if(fixedLocalMapPath_.empty())
-	{
-		Odometry::reset(initialPose);
-		memory_->init("", false, ParametersMap());
-		localMap_.clear();
-	}
-	else
-	{
-		UWARN("Odometry cannot be reset when a fixed local map is set.");
-	}
-}
-
-// return not null transform if odometry is correctly computed
-Transform OdometryLocalMap::computeTransform(
-		const SensorData & data,
-		OdometryInfo * info)
-{
-	UTimer timer;
-	Transform output;
-
-	if(info)
-	{
-		info->type = 0;
-	}
-
-	RegistrationInfo regInfo;
-	int nFeatures = 0;
-
-	if(memory_->update(data))
-	{
-		const Signature * newSignature = memory_->getLastWorkingSignature();
-		if(newSignature)
-		{
-			nFeatures = (int)newSignature->getWords().size();
-			if(this->isInfoDataFilled() && info)
-			{
-				info->words = newSignature->getWords();
-			}
-		}
-
-		if(localMap_.size() && newSignature)
-		{
-			Transform transform;
-			if((int)localMap_.size() >= regVis_->getMinInliers() &&
-			   (int)newSignature->getWords().size()>=regVis_->getMinInliers())
-			{
-				Transform t;
-				Signature tmpLocalMap(-1);
-				tmpLocalMap.setWords3(localMap_);
-				t = regVis_->computeTransformation(tmpLocalMap, *newSignature, this->getPose(), &regInfo);
-
-				if(!t.isNull())
-				{
-					// make it incremental
-					transform = this->getPose().inverse() * t;
-				}
-				else if(!regInfo.rejectedMsg.empty())
-				{
-					UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
-				}
-				else
-				{
-					UWARN("Unknown registration error");
-				}
-			}
-			else if((int)newSignature->getWords().size()<regVis_->getMinInliers())
-			{
-				UWARN("New signature has too low extracted features (%d < %d)", (int)newSignature->getWords().size(), regVis_->getMinInliers());
-			}
-			else
-			{
-				UWARN("Local map too small!? (%d < %d)", (int)localMap_.size(), regVis_->getMinInliers());
-			}
-
-			if(transform.isNull())
-			{
-				memory_->deleteLocation(newSignature->id());
-			}
-			else if(fixedLocalMapPath_.empty())
-			{
-				output = transform;
-
-				// remove words if history max size is reached
-				while(localMap_.size() && (int)localMap_.size() > localHistoryMaxSize_ && memory_->getStMem().size()>1)
-				{
-					int nodeId = *memory_->getStMem().begin();
-					std::list<int> removedPts;
-					memory_->deleteLocation(nodeId, &removedPts);
-					for(std::list<int>::iterator iter = removedPts.begin(); iter!=removedPts.end(); ++iter)
-					{
-						localMap_.erase(*iter);
-					}
-				}
-
-				if(localHistoryMaxSize_ == 0 && localMap_.size() > 0 && localMap_.size() > newSignature->getWords3().size())
-				{
-					UERROR("Local map should have only words of the last added signature here! (size=%d, max history size=%d, newWords=%d)",
-							(int)localMap_.size(), localHistoryMaxSize_, (int)newSignature->getWords3().size());
-				}
-
-				// update local map
-				std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
-				Transform t = this->getPose()*output;
-				for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
-				{
-					// Only add unique words not in local map
-					if(newSignature->getWords3().count(*iter) == 1)
-					{
-						// keep old word
-						if(localMap_.find(*iter) == localMap_.end())
-						{
-							const cv::Point3f & pt = newSignature->getWords3().find(*iter)->second;
-							if(util3d::isFinite(pt))
-							{
-								cv::Point3f pt2 = util3d::transformPoint(pt, t);
-								localMap_.insert(std::make_pair(*iter, pt2));
-							}
-						}
-					}
-					else
-					{
-						localMap_.erase(*iter);
-					}
-				}
-			}
-			else
-			{
-				// fixed local map, just delete the new signature
-				output = transform;
-				memory_->deleteLocation(newSignature->id());
-			}
-		}
-		else if(newSignature)
-		{
-			int count = 0;
-			std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
-			if(fixedLocalMapPath_.empty() && (int)uniques.size() >= regVis_->getMinInliers())
-			{
-				output.setIdentity();
-				// a very high variance tells that the new pose is not linked with the previous one
-				regInfo.variance = 9999;
-
-				Transform t = this->getPose(); // initial pose maybe not identity...
-				for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
-				{
-					// Only add unique words
-					if(newSignature->getWords3().count(*iter) == 1)
-					{
-						const cv::Point3f & pt = newSignature->getWords3().find(*iter)->second;
-						if(util3d::isFinite(pt))
-						{
-							cv::Point3f pt2 = util3d::transformPoint(pt, t);
-							localMap_.insert(std::make_pair(*iter, pt2));
-						}
-						else
-						{
-							++count;
-						}
-					}
-				}
-			}
-			else
-			{
-				// not enough features, just delete it
-				memory_->deleteLocation(newSignature->id());
-			}
-			UDEBUG("uniques=%d, pt not finite = %d", (int)uniques.size(),count);
-		}
-
-		memory_->emptyTrash();
-	}
-
-	if(info)
-	{
-		info->variance = regInfo.variance;
-		info->inliers = regInfo.inliers;
-		info->matches = regInfo.matches;
-		info->features = nFeatures;
-		info->localMapSize = (int)localMap_.size();
-
-		if(this->isInfoDataFilled())
-		{
-			info->wordMatches = regInfo.matchesIDs;
-			info->wordInliers = regInfo.inliersIDs;
-		}
-	}
-
-	UINFO("Odom update time = %fs lost=%s features=%d inliers=%d/%d variance=%f local_map=%d dict=%d nodes=%d",
-			timer.elapsed(),
-			output.isNull()?"true":"false",
-			nFeatures,
-			regInfo.inliers,
-			regInfo.matches,
-			regInfo.variance,
-			(int)localMap_.size(),
-			(int)memory_->getVWDictionary()->getVisualWords().size(),
-			(int)memory_->getStMem().size());
-	return output;
-}
-
-} // namespace rtabmap
@@ -59,7 +59,7 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
 	pnpReprojError_(Parameters::defaultVisPnPReprojError()),
 	pnpFlags_(Parameters::defaultVisPnPFlags()),
 	pnpRefineIterations_(Parameters::defaultVisPnPRefineIterations()),
-	localHistoryMaxSize_(Parameters::defaultOdomLocalMapHistorySize()),
+	localHistoryMaxSize_(Parameters::defaultOdomF2MMaxSize()),
 	initMinFlow_(Parameters::defaultOdomMonoInitMinFlow()),
 	initMinTranslation_(Parameters::defaultOdomMonoInitMinTranslation()),
 	minTranslation_(Parameters::defaultOdomMonoMinTranslation()),
@@ -77,7 +77,7 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kVisPnPReprojError(), pnpReprojError_);
 	Parameters::parse(parameters, Parameters::kVisPnPFlags(), pnpFlags_);
 	Parameters::parse(parameters, Parameters::kVisPnPRefineIterations(), pnpRefineIterations_);
-	Parameters::parse(parameters, Parameters::kOdomLocalMapHistorySize(), localHistoryMaxSize_);
+	Parameters::parse(parameters, Parameters::kOdomF2MMaxSize(), localHistoryMaxSize_);

 	Parameters::parse(parameters, Parameters::kOdomMonoInitMinFlow(), initMinFlow_);
 	Parameters::parse(parameters, Parameters::kOdomMonoInitMinTranslation(), initMinTranslation_);
@@ -129,15 +129,7 @@ OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
 	// add only feature stuff
 	for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
 	{
-		std::string group = uSplit(iter->first, '/').front();
-		if(group.compare("SURF") == 0 ||
-			group.compare("SIFT") == 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)
+		if(Parameters::isFeatureParameter(iter->first))
 		{
 			customParameters.insert(*iter);
 		}
@@ -25,10 +25,10 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

+#include <rtabmap/core/OdometryF2M.h>
 #include "rtabmap/core/OdometryThread.h"
 #include "rtabmap/core/Odometry.h"
 #include "rtabmap/core/OdometryMono.h"
-#include "rtabmap/core/OdometryLocalMap.h"
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/core/CameraEvent.h"
 #include "rtabmap/core/OdometryEvent.h"
@@ -103,7 +103,7 @@ void OdometryThread::mainLoop()

 void OdometryThread::addData(const SensorData & data)
 {
-	if(dynamic_cast<OdometryMono*>(_odometry) == 0 && dynamic_cast<OdometryLocalMap*>(_odometry) == 0)
+	if(dynamic_cast<OdometryMono*>(_odometry) == 0 && dynamic_cast<OdometryF2M*>(_odometry) == 0)
 	{
 		if(data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection()))
 		{
@@ -140,11 +140,16 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters

+		// 0.11.2
+		removedParameters_.insert(std::make_pair("OdomLocalMap/HistorySize",      std::make_pair(true, Parameters::kOdomF2MMaxSize())));
+		removedParameters_.insert(std::make_pair("OdomLocalMap/FixedMapPath",     std::make_pair(true, Parameters::kOdomF2MFixedMapPath())));
+		removedParameters_.insert(std::make_pair("OdomF2F/GuessMotion",           std::make_pair(true, Parameters::kOdomGuessMotion())));
+
 		// 0.11.0
-		removedParameters_.insert(std::make_pair("OdomBow/LocalHistorySize",      std::make_pair(true, Parameters::kOdomLocalMapHistorySize())));
-		removedParameters_.insert(std::make_pair("OdomBow/FixedLocalMapPath",     std::make_pair(true, Parameters::kOdomLocalMapFixedMapPath())));
+		removedParameters_.insert(std::make_pair("OdomBow/LocalHistorySize",      std::make_pair(true, Parameters::kOdomF2MMaxSize())));
+		removedParameters_.insert(std::make_pair("OdomBow/FixedLocalMapPath",     std::make_pair(true, Parameters::kOdomF2MFixedMapPath())));
 		removedParameters_.insert(std::make_pair("OdomFlow/KeyFrameThr",          std::make_pair(true, Parameters::kOdomF2FKeyFrameThr())));
-		removedParameters_.insert(std::make_pair("OdomFlow/GuessMotion",          std::make_pair(true, Parameters::kOdomF2FGuessMotion())));
+		removedParameters_.insert(std::make_pair("OdomFlow/GuessMotion",          std::make_pair(true, Parameters::kOdomGuessMotion())));

 		removedParameters_.insert(std::make_pair("Kp/WordsPerImage",              std::make_pair(true, Parameters::kKpMaxFeatures())));

@@ -248,7 +253,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 		removedParameters_.insert(std::make_pair("RGBD/LocalLoopDetectionMaxDiffID", std::make_pair(false, "")));
 		removedParameters_.insert(std::make_pair("Odom/Type",                        std::make_pair(true, Parameters::kVisFeatureType())));
 		removedParameters_.insert(std::make_pair("Odom/MaxWords",                    std::make_pair(true, Parameters::kVisMaxFeatures())));
-		removedParameters_.insert(std::make_pair("Odom/LocalHistory",                std::make_pair(true, Parameters::kOdomLocalMapHistorySize())));
+		removedParameters_.insert(std::make_pair("Odom/LocalHistory",                std::make_pair(true, Parameters::kOdomF2MMaxSize())));
 		removedParameters_.insert(std::make_pair("Odom/NearestNeighbor",             std::make_pair(true, Parameters::kVisCorNNType())));
 		removedParameters_.insert(std::make_pair("Odom/NNDR",                        std::make_pair(true, Parameters::kVisCorNNDR())));
 	}
@@ -40,6 +40,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/utilite/UMath.h>

+#include <rtflann/flann.hpp>
+
 namespace rtabmap {

 RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration * child) :
@@ -59,6 +61,8 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 		_flowIterations(Parameters::defaultVisCorFlowIterations()),
 		_flowEps(Parameters::defaultVisCorFlowEps()),
 		_flowMaxLevel(Parameters::defaultVisCorFlowMaxLevel()),
+		_nndr(Parameters::defaultVisCorNNDR()),
+		_guessWinSize(Parameters::defaultVisCorGuessWinSize()),
 		_useDepthAsMask(Parameters::defaultVisUseDepthAsMask())
 {
 	_featureParameters = Parameters::getDefaultParameters();
@@ -95,6 +99,8 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), _flowIterations);
 	Parameters::parse(parameters, Parameters::kVisCorFlowEps(), _flowEps);
 	Parameters::parse(parameters, Parameters::kVisCorFlowMaxLevel(), _flowMaxLevel);
+	Parameters::parse(parameters, Parameters::kVisCorNNDR(), _nndr);
+	Parameters::parse(parameters, Parameters::kVisCorGuessWinSize(), _guessWinSize);
 	Parameters::parse(parameters, Parameters::kVisUseDepthAsMask(), _useDepthAsMask);

 	UASSERT_MSG(_minInliers >= 1, uFormat("value=%d", _minInliers).c_str());
@@ -157,10 +163,15 @@ RegistrationVis::~RegistrationVis()
 {
 }

+Feature2D * RegistrationVis::createFeatureDetector() const
+{
+	return Feature2D::create(_featureParameters);
+}
+
 Transform RegistrationVis::computeTransformationImpl(
 			Signature & fromSignature,
 			Signature & toSignature,
-			Transform guess, // guess is only used by Optical Flow correspondences (flowMaxLevel is set to 0 when guess is used)
+			Transform guess, // (flowMaxLevel is set to 0 when guess is used)
 			RegistrationInfo & info) const
 {
 	UDEBUG("%s=%d", Parameters::kVisMinInliers().c_str(), _minInliers);
@@ -212,7 +223,8 @@ Transform RegistrationVis::computeTransformationImpl(
 	{
 		UDEBUG("");
 		// just some checks to make sure that input data are ok
-		UASSERT((fromSignature.getWords().empty() && fromSignature.getWords3().empty())||
+		UASSERT(fromSignature.getWords().empty() ||
+				fromSignature.getWords3().empty() ||
 				(fromSignature.getWords().size() == fromSignature.getWords3().size()));
 		UASSERT((int)fromSignature.sensorData().keypoints().size() == fromSignature.sensorData().descriptors().rows ||
 				fromSignature.getWords().size() == fromSignature.getWordsDescriptors().size() ||
@@ -224,38 +236,43 @@ Transform RegistrationVis::computeTransformationImpl(
 				toSignature.getWords().size() == toSignature.getWordsDescriptors().size() ||
 				toSignature.sensorData().descriptors().rows == 0 ||
 				toSignature.getWordsDescriptors().size() == 0);
-		UASSERT(fromSignature.sensorData().imageRaw().type() == CV_8UC1 ||
+		UASSERT(fromSignature.sensorData().imageRaw().empty() ||
+				fromSignature.sensorData().imageRaw().type() == CV_8UC1 ||
 				fromSignature.sensorData().imageRaw().type() == CV_8UC3);
-		UASSERT(toSignature.sensorData().imageRaw().type() == CV_8UC1 ||
+		UASSERT(toSignature.sensorData().imageRaw().empty() ||
+				toSignature.sensorData().imageRaw().type() == CV_8UC1 ||
 				toSignature.sensorData().imageRaw().type() == CV_8UC3);

-		Feature2D * detector = Feature2D::create(_featureParameters);
+		Feature2D * detector = createFeatureDetector();
 		std::vector<cv::KeyPoint> kptsFrom;
 		if(fromSignature.getWords().empty())
 		{
 			if(fromSignature.sensorData().keypoints().empty())
 			{
-				if(fromSignature.sensorData().imageRaw().channels() > 1)
+				if(!fromSignature.sensorData().imageRaw().empty())
 				{
-					cv::Mat tmp;
-					cv::cvtColor(fromSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
-					fromSignature.sensorData().setImageRaw(tmp);
-				}
-
-				cv::Mat depthMask;
-				if(_useDepthAsMask && !fromSignature.sensorData().depthRaw().empty())
-				{
-					if(fromSignature.sensorData().imageRaw().rows % fromSignature.sensorData().depthRaw().rows == 0 &&
-						fromSignature.sensorData().imageRaw().cols % fromSignature.sensorData().depthRaw().cols == 0 &&
-						fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows == fromSignature.sensorData().imageRaw().cols/fromSignature.sensorData().depthRaw().cols)
+					if(fromSignature.sensorData().imageRaw().channels() > 1)
 					{
-						depthMask = util2d::interpolate(fromSignature.sensorData().depthRaw(), fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows, 0.1f);
+						cv::Mat tmp;
+						cv::cvtColor(fromSignature.sensorData().imageRaw(), tmp, cv::COLOR_BGR2GRAY);
+						fromSignature.sensorData().setImageRaw(tmp);
 					}
-				}

-				kptsFrom = detector->generateKeypoints(
-						fromSignature.sensorData().imageRaw(),
-						depthMask);
+					cv::Mat depthMask;
+					if(_useDepthAsMask && !fromSignature.sensorData().depthRaw().empty())
+					{
+						if(fromSignature.sensorData().imageRaw().rows % fromSignature.sensorData().depthRaw().rows == 0 &&
+							fromSignature.sensorData().imageRaw().cols % fromSignature.sensorData().depthRaw().cols == 0 &&
+							fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows == fromSignature.sensorData().imageRaw().cols/fromSignature.sensorData().depthRaw().cols)
+						{
+							depthMask = util2d::interpolate(fromSignature.sensorData().depthRaw(), fromSignature.sensorData().imageRaw().rows/fromSignature.sensorData().depthRaw().rows, 0.1f);
+						}
+					}
+
+					kptsFrom = detector->generateKeypoints(
+							fromSignature.sensorData().imageRaw(),
+							depthMask);
+				}
 			}
 			else
 			{
@@ -271,6 +288,8 @@ Transform RegistrationVis::computeTransformationImpl(
 		std::multimap<int, cv::KeyPoint> wordsTo;
 		std::multimap<int, cv::Point3f> words3From;
 		std::multimap<int, cv::Point3f> words3To;
+		std::multimap<int, cv::Mat> wordsDescFrom;
+		std::multimap<int, cv::Mat> wordsDescTo;
 		if(_correspondencesApproach == 1) //Optical Flow
 		{
 			UDEBUG("");
@@ -325,7 +344,6 @@ Transform RegistrationVis::computeTransformationImpl(
 				std::vector<unsigned char> status;
 				std::vector<float> err;
 				UDEBUG("cv::calcOpticalFlowPyrLK() begin");
-				int winSize = _flowWinSize;
 				cv::calcOpticalFlowPyrLK(
 						fromSignature.sensorData().imageRaw(),
 						toSignature.sensorData().imageRaw(),
@@ -333,7 +351,7 @@ Transform RegistrationVis::computeTransformationImpl(
 						cornersTo,
 						status,
 						err,
-						cv::Size(winSize, winSize),
+						cv::Size(_flowWinSize, _flowWinSize),
 						guessSet?0:_flowMaxLevel,
 						cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, _flowIterations, _flowEps),
 						cv::OPTFLOW_LK_GET_MIN_EIGENVALS | (guessSet?cv::OPTFLOW_USE_INITIAL_FLOW:0), 1e-4);
@@ -440,30 +458,28 @@ Transform RegistrationVis::computeTransformationImpl(
 			UDEBUG("kptsFrom=%d", (int)kptsFrom.size());
 			UDEBUG("kptsTo=%d", (int)kptsTo.size());
 			cv::Mat descriptorsFrom;
-			if(kptsFrom.size())
+			if((kptsFrom.empty() && fromSignature.getWordsDescriptors().size()) ||
+				fromSignature.getWordsDescriptors().size() == (int)kptsFrom.size())
 			{
-				if(fromSignature.getWordsDescriptors().size() == (int)kptsFrom.size())
+				descriptorsFrom = cv::Mat(fromSignature.getWordsDescriptors().size(),
+						fromSignature.getWordsDescriptors().begin()->second.cols,
+						fromSignature.getWordsDescriptors().begin()->second.type());
+				int i=0;
+				for(std::multimap<int, cv::Mat>::const_iterator iter=fromSignature.getWordsDescriptors().begin();
+					iter!=fromSignature.getWordsDescriptors().end();
+					++iter, ++i)
 				{
-					descriptorsFrom = cv::Mat(fromSignature.getWordsDescriptors().size(),
-							fromSignature.getWordsDescriptors().begin()->second.cols,
-							fromSignature.getWordsDescriptors().begin()->second.type());
-					int i=0;
-					for(std::multimap<int, cv::Mat>::const_iterator iter=fromSignature.getWordsDescriptors().begin();
-						iter!=fromSignature.getWordsDescriptors().end();
-						++iter, ++i)
-					{
-						iter->second.copyTo(descriptorsFrom.row(i));
-					}
-				}
-				else if(fromSignature.sensorData().descriptors().rows == (int)kptsFrom.size())
-				{
-					descriptorsFrom = fromSignature.sensorData().descriptors();
-				}
-				else if(!fromSignature.sensorData().imageRaw().empty())
-				{
-					descriptorsFrom = detector->generateDescriptors(fromSignature.sensorData().imageRaw(), kptsFrom);
+					iter->second.copyTo(descriptorsFrom.row(i));
 				}
 			}
+			else if(fromSignature.sensorData().descriptors().rows == (int)kptsFrom.size())
+			{
+				descriptorsFrom = fromSignature.sensorData().descriptors();
+			}
+			else if(!fromSignature.sensorData().imageRaw().empty())
+			{
+				descriptorsFrom = detector->generateDescriptors(fromSignature.sensorData().imageRaw(), kptsFrom);
+			}

 			cv::Mat descriptorsTo;
 			if(kptsTo.size())
@@ -513,57 +529,239 @@ Transform RegistrationVis::computeTransformationImpl(
 				kptsTo3D = uValues(toSignature.getWords3());
 			}

-			// We have all data we need here, so match using the vocabulary
-			UDEBUG("descriptorsFrom=%d", descriptorsFrom.rows);
-			VWDictionary dictionary(_featureParameters);
-			std::list<int> fromWordIds = dictionary.addNewWords(descriptorsFrom, 1);
-			std::list<int> toWordIds;
-			UDEBUG("descriptorsTo=%d", descriptorsTo.rows);
-			if(descriptorsTo.rows)
-			{
-				dictionary.update();
-				toWordIds = dictionary.addNewWords(descriptorsTo, 2);
-			}
-			dictionary.clear(false);
-
-			std::multiset<int> fromWordIdsSet(fromWordIds.begin(), fromWordIds.end());
-			std::multiset<int> toWordIdsSet(toWordIds.begin(), toWordIds.end());
-
-			UASSERT(kptsFrom3D.size() == kptsFrom.size());
-			UASSERT(fromWordIds.size() == kptsFrom.size());
-			int i=0;
-			for(std::list<int>::iterator iter=fromWordIds.begin(); iter!=fromWordIds.end(); ++iter)
-			{
-				if(fromWordIdsSet.count(*iter) == 1)
-				{
-					wordsFrom.insert(std::make_pair(*iter, kptsFrom[i]));
-					words3From.insert(std::make_pair(*iter, kptsFrom3D[i]));
-				}
-				++i;
-			}
-			UASSERT(kptsTo3D.size() == 0 || kptsTo3D.size() == kptsTo.size());
-			UASSERT(toWordIds.size() == kptsTo.size());
-			i=0;
-			for(std::list<int>::iterator iter=toWordIds.begin(); iter!=toWordIds.end(); ++iter)
-			{
-				if(toWordIdsSet.count(*iter) == 1)
-				{
-					wordsTo.insert(std::make_pair(*iter, kptsTo[i]));
-					if(kptsTo3D.size())
-					{
-						words3To.insert(std::make_pair(*iter, kptsTo3D[i]));
-					}
-				}
-				++i;
-			}
-			//remove doubles
 			fromSignature.sensorData().setFeatures(kptsFrom, descriptorsFrom);
 			toSignature.sensorData().setFeatures(kptsTo, descriptorsTo);
+
+			UDEBUG("descriptorsFrom=%d", descriptorsFrom.rows);
+			UDEBUG("descriptorsTo=%d", descriptorsTo.rows);
+
+			// We have all data we need here, so match!
+			if(descriptorsFrom.rows > 0 && descriptorsTo.rows > 0)
+			{
+				// If guess is set, limit the search of matches using optical flow window size
+				bool guessSet = !guess.isIdentity() && !guess.isNull();
+				if(guessSet)
+				{
+					UDEBUG("");
+					UASSERT(kptsTo.size() == descriptorsTo.rows);
+
+					// Use guess to project 3D "from" keypoints into "to" image
+					std::vector<cv::Point2f> cornersProjected;
+					if(kptsFrom3D.size() && (guessSet || kptsFrom.size()==0))
+					{
+						if(fromSignature.sensorData().cameraModels().size() > 1)
+						{
+							UFATAL("Radius feature matching is not supported for multiple cameras.");
+						}
+						Transform localTransform = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].localTransform():fromSignature.sensorData().stereoCameraModel().left().localTransform();
+						Transform guessCameraRef = (guess * localTransform).inverse();
+						cv::Mat R = (cv::Mat_<double>(3,3) <<
+								(double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
+								(double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(),
+								(double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33());
+						cv::Mat rvec(1,3, CV_64FC1);
+						cv::Rodrigues(R, rvec);
+						cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
+						cv::Mat K = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].K():fromSignature.sensorData().stereoCameraModel().left().K();
+						cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), cornersProjected);
+					}
+					else if(kptsFrom.size())
+					{
+						cv::KeyPoint::convert(kptsFrom, cornersProjected);
+					}
+					UDEBUG("");
+
+					// For each projected feature guess of "from" in "to", find its matching feature in
+					// the radius around the projected guess.
+					// TODO: do cross-check?
+					if(cornersProjected.size())
+					{
+						// Create kd-tree for keypoints "to"
+						std::vector<cv::Point2f> pointsTo;
+						cv::KeyPoint::convert(kptsTo, pointsTo);
+						rtflann::Matrix<float> pointsToMat((float*)pointsTo.data(), pointsTo.size(), 2);
+						rtflann::Index<rtflann::L2<float> > index(pointsToMat, rtflann::KDTreeIndexParams());
+						index.buildIndex();
+
+						std::vector< std::vector<size_t> > indices;
+						std::vector<std::vector<float> > dists;
+						float radius = (float)_guessWinSize; // pixels
+						rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
+						index.radiusSearch(cornersProjectedMat, indices, dists, radius*radius, rtflann::SearchParams());
+
+						UASSERT((int)indices.size() == cornersProjectedMat.rows);
+						UASSERT(descriptorsFrom.cols == descriptorsTo.cols);
+						UASSERT(cornersProjectedMat.rows == descriptorsFrom.rows);
+						UASSERT(kptsFrom.empty() || cornersProjectedMat.rows == (int)kptsFrom.size());
+						UASSERT(kptsFrom3D.empty() || cornersProjectedMat.rows == (int)kptsFrom3D.size());
+
+						UDEBUG("");
+
+						// Process results (Nearest Neighbor Distance Ratio)
+						int notMatchedUniqueId = cornersProjectedMat.rows;
+						std::set<int> addedWordsTo;
+						for(int i = 0; i < cornersProjectedMat.rows; ++i)
+						{
+							int matchedIndex = -1;
+							if(indices[i].size() >= 2)
+							{
+								cv::Mat descriptors(indices[i].size(), descriptorsTo.cols, descriptorsTo.type());
+								for(unsigned int j=0; j<indices[i].size(); ++j)
+								{
+									descriptorsTo.row(indices[i].at(j)).copyTo(descriptors.row(j));
+									addedWordsTo.insert(indices[i].at(j));
+								}
+
+								std::vector<std::vector<cv::DMatch> > matches;
+								cv::BFMatcher matcher(descriptors.type()==CV_8U?cv::NORM_HAMMING:cv::NORM_L2SQR);
+								matcher.knnMatch(descriptorsFrom.row(i), descriptors, matches, 2);
+								UASSERT(matches.size() == 1);
+								UASSERT(matches[0].size() == 2);
+								if(matches[0].at(0).distance < _nndr * matches[0].at(1).distance)
+								{
+									matchedIndex = indices[i].at(matches[0].at(0).trainIdx);
+								}
+							}
+							else if(indices[i].size() == 1)
+							{
+								matchedIndex = indices[i].at(0);
+							}
+
+							if(matchedIndex >= 0)
+							{
+								if(kptsFrom.size())
+								{
+									wordsFrom.insert(std::make_pair(i, kptsFrom[i]));
+								}
+								if(kptsFrom3D.size())
+								{
+									words3From.insert(std::make_pair(i, kptsFrom3D[i]));
+								}
+								wordsDescFrom.insert(std::make_pair(i, descriptorsFrom.row(i)));
+
+								wordsTo.insert(std::make_pair(i, kptsTo[matchedIndex]));
+								wordsDescTo.insert(std::make_pair(i, descriptorsTo.row(matchedIndex)));
+								if(kptsTo3D.size())
+								{
+									words3To.insert(std::make_pair(i, kptsTo3D[matchedIndex]));
+								}
+							}
+							else
+							{
+								// gen fake ids
+								if(kptsFrom.size())
+								{
+									wordsFrom.insert(std::make_pair(notMatchedUniqueId, kptsFrom[i]));
+								}
+								if(kptsFrom3D.size())
+								{
+									words3From.insert(std::make_pair(notMatchedUniqueId, kptsFrom3D[i]));
+								}
+								wordsDescFrom.insert(std::make_pair(notMatchedUniqueId, descriptorsFrom.row(i)));
+
+								++notMatchedUniqueId;
+							}
+						}
+
+						UDEBUG("addedWordsTo=%d, kptsTo=%d, wordsTo=%d", (int)addedWordsTo.size(), (int)kptsTo.size(), (int)wordsTo.size());
+
+						// create fake ids for not matched words from "to"
+						for(unsigned int i=0; i<kptsTo.size(); ++i)
+						{
+							if(addedWordsTo.find(i) == addedWordsTo.end())
+							{
+								wordsTo.insert(std::make_pair(notMatchedUniqueId, kptsTo[i]));
+								wordsDescTo.insert(std::make_pair(notMatchedUniqueId, descriptorsTo.row(i)));
+								if(kptsTo3D.size())
+								{
+									words3To.insert(std::make_pair(notMatchedUniqueId, kptsTo3D[i]));
+								}
+
+								++notMatchedUniqueId;
+							}
+						}
+					}
+					UDEBUG("");
+				}
+				else
+				{
+					UDEBUG("");
+					// match between all descriptors
+					VWDictionary dictionary(_featureParameters);
+					std::list<int> fromWordIds = dictionary.addNewWords(descriptorsFrom, 1);
+					std::list<int> toWordIds;
+
+					if(descriptorsTo.rows)
+					{
+						dictionary.update();
+						toWordIds = dictionary.addNewWords(descriptorsTo, 2);
+					}
+					dictionary.clear(false);
+
+					std::multiset<int> fromWordIdsSet(fromWordIds.begin(), fromWordIds.end());
+					std::multiset<int> toWordIdsSet(toWordIds.begin(), toWordIds.end());
+
+					UASSERT(kptsFrom.empty() || fromWordIds.size() == kptsFrom.size());
+					UASSERT(kptsFrom3D.empty() || fromWordIds.size() == kptsFrom3D.size());
+					UASSERT(fromWordIds.size() == descriptorsFrom.rows);
+					int i=0;
+					for(std::list<int>::iterator iter=fromWordIds.begin(); iter!=fromWordIds.end(); ++iter)
+					{
+						if(fromWordIdsSet.count(*iter) == 1)
+						{
+							if(kptsFrom.size())
+							{
+								wordsFrom.insert(std::make_pair(*iter, kptsFrom[i]));
+							}
+							if(kptsFrom3D.size())
+							{
+								words3From.insert(std::make_pair(*iter, kptsFrom3D[i]));
+							}
+							wordsDescFrom.insert(std::make_pair(*iter, descriptorsFrom.row(i)));
+						}
+						++i;
+					}
+					UASSERT(kptsTo3D.size() == 0 || kptsTo3D.size() == kptsTo.size());
+					UASSERT(toWordIds.size() == kptsTo.size());
+					UASSERT(toWordIds.size() == descriptorsTo.rows);
+					i=0;
+					for(std::list<int>::iterator iter=toWordIds.begin(); iter!=toWordIds.end(); ++iter)
+					{
+						if(toWordIdsSet.count(*iter) == 1)
+						{
+							wordsTo.insert(std::make_pair(*iter, kptsTo[i]));
+							wordsDescTo.insert(std::make_pair(*iter, descriptorsTo.row(i)));
+							if(kptsTo3D.size())
+							{
+								words3To.insert(std::make_pair(*iter, kptsTo3D[i]));
+							}
+						}
+						++i;
+					}
+				}
+			}
+			else if(descriptorsFrom.rows)
+			{
+				//just create fake words
+				UASSERT(kptsFrom.size() == descriptorsFrom.rows);
+				UASSERT(words3From.empty() || kptsFrom.size() == words3From.size());
+				for(unsigned int i=0; i<kptsFrom.size(); ++i)
+				{
+					wordsFrom.insert(std::make_pair(i, kptsFrom[i]));
+					wordsDescFrom.insert(std::make_pair(i, descriptorsFrom.row(i)));
+					if(kptsFrom3D.size())
+					{
+						words3From.insert(std::make_pair(i, kptsFrom3D[i]));
+					}
+				}
+			}
 		}
 		fromSignature.setWords(wordsFrom);
 		fromSignature.setWords3(words3From);
+		fromSignature.setWordsDescriptors(wordsDescFrom);
 		toSignature.setWords(wordsTo);
 		toSignature.setWords3(words3To);
+		toSignature.setWordsDescriptors(wordsDescTo);
 		delete detector;
 	}

@@ -700,7 +898,7 @@ Transform RegistrationVis::computeTransformationImpl(
 								_PnPFlags,
 								_PnPRefineIterations,
 								dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
-								uMultimapToMapUnique(signatureA->getWords3()),
+								uMultimapToMapUnique(signatureB->getWords3()),
 								varianceFromInliersCount()?0:&variances[dir],
 								&matchesV,
 								&inliersV);
@@ -708,8 +906,8 @@ Transform RegistrationVis::computeTransformationImpl(
 						matches[dir] = matchesV;
 						if(transforms[dir].isNull())
 						{
-							msg = uFormat("Not enough inliers %d/%d between %d and %d",
-									(int)inliers[dir].size(), _minInliers, signatureA->id(), signatureB->id());
+							msg = uFormat("Not enough inliers %d/%d (matches=%d) between %d and %d",
+									(int)inliers[dir].size(), _minInliers, (int)matches[dir].size(), signatureA->id(), signatureB->id());
 							UINFO(msg.c_str());
 						}
 					}
@@ -61,7 +61,8 @@ public:
 		nextIndex_(0),
 		featuresType_(0),
 		featuresDim_(0),
-		isLSH_(false)
+		isLSH_(false),
+		useDistanceL1_(false)
 	{
 	}
 	virtual ~FlannIndex()
@@ -1099,8 +1100,6 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 {
 	UTimer timer;
 	timer.start();
-	std::vector<int> resultIds(vws.size(), 0);
-	unsigned int k=2; // k nearest neighbor

 	if(_visualWords.size() && vws.size())
 	{
@@ -1110,20 +1109,15 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 		if(dim != (*vws.begin())->getDescriptor().cols)
 		{
 			UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", (*vws.begin())->getDescriptor().cols, dim);
-			return resultIds;
+			return std::vector<int>(vws.size(), 0);
 		}

 		if(type != (*vws.begin())->getDescriptor().type())
 		{
 			UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", (*vws.begin())->getDescriptor().type(), type);
-			return resultIds;
+			return std::vector<int>(vws.size(), 0);
 		}

-		std::vector<std::vector<cv::DMatch> > matches;
-		bool bruteForce = false;
-		cv::Mat results;
-		cv::Mat dists;
-
 		// fill the request matrix
 		int index = 0;
 		VisualWord * vw;
@@ -1139,10 +1133,43 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 		}
 		ULOGGER_DEBUG("Preparation time = %fs", timer.ticks());

+		return findNN(query);
+	}
+	return std::vector<int>(vws.size(), 0);
+}
+std::vector<int> VWDictionary::findNN(const cv::Mat & query) const
+{
+	UTimer timer;
+	timer.start();
+	std::vector<int> resultIds(query.rows, 0);
+	unsigned int k=2; // k nearest neighbor
+
+	if(_visualWords.size() && query.rows)
+	{
+		int dim = _visualWords.begin()->second->getDescriptor().cols;
+		int type = _visualWords.begin()->second->getDescriptor().type();
+
+		if(dim != query.cols)
+		{
+			UERROR("Descriptors (size=%d) are not the same size as already added words in dictionary(size=%d)", query.cols, dim);
+			return resultIds;
+		}
+
+		if(type != query.type())
+		{
+			UERROR("Descriptors (type=%d) are not the same type as already added words in dictionary(type=%d)", query.type(), type);
+			return resultIds;
+		}
+
+		std::vector<std::vector<cv::DMatch> > matches;
+		bool bruteForce = false;
+		cv::Mat results;
+		cv::Mat dists;
+
 		if(_flannIndex->isBuilt() || (!_dataTree.empty() && _dataTree.rows >= (int)k))
 		{
 			//Find nearest neighbors
-			UDEBUG("newPts.total()=%d ", query.total());
+			UDEBUG("query.rows=%d ", query.rows);

 			if(_strategy == kNNFlannNaive || _strategy == kNNFlannKdTree || _strategy == kNNFlannLSH)
 			{
@@ -1231,7 +1258,7 @@ std::vector<int> VWDictionary::findNN(const std::list<VisualWord *> & vws) const
 		}
 		ULOGGER_DEBUG("Search not yet indexed words time = %fs", timer.ticks());

-		for(unsigned int i=0; i<vws.size(); ++i)
+		for(unsigned int i=0; i<query.rows; ++i)
 		{
 			std::multimap<float, int> fullResults; // Contains results from the kd-tree search [and the naive search in new words]
 			if(!bruteForce && dists.cols)
@@ -27,10 +27,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include <rtabmap/core/Rtabmap.h>
 #include <rtabmap/core/CameraStereo.h>
-#include <rtabmap/core/OdometryLocalMap.h>
 #include <rtabmap/utilite/UThread.h>
 #include "MapBuilder.h"
 #include <pcl/visualization/cloud_viewer.h>
+#include <rtabmap/core/OdometryF2M.h>
 #include <QApplication>
 #include <stdio.h>

@@ -103,7 +103,7 @@ int main(int argc, char * argv[])

 	if(camera.init(calibrationDir, calibrationName))
 	{
-		OdometryLocalMap odom;
+		OdometryF2M odom;
 		Rtabmap rtabmap;
 		rtabmap.init();

@@ -25,11 +25,11 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

+#include <rtabmap/core/OdometryF2M.h>
 #include "rtabmap/core/Rtabmap.h"
 #include "rtabmap/core/RtabmapThread.h"
 #include "rtabmap/core/CameraRGBD.h"
 #include "rtabmap/core/CameraThread.h"
-#include "rtabmap/core/OdometryLocalMap.h"
 #include "rtabmap/core/OdometryThread.h"
 #include "rtabmap/utilite/UEventsManager.h"
 #include <QApplication>
@@ -128,7 +128,7 @@ int main(int argc, char * argv[])
 	MapBuilder mapBuilder(&cameraThread);

 	// Create an odometry thread to process camera events, it will send OdometryEvent.
-	OdometryThread odomThread(new OdometryLocalMap());
+	OdometryThread odomThread(new OdometryF2M());


 	// Create RTAB-Map to process OdometryEvent
@@ -25,11 +25,11 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

+#include <rtabmap/core/OdometryF2M.h>
 #include "rtabmap/core/Rtabmap.h"
 #include "rtabmap/core/RtabmapThread.h"
 #include "rtabmap/core/CameraRGBD.h"
 #include "rtabmap/core/CameraThread.h"
-#include "rtabmap/core/OdometryLocalMap.h"
 #include "rtabmap/core/OdometryThread.h"
 #include "rtabmap/utilite/UEventsManager.h"
 #include <QApplication>
@@ -171,7 +171,7 @@ int main(int argc, char * argv[])
 	MapBuilderWifi mapBuilderWifi(&cameraThread);

 	// Create an odometry thread to process camera events, it will send OdometryEvent.
-	OdometryThread odomThread(new OdometryLocalMap());
+	OdometryThread odomThread(new OdometryF2M());

 	// Create RTAB-Map to process OdometryEvent
 	Rtabmap * rtabmap = new Rtabmap();
@@ -675,6 +675,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->stackedWidget_loopClosureCorrespondences->setCurrentIndex(Parameters::defaultVisCorType());
 	_ui->reextract_nn->setObjectName(Parameters::kVisCorNNType().c_str());
 	_ui->reextract_nndrRatio->setObjectName(Parameters::kVisCorNNDR().c_str());
+	_ui->spinBox_visCorGuessWinSize->setObjectName(Parameters::kVisCorGuessWinSize().c_str());
 	_ui->reextract_type->setObjectName(Parameters::kVisFeatureType().c_str());
 	_ui->reextract_maxFeatures->setObjectName(Parameters::kVisMaxFeatures().c_str());
 	_ui->loopClosure_bowMaxDepth->setObjectName(Parameters::kVisMaxDepth().c_str());
@@ -706,18 +707,18 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->odom_fillInfoData->setObjectName(Parameters::kOdomFillInfoData().c_str());
 	_ui->odom_dataBufferSize->setObjectName(Parameters::kOdomImageBufferSize().c_str());

-	//Odometry BOW
-	_ui->odom_localHistory->setObjectName(Parameters::kOdomLocalMapHistorySize().c_str());
-	_ui->odom_fixedLocalMapPath->setObjectName(Parameters::kOdomLocalMapFixedMapPath().c_str());
+	//Odometry Frame to Map
+	_ui->odom_localHistory->setObjectName(Parameters::kOdomF2MMaxSize().c_str());
+	_ui->odom_fixedLocalMapPath->setObjectName(Parameters::kOdomF2MFixedMapPath().c_str());
 	connect(_ui->toolButton_odomBowFixedLocalMap, SIGNAL(clicked()), this, SLOT(changeOdomBowFixedLocalMapPath()));

-	//Odometry Optical Flow
+	//Odometry Frame to Frame
 	_ui->odom_flow_keyframeThr->setObjectName(Parameters::kOdomF2FKeyFrameThr().c_str());
 	_ui->odom_flow_winSize_2->setObjectName(Parameters::kVisCorFlowWinSize().c_str());
 	_ui->odom_flow_maxLevel_2->setObjectName(Parameters::kVisCorFlowMaxLevel().c_str());
 	_ui->odom_flow_iterations_2->setObjectName(Parameters::kVisCorFlowIterations().c_str());
 	_ui->odom_flow_eps_2->setObjectName(Parameters::kVisCorFlowEps().c_str());
-	_ui->odom_flow_guessMotion->setObjectName(Parameters::kOdomF2FGuessMotion().c_str());
+	_ui->odom_flow_guessMotion->setObjectName(Parameters::kOdomGuessMotion().c_str());

 	//Odometry Mono
 	_ui->doubleSpinBox_minFlow->setObjectName(Parameters::kOdomMonoInitMinFlow().c_str());
@@ -63,7 +63,7 @@
          <property name="geometry">
           <rect>
            <x>0</x>
-            <y>-1377</y>
+            <y>0</y>
            <width>681</width>
            <height>2010</height>
           </rect>
@@ -86,7 +86,7 @@
                <enum>QFrame::Raised</enum>
               </property>
               <property name="currentIndex">
-                <number>3</number>
+                <number>16</number>
               </property>
               <widget class="QWidget" name="page_22">
                <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -7218,7 +7218,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="6" column="0">
+                      <item row="7" column="0">
                       <widget class="QSpinBox" name="odom_dataBufferSize">
                        <property name="maximum">
                         <number>999999</number>
@@ -7245,7 +7245,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                        <item>
                         <property name="text">
-                          <string>Local Map</string>
+                          <string>Frame to Map</string>
                         </property>
                        </item>
                        <item>
@@ -7255,7 +7255,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </item>
                       </widget>
                      </item>
-                      <item row="7" column="0">
+                      <item row="8" column="0">
                       <widget class="QPushButton" name="pushButton_testOdometry">
                        <property name="text">
                         <string>Test selected odometry</string>
@@ -7275,7 +7275,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
-                      <item row="6" column="1">
+                      <item row="7" column="1">
                       <widget class="QLabel" name="label_232">
                        <property name="text">
                         <string>Data buffer size (0 means inf).</string>
@@ -7330,6 +7330,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </item>
                       </widget>
                      </item>
+                      <item row="6" column="1">
+                       <widget class="QLabel" name="label_198">
+                        <property name="text">
+                         <string>Guess based on previous motion.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="6" column="0">
+                       <widget class="QCheckBox" name="odom_flow_guessMotion">
+                        <property name="text">
+                         <string/>
+                        </property>
+                       </widget>
+                      </item>
                     </layout>
                    </item>
                    <item>
@@ -7342,13 +7362,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        <item>
                         <widget class="QGroupBox" name="groupBox_odometryBOW2">
                          <property name="title">
-                           <string>Local Map</string>
+                           <string>Frame to Map</string>
                          </property>
                          <layout class="QVBoxLayout" name="verticalLayout_54">
                           <item>
                            <widget class="QLabel" name="label_226">
                             <property name="text">
-                              <string>The approach maintains a local map of features to match to. It adds incrementally new points to local map up to local history size, then removes old points as new points are added. See &quot;Motion Estimation&quot; panel for registration parameters. Note that only visual registration parameters are used in this approach. If you want to use a different registration pipeline than Visual (e.g. with ICP), use &quot;Frame to Frame&quot; approach instead.</string>
+                              <string>The approach maintains a local map of features to match to. It adds incrementally new points to local map up to maximum map size, then removes old points as new points are added. See &quot;Motion Estimation&quot; panel for registration parameters. Note that only visual registration parameters are used in this approach. If you want to use a different registration pipeline than Visual (e.g. with ICP), use &quot;Frame to Frame&quot; approach instead.</string>
                             </property>
                             <property name="wordWrap">
                              <bool>true</bool>
@@ -7363,7 +7383,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                             <item row="0" column="2">
                              <widget class="QLabel" name="label_190">
                               <property name="text">
-                                <string>Local history size: If &gt; 0 (example 5000), the odometry will maintain a local map of X maximum words. This will decrease odometry drifting when the camera is not moving.</string>
+                                <string>Maximum map size: If &gt; 0 (example 5000), the odometry will maintain a local map of X maximum words. This will decrease odometry drifting when the camera is not moving.</string>
                               </property>
                               <property name="wordWrap">
                                <bool>true</bool>
@@ -7484,26 +7504,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                               </property>
                              </widget>
                             </item>
-                             <item row="1" column="1">
-                              <widget class="QLabel" name="label_198">
-                               <property name="text">
-                                <string>Guess optical flow based on previous motion.</string>
-                               </property>
-                               <property name="wordWrap">
-                                <bool>true</bool>
-                               </property>
-                               <property name="textInteractionFlags">
-                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                               </property>
-                              </widget>
-                             </item>
-                             <item row="1" column="0">
-                              <widget class="QCheckBox" name="odom_flow_guessMotion">
-                               <property name="text">
-                                <string/>
-                               </property>
-                              </widget>
-                             </item>
                            </layout>
                           </item>
                           <item>
@@ -8123,39 +8123,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                   <layout class="QVBoxLayout" name="verticalLayout_41" stretch="0,0,0,1">
                    <item>
                     <layout class="QGridLayout" name="gridLayout_23" columnstretch="0,1">
-                      <item row="5" column="0">
-                       <widget class="QCheckBox" name="loopClosure_bowVarianceFromInliersCount">
-                        <property name="text">
-                         <string/>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="0" column="1">
-                       <widget class="QLabel" name="label_149">
-                        <property name="text">
-                         <string>Correspondences computation approach.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="2" column="0">
-                       <widget class="QSpinBox" name="loopClosure_bowMinInliers">
-                        <property name="minimum">
-                         <number>1</number>
-                        </property>
-                        <property name="maximum">
-                         <number>1000</number>
-                        </property>
-                        <property name="value">
-                         <number>10</number>
-                        </property>
-                       </widget>
-                      </item>
                      <item row="1" column="1">
                       <widget class="QLabel" name="label_146">
                        <property name="text">
@@ -8223,6 +8190,9 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                      </item>
                      <item row="1" column="0">
                       <widget class="QComboBox" name="loopClosure_estimationType">
+                        <property name="sizeAdjustPolicy">
+                         <enum>QComboBox::AdjustToContentsOnFirstShow</enum>
+                        </property>
                        <item>
                         <property name="text">
                          <string>3D to 3D</string>
@@ -8235,13 +8205,22 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </item>
                        <item>
                         <property name="text">
-                          <string>2D to 2D (Epipolar Geometry)</string>
+                          <string>2D to 2D</string>
                         </property>
                        </item>
                       </widget>
                      </item>
                      <item row="0" column="0">
                       <widget class="QComboBox" name="loopClosure_correspondencesType">
+                        <property name="sizePolicy">
+                         <sizepolicy hsizetype="Preferred" vsizetype="Fixed">
+                          <horstretch>0</horstretch>
+                          <verstretch>0</verstretch>
+                         </sizepolicy>
+                        </property>
+                        <property name="sizeAdjustPolicy">
+                         <enum>QComboBox::AdjustToContentsOnFirstShow</enum>
+                        </property>
                        <item>
                         <property name="text">
                          <string>Features Matching</string>
@@ -8274,12 +8253,45 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
+                      <item row="5" column="0">
+                       <widget class="QCheckBox" name="loopClosure_bowVarianceFromInliersCount">
+                        <property name="text">
+                         <string/>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="0" column="1">
+                       <widget class="QLabel" name="label_149">
+                        <property name="text">
+                         <string>Correspondences computation approach.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="2" column="0">
+                       <widget class="QSpinBox" name="loopClosure_bowMinInliers">
+                        <property name="minimum">
+                         <number>1</number>
+                        </property>
+                        <property name="maximum">
+                         <number>1000</number>
+                        </property>
+                        <property name="value">
+                         <number>10</number>
+                        </property>
+                       </widget>
+                      </item>
                     </layout>
                    </item>
                    <item>
                     <widget class="QStackedWidget" name="stackedWidget_loopClosureCorrespondences">
                      <property name="currentIndex">
-                       <number>1</number>
+                       <number>0</number>
                      </property>
                      <widget class="QWidget" name="page_54">
                       <layout class="QVBoxLayout" name="verticalLayout_85">
@@ -8374,7 +8386,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                             </property>
                            </widget>
                           </item>
-                           <item row="2" column="0">
+                           <item row="3" column="0">
                            <spacer name="verticalSpacer_44">
                             <property name="orientation">
                              <enum>Qt::Vertical</enum>
@@ -8387,6 +8399,35 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                             </property>
                            </spacer>
                           </item>
+                           <item row="2" column="1">
+                            <widget class="QLabel" name="label_303">
+                             <property name="text">
+                              <string>Matching window size around projected points when a guess transform is provided to find correspondences.</string>
+                             </property>
+                             <property name="wordWrap">
+                              <bool>true</bool>
+                             </property>
+                             <property name="textInteractionFlags">
+                              <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                             </property>
+                            </widget>
+                           </item>
+                           <item row="2" column="0">
+                            <widget class="QSpinBox" name="spinBox_visCorGuessWinSize">
+                             <property name="suffix">
+                              <string> pixels</string>
+                             </property>
+                             <property name="minimum">
+                              <number>3</number>
+                             </property>
+                             <property name="maximum">
+                              <number>1000</number>
+                             </property>
+                             <property name="value">
+                              <number>16</number>
+                             </property>
+                            </widget>
+                           </item>
                          </layout>
                         </widget>
                        </item>
@@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UEventsManager.h>
 #include <rtabmap/utilite/UFile.h>
 #include <rtabmap/utilite/UConversion.h>
-#include <rtabmap/core/OdometryLocalMap.h>
 #include <rtabmap/core/OdometryF2F.h>
 #include <rtabmap/core/OdometryMono.h>
 #include <rtabmap/core/OdometryThread.h>
@@ -42,6 +41,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QApplication>
 #include <QPushButton>
 #include <pcl/console/print.h>
+#include <rtabmap/core/OdometryF2M.h>

 void showUsage()
 {
@@ -121,7 +121,7 @@ int main (int argc, char * argv[])
 	int fastThr = rtabmap::Parameters::defaultFASTThreshold();
 	float sec = 0.0f;
 	bool gpu = false;
-	int localHistory = rtabmap::Parameters::defaultOdomLocalMapHistorySize();
+	int localHistory = rtabmap::Parameters::defaultOdomF2MMaxSize();
 	bool p2p = false;

 	for(int i=1; i<argc; ++i)
@@ -646,13 +646,13 @@ int main (int argc, char * argv[])
 		}
 		else
 		{
-			//BOW
+			//Frame to Map
 			UINFO("Nearest neighbor =         %s", nnName.c_str());
 			UINFO("Nearest neighbor ratio =  %f", nndr);
 			UINFO("Local history =           %d", localHistory);
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisCorNNType(), uNumber2Str(nnType)));
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisCorNNDR(), uNumber2Str(nndr)));
-			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomLocalMapHistorySize(), uNumber2Str(localHistory)));
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomF2MMaxSize(), uNumber2Str(localHistory)));

 			if(mono)
 			{
@@ -663,7 +663,7 @@ int main (int argc, char * argv[])
 			}
 			else
 			{
-				odom = new rtabmap::OdometryLocalMap(parameters);
+				odom = new rtabmap::OdometryF2M(parameters);
 			}
 		}
 	}