Removed parameter "OdomF2M/FixedMapPath". GUI: added odometry disabled option. Rtabmap can now localize even if odometry input is null (only in localization mode).

corelib/include/rtabmap/core/OdometryF2M.h

@@ -62,7 +62,6 @@ private:
 	float scanKeyFrameThr_;
 	int scanMaximumMapSize_;
 	float scanSubtractRadius_;
-	std::string fixedMapPath_;
 	int bundleAdjustment_;
 	int	bundleAdjustmentMaxFrames_;
 

corelib/include/rtabmap/core/Parameters.h

@@ -379,7 +379,6 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(OdomF2M, MaxNewFeatures,      int, 0,       "[Visual] Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit.");
 	RTABMAP_PARAM(OdomF2M, ScanMaxSize,         int, 2000,    "[Geometry] Maximum local scan map size.");
 	RTABMAP_PARAM(OdomF2M, ScanSubtractRadius,  float, 0.05,  "[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the scans.");
-	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.")
 	RTABMAP_PARAM(OdomF2M, BundleAdjustment,          int, 0, "Local bundle adjustment: 0=disabled, 1=g2o, 2=cvsba.");
 	RTABMAP_PARAM(OdomF2M, BundleAdjustmentMaxFrames, int, 0, "Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).");
 

corelib/include/rtabmap/core/Rtabmap.h

@@ -61,7 +61,7 @@ public:
 	bool process(const cv::Mat & image, int id=0); // for convenience, an id is automatically generated if id=0
 	bool process(
 			const SensorData & data,
-			const Transform & odomPose,
+			Transform odomPose,
 			const cv::Mat & covariance = cv::Mat::eye(6,6,CV_64FC1)); // for convenience
 
 	void init(const ParametersMap & parameters, const std::string & databasePath = "");

corelib/src/Memory.cpp

@@ -3456,52 +3456,41 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	if(!pose.isNull() &&
 		data.cameraModels().size() == 1 &&
 		words.size() &&
-		words3D.size() == 0)
+		words3D.size() == 0 &&
+		_signatures.size() &&
+		_signatures.rbegin()->second->mapId() == _idMapCount) // same map
 	{
-		bool fillWithNaN = true;
+		UDEBUG("Generate 3D words using odometry");
-		if(_signatures.size())
+		Signature * previousS = _signatures.rbegin()->second;
+		if(previousS->getWords().size() > 8 && words.size() > 8 && !previousS->getPose().isNull())
 		{
-			UDEBUG("Generate 3D words using odometry");
+			Transform cameraTransform = pose.inverse() * previousS->getPose();
-			Signature * previousS = _signatures.rbegin()->second;
+			// compute 3D words by epipolar geometry with the previous signature
-			if(previousS->getWords().size() > 8 && words.size() > 8 && !previousS->getPose().isNull())
+			std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
-			{
+					uMultimapToMapUnique(words),
-				Transform cameraTransform = pose.inverse() * previousS->getPose();
+					uMultimapToMapUnique(previousS->getWords()),
-				// compute 3D words by epipolar geometry with the previous signature
+					data.cameraModels()[0],
-				std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
+					cameraTransform);
-						uMultimapToMapUnique(words),
-						uMultimapToMapUnique(previousS->getWords()),
-						data.cameraModels()[0],
-						cameraTransform);
 
-				// words3D should have the same size than words
+			// words3D should have the same size than words
-				float bad_point = std::numeric_limits<float>::quiet_NaN ();
-				for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
-				{
-					std::map<int, cv::Point3f>::iterator jter=inliers.find(iter->first);
-					if(jter != inliers.end())
-					{
-						words3D.insert(std::make_pair(iter->first, jter->second));
-					}
-					else
-					{
-						words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
-					}
-				}
-
-				t = timer.ticks();
-				UASSERT(words3D.size() == words.size());
-				if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
-				UDEBUG("time keypoints 3D (%d) = %fs", (int)words3D.size(), t);
-				fillWithNaN = false;
-			}
-		}
-		if(fillWithNaN)
-		{
 			float bad_point = std::numeric_limits<float>::quiet_NaN ();
 			for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
 			{
-				words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
+				std::map<int, cv::Point3f>::iterator jter=inliers.find(iter->first);
+				if(jter != inliers.end())
+				{
+					words3D.insert(std::make_pair(iter->first, jter->second));
+				}
+				else
+				{
+					words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
+				}
 			}
+
+			t = timer.ticks();
+			UASSERT(words3D.size() == words.size());
+			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
+			UDEBUG("time keypoints 3D (%d) = %fs", (int)words3D.size(), t);
 		}
 	}
 

corelib/src/Odometry.cpp

@@ -206,56 +206,63 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 	// Ground alignment
 	if(_pose.isIdentity() && _alignWithGround)
 	{
-		UTimer alignTimer;
+		if(data.depthOrRightRaw().empty())
-		pcl::IndicesPtr indices(new std::vector<int>);
-		pcl::IndicesPtr ground, obstacles;
-		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, 1, 0, 0, indices.get());
-		cloud = util3d::voxelize(cloud, indices, 0.01);
-		bool success = false;
-		if(cloud->size())
 		{
-			util3d::segmentObstaclesFromGround<pcl::PointXYZ>(cloud, ground, obstacles, 20, M_PI/4.0f, 0.02, 200, true);
+			UWARN("\"%s\" is true but the input has no depth information, ignoring alignment with ground...", Parameters::kOdomAlignWithGround().c_str());
-			if(ground->size())
-			{
-				pcl::ModelCoefficients coefficients;
-				util3d::extractPlane(cloud, ground, 0.02, 100, &coefficients);
-				if(coefficients.values.at(3) >= 0)
-				{
-					UWARN("Ground detected! coefficients=(%f, %f, %f, %f) time=%fs",
-							coefficients.values.at(0),
-							coefficients.values.at(1),
-							coefficients.values.at(2),
-							coefficients.values.at(3),
-							alignTimer.ticks());
-				}
-				else
-				{
-					UWARN("Ceiling detected! coefficients=(%f, %f, %f, %f) time=%fs",
-							coefficients.values.at(0),
-							coefficients.values.at(1),
-							coefficients.values.at(2),
-							coefficients.values.at(3),
-							alignTimer.ticks());
-				}
-				Eigen::Vector3f n(coefficients.values.at(0), coefficients.values.at(1), coefficients.values.at(2));
-				Eigen::Vector3f z(0,0,1);
-				//get rotation from z to n;
-				Eigen::Matrix3f R;
-				R = Eigen::Quaternionf().setFromTwoVectors(n,z);
-				Transform rotation(
-						R(0,0), R(0,1), R(0,2), 0,
-						R(1,0), R(1,1), R(1,2), 0,
-						R(2,0), R(2,1), R(2,2), coefficients.values.at(3));
-				_pose *= rotation;
-				success = true;
-			}
 		}
-		if(!success)
+		else
 		{
-			UERROR("Odometry failed to detect the ground. You have this "
+			UTimer alignTimer;
-					"error because parameter \"Odom/AlignWithGround\" is true. "
+			pcl::IndicesPtr indices(new std::vector<int>);
-					"Make sure the camera is seeing the ground (e.g., tilt ~30 "
+			pcl::IndicesPtr ground, obstacles;
-					"degrees toward the ground).");
+			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, 1, 0, 0, indices.get());
+			cloud = util3d::voxelize(cloud, indices, 0.01);
+			bool success = false;
+			if(cloud->size())
+			{
+				util3d::segmentObstaclesFromGround<pcl::PointXYZ>(cloud, ground, obstacles, 20, M_PI/4.0f, 0.02, 200, true);
+				if(ground->size())
+				{
+					pcl::ModelCoefficients coefficients;
+					util3d::extractPlane(cloud, ground, 0.02, 100, &coefficients);
+					if(coefficients.values.at(3) >= 0)
+					{
+						UWARN("Ground detected! coefficients=(%f, %f, %f, %f) time=%fs",
+								coefficients.values.at(0),
+								coefficients.values.at(1),
+								coefficients.values.at(2),
+								coefficients.values.at(3),
+								alignTimer.ticks());
+					}
+					else
+					{
+						UWARN("Ceiling detected! coefficients=(%f, %f, %f, %f) time=%fs",
+								coefficients.values.at(0),
+								coefficients.values.at(1),
+								coefficients.values.at(2),
+								coefficients.values.at(3),
+								alignTimer.ticks());
+					}
+					Eigen::Vector3f n(coefficients.values.at(0), coefficients.values.at(1), coefficients.values.at(2));
+					Eigen::Vector3f z(0,0,1);
+					//get rotation from z to n;
+					Eigen::Matrix3f R;
+					R = Eigen::Quaternionf().setFromTwoVectors(n,z);
+					Transform rotation(
+							R(0,0), R(0,1), R(0,2), 0,
+							R(1,0), R(1,1), R(1,2), 0,
+							R(2,0), R(2,1), R(2,2), coefficients.values.at(3));
+					_pose *= rotation;
+					success = true;
+				}
+			}
+			if(!success)
+			{
+				UERROR("Odometry failed to detect the ground. You have this "
+						"error because parameter \"%s\" is true. "
+						"Make sure the camera is seeing the ground (e.g., tilt ~30 "
+						"degrees toward the ground).", Parameters::kOdomAlignWithGround().c_str());
+			}
 		}
 	}
 

corelib/src/OdometryF2M.cpp

@@ -62,7 +62,6 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
 	scanMaximumMapSize_(Parameters::defaultOdomF2MScanMaxSize()),
 	scanSubtractRadius_(Parameters::defaultOdomF2MScanSubtractRadius()),
