0.11.13. OdometryF2M: fixed/improved bundle adjustment option. OptimizerG2O: now doing stereo BA instead of mono BA (interface of Optimizer::optimzeBA() has slightly changed to include depth in word references). Registration and Odometry set angular and linear variances separately. RegistrationVis set x100 smaller variance for rotation. Added keypoints3D member to SensorData. Added Transform::getAngle() for convenience. Odometry normalizes variances. RtabmapThread does variance summation when frames are discarded. Updated how variance is computed by util3d::estimateMotion3Dto2D(), ignoring very large variance from the computation (stereo issue with very far matched features). MainWindow: added option to align with ground truth or not (default true as before), added for odometry statistics (including bundle stuff), update ground truth statistics every time a graph is loaded/optimized. CameraRGB can now load odometry files (to fake an input odometry). PreferencesDialog: Added option to Odometry so it can use a different registration approach than the default one (the one used for loop closure). Parameters: added new g2o/Solver option 3 (Eigen), added g2o/RobustKernelDelta, g2o/Baseline and Odom/VisKeyFrameThr. Database: from 0.11.13, ignoring patch version when comparing is database version is newer than app version used.

2026-09-02 01:20:25 +08:00 · 2017-01-03 21:05:00 -05:00
parent 6550a52cee
commit 6abed48af6
48 changed files with 2202 additions and 1329 deletions
--- a/corelib/src/CameraRGB.cpp
+++ b/corelib/src/CameraRGB.cpp
@@ -74,7 +74,8 @@ CameraImages::CameraImages() :
 		_depthFromScanFillHoles(1),
 		_depthFromScanFillHolesFromBorder(false),
 		_filenamesAreTimestamps(false),
-		syncImageRateWithStamps_(true),
+		_syncImageRateWithStamps(true),
+		_odometryFormat(0),
 		_groundTruthFormat(0),
 		_captureDelay(0.0)
 	{}
@@ -102,7 +103,8 @@ CameraImages::CameraImages(const std::string & path,
 	_depthFromScanFillHoles(1),
 	_depthFromScanFillHolesFromBorder(false),
 	_filenamesAreTimestamps(false),
-	syncImageRateWithStamps_(true),
+	_syncImageRateWithStamps(true),
+	_odometryFormat(0),
 	_groundTruthFormat(0),
 	_captureDelay(0.0)
 {
@@ -226,7 +228,8 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 	}

 	bool success = _dir->isValid();
-	stamps_.clear();
+	_stamps.clear();
+	odometry_.clear();
 	groundTruth_.clear();
 	if(success)
 	{
@@ -246,7 +249,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 					double stamp = uStr2Double(sec + "." + decimals);
 					if(stamp > 0.0)
 					{
-						stamps_.push_back(stamp);
+						_stamps.push_back(stamp);
 					}
 					else
 					{
@@ -254,19 +257,19 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 					}
 				}
 			}
-			if(stamps_.size() != this->imagesCount())
+			if(_stamps.size() != this->imagesCount())
 			{
 				UERROR("The stamps count is not the same as the images (%d vs %d)! "
 					   "Converting filenames to timestamps is activated.",
-						(int)stamps_.size(), this->imagesCount());
-				stamps_.clear();
+						(int)_stamps.size(), this->imagesCount());
+				_stamps.clear();
 				success = false;
 			}
 		}
-		else if(timestampsPath_.size())
+		else if(_timestampsPath.size())
 		{
 			std::ifstream file;
-			file.open(timestampsPath_.c_str(), std::ifstream::in);
+			file.open(_timestampsPath.c_str(), std::ifstream::in);
 			while(file.good())
 			{
 				std::string str;
@@ -290,120 +293,29 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 					}
 					stampStr = stampStr+'.'+millisecStr;
 				}
-				stamps_.push_back(uStr2Double(stampStr));
+				_stamps.push_back(uStr2Double(stampStr));
 			}

 			file.close();

-			if(stamps_.size() != this->imagesCount())
+			if(_stamps.size() != this->imagesCount())
 			{
 				UERROR("The stamps count (%d) is not the same as the images (%d)! Please remove "
 						"the timestamps file path if you don't want to use them (current file path=%s).",
-						(int)stamps_.size(), this->imagesCount(), timestampsPath_.c_str());
-				stamps_.clear();
+						(int)_stamps.size(), this->imagesCount(), _timestampsPath.c_str());
+				_stamps.clear();
 				success = false;
 			}
 		}

-		if(groundTruthPath_.size())
+		if(success && _odometryPath.size())
 		{
-			std::map<int, Transform> poses;
-			std::map<int, double> stamps;
-			if(!graph::importPoses(groundTruthPath_, _groundTruthFormat, poses, 0, &stamps))
-			{
-				UERROR("Cannot read ground truth file \"%s\".", groundTruthPath_.c_str());
-				success = false;
-			}
-			else if((_groundTruthFormat != 1 && _groundTruthFormat != 5 && _groundTruthFormat != 6 && _groundTruthFormat != 7) && poses.size() != this->imagesCount())
-			{
-				UERROR("The ground truth count is not the same as the images (%d vs %d)! Please remove "
-						"the ground truth file path if you don't want to use it (current file path=%s).",
-						(int)poses.size(), this->imagesCount(), groundTruthPath_.c_str());
-				success = false;
-			}
-			else if((_groundTruthFormat == 1 || _groundTruthFormat == 5 || _groundTruthFormat == 6 || _groundTruthFormat == 7) && stamps_.size() == 0)
-			{
-				UERROR("When using RGBD-SLAM, GPS, MALAGA and ST LUCIA formats for ground truth, images must have timestamps!");
-				success = false;
-			}
-			else if(_groundTruthFormat == 1 || _groundTruthFormat == 5 || _groundTruthFormat == 6 || _groundTruthFormat == 7)
-			{
-				UDEBUG("");
-				//Match ground truth values with images
-				groundTruth_.clear();
-				std::map<double, int> stampsToIds;
-				for(std::map<int, double>::iterator iter=stamps.begin(); iter!=stamps.end(); ++iter)
-				{
-					stampsToIds.insert(std::make_pair(iter->second, iter->first));
-				}
-				std::vector<double> values = uValues(stamps);
+			success = readPoses(odometry_, _stamps, _odometryPath, _odometryFormat);
+		}

-				int validPoses = 0;
-				for(std::list<double>::iterator ster=stamps_.begin(); ster!=stamps_.end(); ++ster)
-				{
-					Transform pose; // null transform
-					std::map<double, int>::iterator endIter = stampsToIds.lower_bound(*ster);
-					bool warned = false;
-					if(endIter != stampsToIds.end())
-					{
-						if(endIter->first == *ster)
-						{
-							pose = poses.at(endIter->second);
-						}
-						else if(endIter != stampsToIds.begin())
-						{
-							//interpolate
-							std::map<double, int>::iterator beginIter = endIter;
-							--beginIter;
-							double stampBeg = beginIter->first;
-							double stampEnd = endIter->first;
-							UASSERT(stampEnd > stampBeg && *ster>stampBeg && *ster < stampEnd);
-							if(stampEnd - stampBeg > 10.0)
-							{
-								warned = true;
-								UDEBUG("Cannot interpolate ground truth pose for stamp %f between %f and %f (>10 sec)", 
-									*ster,
-									stampBeg,
-									stampEnd);
-							}
-							else
-							{
-								float t = (*ster - stampBeg) / (stampEnd-stampBeg);
-								Transform & ta = poses.at(beginIter->second);
-								Transform & tb = poses.at(endIter->second);
-								if(!ta.isNull() && !tb.isNull())
-								{
-									++validPoses;
-									pose = ta.interpolate(t, tb);
-								}
-							}
-						}
-					}
-					if(pose.isNull() && !warned)
-					{
-						UDEBUG("Ground truth pose not found for stamp %f", *ster);
-					}
-					groundTruth_.push_back(pose);
-				}
-				if(validPoses != (int)stamps_.size())
-				{
-					UWARN("%d valid ground truth poses of %d stamps", validPoses, (int)stamps_.size());
-				}
-			}
-			else
-			{
-				UDEBUG("");
-				groundTruth_ = uValuesList(poses);
-				if(stamps_.size() == 0 && stamps.size() == poses.size())
-				{
-					stamps_ = uValuesList(stamps);
-				}
-				else if(_groundTruthFormat==8 && stamps_.size() == 0 && stamps.size()>0 && stamps.size() != poses.size())
-				{
-					UERROR("With Karlsruhe ground truth format, timestamps (%d) and poses (%d) should match!", (int)stamps.size(), (int)poses.size());
-				}
-			}
-			UASSERT_MSG(groundTruth_.size() == stamps_.size(), uFormat("%d vs %d", (int)groundTruth_.size(), (int)stamps_.size()).c_str());
+		if(success && _groundTruthPath.size())
+		{
+			success = readPoses(groundTruth_, _stamps, _groundTruthPath, _groundTruthFormat);
 		}
 	}

@@ -412,6 +324,110 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 	return success;
 }

+bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<double> & inOutStamps, const std::string & filePath, int format) const
+{
+	outputPoses.clear();
+	std::map<int, Transform> poses;
+	std::map<int, double> stamps;
+	if(!graph::importPoses(filePath, format, poses, 0, &stamps))
+	{
+		UERROR("Cannot read pose file \"%s\".", filePath.c_str());
+		return false;
+	}
+	else if((format != 1 && format != 5 && format != 6 && format != 7) && poses.size() != this->imagesCount())
+	{
+		UERROR("The pose count is not the same as the images (%d vs %d)! Please remove "
+				"the pose file path if you don't want to use it (current file path=%s).",
+				(int)poses.size(), this->imagesCount(), filePath.c_str());
+		return false;
+	}
+	else if((format == 1 || format == 5 || format == 6 || format == 7) && inOutStamps.size() == 0)
+	{
+		UERROR("When using RGBD-SLAM, GPS, MALAGA and ST LUCIA formats, images must have timestamps!");
+		return false;
+	}
+	else if(format == 1 || format == 5 || format == 6 || format == 7)
+	{
+		UDEBUG("");
+		//Match ground truth values with images
+		outputPoses.clear();
+		std::map<double, int> stampsToIds;
+		for(std::map<int, double>::iterator iter=stamps.begin(); iter!=stamps.end(); ++iter)
+		{
+			stampsToIds.insert(std::make_pair(iter->second, iter->first));
+		}
+		std::vector<double> values = uValues(stamps);
+
+		int validPoses = 0;
+		for(std::list<double>::iterator ster=inOutStamps.begin(); ster!=inOutStamps.end(); ++ster)
+		{
+			Transform pose; // null transform
+			std::map<double, int>::iterator endIter = stampsToIds.lower_bound(*ster);
+			bool warned = false;
+			if(endIter != stampsToIds.end())
+			{
+				if(endIter->first == *ster)
+				{
+					pose = poses.at(endIter->second);
+				}
+				else if(endIter != stampsToIds.begin())
+				{
+					//interpolate
+					std::map<double, int>::iterator beginIter = endIter;
+					--beginIter;
+					double stampBeg = beginIter->first;
+					double stampEnd = endIter->first;
+					UASSERT(stampEnd > stampBeg && *ster>stampBeg && *ster < stampEnd);
+					if(stampEnd - stampBeg > 10.0)
+					{
+						warned = true;
+						UDEBUG("Cannot interpolate pose for stamp %f between %f and %f (>10 sec)",
+							*ster,
+							stampBeg,
+							stampEnd);
+					}
+					else
+					{
+						float t = (*ster - stampBeg) / (stampEnd-stampBeg);
+						Transform & ta = poses.at(beginIter->second);
+						Transform & tb = poses.at(endIter->second);
+						if(!ta.isNull() && !tb.isNull())
+						{
+							++validPoses;
+							pose = ta.interpolate(t, tb);
+						}
+					}
+				}
+			}
+			if(pose.isNull() && !warned)
+			{
+				UDEBUG("Pose not found for stamp %f", *ster);
+			}
+			outputPoses.push_back(pose);
+		}
+		if(validPoses != (int)inOutStamps.size())
+		{
+			UWARN("%d valid poses of %d stamps", validPoses, (int)inOutStamps.size());
+		}
+	}
+	else
+	{
+		UDEBUG("");
+		outputPoses = uValuesList(poses);
+		if(inOutStamps.size() == 0 && stamps.size() == poses.size())
+		{
+			inOutStamps = uValuesList(stamps);
+		}
+		else if(format==8 && inOutStamps.size() == 0 && stamps.size()>0 && stamps.size() != poses.size())
+		{
+			UERROR("With Karlsruhe format, timestamps (%d) and poses (%d) should match!", (int)stamps.size(), (int)poses.size());
+			return false;
+		}
+	}
+	UASSERT_MSG(outputPoses.size() == inOutStamps.size(), uFormat("%d vs %d", (int)outputPoses.size(), (int)inOutStamps.size()).c_str());
+	return true;
+}
+
 bool CameraImages::isCalibrated() const
 {
 	return _model.isValidForProjection();
@@ -442,7 +458,7 @@ std::vector<std::string> CameraImages::filenames() const

 SensorData CameraImages::captureImage(CameraInfo * info)
 {
-	if(syncImageRateWithStamps_ && _captureDelay>0.0)
+	if(_syncImageRateWithStamps && _captureDelay>0.0)
 	{
 		int sleepTime = (1000*_captureDelay - 1000.0f*_captureTimer.getElapsedTime());
 		if(sleepTime > 2)
@@ -476,6 +492,7 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 	cv::Mat img;
 	cv::Mat scan;
 	double stamp = UTimer::now();
+	Transform odometryPose;
 	Transform groundTruthPose;
 	cv::Mat depthFromScan;
 	UDEBUG("");
@@ -541,13 +558,18 @@ SensorData CameraImages::captureImage(CameraInfo * info)
 			}
 		}

-		if(stamps_.size())
+		if(_stamps.size())
 		{
-			stamp = stamps_.front();
-			stamps_.pop_front();
-			if(stamps_.size())
+			stamp = _stamps.front();
+			_stamps.pop_front();
+			if(_stamps.size())
 			{
-				_captureDelay = stamps_.front() - stamp;
+				_captureDelay = _stamps.front() - stamp;
+			}
+			if(odometry_.size())
+			{
+				odometryPose = odometry_.front();
+				odometry_.pop_front();
 			}
 			if(groundTruth_.size())
 			{
@@ -688,6 +710,13 @@ SensorData CameraImages::captureImage(CameraInfo * info)

 	SensorData data(scan, LaserScanInfo(scan.empty()?0:_scanMaxPts, 0, _scanLocalTransform), _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, _model, this->getNextSeqID(), stamp);
 	data.setGroundTruth(groundTruthPose);
+
+	if(info && !odometryPose.isNull())
+	{
+		info->odomPose = odometryPose;
+		info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1); // Note that with TORO and g2o file formats, we could get the covariance
+	}
+
 	return data;
 }

--- a/corelib/src/CameraThread.cpp
+++ b/corelib/src/CameraThread.cpp
@@ -228,7 +228,15 @@ void CameraThread::mainLoop()
 					_stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()),
 					data.stereoCameraModel().left().fx(),
 					data.stereoCameraModel().baseline());
-			data.setCameraModel(data.stereoCameraModel().left());
+			// set Tx for stereo bundle adjustment (when used)
+			CameraModel model = CameraModel(
+					data.stereoCameraModel().left().fx(),
+					data.stereoCameraModel().left().fy(),
+					data.stereoCameraModel().left().cx(),
+					data.stereoCameraModel().left().cy(),
+					data.stereoCameraModel().localTransform(),
+					-data.stereoCameraModel().baseline()*data.stereoCameraModel().left().fx());
+			data.setCameraModel(model);
 			data.setDepthOrRightRaw(depth);
 			data.setStereoCameraModel(StereoCameraModel());
 			info.timeDisparity = timer.ticks();
--- a/corelib/src/DBDriverSqlite3.cpp
+++ b/corelib/src/DBDriverSqlite3.cpp
@@ -307,7 +307,6 @@ bool DBDriverSqlite3::getDatabaseVersionQuery(std::string & version) const
 	return false;
 }

