Added Bundle Adjustment options to OdometryF2M (OdomF2M/BundleAdjustment) and RegistrationVis (Vis/BundleAdjustment). Added logger thread filter. RegistrationVis: Copy back corresponding input word IDs to output words when available. Memory::computeTransform() added option to use already computed corespondences if possible (proximity detection by time uses this). Updated Optimizer::optimizeBA() interface.

2026-10-03 16:27:46 +08:00 · 2016-11-14 19:54:31 -05:00
parent 9fbd02b06d
commit 9276607920
39 changed files with 2214 additions and 594 deletions
@@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 11)
-SET(RTABMAP_PATCH_VERSION 11)
+SET(RTABMAP_PATCH_VERSION 12)
 SET(RTABMAP_VERSION
  ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
  
@@ -87,6 +87,7 @@ public:
 	Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted

 private:
+	virtual void mainLoopBegin();
 	virtual void mainLoop();
 	virtual void mainLoopKill();

@@ -193,8 +193,8 @@ public:
 			std::multimap<int, Link> & links,
 			bool lookInDatabase = false);

-	Transform computeTransform(Signature & fromS, Signature & toS, Transform guess, RegistrationInfo * info = 0) const;
-	Transform computeTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0);
+	Transform computeTransform(Signature & fromS, Signature & toS, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false) const;
+	Transform computeTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0, bool useKnownCorrespondencesIfPossible = false);
 	Transform computeIcpTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0);
 	Transform computeIcpTransformMulti(
 			int newId,
@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl/pcl_base.h>
+#include <rtabmap/core/Link.h>

 namespace rtabmap {

@@ -62,11 +63,20 @@ private:
 	int scanMaximumMapSize_;
 	float scanSubtractRadius_;
 	std::string fixedMapPath_;
+	int bundleAdjustment_;
+	int	bundleAdjustmentMaxFrames_;

 	Registration * regPipeline_;
 	Signature * map_;
 	Signature * lastFrame_;
 	std::vector<std::pair<pcl::PointCloud<pcl::PointNormal>::Ptr, pcl::IndicesPtr> > scansBuffer_;
+
+	std::map<int, std::map<int, cv::Point2f> > bundleWordReferences_;
+	std::map<int, Transform> bundlePoses_;
+	std::multimap<int, Link> bundleLinks_;
+	std::map<int, CameraModel> bundleModels_;
+	std::map<int, int> bundlePoseReferences_;
+	ParametersMap bundleParameters_;
 };

 }
@@ -48,12 +48,13 @@ protected:
 	virtual void handleEvent(UEvent * event);

 private:
-	void mainLoopKill();
+	virtual void mainLoopBegin();
+	virtual void mainLoopKill();

 	//============================================================
 	// MAIN LOOP
 	//============================================================
-	void mainLoop();
+	virtual void mainLoop();
 	void addData(const SensorData & data);
 	bool getData(SensorData & data);

@@ -83,6 +83,8 @@ public:
 	void setEpsilon(double epsilon) {epsilon_ = epsilon;}
 	void setRobust(bool enabled) {robust_ = enabled;}

+	virtual void parseParameters(const ParametersMap & parameters);
+
 	// inherited classes should implement one of these methods
 	virtual std::map<int, Transform> optimize(
 			int rootId,
@@ -92,12 +94,24 @@ public:
 			double * finalError = 0,
 			int * iterationsDone = 0);
 	virtual std::map<int, Transform> optimizeBA(
+			int rootId,
+			const std::map<int, Transform> & poses,
+			const std::multimap<int, Link> & links,
+			const std::map<int, CameraModel> & models,
+			std::map<int, cv::Point3f> & points3DMap,
+			const std::map<int, std::map<int, cv::Point2f> > & wordReferences); // <ID words, IDs frames + keypoint>);
+
+	std::map<int, Transform> optimizeBA(
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
 			const std::map<int, Signature> & signatures);

-	virtual void parseParameters(const ParametersMap & parameters);
+	Transform optimizeBA(
+			const Link & link,
+			const CameraModel & model,
+			std::map<int, cv::Point3f> & points3DMap,
+			const std::map<int, std::map<int, cv::Point2f> > & wordReferences);

 	void computeBACorrespondences(
 			const std::map<int, Transform> & poses,
@@ -109,7 +123,7 @@ public:
 protected:
 	Optimizer(
 			int iterations         = Parameters::defaultOptimizerIterations(),
-			bool slam2d            = Parameters::defaultOptimizerSlam2D(),
+			bool slam2d            = Parameters::defaultRegForce3DoF(),
 			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
 			double epsilon         = Parameters::defaultOptimizerEpsilon(),
 			bool robust            = Parameters::defaultOptimizerRobust());
@@ -42,7 +42,7 @@ public:
 public:
 	OptimizerCVSBA(
 			int iterations         = Parameters::defaultOptimizerIterations(),
-			bool slam2d            = Parameters::defaultOptimizerSlam2D(),
+			bool slam2d            = Parameters::defaultRegForce3DoF(),
 			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored()) :
 		Optimizer(iterations, slam2d, covarianceIgnored) {}
 	OptimizerCVSBA(const ParametersMap & parameters) :
@@ -55,7 +55,9 @@ public:
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
-			const std::map<int, Signature> & signatures);
+			const std::map<int, CameraModel> & models,
+			std::map<int, cv::Point3f> & points3DMap,
+			const std::map<int, std::map<int, cv::Point2f> > & wordReferences); // <ID words, IDs frames + keypoint>);
 };

 } /* namespace rtabmap */
@@ -73,7 +73,9 @@ public:
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
-			const std::map<int, Signature> & signatures);
+			const std::map<int, CameraModel> & models,
+			std::map<int, cv::Point3f> & points3DMap,
+			const std::map<int, std::map<int, cv::Point2f> > & wordReferences); // <ID words, IDs frames + keypoint>

 private:
 	int solver_;
@@ -42,7 +42,7 @@ public:
 public:
 	OptimizerGTSAM(
 			int iterations         = Parameters::defaultOptimizerIterations(),
-			bool slam2d            = Parameters::defaultOptimizerSlam2D(),
+			bool slam2d            = Parameters::defaultRegForce3DoF(),
 			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
 			double epsilon         = Parameters::defaultOptimizerEpsilon(),
 			bool robust            = Parameters::defaultOptimizerRobust()) :
@@ -52,7 +52,7 @@ public:
 public:
 	OptimizerTORO(
 			int iterations         = Parameters::defaultOptimizerIterations(),
-			bool slam2d            = Parameters::defaultOptimizerSlam2D(),
+			bool slam2d            = Parameters::defaultRegForce3DoF(),
 			bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(),
 			double epsilon         = Parameters::defaultOptimizerEpsilon()) :
 		Optimizer(iterations, slam2d, covarianceIgnored, epsilon) {}
@@ -336,22 +336,23 @@ class RTABMAP_EXP Parameters
 	// Graph optimization
 #ifdef RTABMAP_GTSAM
 	RTABMAP_PARAM(Optimizer, Strategy,          int, 2,          "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
+	RTABMAP_PARAM(Optimizer, Iterations,        int, 10,          "Optimization iterations.");
 #else
 #ifdef RTABMAP_G2O
 	RTABMAP_PARAM(Optimizer, Strategy,          int, 1,          "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
+	RTABMAP_PARAM(Optimizer, Iterations,        int, 10,          "Optimization iterations.");
 #else
 	RTABMAP_PARAM(Optimizer, Strategy,          int, 0,          "Graph optimization strategy: 0=TORO, 1=g2o and 2=GTSAM.");
-#endif
-#endif
 	RTABMAP_PARAM(Optimizer, Iterations,        int, 100,        "Optimization iterations.");
-	RTABMAP_PARAM(Optimizer, Slam2D,            bool, false,     "If optimization is done only on x,y and theta (3DoF). Otherwise, it is done on full 6DoF poses.");
+#endif
+#endif
 	RTABMAP_PARAM(Optimizer, VarianceIgnored,   bool, false,     "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links.");
-	RTABMAP_PARAM(Optimizer, Epsilon,           double, 0.0001,  "Stop optimizing when the error improvement is less than this value.");
+	RTABMAP_PARAM(Optimizer, Epsilon,           double, 0.00001, "Stop optimizing when the error improvement is less than this value.");
 	RTABMAP_PARAM(Optimizer, Robust,            bool, false,     uFormat("Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"%s\" if enabled.", kRGBDOptimizeMaxError().c_str()));

 	RTABMAP_PARAM(g2o, Solver,                  int, 0,          "0=csparse 1=pcg 2=cholmod");
 	RTABMAP_PARAM(g2o, Optimizer,               int, 0,          "0=Levenberg 1=GaussNewton");
-	RTABMAP_PARAM(g2o, PixelVariance,           double, 1,       "Pixel variance used for SBA.");
+	RTABMAP_PARAM(g2o, PixelVariance,           double, 1.0,     "Pixel variance used for bundle adjustment.");

 	// Odometry
 	RTABMAP_PARAM(Odom, Strategy,           	int, 0, 		"0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F)");
@@ -379,6 +380,8 @@ class RTABMAP_EXP Parameters
 	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).");

 	// Odometry Mono
 	RTABMAP_PARAM(OdomMono, InitMinFlow,            float, 100,  "Minimum optical flow required for the initialization step.");
@@ -428,6 +431,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Vis, CorFlowIterations,        int, 30,       uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
 	RTABMAP_PARAM(Vis, CorFlowEps,               float, 0.01,   uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
 	RTABMAP_PARAM(Vis, CorFlowMaxLevel,          int, 3,        uFormat("[%s=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.", kVisCorType().c_str()));
+	RTABMAP_PARAM(Vis, BundleAdjustment,         int, 0,        "Optimization with bundle adjustment: 0=disabled, 1=g2o, 2=cvsba.");

 	// ICP registration parameters
 	RTABMAP_PARAM(Icp, MaxTranslation,            float, 0.2,   "Maximum ICP translation correction accepted (m).");
@@ -81,8 +81,10 @@ private:
 	int _flowMaxLevel;
 	float _nndr;
 	int _guessWinSize;
+	int _bundleAdjustment;

 	ParametersMap _featureParameters;
+	ParametersMap _bundleParameters;
 };

 }
@@ -117,6 +117,11 @@ void CameraThread::enableBilateralFiltering(float sigmaS, float sigmaR)
 	_bilateralSigmaR = sigmaR;
 }

+void CameraThread::mainLoopBegin()
+{
+	ULogger::registerCurrentThread("Camera");
+}
+
 void CameraThread::mainLoop()
 {
 	UTimer totalTime;
@@ -2091,7 +2091,8 @@ Transform Memory::computeTransform(
 		int fromId,
 		int toId,
 		Transform guess,
-		RegistrationInfo * info)
+		RegistrationInfo * info,
+		bool useKnownCorrespondencesIfPossible)
 {
 	Signature * fromS = this->_getSignature(fromId);
 	Signature * toS = this->_getSignature(toId);
@@ -2100,7 +2101,7 @@ Transform Memory::computeTransform(

 	if(fromS && toS)
 	{
-		return computeTransform(*fromS, *toS, guess, info);
+		return computeTransform(*fromS, *toS, guess, info, useKnownCorrespondencesIfPossible);
 	}
 	else
 	{
@@ -2119,7 +2120,8 @@ Transform Memory::computeTransform(
 		Signature & fromS,
 		Signature & toS,
 		Transform guess,
-		RegistrationInfo * info) const
+		RegistrationInfo * info,
+		bool useKnownCorrespondencesIfPossible) const
 {
 	Transform transform;

@@ -2172,6 +2174,12 @@ Transform Memory::computeTransform(
 			tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
 			tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
 		}
+		else if(useKnownCorrespondencesIfPossible)
+		{
+			// This will make RegistrationVis bypassing the correspondences computation
+			tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
+			tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
+		}

 		if(guess.isNull() && !_registrationPipeline->isImageRequired())
 		{
@@ -2181,12 +2189,12 @@ Transform Memory::computeTransform(
 			guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
 			if(!guess.isNull())
 			{
-				transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
+				transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info);
 			}
 		}
 		else
 		{
-			transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
+			transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info);
 		}

 		if(!transform.isNull())
@@ -63,6 +63,8 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	scanMaximumMapSize_(Parameters::defaultOdomF2MScanMaxSize()),
 	scanSubtractRadius_(Parameters::defaultOdomF2MScanSubtractRadius()),
 	fixedMapPath_(Parameters::defaultOdomF2MFixedMapPath()),
+	bundleAdjustment_(Parameters::defaultOdomF2MBundleAdjustment()),
+	bundleAdjustmentMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()),
 	regPipeline_(Registration::create(parameters)),
 	map_(new Signature(-1)),
 	lastFrame_(new Signature(1))
@@ -75,6 +77,9 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	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;
 	UASSERT(maximumMapSize_ >= 0);
 	UASSERT(keyFrameThr_ >= 0.0f && keyFrameThr_<=1.0f);
 	UASSERT(scanKeyFrameThr_ >= 0.0f && scanKeyFrameThr_<=1.0f);
@@ -164,6 +169,12 @@ OdometryF2M::~OdometryF2M()
 {
 	delete map_;
 	delete lastFrame_;
+	scansBuffer_.clear();
+	bundleWordReferences_.clear();
+	bundlePoses_.clear();
+	bundleLinks_.clear();
+	bundleModels_.clear();
+	bundlePoseReferences_.clear();
 	UDEBUG("");
 }

@@ -203,6 +214,13 @@ Transform OdometryF2M::computeTransform(
 	delete lastFrame_;
 	lastFrame_ = new Signature(data);

+	if(bundleAdjustment_ > 0 &&
+	   data.cameraModels().size() > 1)
+	{
+		UERROR("Odometry bundle adjustment doesn't work with multi-cameras. It is disabled.");
+		bundleAdjustment_ = 0;
+	}
+
 	// Generate keypoints from the new data
 	if(lastFrame_->sensorData().isValid())
 	{
@@ -219,8 +237,106 @@ Transform OdometryF2M::computeTransform(

 			data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().descriptors());

+			std::map<int, cv::Point3f> points3DMap;
+			std::map<int, Transform> bundlePoses;
+			std::multimap<int, Link> bundleLinks;
+			std::map<int, CameraModel> bundleModels;
 			if(!transform.isNull())
 			{
+				// local bundle adjustment
+				if(bundleAdjustment_>0 &&
+				   regPipeline_->isImageRequired() &&
+				   ((bundleAdjustment_==1 && Optimizer::isAvailable(Optimizer::kTypeG2O)) ||
+			        (bundleAdjustment_==2 && Optimizer::isAvailable(Optimizer::kTypeCVSBA))) &&
+				   regInfo.inliersIDs.size())
+				{
+					UDEBUG("Local Bundle Adjustment");
+
+					// make sure the IDs of words in the map are not modified (Optical Flow Registration issue)
+					UASSERT(map_->getWords().size() && tmpMap.getWords().size());
+					if(map_->getWords().size() != tmpMap.getWords().size() ||
+					   map_->getWords().begin()->first != tmpMap.getWords().begin()->first ||
+					   map_->getWords().rbegin()->first != tmpMap.getWords().rbegin()->first)
+					{
+						UERROR("Bundle Adjustment cannot be used with a registration approach recomputing features from the \"from\" signature (e.g., Optical Flow).");
+						bundleAdjustment_ = 0;
+					}
+					else
+					{
+						UTimer bundleTime;
+						Optimizer * sba = Optimizer::create(bundleAdjustment_==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, bundleParameters_);
+
+						std::map<int, std::map<int, cv::Point2f> > wordReferences;
+						UASSERT(bundlePoses_.size());
+						UASSERT(bundlePoses_.size()-1 == bundleLinks_.size() && bundlePoses_.size() == bundleModels_.size());
+						if(bundleAdjustmentMaxFrames_ > 0)
+						{
+							std::map<int, Transform>::reverse_iterator iter = bundlePoses_.rbegin();
+							for(int i = 0; i<bundleAdjustmentMaxFrames_ && i < (int)bundlePoses_.size()-1; ++i, ++iter)
+							{
+								bundlePoses.insert(*iter);
+								UASSERT(bundleLinks_.find(iter->first) != bundleLinks_.end());
+								bundleLinks.insert(*bundleLinks_.find(iter->first));
+								UASSERT(bundleModels_.find(iter->first) != bundleModels_.end());
+								bundleModels.insert(*bundleModels_.find(iter->first));
+							}
+							//make sure the origin is there
+							bundlePoses.insert(*bundlePoses_.find(0));
+							bundleModels.insert(*bundleModels_.find(0));
+						}
+						else
+						{
+							bundlePoses = bundlePoses_;
+							bundleLinks = bundleLinks_;
+							bundleModels = bundleModels_;
+						}
+						bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(0, lastFrame_->id(), Link::kNeighbor, transform, regInfo.variance, regInfo.variance)));
+						bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));
+						for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
+						{
+							std::multimap<int, cv::Point3f>::const_iterator iter3D = tmpMap.getWords3().find(regInfo.inliersIDs[i]);
+							UASSERT(iter3D!=tmpMap.getWords3().end());
+							points3DMap.insert(*iter3D);
+
+							std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().find(regInfo.inliersIDs[i]);
+							UASSERT(iter2D!=lastFrame_->getWords().end());
+
+							if(wordReferences.find(iter2D->first) == wordReferences.end())
+							{
+								UASSERT(bundleWordReferences_.find(iter2D->first) != bundleWordReferences_.end());
+								wordReferences.insert(*bundleWordReferences_.find(iter2D->first));
+							}
+
+							wordReferences.find(iter2D->first)->second.insert(std::make_pair(lastFrame_->id(), iter2D->second.pt));
+						}
+
+						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());
+						bundleModels.insert(std::make_pair(lastFrame_->id(), model));
+
+						bundlePoses = sba->optimizeBA(0, bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences);
+						delete sba;
+
+						UDEBUG("bundleTime=%fs (poses=%d wordRef=%d)", bundleTime.ticks(), (int)bundlePoses.size(), (int)bundleWordReferences_.size());
+
+						UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
+						UDEBUG("Local Bundle Adjustment After : %s", bundlePoses.rbegin()->second.prettyPrint().c_str());
+
+						if(!bundlePoses.rbegin()->second.isNull())
+						{
+							transform = bundlePoses.rbegin()->second;
+						}
+					}
+				}
+
 				// make it incremental
 				transform = this->getPose().inverse() * transform;
 			}