-	fixedMapPath_(Parameters::defaultOdomF2MFixedMapPath()),
 	bundleAdjustment_(Parameters::defaultOdomF2MBundleAdjustment()),
 	bundleAdjustmentMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()),
 	regPipeline_(Registration::create(parameters)),
@@ -76,7 +75,6 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
 	Parameters::parse(parameters, Parameters::kOdomF2MScanMaxSize(), scanMaximumMapSize_);
 	Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractRadius(), scanSubtractRadius_);
-	Parameters::parse(parameters, Parameters::kOdomF2MFixedMapPath(), fixedMapPath_);
 	Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustment(), bundleAdjustment_);
 	Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxFrames(), bundleAdjustmentMaxFrames_);
 	bundleParameters_ = parameters;
@@ -84,90 +82,6 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	UASSERT(keyFrameThr_ >= 0.0f && keyFrameThr_<=1.0f);
 	UASSERT(scanKeyFrameThr_ >= 0.0f && scanKeyFrameThr_<=1.0f);
 	UASSERT(maxNewFeatures_ >= 0);
-
-	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 if(memory.getLastWorkingSignature())
-		{
-			// get the graph
-			std::map<int, int> ids = memory.getNeighborsId(memory.getLastWorkingSignature()->id(), 0, -1);
-			UDEBUG("ids=%d", (int)ids.size());
-			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;
-
-				UDEBUG("optimizedPoses=%d", (int)optimizedPoses.size());
-
-				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)
-					{
-						UDEBUG("%d has %d words", posesIter->first, (int)s->getWords3().size());
-						// 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());
-				UDEBUG("words3D=%d", (int)words3D.size());
-				map_->setWords3(words3D);
-				map_->setWordsDescriptors(wordsDescriptors);
-			}
-			else
-			{
-				UERROR("No pose loaded from database \"%s\"", fixedMapPath_.c_str());
-			}
-		}
-		if((int)map_->getWords3().size() < regPipeline_->getMinVisualCorrespondences() || map_->getWords3().size() == 0)
-		{
-			// TODO: support geometric-only maps?
-			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()
@@ -188,15 +102,7 @@ void OdometryF2M::reset(const Transform & initialPose)
 {
 	Odometry::reset(initialPose);
 	*lastFrame_ = Signature(1);
-
+	*map_ = Signature(-1);
-	if(fixedMapPath_.empty())
-	{
-		*map_ = Signature(-1);
-	}
-	else
-	{
-		UWARN("Odometry cannot be reset when a fixed local map is set.");
-	}
 }
 
 // return not null transform if odometry is correctly computed
@@ -358,300 +264,293 @@ Transform OdometryF2M::computeTransform(
 			{
 				output = transform;
 
-				if(fixedMapPath_.empty())
+				bool modified = false;
+				Transform newFramePose = this->getPose()*output;
+
+				// fields to update
+				cv::Mat mapScan = tmpMap.sensorData().laserScanRaw();
+				std::multimap<int, cv::KeyPoint> mapWords = tmpMap.getWords();
+				std::multimap<int, cv::Point3f> mapPoints = tmpMap.getWords3();
+				std::multimap<int, cv::Mat> mapDescriptors = tmpMap.getWordsDescriptors();
+
+				//Visual
+				int added = 0;
+				int removed = 0;
+				UDEBUG("keyframeThr=%f matches=%d inliers=%d features=%d mp=%d", keyFrameThr_, regInfo.matches, regInfo.inliers, (int)lastFrame_->sensorData().keypoints().size(), (int)mapPoints.size());
+				if(regPipeline_->isImageRequired() &&
+					(keyFrameThr_==0 || float(regInfo.inliers) <= keyFrameThr_*float(lastFrame_->sensorData().keypoints().size())))
 				{
-					bool modified = false;
+					UDEBUG("Update local map (ratio=%f < %f)", float(regInfo.inliers)/float(lastFrame_->sensorData().keypoints().size()), keyFrameThr_);
-					Transform newFramePose = this->getPose()*output;
 
-					// fields to update
+					// update local map
-					cv::Mat mapScan = tmpMap.sensorData().laserScanRaw();
+					UASSERT(mapWords.size() == mapPoints.size());
-					std::multimap<int, cv::KeyPoint> mapWords = tmpMap.getWords();
+					UASSERT(mapPoints.size() == mapDescriptors.size());
-					std::multimap<int, cv::Point3f> mapPoints = tmpMap.getWords3();
+					UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
-					std::multimap<int, cv::Mat> mapDescriptors = tmpMap.getWordsDescriptors();
 
-					//Visual
+					std::map<int, int>::iterator iterBundlePosesRef = bundlePoseReferences_.end();
-					int added = 0;
+					if(bundleAdjustment_>0)
-					int removed = 0;
-					UDEBUG("keyframeThr=%f matches=%d inliers=%d features=%d mp=%d", keyFrameThr_, regInfo.matches, regInfo.inliers, (int)lastFrame_->sensorData().keypoints().size(), (int)mapPoints.size());
-					if(regPipeline_->isImageRequired() &&
-						(keyFrameThr_==0 || float(regInfo.inliers) <= keyFrameThr_*float(lastFrame_->sensorData().keypoints().size())))
 					{
-						UDEBUG("Update local map (ratio=%f < %f)", float(regInfo.inliers)/float(lastFrame_->sensorData().keypoints().size()), keyFrameThr_);
+						// update local map 3D points (if bundle adjustment was done)
-
+						for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
-						// update local map
-						UASSERT(mapWords.size() == mapPoints.size());
-						UASSERT(mapPoints.size() == mapDescriptors.size());
-						UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
-
-						std::map<int, int>::iterator iterBundlePosesRef = bundlePoseReferences_.end();
-						if(bundleAdjustment_>0)
 						{
-							// update local map 3D points (if bundle adjustment was done)
+							UASSERT(mapPoints.count(iter->first) == 1);
-							for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
+							mapPoints.find(iter->first)->second = iter->second;
-							{
-								UASSERT(mapPoints.count(iter->first) == 1);
-								mapPoints.find(iter->first)->second = iter->second;
-							}
-							bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), 0));
-							uInsert(bundlePoses_, bundlePoses);
-							UASSERT(bundleModels.find(lastFrame_->id()) != bundleModels.end());
-							bundleModels_.insert(*bundleModels.find(lastFrame_->id()));
-							UASSERT(bundleLinks.find(lastFrame_->id()) != bundleLinks.end());
-							bundleLinks_.insert(*bundleLinks.find(lastFrame_->id()));
-							iterBundlePosesRef = bundlePoseReferences_.find(lastFrame_->id());
 						}
-
+						bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), 0));
-						// sort by feature response
+						uInsert(bundlePoses_, bundlePoses);
-						std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > > newIds;
+						UASSERT(bundleModels.find(lastFrame_->id()) != bundleModels.end());
-						UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
+						bundleModels_.insert(*bundleModels.find(lastFrame_->id()));
-						std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().begin();
+						UASSERT(bundleLinks.find(lastFrame_->id()) != bundleLinks.end());
-						std::multimap<int, cv::Mat>::const_iterator iterDesc = lastFrame_->getWordsDescriptors().begin();
+						bundleLinks_.insert(*bundleLinks.find(lastFrame_->id()));
-						for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin(); iter!=lastFrame_->getWords3().end(); ++iter, ++iter2D, ++iterDesc)
+						iterBundlePosesRef = bundlePoseReferences_.find(lastFrame_->id());
-						{
-							if(util3d::isFinite(iter->second))
-							{
-								if(mapPoints.find(iter->first) == mapPoints.end()) // Point not in map
-								{
-									newIds.insert(
-											std::make_pair(iter2D->second.response>0?1.0f/iter2D->second.response:0.0f,
-													std::make_pair(iter->first,
-															std::make_pair(iter2D->second,
-																	std::make_pair(iter->second, iterDesc->second)))));
-								}
-								else if(bundleAdjustment_>0)
-								{
-									if(lastFrame_->getWords().count(iter->first) == 1)
-									{
-										UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
-										iterBundlePosesRef->second += 1;
-
-										if(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end())
-										{
-											std::map<int, cv::Point2f> framePt;
-											framePt.insert(std::make_pair(lastFrame_->id(), iter2D->second.pt));
-											bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
-										}
-										else
-										{
-											bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), iter2D->second.pt));
-										}
-									}
-								}
-							}
-						}
-
-						for(std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > >::iterator iter=newIds.begin();
-							iter!=newIds.end();
-							++iter)
-						{
-							if(maxNewFeatures_ == 0  || added < maxNewFeatures_)
-							{
-								if(bundleAdjustment_>0)
-								{
-									if(lastFrame_->getWords().count(iter->second.first) == 1)
-									{
-										UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
-										iterBundlePosesRef->second += 1;
-
-										if(bundleWordReferences_.find(iter->second.first) == bundleWordReferences_.end())
-										{
-											std::map<int, cv::Point2f> framePt;
-											framePt.insert(std::make_pair(lastFrame_->id(), iter->second.second.first.pt));
-											bundleWordReferences_.insert(std::make_pair(iter->second.first, framePt));
-										}
-										else
-										{
-											bundleWordReferences_.find(iter->second.first)->second.insert(std::make_pair(lastFrame_->id(), iter->second.second.first.pt));
-										}
-									}
-								}
-
-								mapWords.insert(std::make_pair(iter->second.first, iter->second.second.first));
-								mapPoints.insert(std::make_pair(iter->second.first, util3d::transformPoint(iter->second.second.second.first, newFramePose)));
-								mapDescriptors.insert(std::make_pair(iter->second.first, iter->second.second.second.second));
-								++added;
-							}
-						}
-
-						// remove words in map if max size is reached
-						if((int)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 iterMapDescriptors = mapDescriptors.begin();
-							std::multimap<int, cv::KeyPoint>::iterator iterMapWords = mapWords.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())
-								{
-									std::map<int, std::map<int, cv::Point2f> >::iterator iterRef = bundleWordReferences_.find(iter->first);
-									if(iterRef != bundleWordReferences_.end())
-									{
-										for(std::map<int, cv::Point2f>::iterator iterKp = iterRef->second.begin(); iterKp != iterRef->second.end(); ++iterKp)
-										{
-											if(bundlePoseReferences_.find(iterKp->first) != bundlePoseReferences_.end())
-											{
-												bundlePoseReferences_.at(iterKp->first) -= 1;
-												if(bundlePoseReferences_.at(iterKp->first) <= regPipeline_->getMinVisualCorrespondences())
-												{
-													bundlePoses_.erase(iterKp->first);
-													bundleLinks_.erase(iterKp->first);
-													bundleModels_.erase(iterKp->first);
-													bundlePoseReferences_.erase(iterKp->first);
-													UDEBUG("bundlePoseReferences_ erased all words from cam %d", iterKp->first);
-												}
-											}
-										}
-										bundleWordReferences_.erase(iterRef);
-									}
-
-									mapPoints.erase(iter++);
-									mapDescriptors.erase(iterMapDescriptors++);
-									mapWords.erase(iterMapWords++);
-									++removed;
-								}
-								else
-								{
-									++iter;
-									++iterMapDescriptors;
-									++iterMapWords;
-								}
-							}
-						}
-						modified = true;
 					}
 