-
 bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwritten)
 {
 	this->disconnectDatabaseQuery();
@@ -381,13 +380,15 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri
 	UASSERT(this->getDatabaseVersionQuery(_version)); // must be true!
 	UINFO("Database version = %s", _version.c_str());

-	if(uStrNumCmp(_version, RTABMAP_VERSION) > 0)
+	// From 0.11.13, compare only with minor version (patch will be used for non-database structural changes)
+	if((uStrNumCmp(_version, "0.11.12") <= 0 && uStrNumCmp(_version, RTABMAP_VERSION) > 0) ||
+	   (uStrNumCmp(_version, "0.11.12") > 0 && uStrNumCmp(RTABMAP_VERSION, "0.11.12") > 0 && uStrNumCmp(_version, uFormat("%d.%d.99", RTABMAP_VERSION_MAJOR, RTABMAP_VERSION_MINOR)) > 0))
 	{
-		UERROR("Opened database version (%s) is more recent than rtabmap "
-			   "installed version (%s). Please update rtabmap to new version!",
-			   _version.c_str(), RTABMAP_VERSION);
-		this->disconnectDatabaseQuery(false);
-		return false;
+			UERROR("Opened database version (%s) is more recent than rtabmap "
+				   "installed version (%s). Please update rtabmap to new version!",
+				   _version.c_str(), RTABMAP_VERSION);
+			this->disconnectDatabaseQuery(false);
+			return false;
 	}

 	//Set database optimizations
--- a/corelib/src/Features2d.cpp
+++ b/corelib/src/Features2d.cpp
@@ -209,8 +209,15 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKey
 }

 void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints)
+{
+	std::vector<cv::Point3f> keypoints3D;
+	limitKeypoints(keypoints, keypoints3D, descriptors, maxKeypoints);
+}
+
+void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints)
 {
 	UASSERT_MSG((int)keypoints.size() == descriptors.rows || descriptors.rows == 0, uFormat("keypoints=%d descriptors=%d", (int)keypoints.size(), descriptors.rows).c_str());
+	UASSERT_MSG((int)keypoints.size() == keypoints3D.size() || keypoints3D.size() == 0, uFormat("keypoints=%d keypoints3D=%d", (int)keypoints.size(), (int)keypoints3D.size()).c_str());
 	if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
 	{
 		UTimer timer;
@@ -229,6 +236,7 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat &
 		int removed = (int)hessianMap.size()-maxKeypoints;
 		std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
 		std::vector<cv::KeyPoint> kptsTmp(maxKeypoints);
+		std::vector<cv::Point3f> kpts3DTmp(maxKeypoints);
 		cv::Mat descriptorsTmp;
 		if(descriptors.rows)
 		{
@@ -237,6 +245,10 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat &
 		for(unsigned int k=0; k < kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
 		{
 			kptsTmp[k] = keypoints[iter->second];
+			if(keypoints3D.size())
+			{
+				kpts3DTmp[k] = keypoints3D[iter->second];
+			}
 			if(descriptors.rows)
 			{
 				if(descriptors.type() == CV_32FC1)
@@ -252,6 +264,7 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat &
 		ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, (int)kptsTmp.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
 		ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
 		keypoints = kptsTmp;
+		keypoints3D = kpts3DTmp;
 		if(descriptors.rows)
 		{
 			descriptors = descriptorsTmp;
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -2187,11 +2187,11 @@ Transform Memory::computeTransform(
 			tmpFrom.setWords(std::multimap<int, cv::KeyPoint>());
 			tmpFrom.setWords3(std::multimap<int, cv::Point3f>());
 			tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
-			tmpFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
+			tmpFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
 			tmpTo.setWords(std::multimap<int, cv::KeyPoint>());
 			tmpTo.setWords3(std::multimap<int, cv::Point3f>());
 			tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
-			tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
+			tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
 		}
 		else if(useKnownCorrespondencesIfPossible)
 		{
@@ -3317,14 +3317,19 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	{
 		UINFO("Use odometry features");
 		keypoints = data.keypoints();
+		keypoints3D = data.keypoints3D();
 		descriptors = data.descriptors().clone();

 		UASSERT(descriptors.empty() || descriptors.rows == (int)keypoints.size());
+		UASSERT(keypoints3D.empty() || keypoints3D.size() == keypoints.size());

 		if((int)keypoints.size() > _feature2D->getMaxFeatures())
 		{
-			_feature2D->limitKeypoints(keypoints, descriptors, _feature2D->getMaxFeatures());
+			_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, _feature2D->getMaxFeatures());
 		}
+		t = timer.ticks();
+		if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
+		UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), t);

 		if(descriptors.empty())
 		{
@@ -3339,13 +3344,14 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 			}

 			descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
-			t = timer.ticks();
-			if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
-			UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);
 		}
+		t = timer.ticks();
+		if(stats) stats->addStatistic(Statistics::kTimingMemDescriptors_extraction(), t*1000.0f);
+		UDEBUG("time descriptors (%d) = %fs", descriptors.rows, t);

-		if((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) ||
-		   (!data.rightRaw().empty() && data.stereoCameraModel().isValidForProjection()))
+		if(keypoints3D.empty() &&
+			((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) ||
+		   (!data.rightRaw().empty() && data.stereoCameraModel().isValidForProjection())))
 		{
 			keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
 			if(_feature2D->getMinDepth() > 0.0f || _feature2D->getMaxDepth() > 0.0f)
@@ -3376,10 +3382,10 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 				keypoints3D = validKeypoints3D;
 				descriptors = validDescriptors.rowRange(0, oi).clone();
 			}
-			t = timer.ticks();
-			if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
-			UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);
 		}
+		t = timer.ticks();
+		if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_3D(), t*1000.0f);
+		UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D.size(), t);

 		UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
 		if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
@@ -3500,7 +3506,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	StereoCameraModel stereoCameraModel = data.stereoCameraModel();

 	// apply decimation?
-	if(_imagePostDecimation > 1)
+	if(_imagePostDecimation > 1 && !isIntermediateNode)
 	{
 		image = util2d::decimate(image, _imagePostDecimation);
 		depthOrRightImage = util2d::decimate(depthOrRightImage, _imagePostDecimation);
@@ -3521,7 +3527,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	// downsampling the laser scan?
 	cv::Mat laserScan = data.laserScanRaw();
 	int maxLaserScanMaxPts = data.laserScanInfo().maxPoints();
-	if(!laserScan.empty() && _laserScanDownsampleStepSize > 1)
+	if(!laserScan.empty() && _laserScanDownsampleStepSize > 1 && !isIntermediateNode)
 	{
 		laserScan = util3d::downsample(laserScan, _laserScanDownsampleStepSize);
 		maxLaserScanMaxPts /= _laserScanDownsampleStepSize;
@@ -3530,7 +3536,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 		if(stats) stats->addStatistic(Statistics::kTimingMemScan_downsampling(), t*1000.0f);
 		UDEBUG("time downsampling scan = %fs", t);
 	}
-	if(!laserScan.empty() && _laserScanNormalK > 0 && laserScan.channels() == 3)
+	if(!laserScan.empty() && _laserScanNormalK > 0 && laserScan.channels() == 3 && !isIntermediateNode)
 	{
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(laserScan);
 		float x,y,z;
@@ -3543,7 +3549,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	}

 	Signature * s;
-	if(this->isBinDataKept())
+	if(this->isBinDataKept() && !isIntermediateNode)
 	{
 		UDEBUG("Bin data kept: rgb=%d, depth=%d, scan=%d, userData=%d",
 				image.empty()?0:1,
@@ -3564,10 +3570,22 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 		rtabmap::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
 		rtabmap::CompressionThread ctLaserScan(laserScan);
 		rtabmap::CompressionThread ctUserData(data.userDataRaw());
-		ctImage.start();
-		ctDepth.start();
-		ctLaserScan.start();
-		ctUserData.start();
+		if(!image.empty())
+		{
+			ctImage.start();
+		}
+		if(!depthOrRightImage.empty())
+		{
+			ctDepth.start();
+		}
+		if(!laserScan.empty())
+		{
+			ctLaserScan.start();
+		}
+		if(!data.userDataRaw().empty())
+		{
+			ctUserData.start();
+		}
 		ctImage.join();
 		ctDepth.join();
 		ctLaserScan.join();
@@ -3602,11 +3620,21 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	}
 	else
 	{
+		UDEBUG("Bin data kept: scan=%d, userData=%d",
+						laserScan.empty()?0:1,
+						data.userDataRaw().empty()?0:1);
+
 		// just compress user data and laser scan (scans can be used for local scan matching)
 		rtabmap::CompressionThread ctUserData(data.userDataRaw());
 		rtabmap::CompressionThread ctLaserScan(laserScan);
-		ctUserData.start();
-		ctLaserScan.start();
+		if(!data.userDataRaw().empty() && !isIntermediateNode)
+		{
+			ctUserData.start();
+		}
+		if(!laserScan.empty() && !isIntermediateNode)
+		{
+			ctLaserScan.start();
+		}
 		ctUserData.join();
 		ctLaserScan.join();

@@ -3662,7 +3690,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
 	cv::Mat ground, obstacles;
 	float cellSize = 0.0f;
 	cv::Point3f viewPoint(0,0,0);
-	if(_createOccupancyGrid && !data.depthOrRightRaw().empty())
+	if(_createOccupancyGrid && !data.depthOrRightRaw().empty() && !isIntermediateNode)
 	{
 		_occupancy->createLocalMap(*s, ground, obstacles, viewPoint);
 		cellSize = _occupancy->getCellSize();
@@ -3817,19 +3845,12 @@ void Memory::enableWordsRef(const std::list<int> & signatureIds)
 		{
 			const VisualWord * wordFirst = _vwd->getWord(keys.front()); //get descriptor size
 			UASSERT(wordFirst!=0);
-			//Descriptors used for Memory::computeTransform()
-			cv::Mat descriptors(keys.size(), wordFirst->getDescriptor().cols, wordFirst->getDescriptor().type());
+
 			// Add all references
 			for(unsigned int i=0; i<keys.size(); ++i)
 			{
 				_vwd->addWordRef(keys.at(i), (*j)->id());
-				const VisualWord * word = _vwd->getWord(keys.at(i));
-				UASSERT(word != 0);
-
-				word->getDescriptor().copyTo(descriptors.row(i));
-
 			}
-			(*j)->sensorData().setFeatures(std::vector<cv::KeyPoint>(), descriptors);
 			(*j)->setEnabled(true);
 		}
 	}
--- a/corelib/src/Odometry.cpp
+++ b/corelib/src/Odometry.cpp
@@ -332,7 +332,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			kpts[i].size *= _imageDecimation;
 			kpts[i].octave += log2value;
 		}
-		data.setFeatures(kpts, decimatedData.descriptors());
+		data.setFeatures(kpts, decimatedData.keypoints3D(), decimatedData.descriptors());

 		if(info)
 		{
@@ -496,8 +496,14 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			}
 		}

+		if(data.stamp() == 0)
+		{
+			UWARN("Null stamp detected");
+		}
+
 		previousStamp_ = data.stamp();
 		previousVelocityTransform_.setNull();
+
 		if(dt)
 		{
 			previousVelocityTransform_ = Transform(vx, vy, vz, vroll, vpitch, vyaw);
@@ -509,6 +515,9 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 			info->distanceTravelled = distanceTravelled_;
 		}

+		info->varianceLin *= t.getNorm();
+		info->varianceAng *= t.getAngle();
+
 		return _pose *= t; // update
 	}
 	else if(_resetCurrentCount > 0)
--- a/corelib/src/OdometryF2F.cpp
+++ b/corelib/src/OdometryF2F.cpp
@@ -39,12 +39,15 @@ namespace rtabmap {
 OdometryF2F::OdometryF2F(const ParametersMap & parameters) :
 	Odometry(parameters),
 	keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
+	visKeyFrameThr_(Parameters::defaultOdomVisKeyFrameThr()),
 	scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr())
 {
 	registrationPipeline_ = Registration::create(parameters);
 	Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
+	Parameters::parse(parameters, Parameters::kOdomVisKeyFrameThr(), visKeyFrameThr_);
 	Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
 	UASSERT(keyFrameThr_>=0.0f && keyFrameThr_<=1.0f);
+	UASSERT(visKeyFrameThr_>=0);
 	UASSERT(scanKeyFrameThr_>=0.0f && scanKeyFrameThr_<=1.0f);
 }

@@ -100,6 +103,30 @@ Transform OdometryF2F::computeTransform(
 				!guess.isNull()?motionSinceLastKeyFrame*guess:!registrationPipeline_->isImageRequired()&&this->getPose().isIdentity()?Transform::getIdentity():Transform(),
 				&regInfo);

+		if(output.isNull() && !guess.isNull() && registrationPipeline_->isImageRequired())
+		{
+			tmpRefFrame = refFrame_;
+			// reset matches, but keep already extracted features in newFrame.sensorData()
+			newFrame.setWords(std::multimap<int, cv::KeyPoint>());
+			newFrame.setWords3(std::multimap<int, cv::Point3f>());
+			newFrame.setWordsDescriptors(std::multimap<int, cv::Mat>());
+			UWARN("Failed to find a transformation with the provided guess (%s), trying again without a guess.", guess.prettyPrint().c_str());
+			output = registrationPipeline_->computeTransformationMod(
+				tmpRefFrame,
+				newFrame,
+				Transform(), // null guess
+				&regInfo);
+
+			if(output.isNull())
+			{
+				UWARN("Trial with no guess still fail.");
+			}
+			else
+			{
+				UWARN("Trial with no guess succeeded.");
+			}
+		}
+
 		if(info && this->isInfoDataFilled())
 		{
 			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
@@ -141,7 +168,8 @@ Transform OdometryF2F::computeTransform(
 		//return Identity
 		output = Transform::getIdentity();
 		// a very high variance tells that the new pose is not linked with the previous one
-		regInfo.variance = 9999;
+		regInfo.varianceLin = 9999;
+		regInfo.varianceAng = 9999;
 	}

 	if(!output.isNull())
@@ -149,8 +177,12 @@ Transform OdometryF2F::computeTransform(
 		output = motionSinceLastKeyFrame.inverse() * output;

 		// new key-frame?
-		if( (registrationPipeline_->isImageRequired() && (keyFrameThr_ == 0 || float(regInfo.inliers) <= keyFrameThr_*float(refFrame_.sensorData().keypoints().size()))) ||
-			(registrationPipeline_->isScanRequired() && (scanKeyFrameThr_ == 0 || regInfo.icpInliersRatio <= scanKeyFrameThr_)))
+		if( (registrationPipeline_->isImageRequired() &&
+				(keyFrameThr_ == 0.0f ||
+				 visKeyFrameThr_ == 0 ||
+				 float(regInfo.inliers) <= keyFrameThr_*float(refFrame_.sensorData().keypoints().size()) ||
+				 regInfo.inliers <= visKeyFrameThr_)) ||
+			(registrationPipeline_->isScanRequired() && (scanKeyFrameThr_ == 0.0f || regInfo.icpInliersRatio <= scanKeyFrameThr_)))
 		{
 			UDEBUG("Update key frame");
 			int features = newFrame.getWordsDescriptors().size();
@@ -208,12 +240,13 @@ Transform OdometryF2F::computeTransform(
 		UWARN("Registration failed: \"%s\"", regInfo.rejectedMsg.c_str());
 	}

-	data.setFeatures(newFrame.sensorData().keypoints(), newFrame.sensorData().descriptors());
+	data.setFeatures(newFrame.sensorData().keypoints(), newFrame.sensorData().keypoints3D(), newFrame.sensorData().descriptors());

 	if(info)
 	{
 		info->type = 1;
-		info->variance = regInfo.variance;
+		info->varianceLin = regInfo.varianceLin;
+		info->varianceAng = regInfo.varianceAng;
 		info->inliers = regInfo.inliers;
 		info->icpInliersRatio = regInfo.icpInliersRatio;
 		info->matches = regInfo.matches;
--- a/corelib/src/OdometryF2M.cpp
+++ b/corelib/src/OdometryF2M.cpp
@@ -38,6 +38,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Optimizer.h"
 #include "rtabmap/core/VWDictionary.h"
 #include "rtabmap/core/util3d.h"
+#include "rtabmap/core/Graph.h"
+#include "rtflann/flann.hpp"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UMath.h"
@@ -58,30 +60,52 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	Odometry(parameters),
 	maximumMapSize_(Parameters::defaultOdomF2MMaxSize()),
 	keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
+	visKeyFrameThr_(Parameters::defaultOdomVisKeyFrameThr()),
 	maxNewFeatures_(Parameters::defaultOdomF2MMaxNewFeatures()),
 	scanKeyFrameThr_(Parameters::defaultOdomScanKeyFrameThr()),
 	scanMaximumMapSize_(Parameters::defaultOdomF2MScanMaxSize()),
 	scanSubtractRadius_(Parameters::defaultOdomF2MScanSubtractRadius()),
 	bundleAdjustment_(Parameters::defaultOdomF2MBundleAdjustment()),
-	bundleAdjustmentMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()),
-	regPipeline_(Registration::create(parameters)),
+	bundleMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()),
 	map_(new Signature(-1)),
-	lastFrame_(new Signature(1))
+	lastFrame_(new Signature(1)),
+	sba_(0)
 {
 	UDEBUG("");
 	Parameters::parse(parameters, Parameters::kOdomF2MMaxSize(), maximumMapSize_);
 	Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
+	Parameters::parse(parameters, Parameters::kOdomVisKeyFrameThr(), visKeyFrameThr_);
 	Parameters::parse(parameters, Parameters::kOdomF2MMaxNewFeatures(), maxNewFeatures_);
 	Parameters::parse(parameters, Parameters::kOdomScanKeyFrameThr(), scanKeyFrameThr_);
 	Parameters::parse(parameters, Parameters::kOdomF2MScanMaxSize(), scanMaximumMapSize_);
 	Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractRadius(), scanSubtractRadius_);
 	Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustment(), bundleAdjustment_);
-	Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxFrames(), bundleAdjustmentMaxFrames_);
-	bundleParameters_ = parameters;
+	Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxFrames(), bundleMaxFrames_);
+	UASSERT(bundleMaxFrames_ >= 0);
+	ParametersMap bundleParameters = parameters;
+	if(bundleAdjustment_ > 0)
+	{
+		if((bundleAdjustment_==1 && Optimizer::isAvailable(Optimizer::kTypeG2O)) ||
+		   (bundleAdjustment_==2 && Optimizer::isAvailable(Optimizer::kTypeCVSBA)))
+		{
+			// disable bundle in RegistrationVis as we do it already here
+			uInsert(bundleParameters, ParametersPair(Parameters::kVisBundleAdjustment(), "0"));
+			sba_ = Optimizer::create(bundleAdjustment_==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, bundleParameters);
+		}
+		else
+		{
+			UWARN("Selected bundle adjustment approach (\"%s\"=\"%d\") is not available, "
+					"local bundle adjustment is then disabled.", Parameters::kOdomF2MBundleAdjustment().c_str(), bundleAdjustment_);
+			bundleAdjustment_ = 0;
+		}
+	}
 	UASSERT(maximumMapSize_ >= 0);
 	UASSERT(keyFrameThr_ >= 0.0f && keyFrameThr_<=1.0f);