@@ -262,6 +378,24 @@ Transform OdometryF2M::computeTransform(
 						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)
+							for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
+							{
+								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());
+						}
+
 						// sort by feature response
 						std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > > newIds;
 						UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
@@ -279,6 +413,25 @@ Transform OdometryF2M::computeTransform(
 															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));
+										}
+									}
+								}
 							}
 						}

@@ -288,6 +441,26 @@ Transform OdometryF2M::computeTransform(
 						{
 							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));
@@ -307,6 +480,27 @@ Transform OdometryF2M::computeTransform(
 							{
 								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++);
@@ -514,6 +708,42 @@ Transform OdometryF2M::computeTransform(
 								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);
@@ -74,6 +74,11 @@ void OdometryThread::handleEvent(UEvent * event)
 	}
 }

+void OdometryThread::mainLoopBegin()
+{
+	ULogger::registerCurrentThread("Odometry");
+}
+
 void OdometryThread::mainLoopKill()
 {
 	_dataAdded.release();
@@ -154,56 +154,6 @@ Optimizer * Optimizer::create(Optimizer::Type type, const ParametersMap & parame
 	return optimizer;
 }

-Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust) :
-		iterations_(iterations),
-		slam2d_(slam2d),
-		covarianceIgnored_(covarianceIgnored),
-		epsilon_(epsilon),
-		robust_(robust)
-{
-}
-
-Optimizer::Optimizer(const ParametersMap & parameters) :
-		iterations_(Parameters::defaultOptimizerIterations()),
-		slam2d_(Parameters::defaultOptimizerSlam2D()),
-		covarianceIgnored_(Parameters::defaultOptimizerVarianceIgnored()),
-		epsilon_(Parameters::defaultOptimizerEpsilon()),
-		robust_(Parameters::defaultOptimizerRobust())
-{
-	parseParameters(parameters);
-}
-
-void Optimizer::parseParameters(const ParametersMap & parameters)
-{
-	Parameters::parse(parameters, Parameters::kOptimizerIterations(), iterations_);
-	Parameters::parse(parameters, Parameters::kOptimizerVarianceIgnored(), covarianceIgnored_);
-	Parameters::parse(parameters, Parameters::kOptimizerSlam2D(), slam2d_);
-	Parameters::parse(parameters, Parameters::kOptimizerEpsilon(), epsilon_);
-	Parameters::parse(parameters, Parameters::kOptimizerRobust(), robust_);
-}
-
-std::map<int, Transform> Optimizer::optimize(
-		int rootId,
-		const std::map<int, Transform> & poses,
-		const std::multimap<int, Link> & constraints,
-		std::list<std::map<int, Transform> > * intermediateGraphes,
-		double * finalError,
-		int * iterationsDone)
-{
-	UERROR("Optimizer %d doesn't implement optimize() method.", (int)this->type());
-	return std::map<int, Transform>();
-}
-
-std::map<int, Transform> Optimizer::optimizeBA(
-		int rootId,
-		const std::map<int, Transform> & poses,
-		const std::multimap<int, Link> & links,
-		const std::map<int, Signature> & signatures)
-{
-	UERROR("Optimizer %d doesn't implement optimizeBA() method.", (int)this->type());
-	return std::map<int, Transform>();
-}
-
 void Optimizer::getConnectedGraph(
 		int fromId,
 		const std::map<int, Transform> & posesIn,
@@ -271,6 +221,129 @@ void Optimizer::getConnectedGraph(
 	}
 }

+Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust) :
+		iterations_(iterations),
+		slam2d_(slam2d),
+		covarianceIgnored_(covarianceIgnored),
+		epsilon_(epsilon),
+		robust_(robust)
+{
+}
+
+Optimizer::Optimizer(const ParametersMap & parameters) :
+		iterations_(Parameters::defaultOptimizerIterations()),
+		slam2d_(Parameters::defaultRegForce3DoF()),
+		covarianceIgnored_(Parameters::defaultOptimizerVarianceIgnored()),
+		epsilon_(Parameters::defaultOptimizerEpsilon()),
+		robust_(Parameters::defaultOptimizerRobust())
+{
+	parseParameters(parameters);
+}
+
+void Optimizer::parseParameters(const ParametersMap & parameters)
+{
+	Parameters::parse(parameters, Parameters::kOptimizerIterations(), iterations_);
+	Parameters::parse(parameters, Parameters::kOptimizerVarianceIgnored(), covarianceIgnored_);
+	Parameters::parse(parameters, Parameters::kRegForce3DoF(), slam2d_);
+	Parameters::parse(parameters, Parameters::kOptimizerEpsilon(), epsilon_);
+	Parameters::parse(parameters, Parameters::kOptimizerRobust(), robust_);
+}
+
+std::map<int, Transform> Optimizer::optimize(
+		int rootId,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & constraints,
+		std::list<std::map<int, Transform> > * intermediateGraphes,
+		double * finalError,
+		int * iterationsDone)
+{
+	UERROR("Optimizer %d doesn't implement optimize() method.", (int)this->type());
+	return std::map<int, Transform>();
+}
+
+std::map<int, Transform> Optimizer::optimizeBA(
+		int rootId,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & links,
+		const std::map<int, CameraModel> & models,
+		std::map<int, cv::Point3f> & points3DMap,
+		const std::map<int, std::map<int, cv::Point2f> > & wordReferences)
+{
+	UERROR("Optimizer %d doesn't implement optimizeBA() method.", (int)this->type());
+	return std::map<int, Transform>();
+}
+
+std::map<int, Transform> Optimizer::optimizeBA(
+		int rootId,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & links,
+		const std::map<int, Signature> & signatures)
+{
+	UDEBUG("");
+	std::map<int, CameraModel> models;
+	for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+	{
+		// Get camera model
+		CameraModel model;
+		if(uContains(signatures, iter->first))
+		{
+			if(signatures.at(iter->first).sensorData().cameraModels().size() == 1 && signatures.at(iter->first).sensorData().cameraModels().at(0).isValidForProjection())
+			{
+				model = signatures.at(iter->first).sensorData().cameraModels()[0];
+			}
+			else if(signatures.at(iter->first).sensorData().stereoCameraModel().isValidForProjection())
+			{
+				model = signatures.at(iter->first).sensorData().stereoCameraModel().left();
+			}
+			else
+			{
+				UERROR("Missing calibration for node %d", iter->first);
+				return std::map<int, Transform>();
+			}
+		}
+		else
+		{
+			UERROR("Did not find node %d in cache", iter->first);
+			return std::map<int, Transform>();
+		}
+
+		UASSERT(model.isValidForProjection());
+
+		models.insert(std::make_pair(iter->first, model));
+	}
+
+	// compute correspondences
+	std::map<int, cv::Point3f> points3DMap;
+	std::map<int, std::map<int, cv::Point2f> > wordReferences;
+	this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences);
+
+	return optimizeBA(rootId, poses, links, models, points3DMap, wordReferences);
+}
+
+Transform Optimizer::optimizeBA(
+		const Link & link,
+		const CameraModel & model,
+		std::map<int, cv::Point3f> & points3DMap,
+		const std::map<int, std::map<int, cv::Point2f> > & wordReferences)
+{
+	std::map<int, Transform> poses;
+	poses.insert(std::make_pair(link.from(), Transform::getIdentity()));
+	poses.insert(std::make_pair(link.to(), link.transform()));
+	std::multimap<int, Link> links;
+	links.insert(std::make_pair(link.from(), link));
+	std::map<int, CameraModel> models;
+	models.insert(std::make_pair(link.from(), model));
+	models.insert(std::make_pair(link.to(), model));
+	poses = optimizeBA(link.from(), poses, links, models, points3DMap, wordReferences);
+	if(poses.size() == 2)
+	{
+		return poses.rbegin()->second;
+	}
+	else
+	{
+		return link.transform();
+	}
+}

 void Optimizer::computeBACorrespondences(
 		const std::map<int, Transform> & poses,
@@ -279,6 +352,7 @@ void Optimizer::computeBACorrespondences(
 		std::map<int, cv::Point3f> & points3DMap,
 		std::map<int, std::map<int, cv::Point2f> > & wordReferences) // <ID words, IDs frames + keypoint>
 {
+	UDEBUG("");
 	int wordCount = 0;
 	int edgeWithWordsAdded = 0;
 	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
@@ -57,7 +57,9 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
 		int rootId,
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & links,
-		const std::map<int, Signature> & signatures)
+		const std::map<int, CameraModel> & models,
+		std::map<int, cv::Point3f> & points3DMap,
+		const std::map<int, std::map<int, cv::Point2f> > & wordReferences) // <ID words, IDs frames + keypoint>)
 {
 #ifdef RTABMAP_CVSBA
 	// run sba optimization
@@ -73,100 +75,72 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
 	params.verbose=ULogger::level() <= ULogger::kInfo;
 	sba.setParams(params);

-	std::map<int, Transform> frames = poses;
-
-	std::vector<cv::Mat> cameraMatrix(frames.size()); //nframes
-	std::vector<cv::Mat> R(frames.size()); //nframes
-	std::vector<cv::Mat> T(frames.size()); //nframes
-	std::vector<cv::Mat> distCoeffs(frames.size()); //nframes
+	std::vector<cv::Mat> cameraMatrix(poses.size()); //nframes
+	std::vector<cv::Mat> R(poses.size()); //nframes
+	std::vector<cv::Mat> T(poses.size()); //nframes
+	std::vector<cv::Mat> distCoeffs(poses.size()); //nframes
 	std::map<int, int> frameIdToIndex;
-	std::map<int, CameraModel> models;
 	int oi=0;
-	for(std::map<int, Transform>::iterator iter=frames.begin(); iter!=frames.end(); )
+	for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
 	{
-		CameraModel model;
-		if(uContains(signatures, iter->first))
+		// Get camera model
+		std::map<int, CameraModel>::const_iterator iterModel = models.find(iter->first);
+		UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
+
+		frameIdToIndex.insert(std::make_pair(iter->first, oi));
+
+		cameraMatrix[oi] = iterModel->second.K();
+		if(iterModel->second.D().cols != 5)
 		{
-			if(signatures.at(iter->first).sensorData().cameraModels().size() == 1 && signatures.at(iter->first).sensorData().cameraModels().at(0).isValidForProjection())
-			{
-				model = signatures.at(iter->first).sensorData().cameraModels()[0];
-			}
-			else if(signatures.at(iter->first).sensorData().stereoCameraModel().isValidForProjection())
-			{
-				model = signatures.at(iter->first).sensorData().stereoCameraModel().left();
-			}
-			else
-			{
-				UERROR("Missing calibration for node %d", iter->first);
-			}
+			distCoeffs[oi] = cv::Mat::zeros(1, 5, CV_64FC1);
+			UWARN("Camera model %d: Distortion coefficients are not 5, setting all them to 0 (assuming no distortion)", iter->first);
 		}
 		else
 		{
-			UERROR("Did not find node %d in cache", iter->first);
+			distCoeffs[oi] = iterModel->second.D();
 		}

-		if(model.isValidForProjection())
-		{
-			frameIdToIndex.insert(std::make_pair(iter->first, oi));
+		Transform t = (iter->second * iterModel->second.localTransform()).inverse();

-			cameraMatrix[oi] = model.K();
-			if(model.D().cols != 5)
-			{
-				distCoeffs[oi] = cv::Mat::zeros(1, 5, CV_64FC1);
-				UWARN("Camera model %d: Distortion coefficients are not 5, setting all them to 0 (assuming no distortion)", iter->first);
-			}
-			else
-			{
-				distCoeffs[oi] = model.D();
-			}
+		R[oi] = (cv::Mat_<double>(3,3) <<
+				(double)t.r11(), (double)t.r12(), (double)t.r13(),
+				(double)t.r21(), (double)t.r22(), (double)t.r23(),
+				(double)t.r31(), (double)t.r32(), (double)t.r33());
+		T[oi] = (cv::Mat_<double>(1,3) << (double)t.x(), (double)t.y(), (double)t.z());
+		++oi;

-			Transform t = (iter->second * model.localTransform()).inverse();
-
-			R[oi] = (cv::Mat_<double>(3,3) <<
-					(double)t.r11(), (double)t.r12(), (double)t.r13(),
-					(double)t.r21(), (double)t.r22(), (double)t.r23(),
-					(double)t.r31(), (double)t.r32(), (double)t.r33());
-			T[oi] = (cv::Mat_<double>(1,3) << (double)t.x(), (double)t.y(), (double)t.z());
-			++oi;
-
-			models.insert(std::make_pair(iter->first, model));
-
-			UDEBUG("Pose %d = %s", iter->first, t.prettyPrint().c_str());
-
-			++iter;
-		}
-		else
-		{
-			frames.erase(iter++);
-		}
+		UDEBUG("Pose %d = %s", iter->first, t.prettyPrint().c_str());
 	}
 	cameraMatrix.resize(oi);
 	R.resize(oi);
 	T.resize(oi);
 	distCoeffs.resize(oi);

-	std::map<int, cv::Point3f> points3DMap;
-	std::map<int, std::map<int, cv::Point2f> > wordReferences; // <ID words, IDs frames + keypoint>
-	computeBACorrespondences(frames, links, signatures, points3DMap, wordReferences);
-
-	UDEBUG("points=%d frames=%d", (int)wordReferences.size(), (int)frames.size());
-	std::vector<cv::Point3f> points(wordReferences.size()); //npoints
-	std::vector<std::vector<cv::Point2f> >  imagePoints(frames.size()); //nframes -> npoints
-	std::vector<std::vector<int> > visibility(frames.size()); //nframes -> npoints
-	for(unsigned int i=0; i<frames.size(); ++i)
+	UDEBUG("points=%d frames=%d", (int)points3DMap.size(), (int)poses.size());
+	std::vector<cv::Point3f> points(points3DMap.size()); //npoints
+	std::vector<std::vector<cv::Point2f> >  imagePoints(poses.size()); //nframes -> npoints
+	std::vector<std::vector<int> > visibility(poses.size()); //nframes -> npoints
+	for(unsigned int i=0; i<poses.size(); ++i)
 	{
 		imagePoints[i].resize(wordReferences.size(), cv::Point2f(std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::quiet_NaN()));
 		visibility[i].resize(wordReferences.size(), 0);
 	}
 	int i=0;
-	for(std::map<int, std::map<int, cv::Point2f> >::iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
+	for(std::map<int, cv::Point3f>::const_iterator kter = points3DMap.begin(); kter!=points3DMap.end(); ++kter)
 	{
-		points[i] = points3DMap.at(iter->first);
+		points[i] = kter->second;

-		for(std::map<int, cv::Point2f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
+		std::map<int, std::map<int, cv::Point2f> >::const_iterator iter = wordReferences.find(kter->first);
+		if(iter != wordReferences.end())
 		{
-			imagePoints[frameIdToIndex.at(jter->first)][i] = jter->second;
-			visibility[frameIdToIndex.at(jter->first)][i] = 1;
+			for(std::map<int, cv::Point2f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
+			{
+				if(frameIdToIndex.find(jter->first) != frameIdToIndex.end())
+				{
+					imagePoints[frameIdToIndex.at(jter->first)][i] = jter->second;
+					visibility[frameIdToIndex.at(jter->first)][i] = 1;
+				}
+			}
 		}
 		++i;
 	}
@@ -184,7 +158,8 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(

 	//update poses
 	i=0;
-	for(std::map<int, Transform>::iterator iter=frames.begin(); iter!=frames.end(); ++iter)
+	std::map<int, Transform> newPoses = poses;
+	for(std::map<int, Transform>::iterator iter=newPoses.begin(); iter!=newPoses.end(); ++iter)
 	{
 		Transform t(R[i].at<double>(0,0), R[i].at<double>(0,1), R[i].at<double>(0,2), T[i].at<double>(0),
 					R[i].at<double>(1,0), R[i].at<double>(1,1), R[i].at<double>(1,2), T[i].at<double>(1),
@@ -192,12 +167,28 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(

 		UDEBUG("New pose %d = %s", iter->first, t.prettyPrint().c_str());

-		iter->second = (models.at(iter->first).localTransform() * t).inverse();
+		if(this->isSlam2d())
+		{
+			t = (models.at(iter->first).localTransform() * t).inverse();
+			t = iter->second.inverse() * t;
+			iter->second *= t.to3DoF();
+		}
+		else
+		{
+			iter->second = (models.at(iter->first).localTransform() * t).inverse();
+		}

 		++i;
 	}

-	return frames;
+	//update 3D points
+	i=0;
+	for(std::map<int, cv::Point3f>::iterator kter = points3DMap.begin(); kter!=points3DMap.end(); ++kter)
+	{
+		kter->second = points[i++];
+	}
+
+	return newPoses;

 #else
 	UERROR("RTAB-Map is not built with cvsba!");
@@ -235,7 +235,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
 		int vertigoVertexId = poses.rbegin()->first+1;
 		for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
 		{
-			int id1 = iter->first;
+			int id1 = iter->second.from();
 			int id2 = iter->second.to();

 			UASSERT(!iter->second.transform().isNull());
@@ -547,14 +547,16 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 		int rootId,
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & links,
-		const std::map<int, Signature> & signatures)
+		const std::map<int, CameraModel> & models,
+		std::map<int, cv::Point3f> & points3DMap,
+		const std::map<int, std::map<int, cv::Point2f> > & wordReferences) // <ID words, IDs frames + keypoint>
 {
 	std::map<int, Transform> optimizedPoses;
 #ifdef RTABMAP_G2O
 	UDEBUG("Optimizing graph...");

 	optimizedPoses.clear();
-	if(links.size()>=1 && poses.size()>=2 && iterations() > 0)
+	if(links.size()>=1 && poses.size()>=2 && iterations() > 0 && models.size() == poses.size())
 	{
 		g2o::SparseOptimizer optimizer;
 		optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
@@ -593,40 +595,14 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 			optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(solver_ptr));
 		}

-		std::map<int, Transform> frames = poses;
-
 		UDEBUG("fill poses to g2o...");
-		std::map<int, CameraModel> models;
-		for(std::map<int, Transform>::iterator iter=frames.begin(); iter!=frames.end(); )
+		for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); )
 		{
 			// Get camera model
-			CameraModel model;
-			if(uContains(signatures, iter->first))
-			{
-				if(signatures.at(iter->first).sensorData().cameraModels().size() == 1 && signatures.at(iter->first).sensorData().cameraModels().at(0).isValidForProjection())
-				{
-					model = signatures.at(iter->first).sensorData().cameraModels()[0];
-				}
-				else if(signatures.at(iter->first).sensorData().stereoCameraModel().isValidForProjection())
-				{
-					model = signatures.at(iter->first).sensorData().stereoCameraModel().left();
-				}
-				else
-				{
-					UERROR("Missing calibration for node %d", iter->first);
-					return optimizedPoses;
-				}
-			}
-			else
-			{
-				UERROR("Did not find node %d in cache", iter->first);
-				return optimizedPoses;
-			}
+			std::map<int, CameraModel>::const_iterator iterModel = models.find(iter->first);
+			UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());

-			UASSERT(model.isValidForProjection());
-
-			models.insert(std::make_pair(iter->first, model));
-			Transform camPose = iter->second * model.localTransform();
+			Transform camPose = iter->second * iterModel->second.localTransform();
 			//iter->second = (iter->second * model.localTransform()).inverse();
 			UDEBUG("%d t=%s", iter->first, camPose.prettyPrint().c_str());

@@ -636,7 +612,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(

 			Eigen::Affine3d a = camPose.toEigen3d();
 			g2o::SBACam cam(Eigen::Quaterniond(a.rotation()), a.translation());
-			cam.setKcam(model.fx(), model.fy(), model.cx(), model.cy(), 0);
+			cam.setKcam(iterModel->second.fx(), iterModel->second.fy(), iterModel->second.cx(), iterModel->second.cy(), 0);
 			vCam->setEstimate(cam);
 			if(iter->first == rootId)
 			{
@@ -652,18 +628,11 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 		UDEBUG("fill edges to g2o and associate each 3D point to all frames observing it...");
 		for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 		{
-			Link link = iter->second;
-			if(link.to() < link.from())
-			{
-				link = link.inverse();
-			}
-			if(uContains(signatures, link.from()) &&
-			   uContains(signatures, link.to()) &&
-			   uContains(frames, link.from()) &&
-			   uContains(frames, link.to()))
+			if(uContains(poses, iter->second.from()) &&
+			   uContains(poses, iter->second.to()))
 			{
 				// add edge
-				int id1 = iter->first;
+				int id1 = iter->second.from();
 				int id2 = iter->second.to();

 				UASSERT(!iter->second.transform().isNull());
@@ -703,47 +672,48 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 			}
 		}

-		std::map<int, cv::Point3f> points3DMap;
-		std::map<int, std::map<int, cv::Point2f> > wordReferences; // <ID words, IDs frames + keypoint>
-		this->computeBACorrespondences(frames, links, signatures, points3DMap, wordReferences);
-
 		UDEBUG("fill 3D points to g2o...");
-		int stepVertexId = frames.rbegin()->first+1;
-		for(std::map<int, std::map<int, cv::Point2f> >::iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
+		int stepVertexId = poses.rbegin()->first+1;
+		for(std::map<int, std::map<int, cv::Point2f> >::const_iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
 		{
-			const cv::Point3f & pt3d = points3DMap.at(iter->first);
-			g2o::VertexSBAPointXYZ* vpt3d = new g2o::VertexSBAPointXYZ();
-
-			vpt3d->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
-			vpt3d->setId(stepVertexId + iter->first);
-			vpt3d->setMarginalized(true);
-			optimizer.addVertex(vpt3d);
-
-			// set observations
-			for(std::map<int, cv::Point2f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
+			if(points3DMap.find(iter->first) != points3DMap.end())
 			{
-				int camId = jter->first;
+				const cv::Point3f & pt3d = points3DMap.at(iter->first);
+				g2o::VertexSBAPointXYZ* vpt3d = new g2o::VertexSBAPointXYZ();

-				const cv::Point2f & pt = jter->second;
+				vpt3d->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
+				vpt3d->setId(stepVertexId + iter->first);
+				vpt3d->setMarginalized(true);
+				optimizer.addVertex(vpt3d);

-				Eigen::Matrix<double,2,1> obs;
-				obs << pt.x, pt.y;
-
-				UDEBUG("Added observation pt=%d to cam=%d (%f,%f)", vpt3d->id(), camId, pt.x, pt.y);
-
-				g2o::EdgeProjectP2MC* e = new g2o::EdgeProjectP2MC();
-
-				e->setVertex(0, vpt3d);
-				e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(camId)));
-				e->setMeasurement(obs);
-				e->setInformation(Eigen::Matrix2d::Identity() / pixelVariance_);
-
-				if(robustKernel)
+				// set observations
+				for(std::map<int, cv::Point2f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
 				{
-					e->setRobustKernel(new g2o::RobustKernelHuber);
-				}
+					int camId = jter->first;
+					if(poses.find(camId) != poses.end())
+					{
+						const cv::Point2f & pt = jter->second;

-				optimizer.addEdge(e);
+						Eigen::Matrix<double,2,1> obs;
+						obs << pt.x, pt.y;
+
+						//UDEBUG("Added observation pt=%d to cam=%d (%f,%f)", vpt3d->id(), camId, pt.x, pt.y);
+
+						g2o::EdgeProjectP2MC* e = new g2o::EdgeProjectP2MC();
+
+						e->setVertex(0, vpt3d);
+						e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(camId)));
+						e->setMeasurement(obs);
+						e->setInformation(Eigen::Matrix2d::Identity() / pixelVariance_);
+
+						if(robustKernel)
+						{
+							e->setRobustKernel(new g2o::RobustKernelHuber);
+						}
+
+						optimizer.addEdge(e);
+					}
+				}
 			}
 		}

@@ -809,23 +779,55 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 			return optimizedPoses;
 		}

+		// update poses
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 		{
 			const g2o::VertexCam* v = (const g2o::VertexCam*)optimizer.vertex(iter->first);
 			if(v)
 			{
 				Transform t = Transform::fromEigen3d(v->estimate());
-				UDEBUG("%d t=%s", iter->first, t.prettyPrint().c_str());
 				// remove model local transform
 				t *= models.at(iter->first).localTransform().inverse();
-				optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
+				//UDEBUG("%d from=%s to=%s", iter->first, iter->second.prettyPrint().c_str(), t.prettyPrint().c_str());
 				UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
+
+				// FIXME: is there a way that we can add the 2D constraint directly in SBA?
+				if(this->isSlam2d())
+				{
+					// get transform between old and new pose
+					t = iter->second.inverse() * t;
+					optimizedPoses.insert(std::pair<int, Transform>(iter->first, iter->second * t.to3DoF()));
+				}
+				else
+				{
+					optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
+				}
 			}
 			else
 			{
-				UERROR("Vertex %d not found!?", iter->first);
+				UERROR("Vertex (pose) %d not found!?", iter->first);
 			}
 		}
+
+		//update points3D
+		for(std::map<int, cv::Point3f>::iterator iter = points3DMap.begin(); iter!=points3DMap.end(); ++iter)
+		{
+			const g2o::VertexSBAPointXYZ* v = (const g2o::VertexSBAPointXYZ*)optimizer.vertex(stepVertexId + iter->first);
+			if(v)
+			{
+				cv::Point3f p(v->estimate()[0], v->estimate()[1], v->estimate()[2]);
+				//UDEBUG("%d from=%f,%f,%f to=%f,%f,%f", iter->first, iter->second.x, iter->second.y, iter->second.z, p.x, p.y, p.z);
+				iter->second = p;
+			}
+			else
+			{
+				UERROR("Vertex (point3D) %d not found!?", iter->first);
+			}
+		}
+	}
+	else if(poses.size() > 1 && poses.size() != models.size())
+	{
+		UERROR("This method should be called with size of poses = size camera models!");
 	}
 	else if(poses.size() == 1 || iterations() <= 0)
 	{
@@ -893,7 +895,7 @@ bool OptimizerG2O::saveGraph(
 			Eigen::Quaternionf q = iter->second.transform().getQuaternionf();
 			fprintf(file, "%s %d %d%s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
 					prefix.c_str(),
-					iter->first,
+					iter->second.from(),
 					iter->second.to(),
 					suffix.c_str(),
 					iter->second.transform().x(),
@@ -126,7 +126,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 		int switchCounter = poses.rbegin()->first+1;
 		for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
 		{
-			int id1 = iter->first;
+			int id1 = iter->second.from();
 			int id2 = iter->second.to();

 			UASSERT(!iter->second.transform().isNull());
@@ -124,7 +124,7 @@ std::map<int, Transform> OptimizerTORO::optimize(
 					inf.values[2][2] = iter->second.infMatrix().at<double>(5,5); // theta-theta
 				}

-				int id1 = iter->first;
+				int id1 = iter->second.from();
 				int id2 = iter->second.to();
 				AISNavigation::TreePoseGraph2::Vertex* v1=pg2.vertex(id1);
 				AISNavigation::TreePoseGraph2::Vertex* v2=pg2.vertex(id2);
@@ -152,7 +152,7 @@ std::map<int, Transform> OptimizerTORO::optimize(
 					memcpy(inf[0], iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
 				}

-				int id1 = iter->first;
+				int id1 = iter->second.from();
 				int id2 = iter->second.to();
 				AISNavigation::TreePoseGraph3::Vertex* v1=pg3.vertex(id1);
 				AISNavigation::TreePoseGraph3::Vertex* v2=pg3.vertex(id2);
@@ -356,7 +356,7 @@ bool OptimizerTORO::saveGraph(
 			float x,y,z, yaw,pitch,roll;
 			iter->second.transform().getTranslationAndEulerAngles(x,y,z, roll, pitch, yaw);
 			fprintf(file, "EDGE3 %d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
-					iter->first,
+					iter->second.from(),
 					iter->second.to(),
 					x,
 					y,
@@ -224,6 +224,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 	{
 		// removed parameters

+		// 0.11.12
+		removedParameters_.insert(std::make_pair("Optimizer/Slam2D",              std::make_pair(true,  Parameters::kRegForce3DoF())));
+
 		// 0.11.10 typos
 		removedParameters_.insert(std::make_pair("Grid/FlatObstaclesDetected",    std::make_pair(true,  Parameters::kGridFlatObstacleDetected())));

@@ -332,8 +335,8 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
 		removedParameters_.insert(std::make_pair("RGBD/OptimizeEpsilon",             std::make_pair(true,  Parameters::kOptimizerEpsilon())));
 		removedParameters_.insert(std::make_pair("RGBD/OptimizeIterations",          std::make_pair(true,  Parameters::kOptimizerIterations())));
 		removedParameters_.insert(std::make_pair("RGBD/OptimizeRobust",              std::make_pair(true,  Parameters::kOptimizerRobust())));
-		removedParameters_.insert(std::make_pair("RGBD/OptimizeSlam2D",              std::make_pair(true,  Parameters::kOptimizerSlam2D())));
-		removedParameters_.insert(std::make_pair("RGBD/OptimizeSlam2d",              std::make_pair(true,  Parameters::kOptimizerSlam2D())));
+		removedParameters_.insert(std::make_pair("RGBD/OptimizeSlam2D",              std::make_pair(true,  Parameters::kRegForce3DoF())));
+		removedParameters_.insert(std::make_pair("RGBD/OptimizeSlam2d",              std::make_pair(true,  Parameters::kRegForce3DoF())));
 		removedParameters_.insert(std::make_pair("RGBD/OptimizeVarianceIgnored",     std::make_pair(true,  Parameters::kOptimizerVarianceIgnored())));

 		removedParameters_.insert(std::make_pair("Stereo/WinSize",                   std::make_pair(true,  Parameters::kStereoWinWidth())));
@@ -34,6 +34,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/VWDictionary.h>
 #include <rtabmap/core/util2d.h>
 #include <rtabmap/core/Features2d.h>
+#include <rtabmap/core/VisualWord.h>
+#include <rtabmap/core/Optimizer.h>
+#include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UStl.h>
@@ -62,7 +65,8 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 		_flowEps(Parameters::defaultVisCorFlowEps()),
 		_flowMaxLevel(Parameters::defaultVisCorFlowMaxLevel()),
 		_nndr(Parameters::defaultVisCorNNDR()),
-		_guessWinSize(Parameters::defaultVisCorGuessWinSize())
+		_guessWinSize(Parameters::defaultVisCorGuessWinSize()),
+		_bundleAdjustment(Parameters::defaultVisBundleAdjustment())
 {
 	_featureParameters = Parameters::getDefaultParameters();
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), _featureParameters.at(Parameters::kVisCorNNType())));
@@ -100,6 +104,8 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kVisCorFlowMaxLevel(), _flowMaxLevel);
 	Parameters::parse(parameters, Parameters::kVisCorNNDR(), _nndr);
 	Parameters::parse(parameters, Parameters::kVisCorGuessWinSize(), _guessWinSize);
+	Parameters::parse(parameters, Parameters::kVisBundleAdjustment(), _bundleAdjustment);
+	uInsert(_bundleParameters, parameters);

 	UASSERT_MSG(_minInliers >= 1, uFormat("value=%d", _minInliers).c_str());
 	UASSERT_MSG(_inlierDistance > 0.0f, uFormat("value=%f", _inlierDistance).c_str());
@@ -247,6 +253,7 @@ Transform RegistrationVis::computeTransformationImpl(
 		cv::Mat imageFrom = fromSignature.sensorData().imageRaw();
 		cv::Mat imageTo = toSignature.sensorData().imageRaw();

+		std::vector<int> orignalWordsFromIds;
 		if(fromSignature.getWords().empty())
 		{
 			if(fromSignature.sensorData().keypoints().empty())
@@ -283,7 +290,14 @@ Transform RegistrationVis::computeTransformationImpl(
 		}
 		else
 		{
-			kptsFrom = uValues(fromSignature.getWords());
+			kptsFrom.resize(fromSignature.getWords().size());
+			orignalWordsFromIds.resize(fromSignature.getWords().size());
+			int i=0;
+			for(std::multimap<int, cv::KeyPoint>::const_iterator iter=fromSignature.getWords().begin(); iter!=fromSignature.getWords().end(); ++iter)
+			{
+				kptsFrom[i] = iter->second;
+				orignalWordsFromIds[i++] = iter->first;
+			}
 		}

 		std::multimap<int, cv::KeyPoint> wordsFrom;
@@ -363,6 +377,7 @@ Transform RegistrationVis::computeTransformationImpl(
 				UASSERT(kptsFrom.size() == kptsFrom3D.size());
 				std::vector<cv::KeyPoint> kptsTo(kptsFrom.size());
 				std::vector<cv::Point3f> kptsFrom3DKept(kptsFrom3D.size());
+				std::vector<int> orignalWordsFromIdsCpy = orignalWordsFromIds;
 				int ki = 0;
 				for(unsigned int i=0; i<status.size(); ++i)
 				{
@@ -370,11 +385,19 @@ Transform RegistrationVis::computeTransformationImpl(
 					   uIsInBounds(cornersTo[i].x, 0.0f, float(imageTo.cols)) &&
 					   uIsInBounds(cornersTo[i].y, 0.0f, float(imageTo.rows)))
 					{
+						if(orignalWordsFromIdsCpy.size())
+						{
+							orignalWordsFromIds[ki] = orignalWordsFromIdsCpy[i];
+						}
 						kptsFrom[ki] = cv::KeyPoint(cornersFrom[i], 1);
 						kptsFrom3DKept[ki] = kptsFrom3D[i];
 						kptsTo[ki++] = cv::KeyPoint(cornersTo[i], 1);
 					}
 				}
+				if(orignalWordsFromIds.size())
+				{
+					orignalWordsFromIds.resize(ki);
+				}
 				kptsFrom.resize(ki);
 				kptsTo.resize(ki);
 				kptsFrom3DKept.resize(ki);
@@ -390,12 +413,13 @@ Transform RegistrationVis::computeTransformationImpl(
 				UASSERT(kptsTo3D.size() == 0 || kptsTo.size() == kptsTo3D.size());
 				for(unsigned int i=0; i< kptsFrom3DKept.size(); ++i)
 				{
-					wordsFrom.insert(std::make_pair(i, kptsFrom[i]));
-					words3From.insert(std::make_pair(i, kptsFrom3DKept[i]));
-					wordsTo.insert(std::make_pair(i, kptsTo[i]));
+					int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
+					wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
+					words3From.insert(std::make_pair(id, kptsFrom3DKept[i]));
+					wordsTo.insert(std::make_pair(id, kptsTo[i]));
 					if(kptsTo3D.size())
 					{
-						words3To.insert(std::make_pair(i, kptsTo3D[i]));
+						words3To.insert(std::make_pair(id, kptsTo3D[i]));
 					}
 				}
 				toSignature.sensorData().setFeatures(kptsTo, cv::Mat());
@@ -407,8 +431,9 @@ Transform RegistrationVis::computeTransformationImpl(
 				{
 					if(util3d::isFinite(kptsFrom3D[i]))
 					{
-						wordsFrom.insert(std::make_pair(i, kptsFrom[i]));
-						words3From.insert(std::make_pair(i, kptsFrom3D[i]));
+						int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
+						wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
+						words3From.insert(std::make_pair(id, kptsFrom3D[i]));
 					}
 				}
 				toSignature.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
@@ -705,9 +730,7 @@ Transform RegistrationVis::computeTransformationImpl(
 						UDEBUG("");

 						// Process results (Nearest Neighbor Distance Ratio)
-						int matchedID = descriptorsFrom.rows+descriptorsTo.rows;
 						int newToId = descriptorsFrom.rows;
-						int notMatchedFromId = 0;
 						std::map<int,int> addedWordsFrom; //<id, index>
 						std::map<int, int> duplicates; //<fromId, toId>
 						int newWords = 0;
@@ -759,7 +782,7 @@ Transform RegistrationVis::computeTransformationImpl(
 								if(matchedIndex >= 0)
 								{
 									matchedIndex = projectedIndexToDescIndex[matchedIndex];
-									int id = matchedID++;
+									int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndex]:matchedIndex;

 									if(addedWordsFrom.find(matchedIndex) != addedWordsFrom.end())
 									{
@@ -811,11 +834,11 @@ Transform RegistrationVis::computeTransformationImpl(
 						{
 							if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
 							{
-								wordsFrom.insert(std::make_pair(notMatchedFromId, kptsFrom[i]));
-								wordsDescFrom.insert(std::make_pair(notMatchedFromId, descriptorsFrom.row(i)));
-								words3From.insert(std::make_pair(notMatchedFromId, kptsFrom3D[i]));
+								int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
+								wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
+								wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(i)));
+								words3From.insert(std::make_pair(id, kptsFrom3D[i]));

-								++notMatchedFromId;
 								++addWordsFromNotMatched;
 							}
 						}
@@ -866,9 +889,15 @@ Transform RegistrationVis::computeTransformationImpl(
 					UDEBUG("");
 					// match between all descriptors
 					VWDictionary dictionary(_featureParameters);
-					std::list<int> fromWordIds = dictionary.addNewWords(descriptorsFrom, 1);
-					std::list<int> toWordIds;
+					std::list<int> fromWordIds;
+					for(unsigned int i=0; i<kptsFrom.size(); ++i)
+					{
+						int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
+						dictionary.addWord(new VisualWord(id, descriptorsFrom.row(i), 1));
+						fromWordIds.push_back(id);
+					}

+					std::list<int> toWordIds;
 					if(descriptorsTo.rows)
 					{
 						dictionary.update();
@@ -954,8 +983,8 @@ Transform RegistrationVis::computeTransformationImpl(
 		for(int dir=0; dir<(!_forwardEstimateOnly?2:1); ++dir)
 		{
 			// A to B
-			const Signature * signatureA;
-			const Signature * signatureB;
+			Signature * signatureA;
+			Signature * signatureB;
 			if(dir == 0)
 			{
 				signatureA = &fromSignature;
@@ -1140,6 +1169,115 @@ Transform RegistrationVis::computeTransformationImpl(
 			UDEBUG("from->to=%s", transforms[0].prettyPrint().c_str());
 			UDEBUG("from->from=%s", transforms[1].prettyPrint().c_str());
 		}
+
+		if(_bundleAdjustment > 0 &&
+			_estimationType < 2 &&
+			!transforms[0].isNull() &&
+			inliers[0].size() &&
+			fromSignature.getWords3().size() &&
+			toSignature.getWords().size() &&
+			fromSignature.sensorData().cameraModels().size() <= 1 &&
+			toSignature.sensorData().cameraModels().size() <= 1)
+		{
+			UASSERT(fromSignature.sensorData().stereoCameraModel().isValidForProjection() || (fromSignature.sensorData().cameraModels().size() == 1 && fromSignature.sensorData().cameraModels()[0].isValidForProjection()));
+			UASSERT(toSignature.sensorData().stereoCameraModel().isValidForProjection() || (toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValidForProjection()));
+			std::map<int, Transform> poses;
+			std::multimap<int, Link> links;
+			std::map<int, CameraModel> models;
+			std::map<int, cv::Point3f> points3DMap;
+			std::map<int, std::map<int, cv::Point2f> > wordReferences;
+
+			const CameraModel & cameraModelFrom = fromSignature.sensorData().stereoCameraModel().isValidForProjection()?fromSignature.sensorData().stereoCameraModel().left():fromSignature.sensorData().cameraModels()[0];
+			const CameraModel & cameraModelTo = toSignature.sensorData().stereoCameraModel().isValidForProjection()?toSignature.sensorData().stereoCameraModel().left():toSignature.sensorData().cameraModels()[0];
+			models.insert(std::make_pair(1, cameraModelFrom));
+			models.insert(std::make_pair(2, cameraModelTo));
+
+			poses.insert(std::make_pair(1, Transform::getIdentity()));
+			poses.insert(std::make_pair(2, transforms[0]));
+
+			links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transforms[0], variances[0], variances[0])));
+			if(!transforms[1].isNull() && inliers[1].size())
+			{
+				links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], variances[1], variances[1])));
+			}
+
+			for(unsigned int i=0; i<inliers[0].size(); ++i)
+			{
+				points3DMap.insert(*fromSignature.getWords3().find(inliers[0][i]));
+				std::map<int, cv::Point2f> ptMap;
+				/*if(fromSignature.getWords().size())
+				{
+					ptMap.insert(std::make_pair(1, fromSignature.getWords().find(inliers[0][i])->second.pt));
+				}*/
+				if(toSignature.getWords().size())
+				{
+					ptMap.insert(std::make_pair(2, toSignature.getWords().find(inliers[0][i])->second.pt));
+				}
+				wordReferences.insert(std::make_pair(inliers[0][i], ptMap));
+			}
+
+			for(unsigned int i=0; i<inliers[1].size(); ++i)
+			{
+				std::multimap<int, cv::Point3f>::const_iterator iter = fromSignature.getWords3().find(inliers[1][i]);
+				if(iter!=fromSignature.getWords3().end())
+				{
+					std::map<int, std::map<int, cv::Point2f> >::iterator jter = wordReferences.find(inliers[1][i]);
+					if(jter == wordReferences.end())
+					{
+						points3DMap.insert(*fromSignature.getWords3().find(inliers[1][i]));
+						std::map<int, cv::Point2f> ptMap;
+						if(fromSignature.getWords().size())
+						{
+							ptMap.insert(std::make_pair(1, fromSignature.getWords().find(inliers[1][i])->second.pt));
+						}
+						if(toSignature.getWords().size())
+						{
+							ptMap.insert(std::make_pair(2, toSignature.getWords().find(inliers[1][i])->second.pt));
+						}
+						wordReferences.insert(std::make_pair(inliers[1][i], ptMap));
+					}
+					else
+					{
+						if(jter->second.find(1) == jter->second.end())
+						{
+							jter->second.insert(std::make_pair(1, fromSignature.getWords().find(inliers[1][i])->second.pt));
+						}
+						if(jter->second.find(2) == jter->second.end())
+						{
+							jter->second.insert(std::make_pair(1, toSignature.getWords().find(inliers[1][i])->second.pt));
+						}
+					}
+				}
+			}
+
+			Optimizer * sba = Optimizer::create(_bundleAdjustment==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, _bundleParameters);
+			std::map<int, Transform> optimizedPoses = sba->optimizeBA(1, poses, links, models, points3DMap, wordReferences);
+			delete sba;
+
+			//update transform
+			if(optimizedPoses.size() == 2 &&
+				!optimizedPoses.begin()->second.isNull() &&
+				!optimizedPoses.rbegin()->second.isNull())
+			{
+				transforms[0] = optimizedPoses.rbegin()->second;
+				transforms[1].setNull();
+				// update 3D points, both from and to signatures
+				/*std::multimap<int, cv::Point3f> cpyWordsFrom3 = fromSignature.getWords3();
+				std::multimap<int, cv::Point3f> cpyWordsTo3 = toSignature.getWords3();
+				Transform invT = transforms[0].inverse();
+				for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
+				{
+					cpyWordsFrom3.find(iter->first)->second = iter->second;
+					if(cpyWordsTo3.find(iter->first) != cpyWordsTo3.end())
+					{
+						cpyWordsTo3.find(iter->first)->second = util3d::transformPoint(iter->second, invT);
+					}
+				}
+				fromSignature.setWords3(cpyWordsFrom3);
+				toSignature.setWords3(cpyWordsTo3);*/
+			}
+		}
+
 		if(!transforms[1].isNull())
 		{
 			if(transforms[0].isNull())
@@ -1184,7 +1184,8 @@ bool Rtabmap::process(
 						guess = newPose.inverse() * _optimizedPoses.at(*iter);
 					}

-					Transform transform = _memory->computeTransform(signature->id(), *iter, guess, &info);
+					// 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);

 					if(!transform.isNull())
 					{
@@ -1217,7 +1218,7 @@ bool Rtabmap::process(
 	}

 	timeProximityByTimeDetection = timer.ticks();
-	UINFO("timeLocalTimeDetection=%fs", timeProximityByTimeDetection);
+	UINFO("timeProximityByTimeDetection=%fs", timeProximityByTimeDetection);

 	//============================================================
 	// Bayes filter update
@@ -171,6 +171,7 @@ void RtabmapThread::publishMap(bool optimized, bool full, bool graphOnly) const

 void RtabmapThread::mainLoopBegin()
 {
+	ULogger::registerCurrentThread("Rtabmap");
 	if(_rtabmap == 0)
 	{
 		UERROR("Cannot start rtabmap thread if no rtabmap object is set! Stopping the thread...");
@@ -553,7 +553,16 @@ void SensorData::uncompressData(
 		cv::Mat * groundCellsRaw,
 		cv::Mat * obstacleCellsRaw)
 {
-	UDEBUG("%d", this->id());
+	UDEBUG("%d data(%d,%d,%d,%d,%d)", this->id(), imageRaw?1:0, depthRaw?1:0, laserScanRaw?1:0, userDataRaw?1:0, groundCellsRaw?1:0, obstacleCellsRaw?1:0);
+	if(imageRaw == 0 &&
+		depthRaw == 0 &&
+		laserScanRaw == 0 &&
+		userDataRaw == 0 &&
+		groundCellsRaw == 0 &&
+		obstacleCellsRaw == 0)
+	{
+		return;
+	}
 	uncompressDataConst(
 			imageRaw,
 			depthRaw,
@@ -1194,6 +1194,10 @@ void VWDictionary::addWord(VisualWord * vw)
 		{
 			_unusedWords.insert(std::pair<int, VisualWord *>(vw->id(), vw));
 		}
+		if(_lastWordId < vw->id())
+		{
+			_lastWordId = vw->id();
+		}
 	}
 }

@@ -35,8 +35,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d_correspondences.h"
 #include "rtabmap/core/util3d.h"

-#include "rtabmap/core/OptimizerG2O.h"
-
 #if CV_MAJOR_VERSION < 3
 #include "opencv/solvepnp.h"
 #endif
@@ -139,6 +139,7 @@ public:
 	int getGeneralLoggerType() const;
 	bool getGeneralLoggerPrintTime() const;
 	bool getGeneralLoggerPrintThreadId() const;
+	std::vector<std::string> getGeneralLoggerThreads() const;
 	bool isVerticalLayoutUsed() const;
 	bool imageRejectedShown() const;
 	bool imageHighestHypShown() const;
@@ -288,7 +289,6 @@ private slots:
 	void addParameter(const QString & value);
 	void updatePredictionPlot();
 	void updateKpROI();
-	void updateG2oVisibility();
 	void updateStereoDisparityVisibility();
 	void useOdomFeatures();
 	void changeWorkingDirectory();
@@ -0,0 +1,230 @@
+/****************************************************************************
+**
+** Copyright (c) 2012 Richard Steffen and/or its subsidiary(-ies).
+** All rights reserved.
+** Contact: rsteffen@messbild.de, rsteffen@uni-bonn.de
+**
+** QMultiComboBox is free to use unter the terms of the LGPL 2.1 License in
+** Free and Commercial Products.
+****************************************************************************/
+
+#include "QMultiComboBox.h"
+#include <QApplication>
+#include <QCoreApplication>
+#include <QDesktopWidget>
+
+
+QMultiComboBox::QMultiComboBox(QWidget *widget ) :
+    QComboBox(widget),
+    popheight_(0),
+    screenbound_(50),
+    popframe_(NULL, Qt::Popup)
+{
+
+    SetDisplayText("Not Set");
+
+    // setup the popup list
+    vlist_.setSelectionMode(QAbstractItemView::MultiSelection);
+    vlist_.setSelectionBehavior(QAbstractItemView::SelectItems);
+    vlist_.clearSelection();
+    popframe_.setLayout(new QVBoxLayout());
+    popframe_.layout()->addWidget(&vlist_);
+    popframe_.layout()->setContentsMargins(0,0,0,0);
+
+    connect(&vlist_, SIGNAL(itemChanged(QListWidgetItem*)), this, SLOT(scanItemSelect(QListWidgetItem*)));
+
+}
+
+
+QMultiComboBox::~QMultiComboBox()
+{
+    disconnect(&vlist_,0,0,0);
+}
+
+
+void QMultiComboBox::SetDisplayText(QString text)
+{
+    m_DisplayText_ = text;
+    const int textWidth = fontMetrics().width(text);
+    setMinimumWidth(textWidth + 30);
+    updateGeometry();
+    repaint();
+}
+
+
+QString QMultiComboBox::GetDisplayText() const
+{
+    return m_DisplayText_;
+}
+
+
+void QMultiComboBox::setPopupHeight(int h)
+{
+    popheight_ = h;
+}
+
+
+void QMultiComboBox::paintEvent(QPaintEvent *e)
+{
+    QStylePainter painter(this);
+    painter.setPen(palette().color(QPalette::Text));
+    // draw the combobox frame, focusrect and selected etc.
+    QStyleOptionComboBox opt;
+
+    initStyleOption(&opt);
+    opt.currentText = m_DisplayText_;
+    painter.drawComplexControl(QStyle::CC_ComboBox, opt);
+    // draw the icon and text
+    painter.drawControl(QStyle::CE_ComboBoxLabel, opt);
+}
+
+
+void QMultiComboBox::showPopup()
+{
+
+    QRect rec = QRect(geometry());
+
+    //QPoint p = this->mapToGlobal(QPoint(0,rec.height()));
+    //QRect rec2(p , p + QPoint(rec.width(), rec.height()));
+
+    // get the two possible list points and height
+    QRect screen = QApplication::desktop()->screenGeometry(this);
+    QPoint above = this->mapToGlobal(QPoint(0,0));
+    int aboveHeight = above.y() - screen.y();
+    QPoint below = this->mapToGlobal(QPoint(0,rec.height()));
+    int belowHeight = screen.bottom() - below.y();
+
+    // first activate it with height 1px to get all the items initialized
+    QRect rec2;
+    rec2.setTopLeft(below);
+    rec2.setWidth(rec.width());
+    rec.setHeight(1);
+    popframe_.setGeometry(rec2);
+    popframe_.raise();
+    popframe_.show();
+    QCoreApplication::processEvents();
+
+    // determine rect
+    int contheight = vlist_.count()*vlist_.sizeHintForRow(0) + 4; // +4 - should be determined by margins?
+    belowHeight = min(abs(belowHeight)-screenbound_, contheight);
+    aboveHeight = min(abs(aboveHeight)-screenbound_, contheight);
+
+    if (popheight_ > 0) // fixed
+    {
+        rec2.setHeight(popheight_);
+    }
+    else // dynamic
+    {
+        // do we use below or above
+        if (belowHeight==contheight || belowHeight>aboveHeight)
+        {
+            rec2.setTopLeft(below);
+            rec2.setHeight(belowHeight);
+        }
+        else
+        {
+            rec2.setTopLeft(above - QPoint(0,aboveHeight));
+            rec2.setHeight(aboveHeight);
+        }
+    }
+
+    popframe_.setGeometry(rec2);
+    popframe_.raise();
+    popframe_.show();
+}
+
+
+void QMultiComboBox::hidePopup()
+{
+    popframe_.hide();
+}
+
+
+void QMultiComboBox::addItem ( const QString & text, const QVariant & userData)
+{
+    QListWidgetItem* wi = new QListWidgetItem(text);
+    wi->setFlags(wi->flags() | Qt::ItemIsUserCheckable);
+    if (userData.toBool())
+        wi->setCheckState(Qt::Checked);
+    else
+        wi->setCheckState(Qt::Unchecked);
+    vlist_.addItem(wi);
+}
+
+
+int QMultiComboBox::count()
+{
+    return vlist_.count();
+}
+
+
+void QMultiComboBox::setCurrentIndex(int index)
+{
+    cout << __FUNCTION__ << "DONT USE THIS ................" << endl;
+}
+
+
+QString QMultiComboBox::currentText()
+{
+    return vlist_.currentItem()->text();
+}
+
+
+QString QMultiComboBox::itemText(int row)
+{
+    return vlist_.item(row)->text();
+}
+
+
+QVariant QMultiComboBox::itemData(int row)
+{
+    QListWidgetItem* item = vlist_.item(row);
+    if (item->checkState() == Qt::Checked) return QVariant(true);
+    return QVariant(false);
+}
+
+void QMultiComboBox::setItemChecked(int row, bool checked)
+{
+	QListWidgetItem* item = vlist_.item(row);
+	bool wasChecked = item->checkState() == Qt::Checked;
+	if (wasChecked != checked)
+	{
+		item->setCheckState(checked?Qt::Checked:Qt::Unchecked);
+		emit itemChanged();
+	}
+}
+
+
+void QMultiComboBox::scanItemSelect(QListWidgetItem* item)
+{
+
+    QList<QListWidgetItem*> list = vlist_.selectedItems();
+    for (int i = 0; i < list.count(); i++)
+    {
+        if (item->checkState() == Qt::Checked)
+        {
+            list[i]->setCheckState(Qt::Checked);
+        }
+        else
+        {
+            list[i]->setCheckState(Qt::Unchecked);
+        }
+        list[i]->setSelected(false);
+    }
+    emit itemChanged();
+}
+
+void QMultiComboBox::initStyleOption(QStyleOptionComboBox *option) const
+{
+    //Initializes the state, direction, rect, palette, and fontMetrics member variables based on the specified widget.
+    //This is a convenience function; the member variables can also be initialized manually.
+    option->initFrom(this);
+
+}
+
+void QMultiComboBox::clear()
+{
+	vlist_.clear();
+	QComboBox::clear();
+}
+
@@ -0,0 +1,92 @@
+/****************************************************************************
+**
+** Copyright (C) 2010 Richard Steffen and/or its subsidiary(-ies).
+** All rights reserved.
+** Contact: rsteffen@messbild.de, rsteffen@uni-bonn.de
+**
+** Observe the License Information
+**
+****************************************************************************/
+
+#ifndef __MULTIBOXCOMBO_H__
+#define __MULTIBOXCOMBO_H__
+
+#include <iostream>
+using namespace std;
+
+#include <QComboBox>
+#include <QListWidget>
+#include <QVBoxLayout>
+#include <QStylePainter>
+
+class QMultiComboBox: public QComboBox
+{
+    Q_OBJECT
+public:
+
+    /// Constructor
+    QMultiComboBox(QWidget *widget = 0);
+
+    virtual ~QMultiComboBox();
+
+    /// the main display text
+    void SetDisplayText(QString text);
+
+    /// get the main display text
+    QString GetDisplayText() const;
+
+    /// add a item to the list
+    void addItem(const QString& text, const QVariant& userData = QVariant());
+
+    /// custom paint
+    virtual void paintEvent(QPaintEvent *e);
+
+    /// set the height of the popup
+    void setPopupHeight(int h);
+
+    /// replace standard QComboBox Popup
+    void showPopup();
+    void hidePopup();
+
+    /// replace neccessary data access
+    int count();
+    void setCurrentIndex(int index);
+    QString currentText();
+    QString itemText(int row);
+    QVariant itemData(int row);
+    void setItemChecked(int row, bool checked);
+
+signals:
+    /// item changed
+    void itemChanged();
+
+public slots:
+
+    /// react on changes of the item checkbox
+    void scanItemSelect(QListWidgetItem* item);
+
+    /// the init style
+    void initStyleOption(QStyleOptionComboBox *option) const;
+
+    void clear();
+
+protected:
+
+    /// the height of the popup
+    int popheight_;
+
+    /// lower/upper screen bound
+    int screenbound_;
+
+    /// hold the main display text
+    QString m_DisplayText_;
+
+    /// popup frame
+    QFrame popframe_;
+
+    /// multi selection list in the popup frame
+    QListWidget vlist_;
+
+};
+
+#endif
@@ -0,0 +1,504 @@
+		  GNU LESSER GENERAL PUBLIC LICENSE
+		       Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+			    Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
+
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
+
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+  
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+			    NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
+		     END OF TERMS AND CONDITIONS
+
+           How to Apply These Terms to Your New Libraries
+
+  If you develop a new library, and you want it to be of the greatest
+possible use to the public, we recommend making it free software that
+everyone can redistribute and change.  You can do so by permitting
+redistribution under these terms (or, alternatively, under the terms of the
+ordinary General Public License).
+
+  To apply these terms, attach the following notices to the library.  It is
+safest to attach them to the start of each source file to most effectively
+convey the exclusion of warranty; and each file should have at least the
+"copyright" line and a pointer to where the full notice is found.
+
+    <one line to give the library's name and a brief idea of what it does.>
+    Copyright (C) <year>  <name of author>
+
+    This library is free software; you can redistribute it and/or
+    modify it under the terms of the GNU Lesser General Public
+    License as published by the Free Software Foundation; either
+    version 2.1 of the License, or (at your option) any later version.
+
+    This library is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+    Lesser General Public License for more details.
+
+    You should have received a copy of the GNU Lesser General Public
+    License along with this library; if not, write to the Free Software
+    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+
+Also add information on how to contact you by electronic and paper mail.
+
+You should also get your employer (if you work as a programmer) or your
+school, if any, to sign a "copyright disclaimer" for the library, if
+necessary.  Here is a sample; alter the names:
+
+  Yoyodyne, Inc., hereby disclaims all copyright interest in the
+  library `Frob' (a library for tweaking knobs) written by James Random Hacker.
+
+  <signature of Ty Coon>, 1 April 1990
+  Ty Coon, President of Vice
+
+That's all there is to it!
+
+
@@ -28,6 +28,7 @@ SET(headers_ui
 	./CreateSimpleCalibrationDialog.h
 	./ParametersToolBox.h
 	./DepthCalibrationDialog.h
+	./3rdParty/QMultiComboBox.h
 )

 SET(uis
@@ -94,6 +95,7 @@ SET(SRC_FILES
 	./CreateSimpleCalibrationDialog.cpp
 	./ParametersToolBox.cpp
 	./DepthCalibrationDialog.cpp
+	./3rdParty/QMultiComboBox.cpp
 	
 	${moc_srcs} 
 	${moc_uis} 
@@ -169,6 +169,7 @@ MainWindow::MainWindow(PreferencesDialog * prefDialog, QWidget * parent) :
 	_firstCall(true),
 	_progressCanceled(false)
 {
+	ULogger::registerCurrentThread("MainWindow");
 	UDEBUG("");

 	initGuiResource();
@@ -3474,6 +3475,7 @@ void MainWindow::applyPrefSettings(PreferencesDialog::PANEL_FLAGS flags)
 						 (_preferencesDialog->getWorkingDirectory()+QDir::separator()+LOG_FILE_NAME).toStdString(), true);
 		ULogger::setPrintTime(_preferencesDialog->getGeneralLoggerPrintTime());
 		ULogger::setPrintThreadId(_preferencesDialog->getGeneralLoggerPrintThreadId());
+		ULogger::setTreadIdFilter(_preferencesDialog->getGeneralLoggerThreads());
 	}
 }

@@ -185,6 +185,9 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	if(!Optimizer::isAvailable(Optimizer::kTypeG2O))
 	{
 		_ui->graphOptimization_type->setItemData(1, 0, Qt::UserRole - 1);
+		_ui->odom_f2m_bundleStrategy->setItemData(1, 0, Qt::UserRole - 1);
+		_ui->loopClosure_bundle->setItemData(1, 0, Qt::UserRole - 1);
+		_ui->groupBoxx_g2o->setEnabled(false);
 	}
 	if(!OptimizerG2O::isCSparseAvailable())
 	{
@@ -202,6 +205,11 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	{
 		_ui->graphOptimization_type->setItemData(2, 0, Qt::UserRole - 1);
 	}
+	if(!Optimizer::isAvailable(Optimizer::kTypeCVSBA))
+	{
+		_ui->odom_f2m_bundleStrategy->setItemData(2, 0, Qt::UserRole - 1);
+		_ui->loopClosure_bundle->setItemData(2, 0, Qt::UserRole - 1);
+	}
 #ifdef RTABMAP_VERTIGO
 	if(!Optimizer::isAvailable(Optimizer::kTypeG2O) && !Optimizer::isAvailable(Optimizer::kTypeGTSAM))
 #endif
@@ -397,6 +405,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->checkBox_logger_printTime, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteLoggingPanel()));
 	connect(_ui->checkBox_logger_printThreadId, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteLoggingPanel()));
 	connect(_ui->comboBox_loggerType, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteLoggingPanel()));
+	_ui->comboBox_loggerFilter->SetDisplayText("Select:");
+	connect(_ui->comboBox_loggerFilter, SIGNAL(itemChanged()), this, SLOT(makeObsoleteLoggingPanel()));

 	//Source panel
 	connect(_ui->general_doubleSpinBox_imgRate, SIGNAL(valueChanged(double)), this, SLOT(makeObsoleteSourcePanel()));
@@ -705,7 +715,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->spinBox_maxLocalLocationsRetrieved->setObjectName(Parameters::kRGBDMaxLocalRetrieved().c_str());

 	_ui->graphOptimization_type->setObjectName(Parameters::kOptimizerStrategy().c_str());
-	_ui->graphOptimization_slam2d->setObjectName(Parameters::kOptimizerSlam2D().c_str());
 	_ui->graphOptimization_iterations->setObjectName(Parameters::kOptimizerIterations().c_str());
 	_ui->graphOptimization_covarianceIgnored->setObjectName(Parameters::kOptimizerVarianceIgnored().c_str());
 	_ui->graphOptimization_fromGraphEnd->setObjectName(Parameters::kRGBDOptimizeFromGraphEnd().c_str());
@@ -715,7 +724,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :

 	_ui->comboBox_g2o_solver->setObjectName(Parameters::kg2oSolver().c_str());
 	_ui->comboBox_g2o_optimizer->setObjectName(Parameters::kg2oOptimizer().c_str());
-	_ui->doubleSpinBox_g2o_variance->setObjectName(Parameters::kg2oPixelVariance().c_str());
+	_ui->doubleSpinBox_g2o_pixelVariance->setObjectName(Parameters::kg2oPixelVariance().c_str());

 	_ui->graphPlan_goalReachedRadius->setObjectName(Parameters::kRGBDGoalReachedRadius().c_str());
 	_ui->graphPlan_goalsSavedInUserData->setObjectName(Parameters::kRGBDGoalsSavedInUserData().c_str());
@@ -772,6 +781,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->odom_flow_maxLevel_2->setObjectName(Parameters::kVisCorFlowMaxLevel().c_str());
 	_ui->odom_flow_iterations_2->setObjectName(Parameters::kVisCorFlowIterations().c_str());
 	_ui->odom_flow_eps_2->setObjectName(Parameters::kVisCorFlowEps().c_str());
+	_ui->loopClosure_bundle->setObjectName(Parameters::kVisBundleAdjustment().c_str());

 	//RegistrationIcp
 	_ui->globalDetection_icpMaxTranslation->setObjectName(Parameters::kIcpMaxTranslation().c_str());
@@ -834,6 +844,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_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());

 	//Odometry Mono
 	_ui->doubleSpinBox_minFlow->setObjectName(Parameters::kOdomMonoInitMinFlow().c_str());
@@ -891,7 +903,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->doubleSpinBox_kp_roi1, SIGNAL(valueChanged(double)), this, SLOT(updateKpROI()));
 	connect(_ui->doubleSpinBox_kp_roi2, SIGNAL(valueChanged(double)), this, SLOT(updateKpROI()));
 	connect(_ui->doubleSpinBox_kp_roi3, SIGNAL(valueChanged(double)), this, SLOT(updateKpROI()));
-	connect(_ui->graphOptimization_type, SIGNAL(currentIndexChanged(int)), this, SLOT(updateG2oVisibility()));
 	connect(_ui->checkBox_useOdomFeatures, SIGNAL(toggled(bool)), this, SLOT(useOdomFeatures()));

 	//Create a model from the stacked widgets
@@ -1276,6 +1287,10 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
 		_ui->checkBox_logger_printTime->setChecked(true);
 		_ui->checkBox_logger_printThreadId->setChecked(false);
 		_ui->comboBox_loggerType->setCurrentIndex(1);
+		for(int i=0; i<_ui->comboBox_loggerFilter->count(); ++i)
+		{
+			_ui->comboBox_loggerFilter->setItemChecked(i, false);
+		}
 	}
 	else if(groupBox->objectName() == _ui->groupBox_source0->objectName())
 	{
@@ -2369,6 +2384,45 @@ bool PreferencesDialog::validateForm()
 		}
 	}

+	// Registration bundle adjustment strategy
+	if(_ui->loopClosure_bundle->currentIndex() == 1 && !Optimizer::isAvailable(Optimizer::kTypeG2O))
+	{
+		QMessageBox::warning(this, tr("Parameter warning"),
+				tr("Selected visual registration bundle adjustment optimization strategy (g2o) is not available. RTAB-Map is not built "
+				   "with g2o. Bundle adjustment is disabled."));
+		_ui->loopClosure_bundle->setCurrentIndex(0);
+	}
+	if(_ui->loopClosure_bundle->currentIndex() == 2 && !Optimizer::isAvailable(Optimizer::kTypeCVSBA))
+	{
+		QMessageBox::warning(this, tr("Parameter warning"),
+				tr("Selected visual registration bundle adjustment optimization strategy (cvsba) is not available. RTAB-Map is not built "
+				   "with cvsba. Bundle adjustment is disabled."));
+		_ui->loopClosure_bundle->setCurrentIndex(0);
+	}
+
+	// Local bundle adjustment strategy for odometry F2M
+	if(_ui->odom_f2m_bundleStrategy->currentIndex() == 1 && !Optimizer::isAvailable(Optimizer::kTypeG2O))
+	{
+		QMessageBox::warning(this, tr("Parameter warning"),
+				tr("Selected odometry local bundle adjustment optimization strategy (g2o) is not available. RTAB-Map is not built "
+				   "with g2o. Bundle adjustment is disabled."));
+		_ui->odom_f2m_bundleStrategy->setCurrentIndex(0);
+	}
+	if(_ui->odom_f2m_bundleStrategy->currentIndex() == 2 && !Optimizer::isAvailable(Optimizer::kTypeCVSBA))
+	{
+		QMessageBox::warning(this, tr("Parameter warning"),
+				tr("Selected odometry local bundle adjustment optimization strategy (cvsba) is not available. RTAB-Map is not built "
+				   "with cvsba. Bundle adjustment is disabled."));
+		_ui->odom_f2m_bundleStrategy->setCurrentIndex(0);
+	}
+	if(_ui->odom_f2m_bundleStrategy->currentIndex() > 0 && _ui->loopClosure_correspondencesType->currentIndex() == 1)
+	{
+		QMessageBox::warning(this, tr("Parameter warning"),
+				tr("Odometry local bundle adjustment optimization cannot be used at the same time than Optical Flow correspondences "
+					"strategy (see Visual Registration panel). Bundle adjustment is disabled."));
+		_ui->odom_f2m_bundleStrategy->setCurrentIndex(0);
+	}
+
 	// verify that Robust and Reject threshold are not set at the same time
 	if(_ui->graphOptimization_robust->isChecked() && _ui->graphOptimization_maxError->value()>0.0)
 	{
@@ -2487,6 +2541,24 @@ void PreferencesDialog::showEvent ( QShowEvent * event )

 		this->setWindowTitle(tr("Preferences"));
 	}
+
+	// setup logger filter
+	std::map<std::string, unsigned long> threads = ULogger::getRegisteredThreads();
+	std::vector<std::string> threadsChecked = this->getGeneralLoggerThreads();
+	std::set<std::string> threadsCheckedSet(threadsChecked.begin(), threadsChecked.end());
+	_ui->comboBox_loggerFilter->clear();
+	for(std::map<std::string, unsigned long>::iterator iter=threads.begin(); iter!=threads.end(); ++iter)
+	{
+		if(threadsCheckedSet.find(iter->first.c_str()) != threadsCheckedSet.end())
+		{
+			_ui->comboBox_loggerFilter->addItem(QString(iter->first.c_str()), QVariant(true));
+		}
+		else
+		{
+			_ui->comboBox_loggerFilter->addItem(QString(iter->first.c_str()), QVariant(false));
+		}
+	}
+
 	this->readSettingsBegin();
 }

@@ -3625,11 +3697,6 @@ void PreferencesDialog::updateKpROI()
 	_ui->lineEdit_kp_roi->setText(strings.join(" "));
 }

-void PreferencesDialog::updateG2oVisibility()
-{
-	_ui->groupBox_g2o->setVisible(_ui->graphOptimization_type->currentIndex() == 1);
-}
-
 void PreferencesDialog::updateStereoDisparityVisibility()
 {
 	Src driver = this->getSourceDriver();
@@ -3776,6 +3843,18 @@ bool PreferencesDialog::getGeneralLoggerPrintThreadId() const
 {
 	return _ui->checkBox_logger_printThreadId->isChecked();
 }
+std::vector<std::string> PreferencesDialog::getGeneralLoggerThreads() const
+{
+	std::vector<std::string> threads;
+	for(int i=0; i<_ui->comboBox_loggerFilter->count(); ++i)
+	{
+		if(_ui->comboBox_loggerFilter->itemData(i).toBool())
+		{
+			threads.push_back(_ui->comboBox_loggerFilter->itemText(i).toStdString());
+		}
+	}
+	return threads;
+}
 bool PreferencesDialog::isVerticalLayoutUsed() const
 {
 	return _ui->checkBox_verticalLayoutUsed->isChecked();
@@ -115,7 +115,7 @@
      <item>
       <widget class="QLabel" name="label">
        <property name="text">
-         <string>See &quot;RTAB-Map settings/RGB-D SLAM/Loop closure constraint&quot; to tune the parameters used.</string>
+         <string>See &quot;RTAB-Map settings/Motion Estimation&quot; to tune the parameters used.</string>
        </property>
        <property name="wordWrap">
         <bool>true</bool>
@@ -128,7 +128,7 @@
   <item>
    <widget class="QGroupBox" name="groupBox_3">
     <property name="title">
-      <string>Refine links with ICP 3D</string>
+      <string>Refine links with ICP</string>
     </property>
     <property name="checkable">
      <bool>false</bool>
@@ -137,7 +137,7 @@
      <item>
       <widget class="QLabel" name="label_2">
        <property name="text">
-         <string>See &quot;RTAB-Map settings/RGB-D SLAM/ICP&quot; to tune the parameters used.</string>
+         <string>Laser scans are required. See &quot;RTAB-Map settings/Motion Estimation/ICP&quot; to tune the parameters used.</string>
        </property>
        <property name="wordWrap">
         <bool>true</bool>
@@ -7,7 +7,7 @@
    <x>0</x>
    <y>0</y>
    <width>976</width>
-    <height>731</height>
+    <height>747</height>
   </rect>
  </property>
  <property name="sizePolicy">
@@ -23,8 +23,8 @@
   <iconset resource="../GuiLib.qrc">
    <normaloff>:/images/RTAB-Map.ico</normaloff>:/images/RTAB-Map.ico</iconset>
  </property>
-  <layout class="QVBoxLayout" name="verticalLayout_26">
-   <item>
+  <layout class="QGridLayout" name="gridLayout_49">
+   <item row="0" column="0">
    <layout class="QHBoxLayout" name="horizontalLayout_2" stretch="0,1">
     <property name="spacing">
      <number>0</number>
@@ -63,7 +63,7 @@
          <property name="geometry">
           <rect>
            <x>0</x>
-            <y>-427</y>
+            <y>0</y>
            <width>673</width>
            <height>2520</height>
           </rect>
@@ -86,7 +86,7 @@
                <enum>QFrame::Raised</enum>
               </property>
               <property name="currentIndex">
-                <number>5</number>
+                <number>4</number>
               </property>
               <widget class="QWidget" name="page_22">
                <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -1894,6 +1894,19 @@ Show a yellow background when the number of odometry inliers goes under this thr
                    <string>Logging</string>
                   </property>
                   <layout class="QGridLayout" name="gridLayout_40" columnstretch="0,1">
+                    <item row="0" column="1">
+                     <widget class="QLabel" name="label_60">
+                      <property name="text">
+                       <string>Logger level.</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="comboBox_loggerLevel">
                      <property name="currentIndex">
@@ -1924,19 +1937,6 @@ Show a yellow background when the number of odometry inliers goes under this thr
                      </item>
                     </widget>
                    </item>
-                    <item row="0" column="1">
-                     <widget class="QLabel" name="label_60">
-                      <property name="text">
-                       <string>Logger level.</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="QComboBox" name="comboBox_loggerEventLevel">
                      <property name="currentIndex">
@@ -2113,6 +2113,29 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                      </property>
                     </widget>
                    </item>
+                    <item row="6" column="0">
+                     <widget class="QMultiComboBox" name="comboBox_loggerFilter">
+                      <property name="currentIndex">
+                       <number>-1</number>
+                      </property>
+                      <property name="sizeAdjustPolicy">
+                       <enum>QComboBox::AdjustToContents</enum>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="6" column="1">
+                     <widget class="QLabel" name="label_164">
+                      <property name="text">
+                       <string>Logger thread filter. If some threads are selected, only messages from these threads will be printed. If no thread is selected, then all messages are logged. </string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                      <property name="textInteractionFlags">
+                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                      </property>
+                     </widget>
+                    </item>
                   </layout>
                  </widget>
                 </item>
@@ -7867,13 +7890,13 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                </layout>
               </widget>
               <widget class="QWidget" name="page_19">
-                <layout class="QVBoxLayout" name="verticalLayout_94">
+                <layout class="QVBoxLayout" name="verticalLayout_26">
                 <item>
                  <widget class="QGroupBox" name="groupBox_graphOptimization2">
                   <property name="title">
                    <string>Graph Optimization</string>
                   </property>
-                   <layout class="QGridLayout" name="gridLayout_49" columnstretch="0,1">
+                   <layout class="QGridLayout" name="gridLayout_81" columnstretch="0,1">
                    <item row="0" column="0">
                     <widget class="QComboBox" name="graphOptimization_type">
                      <item>
@@ -7893,169 +7916,24 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                      </item>
                     </widget>
                    </item>
-                    <item row="2" column="0">
-                     <widget class="QSpinBox" name="graphOptimization_iterations">
-                      <property name="minimum">
-                       <number>0</number>
-                      </property>
-                      <property name="maximum">
-                       <number>10000</number>
-                      </property>
-                      <property name="value">
-                       <number>100</number>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="4" column="0">
-                     <widget class="QCheckBox" name="graphOptimization_covarianceIgnored">
-                      <property name="text">
-                       <string/>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="9" column="0">
-                     <widget class="QCheckBox" name="graphOptimization_fromGraphEnd">
-                      <property name="text">
-                       <string/>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="9" column="1">
-                     <widget class="QLabel" name="label_151">
-                      <property name="text">
-                       <string>Optimize graph from the newest node.</string>
-                      </property>
-                      <property name="wordWrap">
-                       <bool>true</bool>
-                      </property>
-                      <property name="textInteractionFlags">
-                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="8" column="1">
-                     <widget class="Line" name="line">
-                      <property name="orientation">
-                       <enum>Qt::Horizontal</enum>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="5" column="0">
-                     <widget class="QCheckBox" name="graphOptimization_slam2d">
-                      <property name="text">
-                       <string/>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="5" column="1">
-                     <widget class="QLabel" name="label_164">
-                      <property name="text">
-                       <string>2d SLAM: use fast 3DoF (x, y, theta) optimization instead of 6DoF (x, y, z, roll, pitch, yaw) optimization.</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="1">
                     <widget class="QLabel" name="label_45">
                      <property name="text">
-                       <string>Graph optimization algorithm.</string>
+                       <string>Graph optimization algorithm. See Optimizer panel.</string>
                      </property>
                      <property name="textInteractionFlags">
                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                      </property>
                     </widget>
                    </item>
-                    <item row="11" column="1">
-                     <widget class="QLabel" name="label_183">
-                      <property name="text">
-                       <string>-If false, the graph is optimized from the oldest node of the current graph. It can be useful to preserve the map referential from the oldest node. An odometry correction between frames /map to /odom is computed. Warning: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).</string>
-                      </property>
-                      <property name="wordWrap">
-                       <bool>true</bool>
-                      </property>
-                      <property name="textInteractionFlags">
-                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="10" column="1">
-                     <widget class="QLabel" name="label_211">
-                      <property name="text">
-                       <string>-If true, there is no odometry correction computed. All previous poses in the map are corrected instead, not the last one (which corresponds to latest odometry value). So, the transform between frames /map to /odom will be always Identity even on loop closures.</string>
-                      </property>
-                      <property name="wordWrap">
-                       <bool>true</bool>
-                      </property>
-                      <property name="textInteractionFlags">
-                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="2" column="1">
-                     <widget class="QLabel" name="label_31">
-                      <property name="text">
-                       <string>Iterations.</string>
-                      </property>
-                      <property name="textInteractionFlags">
-                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="3" column="1">
-                     <widget class="QLabel" name="label_64">
-                      <property name="text">
-                       <string>Stop optimizing when the error improvement is less than this value.</string>
-                      </property>
-                      <property name="textInteractionFlags">
-                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="3" column="0">
-                     <widget class="QDoubleSpinBox" name="graphOptimization_stopEpsilon">
-                      <property name="decimals">
-                       <number>5</number>
-                      </property>
-                      <property name="minimum">
-                       <double>0.000000000000000</double>
-                      </property>
-                      <property name="maximum">
-                       <double>1.000000000000000</double>
-                      </property>
-                      <property name="singleStep">
-                       <double>0.001000000000000</double>
-                      </property>
-                      <property name="value">
-                       <double>0.001000000000000</double>
-                      </property>
-                     </widget>
-                    </item>
-                    <item row="4" column="1">
-                     <widget class="QLabel" name="label_141">
-                      <property name="text">
-                       <string>Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links (transitional and rotational variances).</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">
+                    <item row="1" column="0">
                     <widget class="QCheckBox" name="graphOptimization_robust">
                      <property name="text">
                       <string/>
                      </property>
                     </widget>
                    </item>
-                    <item row="6" column="1">
+                    <item row="1" column="1">
                     <widget class="QLabel" name="label_258">
                      <property name="text">
                       <string>Robust graph optimization using Vertigo (only for g2o and GTSAM optimization strategies). This approach can filter wrong loop closure detections.</string>
@@ -8068,7 +7946,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                      </property>
                     </widget>
                    </item>
-                    <item row="7" column="0">
+                    <item row="2" column="0">
                     <widget class="QDoubleSpinBox" name="graphOptimization_maxError">
                      <property name="suffix">
                       <string> m</string>
@@ -8090,10 +7968,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="1">
                     <widget class="QLabel" name="label_259">
                      <property name="text">
-                       <string>Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph.</string>
+                       <string>Reject loop closures if optimization error is greater than this value (0=disabled). This will help to detect when a wrong loop closure is added to the graph. It should not be used at the same time than Vertigo above.</string>
                      </property>
                      <property name="wordWrap">
                       <bool>true</bool>
@@ -8103,89 +7981,57 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                      </property>
                     </widget>
                    </item>
-                    <item row="1" column="1">
-                     <widget class="QGroupBox" name="groupBox_g2o">
-                      <property name="title">
-                       <string>g2o</string>
+                    <item row="3" column="0" rowspan="2">
+                     <widget class="QCheckBox" name="graphOptimization_fromGraphEnd">
+                      <property name="text">
+                       <string/>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="3" column="1">
+                     <widget class="Line" name="line">
+                      <property name="orientation">
+                       <enum>Qt::Horizontal</enum>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="4" column="1">
+                     <widget class="QLabel" name="label_151">
+                      <property name="text">
+                       <string>Optimize graph from the newest node.</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="1">
+                     <widget class="QLabel" name="label_211">
+                      <property name="text">
+                       <string>-If true, there is no odometry correction computed. All previous poses in the map are corrected instead, not the last one (which corresponds to latest odometry value). So, the transform between frames /map to /odom will be always Identity even on loop closures.</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="1">
+                     <widget class="QLabel" name="label_183">
+                      <property name="text">
+                       <string>-If false, the graph is optimized from the oldest node of the current graph. It can be useful to preserve the map referential from the oldest node. An odometry correction between frames /map to /odom is computed. Warning: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                      <property name="textInteractionFlags">
+                       <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                      </property>
-                      <layout class="QGridLayout" name="gridLayout_70" columnstretch="0,1">
-                       <item row="1" column="0">
-                        <widget class="QComboBox" name="comboBox_g2o_optimizer">
-                         <item>
-                          <property name="text">
-                           <string>Levenberg</string>
-                          </property>
-                         </item>
-                         <item>
-                          <property name="text">
-                           <string>Gauss Newton</string>
-                          </property>
-                         </item>
-                        </widget>
-                       </item>
-                       <item row="0" column="0">
-                        <widget class="QComboBox" name="comboBox_g2o_solver">
-                         <item>
-                          <property name="text">
-                           <string>CSparse</string>
-                          </property>
-                         </item>
-                         <item>
-                          <property name="text">
-                           <string>PCG</string>
-                          </property>
-                         </item>
-                         <item>
-                          <property name="text">
-                           <string>Cholmod</string>
-                          </property>
-                         </item>
-                        </widget>
-                       </item>
-                       <item row="0" column="1">
-                        <widget class="QLabel" name="label_107">
-                         <property name="text">
-                          <string>Solver.</string>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="1" column="1">
-                        <widget class="QLabel" name="label_138">
-                         <property name="text">
-                          <string>Optimization algorithm.</string>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="2" column="0">
-                        <widget class="QDoubleSpinBox" name="doubleSpinBox_g2o_variance">
-                         <property name="enabled">
-                          <bool>false</bool>
-                         </property>
-                         <property name="decimals">
-                          <number>4</number>
-                         </property>
-                         <property name="minimum">
-                          <double>0.000100000000000</double>
-                         </property>
-                         <property name="maximum">
-                          <double>10.000000000000000</double>
-                         </property>
-                         <property name="value">
-                          <double>1.000000000000000</double>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="2" column="1">
-                        <widget class="QLabel" name="label_148">
-                         <property name="enabled">
-                          <bool>false</bool>
-                         </property>
-                         <property name="text">
-                          <string>Pixel variance for SBA.</string>
-                         </property>
-                        </widget>
-                       </item>
-                      </layout>
                     </widget>
                    </item>
                   </layout>
@@ -8198,8 +8044,8 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                   </property>
                   <property name="sizeHint" stdset="0">
                    <size>
-                     <width>20</width>
-                     <height>1338</height>
+                     <width>17</width>
+                     <height>2098</height>
                    </size>
                   </property>
                  </spacer>
@@ -9474,7 +9320,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                           <item>
                            <widget class="QLabel" name="label_226">
                             <property name="text">
-                              <string>The approach maintains a local map of features to match to. It adds incrementally new points to local map up to maximum map size, then removes old points as new points are added. See &quot;Motion Estimation&quot; panel for registration parameters. Note that only visual registration parameters are used in this approach. If you want to use a different registration pipeline than Visual (e.g. with ICP), use &quot;Frame to Frame&quot; approach instead.</string>
+                              <string>The approach maintains a local map of features to match to. It adds incrementally new points to local map up to maximum map size, then removes old points as new points are added. See &quot;Motion Estimation&quot; panel for registration parameters.</string>
                             </property>
                             <property name="wordWrap">
                              <bool>true</bool>
@@ -9486,6 +9332,35 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                           </item>
                           <item>
                            <layout class="QGridLayout" name="gridLayout_29" columnstretch="0,0,1">
+                             <item row="1" column="0">
+                              <widget class="QSpinBox" name="spinBox_odom_f2m_maxNewFeatures">
+                               <property name="minimum">
+                                <number>0</number>
+                               </property>
+                               <property name="maximum">
+                                <number>999999</number>
+                               </property>
+                               <property name="singleStep">
+                                <number>1</number>
+                               </property>
+                               <property name="value">
+                                <number>0</number>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="3" column="2">
+                              <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>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
                             <item row="2" column="2">
                              <widget class="QLabel" name="label_195">
                               <property name="text">
@@ -9564,35 +9439,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                               </property>
                              </widget>
                             </item>
-                             <item row="1" column="0">
-                              <widget class="QSpinBox" name="spinBox_odom_f2m_maxNewFeatures">
-                               <property name="minimum">
-                                <number>0</number>
-                               </property>
-                               <property name="maximum">
-                                <number>999999</number>
-                               </property>
-                               <property name="singleStep">
-                                <number>1</number>
-                               </property>
-                               <property name="value">
-                                <number>0</number>
-                               </property>
-                              </widget>
-                             </item>
-                             <item row="3" column="2">
-                              <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>
-                               </property>
-                               <property name="wordWrap">
-                                <bool>true</bool>
-                               </property>
-                               <property name="textInteractionFlags">
-                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                               </property>
-                              </widget>
-                             </item>
                             <item row="3" column="0">
                              <widget class="QDoubleSpinBox" name="doubleSpinBox_odom_f2m_scanRadius">
                               <property name="suffix">
@@ -9613,6 +9459,61 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                               </property>
                              </widget>
                             </item>
+                             <item row="5" column="2">
+                              <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>
+                               </property>
+                               <property name="wordWrap">
+                                <bool>true</bool>
+                               </property>
+                               <property name="textInteractionFlags">
+                                <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                               </property>
+                              </widget>
+                             </item>
+                             <item row="6" column="2">
+                              <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>
+                               </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="QComboBox" name="odom_f2m_bundleStrategy">
+                               <property name="sizeAdjustPolicy">
+                                <enum>QComboBox::AdjustToContents</enum>
+                               </property>
+                               <item>
+                                <property name="text">
+                                 <string>Disabled</string>
+                                </property>
+                               </item>
+                               <item>
+                                <property name="text">
+                                 <string>g2o</string>
+                                </property>
+                               </item>
+                               <item>
+                                <property name="text">
+                                 <string>cvsba</string>
+                                </property>
+                               </item>
+                              </widget>
+                             </item>
+                             <item row="6" column="0">
+                              <widget class="QSpinBox" name="odom_f2m_bundleMaxFrames">
+                               <property name="maximum">
+                                <number>999999</number>
+                               </property>
+                              </widget>
+                             </item>
                            </layout>
                           </item>
                           <item>
@@ -10271,45 +10172,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                   <layout class="QVBoxLayout" name="verticalLayout_41" stretch="0,0,0,1">
                    <item>
                     <layout class="QGridLayout" name="gridLayout_23" columnstretch="0,1">
-                      <item row="1" column="1">
-                       <widget class="QLabel" name="label_146">
-                        <property name="text">
-                         <string>Motion estimation approach using 3D and/or 2D visual words correspondences.</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="1">
-                       <widget class="QLabel" name="label_262">
-                        <property name="text">
-                         <string>Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.</string>
-                        </property>
-                        <property name="wordWrap">
-                         <bool>true</bool>
-                        </property>
-                        <property name="textInteractionFlags">
-                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                        </property>
-                       </widget>
-                      </item>
-                      <item row="2" column="1">
-                       <widget class="QLabel" name="label_2">
-                        <property name="text">
-                         <string>Minimum correspondences to accept the estimated transformation.</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="0">
                       <widget class="QSpinBox" name="loopClosure_bowIterations">
                        <property name="minimum">
@@ -10434,6 +10296,80 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
                        </property>
                       </widget>
                      </item>
+                      <item row="1" column="1">
+                       <widget class="QLabel" name="label_146">
+                        <property name="text">
+                         <string>Motion estimation approach using 3D and/or 2D visual words correspondences.</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="1">
+                       <widget class="QLabel" name="label_262">
+                        <property name="text">
+                         <string>Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="2" column="1">
+                       <widget class="QLabel" name="label_2">
+                        <property name="text">
+                         <string>Minimum correspondences to accept the estimated transformation.</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="QComboBox" name="loopClosure_bundle">
+                        <property name="sizeAdjustPolicy">
+                         <enum>QComboBox::AdjustToContentsOnFirstShow</enum>
+                        </property>
+                        <item>
+                         <property name="text">
+                          <string>Disabled</string>
+                         </property>
+                        </item>
+                        <item>
+                         <property name="text">
+                          <string>g2o</string>
+                         </property>
+                        </item>
+                        <item>
+                         <property name="text">
+                          <string>cvsba</string>
+                         </property>
+                        </item>
+                       </widget>
+                      </item>
+                      <item row="7" column="1">
+                       <widget class="QLabel" name="label_346">
+                        <property name="text">
+                         <string>Refine transformation with bundle adjustment. See Optimizer panel.</string>
+                        </property>
+                        <property name="wordWrap">
+                         <bool>true</bool>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
                     </layout>
                    </item>
                    <item>
@@ -11670,6 +11606,190 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                 </item>
                </layout>
               </widget>
+               <widget class="QWidget" name="page_65">
+                <layout class="QVBoxLayout" name="verticalLayout_114">
+                 <item>
+                  <widget class="QGroupBox" name="groupBox_optimizer1">
+                   <property name="title">
+                    <string>Optimizer</string>
+                   </property>
+                   <layout class="QVBoxLayout" name="verticalLayout_113">
+                    <item>
+                     <layout class="QGridLayout" name="gridLayout_80" columnstretch="0,1">
+                      <item row="0" column="0">
+                       <widget class="QSpinBox" name="graphOptimization_iterations">
+                        <property name="minimum">
+                         <number>0</number>
+                        </property>
+                        <property name="maximum">
+                         <number>10000</number>
+                        </property>
+                        <property name="value">
+                         <number>100</number>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="1" column="0">
+                       <widget class="QDoubleSpinBox" name="graphOptimization_stopEpsilon">
+                        <property name="decimals">
+                         <number>5</number>
+                        </property>
+                        <property name="minimum">
+                         <double>0.000000000000000</double>
+                        </property>
+                        <property name="maximum">
+                         <double>1.000000000000000</double>
+                        </property>
+                        <property name="singleStep">
+                         <double>0.001000000000000</double>
+                        </property>
+                        <property name="value">
+                         <double>0.001000000000000</double>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="2" column="1">
+                       <widget class="QLabel" name="label_141">
+                        <property name="text">
+                         <string>Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links (transitional and rotational variances).</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="1">
+                       <widget class="QLabel" name="label_64">
+                        <property name="text">
+                         <string>Stop optimizing when the error improvement is less than this value.</string>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="2" column="0">
+                       <widget class="QCheckBox" name="graphOptimization_covarianceIgnored">
+                        <property name="text">
+                         <string/>
+                        </property>
+                       </widget>
+                      </item>
+                      <item row="0" column="1">
+                       <widget class="QLabel" name="label_31">
+                        <property name="text">
+                         <string>Iterations.</string>
+                        </property>
+                        <property name="textInteractionFlags">
+                         <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                        </property>
+                       </widget>
+                      </item>
+                     </layout>
+                    </item>
+                    <item>
+                     <widget class="QGroupBox" name="groupBoxx_g2o">
+                      <property name="title">
+                       <string>g2o</string>
+                      </property>
+                      <layout class="QGridLayout" name="gridLayout_70" columnstretch="0,1">
+                       <item row="1" column="0">
+                        <widget class="QComboBox" name="comboBox_g2o_optimizer">
+                         <item>
+                          <property name="text">
+                           <string>Levenberg</string>
+                          </property>
+                         </item>
+                         <item>
+                          <property name="text">
+                           <string>Gauss Newton</string>
+                          </property>
+                         </item>
+                        </widget>
+                       </item>
+                       <item row="0" column="0">
+                        <widget class="QComboBox" name="comboBox_g2o_solver">
+                         <item>
+                          <property name="text">
+                           <string>CSparse</string>
+                          </property>
+                         </item>
+                         <item>
+                          <property name="text">
+                           <string>PCG</string>
+                          </property>
+                         </item>
+                         <item>
+                          <property name="text">
+                           <string>Cholmod</string>
+                          </property>
+                         </item>
+                        </widget>
+                       </item>
+                       <item row="0" column="1">
+                        <widget class="QLabel" name="label_107">
+                         <property name="text">
+                          <string>Solver.</string>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="1">
+                        <widget class="QLabel" name="label_138">
+                         <property name="text">
+                          <string>Optimization algorithm.</string>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="2" column="1">
+                        <widget class="QLabel" name="label_148">
+                         <property name="text">
+                          <string>Pixel variance used for bundle adjustment.</string>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="2" column="0">
+                        <widget class="QDoubleSpinBox" name="doubleSpinBox_g2o_pixelVariance">
+                         <property name="decimals">
+                          <number>5</number>
+                         </property>
+                         <property name="minimum">
+                          <double>0.000000000000000</double>
+                         </property>
+                         <property name="maximum">
+                          <double>1.000000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.001000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>0.001000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                      </layout>
+                     </widget>
+                    </item>
+                   </layout>
+                  </widget>
+                 </item>
+                 <item>
+                  <spacer name="verticalSpacer_58">
+                   <property name="orientation">
+                    <enum>Qt::Vertical</enum>
+                   </property>
+                   <property name="sizeHint" stdset="0">
+                    <size>
+                     <width>20</width>
+                     <height>2185</height>
+                    </size>
+                   </property>
+                  </spacer>
+                 </item>
+                </layout>
+               </widget>
               <widget class="QWidget" name="page_29">
                <layout class="QVBoxLayout" name="verticalLayout_57">
                 <item>
@@ -13487,7 +13607,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
     </item>
    </layout>
   </item>
-   <item>
+   <item row="1" column="0">
    <layout class="QHBoxLayout" name="horizontalLayout_3">
     <item>
      <widget class="QRadioButton" name="radioButton_basic">
@@ -13527,6 +13647,11 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
   <header>../include/rtabmap/utilite/UPlot.h</header>
   <container>1</container>
  </customwidget>
+  <customwidget>
+   <class>QMultiComboBox</class>
+   <extends>QComboBox</extends>
+   <header>3rdParty/QMultiComboBox.h</header>
+  </customwidget>
 </customwidgets>
 <resources>
  <include location="../GuiLib.qrc"/>
@@ -31,6 +31,8 @@
 #include <time.h>
 #include <string>
 #include <vector>
+#include <map>
+#include <set>

 #include <stdarg.h>

@@ -346,6 +348,21 @@ public:
 	static void setEventLevel(ULogger::Level eventSentLevel) {eventLevel_ = eventSentLevel;}
 	static ULogger::Level eventLevel() {return eventLevel_;}

+	/**
+	 * If not empty, only show log messages from threads included in this list.
+	 */
+	static void setTreadIdFilter(const std::set<unsigned long> & ids) {threadIdFilter_ = ids;}
+	static void setTreadIdFilter(const std::vector<std::string> & ids);
+	static const std::set<unsigned long> & getTreadIdFilter() {return threadIdFilter_;}
+
+	/**
+	 * Threads can register to this list. If name is empty, it will
+	 * clear the thread in the list. Should be called from the thread itself.
+	 */
+	static void registerCurrentThread(const std::string & name);
+	static void unregisterCurrentThread();
+	static std::map<std::string, unsigned long> getRegisteredThreads();
+
    /**
     * Reset to default parameters.
     */
@@ -550,6 +567,9 @@ private:
 	static bool buffered_;

 	static std::string bufferedMsgs_;
+
+	static std::set<unsigned long> threadIdFilter_;
+	static std::map<std::string, unsigned long> registeredThreads_;
 };

 #endif // ULOGGER_H
@@ -63,6 +63,8 @@ UMutex ULogger::loggerMutex_;
 const std::string ULogger::kDefaultLogFileName = "./ULog.txt";
 std::string ULogger::logFileName_;
 std::string ULogger::bufferedMsgs_;
+std::set<unsigned long> ULogger::threadIdFilter_;
+std::map<std::string, unsigned long> ULogger::registeredThreads_;

 /**
 * This class is used to write logs in the console. This class cannot
@@ -199,6 +201,58 @@ void ULogger::setType(Type type, const std::string &fileName, bool append)
    loggerMutex_.unlock();
 }

+void ULogger::setTreadIdFilter(const std::vector<std::string> & ids)
+{
+	loggerMutex_.lock();
+	threadIdFilter_.clear();
+	for(unsigned int i=0;i<ids.size();++i)
+	{
+		if(registeredThreads_.find(ids[i]) != registeredThreads_.end())
+		{
+			threadIdFilter_.insert(registeredThreads_.at(ids[i]));
+		}
+	}
+	loggerMutex_.unlock();
+}
+
+void ULogger::registerCurrentThread(const std::string & name)
+{
+	loggerMutex_.lock();
+	UASSERT(!name.empty());
+	uInsert(registeredThreads_, std::make_pair(name, UThread::currentThreadId()));
+	loggerMutex_.unlock();
+}
+
+void ULogger::unregisterCurrentThread()
+{
+	loggerMutex_.lock();
+
+	unsigned long id = UThread::currentThreadId();
+	for(std::map<std::string, unsigned long>::iterator iter=registeredThreads_.begin(); iter!=registeredThreads_.end();)
+	{
+		if(iter->second == id)
+		{
+			iter = registeredThreads_.erase(iter);
+			threadIdFilter_.erase(id);
+			break;
+		}
+		else
+		{
+			++iter;
+		}
+	}
+
+	loggerMutex_.unlock();
+}
+
+std::map<std::string, unsigned long> ULogger::getRegisteredThreads()
+{
+	loggerMutex_.lock();
+	std::map<std::string, unsigned long> out = registeredThreads_;
+	loggerMutex_.unlock();
+	return out;
+}
+
 void ULogger::reset()
 {
 	ULogger::setType(ULogger::kTypeNoLog);
@@ -323,6 +377,13 @@ void ULogger::write(ULogger::Level level,
 		// No need to show an empty message if we don't print where.
 		return;
 	}
+	if(level < kFatal &&
+		threadIdFilter_.size() &&
+		threadIdFilter_.find(UThread::currentThreadId()) == threadIdFilter_.end())
+	{
+		loggerMutex_.unlock();
+		return;
+	}

    if(level >= level_ || level >= eventLevel_)
    {