-					// Geometric
+					// sort by feature response
-					UDEBUG("scankeyframeThr=%f icpInliersRatio=%f", scanKeyFrameThr_, regInfo.icpInliersRatio);
+					std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > > newIds;
-					if(regPipeline_->isScanRequired() &&
+					UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
-						(scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_))
+					std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().begin();
+					std::multimap<int, cv::Mat>::const_iterator iterDesc = lastFrame_->getWordsDescriptors().begin();
+					for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin(); iter!=lastFrame_->getWords3().end(); ++iter, ++iter2D, ++iterDesc)
 					{
-						UINFO("Update local scan map %d (ratio=%f < %f)", lastFrame_->id(), regInfo.icpInliersRatio, scanKeyFrameThr_);
+						if(util3d::isFinite(iter->second))
-
-						UTimer tmpTimer;
-						if(lastFrame_->sensorData().laserScanRaw().cols)
 						{
-							pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan);
+							if(mapPoints.find(iter->first) == mapPoints.end()) // Point not in map
-							pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform());
-
-							pcl::IndicesPtr frameCloudNormalsIndices(new std::vector<int>);
-							int newPoints;
-							if(mapCloudNormals->size() && scanSubtractRadius_ > 0.0f)
 							{
-								frameCloudNormalsIndices = util3d::subtractFiltering(
+								newIds.insert(
-										frameCloudNormals,
+										std::make_pair(iter2D->second.response>0?1.0f/iter2D->second.response:0.0f,
-										pcl::IndicesPtr(new std::vector<int>),
+												std::make_pair(iter->first,
-										mapCloudNormals,
+														std::make_pair(iter2D->second,
-										pcl::IndicesPtr(new std::vector<int>),
+																std::make_pair(iter->second, iterDesc->second)))));
-										scanSubtractRadius_,
-										0.0f);
-								newPoints = frameCloudNormalsIndices->size();
 							}
-							else
+							else if(bundleAdjustment_>0)
 							{
-								newPoints = mapCloudNormals->size();
+								if(lastFrame_->getWords().count(iter->first) == 1)
-							}
-
-							if(newPoints)
-							{
-								scansBuffer_.push_back(std::make_pair(frameCloudNormals, frameCloudNormalsIndices));
-
-								//remove points if too big
-								UDEBUG("scansBuffer=%d, mapSize=%d newPoints=%d maxPoints=%d",
-										(int)scansBuffer_.size(),
-										int(mapCloudNormals->size()),
-										newPoints,
-										scanMaximumMapSize_);
-
-								if(newPoints < 20)
 								{
-									UWARN("The number of new scan points added to local odometry "
+									UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
-										  "map is low (%d), you may want to decrease the parameter \"%s\" "
+									iterBundlePosesRef->second += 1;
-										  "(current value=%f and ICP inliers ratio is %f)",
-											newPoints,
-											Parameters::kOdomScanKeyFrameThr().c_str(),
-											scanKeyFrameThr_,
-											regInfo.icpInliersRatio);
-								}
 
-								if(scansBuffer_.size() > 1 &&
+									if(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end())
-									int(mapCloudNormals->size() + newPoints) > scanMaximumMapSize_)
-								{
-									//regenerate the local map
-									mapCloudNormals->clear();
-									std::list<int> toRemove;
-									int i = int(scansBuffer_.size())-1;
-									for(; i>=0; --i)
 									{
-										int pointsToAdd = scansBuffer_[i].second->size()?scansBuffer_[i].second->size():scansBuffer_[i].first->size();
+										std::map<int, cv::Point2f> framePt;
-										if((int)mapCloudNormals->size() + pointsToAdd > scanMaximumMapSize_ ||
+										framePt.insert(std::make_pair(lastFrame_->id(), iter2D->second.pt));
-											i == 0)
+										bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
-										{
-											*mapCloudNormals += *scansBuffer_[i].first;
-											break;
-										}
-										else
-										{
-											if(scansBuffer_[i].second->size())
-											{
-												pcl::PointCloud<pcl::PointNormal> tmp;
-												pcl::copyPointCloud(*scansBuffer_[i].first, *scansBuffer_[i].second, tmp);
-												*mapCloudNormals += tmp;
-											}
-											else
-											{
-												*mapCloudNormals += *scansBuffer_[i].first;
-											}
-										}
-									}
-									// remove old clouds
-									if(i > 0)
-									{
-										std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansTmp(scansBuffer_.size()-i);
-										int oi = 0;
-										for(; i<(int)scansBuffer_.size(); ++i)
-										{
-											UASSERT(oi < (int)scansTmp.size());
-											scansTmp[oi++] = scansBuffer_[i];
-										}
-										scansBuffer_ = scansTmp;
-									}
-								}
-								else
-								{
-									// just append the last cloud
-									if(scansBuffer_.back().second->size())
-									{
-										pcl::PointCloud<pcl::PointNormal> tmp;
-										pcl::copyPointCloud(*scansBuffer_.back().first, *scansBuffer_.back().second, tmp);
-										*mapCloudNormals += tmp;
 									}
 									else
 									{
-										*mapCloudNormals += *scansBuffer_.back().first;
+										bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), iter2D->second.pt));
 									}
 								}
-								mapScan = util3d::laserScanFromPointCloud(*mapCloudNormals);
-								modified=true;
 							}
 						}
-						UDEBUG("Update local map = %fs", tmpTimer.ticks());
 					}
 
-					if(modified)
+					for(std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > >::iterator iter=newIds.begin();
+						iter!=newIds.end();
+						++iter)
 					{
-						*map_ = tmpMap;
+						if(maxNewFeatures_ == 0  || added < maxNewFeatures_)
+						{
+							if(bundleAdjustment_>0)
+							{
+								if(lastFrame_->getWords().count(iter->second.first) == 1)
+								{
+									UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
+									iterBundlePosesRef->second += 1;
 
-						map_->sensorData().setLaserScanRaw(mapScan, LaserScanInfo(0, 0));
+									if(bundleWordReferences_.find(iter->second.first) == bundleWordReferences_.end())
-						map_->setWords(mapWords);
+									{
-						map_->setWords3(mapPoints);
+										std::map<int, cv::Point2f> framePt;
-						map_->setWordsDescriptors(mapDescriptors);
+										framePt.insert(std::make_pair(lastFrame_->id(), iter->second.second.first.pt));
+										bundleWordReferences_.insert(std::make_pair(iter->second.first, framePt));
+									}
+									else
+									{
+										bundleWordReferences_.find(iter->second.first)->second.insert(std::make_pair(lastFrame_->id(), iter->second.second.first.pt));
+									}
+								}
+							}
+
+							mapWords.insert(std::make_pair(iter->second.first, iter->second.second.first));
+							mapPoints.insert(std::make_pair(iter->second.first, util3d::transformPoint(iter->second.second.second.first, newFramePose)));
+							mapDescriptors.insert(std::make_pair(iter->second.first, iter->second.second.second.second));
+							++added;
+						}
 					}
+
+					// remove words in map if max size is reached
+					if((int)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 iterMapDescriptors = mapDescriptors.begin();
+						std::multimap<int, cv::KeyPoint>::iterator iterMapWords = mapWords.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())
+							{
+								std::map<int, std::map<int, cv::Point2f> >::iterator iterRef = bundleWordReferences_.find(iter->first);
+								if(iterRef != bundleWordReferences_.end())
+								{
+									for(std::map<int, cv::Point2f>::iterator iterKp = iterRef->second.begin(); iterKp != iterRef->second.end(); ++iterKp)
+									{
+										if(bundlePoseReferences_.find(iterKp->first) != bundlePoseReferences_.end())
+										{
+											bundlePoseReferences_.at(iterKp->first) -= 1;
+											if(bundlePoseReferences_.at(iterKp->first) <= regPipeline_->getMinVisualCorrespondences())
+											{
+												bundlePoses_.erase(iterKp->first);
+												bundleLinks_.erase(iterKp->first);
+												bundleModels_.erase(iterKp->first);
+												bundlePoseReferences_.erase(iterKp->first);
+												UDEBUG("bundlePoseReferences_ erased all words from cam %d", iterKp->first);
+											}
+										}
+									}
+									bundleWordReferences_.erase(iterRef);
+								}
+
+								mapPoints.erase(iter++);
+								mapDescriptors.erase(iterMapDescriptors++);
+								mapWords.erase(iterMapWords++);
+								++removed;
+							}
+							else
+							{
+								++iter;
+								++iterMapDescriptors;
+								++iterMapWords;
+							}
+						}
+					}
+					modified = true;
 				}