+	UASSERT(visKeyFrameThr_>=0);
 	UASSERT(scanKeyFrameThr_ >= 0.0f && scanKeyFrameThr_<=1.0f);
 	UASSERT(maxNewFeatures_ >= 0);
+
+	regPipeline_ = Registration::create(bundleParameters);
 }

 OdometryF2M::~OdometryF2M()
@@ -94,6 +118,11 @@ OdometryF2M::~OdometryF2M()
 	bundleLinks_.clear();
 	bundleModels_.clear();
 	bundlePoseReferences_.clear();
+	if(sba_)
+	{
+		delete sba_;
+	}
+	delete regPipeline_;
 	UDEBUG("");
 }

@@ -103,6 +132,12 @@ void OdometryF2M::reset(const Transform & initialPose)
 	Odometry::reset(initialPose);
 	*lastFrame_ = Signature(1);
 	*map_ = Signature(-1);
+	scansBuffer_.clear();
+	bundleWordReferences_.clear();
+	bundlePoses_.clear();
+	bundleLinks_.clear();
+	bundleModels_.clear();
+	bundlePoseReferences_.clear();
 }

 // return not null transform if odometry is correctly computed
@@ -131,6 +166,10 @@ Transform OdometryF2M::computeTransform(
 		UERROR("Odometry bundle adjustment doesn't work with multi-cameras. It is disabled.");
 		bundleAdjustment_ = 0;
 	}
+	bool addKeyFrame = false;
+	int totalBundleWordReferencesUsed = 0;
+	int totalBundleOutliers = 0;
+	float bundleTime = 0.0f;

 	// Generate keypoints from the new data
 	if(lastFrame_->sensorData().isValid())
@@ -145,20 +184,41 @@ Transform OdometryF2M::computeTransform(
 					// special case for ICP-only odom, set guess to identity if we just started
 					!guess.isNull()?this->getPose()*guess:!regPipeline_->isImageRequired()&&this->getPose().isIdentity()?Transform::getIdentity():Transform(),
 					&regInfo);
-
-			data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().descriptors());
+			if(transform.isNull() && !guess.isNull() && regPipeline_->isImageRequired())
+			{
+				tmpMap = *map_;
+				// reset matches, but keep already extracted features in lastFrame_->sensorData()
+				lastFrame_->setWords(std::multimap<int, cv::KeyPoint>());
+				lastFrame_->setWords3(std::multimap<int, cv::Point3f>());
+				lastFrame_->setWordsDescriptors(std::multimap<int, cv::Mat>());
+				UWARN("Failed to find a transformation with the provided guess (%s), trying again without a guess.", guess.prettyPrint().c_str());
+				transform = regPipeline_->computeTransformationMod(
+						tmpMap,
+						*lastFrame_,
+						Transform(), // null guess
+						&regInfo);
+				if(transform.isNull())
+				{
+					UWARN("Trial with no guess still fail.");
+				}
+				else
+				{
+					UWARN("Trial with no guess succeeded.");
+				}
+			}
+			data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().keypoints3D(), lastFrame_->sensorData().descriptors());

 			std::map<int, cv::Point3f> points3DMap;
 			std::map<int, Transform> bundlePoses;
 			std::multimap<int, Link> bundleLinks;
 			std::map<int, CameraModel> bundleModels;
+			std::map<int, StereoCameraModel> bundleStereoModels;
 			if(!transform.isNull())
 			{
 				// local bundle adjustment
-				if(bundleAdjustment_>0 &&
+				if(bundleAdjustment_>0 && sba_ &&
 				   regPipeline_->isImageRequired() &&
-				   ((bundleAdjustment_==1 && Optimizer::isAvailable(Optimizer::kTypeG2O)) ||
-			        (bundleAdjustment_==2 && Optimizer::isAvailable(Optimizer::kTypeCVSBA))) &&
+				   lastFrame_->sensorData().cameraModels().size() <= 1 && // multi-cameras not supported
 				   regInfo.inliersIDs.size())
 				{
 					UDEBUG("Local Bundle Adjustment");
@@ -174,51 +234,67 @@ Transform OdometryF2M::computeTransform(
 					}
 					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)));
+						UASSERT_MSG(bundlePoses_.size()-1 == bundleLinks_.size(), uFormat("poses=%d links=%d", (int)bundlePoses_.size(), (int)bundleLinks_.size()).c_str());
+						UASSERT(bundlePoses_.size() == bundleModels_.size());
+
+						bundlePoses = bundlePoses_;
+						bundleLinks = bundleLinks_;
+						bundleModels = bundleModels_;
+
+						bundleLinks.insert(std::make_pair(bundlePoses_.rbegin()->first, Link(bundlePoses_.rbegin()->first, lastFrame_->id(), Link::kNeighbor, bundlePoses_.rbegin()->second.inverse()*transform, regInfo.varianceAng, regInfo.varianceLin)));
 						bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));
+
+						UDEBUG("Fill matches (%d)", (int)regInfo.inliersIDs.size());
+						std::map<int, std::map<int, cv::Point3f> > wordReferences;
 						for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
 						{
-							std::multimap<int, cv::Point3f>::const_iterator iter3D = tmpMap.getWords3().find(regInfo.inliersIDs[i]);
+							int wordId =regInfo.inliersIDs[i];
+
+							// 3D point
+							std::multimap<int, cv::Point3f>::const_iterator iter3D = tmpMap.getWords3().find(wordId);
 							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());
+							std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().find(wordId);

-							if(wordReferences.find(iter2D->first) == wordReferences.end())
+							// all other references
+							std::map<int, std::map<int, cv::Point3f> >::iterator refIter = bundleWordReferences_.find(wordId);
+							UASSERT_MSG(refIter != bundleWordReferences_.end(), uFormat("wordId=%d", wordId).c_str());
+
+							std::map<int, cv::Point3f> references;
+							int step = bundleMaxFrames_>0?(refIter->second.size() / bundleMaxFrames_):1;
+							if(step == 0)
 							{
-								UASSERT(bundleWordReferences_.find(iter2D->first) != bundleWordReferences_.end());
-								wordReferences.insert(*bundleWordReferences_.find(iter2D->first));
+								step = 1;
+							}
+							int oi=0;
+							for(std::map<int, cv::Point3f>::iterator jter=refIter->second.begin(); jter!=refIter->second.end(); ++jter)
+							{
+								if(oi++ % step == 0 && bundlePoses.find(jter->first)!=bundlePoses.end())
+								{
+									references.insert(*jter);
+									++totalBundleWordReferencesUsed;
+								}
+							}
+							//make sure the last reference is here
+							if(refIter->second.size() > 1)
+							{
+								references.insert(*refIter->second.rbegin());
 							}

-							wordReferences.find(iter2D->first)->second.insert(std::make_pair(lastFrame_->id(), iter2D->second.pt));
+							if(iter2D!=lastFrame_->getWords().end())
+							{
+								UASSERT(lastFrame_->getWords3().find(wordId) != lastFrame_->getWords3().end());
+								references.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, lastFrame_->getWords3().find(wordId)->second.x)));
+							}
+							wordReferences.insert(std::make_pair(wordId, references));
+
+							//UDEBUG("%d (%f,%f,%f)", iter3D->first, iter3D->second.x, iter3D->second.y, iter3D->second.z);
+							//for(std::map<int, cv::Point2f>::iterator iter=inserted.first->second.begin(); iter!=inserted.first->second.end(); ++iter)
+							//{
+							//	UDEBUG("%d (%f,%f)", iter->first, iter->second.x, iter->second.y);
+							//}
 						}

 						CameraModel model;
@@ -229,21 +305,61 @@ Transform OdometryF2M::computeTransform(
 						else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
 						{
 							model = lastFrame_->sensorData().stereoCameraModel().left();
+							// Set Tx for stereo BA
+							model = CameraModel(model.fx(),
+									model.fy(),
+									model.cx(),
+									model.cy(),
+									model.localTransform(),
+									-lastFrame_->sensorData().stereoCameraModel().baseline()*model.fx());
+						}
+						else
+						{
+							UFATAL("no valid camera model!");
 						}
-						UASSERT(model.isValidForProjection());
 						bundleModels.insert(std::make_pair(lastFrame_->id(), model));

-						bundlePoses = sba->optimizeBA(0, bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences);
-						delete sba;
+						UDEBUG("sba...start");
+						// set root negative to fix all other poses
+						std::set<int> sbaOutliers;
+						UTimer bundleTimer;
+						bundlePoses = sba_->optimizeBA(-lastFrame_->id(), bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences, &sbaOutliers);
+						bundleTime = bundleTimer.ticks();
+						UDEBUG("sba...end");
+						totalBundleOutliers = (int)sbaOutliers.size();

-						UDEBUG("bundleTime=%fs (poses=%d wordRef=%d)", bundleTime.ticks(), (int)bundlePoses.size(), (int)bundleWordReferences_.size());
+						UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime, (int)bundlePoses.size(), (int)bundleWordReferences_.size(), (int)sbaOutliers.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())
+						if(bundlePoses.size() == bundlePoses_.size()+1)
 						{
-							transform = bundlePoses.rbegin()->second;
+							if(!bundlePoses.rbegin()->second.isNull())
+							{
+								transform = bundlePoses.rbegin()->second;
+								bundleLinks.find(bundlePoses_.rbegin()->first)->second.setTransform(bundlePoses_.rbegin()->second.inverse()*transform);
+
+								if(sbaOutliers.size())
+								{
+									std::vector<int> newInliers(regInfo.inliersIDs.size());
+									int oi=0;
+									for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
+									{
+										if(sbaOutliers.find(regInfo.inliersIDs[i]) == sbaOutliers.end())
+										{
+											newInliers[oi++] = regInfo.inliersIDs[i];
+										}
+									}
+									newInliers.resize(oi);
+									UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(regInfo.inliersIDs.size()));
+									regInfo.inliers = (int)newInliers.size();
+									regInfo.inliersIDs = newInliers;
+								}
+							}
+							UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
+						}
+						else
+						{
+							UWARN("Local bundle adjustment failed! transform is not refined.");
 						}
 					}
 				}
@@ -273,14 +389,25 @@ Transform OdometryF2M::computeTransform(
 				std::multimap<int, cv::Point3f> mapPoints = tmpMap.getWords3();
 				std::multimap<int, cv::Mat> mapDescriptors = tmpMap.getWordsDescriptors();

-				//Visual
-				int added = 0;
-				int removed = 0;
-				UDEBUG("keyframeThr=%f matches=%d inliers=%d features=%d mp=%d", keyFrameThr_, regInfo.matches, regInfo.inliers, (int)lastFrame_->sensorData().keypoints().size(), (int)mapPoints.size());
-				if(regPipeline_->isImageRequired() &&
-					(keyFrameThr_==0 || float(regInfo.inliers) <= keyFrameThr_*float(lastFrame_->sensorData().keypoints().size())))
+				bool addVisualKeyFrame = regPipeline_->isImageRequired() &&
+						 (keyFrameThr_ == 0.0f ||
+						  visKeyFrameThr_ == 0 ||
+						  float(regInfo.inliers) <= (keyFrameThr_*float(lastFrame_->sensorData().keypoints().size())) ||
+						  regInfo.inliers <= visKeyFrameThr_);
+				bool addGeometricKeyFrame = regPipeline_->isScanRequired() && (scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_);
+
+				addKeyFrame = false;//bundleLinks.rbegin()->second.transform().getNorm() > 5.0f*0.075f;
+				addKeyFrame = addKeyFrame || addVisualKeyFrame || addGeometricKeyFrame;
+
+				UDEBUG("keyframeThr=%f visKeyFrameThr_=%d matches=%d inliers=%d features=%d mp=%d", keyFrameThr_, visKeyFrameThr_, regInfo.matches, regInfo.inliers, (int)lastFrame_->sensorData().keypoints().size(), (int)mapPoints.size());
+				if(addKeyFrame)
 				{
-					UDEBUG("Update local map (ratio=%f < %f)", float(regInfo.inliers)/float(lastFrame_->sensorData().keypoints().size()), keyFrameThr_);
+					//Visual
+					int added = 0;
+					int removed = 0;
+					UTimer tmpTimer;
+
+					UDEBUG("Update local map");

 					// update local map
 					UASSERT(mapWords.size() == mapPoints.size());
@@ -290,19 +417,21 @@ Transform OdometryF2M::computeTransform(
 					std::map<int, int>::iterator iterBundlePosesRef = bundlePoseReferences_.end();
 					if(bundleAdjustment_>0)
 					{
+						bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), 0));
+						UASSERT(graph::findLink(bundleLinks, bundlePoses_.rbegin()->first, lastFrame_->id(), false) != bundleLinks.end());
+						bundleLinks_.insert(*bundleLinks.find(bundlePoses_.rbegin()->first));
+						uInsert(bundlePoses_, bundlePoses);
+						UASSERT(bundleModels.find(lastFrame_->id()) != bundleModels.end());
+						bundleModels_.insert(*bundleModels.find(lastFrame_->id()));
+						iterBundlePosesRef = bundlePoseReferences_.find(lastFrame_->id());
+
 						// 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);
+							//UDEBUG("Updated %d (%f,%f,%f) -> (%f,%f,%f)", iter->first, mapPoints.find(origin)->second.x, mapPoints.find(origin)->second.y, mapPoints.find(origin)->second.z, iter->second.x, iter->second.y, iter->second.z);
 							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
@@ -310,6 +439,8 @@ Transform OdometryF2M::computeTransform(
 					UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
 					std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().begin();
 					std::multimap<int, cv::Mat>::const_iterator iterDesc = lastFrame_->getWordsDescriptors().begin();
+					UDEBUG("new frame words3=%d", (int)lastFrame_->getWords3().size());
+					std::set<int> seenStatusUpdated;
 					for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin(); iter!=lastFrame_->getWords3().end(); ++iter, ++iter2D, ++iterDesc)
 					{
 						if(util3d::isFinite(iter->second))
@@ -326,26 +457,34 @@ Transform OdometryF2M::computeTransform(
 							{
 								if(lastFrame_->getWords().count(iter->first) == 1)
 								{
+									std::multimap<int, cv::KeyPoint>::iterator iterKpts = mapWords.find(iter->first);
+									if(iterKpts!=mapWords.end())
+									{
+										iterKpts->second.octave = iter2D->second.octave;
+									}
+
 									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));
+										std::map<int, cv::Point3f> framePt;
+
+										framePt.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, iter->second.x)));
 										bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
 									}
 									else
 									{
-										bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), iter2D->second.pt));
+										bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter2D->second.pt.x, iter2D->second.pt.y, iter->second.x)));
 									}
 								}
 							}
 						}
 					}
+					UDEBUG("newIds=%d", (int)newIds.size());

-					for(std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > >::iterator iter=newIds.begin();
-						iter!=newIds.end();
+					for(std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > >::reverse_iterator iter=newIds.rbegin();
+						iter!=newIds.rend();
 						++iter)
 					{
 						if(maxNewFeatures_ == 0  || added < maxNewFeatures_)
@@ -359,13 +498,13 @@ Transform OdometryF2M::computeTransform(

 									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));
+										std::map<int, cv::Point3f> framePt;
+										framePt.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter->second.second.first.pt.x, iter->second.second.first.pt.y, iter->second.second.second.first.x)));
 										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));
+										bundleWordReferences_.find(iter->second.first)->second.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter->second.second.first.pt.x, iter->second.second.first.pt.y, iter->second.second.second.first.x)));
 									}
 								}
 							}
@@ -380,7 +519,7 @@ Transform OdometryF2M::computeTransform(
 					// remove words in map if max size is reached
 					if((int)mapPoints.size() > maximumMapSize_)
 					{
-						// remove oldest first, keep matched features
+						// remove oldest first, keep matched features with their aliases
 						std::set<int> matches(regInfo.matchesIDs.begin(), regInfo.matchesIDs.end());
 						std::multimap<int, cv::Mat>::iterator iterMapDescriptors = mapDescriptors.begin();
 						std::multimap<int, cv::KeyPoint>::iterator iterMapWords = mapWords.begin();
@@ -389,22 +528,14 @@ Transform OdometryF2M::computeTransform(
 						{
 							if(matches.find(iter->first) == matches.end())
 							{
-								std::map<int, std::map<int, cv::Point2f> >::iterator iterRef = bundleWordReferences_.find(iter->first);
+								std::map<int, std::map<int, cv::Point3f> >::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)
+									for(std::map<int, cv::Point3f>::iterator iterFrame = iterRef->second.begin(); iterFrame != iterRef->second.end(); ++iterFrame)
 									{
-										if(bundlePoseReferences_.find(iterKp->first) != bundlePoseReferences_.end())
+										if(bundlePoseReferences_.find(iterFrame->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);
-											}
+											bundlePoseReferences_.at(iterFrame->first) -= 1;
 										}
 									}
 									bundleWordReferences_.erase(iterRef);
@@ -422,18 +553,34 @@ Transform OdometryF2M::computeTransform(
 								++iterMapWords;
 							}
 						}
-					}
-					modified = true;
-				}

-				// Geometric
-				UDEBUG("scankeyframeThr=%f icpInliersRatio=%f", scanKeyFrameThr_, regInfo.icpInliersRatio);
-				if(regPipeline_->isScanRequired() &&
-					(scanKeyFrameThr_==0 || regInfo.icpInliersRatio <= scanKeyFrameThr_))
-				{
+						for(std::map<int, int>::iterator iter=bundlePoseReferences_.begin(); iter!=bundlePoseReferences_.end();)
+						{
+							if((iter->second <= 0 && // <= regPipeline_->getMinVisualCorrespondences() &&
+								bundlePoses_.begin()->first == iter->first)) // remove oldest pose first
+							{
+								UASSERT(bundlePoses_.erase(iter->first) == 1);
+								bundleLinks_.erase(iter->first);
+								bundleModels_.erase(iter->first);
+								bundlePoseReferences_.erase(iter++);
+							}
+							else
+							{
+								++iter;
+							}
+						}
+					}
+
+					if(added || removed)
+					{
+						modified = true;
+					}
+					UDEBUG("Update local features map = %fs", tmpTimer.ticks());
+
+					// Geometric
+					UDEBUG("scankeyframeThr=%f icpInliersRatio=%f", scanKeyFrameThr_, regInfo.icpInliersRatio);
 					UINFO("Update local scan map %d (ratio=%f < %f)", lastFrame_->id(), regInfo.icpInliersRatio, scanKeyFrameThr_);

-					UTimer tmpTimer;
 					if(lastFrame_->sensorData().laserScanRaw().cols)
 					{
 						pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(mapScan);
@@ -540,7 +687,7 @@ Transform OdometryF2M::computeTransform(
 							modified=true;
 						}
 					}
-					UDEBUG("Update local map = %fs", tmpTimer.ticks());
+					UDEBUG("Update local scan map = %fs", tmpTimer.ticks());
 				}

 				if(modified)
@@ -550,7 +697,7 @@ Transform OdometryF2M::computeTransform(
 					map_->sensorData().setLaserScanRaw(mapScan, LaserScanInfo(0, 0));
 					map_->setWords(mapWords);
 					map_->setWords3(mapPoints);
-					map_->setWordsDescriptors(mapDescriptors);
+				 	map_->setWordsDescriptors(mapDescriptors);
 				}
 			}

@@ -577,10 +724,11 @@ Transform OdometryF2M::computeTransform(
 						dummy);
 			}

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

 			// a very high variance tells that the new pose is not linked with the previous one
-			regInfo.variance = 9999;
+			regInfo.varianceLin = 9999;
+			regInfo.varianceAng = 9999;

 			bool frameValid = false;
 			Transform newFramePose = this->getPose(); // initial pose may be not identity...
@@ -619,11 +767,20 @@ Transform OdometryF2M::computeTransform(
 							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));
+								std::map<int, cv::Point3f> framePt;
+
+								//get depth
+								float d = 0.0f;
+								if(lastFrame_->getWords3().count(iter->first) == 1)
+								{
+									d = lastFrame_->getWords3().find(iter->first)->second.x;
+								}
+
+								framePt.insert(std::make_pair(lastFrame_->id(), cv::Point3f(iter->second.pt.x, iter->second.pt.y, d)));
 								bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
 							}
 						}
+
 						bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), (int)bundleWordReferences_.size()));

 						CameraModel model;
@@ -634,24 +791,28 @@ Transform OdometryF2M::computeTransform(
 						else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
 						{
 							model = lastFrame_->sensorData().stereoCameraModel().left();
+							// Set Tx for stereo BA
+							model = CameraModel(model.fx(),
+									model.fy(),
+									model.cx(),
+									model.cy(),
+									model.localTransform(),
+									-lastFrame_->sensorData().stereoCameraModel().baseline()*model.fx());
+						}
+						else
+						{
+							UFATAL("invalid camera model!");
 						}
-						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));
+						UASSERT_MSG(lastFrame_->id() > 0, uFormat("Input data should have ID greater than 0 when odometry bundle adjustment is enabled!").c_str());
+						bundlePoses_.insert(std::make_pair(lastFrame_->id(), newFramePose));
 					}

 					map_->setWords(words);
 					map_->setWords3(transformedPoints);
 					map_->setWordsDescriptors(descriptors);
-
-					map_->sensorData().setCameraModels(lastFrame_->sensorData().cameraModels());
-					map_->sensorData().setStereoCameraModel(lastFrame_->sensorData().stereoCameraModel());
+					addKeyFrame = true;
 				}
 				else
 				{
@@ -666,6 +827,7 @@ Transform OdometryF2M::computeTransform(
 					pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose * lastFrame_->sensorData().laserScanInfo().localTransform());
 					scansBuffer_.push_back(std::make_pair(mapCloudNormals, pcl::IndicesPtr(new std::vector<int>)));
 					map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), LaserScanInfo(0,0));
+					addKeyFrame = true;
 				}
 				else
 				{
@@ -692,7 +854,7 @@ Transform OdometryF2M::computeTransform(
 			}
 		}

-		map_->sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat()); // clear sensorData features
+		map_->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat()); // clear sensorData features

 		nFeatures = lastFrame_->getWords().size();
 		if(this->isInfoDataFilled() && info)
@@ -706,11 +868,17 @@ Transform OdometryF2M::computeTransform(

 	if(info)
 	{
-		info->variance = regInfo.variance;
+		info->varianceLin = regInfo.varianceLin;
+		info->varianceAng = regInfo.varianceAng;
 		info->inliers = regInfo.inliers;
 		info->matches = regInfo.matches;
 		info->icpInliersRatio = regInfo.icpInliersRatio;
 		info->features = nFeatures;
+		info->localKeyFrames = (int)bundlePoses_.size();
+		info->keyFrameAdded = addKeyFrame;
+		info->localBundleOutliers = totalBundleOutliers;
+		info->localBundleConstraints = totalBundleWordReferencesUsed;
+		info->localBundleTime = bundleTime;

 		if(this->isInfoDataFilled())
 		{
@@ -719,15 +887,17 @@ Transform OdometryF2M::computeTransform(
 		}
 	}

-	UINFO("Odom update time = %fs lost=%s features=%d inliers=%d/%d variance=%f local_map=%d local_scan_map=%d",
+	UINFO("Odom update time = %fs lost=%s features=%d inliers=%d/%d variance:lin=%f, ang=%f local_map=%d local_scan_map=%d",
 			timer.elapsed(),
 			output.isNull()?"true":"false",
 			nFeatures,
 			regInfo.inliers,
 			regInfo.matches,
-			regInfo.variance,
+			regInfo.varianceLin,
+			regInfo.varianceAng,
 			regPipeline_->isImageRequired()?(int)map_->getWords3().size():0,
 			regPipeline_->isScanRequired()?(int)map_->sensorData().laserScanRaw().cols:0);
+
 	return output;
 }

--- a/corelib/src/OdometryMono.cpp
+++ b/corelib/src/OdometryMono.cpp
@@ -976,7 +976,8 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 		if(info)
 		{
 			// a very high variance tells that the new pose is not linked with the previous one
-			info->variance = 9999;
+			info->varianceLin = 9999;
+			info->varianceAng = 9999;
 		}

 			// generate kpts
--- a/corelib/src/OdometryThread.cpp
+++ b/corelib/src/OdometryThread.cpp
@@ -101,9 +101,10 @@ void OdometryThread::mainLoop()
 		OdometryInfo info;
 		Transform pose = _odometry->process(data, &info);
 		// a null pose notify that odometry could not be computed
-		double variance = info.variance>0?info.variance:1;
+		double varianceLin = info.varianceLin>0?info.varianceLin:1;
+		double varianceAng = info.varianceAng>0?info.varianceAng:1;
 		UDEBUG("Odom pose = %s", pose.prettyPrint().c_str());
-		this->post(new OdometryEvent(data, pose, variance, variance, info));
+		this->post(new OdometryEvent(data, pose, varianceAng, varianceLin, info));
 	}
 }

--- a/corelib/src/Optimizer.cpp
+++ b/corelib/src/Optimizer.cpp
@@ -267,7 +267,8 @@ std::map<int, Transform> Optimizer::optimizeBA(
 		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)
+		const std::map<int, std::map<int, cv::Point3f> > & wordReferences,
+		std::set<int> * outliers)
 {
 	UERROR("Optimizer %d doesn't implement optimizeBA() method.", (int)this->type());
 	return std::map<int, Transform>();
@@ -294,6 +295,15 @@ std::map<int, Transform> Optimizer::optimizeBA(
 			else if(signatures.at(iter->first).sensorData().stereoCameraModel().isValidForProjection())
 			{
 				model = signatures.at(iter->first).sensorData().stereoCameraModel().left();
+
+				// Set Tx = -baseline*fx for stereo BA
+				model = CameraModel(
+						model.fx(),
+						model.fy(),
+						model.cx(),
+						model.cy(),
+						model.localTransform(),
+						-signatures.at(iter->first).sensorData().stereoCameraModel().baseline()*model.fx());
 			}
 			else
 			{
@@ -314,7 +324,7 @@ std::map<int, Transform> Optimizer::optimizeBA(

 	// compute correspondences
 	std::map<int, cv::Point3f> points3DMap;
-	std::map<int, std::map<int, cv::Point2f> > wordReferences;
+	std::map<int, std::map<int, cv::Point3f> > wordReferences;
 	this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences);

 	return optimizeBA(rootId, poses, links, models, points3DMap, wordReferences);
@@ -324,7 +334,8 @@ 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)
+		const std::map<int, std::map<int, cv::Point3f> > & wordReferences,
+		std::set<int> * outliers)
 {
 	std::map<int, Transform> poses;
 	poses.insert(std::make_pair(link.from(), Transform::getIdentity()));
@@ -334,7 +345,7 @@ Transform Optimizer::optimizeBA(
 	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);
+	poses = optimizeBA(link.from(), poses, links, models, points3DMap, wordReferences, outliers);
 	if(poses.size() == 2)
 	{
 		return poses.rbegin()->second;
@@ -350,7 +361,7 @@ void Optimizer::computeBACorrespondences(
 		const std::multimap<int, Link> & links,
 		const std::map<int, Signature> & signatures,
 		std::map<int, cv::Point3f> & points3DMap,
-		std::map<int, std::map<int, cv::Point2f> > & wordReferences) // <ID words, IDs frames + keypoint>
+		std::map<int, std::map<int, cv::Point3f> > & wordReferences) // <ID words, IDs frames + keypoint/depth>
 {
 	UDEBUG("");
 	int wordCount = 0;
@@ -367,49 +378,75 @@ void Optimizer::computeBACorrespondences(
 		   uContains(poses, link.from()))
 		{
 			Signature sFrom = signatures.at(link.from());
-			Signature sTo = signatures.at(link.to());
-
-			if(sFrom.getWords().size() &&
-				sTo.getWords().size() &&
-				sFrom.getWords3().size())
+			if(sFrom.getWeight() >= 0) // ignore intermediate links
 			{
-				ParametersMap regParam;
-				regParam.insert(ParametersPair(Parameters::kVisEstimationType(), "1"));
-				regParam.insert(ParametersPair(Parameters::kVisPnPReprojError(), "5"));
-				regParam.insert(ParametersPair(Parameters::kVisMinInliers(), "5"));
-				regParam.insert(ParametersPair(Parameters::kVisCorNNDR(), "0.6"));
-				RegistrationVis reg(regParam);
-
-				//sFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
-				//sTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
-
-				RegistrationInfo info;
-				Transform t = reg.computeTransformationMod(sFrom, sTo, Transform(), &info);
-				//Transform t = reg.computeTransformationMod(sFrom, sTo, iter->second.transform(), &info);
-				UDEBUG("%d->%d, inliers=%d",sFrom.id(), sTo.id(), (int)info.inliersIDs.size());
-
-				if(!t.isNull())
+				Signature sTo = signatures.at(link.to());
+				if(sTo.getWeight() < 0)
 				{
-					Transform pose = poses.at(sFrom.id());
-					for(unsigned int i=0; i<info.inliersIDs.size(); ++i)
+					for(std::multimap<int, Link>::const_iterator jter=links.find(sTo.id());
+						sTo.getWeight() < 0 && jter!=links.end() &&  uContains(signatures, jter->second.to());
+						++jter)
 					{
-						cv::Point3f p = sFrom.getWords3().lower_bound(info.inliersIDs[i])->second;
-						if(p.x > 0.0f) // make sure the point is valid
-						{
-							int wordId = ++wordCount;
-
-							p = util3d::transformPoint(p, pose);
-							points3DMap.insert(std::make_pair(wordId, p));
-							wordReferences.insert(std::make_pair(wordId, std::map<int, cv::Point2f>()));
-							wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), sFrom.getWords().lower_bound(info.inliersIDs[i])->second.pt));
-							wordReferences.at(wordId).insert(std::make_pair(sTo.id(), sTo.getWords().lower_bound(info.inliersIDs[i])->second.pt));
-						}
+						sTo = signatures.at(jter->second.to());
 					}
-					++edgeWithWordsAdded;
 				}
-				else
+
+				if(sFrom.getWords().size() &&
+					sTo.getWords().size() &&
+					sFrom.getWords3().size())
 				{
-					UWARN("Not enough inliers (%d) between %d and %d", info.inliersIDs.size(), sFrom.id(), sTo.id());
+					ParametersMap regParam;
+					regParam.insert(ParametersPair(Parameters::kVisEstimationType(), "1"));
+					regParam.insert(ParametersPair(Parameters::kVisPnPReprojError(), "5"));
+					regParam.insert(ParametersPair(Parameters::kVisMinInliers(), "5"));
+					regParam.insert(ParametersPair(Parameters::kVisCorNNDR(), "0.6"));
+					RegistrationVis reg(regParam);
+
+					//sFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
+					//sTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
+
+					RegistrationInfo info;
+					Transform t = reg.computeTransformationMod(sFrom, sTo, Transform(), &info);
+					//Transform t = reg.computeTransformationMod(sFrom, sTo, iter->second.transform(), &info);
+					UDEBUG("%d->%d, inliers=%d",sFrom.id(), sTo.id(), (int)info.inliersIDs.size());
+
+					if(!t.isNull())
+					{
+						Transform pose = poses.at(sFrom.id());
+						UASSERT(!pose.isNull());
+						for(unsigned int i=0; i<info.inliersIDs.size(); ++i)
+						{
+							cv::Point3f p = sFrom.getWords3().lower_bound(info.inliersIDs[i])->second;
+							if(p.x > 0.0f) // make sure the point is valid
+							{
+								int wordId = ++wordCount;
+
+								wordReferences.insert(std::make_pair(wordId, std::map<int, cv::Point3f>()));
+
+								cv::Point2f pt = sFrom.getWords().lower_bound(info.inliersIDs[i])->second.pt;
+								wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), cv::Point3f(pt.x, pt.y, p.x)));
+
+
+								pt = sTo.getWords().lower_bound(info.inliersIDs[i])->second.pt;
+								float depth = 0.0f;
+								std::multimap<int, cv::Point3f>::const_iterator iterTo = sTo.getWords3().lower_bound(info.inliersIDs[i]);
+								if( iterTo!=sTo.getWords3().end() &&
+									iterTo->second.x > 0)
+								{
+									depth = iterTo->second.x;
+								}
+								wordReferences.at(wordId).insert(std::make_pair(sTo.id(), cv::Point3f(pt.x, pt.y, depth)));
+
+								p = util3d::transformPoint(p, pose);
+								points3DMap.insert(std::make_pair(wordId, p));
+							}
+						}
+						++edgeWithWordsAdded;
+					}
+					else
+					{
+						UWARN("Not enough inliers (%d) between %d and %d", info.inliersIDs.size(), sFrom.id(), sTo.id());
+					}
 				}
 			}
 		}