-				else
+
+				// Geometric
+				UDEBUG("scankeyframeThr=%f icpInliersRatio=%f", scanKeyFrameThr_, regInfo.icpInliersRatio);
+				if(regPipeline_->isScanRequired() &&
+					(scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_))
 				{
-					// fixed local map, don't update with the new signature
+					UINFO("Update local scan map %d (ratio=%f < %f)", lastFrame_->id(), regInfo.icpInliersRatio, scanKeyFrameThr_);
+
+					UTimer tmpTimer;
+					if(lastFrame_->sensorData().laserScanRaw().cols)
+					{
+						pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan);
+						pcl::PointCloud<pcl::PointNormal>::Ptr frameCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform());
+
+						pcl::IndicesPtr frameCloudNormalsIndices(new std::vector<int>);
+						int newPoints;
+						if(mapCloudNormals->size() && scanSubtractRadius_ > 0.0f)
+						{
+							frameCloudNormalsIndices = util3d::subtractFiltering(
+									frameCloudNormals,
+									pcl::IndicesPtr(new std::vector<int>),
+									mapCloudNormals,
+									pcl::IndicesPtr(new std::vector<int>),
+									scanSubtractRadius_,
+									0.0f);
+							newPoints = frameCloudNormalsIndices->size();
+						}
+						else
+						{
+							newPoints = mapCloudNormals->size();
+						}
+
+						if(newPoints)
+						{
+							scansBuffer_.push_back(std::make_pair(frameCloudNormals, frameCloudNormalsIndices));
+
+							//remove points if too big
+							UDEBUG("scansBuffer=%d, mapSize=%d newPoints=%d maxPoints=%d",
+									(int)scansBuffer_.size(),
+									int(mapCloudNormals->size()),
+									newPoints,
+									scanMaximumMapSize_);
+
+							if(newPoints < 20)
+							{
+								UWARN("The number of new scan points added to local odometry "
+									  "map is low (%d), you may want to decrease the parameter \"%s\" "
+									  "(current value=%f and ICP inliers ratio is %f)",
+										newPoints,
+										Parameters::kOdomScanKeyFrameThr().c_str(),
+										scanKeyFrameThr_,
+										regInfo.icpInliersRatio);
+							}
+
+							if(scansBuffer_.size() > 1 &&
+								int(mapCloudNormals->size() + newPoints) > scanMaximumMapSize_)
+							{
+								//regenerate the local map
+								mapCloudNormals->clear();
+								std::list<int> toRemove;
+								int i = int(scansBuffer_.size())-1;
+								for(; i>=0; --i)
+								{
+									int pointsToAdd = scansBuffer_[i].second->size()?scansBuffer_[i].second->size():scansBuffer_[i].first->size();
+									if((int)mapCloudNormals->size() + pointsToAdd > scanMaximumMapSize_ ||
+										i == 0)
+									{
+										*mapCloudNormals += *scansBuffer_[i].first;
+										break;
+									}
+									else
+									{
+										if(scansBuffer_[i].second->size())
+										{
+											pcl::PointCloud<pcl::PointNormal> tmp;
+											pcl::copyPointCloud(*scansBuffer_[i].first, *scansBuffer_[i].second, tmp);
+											*mapCloudNormals += tmp;
+										}
+										else
+										{
+											*mapCloudNormals += *scansBuffer_[i].first;
+										}
+									}
+								}
+								// remove old clouds
+								if(i > 0)
+								{
+									std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansTmp(scansBuffer_.size()-i);
+									int oi = 0;
+									for(; i<(int)scansBuffer_.size(); ++i)
+									{
+										UASSERT(oi < (int)scansTmp.size());
+										scansTmp[oi++] = scansBuffer_[i];
+									}
+									scansBuffer_ = scansTmp;
+								}
+							}
+							else
+							{
+								// just append the last cloud
+								if(scansBuffer_.back().second->size())
+								{
+									pcl::PointCloud<pcl::PointNormal> tmp;
+									pcl::copyPointCloud(*scansBuffer_.back().first, *scansBuffer_.back().second, tmp);
+									*mapCloudNormals += tmp;
+								}
+								else
+								{
+									*mapCloudNormals += *scansBuffer_.back().first;
+								}
+							}
+							mapScan = util3d::laserScanFromPointCloud(*mapCloudNormals);
+							modified=true;
+						}
+					}
+					UDEBUG("Update local map = %fs", tmpTimer.ticks());
+				}
+
+				if(modified)
+				{
+					*map_ = tmpMap;
+
+					map_->sensorData().setLaserScanRaw(mapScan, LaserScanInfo(0, 0));
+					map_->setWords(mapWords);
+					map_->setWords3(mapPoints);
+					map_->setWordsDescriptors(mapDescriptors);
 				}
 			}
 
@@ -690,72 +589,69 @@ Transform OdometryF2M::computeTransform(
 				if ((int)lastFrame_->getWords3().size() >= regPipeline_->getMinVisualCorrespondences())
 				{
 					frameValid = true;
-					if (fixedMapPath_.empty())
+					// update local map
+					UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
+					UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
+
+					std::multimap<int, cv::KeyPoint> words;
+					std::multimap<int, cv::Point3f> transformedPoints;
+					std::multimap<int, cv::Mat> descriptors;
+					UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWordsDescriptors().size());
+					std::multimap<int, cv::KeyPoint>::const_iterator wordsIter = lastFrame_->getWords().begin();
+					std::multimap<int, cv::Mat>::const_iterator descIter = lastFrame_->getWordsDescriptors().begin();
+					for (std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
+						iter != lastFrame_->getWords3().end();
+						++iter, ++descIter, ++wordsIter)
 					{
-						// update local map
+						if (util3d::isFinite(iter->second))
-						UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
-						UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
-
-						std::multimap<int, cv::KeyPoint> words;
-						std::multimap<int, cv::Point3f> transformedPoints;
-						std::multimap<int, cv::Mat> descriptors;
-						UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWordsDescriptors().size());
-						std::multimap<int, cv::KeyPoint>::const_iterator wordsIter = lastFrame_->getWords().begin();
-						std::multimap<int, cv::Mat>::const_iterator descIter = lastFrame_->getWordsDescriptors().begin();
-						for (std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
-							iter != lastFrame_->getWords3().end();
-							++iter, ++descIter, ++wordsIter)
 						{
-							if (util3d::isFinite(iter->second))
+							words.insert(*wordsIter);
-							{
+							transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, newFramePose)));
-								words.insert(*wordsIter);
+							descriptors.insert(*descIter);
-								transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, newFramePose)));
-								descriptors.insert(*descIter);
-							}
 						}
-
-						if(bundleAdjustment_>0)
-						{
-							// update bundleWordReferences_: used for bundle adjustment
-							for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
-							{
-								if(words.count(iter->first) == 1)
-								{
-									UASSERT(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end());
-									std::map<int, cv::Point2f> framePt;
-									framePt.insert(std::make_pair(lastFrame_->id(), iter->second.pt));
-									bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
-								}
-							}
-							bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), (int)bundleWordReferences_.size()));
-
-							CameraModel model;
-							if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
-							{
-								model = lastFrame_->sensorData().cameraModels()[0];
-							}
-							else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
-							{
-								model = lastFrame_->sensorData().stereoCameraModel().left();
-							}
-							UASSERT(model.isValidForProjection());
-							UASSERT_MSG(lastFrame_->id() > 0, uFormat("Input data should have ID greater than 0 when odometry bundle adjustment is enabled!").c_str());
-							bundleModels_.insert(std::make_pair(lastFrame_->id(), model));
-							bundlePoses_.insert(std::make_pair(lastFrame_->id(), newFramePose));
-							bundleLinks_.insert(std::make_pair(lastFrame_->id(), Link(0, lastFrame_->id(), Link::kNeighbor, newFramePose, 0.000001, 0.00001)));
-
-							//origin
-							bundlePoses_.insert(std::make_pair(0, Transform::getIdentity()));
-							bundleModels_.insert(std::make_pair(0, model));
-						}
-
-						map_->setWords(words);
-						map_->setWords3(transformedPoints);
-						map_->setWordsDescriptors(descriptors);
-
-						map_->sensorData().setCameraModels(lastFrame_->sensorData().cameraModels());
-						map_->sensorData().setStereoCameraModel(lastFrame_->sensorData().stereoCameraModel());
 					}