--- a/corelib/src/OptimizerCVSBA.cpp
+++ b/corelib/src/OptimizerCVSBA.cpp
@@ -59,7 +59,8 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
 		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>)
+		const std::map<int, std::map<int, cv::Point3f> > & wordReferences, // <ID words, IDs frames + keypoint/Disparity>)
+		std::set<int> * outliers)
 {
 #ifdef RTABMAP_CVSBA
 	// run sba optimization
@@ -130,14 +131,14 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
 	{
 		points[i] = kter->second;

-		std::map<int, std::map<int, cv::Point2f> >::const_iterator iter = wordReferences.find(kter->first);
+		std::map<int, std::map<int, cv::Point3f> >::const_iterator iter = wordReferences.find(kter->first);
 		if(iter != wordReferences.end())
 		{
-			for(std::map<int, cv::Point2f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
+			for(std::map<int, cv::Point3f>::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;
+					imagePoints[frameIdToIndex.at(jter->first)][i] = cv::Point2f(jter->second.x, jter->second.y);
 					visibility[frameIdToIndex.at(jter->first)][i] = 1;
 				}
 			}
--- a/corelib/src/OptimizerG2O.cpp
+++ b/corelib/src/OptimizerG2O.cpp
@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/OptimizerG2O.h>
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/util3d_motion_estimation.h>
+#include <rtabmap/core/util3d.h>

 #ifdef RTABMAP_G2O
 #include "g2o/config.h"
@@ -54,12 +55,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifdef G2O_HAVE_CHOLMOD
 #include "g2o/solvers/cholmod/linear_solver_cholmod.h"
 #endif
+#include "g2o/solvers/eigen/linear_solver_eigen.h"
 #include "g2o/types/slam3d/vertex_se3.h"
 #include "g2o/types/slam3d/edge_se3.h"
 #include "g2o/types/slam2d/vertex_se2.h"
 #include "g2o/types/slam2d/edge_se2.h"

 typedef g2o::BlockSolver< g2o::BlockSolverTraits<-1, -1> > SlamBlockSolver;
+typedef g2o::LinearSolverEigen<SlamBlockSolver::PoseMatrixType> SlamLinearEigenSolver;
 typedef g2o::LinearSolverPCG<SlamBlockSolver::PoseMatrixType> SlamLinearPCGSolver;
 #ifdef G2O_HAVE_CSPARSE
 typedef g2o::LinearSolverCSparse<SlamBlockSolver::PoseMatrixType> SlamLinearCSparseSolver;
@@ -113,7 +116,10 @@ void OptimizerG2O::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kg2oSolver(), solver_);
 	Parameters::parse(parameters, Parameters::kg2oOptimizer(), optimizer_);
 	Parameters::parse(parameters, Parameters::kg2oPixelVariance(), pixelVariance_);
+	Parameters::parse(parameters, Parameters::kg2oRobustKernelDelta(), robustKernelDelta_);
+	Parameters::parse(parameters, Parameters::kg2oBaseline(), baseline_);
 	UASSERT(pixelVariance_ > 0.0);
+	UASSERT(baseline_ >= 0.0);

 #ifndef G2O_HAVE_CHOLMOD
 	if(solver_ == 2)
@@ -162,7 +168,14 @@ std::map<int, Transform> OptimizerG2O::optimize(

 		SlamBlockSolver * blockSolver = 0;

-		if(solver_ == 2)
+		if(solver_ == 3)
+		{
+			//eigen
+			SlamLinearEigenSolver * linearSolver = new SlamLinearEigenSolver();
+			linearSolver->setBlockOrdering(false);
+			blockSolver = new SlamBlockSolver(linearSolver);
+		}
+		else if(solver_ == 2)
 		{
 #ifdef G2O_HAVE_CHOLMOD
 			//chmold
@@ -545,21 +558,26 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 		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>
+		const std::map<int, std::map<int, cv::Point3f> > & wordReferences,
+		std::set<int> * outliers)
 {
 	std::map<int, Transform> optimizedPoses;
 #ifdef RTABMAP_G2O
 	UDEBUG("Optimizing graph...");

 	optimizedPoses.clear();
-	if(links.size()>=1 && poses.size()>=2 && iterations() > 0 && models.size() == poses.size())
+	if(poses.size()>=2 && iterations() > 0 && models.size() == poses.size())
 	{
 		g2o::SparseOptimizer optimizer;
 		optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
 		g2o::BlockSolver_6_3::LinearSolverType * linearSolver = 0;
-		bool robustKernel = true;

-		if(solver_ == 2)
+		if(solver_ == 3)
+		{
+			//eigen
+			linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
+		}
+		else if(solver_ == 2)
 		{
 #ifdef G2O_HAVE_CHOLMOD
 			//chmold
@@ -599,8 +617,6 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 			UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());

 			Transform camPose = iter->second * iterModel->second.localTransform();
-			//iter->second = (iter->second * model.localTransform()).inverse();
-			UDEBUG("%d t=%s", iter->first, camPose.prettyPrint().c_str());

 			// Add node's pose
 			UASSERT(!camPose.isNull());
@@ -608,20 +624,35 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(

 			Eigen::Affine3d a = camPose.toEigen3d();
 			g2o::SBACam cam(Eigen::Quaterniond(a.rotation()), a.translation());
-			cam.setKcam(iterModel->second.fx(), iterModel->second.fy(), iterModel->second.cx(), iterModel->second.cy(), 0);
+			cam.setKcam(
+					iterModel->second.fx(),
+					iterModel->second.fy(),
+					iterModel->second.cx(),
+					iterModel->second.cy(),
+					iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_); // baseline in meters
 			vCam->setEstimate(cam);
-			if(iter->first == rootId)
-			{
-				vCam->setFixed(true);
-			}
 			vCam->setId(iter->first);
-			//std::cout << cam << std::endl;
+
+			// negative root means that all other poses should be fixed instead of the root
+			vCam->setFixed((rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId));
+
+			UDEBUG("cam %d (fixed=%d) fx=%f fy=%f cx=%f cy=%f Tx=%f baseline=%f t=%s",
+					iter->first,
+					vCam->fixed()?1:0,
+					iterModel->second.fx(),
+					iterModel->second.fy(),
+					iterModel->second.cx(),
+					iterModel->second.cy(),
+					iterModel->second.Tx(),
+					iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_,
+					camPose.prettyPrint().c_str());
+
 			UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert vertex %d!?", iter->first).c_str());

 			++iter;
 		}

-		UDEBUG("fill edges to g2o and associate each 3D point to all frames observing it...");
+		UDEBUG("fill edges to g2o...");
 		for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 		{
 			if(uContains(poses, iter->second.from()) &&
@@ -641,17 +672,15 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(

 				// between cameras, not base_link
 				Transform camLink = models.at(id1).localTransform().inverse()*iter->second.transform()*models.at(id2).localTransform();
-				//Transform t = iter->second.transform();
-				UDEBUG("added edge %d=%s -> %d=%s",
+				UDEBUG("added edge %d->%d (in cam frame=%s)",
 						id1,
-						iter->second.transform().prettyPrint().c_str(),
 						id2,
 						camLink.prettyPrint().c_str());
 				Eigen::Affine3d a = camLink.toEigen3d();

 				g2o::EdgeSBACam * e = new g2o::EdgeSBACam();
-				g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1);
-				g2o::VertexSE3* v2 = (g2o::VertexSE3*)optimizer.vertex(id2);
+				g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1);
+				g2o::VertexCam* v2 = (g2o::VertexCam*)optimizer.vertex(id2);
 				UASSERT(v1 != 0);
 				UASSERT(v2 != 0);
 				e->setVertex(0, v1);
@@ -669,8 +698,9 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 		}

 		UDEBUG("fill 3D points to g2o...");
-		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 int stepVertexId = poses.rbegin()->first+1;
+		std::list<g2o::OptimizableGraph::Edge*> edges;
+		for(std::map<int, std::map<int, cv::Point3f> >::const_iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
 		{
 			if(points3DMap.find(iter->first) != points3DMap.end())
 			{
@@ -682,32 +712,61 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 				vpt3d->setMarginalized(true);
 				optimizer.addVertex(vpt3d);

+				//UDEBUG("Added 3D point %d (%f,%f,%f)", vpt3d->id()-stepVertexId, pt3d.x, pt3d.y, pt3d.z);
+
 				// set observations
-				for(std::map<int, cv::Point2f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
+				for(std::map<int, cv::Point3f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
 				{
 					int camId = jter->first;
-					if(poses.find(camId) != poses.end())
+					if(poses.find(camId) != poses.end() && optimizer.vertex(camId) != 0)
 					{
-						const cv::Point2f & pt = jter->second;
+						const cv::Point3f & pt = jter->second;
+						double depth = pt.z;

-						Eigen::Matrix<double,2,1> obs;
-						obs << pt.x, pt.y;
+						//UDEBUG("Added observation pt=%d to cam=%d (%f,%f) d=%f", vpt3d->id()-stepVertexId, camId, pt.x, pt.y, depth);

-						//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)
+						g2o::OptimizableGraph::Edge * e;
+						g2o::VertexCam* vcam = dynamic_cast<g2o::VertexCam*>(optimizer.vertex(camId));
+						double variance = pixelVariance_;
+						if(uIsFinite(depth) && depth > 0.0 && vcam->estimate().baseline > 0.0)
 						{
-							e->setRobustKernel(new g2o::RobustKernelHuber);
+							// stereo edge
+							g2o::EdgeProjectP2SC* es = new g2o::EdgeProjectP2SC();
+							float disparity = vcam->estimate().baseline * vcam->estimate().Kcam(0,0) / depth;
+							Eigen::Vector3d obs( pt.x, pt.y, pt.x-disparity);
+							es->setMeasurement(obs);
+							//variance *= log(exp(1)+disparity);
+							es->setInformation(Eigen::Matrix3d::Identity() / variance);
+							e = es;
+						}
+						else
+						{
+							if(vcam->estimate().baseline > 0.0)
+							{
+								UWARN("Stereo camera model detected but current "
+										"observation (pt=%d to cam=%d) has null depth (%f m), adding "
+										"mono observation instead.",
+										vpt3d->id()-stepVertexId, camId, depth);
+							}
+							// mono edge
+							g2o::EdgeProjectP2MC* em = new g2o::EdgeProjectP2MC();
+							Eigen::Vector2d obs( pt.x, pt.y);
+							em->setMeasurement(obs);
+							em->setInformation(Eigen::Matrix2d::Identity() / variance);
+							e = em;
+						}
+						e->setVertex(0, vpt3d);
+						e->setVertex(1, vcam);
+
+						if(robustKernelDelta_ > 0.0)
+						{
+							g2o::RobustKernelHuber* kernel = new g2o::RobustKernelHuber;
+							kernel->setDelta(robustKernelDelta_);
+							e->setRobustKernel(kernel);
 						}

 						optimizer.addEdge(e);
+						edges.push_back(e);
 					}
 				}
 			}
@@ -718,56 +777,65 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(

 		UASSERT(optimizer.verifyInformationMatrices());

-		UINFO("g2o optimizing begin (max iterations=%d, epsilon=%f robustKernel=%d)", iterations(), this->epsilon(), robustKernel?1:0);
+		UINFO("g2o optimizing begin (max iterations=%d, epsilon=%f robustKernel=%f)", iterations(), this->epsilon(), robustKernelDelta_);

 		int it = 0;
 		UTimer timer;
-		double lastError = 0.0;
-		if(this->epsilon() > 0.0)
+		int outliersCount = 0;
+		int outliersCountFar = 0;
+
+		for(int i=0; i<(robustKernelDelta_>0.0?2:1); ++i)
 		{
-			for(int i=0; i<iterations(); ++i)
+			it += optimizer.optimize(i==0&&robustKernelDelta_>0.0?3:iterations());
+
+			// early stop condition
+			optimizer.computeActiveErrors();
+			double chi2 = optimizer.activeRobustChi2();
+			if(uIsNan(chi2))
 			{
-				it += optimizer.optimize(1);
+				UERROR("Optimization generated NANs, aborting optimization! Try another g2o's optimizer (current=%d).", optimizer_);
+				return optimizedPoses;
+			}
+			UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);

-				// early stop condition
-				optimizer.computeActiveErrors();
-				double chi2 = optimizer.activeRobustChi2();
-				UDEBUG("iteration %d: %d nodes, %d edges, chi2: %f", i, (int)optimizer.vertices().size(), (int)optimizer.edges().size(), chi2);
+			if(i>0 && (optimizer.activeRobustChi2() > 1000000000000.0 || !uIsFinite(optimizer.activeRobustChi2())))
+			{
+				UWARN("g2o: Large optimization error detected (%f), aborting optimization!");
+				return optimizedPoses;
+			}

-				if(i>0 && (optimizer.activeRobustChi2() > 1000000000000.0 || !uIsFinite(optimizer.activeRobustChi2())))
+			if(robustKernelDelta_>0.0)
+			{
+				for(std::list<g2o::OptimizableGraph::Edge*>::iterator iter=edges.begin(); iter!=edges.end();++iter)
 				{
-					UWARN("g2o: Large optimization error detected (%f), aborting optimization!");
-					return optimizedPoses;
-				}
-
-				double errorDelta = lastError - chi2;
-				if(i>0 && errorDelta < this->epsilon())
-				{
-					if(errorDelta < 0)
+					if((*iter)->level() == 0 && (*iter)->chi2() > (*iter)->robustKernel()->delta())
 					{
-						UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
-					}
-					else
-					{
-						UINFO("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
-						break;
+						(*iter)->setLevel(1);
+						++outliersCount;
+						double d = ((g2o::EdgeProjectP2SC*)(*iter))->measurement()[0]-((g2o::EdgeProjectP2SC*)(*iter))->measurement()[2];
+						UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeProjectP2SC*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
+
+						const cv::Point3f & pt3d = points3DMap.at((*iter)->vertex(0)->id()-stepVertexId);
+						((g2o::VertexSBAPointXYZ*)(*iter)->vertex(0))->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
+
+						if(outliers)
+						{
+							outliers->insert((*iter)->vertex(0)->id()-stepVertexId);
+						}
+						if(d < 5.0)
+						{
+							outliersCountFar++;
+						}
 					}
+					//(*iter)->setRobustKernel(0);
 				}
-				else if(i==0 && chi2 < this->epsilon())
-				{
-					UINFO("Stop optimizing, error is already under epsilon (%f < %f)", chi2, this->epsilon());
-					break;
-				}
-				lastError = chi2;
+				if(i==0)
+					optimizer.initializeOptimization(0);
+				UDEBUG("outliers=%d outliersCountFar=%d", outliersCount, outliersCountFar);
 			}
 		}
-		else
-		{
-			it = optimizer.optimize(iterations());
-			optimizer.computeActiveErrors();
-			UDEBUG("%d nodes, %d edges, chi2: %f", (int)optimizer.vertices().size(), (int)optimizer.edges().size(), optimizer.activeRobustChi2());
-		}
-		UINFO("g2o optimizing end (%d iterations done, error=%f, time = %f s)", it, optimizer.activeRobustChi2(), timer.ticks());
+
+		UINFO("g2o optimizing end (%d iterations done, error=%f, outliers=%d/%d (delta=%f) time = %f s)", it, optimizer.activeRobustChi2(), outliersCount, (int)edges.size(), robustKernelDelta_, timer.ticks());

 		if(optimizer.activeRobustChi2() > 1000000000000.0)
 		{
@@ -782,10 +850,16 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 			if(v)
 			{
 				Transform t = Transform::fromEigen3d(v->estimate());
+
 				// remove model local transform
 				t *= models.at(iter->first).localTransform().inverse();
-				//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());
+				UDEBUG("%d from=%s to=%s", iter->first, iter->second.prettyPrint().c_str(), t.prettyPrint().c_str());
+				if(t.isNull())
+				{
+					UERROR("Optimized pose %d is null!?!?", iter->first);
+					optimizedPoses.clear();
+					return optimizedPoses;
+				}

 				// FIXME: is there a way that we can add the 2D constraint directly in SBA?
 				if(this->isSlam2d())
@@ -806,6 +880,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 		}

 		//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);
@@ -817,7 +892,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 			}
 			else
 			{
-				UERROR("Vertex (point3D) %d not found!?", iter->first);
+				iter->second.x = iter->second.y = iter->second.z = std::numeric_limits<float>::quiet_NaN();
 			}
 		}
 	}
--- a/corelib/src/Registration.cpp
+++ b/corelib/src/Registration.cpp
@@ -197,13 +197,16 @@ Transform Registration::computeTransformationMod(
 	{
 		if(info.icpInliersRatio)
 		{
-			info.variance = info.icpInliersRatio > 0?1.0/double(info.icpInliersRatio):1.0;
+			info.varianceLin = info.icpInliersRatio > 0?1.0/double(info.icpInliersRatio):1.0;
+			info.varianceAng = info.icpInliersRatio > 0?1.0/double(info.icpInliersRatio):1.0;
 		}
 		else
 		{
-			info.variance = info.inliers > 0?1.0f/float(info.inliers):1.0f;
+			info.varianceLin = info.inliers > 0?1.0f/float(info.inliers):1.0f;
+			info.varianceAng = info.inliers > 0?1.0f/float(info.inliers):1.0f;
 		}
-		info.variance = info.variance>0.0f?info.variance:0.0001f; // epsilon if exact transform
+		info.varianceLin = info.varianceLin>0.0f?info.varianceLin:0.0001f; // epsilon if exact transform
+		info.varianceAng = info.varianceAng>0.0f?info.varianceAng:0.0001f; // epsilon if exact transform
 	}

 	if(child_)
--- a/corelib/src/RegistrationIcp.cpp
+++ b/corelib/src/RegistrationIcp.cpp
@@ -360,7 +360,8 @@ Transform RegistrationIcp::computeTransformationImpl(
 							info.icpTranslation,
 							info.icpRotation);

-					info.variance = variance>0.0f?variance:0.0001; // epsilon if exact transform
+					info.varianceLin = variance>0.0f?variance:0.0001; // epsilon if exact transform
+					info.varianceAng = variance>0.0f?variance:0.0001; // epsilon if exact transform
 					info.icpInliersRatio = correspondencesRatio;

 					if(correspondencesRatio < _correspondenceRatio)
--- a/corelib/src/RegistrationVis.cpp
+++ b/corelib/src/RegistrationVis.cpp
@@ -93,6 +93,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kVisIterations(), _iterations);
 	Parameters::parse(parameters, Parameters::kVisRefineIterations(), _refineIterations);
 	Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType);
+	Parameters::parse(parameters, Parameters::kVisForwardEstOnly(), _forwardEstimateOnly);
 	Parameters::parse(parameters, Parameters::kVisEpipolarGeometryVar(), _epipolarGeometryVar);
 	Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError);
 	Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags);
@@ -193,20 +194,22 @@ Transform RegistrationVis::computeTransformationImpl(
 	UDEBUG("%s=%d", Parameters::kVisCorFlowMaxLevel().c_str(), _flowMaxLevel);
 	UDEBUG("guess=%s", guess.prettyPrint().c_str());

-	UDEBUG("Input(%d): from=%d words, %d 3D words, %d words descriptors,  %d kpts, %d descriptors",
+	UDEBUG("Input(%d): from=%d words, %d 3D words, %d words descriptors,  %d kpts, %d kpts3D, %d descriptors",
 			fromSignature.id(),
 			(int)fromSignature.getWords().size(),
 			(int)fromSignature.getWords3().size(),
 			(int)fromSignature.getWordsDescriptors().size(),
 			(int)fromSignature.sensorData().keypoints().size(),
+			(int)fromSignature.sensorData().keypoints3D().size(),
 			fromSignature.sensorData().descriptors().rows);

-	UDEBUG("Input(%d): to=%d words, %d 3D words, %d words descriptors, %d kpts, %d descriptors",
+	UDEBUG("Input(%d): to=%d words, %d 3D words, %d words descriptors, %d kpts, %d kpts3D, %d descriptors",
 			toSignature.id(),
 			(int)toSignature.getWords().size(),
 			(int)toSignature.getWords3().size(),
 			(int)toSignature.getWordsDescriptors().size(),
 			(int)toSignature.sensorData().keypoints().size(),
+			(int)toSignature.sensorData().keypoints3D().size(),
 			toSignature.sensorData().descriptors().rows);

 	std::string msg;
@@ -327,13 +330,17 @@ Transform RegistrationVis::computeTransformationImpl(
 			}

 			std::vector<cv::Point3f> kptsFrom3D;
-			if(fromSignature.getWords3().empty())
+			if(kptsFrom.size() == fromSignature.getWords3().size())
 			{
-				kptsFrom3D = detector->generateKeypoints3D(fromSignature.sensorData(), kptsFrom);
+				kptsFrom3D = uValues(fromSignature.getWords3());
+			}
+			else if(kptsFrom.size() == fromSignature.sensorData().keypoints3D().size())
+			{
+				kptsFrom3D = fromSignature.sensorData().keypoints3D();
 			}
 			else
 			{
-				kptsFrom3D = uValues(fromSignature.getWords3());
+				kptsFrom3D = detector->generateKeypoints3D(fromSignature.sensorData(), kptsFrom);
 			}

 			if(!imageTo.empty())
@@ -402,6 +409,7 @@ Transform RegistrationVis::computeTransformationImpl(
 				kptsFrom.resize(ki);
 				kptsTo.resize(ki);
 				kptsFrom3DKept.resize(ki);
+				kptsFrom3D = kptsFrom3DKept;

 				std::vector<cv::Point3f> kptsTo3D;
 				if(_estimationType == 0 || (_estimationType == 1 && !varianceFromInliersCount()) || !_forwardEstimateOnly)
@@ -423,7 +431,7 @@ Transform RegistrationVis::computeTransformationImpl(
 						words3To.insert(std::make_pair(id, kptsTo3D[i]));
 					}
 				}
-				toSignature.sensorData().setFeatures(kptsTo, cv::Mat());
+				toSignature.sensorData().setFeatures(kptsTo, kptsTo3D, cv::Mat());
 			}
 			else
 			{
@@ -437,10 +445,10 @@ Transform RegistrationVis::computeTransformationImpl(
 						words3From.insert(std::make_pair(id, kptsFrom3D[i]));
 					}
 				}
-				toSignature.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
+				toSignature.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
 			}

-			fromSignature.sensorData().setFeatures(kptsFrom, cv::Mat());
+			fromSignature.sensorData().setFeatures(kptsFrom, kptsFrom3D, cv::Mat());
 		}
 		else // Features Matching
 		{
@@ -514,6 +522,7 @@ Transform RegistrationVis::computeTransformationImpl(
 					cv::cvtColor(imageFrom, tmp, cv::COLOR_BGR2GRAY);
 					imageFrom = tmp;
 				}
+				orignalWordsFromIds.clear();
 				descriptorsFrom = detector->generateDescriptors(imageFrom, kptsFrom);
 			}

@@ -553,23 +562,45 @@ Transform RegistrationVis::computeTransformationImpl(
 			// create 3D keypoints
 			std::vector<cv::Point3f> kptsFrom3D;
 			std::vector<cv::Point3f> kptsTo3D;
-			if(fromSignature.getWords3().empty() || (kptsFrom.size() && kptsFrom.size() != fromSignature.getWords3().size()))
+			if(kptsFrom.size() == fromSignature.getWords3().size())
+			{
+				kptsFrom3D = uValues(fromSignature.getWords3());
+			}
+			else if(kptsFrom.size() == fromSignature.sensorData().keypoints3D().size())
+			{
+				kptsFrom3D = fromSignature.sensorData().keypoints3D();
+			}
+			else
 			{
 				if(fromSignature.getWords3().size() && kptsFrom.size() != fromSignature.getWords3().size())
 				{
 					UWARN("kptsFrom (%d) is not the same size as fromSignature.getWords3() (%d), there "
-						   "is maybe a problem with the logic above (getWords3() should be null or equal to kptsfrom).");
+						   "is maybe a problem with the logic above (getWords3() should be null or equal to kptsfrom).",
+						   kptsFrom.size(),
+						   fromSignature.getWords3().size());
+				}
+				else if(fromSignature.sensorData().keypoints3D().size() && kptsFrom.size() != fromSignature.sensorData().keypoints3D().size())
+				{
+					UWARN("kptsFrom (%d) is not the same size as fromSignature.sensorData().keypoints3D() (%d), there "
+						   "is maybe a problem with the logic above (keypoints3D() should be null or equal to kptsfrom).",
+						   kptsFrom.size(),
+						   fromSignature.sensorData().keypoints3D().size());
 				}
 				kptsFrom3D = detector->generateKeypoints3D(fromSignature.sensorData(), kptsFrom);
+				UDEBUG("generated kptsFrom3D=%d", (int)kptsFrom3D.size());
 				if(detector->getMinDepth() > 0.0f || detector->getMaxDepth() > 0.0f)
 				{
-					UDEBUG("");
 					//remove all keypoints/descriptors with no valid 3D points
 					UASSERT((int)kptsFrom.size() == descriptorsFrom.rows &&
 							kptsFrom3D.size() == kptsFrom.size());
 					std::vector<cv::KeyPoint> validKeypoints(kptsFrom.size());
 					std::vector<cv::Point3f> validKeypoints3D(kptsFrom.size());
 					cv::Mat validDescriptors(descriptorsFrom.size(), descriptorsFrom.type());
+					std::vector<int> validKeypointsIds;
+					if(orignalWordsFromIds.size())
+					{
+						validKeypointsIds.resize(kptsFrom.size());
+					}

 					int oi=0;
 					for(unsigned int i=0; i<kptsFrom3D.size(); ++i)
@@ -578,6 +609,10 @@ Transform RegistrationVis::computeTransformationImpl(
 						{
 							validKeypoints[oi] = kptsFrom[i];
 							validKeypoints3D[oi] = kptsFrom3D[i];
+							if(orignalWordsFromIds.size())
+							{
+								validKeypointsIds[oi] = orignalWordsFromIds[i];
+							}
 							descriptorsFrom.row(i).copyTo(validDescriptors.row(oi));
 							++oi;
 						}
@@ -587,19 +622,39 @@ Transform RegistrationVis::computeTransformationImpl(
 					validKeypoints3D.resize(oi);
 					kptsFrom = validKeypoints;
 					kptsFrom3D = validKeypoints3D;
+					
+					if(orignalWordsFromIds.size())
+					{
+					validKeypointsIds.resize(oi);
+						orignalWordsFromIds = validKeypointsIds;
+					}
 					descriptorsFrom = validDescriptors.rowRange(0, oi).clone();
 				}
 			}
-			else
+
+			if(kptsTo.size() == toSignature.getWords3().size())
 			{
-				kptsFrom3D = uValues(fromSignature.getWords3());
+				kptsTo3D = uValues(toSignature.getWords3());
 			}
-			if(toSignature.getWords3().empty() || kptsTo.size() != toSignature.getWords3().size())
+			else if(kptsTo.size() == toSignature.sensorData().keypoints3D().size())
+			{
+				kptsTo3D = toSignature.sensorData().keypoints3D();
+			}
+			else
 			{
 				if(toSignature.getWords3().size() && kptsTo.size() != toSignature.getWords3().size())
 				{
 					UWARN("kptsTo (%d) is not the same size as toSignature.getWords3() (%d), there "
-						   "is maybe a problem with the logic above (getWords3() should be null or equal to kptsTo).");
+						   "is maybe a problem with the logic above (getWords3() should be null or equal to kptsTo).",
+						   (int)kptsTo.size(),
+						   (int)toSignature.getWords3().size());
+				}
+				else if(toSignature.sensorData().keypoints3D().size() && kptsTo.size() != toSignature.sensorData().keypoints3D().size())
+				{
+					UWARN("kptsTo (%d) is not the same size as toSignature.sensorData().keypoints3D() (%d), there "
+						   "is maybe a problem with the logic above (keypoints3D() should be null or equal to kptsTo).",
+						   (int)kptsTo.size(),
+						   (int)toSignature.sensorData().keypoints3D().size());
 				}
 				kptsTo3D = detector->generateKeypoints3D(toSignature.sensorData(), kptsTo);
 				if(kptsTo3D.size() && (detector->getMinDepth() > 0.0f || detector->getMaxDepth() > 0.0f))
@@ -631,15 +686,11 @@ Transform RegistrationVis::computeTransformationImpl(
 					descriptorsTo = validDescriptors.rowRange(0, oi).clone();
 				}
 			}
-			else
-			{
-				kptsTo3D = uValues(toSignature.getWords3());
-			}

 			UASSERT(kptsFrom.empty() || descriptorsFrom.rows == 0 || int(kptsFrom.size()) == descriptorsFrom.rows);

-			fromSignature.sensorData().setFeatures(kptsFrom, descriptorsFrom);
-			toSignature.sensorData().setFeatures(kptsTo, descriptorsTo);
+			fromSignature.sensorData().setFeatures(kptsFrom, kptsFrom3D, descriptorsFrom);
+			toSignature.sensorData().setFeatures(kptsTo, kptsTo3D, descriptorsTo);

 			UDEBUG("descriptorsFrom=%d", descriptorsFrom.rows);
 			UDEBUG("descriptorsTo=%d", descriptorsTo.rows);
@@ -651,9 +702,9 @@ Transform RegistrationVis::computeTransformationImpl(
 				bool isCalibrated = false; // multiple cameras not supported.
 				if(imageSize.height == 0 || imageSize.width == 0)
 				{
-					imageSize = fromSignature.sensorData().cameraModels().size() == 1?fromSignature.sensorData().cameraModels()[0].imageSize():fromSignature.sensorData().stereoCameraModel().left().imageSize();
+					imageSize = toSignature.sensorData().cameraModels().size() == 1?toSignature.sensorData().cameraModels()[0].imageSize():toSignature.sensorData().stereoCameraModel().left().imageSize();
 				}
-				isCalibrated = imageSize.height != 0 && imageSize.width != 0 && fromSignature.sensorData().cameraModels().size()==1?fromSignature.sensorData().cameraModels()[0].isValidForProjection():fromSignature.sensorData().stereoCameraModel().isValidForProjection();
+				isCalibrated = imageSize.height != 0 && imageSize.width != 0 && toSignature.sensorData().cameraModels().size()==1?toSignature.sensorData().cameraModels()[0].isValidForProjection():toSignature.sensorData().stereoCameraModel().isValidForProjection();

 				// If guess is set, limit the search of matches using optical flow window size
 				bool guessSet = !guess.isIdentity() && !guess.isNull();
@@ -665,12 +716,12 @@ Transform RegistrationVis::computeTransformationImpl(
 					UASSERT((int)kptsFrom3D.size() == descriptorsFrom.rows);

 					// Use guess to project 3D "from" keypoints into "to" image
-					if(fromSignature.sensorData().cameraModels().size() > 1)
+					if(toSignature.sensorData().cameraModels().size() > 1)
 					{
 						UFATAL("Guess reprojection feature matching is not supported for multiple cameras.");
 					}

-					Transform localTransform = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].localTransform():fromSignature.sensorData().stereoCameraModel().left().localTransform();
+					Transform localTransform = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].localTransform():toSignature.sensorData().stereoCameraModel().left().localTransform();
 					Transform guessCameraRef = (guess * localTransform).inverse();
 					cv::Mat R = (cv::Mat_<double>(3,3) <<
 							(double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
@@ -679,7 +730,7 @@ Transform RegistrationVis::computeTransformationImpl(
 					cv::Mat rvec(1,3, CV_64FC1);
 					cv::Rodrigues(R, rvec);
 					cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
-					cv::Mat K = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].K():fromSignature.sensorData().stereoCameraModel().left().K();
+					cv::Mat K = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].K():toSignature.sensorData().stereoCameraModel().left().K();
 					std::vector<cv::Point2f> projected;
 					cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), projected);

@@ -688,10 +739,12 @@ Transform RegistrationVis::computeTransformationImpl(
 					std::vector<cv::Point2f> cornersProjected(projected.size());
 					std::vector<int> projectedIndexToDescIndex(projected.size());
 					int oi=0;
+					Transform guessInv = guess.inverse();
 					for(unsigned int i=0; i<projected.size(); ++i)
 					{
 						if(uIsInBounds(projected[i].x, 0.0f, float(imageSize.width-1)) &&
-						   uIsInBounds(projected[i].y, 0.0f, float(imageSize.height-1)))
+						   uIsInBounds(projected[i].y, 0.0f, float(imageSize.height-1)) &&
+						   util3d::transformPoint(kptsFrom3D[i], guessInv).x > 0.0)
 						{
 							projectedIndexToDescIndex[oi] = i;
 							cornersProjected[oi++] = projected[i];
@@ -712,7 +765,7 @@ Transform RegistrationVis::computeTransformationImpl(

 						// Create kd-tree for projected keypoints
 						rtflann::Matrix<float> cornersProjectedMat((float*)cornersProjected.data(), cornersProjected.size(), 2);
-						rtflann::Index<rtflann::L2<float> > index(cornersProjectedMat, rtflann::KDTreeIndexParams());
+						rtflann::Index<rtflann::L2_Simple<float> > index(cornersProjectedMat, rtflann::KDTreeIndexParams());
 						index.buildIndex();

 						std::vector< std::vector<size_t> > indices;
@@ -728,10 +781,10 @@ Transform RegistrationVis::computeTransformationImpl(
 						UASSERT(descriptorsFrom.rows == (int)kptsFrom.size());
 						UASSERT((int)pointsToMat.rows == descriptorsTo.rows);
 						UASSERT(pointsToMat.rows == kptsTo.size());
-						UDEBUG("");
+						UDEBUG("radius search done for guess");

 						// Process results (Nearest Neighbor Distance Ratio)
-						int newToId = descriptorsFrom.rows;
+						int newToId = orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows;
 						std::map<int,int> addedWordsFrom; //<id, index>
 						std::map<int, int> duplicates; //<fromId, toId>
 						int newWords = 0;
@@ -748,8 +801,7 @@ Transform RegistrationVis::computeTransformationImpl(
 									int oi=0;
 									for(unsigned int j=0; j<indices[i].size(); ++j)
 									{
-										if(kptsFrom.at(projectedIndexToDescIndex[indices[i].at(j)]).octave>=octave-1 &&
-										   kptsFrom.at(projectedIndexToDescIndex[indices[i].at(j)]).octave<=octave+1)
+										if(kptsFrom.at(projectedIndexToDescIndex[indices[i].at(j)]).octave==octave)
 										{
 											descriptors.push_back(descriptorsFrom.row(projectedIndexToDescIndex[indices[i].at(j)]));
 											descriptorsIndices[oi++] = indices[i].at(j);
@@ -774,8 +826,7 @@ Transform RegistrationVis::computeTransformationImpl(
 									}
 								}
 								else if(indices[i].size() == 1 &&
-									    kptsFrom.at(projectedIndexToDescIndex[indices[i].at(0)]).octave >= octave-1 &&
-										kptsFrom.at(projectedIndexToDescIndex[indices[i].at(0)]).octave <= octave+1)
+									    kptsFrom.at(projectedIndexToDescIndex[indices[i].at(0)]).octave == octave)
 								{
 									matchedIndex = indices[i].at(0);
 								}
@@ -849,7 +900,7 @@ Transform RegistrationVis::computeTransformationImpl(
 						int oi=0;
 						for(std::multimap<int, cv::KeyPoint>::iterator iter = wordsTo.begin(); iter!=wordsTo.end(); ++iter)
 						{
-							if(iter->first >= descriptorsFrom.rows+descriptorsTo.rows && wordsTo.count(iter->first) <= 1)
+							if(iter->first < (orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows) && wordsTo.count(iter->first) <= 1)
 							{
 								matches[oi++] = iter->second;
 							}
@@ -857,10 +908,10 @@ Transform RegistrationVis::computeTransformationImpl(
 						matches.resize(oi);
 						UDEBUG("guess=%s", guess.prettyPrint().c_str());
 						std::vector<cv::KeyPoint> projectedKpts;
-						cv::KeyPoint::convert(projected, projectedKpts);
+						cv::KeyPoint::convert(cornersProjected, projectedKpts);
 						cv::Mat image = toSignature.sensorData().imageRaw().clone();
-						drawKeypoints(image, projectedKpts, image, cv::Scalar(255,0,0));
 						drawKeypoints(image, kptsTo, image, cv::Scalar(0,0,255));
+						drawKeypoints(image, projectedKpts, image, cv::Scalar(0,255,255)); // BGR
 						drawKeypoints(image, matches, image, cv::Scalar(0,255,0));
 						cv::imwrite("projected.bmp", image);
 						UWARN("saved projected.bmp");*/
@@ -1167,38 +1218,57 @@ Transform RegistrationVis::computeTransformationImpl(
 			}
 		}

-		if(!transforms[1].isNull())
-		{
-			transforms[1] = transforms[1].inverse();
-		}
-
 		if(!_forwardEstimateOnly)
 		{
 			UDEBUG("from->to=%s", transforms[0].prettyPrint().c_str());
-			UDEBUG("from->from=%s", transforms[1].prettyPrint().c_str());
+			UDEBUG("to->from=%s", transforms[1].prettyPrint().c_str());
+		}
+
+		std::vector<int> allInliers = inliers[0];
+		if(inliers[1].size())
+		{
+			std::set<int> allInliersSet(allInliers.begin(), allInliers.end());
+			unsigned int oi = allInliers.size();
+			allInliers.resize(allInliers.size() + inliers[1].size());
+			for(unsigned int i=0; i<inliers[1].size(); ++i)
+			{
+				if(allInliersSet.find(inliers[1][i]) == allInliersSet.end())
+				{
+					allInliers[oi++] = inliers[1][i];
+				}
+			}
+			allInliers.resize(oi);
+		}
+		std::vector<int> allMatches = matches[0];
+		if(matches[1].size())
+		{
+			std::set<int> allMatchesSet(allMatches.begin(), allMatches.end());
+			unsigned int oi = allMatches.size();
+			allMatches.resize(allMatches.size() + matches[1].size());
+			for(unsigned int i=0; i<matches[1].size(); ++i)
+			{
+				if(allMatchesSet.find(matches[1][i]) == allMatchesSet.end())
+				{
+					allMatches[oi++] = matches[1][i];
+				}
+			}
+			allMatches.resize(oi);
 		}

 		if(_bundleAdjustment > 0 &&
 			_estimationType < 2 &&
 			!transforms[0].isNull() &&
-			inliers[0].size() &&
+			allInliers.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()));
+			Optimizer * sba = Optimizer::create(_bundleAdjustment==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, _bundleParameters);
+
 			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]));
@@ -1209,57 +1279,81 @@ Transform RegistrationVis::computeTransformationImpl(
 				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)
+			std::map<int, Transform> optimizedPoses;
+
+			UASSERT(toSignature.sensorData().stereoCameraModel().isValidForProjection() ||
+					(toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValidForProjection()));
+
+			std::map<int, CameraModel> models;
+
+			CameraModel cameraModelFrom;
+			if(fromSignature.sensorData().stereoCameraModel().isValidForProjection())
 			{
-				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));
+				cameraModelFrom = fromSignature.sensorData().stereoCameraModel().left();
+				// Set Tx=-baseline*fx for Stereo BA
+				cameraModelFrom = CameraModel(cameraModelFrom.fx(),
+						cameraModelFrom.fy(),
+						cameraModelFrom.cx(),
+						cameraModelFrom.cy(),
+						cameraModelFrom.localTransform(),
+						-fromSignature.sensorData().stereoCameraModel().baseline()*cameraModelFrom.fy());
+			}
+			else if(fromSignature.sensorData().cameraModels().size() == 1)
+			{
+				cameraModelFrom = fromSignature.sensorData().cameraModels()[0];
 			}

-			for(unsigned int i=0; i<inliers[1].size(); ++i)
+			CameraModel cameraModelTo;
+			if(toSignature.sensorData().stereoCameraModel().isValidForProjection())
 			{
-				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));
-						}
-					}
-				}
+				cameraModelTo = toSignature.sensorData().stereoCameraModel().left();
+				// Set Tx=-baseline*fx for Stereo BA
+				cameraModelTo = CameraModel(cameraModelTo.fx(),
+						cameraModelTo.fy(),
+						cameraModelTo.cx(),
+						cameraModelTo.cy(),
+						cameraModelTo.localTransform(),
+						-toSignature.sensorData().stereoCameraModel().baseline()*cameraModelTo.fy());
+			}
+			else if(toSignature.sensorData().cameraModels().size() == 1)
+			{
+				cameraModelTo = toSignature.sensorData().cameraModels()[0];
 			}

-			Optimizer * sba = Optimizer::create(_bundleAdjustment==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, _bundleParameters);
-			std::map<int, Transform> optimizedPoses = sba->optimizeBA(1, poses, links, models, points3DMap, wordReferences);
+			models.insert(std::make_pair(1, cameraModelFrom.isValidForProjection()?cameraModelFrom:cameraModelTo));
+			models.insert(std::make_pair(2, cameraModelTo));
+
+			std::map<int, std::map<int, cv::Point3f> > wordReferences;
+			for(unsigned int i=0; i<allInliers.size(); ++i)
+			{
+				int wordId = allInliers[i];
+				const cv::Point3f & pt3D = fromSignature.getWords3().find(wordId)->second;
+				points3DMap.insert(std::make_pair(wordId, pt3D));
+
+				std::map<int, cv::Point3f> ptMap;
+				if(fromSignature.getWords().size() && cameraModelFrom.isValidForProjection())
+				{
+					const cv::Point2f & kpt = fromSignature.getWords().find(wordId)->second.pt;
+					ptMap.insert(std::make_pair(1,cv::Point3f(kpt.x, kpt.y, pt3D.x)));
+				}
+				if(toSignature.getWords().size() && cameraModelTo.isValidForProjection())
+				{
+					const cv::Point2f & kpt = toSignature.getWords().find(wordId)->second.pt;
+					UASSERT(toSignature.getWords3().find(wordId) != toSignature.getWords3().end());
+					ptMap.insert(std::make_pair(2,cv::Point3f(kpt.x, kpt.y, toSignature.getWords3().find(wordId)->second.x)));
+				}
+
+				wordReferences.insert(std::make_pair(wordId, ptMap));
+
+				//UDEBUG("%d (%f,%f,%f)", wordId, points3DMap.at(wordId).x, points3DMap.at(wordId).y, points3DMap.at(wordId).z);
+				//for(std::map<int, cv::Point3f>::iterator iter=ptMap.begin(); iter!=ptMap.end(); ++iter)
+				//{
+				//	UDEBUG("%d (%f,%f) d=%f", iter->first, iter->second.x, iter->second.y, iter->second.z);
+				//}
+			}
+
+			std::set<int> sbaOutliers;
+			optimizedPoses = sba->optimizeBA(1, poses, links, models, points3DMap, wordReferences, &sbaOutliers);
 			delete sba;

 			//update transform
@@ -1267,6 +1361,7 @@ Transform RegistrationVis::computeTransformationImpl(
 				!optimizedPoses.begin()->second.isNull() &&
 				!optimizedPoses.rbegin()->second.isNull())
 			{
+				UDEBUG("Pose optimization: %s -> %s", transforms[0].prettyPrint().c_str(), optimizedPoses.rbegin()->second.prettyPrint().c_str());
 				transforms[0] = optimizedPoses.rbegin()->second;
 				transforms[1].setNull();
 				// update 3D points, both from and to signatures
@@ -1283,41 +1378,47 @@ Transform RegistrationVis::computeTransformationImpl(
 				}
 				fromSignature.setWords3(cpyWordsFrom3);
 				toSignature.setWords3(cpyWordsTo3);*/
+
+				if(sbaOutliers.size())
+				{
+					std::vector<int> newInliers(allInliers.size());
+					int oi=0;
+					for(unsigned int i=0; i<allInliers.size(); ++i)
+					{
+						if(sbaOutliers.find(allInliers[i]) == sbaOutliers.end())
+						{
+							newInliers[oi++] = allInliers[i];
+						}
+					}
+					newInliers.resize(oi);
+					UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(allInliers.size()));
+					allInliers = newInliers;
+				}
 			}
 		}

+		info.inliersIDs = allInliers;
+		info.matchesIDs = allMatches;
+		inliersCount = (int)allInliers.size();
+		matchesCount = (int)allMatches.size();
 		if(!transforms[1].isNull())
 		{
+			transforms[1] = transforms[1].inverse();
 			if(transforms[0].isNull())
 			{
 				transform = transforms[1];
-				info.inliersIDs = inliers[1];
-				info.matchesIDs = matches[1];
-
 				variance = variances[1];
-				inliersCount = (int)inliers[1].size();
-				matchesCount = (int)matches[1].size();
 			}
 			else
 			{
 				transform = transforms[0].interpolate(0.5f, transforms[1]);
-				info.inliersIDs = inliers[0];
-				info.matchesIDs = matches[0];
-
 				variance = (variances[0]+variances[1])/2.0f;
-				inliersCount = (int)(inliers[0].size()+inliers[1].size())/2;
-				matchesCount = (int)(matches[0].size()+matches[1].size())/2;
 			}
 		}
 		else
 		{
 			transform = transforms[0];
-			info.inliersIDs = inliers[0];
-			info.matchesIDs = matches[0];
-
 			variance = variances[0];
-			inliersCount = (int)inliers[0].size();
-			matchesCount = (int)matches[0].size();
 		}
 	}
 	else if(toSignature.sensorData().isValid())