+
+					if(bundleAdjustment_>0)
+					{
+						// update bundleWordReferences_: used for bundle adjustment
+						for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
+						{
+							if(words.count(iter->first) == 1)
+							{
+								UASSERT(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end());
+								std::map<int, cv::Point2f> framePt;
+								framePt.insert(std::make_pair(lastFrame_->id(), iter->second.pt));
+								bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
+							}
+						}
+						bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), (int)bundleWordReferences_.size()));
+
+						CameraModel model;
+						if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
+						{
+							model = lastFrame_->sensorData().cameraModels()[0];
+						}
+						else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
+						{
+							model = lastFrame_->sensorData().stereoCameraModel().left();
+						}
+						UASSERT(model.isValidForProjection());
+						UASSERT_MSG(lastFrame_->id() > 0, uFormat("Input data should have ID greater than 0 when odometry bundle adjustment is enabled!").c_str());
+						bundleModels_.insert(std::make_pair(lastFrame_->id(), model));
+						bundlePoses_.insert(std::make_pair(lastFrame_->id(), newFramePose));
+						bundleLinks_.insert(std::make_pair(lastFrame_->id(), Link(0, lastFrame_->id(), Link::kNeighbor, newFramePose, 0.000001, 0.00001)));
+
+						//origin
+						bundlePoses_.insert(std::make_pair(0, Transform::getIdentity()));
+						bundleModels_.insert(std::make_pair(0, model));
+					}
+
+					map_->setWords(words);
+					map_->setWords3(transformedPoints);
+					map_->setWordsDescriptors(descriptors);
+
+					map_->sensorData().setCameraModels(lastFrame_->sensorData().cameraModels());
+					map_->sensorData().setStereoCameraModel(lastFrame_->sensorData().stereoCameraModel());
 				}
 				else
 				{
@@ -767,12 +663,9 @@ Transform OdometryF2M::computeTransform(
 				if (lastFrame_->sensorData().laserScanRaw().cols)
 				{
 					frameValid = true;
-					if (fixedMapPath_.empty())
+					pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform());
-					{
+					scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
-						pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform());
+					map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), LaserScanInfo(0,0));
-						scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
-						map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), LaserScanInfo(0,0));
-					}
 				}
 				else
 				{

corelib/src/OdometryThread.cpp

@@ -109,7 +109,7 @@ void OdometryThread::mainLoop()
 
 void OdometryThread::addData(const SensorData & data)
 {
-	if(dynamic_cast<OdometryMono*>(_odometry) == 0 && dynamic_cast<OdometryF2M*>(_odometry) == 0)
+	if(dynamic_cast<OdometryMono*>(_odometry) == 0)
 	{
 		if(data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection()))
 		{
@@ -119,7 +119,7 @@ void OdometryThread::addData(const SensorData & data)
 	}
 	else
 	{
-		// Mono and BOW can accept RGB only
+		// Mono can accept RGB only
 		if(data.imageRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection()))
 		{
 			ULOGGER_ERROR("Missing some information (image empty or missing calibration)!?");

corelib/src/Parameters.cpp

@@ -226,6 +226,7 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 
 		// 0.11.12
 		removedParameters_.insert(std::make_pair("Optimizer/Slam2D",              std::make_pair(true,  Parameters::kRegForce3DoF())));
+		removedParameters_.insert(std::make_pair("OdomF2M/FixedMapPath",          std::make_pair(false,  "")));
 
 		// 0.11.10 typos
 		removedParameters_.insert(std::make_pair("Grid/FlatObstaclesDetected",    std::make_pair(true,  Parameters::kGridFlatObstacleDetected())));
@@ -242,13 +243,13 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 
 		// 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("OdomLocalMap/FixedMapPath",     std::make_pair(false, "")));
 		removedParameters_.insert(std::make_pair("OdomF2F/GuessMotion",           std::make_pair(true, Parameters::kOdomGuessMotion())));
 		removedParameters_.insert(std::make_pair("OdomF2F/KeyFrameThr",           std::make_pair(false, Parameters::kOdomKeyFrameThr())));
 
 		// 0.11.0
 		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("OdomBow/FixedLocalMapPath",     std::make_pair(false, "")));
 		removedParameters_.insert(std::make_pair("OdomFlow/KeyFrameThr",          std::make_pair(false, Parameters::kOdomKeyFrameThr())));
 		removedParameters_.insert(std::make_pair("OdomFlow/GuessMotion",          std::make_pair(true, Parameters::kOdomGuessMotion())));
 

corelib/src/RegistrationVis.cpp

@@ -236,6 +236,7 @@ Transform RegistrationVis::computeTransformationImpl(
 				fromSignature.sensorData().descriptors().rows == 0 ||
 				fromSignature.getWordsDescriptors().size() == 0);
 		UASSERT((toSignature.getWords().empty() && toSignature.getWords3().empty())||
+				(toSignature.getWords().size() && toSignature.getWords3().empty())||
 				(toSignature.getWords().size() == toSignature.getWords3().size()));
 		UASSERT((int)toSignature.sensorData().keypoints().size() == toSignature.sensorData().descriptors().rows ||
 				toSignature.getWords().size() == toSignature.getWordsDescriptors().size() ||
@@ -601,7 +602,7 @@ Transform RegistrationVis::computeTransformationImpl(
 						   "is maybe a problem with the logic above (getWords3() should be null or equal to kptsTo).");
 				}
 				kptsTo3D = detector->generateKeypoints3D(toSignature.sensorData(), kptsTo);
-				if(detector->getMinDepth() > 0.0f || detector->getMaxDepth() > 0.0f)
+				if(kptsTo3D.size() && (detector->getMinDepth() > 0.0f || detector->getMaxDepth() > 0.0f))
 				{
 					UDEBUG("");
 					//remove all keypoints/descriptors with no valid 3D points

corelib/src/Rtabmap.cpp

@@ -810,7 +810,7 @@ void Rtabmap::resetMemory()
 //============================================================
 bool Rtabmap::process(
 		const SensorData & data,
-		const Transform & odomPose,
+		Transform odomPose,
 		const cv::Mat & covariance)
 {
 	UDEBUG("");
@@ -883,9 +883,20 @@ bool Rtabmap::process(
 	{
 		if(odomPose.isNull())
 		{
-			UERROR("RGB-D SLAM mode is enabled and no odometry is provided. "
+			if(_memory->isIncremental())
-				   "Image %d is ignored!", data.id());
+			{
-			return false;
+				UERROR("RGB-D SLAM mode is enabled, memory is incremental but no odometry is provided. "
+					   "Image %d is ignored!", data.id());
+				return false;
+			}
+			else // fake localization
+			{
+				if(_lastLocalizationPose.isNull())
+				{
+					_lastLocalizationPose = Transform::getIdentity();
+				}
+				odomPose = _mapCorrection.inverse() * _lastLocalizationPose;
+			}
 		}
 		else if(_memory->isIncremental()) // only in mapping mode
 		{
@@ -1185,10 +1196,11 @@ bool Rtabmap::process(
 					}
 
 					// For proximity by time, correspondences should be already enough precise, so don't recompute them
-					Transform transform = _memory->computeTransform(signature->id(), *iter, guess, &info, true);
+					Transform transform = _memory->computeTransform(*iter, signature->id(), guess, &info, true);
 
 					if(!transform.isNull())
 					{
+						transform = transform.inverse();
 						UDEBUG("Add local loop closure in TIME (%d->%d) %s",
 								signature->id(),
 								*iter,
@@ -1740,7 +1752,7 @@ bool Rtabmap::process(
 		info.variance = 1.0f;
 		if(_rgbdSlamMode)
 		{
-			transform = _memory->computeTransform(signature->id(), _loopClosureHypothesis.first, Transform(), &info);
+			transform = _memory->computeTransform(_loopClosureHypothesis.first, signature->id(), Transform(), &info);
 			loopClosureVisualInliers = info.inliers;
 			rejectedHypothesis = transform.isNull();
 			if(rejectedHypothesis)
@@ -1748,6 +1760,10 @@ bool Rtabmap::process(
 				UWARN("Rejected loop closure %d -> %d: %s",
 						_loopClosureHypothesis.first, signature->id(), info.rejectedMsg.c_str());
 			}
+			else
+			{
+				transform = transform.inverse();
+			}
 		}
 		if(!rejectedHypothesis)
 		{
@@ -1848,9 +1864,10 @@ bool Rtabmap::process(
 							++localVisualPathsChecked;
 							RegistrationInfo info;
 							Transform guess = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(nearestId);
-							Transform transform = _memory->computeTransform(signature->id(), nearestId, guess, &info);
+							Transform transform = _memory->computeTransform(nearestId, signature->id(), guess, &info);
 							if(!transform.isNull())
 							{
+								transform = transform.inverse();
 								if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)
 								{
 									UINFO("[Visual] Add local loop closure in SPACE (%d->%d) %s",
@@ -2086,9 +2103,9 @@ bool Rtabmap::process(
 				// Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
 				// parameters just changed state, we should put back all poses without map correction.
 				Transform oldPose = _optimizedPoses.at(localizationLinks.begin()->first);
+				Transform mapCorrectionInv = _mapCorrection.inverse();
 				Transform u = signature->getPose() * localizationLinks.begin()->second.transform();
 				Transform up = u * oldPose.inverse();
-				Transform mapCorrectionInv = _mapCorrection.inverse();
 				for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
 				{
 					iter->second = mapCorrectionInv * up * iter->second;
@@ -2197,6 +2214,7 @@ bool Rtabmap::process(
 		}
 
 		// Update map correction, it should be identify when optimizing from the last node
+		UASSERT(_optimizedPoses.find(signature->id()) != _optimizedPoses.end());
 		_mapCorrection = _optimizedPoses.at(signature->id()) * signature->getPose().inverse();
 		_lastLocalizationPose = _optimizedPoses.at(signature->id()); // update
 		if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
@@ -2333,6 +2351,11 @@ bool Rtabmap::process(
 	}
 
 	Signature lastSignatureData(signature->id());
+	Transform lastSignatureOptimizedPose;
+	if(_optimizedPoses.find(signature->id()) != _optimizedPoses.end())
+	{
+		lastSignatureOptimizedPose = _optimizedPoses.at(signature->id());
+	}
 	if(_publishLastSignatureData)
 	{
 		lastSignatureData = *signature;
@@ -2537,28 +2560,29 @@ bool Rtabmap::process(
 		UDEBUG("Get all node infos...");
 		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 		{
-			Transform odomPose;
+			Transform odomPoseLocal;
 			int weight = -1;
 			int mapId = -1;
 			std::string label;
 			double stamp = 0;
 			Transform groundTruth;
 			std::vector<unsigned char> userData;
-			_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, groundTruth, false);
+			_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, false);
 			signatures.insert(std::make_pair(iter->first,
 					Signature(iter->first,
 							mapId,
 							weight,
 							stamp,
 							label,
-							odomPose,
+							odomPoseLocal,
 							groundTruth)));
 		}
+		localGraphSize = (int)poses.size();
+		poses.insert(std::make_pair(lastSignatureData.id(), lastSignatureOptimizedPose)); // in case we are in localization
 		statistics_.setPoses(poses);
 		statistics_.setConstraints(constraints);
 		statistics_.setSignatures(signatures);
 		statistics_.addStatistic(Statistics::kMemoryLocal_graph_size(), poses.size());
-		localGraphSize = (int)poses.size();
 		UDEBUG("");
 	}
 
@@ -3210,13 +3234,13 @@ void Rtabmap::get3DMap(
 
 		for(std::set<int>::iterator iter = ids.begin(); iter!=ids.end(); ++iter)
 		{
-			Transform odomPose;
+			Transform odomPoseLocal;
 			int weight = -1;
 			int mapId = -1;
 			std::string label;
 			double stamp = 0;
 			Transform groundTruth;
-			_memory->getNodeInfo(*iter, odomPose, mapId, weight, label, stamp, groundTruth, true);
+			_memory->getNodeInfo(*iter, odomPoseLocal, mapId, weight, label, stamp, groundTruth, true);
 			SensorData data = _memory->getNodeData(*iter);
 			data.setId(*iter);
 			std::multimap<int, cv::KeyPoint> words;
@@ -3229,7 +3253,7 @@ void Rtabmap::get3DMap(
 							weight,
 							stamp,
 							label,
-							odomPose,
+							odomPoseLocal,
 							groundTruth,
 							data)));
 			signatures.at(*iter).setWords(words);
@@ -3280,20 +3304,20 @@ void Rtabmap::getGraph(
 		{
 			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 			{
-				Transform odomPose;
+				Transform odomPoseLocal;
 				int weight = -1;
 				int mapId = -1;
 				std::string label;
 				double stamp = 0;
 				Transform groundTruth;
-				_memory->getNodeInfo(iter->first, odomPose, mapId, weight, label, stamp, groundTruth, global);
+				_memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, global);
 				signatures->insert(std::make_pair(iter->first,
 						Signature(iter->first,
 							mapId,
 							weight,
 							stamp,
 							label,
-							odomPose,
+							odomPoseLocal,
 							groundTruth)));
 
 				std::multimap<int, cv::KeyPoint> words;

corelib/src/RtabmapThread.cpp

@@ -336,7 +336,7 @@ void RtabmapThread::handleEvent(UEvent* event)
 			{
 				if (_rtabmap->isRGBDMode())
 				{
-					if (!e->info().odomPose.isNull())
+					if (!e->info().odomPose.isNull() || (_rtabmap->getMemory() && !_rtabmap->getMemory()->isIncremental()))
 					{
 						this->addData(OdometryEvent(e->data(), e->info().odomPose, e->info().odomCovariance));
 					}
@@ -347,7 +347,7 @@ void RtabmapThread::handleEvent(UEvent* event)
 				}
 				else
 				{ 
-					this->addData(OdometryEvent(e->data(), Transform(), 1, 1));
+					this->addData(OdometryEvent(e->data(), e->info().odomPose, e->info().odomCovariance));
 				}
 				
 			}
@@ -356,7 +356,7 @@ void RtabmapThread::handleEvent(UEvent* event)
 		{
 			UDEBUG("OdometryEvent");
 			OdometryEvent * e = (OdometryEvent*)event;
-			if(!e->pose().isNull())
+			if(!e->pose().isNull() || (_rtabmap->getMemory() && !_rtabmap->getMemory()->isIncremental()))
 			{
 				this->addData(*e);
 			}
@@ -630,7 +630,10 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 		_transVariance = 0;
 		while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
 		{
-			ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
+			if(_rate > 0.0f)
+			{
+				ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
+			}
 			_dataBuffer.pop_front();
 			notify = false;
 		}

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -130,6 +130,7 @@ public:
 	QString loadCustomConfig(const QString & section, const QString & key);
 
 	rtabmap::ParametersMap getAllParameters() const;
+	std::string getParameter(const std::string & key) const;
 	void updateParameters(const ParametersMap & parameters);
 
 	//General panel
@@ -149,6 +150,7 @@ public:
 	bool notifyWhenNewGlobalPathIsReceived() const;
 	int getOdomQualityWarnThr() const;
 	bool isPosteriorGraphView() const;
+	bool isOdomDisabled() const;
 
 	bool isGraphsShown() const;
 	bool isLabelsShown() const;
@@ -293,7 +295,6 @@ private slots:
 	void useOdomFeatures();
 	void changeWorkingDirectory();
 	void changeDictionaryPath();
-	void changeOdomBowFixedLocalMapPath();
 	void readSettingsEnd();
 	void setupTreeView();
 	void updateBasicParameter();

guilib/src/MainWindow.cpp

@@ -926,7 +926,6 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 	if(!pose.isNull() && (_ui->dockWidget_cloudViewer->isVisible() || _ui->graphicsView_graphView->isVisible()))
 	{
 		_lastOdomPose = pose;
-		_odometryReceived = true;
 	}
 
 	if(_ui->dockWidget_cloudViewer->isVisible())
@@ -1071,19 +1070,22 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 				// F2M: scan local map
 				if(!odom.info().localScanMap.empty())
 				{
-					pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
+					if(!lost)
-					cloud = util3d::laserScanToPointCloudNormal(odom.info().localScanMap);
-					if(!_cloudViewer->addCloud("scanMapOdom", cloud, _odometryCorrection, Qt::blue))
 					{
-						UERROR("Adding scanMapOdom to viewer failed!");
+						pcl::PointCloud<pcl::PointNormal>::Ptr cloud;
-					}
+						cloud = util3d::laserScanToPointCloudNormal(odom.info().localScanMap);
-					else
+						if(!_cloudViewer->addCloud("scanMapOdom", cloud, _odometryCorrection, Qt::blue))
-					{
+						{
-						_cloudViewer->setCloudVisibility("scanMapOdom", true);
+							UERROR("Adding scanMapOdom to viewer failed!");
-						_cloudViewer->setCloudOpacity("scanMapOdom", _preferencesDialog->getScanOpacity(1));
+						}
-						_cloudViewer->setCloudPointSize("scanMapOdom", _preferencesDialog->getScanPointSize(1));
+						else
-						scanUpdated = true;
+						{
+							_cloudViewer->setCloudVisibility("scanMapOdom", true);
+							_cloudViewer->setCloudOpacity("scanMapOdom", _preferencesDialog->getScanOpacity(1));
+							_cloudViewer->setCloudPointSize("scanMapOdom", _preferencesDialog->getScanPointSize(1));
+						}
 					}
+					scanUpdated = true;
 				}
 				// scan cloud
 				if(!odom.data().laserScanRaw().empty())
@@ -1117,9 +1119,9 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 			}
 
 			// 3d features
-			if(_preferencesDialog->isFeaturesShown(1))
+			if(_preferencesDialog->isFeaturesShown(1) && !odom.info().localMap.empty())
 			{
-				if(!odom.info().localMap.empty())
+				if(!lost)
 				{
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 					cloud->resize(odom.info().localMap.size());
@@ -1140,9 +1142,8 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 					_cloudViewer->addCloud("featuresOdom", cloud, _odometryCorrection);
 					_cloudViewer->setCloudVisibility("featuresOdom", true);
 					_cloudViewer->setCloudPointSize("featuresOdom", _preferencesDialog->getFeaturesPointSize(1));
-
-					featuresUpdated = true;
 				}
+				featuresUpdated = true;
 			}
 		}
 		if(!dataIgnored)
@@ -1168,6 +1169,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
 
 	if(!odom.pose().isNull())
 	{
+		_odometryReceived = true;
 		// update camera position
 		_cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose());
 
@@ -1434,7 +1436,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			signature = stat.getSignatures().at(stat.refImageId());
 			signature.sensorData().uncompressData(); // make sure data are uncompressed
 
-			if(!smallMovement)
+			if(!smallMovement && uStr2Bool(_preferencesDialog->getParameter(Parameters::kMemIncrementalMemory())))
 			{
 				_cachedSignatures.insert(signature.id(), signature);
 				_cachedMemoryUsage += signature.sensorData().getMemoryUsed();
@@ -2456,7 +2458,7 @@ void MainWindow::updateMapCloud(
 			UDEBUG("");
 			_cloudViewer->setCloudVisibility("scanMapOdom", false);
 		}
-		else
+		else if(_cloudViewer->getBackgroundColor() != Qt::darkRed) // not lost
 		{
 			UDEBUG("");
 			_cloudViewer->updateCloudPose("scanMapOdom", _odometryCorrection);
@@ -2471,7 +2473,7 @@ void MainWindow::updateMapCloud(
 			UDEBUG("");
 			_cloudViewer->setCloudVisibility("featuresOdom", false);
 		}
-		else
+		else if(_cloudViewer->getBackgroundColor() != Qt::darkRed) // not lost
 		{
 			UDEBUG("");
 			_cloudViewer->updateCloudPose("featuresOdom", _odometryCorrection);
@@ -4236,7 +4238,7 @@ void MainWindow::startDetection()
 				_odomThread = 0;
 			}
 
-			if(!camera->odomProvided())
+			if(!camera->odomProvided() && !_preferencesDialog->isOdomDisabled())
 			{
 				Odometry * odom = Odometry::create(parameters);
 				_odomThread = new OdometryThread(odom, _preferencesDialog->getOdomBufferSize());
@@ -4282,6 +4284,13 @@ void MainWindow::startDetection()
 	_occupancyGrid->clear();
 	_occupancyGrid->parseParameters(parameters);
 
+	// clear odometry visual stuff
+	_cloudViewer->removeCloud("cloudOdom");
+	_cloudViewer->removeCloud("scanOdom");
+	_cloudViewer->removeCloud("scanMapOdom");
+	_cloudViewer->removeCloud("featuresOdom");
+	_cloudViewer->setBackgroundColor(_cloudViewer->getDefaultBackgroundColor());
+
 	emit stateChanged(kDetecting);
 }
 

guilib/src/PreferencesDialog.cpp

@@ -291,6 +291,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->checkBox_notifyWhenNewGlobalPathIsReceived, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->spinBox_odomQualityWarnThr, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 	connect(_ui->checkBox_posteriorGraphView, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
+	connect(_ui->checkbox_odomDisabled, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteGeneralPanel()));
 
 	// Cloud rendering panel
 	_3dRenderingShowClouds.resize(2);
@@ -842,8 +843,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->spinBox_odom_f2m_maxNewFeatures->setObjectName(Parameters::kOdomF2MMaxNewFeatures().c_str());
 	_ui->spinBox_odom_f2m_scanMaxSize->setObjectName(Parameters::kOdomF2MScanMaxSize().c_str());
 	_ui->doubleSpinBox_odom_f2m_scanRadius->setObjectName(Parameters::kOdomF2MScanSubtractRadius().c_str());
-	_ui->odom_fixedLocalMapPath->setObjectName(Parameters::kOdomF2MFixedMapPath().c_str());
-	connect(_ui->toolButton_odomBowFixedLocalMap, SIGNAL(clicked()), this, SLOT(changeOdomBowFixedLocalMapPath()));
 	_ui->odom_f2m_bundleStrategy->setObjectName(Parameters::kOdomF2MBundleAdjustment().c_str());
 	_ui->odom_f2m_bundleMaxFrames->setObjectName(Parameters::kOdomF2MBundleAdjustmentMaxFrames().c_str());
 
@@ -1212,6 +1211,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->checkBox_imageHighestHypShown->setChecked(false);
 		_ui->spinBox_odomQualityWarnThr->setValue(50);
 		_ui->checkBox_posteriorGraphView->setChecked(true);
+		_ui->checkbox_odomDisabled->setChecked(false);
 	}
 	else if(groupBox->objectName() == _ui->groupBox_cloudRendering1->objectName())
 	{
@@ -1594,6 +1594,7 @@ void PreferencesDialog::readGuiSettings(const QString & filePath)
 	_ui->checkBox_notifyWhenNewGlobalPathIsReceived->setChecked(settings.value("notifyNewGlobalPath", _ui->checkBox_notifyWhenNewGlobalPathIsReceived->isChecked()).toBool());
 	_ui->spinBox_odomQualityWarnThr->setValue(settings.value("odomQualityThr", _ui->spinBox_odomQualityWarnThr->value()).toInt());
 	_ui->checkBox_posteriorGraphView->setChecked(settings.value("posteriorGraphView", _ui->checkBox_posteriorGraphView->isChecked()).toBool());
+	_ui->checkbox_odomDisabled->setChecked(settings.value("odomDisabled", _ui->checkbox_odomDisabled->isChecked()).toBool());
 
 	for(int i=0; i<2; ++i)
 	{
@@ -1971,6 +1972,7 @@ void PreferencesDialog::writeGuiSettings(const QString & filePath) const
 	settings.setValue("notifyNewGlobalPath",  _ui->checkBox_notifyWhenNewGlobalPathIsReceived->isChecked());
 	settings.setValue("odomQualityThr",       _ui->spinBox_odomQualityWarnThr->value());
 	settings.setValue("posteriorGraphView",   _ui->checkBox_posteriorGraphView->isChecked());
+	settings.setValue("odomDisabled",         _ui->checkbox_odomDisabled->isChecked());
 
 	for(int i=0; i<2; ++i)
 	{
@@ -2388,6 +2390,15 @@ bool PreferencesDialog::validateForm()
 		_ui->fastThresholdMin->setValue(_ui->fastThreshold->value());
 	}
 
+	if(_ui->checkbox_odomDisabled->isChecked() &&
+		_ui->general_checkBox_SLAM_mode->isChecked() &&
+		_ui->general_checkBox_activateRGBD->isChecked())
+	{
+		QMessageBox::warning(this, tr("Parameter warning"),
+				tr("Odometry is disabled but incremental RGB-D SLAM is activated! Re-enabling odometry."));
+		_ui->checkbox_odomDisabled->setChecked(false);
+	}
+
 	return true;
 }
 
@@ -2699,6 +2710,17 @@ rtabmap::ParametersMap PreferencesDialog::getAllParameters() const
 	return parameters;
 }
 
+std::string PreferencesDialog::getParameter(const std::string & key) const
+{
+	if(_modifiedParameters.find(key) != _modifiedParameters.end())
+	{
+		return _modifiedParameters.at(key);
+	}
+
+	UASSERT(_parameters.find(key) != _parameters.end());
+	return _parameters.at(key);
+}
+
 void PreferencesDialog::updateParameters(const ParametersMap & parameters)
 {
 	if(parameters.size())
@@ -3642,23 +3664,6 @@ void PreferencesDialog::changeDictionaryPath()
 	}
 }
 
-void PreferencesDialog::changeOdomBowFixedLocalMapPath()
-{
-	QString path;
-	if(_ui->odom_fixedLocalMapPath->text().isEmpty())
-	{
-		path = QFileDialog::getOpenFileName(this, tr("Database"), this->getWorkingDirectory(), tr("RTAB-Map database files (*.db)"));
-	}
-	else
-	{
-		path = QFileDialog::getOpenFileName(this, tr("Database"), _ui->odom_fixedLocalMapPath->text(), tr("RTAB-Map database files (*.db)"));
-	}
-	if(!path.isEmpty())
-	{
-		_ui->odom_fixedLocalMapPath->setText(path);
-	}
-}
-
 void PreferencesDialog::updateSourceGrpVisibility()
 {
 	_ui->groupBox_sourceRGBD->setVisible(_ui->comboBox_sourceType->currentIndex() == 0);
@@ -3776,6 +3781,10 @@ bool PreferencesDialog::isPosteriorGraphView() const
 {
 	return _ui->checkBox_posteriorGraphView->isChecked();
 }
+bool PreferencesDialog::isOdomDisabled() const
+{
+	return _ui->checkbox_odomDisabled->isChecked();
+}
 
 bool PreferencesDialog::isCloudsShown(int index) const
 {

guilib/src/ui/preferencesDialog.ui

@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>4</number>
+                <number>0</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -98,6 +98,42 @@
                    <layout class="QVBoxLayout" name="verticalLayout_28">
                     <item>
                      <layout class="QGridLayout" name="gridLayout_39" columnstretch="0,1">
+                      <item row="3" column="1">
+                       <widget class="QLabel" name="label_247">
+                        <property name="text">
+                         <string>Use time for figures' x-axis. Otherwise, sensor data IDs are used for camera/odometry info and for all mapping statistics, node IDs are used.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="5" column="0">
+                       <widget class="QCheckBox" name="checkBox_notifyWhenNewGlobalPathIsReceived">
+                        <property name="text">
+                         <string/>
+                        </property>
+                        <property name="checked">
+                         <bool>true</bool>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="1" column="1">
+                       <widget class="QLabel" name="label_313">
+                        <property name="text">
+                         <string>Insert created clouds in the GUI cache to avoid regenerating clouds on export.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="0" column="0">
                        <widget class="QCheckBox" name="general_checkBox_imagesKept">
                         <property name="text">
@@ -121,19 +157,6 @@
                         </property>
                        </widget>
                       </item>
-                      <item row="1" column="1">
-                       <widget class="QLabel" name="label_313">
-                        <property name="text">
-                         <string>Insert created clouds in the GUI cache to avoid regenerating clouds on export.</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="general_checkBox_cloudsKept">
                         <property name="text">
@@ -177,16 +200,6 @@
                         </property>
                        </widget>
                       </item>
-                      <item row="5" column="0">
-                       <widget class="QCheckBox" name="checkBox_notifyWhenNewGlobalPathIsReceived">
-                        <property name="text">
-                         <string/>
-                        </property>
-                        <property name="checked">
-                         <bool>true</bool>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="3" column="0">
                        <widget class="QCheckBox" name="checkBox_stamps">
                         <property name="text">
@@ -194,19 +207,6 @@
                         </property>
                        </widget>
                       </item>
-                      <item row="3" column="1">
-                       <widget class="QLabel" name="label_247">
-                        <property name="text">
-                         <string>Use time for figures' x-axis. Otherwise, sensor data IDs are used for camera/odometry info and for all mapping statistics, node IDs are used.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="4" column="1">
                        <widget class="QLabel" name="label_342">
                         <property name="text">
@@ -227,6 +227,26 @@
                         </property>
                        </widget>
                       </item>
+                      <item row="6" column="1">
+                       <widget class="QLabel" name="label_239">
+                        <property name="text">
+                         <string>Disable odometry. This option only makes sense in localization mode with a pre-built map and that odometry cannot be computed from the source selected (e.g. RGB source).</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="checkbox_odomDisabled">
+                        <property name="text">
+                         <string/>
+                        </property>
+                       </widget>
+                      </item>
                      </layout>
                     </item>
                     <item>
@@ -2423,7 +2443,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                     <item>
                      <widget class="QStackedWidget" name="stackedWidget_src">
                       <property name="currentIndex">
-                       <number>0</number>
+                       <number>2</number>
                       </property>
                       <widget class="QWidget" name="page_41">
                        <layout class="QVBoxLayout" name="verticalLayout_64">
@@ -3970,6 +3990,19 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                           <layout class="QVBoxLayout" name="verticalLayout_5">
                            <item>
                             <layout class="QGridLayout" name="gridLayout_5" columnstretch="0,1">
+                             <item row="0" column="1">
+                              <widget class="QLabel" name="label_20">
+                               <property name="text">
+                                <string>Source type.</string>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
                              <item row="0" column="0">
                               <widget class="QComboBox" name="source_comboBox_image_type">
                                <property name="currentIndex">
@@ -3992,25 +4025,12 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                </item>
                               </widget>
                              </item>
-                             <item row="0" column="1">
-                              <widget class="QLabel" name="label_20">
-                               <property name="text">
-                                <string>Source type.</string>
-                               </property>
-                               <property name="wordWrap">
-                                <bool>true</bool>
-                               </property>
-                               <property name="textInteractionFlags">
-                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                               </property>
-                              </widget>
-                             </item>
                             </layout>
                            </item>
                            <item>
                             <widget class="QStackedWidget" name="stackedWidget_image">
                              <property name="currentIndex">
-                              <number>0</number>
+                              <number>1</number>
                              </property>
                              <widget class="QWidget" name="page_7">
                               <layout class="QVBoxLayout" name="verticalLayout_30">
@@ -7387,7 +7407,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                     <item>
                      <widget class="QLabel" name="label_83">
                       <property name="text">
-                       <string>Rigid transformations between nodes are saved on the neighbor links of the RTAB-Map's graph. On loop closures, a new constraint is added to the graph and TORO optimizes the graph.</string>
+                       <string>Rigid transformations between nodes are saved on the neighbor links of the RTAB-Map's graph. On loop closures, a new constraint is added to graph and the graph optimizer optimizes the graph.</string>
                       </property>
                       <property name="wordWrap">
                        <bool>true</bool>
@@ -9035,19 +9055,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                     </item>
                     <item>
                      <layout class="QGridLayout" name="gridLayout_27" columnstretch="0,1">
-                      <item row="8" column="0">
-                       <widget class="QDoubleSpinBox" name="odom_flow_keyframeThr">
-                        <property name="maximum">
-                         <double>1.000000000000000</double>
-                        </property>
-                        <property name="singleStep">
-                         <double>0.100000000000000</double>
-                        </property>
-                        <property name="value">
-                         <double>0.500000000000000</double>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="6" column="0">
                        <widget class="QCheckBox" name="odom_flow_guessMotion">
                         <property name="text">
@@ -9055,26 +9062,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="4" column="1">
-                       <widget class="QLabel" name="label_233">
-                        <property name="text">
-                         <string>Pose estimation filtering strategy.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="5" column="0">
-                       <widget class="QCheckBox" name="odom_fillInfoData">
-                        <property name="text">
-                         <string/>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="0" column="1">
                        <widget class="QLabel" name="label_103">
                         <property name="text">
@@ -9088,13 +9075,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="3" column="0">
-                       <widget class="QCheckBox" name="odom_holonomic">
-                        <property name="text">
-                         <string/>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="10" column="0">
                        <widget class="QSpinBox" name="odom_dataBufferSize">
                         <property name="maximum">
@@ -9102,19 +9082,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="3" column="1">
-                       <widget class="QLabel" name="label_238">
-                        <property name="text">
-                         <string>If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
                       <item row="0" column="0">
                        <widget class="QComboBox" name="odom_strategy">
                         <property name="sizeAdjustPolicy">
@@ -9178,10 +9145,59 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="2" column="0">
+                      <item row="7" column="1">
-                       <widget class="QSpinBox" name="odom_countdown">
+                       <widget class="QLabel" name="label_248">
+                        <property name="text">
+                         <string>Decimation of images before features extraction. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="7" column="0">
+                       <widget class="QSpinBox" name="odom_imageDecimation">
+                        <property name="minimum">
+                         <number>-32</number>
+                        </property>
                         <property name="maximum">
-                         <number>999999</number>
+                         <number>32</number>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="1" column="1">
+                       <widget class="QLabel" name="label_314">
+                        <property name="text">
+                         <string>Align odometry with ground on initialization.</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_alignWithGround">
+                        <property name="text">
+                         <string/>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="3" column="1">
+                       <widget class="QLabel" name="label_238">
+                        <property name="text">
+                         <string>If the robot is holonomic (strafing commands can be issued). If not, y value will be estimated from x and yaw values (y=x*tan(yaw)).</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                         </property>
                        </widget>
                       </item>
@@ -9246,6 +9262,46 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
+                      <item row="8" column="0">
+                       <widget class="QDoubleSpinBox" name="odom_flow_keyframeThr">
+                        <property name="maximum">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.100000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.500000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="4" column="1">
+                       <widget class="QLabel" name="label_233">
+                        <property name="text">
+                         <string>Pose estimation filtering strategy.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="5" column="0">
+                       <widget class="QCheckBox" name="odom_fillInfoData">
+                        <property name="text">
+                         <string/>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="3" column="0">
+                       <widget class="QCheckBox" name="odom_holonomic">
+                        <property name="text">
+                         <string/>
+                        </property>
+                       </widget>
+                      </item>
                       <item row="9" column="0">
                        <widget class="QDoubleSpinBox" name="odom_flow_scanKeyframeThr">
                         <property name="maximum">
@@ -9259,46 +9315,10 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                         </property>
                        </widget>
                       </item>
-                      <item row="7" column="1">
+                      <item row="2" column="0">
-                       <widget class="QLabel" name="label_248">
+                       <widget class="QSpinBox" name="odom_countdown">
-                        <property name="text">
-                         <string>Decimation of images before features extraction. Negative decimation is done from RGB size instead of depth size (if depth is smaller than RGB, it may be interpolated depending of the decimation value).</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="7" column="0">
-                       <widget class="QSpinBox" name="odom_imageDecimation">
-                        <property name="minimum">
-                         <number>-32</number>
-                        </property>
                         <property name="maximum">
-                         <number>32</number>
+                         <number>999999</number>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="1" column="1">
-                       <widget class="QLabel" name="label_314">
-                        <property name="text">
-                         <string>Align odometry with ground on initialization.</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_alignWithGround">
-                        <property name="text">
-                         <string/>
                         </property>
                        </widget>
                       </item>
@@ -9331,7 +9351,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                             </widget>
                            </item>
                            <item>
-                            <layout class="QGridLayout" name="gridLayout_29" columnstretch="0,0,1">
+                            <layout class="QGridLayout" name="gridLayout_29" columnstretch="0,1">
                              <item row="1" column="0">
                               <widget class="QSpinBox" name="spinBox_odom_f2m_maxNewFeatures">
                                <property name="minimum">
@@ -9348,7 +9368,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="3" column="2">
+                             <item row="3" column="1">
                               <widget class="QLabel" name="label_215">
                                <property name="text">
                                 <string>[Geometry] Radius used to filter points of a new added scan to local map. This could match the voxel size of the laser scans.</string>
@@ -9361,7 +9381,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="2" column="2">
+                             <item row="2" column="1">
                               <widget class="QLabel" name="label_195">
                                <property name="text">
                                 <string>[Geometry] Maximum local scan map size (points).</string>
@@ -9374,23 +9394,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="4" column="0">
+                             <item row="0" column="1">
-                              <widget class="QLineEdit" name="odom_fixedLocalMapPath"/>
-                             </item>
-                             <item row="4" column="2">
-                              <widget class="QLabel" name="label_239">
-                               <property name="text">
-                                <string>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 pose estimation is activated.</string>
-                               </property>
-                               <property name="wordWrap">
-                                <bool>true</bool>
-                               </property>
-                               <property name="textInteractionFlags">
-                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                               </property>
-                              </widget>
-                             </item>
-                             <item row="0" column="2">
                               <widget class="QLabel" name="label_190">
                                <property name="text">
                                 <string>[Visual] Maximum map size: If &gt; 0 (example 5000), the odometry will maintain a local map of X maximum features. This will decrease odometry drifting when the camera is not moving.</string>
@@ -9419,14 +9423,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="4" column="1">
+                             <item row="1" column="1">
-                              <widget class="QToolButton" name="toolButton_odomBowFixedLocalMap">
-                               <property name="text">
-                                <string>...</string>
-                               </property>
-                              </widget>
-                             </item>
-                             <item row="1" column="2">
                               <widget class="QLabel" name="label_194">
                                <property name="text">
                                 <string>[Visual] Maximum features (sorted by keypoint response) added to local map from a new key-frame. 0 means no limit.</string>
@@ -9459,7 +9456,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="5" column="2">
+                             <item row="4" column="1">
                               <widget class="QLabel" name="label_357">
                                <property name="text">
                                 <string>[Visual] Local bundle adjustment. See Optimizer panel. This will not work with Optical Flow correspondences strategy is selected in Visual Registration panel.</string>
@@ -9472,7 +9469,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="6" column="2">
+                             <item row="5" column="1">
                               <widget class="QLabel" name="label_358">
                                <property name="text">
                                 <string>[Visual] Maximum frames used for bundle adjustment (0=inf or all current frames in the local map).</string>
@@ -9485,7 +9482,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </property>
                               </widget>
                              </item>
-                             <item row="5" column="0">
+                             <item row="4" column="0">
                               <widget class="QComboBox" name="odom_f2m_bundleStrategy">
                                <property name="sizeAdjustPolicy">
                                 <enum>QComboBox::AdjustToContents</enum>
@@ -9507,7 +9504,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                                </item>
                               </widget>
                              </item>
-                             <item row="6" column="0">
+                             <item row="5" column="0">
                               <widget class="QSpinBox" name="odom_f2m_bundleMaxFrames">
                                <property name="maximum">
                                 <number>999999</number>