@@ -1329,7 +1430,8 @@ Transform RegistrationVis::computeTransformationImpl(
 	info.inliers = inliersCount;
 	info.matches = matchesCount;
 	info.rejectedMsg = msg;
-	info.variance = variance>0.0f?variance:0.0001f; // epsilon if exact transform
+	info.varianceLin = variance>0.0f?variance:0.0001f; // epsilon if exact transform
+	info.varianceAng = info.varianceLin;

 	UDEBUG("transform=%s", transform.prettyPrint().c_str());
 	return transform;
--- a/corelib/src/Rtabmap.cpp
+++ b/corelib/src/Rtabmap.cpp
@@ -981,6 +981,9 @@ bool Rtabmap::process(
 	std::list<int> signaturesRemoved;
 	if(_rgbdSlamMode)
 	{
+		statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), covariance.empty()?1.0f:(float)covariance.at<double>(0,0));
+		statistics_.addStatistic(Statistics::kMemoryOdometry_variance_ang(), covariance.empty()?1.0f:(float)covariance.at<double>(5,5));
+
 		//Verify if there was a rehearsal
 		int rehearsedId = (int)uValue(statistics_.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
 		if(rehearsedId > 0)
@@ -1029,84 +1032,92 @@ bool Rtabmap::process(
 			const Signature * oldS = _memory->getSignature(oldId);
 			UASSERT(oldS != 0);

-			Transform guess = signature->getLinks().begin()->second.transform().inverse();
-
-			if(smallDisplacement)
+			if(signature->getWeight() >= 0 && oldS->getWeight()>=0) // ignore intermediate nodes
 			{
-				if(signature->getLinks().begin()->second.transVariance() == 1)
+				Transform guess = signature->getLinks().begin()->second.transform().inverse();
+
+				if(smallDisplacement)
 				{
-					// set small variance
-					UDEBUG("Set small variance. The robot is not moving.");
-					_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, 0.0001, 0.0001));
+					if(signature->getLinks().begin()->second.transVariance() == 1)
+					{
+						// set small variance
+						UDEBUG("Set small variance. The robot is not moving.");
+						_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, 0.0001, 0.0001));
+					}
+				}
+				else
+				{
+					//============================================================
+					// Refine neighbor links
+					//============================================================
+					if(!signature->sensorData().laserScanCompressed().empty())
+					{
+						UINFO("Odometry refining: guess = %s", guess.prettyPrint().c_str());
+						RegistrationInfo info;
+						Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, &info);
+						if(!t.isNull())
+						{
+							UINFO("Odometry refining: update neighbor link (%d->%d, variance:lin=%f, ang=%f) from %s to %s",
+									oldId,
+									signature->id(),
+									info.varianceLin,
+									info.varianceAng,
+									guess.prettyPrint().c_str(),
+									t.prettyPrint().c_str());
+							UASSERT(info.varianceLin > 0.0 && info.varianceAng > 0.0);
+							_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), t, info.varianceAng, info.varianceLin));
+
+							if(_optimizeFromGraphEnd)
+							{
+								// update all previous nodes
+								// Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
+								// parameters just changed state, we should put back all poses without map correction.
+								Transform u = guess * t.inverse();
+								std::map<int, Transform>::iterator jter = _optimizedPoses.find(oldId);
+								UASSERT(jter!=_optimizedPoses.end());
+								Transform up = jter->second * u * jter->second.inverse();
+								Transform mapCorrectionInv = _mapCorrection.inverse();
+								for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
+								{
+									iter->second = mapCorrectionInv * up * iter->second;
+								}
+							}
+						}
+						else
+						{
+							UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str());
+							if(info.varianceLin > 0 && info.varianceAng > 0)
+							{
+								_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, sqrt(info.varianceAng), sqrt(info.varianceLin)));
+							}
+						}
+						statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
+						statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), info.inliers);
+						statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers_ratio(), info.icpInliersRatio);
+						statistics_.addStatistic(Statistics::kNeighborLinkRefiningPts(), signature->sensorData().laserScanRaw().cols);
+					}
+				}
+				timeNeighborLinkRefining = timer.ticks();
+				ULOGGER_INFO("timeOdometryRefining=%fs", timeNeighborLinkRefining);
+
+				UASSERT(oldS->hasLink(signature->id()));
+				UASSERT(uContains(_optimizedPoses, oldId));
+
+				statistics_.addStatistic(Statistics::kNeighborLinkRefiningVariance(), oldS->getLinks().at(signature->id()).transVariance());
+
+				newPose = _optimizedPoses.at(oldId) * oldS->getLinks().at(signature->id()).transform();
+				_mapCorrection = newPose * signature->getPose().inverse();
+				if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
+				{
+					UERROR("Map correction should be identity when optimizing from the last node. T=%s NewPose=%s OldPose=%s",
+							_mapCorrection.prettyPrint().c_str(),
+							newPose.prettyPrint().c_str(),
+							signature->getPose().prettyPrint().c_str());
 				}
 			}
 			else
 			{
-				//============================================================
-				// Refine neighbor links
-				//============================================================
-				if(!signature->sensorData().laserScanCompressed().empty())
-				{
-					UINFO("Odometry refining: guess = %s", guess.prettyPrint().c_str());
-					RegistrationInfo info;
-					Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, &info);
-					if(!t.isNull())
-					{
-						UINFO("Odometry refining: update neighbor link (%d->%d, variance=%f) from %s to %s",
-								oldId,
-								signature->id(),
-								info.variance,
-								guess.prettyPrint().c_str(),
-								t.prettyPrint().c_str());
-						UASSERT(info.variance > 0.0);
-						_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), t, info.variance, info.variance));
-
-						if(_optimizeFromGraphEnd)
-						{
-							// update all previous nodes
-							// Normally _mapCorrection should be identity, but if _optimizeFromGraphEnd
-							// parameters just changed state, we should put back all poses without map correction.
-							Transform u = guess * t.inverse();
-							std::map<int, Transform>::iterator jter = _optimizedPoses.find(oldId);
-							UASSERT(jter!=_optimizedPoses.end());
-							Transform up = jter->second * u * jter->second.inverse();
-							Transform mapCorrectionInv = _mapCorrection.inverse();
-							for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
-							{
-								iter->second = mapCorrectionInv * up * iter->second;
-							}
-						}
-					}
-					else
-					{
-						UINFO("Odometry refining rejected: %s", info.rejectedMsg.c_str());
-						if(info.variance > 0)
-						{
-							double sqrtVar = sqrt(info.variance);
-							_memory->updateLink(Link(oldId, signature->id(), signature->getLinks().begin()->second.type(), guess, sqrtVar, sqrtVar));
-						}
-					}
-					statistics_.addStatistic(Statistics::kNeighborLinkRefiningAccepted(), !t.isNull()?1.0f:0);
-					statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers(), info.inliers);
-					statistics_.addStatistic(Statistics::kNeighborLinkRefiningInliers_ratio(), info.icpInliersRatio);
-					statistics_.addStatistic(Statistics::kNeighborLinkRefiningVariance(), info.variance);
-					statistics_.addStatistic(Statistics::kNeighborLinkRefiningPts(), signature->sensorData().laserScanRaw().cols);
-				}
-			}
-			timeNeighborLinkRefining = timer.ticks();
-			ULOGGER_INFO("timeOdometryRefining=%fs", timeNeighborLinkRefining);
-
-			UASSERT(oldS->hasLink(signature->id()));
-			UASSERT(uContains(_optimizedPoses, oldId));
-
-			newPose = _optimizedPoses.at(oldId) * oldS->getLinks().at(signature->id()).transform();
-			_mapCorrection = newPose * signature->getPose().inverse();
-			if(_mapCorrection.getNormSquared() > 0.001f && _optimizeFromGraphEnd)
-			{
-				UERROR("Map correction should be identity when optimizing from the last node. T=%s NewPose=%s OldPose=%s",
-						_mapCorrection.prettyPrint().c_str(),
-						newPose.prettyPrint().c_str(),
-						signature->getPose().prettyPrint().c_str());
+				UWARN("Neighbor link refining is activated but there are intermediate nodes, aborting refining...");
 			}
 		}
 		else
@@ -1220,8 +1231,8 @@ bool Rtabmap::process(
 								*iter,
 								transform.prettyPrint().c_str());
 						// Add a loop constraint
-						UASSERT(info.variance > 0.0);
-						if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, info.variance, info.variance)))
+						UASSERT(info.varianceLin > 0.0 && info.varianceAng > 0.0);
+						if(_memory->addLink(Link(signature->id(), *iter, Link::kLocalTimeClosure, transform, info.varianceAng, info.varianceLin)))
 						{
 							++proximityDetectionsInTimeFound;
 							UINFO("Local loop closure found between %d and %d with t=%s",
@@ -1763,7 +1774,7 @@ bool Rtabmap::process(
 		//Compute transform if metric data are present
 		Transform transform;
 		RegistrationInfo info;
-		info.variance = 1.0f;
+		info.varianceLin = info.varianceAng = 1.0f;
 		if(_rgbdSlamMode)
 		{
 			transform = _memory->computeTransform(_loopClosureHypothesis.first, signature->id(), Transform(), &info);
@@ -1782,8 +1793,8 @@ bool Rtabmap::process(
 		if(!rejectedHypothesis)
 		{
 			// Make the new one the parent of the old one
-			UASSERT(info.variance > 0.0);
-			rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, info.variance, info.variance));
+			UASSERT(info.varianceLin > 0.0 && info.varianceAng > 0.0);
+			rejectedHypothesis = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, info.varianceAng, info.varianceLin));
 			if(!rejectedHypothesis)
 			{
 				loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first));
@@ -1877,8 +1888,8 @@ bool Rtabmap::process(
 						{
 							++localVisualPathsChecked;
 							RegistrationInfo info;
-							Transform guess = _optimizedPoses.at(signature->id()).inverse() * _optimizedPoses.at(nearestId);
-							Transform transform = _memory->computeTransform(nearestId, signature->id(), guess, &info);
+							// guess is null to make sure visual correspondences are globally computed
+							Transform transform = _memory->computeTransform(nearestId, signature->id(), Transform(), &info);
 							if(!transform.isNull())
 							{
 								transform = transform.inverse();
@@ -1888,8 +1899,8 @@ bool Rtabmap::process(
 											signature->id(),
 											nearestId,
 											transform.prettyPrint().c_str());
-									UASSERT(info.variance > 0.0);
-									_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, info.variance, info.variance));
+									UASSERT(info.varianceLin > 0.0 && info.varianceAng > 0.0);
+									_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, info.varianceAng, info.varianceLin));
 									loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));

 									if(loopClosureVisualInliers == 0)
@@ -2013,9 +2024,8 @@ bool Rtabmap::process(
 											}

 											// set Identify covariance for laser scan matching only
-											UASSERT(info.variance>0.0);
-											double sqrtVar = sqrt(info.variance);
-											_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, sqrtVar, sqrtVar, scanMatchingIds));
+											UASSERT(info.varianceLin>0.0 && info.varianceAng>0.0);
+											_memory->addLink(Link(signature->id(), nearestId, Link::kLocalSpaceClosure, transform, sqrt(info.varianceAng), sqrt(info.varianceLin), scanMatchingIds));
 											loopClosureLinksAdded.push_back(std::make_pair(signature->id(), nearestId));

 											++proximityDetectionsAddedByICPOnly;
@@ -3397,6 +3407,19 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
 	std::map<int, Signature> signatures;
 	this->getGraph(poses, links, true, true, &signatures);

+	//remove all invalid or intermediate nodes
+	for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end();)
+	{
+		if(signatures.at(iter->first).getWeight() < 0)
+		{
+			poses.erase(iter++);
+		}
+		else
+		{
+			++iter;
+		}
+	}
+
 	for(int n=0; n<iterations; ++n)
 	{
 		UINFO("Looking for more loop closures, clustering poses... (iteration=%d/%d, radius=%f m angle=%f rad)",
@@ -3442,8 +3465,8 @@ int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int
 						UINFO("Added new loop closure between %d and %d.", from, to);
 						addedLinks.insert(from);
 						addedLinks.insert(to);
-						links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, info.variance, info.variance)));
-						loopClosuresAdded.push_back(Link(from, to, Link::kUserClosure, t, info.variance, info.variance));
+						links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, info.varianceAng, info.varianceLin)));
+						loopClosuresAdded.push_back(Link(from, to, Link::kUserClosure, t, info.varianceAng, info.varianceLin));
 						UINFO("Detected loop closure %d->%d! (%d/%d)", from, to, i+1, (int)clusters.size());
 					}
 				}
@@ -3512,7 +3535,7 @@ int Rtabmap::refineLinks()

 		if(!t.isNull())
 		{
-			linksRefined.push_back(Link(from, to, iter->second.type(), t, info.variance, info.variance));
+			linksRefined.push_back(Link(from, to, iter->second.type(), t, info.varianceAng, info.varianceLin));
 			UINFO("Refined link %d->%d! (%d/%d)", from, to, ++i, (int)links.size());
 		}
 	}
--- a/corelib/src/RtabmapThread.cpp
+++ b/corelib/src/RtabmapThread.cpp
@@ -568,7 +568,8 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 		}
 		if(!lastPose_.isIdentity() &&
 						(odomEvent.pose().isIdentity() ||
-						odomEvent.info().variance>=9999 ||
+						odomEvent.info().varianceLin>=9999 ||
+						odomEvent.info().varianceAng>=9999 ||
 						odomEvent.rotVariance()>=9999 ||
 						odomEvent.transVariance()>=9999))
 		{
@@ -581,13 +582,13 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 		double maxRotVar = odomEvent.rotVariance();
 		double maxTransVar = odomEvent.transVariance();
 		// FIXME: should merge the transformations/variances like Link::merge();
-		if(maxRotVar > _rotVariance)
+		if(maxRotVar != 1.0f)
 		{
-			_rotVariance = maxRotVar;
+			_rotVariance += maxRotVar;
 		}
-		if(maxTransVar > _transVariance)
+		if(maxTransVar != 1.0f)
 		{
-			_transVariance = maxTransVar;
+			_transVariance += maxTransVar;
 		}

 		if(ignoreFrame && !_createIntermediateNodes)
@@ -617,7 +618,7 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 			// set negative id so rtabmap will detect it as an intermediate node
 			SensorData tmp = odomEvent.data();
 			tmp.setId(-1);
-			tmp.setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());// remove features
+			tmp.setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());// remove features
 			_dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), _rotVariance, _transVariance));
 		}
 		else
--- a/corelib/src/SensorData.cpp
+++ b/corelib/src/SensorData.cpp
@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/Compression.h"
 #include "rtabmap/utilite/ULogger.h"
 #include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UConversion.h>

 namespace rtabmap
 {
@@ -736,6 +737,15 @@ void SensorData::uncompressDataConst(
 	}
 }

+void SensorData::setFeatures(const std::vector<cv::KeyPoint> & keypoints, const std::vector<cv::Point3f> & keypoints3D, const cv::Mat & descriptors)
+{
+	UASSERT_MSG(keypoints3D.empty() || keypoints.size() == keypoints3D.size(), uFormat("keypoints=%d keypoints3D=%d", (int)keypoints.size(), (int)keypoints3D.size()).c_str());
+	UASSERT_MSG(descriptors.empty() || keypoints.size() == descriptors.rows, uFormat("keypoints=%d descriptors=%d", (int)keypoints.size(), descriptors.rows).c_str());
+	_keypoints = keypoints;
+	_keypoints3D = keypoints3D;
+	_descriptors = descriptors;
+}
+
 long SensorData::getMemoryUsed() const // Return memory usage in Bytes
 {
 	return _imageCompressed.total()*_imageCompressed.elemSize() +
--- a/corelib/src/Transform.cpp
+++ b/corelib/src/Transform.cpp
@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Transform.h>

 #include <pcl/common/eigen.h>
+#include <pcl/common/common.h>
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UMath.h>
@@ -208,6 +209,13 @@ void Transform::getTranslation(float & x, float & y, float & z) const
 	z = this->z();
 }

+float Transform::getAngle(float x, float y, float z) const
+{
+	Eigen::Vector3f vA(x,y,z);
+	Eigen::Vector3f vB = this->toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
+	return pcl::getAngle3D(Eigen::Vector4f(vA[0], vA[1], vA[2], 0), Eigen::Vector4f(vB[0], vB[1], vB[2], 0));
+}
+
 float Transform::getNorm() const
 {
 	return uNorm(this->x(), this->y(), this->z());
--- a/corelib/src/util3d_motion_estimation.cpp
+++ b/corelib/src/util3d_motion_estimation.cpp
@@ -151,7 +151,7 @@ Transform estimateMotion3DTo2D(
 						cv::Point3f newPt = util3d::transformPoint(iter->second, transform);
 						errorSqrdDists[oi] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
 						//ignore very very far features (stereo)
-						if(errorSqrdDists[oi] < 100.0f)
+						if(errorSqrdDists[oi] < iter->second.x/100.0f)
 						{
 							++oi;
 						}
--- a/corelib/src/util3d_registration.cpp
+++ b/corelib/src/util3d_registration.cpp
@@ -353,13 +353,6 @@ Transform icpPointToPlane(
 	icp.setInputTarget (cloud_target);
 	icp.setInputSource (cloud_source);

-	if(icp2D)
-	{
-		pcl::registration::TransformationEstimation2D<pcl::PointNormal, pcl::PointNormal>::Ptr est;
-		est.reset(new pcl::registration::TransformationEstimation2D<pcl::PointNormal, pcl::PointNormal>);
-		icp.setTransformationEstimation(est);
-	}
-
 	pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>::Ptr est;
 	est.reset(new pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal>);
 	icp.setTransformationEstimation(est);
@@ -377,7 +370,15 @@ Transform icpPointToPlane(
 	// Perform the alignment
 	icp.align (cloud_source_registered);
 	hasConverged = icp.hasConverged();
-	return Transform::fromEigen4f(icp.getFinalTransformation());
+	Transform t = Transform::fromEigen4f(icp.getFinalTransformation());
+
+	if(icp2D)
+	{
+		// FIXME probably an estimation approach already 2D like in icp() version above exists.
+		t = t.to3DoF();
+	}
+
+	return t;
 }

 }