Refactored Odometry class:

-new odometry parameters
-new Optical flow strategy
-Stereo data support 
Refactored SensorData class to support stereo images
Updated default parameters (mostly odometry ones)
util3d: new methods to handle/reconstruct 3D clouds from disparity image / stereo images
PreferencesDialog: added Odometry/BOW and Odometry/OpticalFLow panels.
DatabaseViewer: fixed a crash when database is empty. Added cloud reconstruction of stereo images if saved in database
Camera: added 1 second delay to avoid dark images at the starting


git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1849 f169173b-cf89-36c8-b27e-44dbe73f0c83

corelib/src/CameraThread.cpp

@@ -92,6 +92,9 @@ bool CameraThread::init()
 		{
 			return _camera->init();
 		}
+
+		// Added sleep time to ignore first frames (which are darker)
+		uSleep(1000);
 	}
 	else
 	{

corelib/src/DBDriverSqlite3.cpp

@@ -2025,7 +2025,7 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt,
 
 	if(uStrNumCmp(_version, "0.7.0") < 0)
 	{
-		rc = sqlite3_bind_double(ppStmt, index++, 1.0f/fy);
+		rc = sqlite3_bind_double(ppStmt, index++, 1.0f/fx);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error: %s", sqlite3_errmsg(_ppDb)).c_str());
 	}
 	else

corelib/src/Features2d.cpp

@@ -46,41 +46,39 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-void filterKeypointsByDepth(
+void Feature2D::filterKeypointsByDepth(
 		std::vector<cv::KeyPoint> & keypoints,
 		const cv::Mat & depth,
-		float fx,
-		float fy,
-		float cx,
-		float cy,
 		float maxDepth)
 {
 	cv::Mat descriptors;
-	filterKeypointsByDepth(keypoints, descriptors, depth, fx, fy, cx, cy, maxDepth);
+	filterKeypointsByDepth(keypoints, descriptors, depth, maxDepth);
 }
 
-void filterKeypointsByDepth(
+void Feature2D::filterKeypointsByDepth(
 		std::vector<cv::KeyPoint> & keypoints,
 		cv::Mat & descriptors,
 		const cv::Mat & depth,
-		float fx,
-		float fy,
-		float cx,
-		float cy,
 		float maxDepth)
 {
-	if(!depth.empty() && fx > 0.0f && fy > 0.0f && maxDepth > 0.0f && (descriptors.empty() || descriptors.rows == (int)keypoints.size()))
+	if(!depth.empty() && maxDepth > 0.0f && (descriptors.empty() || descriptors.rows == (int)keypoints.size()))
 	{
 		std::vector<cv::KeyPoint> output(keypoints.size());
 		std::vector<int> indexes(keypoints.size(), 0);
 		int oi=0;
+		bool isInMM = depth.type() == CV_16UC1;
 		for(unsigned int i=0; i<keypoints.size(); ++i)
 		{
-			pcl::PointXYZ pt = util3d::getDepth(depth, keypoints[i].pt.x, keypoints[i].pt.y, cx, cy, fx, fy, true);
-			if(uIsFinite(pt.z) && pt.z < maxDepth)
+			int u = int(keypoints[i].pt.x+0.5f);
+			int v = int(keypoints[i].pt.y+0.5f);
+			if(u >=0 && u<depth.cols && v >=0 && v<depth.rows)
 			{
-				output[oi++] = keypoints[i];
-				indexes[i] = 1;
+				float d = isInMM?(float)depth.at<uint16_t>(v,u)*0.001f:depth.at<float>(v,u);
+				if(d!=0.0f && uIsFinite(d) && d < maxDepth)
+				{
+					output[oi++] = keypoints[i];
+					indexes[i] = 1;
+				}
 			}
 		}
 		output.resize(oi);
@@ -116,15 +114,15 @@ void filterKeypointsByDepth(
 	}
 }
 
-void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints)
+void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints)
 {
 	cv::Mat descriptors;
 	limitKeypoints(keypoints, descriptors, maxKeypoints);
 }
 
-void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints)
+void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints)
 {
-	UASSERT((int)keypoints.size() == descriptors.rows || descriptors.rows == 0);
+	UASSERT_MSG((int)keypoints.size() == descriptors.rows || descriptors.rows == 0, uFormat("keypoints=%d descriptors=%d", (int)keypoints.size(), descriptors.rows).c_str());
 	if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
 	{
 		UTimer timer;
@@ -173,7 +171,39 @@ void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors
 	}
 }
 
-cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
+cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::string & roiRatios)
+{
+	std::list<std::string> strValues = uSplit(roiRatios, ' ');
+	if(strValues.size() != 4)
+	{
+		UERROR("The number of values must be 4 (roi=\"%s\")", roiRatios.c_str());
+	}
+	else
+	{
+		std::vector<float> values(4);
+		unsigned int i=0;
+		for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
+		{
+			values[i] = std::atof((*iter).c_str());
+			++i;
+		}
+
+		if(values[0] >= 0 && values[0] < 1 && values[0] < 1.0f-values[1] &&
+			values[1] >= 0 && values[1] < 1 && values[1] < 1.0f-values[0] &&
+			values[2] >= 0 && values[2] < 1 && values[2] < 1.0f-values[3] &&
+			values[3] >= 0 && values[3] < 1 && values[3] < 1.0f-values[2])
+		{
+			return computeRoi(image, values);
+		}
+		else
+		{
+			UERROR("The roi ratios are not valid (roi=\"%s\")", roiRatios.c_str());
+		}
+	}
+	return cv::Rect();
+}
+
+cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
 {
 	if(!image.empty() && roiRatios.size() == 4)
 	{
@@ -222,6 +252,40 @@ cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
 /////////////////////
 // Feature2D
 /////////////////////
+Feature2D * Feature2D::create(Feature2D::Type & type, const ParametersMap & parameters)
+{
+	Feature2D * feature2D = 0;
+	switch(type)
+	{
+	case Feature2D::kFeatureSift:
+		feature2D = new SIFT(parameters);
+		break;
+	case Feature2D::kFeatureFastBrief:
+		feature2D = new FAST_BRIEF(parameters);
+		break;
+	case Feature2D::kFeatureFastFreak:
+		feature2D = new FAST_FREAK(parameters);
+		break;
+	case Feature2D::kFeatureOrb:
+		feature2D = new ORB(parameters);
+		break;
+	case Feature2D::kFeatureGfttFreak:
+		feature2D = new GFTT_FREAK(parameters);
+		break;
+	case Feature2D::kFeatureGfttBrief:
+		feature2D = new GFTT_BRIEF(parameters);
+		break;
+	case Feature2D::kFeatureBrisk:
+		feature2D = new BRISK(parameters);
+		break;
+	case Feature2D::kFeatureSurf:
+	default:
+		feature2D = new SURF(parameters);
+		type = Feature2D::kFeatureSurf;
+		break;
+	}
+	return feature2D;
+}
 std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, int maxKeypoints, const cv::Rect & roi) const
 {
 	ULOGGER_DEBUG("");
@@ -261,7 +325,10 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, in
 
 cv::Mat Feature2D::generateDescriptors(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
 {
-	return generateDescriptorsImpl(image, keypoints);
+	cv::Mat descriptors = generateDescriptorsImpl(image, keypoints);
+	UASSERT_MSG(descriptors.rows == (int)keypoints.size(), uFormat("descriptors=%d, keypoints=%d", descriptors.rows, (int)keypoints.size()).c_str());
+	UDEBUG("Descriptors extracted = %d, remaining kpts=%d", descriptors.rows, (int)keypoints.size());
+	return descriptors;
 }
 
 //////////////////////////

corelib/src/Memory.cpp

@@ -196,6 +196,10 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
 			{
 				_lastSignature = uValue(_signatures, *_stMem.rbegin(), (Signature*)0);
 			}
+			else if(_workingMem.size()>0)
+			{
+				_lastSignature = uValue(_signatures, *_workingMem.rbegin(), (Signature*)0);
+			}
 
 			// Last id
 			_dbDriver->getLastNodeId(_idCount);
@@ -429,42 +433,9 @@ void Memory::parseParameters(const ParametersMap & parameters)
 			_feature2D = 0;
 			_featureType = Feature2D::kFeatureUndef;
 		}
-		switch(detectorStrategy)
-		{
-		case Feature2D::kFeatureSift:
-			_feature2D = new SIFT(parameters);
-			_featureType = Feature2D::kFeatureSift;
-			break;
-		case Feature2D::kFeatureFastBrief:
-			_feature2D = new FAST_BRIEF(parameters);
-			_featureType = Feature2D::kFeatureFastBrief;
-			break;
-		case Feature2D::kFeatureFastFreak:
-			_feature2D = new FAST_FREAK(parameters);
-			_featureType = Feature2D::kFeatureFastFreak;
-			break;
-		case Feature2D::kFeatureOrb:
-			_feature2D = new ORB(parameters);
-			_featureType = Feature2D::kFeatureOrb;
-			break;
-		case Feature2D::kFeatureGfttFreak:
-			_feature2D = new GFTT_FREAK(parameters);
-			_featureType = Feature2D::kFeatureGfttFreak;
-			break;
-		case Feature2D::kFeatureGfttBrief:
-			_feature2D = new GFTT_BRIEF(parameters);
-			_featureType = Feature2D::kFeatureGfttBrief;
-			break;
-		case Feature2D::kFeatureBrisk:
-			_feature2D = new BRISK(parameters);
-			_featureType = Feature2D::kFeatureBrisk;
-			break;
-		case Feature2D::kFeatureSurf:
-		default:
-			_feature2D = new SURF(parameters);
-			_featureType = Feature2D::kFeatureSurf;
-			break;
-		}
+
+		_feature2D = Feature2D::create(detectorStrategy, parameters);
+		_featureType = detectorStrategy;
 	}
 	else if(_feature2D)
 	{
@@ -1605,6 +1576,10 @@ void Memory::moveToTrash(Signature * s, bool saveToDatabase, std::list<int> * de
 			{
 				_lastSignature = this->_getSignature(*_stMem.rbegin());
 			}
+			else if(_workingMem.size())
+			{
+				_lastSignature = this->_getSignature(*_workingMem.rbegin());
+			}
 		}
 
 		if(	saveToDatabase &&
@@ -1727,12 +1702,15 @@ Transform Memory::computeVisualTransform(const Signature & oldS, const Signature
 			UDEBUG("Correspondences = %d", (int)inliersOld->size());
 
 			int inliersCount = 0;
+			std::vector<int> inliersV;
 			Transform t = util3d::transformFromXYZCorrespondences(
 					inliersOld,
 					inliersNew,
 					_bowInlierDistance,
 					_bowIterations,
-					&inliersCount);
+					true, 3.0, 10,
+					&inliersV);
+			inliersCount = inliersV.size();
 			if(!t.isNull() && inliersCount >= _bowMinInliers)
 			{
 				transform = t;
@@ -3038,16 +3016,12 @@ void Memory::extractKeypointsAndDescriptors(
 		std::vector<cv::KeyPoint> & keypoints,
 		cv::Mat & descriptors) const
 {
-	extractKeypointsAndDescriptors(image, cv::Mat(), 0,0,0,0, keypoints, descriptors);
+	extractKeypointsAndDescriptors(image, cv::Mat(), keypoints, descriptors);
 }
 
 void Memory::extractKeypointsAndDescriptors(
 		const cv::Mat & image,
 		const cv::Mat & depth,
-		float fx,
-		float fy,
-		float cx,
-		float cy,
 		std::vector<cv::KeyPoint> & keypoints,
 		cv::Mat & descriptors) const
 {
@@ -3056,12 +3030,12 @@ void Memory::extractKeypointsAndDescriptors(
 		UTimer timer;
 		if(_feature2D)
 		{
-			cv::Rect roi = computeRoi(image, _roiRatios);
+			cv::Rect roi = Feature2D::computeRoi(image, _roiRatios);
 			keypoints = _feature2D->generateKeypoints(image, 0, roi);
 			UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), timer.ticks());
 
-			filterKeypointsByDepth(keypoints, depth, fx, fy, cx, cy, _wordsMaxDepth);
-			limitKeypoints(keypoints, _wordsPerImageTarget);
+			Feature2D::filterKeypointsByDepth(keypoints, depth, _wordsMaxDepth);
+			Feature2D::limitKeypoints(keypoints, _wordsPerImageTarget);
 		}
 		else
 		{
@@ -3100,6 +3074,7 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 {
 	UASSERT(data.image().empty() || data.image().type() == CV_8UC1 || data.image().type() == CV_8UC3);
 	UASSERT(data.depth().empty() || data.depth().type() == CV_16UC1 || data.depth().type() == CV_32FC1);
+	UASSERT(data.rightImage().empty() || data.rightImage().type() == CV_8UC1);
 	UASSERT(data.depth2d().empty() || data.depth2d().type() == CV_32FC2);
 
 	PreUpdateThread preUpdateThread(_vwd);
@@ -3167,8 +3142,6 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 
 		this->extractKeypointsAndDescriptors(imageMono,
 				data.depth(),
-				data.depthFx(), data.depthFy(),
-				data.depthCx(), data.depthCy(),
 				keypoints,
 				descriptors);
 
@@ -3183,12 +3156,10 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 		keypoints = data.keypoints();
 		descriptors = data.descriptors().clone();
 
-		filterKeypointsByDepth(keypoints, descriptors,
+		Feature2D::filterKeypointsByDepth(keypoints, descriptors,
 				data.depth(),
-				data.depthFx(), data.depthFy(),
-				data.depthCx(), data.depthCy(),
 				_wordsMaxDepth);
-		limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
+		Feature2D::limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
 	}
 
 	if(_parallelized)
@@ -3229,9 +3200,9 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 
 	//3d words
 	std::multimap<int, pcl::PointXYZ> words3;
-	if(!data.depth().empty() && data.depthFx() && data.depthFy())
+	if(!data.depth().empty() && data.fx() && data.fy())
 	{
-		words3 = util3d::generateWords3(words, data.depth(), data.depthFx(), data.depthFy(), data.depthCx(), data.depthCy(), data.localTransform());
+		words3 = util3d::generateWords3(words, data.depth(), data.fx(), data.fy(), data.cx(), data.cy(), data.localTransform());
 	}
 
 	Signature * s;
@@ -3243,9 +3214,17 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 		{
 			UWARN("Keeping raw data in database: depth type is 32FC1, use 16UC1 depth format to avoid a conversion.");
 		}
-		cv::Mat depthMM = data.depth().type() == CV_32FC1?util3d::cvtDepthFromFloat(data.depth()):data.depth();
+		cv::Mat depthOrRightImage;
+		if(!data.depth().empty())
+		{
+			depthOrRightImage = data.depth().type() == CV_32FC1?util3d::cvtDepthFromFloat(data.depth()):data.depth();
+		}
+		else if(!data.rightImage().empty())
+		{
+			depthOrRightImage = data.rightImage();
+		}
 		util3d::CompressionThread ctImage(data.image(), std::string(".jpg"));
-		util3d::CompressionThread ctDepth(depthMM, std::string(".png"));
+		util3d::CompressionThread ctDepth(depthOrRightImage, std::string(".png"));
 		ctImage.start();
 		ctDepth.start();
 		ctImage.join();
@@ -3261,10 +3240,10 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 			util3d::compressData(data.depth2d()),
 			imageBytes,
 			depthBytes,
-			data.depthFx(),
-			data.depthFy(),
-			data.depthCx(),
-			data.depthCy(),
+			data.fx(),
+			data.fy()>0.0f?data.fy():data.baseline(),
+			data.cx(),
+			data.cy(),
 			data.localTransform());
 		s->setImageRaw(data.image());
 		s->setDepthRaw(data.depth());

corelib/src/Odometry.cpp

@@ -40,9 +40,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Memory.h>
 #include <rtabmap/core/VWDictionary.h>
 #include "rtabmap/core/Signature.h"
+#include "rtabmap/core/Features2d.h"
 
 #include <pcl/io/pcd_io.h>
 #include <pcl/common/transforms.h>
+#include <pcl/common/distances.h>
 
 #include <opencv2/gpu/gpu.hpp>
 
@@ -55,16 +57,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap {
 
 Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
-		_maxFeatures(Parameters::defaultOdomMaxWords()),
+		_maxFeatures(Parameters::defaultOdomMaxFeatures()),
+		_roiRatios(Parameters::defaultOdomRoiRatios()),
 		_minInliers(Parameters::defaultOdomMinInliers()),
 		_inlierDistance(Parameters::defaultOdomInlierDistance()),
 		_iterations(Parameters::defaultOdomIterations()),
-		_wordsRatio(Parameters::defaultOdomWordsRatio()),
+		_refineIterations(Parameters::defaultOdomRefineIterations()),
+		_featuresRatio(Parameters::defaultOdomFeaturesRatio()),
 		_maxDepth(Parameters::defaultOdomMaxDepth()),
 		_linearUpdate(Parameters::defaultOdomLinearUpdate()),
 		_angularUpdate(Parameters::defaultOdomAngularUpdate()),
 		_resetCountdown(Parameters::defaultOdomResetCountdown()),
-		_localHistoryMaxSize(Parameters::defaultOdomLocalHistory()),
 		_pose(Transform::getIdentity()),
 		_resetCurrentCount(0)
 {
@@ -74,10 +77,11 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomMinInliers(), _minInliers);
 	Parameters::parse(parameters, Parameters::kOdomInlierDistance(), _inlierDistance);
 	Parameters::parse(parameters, Parameters::kOdomIterations(), _iterations);
-	Parameters::parse(parameters, Parameters::kOdomWordsRatio(), _wordsRatio);
+	Parameters::parse(parameters, Parameters::kOdomRefineIterations(), _refineIterations);
+	Parameters::parse(parameters, Parameters::kOdomFeaturesRatio(), _featuresRatio);
 	Parameters::parse(parameters, Parameters::kOdomMaxDepth(), _maxDepth);
-	Parameters::parse(parameters, Parameters::kOdomMaxWords(), _maxFeatures);
-	Parameters::parse(parameters, Parameters::kOdomLocalHistory(), _localHistoryMaxSize);
+	Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), _maxFeatures);
+	Parameters::parse(parameters, Parameters::kOdomRoiRatios(), _roiRatios);
 }
 
 void Odometry::reset()
@@ -126,23 +130,27 @@ Transform Odometry::process(SensorData & data, int * quality, int * features, in
 //OdometryBOW
 OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
 	Odometry(parameters),
+	_localHistoryMaxSize(Parameters::defaultOdomBowLocalHistorySize()),
 	_memory(0)
 {
+	Parameters::parse(parameters, Parameters::kOdomBowLocalHistorySize(), _localHistoryMaxSize);
+
 	ParametersMap customParameters;
 	customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(this->getMaxFeatures()))); // hack
 	customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(this->getMaxDepth())));
+	customParameters.insert(ParametersPair(Parameters::kKpRoiRatios(), this->getRoiRatios()));
 	customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
 	customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "false"));
 	customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
-	int nn = Parameters::defaultOdomNearestNeighbor();
-	float nndr = Parameters::defaultOdomNNDR();
-	int odomType = Parameters::defaultOdomType();
-	Parameters::parse(parameters, Parameters::kOdomNearestNeighbor(), nn);
-	Parameters::parse(parameters, Parameters::kOdomNNDR(), nndr);
-	Parameters::parse(parameters, Parameters::kOdomType(), odomType);
+	int nn = Parameters::defaultOdomBowNNType();
+	float nndr = Parameters::defaultOdomBowNNDR();
+	int featureType = Parameters::defaultOdomFeatureType();
+	Parameters::parse(parameters, Parameters::kOdomBowNNType(), nn);
+	Parameters::parse(parameters, Parameters::kOdomBowNNDR(), nndr);
+	Parameters::parse(parameters, Parameters::kOdomFeatureType(), featureType);
 	customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(nn)));
 	customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
-	customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(odomType)));
+	customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(featureType)));
 
 	// add only feature stuff
 	for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
@@ -204,10 +212,10 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
 		{
 			Transform transform;
 			std::set<int> uniqueCorrespondences;
-			if(newSignature->getWords3().size() < (unsigned int)(getWordsRatio() * float(previousSignature->getWords3().size())))
+			if(newSignature->getWords3().size() < (unsigned int)(this->getFeaturesRatio() * float(previousSignature->getWords3().size())))
 			{
 				UWARN("At least %f%% keypoints of the last image required. New=%d last=%d",
-						getWordsRatio()*100.0f, newSignature->getWords3().size(), previousSignature->getWords3().size());
+						this->getFeaturesRatio()*100.0f, newSignature->getWords3().size(), previousSignature->getWords3().size());
 			}
 			else if(!localMap_.empty() && !newSignature->getWords3().empty())
 			{
@@ -232,13 +240,16 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
 					correspondences = inliers1->size();
 
 					// the transform returned is global odometry pose, not incremental one
+					std::vector<int> inliersV;
 					transform = util3d::transformFromXYZCorrespondences(
 							inliers2,
 							inliers1,
 							this->getInlierDistance(),
 							this->getIterations(),
-							&inliers);
+							this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
+							&inliersV);
 
+					inliers = inliersV.size();
 					if(!transform.isNull())
 					{
 						// make it incremental
@@ -308,7 +319,7 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
 				else
 				{
 					// remove words if history max size is reached
-					while(localMap_.size() && (int)localMap_.size() > this->getLocalHistoryMaxSize() && _memory->getStMem().size()>1)
+					while(localMap_.size() && (int)localMap_.size() > _localHistoryMaxSize && _memory->getStMem().size()>1)
 					{
 						int nodeId = *_memory->getStMem().begin();
 						std::list<int> removedPts;
@@ -319,10 +330,10 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
 						}
 					}
 
-					if(this->getLocalHistoryMaxSize() == 0 && localMap_.size() > 0 && localMap_.size() > newSignature->getWords3().size())
+					if(_localHistoryMaxSize == 0 && localMap_.size() > 0 && localMap_.size() > newSignature->getWords3().size())
 					{
 						UERROR("Local map should have only words of the last added signature here! (size=%d, max history size=%d, newWords=%d)",
-								(int)localMap_.size(), this->getLocalHistoryMaxSize(), (int)newSignature->getWords3().size());
+								(int)localMap_.size(), _localHistoryMaxSize, (int)newSignature->getWords3().size());
 					}
 
 					// update local map
@@ -402,6 +413,612 @@ Transform OdometryBOW::computeTransform(const SensorData & data, int * quality,
 	return output;
 }
 
+//OdometryOpticalFlow
+OdometryOpticalFlow::OdometryOpticalFlow(const ParametersMap & parameters) :
+	Odometry(parameters),
+	flowWinSize_(Parameters::defaultOdomFlowWinSize()),
+	flowIterations_(Parameters::defaultOdomFlowIterations()),
+	flowEps_(Parameters::defaultOdomFlowEps()),
+	flowMaxLevel_(Parameters::defaultOdomFlowMaxLevel()),
+	subPixWinSize_(Parameters::defaultOdomFlowSubPixWinSize()),
+	subPixIterations_(Parameters::defaultOdomFlowSubPixIterations()),
+	subPixEps_(Parameters::defaultOdomFlowSubPixEps()),
+	lastCorners3D_(new pcl::PointCloud<pcl::PointXYZ>)
+{
+	Parameters::parse(parameters, Parameters::kOdomFlowWinSize(), flowWinSize_);
+	Parameters::parse(parameters, Parameters::kOdomFlowIterations(), flowIterations_);
+	Parameters::parse(parameters, Parameters::kOdomFlowEps(), flowEps_);
+	Parameters::parse(parameters, Parameters::kOdomFlowMaxLevel(), flowMaxLevel_);
+	Parameters::parse(parameters, Parameters::kOdomFlowSubPixWinSize(), subPixWinSize_);
+	Parameters::parse(parameters, Parameters::kOdomFlowSubPixIterations(), subPixIterations_);
+	Parameters::parse(parameters, Parameters::kOdomFlowSubPixEps(), subPixEps_);
+
+	ParametersMap::const_iterator iter;
+	Feature2D::Type detectorStrategy = Feature2D::kFeatureUndef;
+	if((iter=parameters.find(Parameters::kOdomFeatureType())) != parameters.end())
+	{
+		detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
+	}
+	feature2D_ = Feature2D::create(detectorStrategy, parameters);
+}
+
+OdometryOpticalFlow::~OdometryOpticalFlow()
+{
+	delete feature2D_;
+}
+
+
+void OdometryOpticalFlow::reset()
+{
+	Odometry::reset();
+	lastFrame_ = cv::Mat();
+	lastCorners_.clear();
+	lastCorners3D_->clear();
+}
+
+// return not null transform if odometry is correctly computed
+Transform OdometryOpticalFlow::computeTransform(
+		const SensorData & data,
+		int * quality,
+		int * features,
+		int * localMapSize)
+{
+	UDEBUG("");
+
+	if(!data.rightImage().empty())
+	{
+		//stereo
+		return computeTransformStereo(data, quality, features);
+	}
+	else
+	{
+		//rgbd
+		return computeTransformRGBD(data, quality, features);
+	}
+}
+
+Transform OdometryOpticalFlow::computeTransformStereo(
+		const SensorData & data,
+		int * quality,
+		int * features)
+{
+	UTimer timer;
+	Transform output;
+
+	int inliers = 0;
+	int correspondences = 0;
+	imgMatches_ = cv::Mat();
+
+	cv::Mat newLeftFrame;
+	// convert to grayscale
+	if(data.image().channels() > 1)
+	{
+		cv::cvtColor(data.image(), newLeftFrame, cv::COLOR_BGR2GRAY);
+	}
+	else
+	{
+		newLeftFrame = data.image().clone();
+	}
+	cv::Mat newRightFrame = data.rightImage().clone();
+
+	std::vector<cv::Point2f> newCorners;
+	UDEBUG("lastCorners_.size()=%d lastFrame_=%d lastRightFrame_=%d", (int)lastCorners_.size(), lastFrame_.empty()?0:1, lastRightFrame_.empty()?0:1);
+	if(!lastFrame_.empty() && !lastRightFrame_.empty() && lastCorners_.size())
+	{
+		UDEBUG("");
+		// Find features in the new left image
+		std::vector<unsigned char> status;
+		std::vector<float> err;
+		UDEBUG("cv::calcOpticalFlowPyrLK() begin");
+		cv::calcOpticalFlowPyrLK(
+				lastFrame_,
+				newLeftFrame,
+				lastCorners_,
+				newCorners,
+				status,
+				err,
+				cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
+				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
+				cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
+		UDEBUG("cv::calcOpticalFlowPyrLK() end");
+
+		std::vector<cv::Point2f> lastCornersKept(status.size());
+		std::vector<cv::Point2f> newCornersKept(status.size());
+		int ki = 0;
+		for(unsigned int i=0; i<status.size(); ++i)
+		{
+			if(status[i])
+			{
+				lastCornersKept[ki] = lastCorners_[i];
+				newCornersKept[ki] = newCorners[i];
+				cv::Point2f pt = lastCorners_[i] - newCorners[i];
+				++ki;
+			}
+		}
+		lastCornersKept.resize(ki);
+		newCornersKept.resize(ki);
+
+		if(ki && ki >= this->getMinInliers())
+		{
+
+			std::vector<unsigned char> statusLast;
+			std::vector<float> errLast;
+			std::vector<cv::Point2f> lastCornersKeptRight;
+			cv::calcOpticalFlowPyrLK(
+						lastFrame_,
+						lastRightFrame_,
+						lastCornersKept,
+						lastCornersKeptRight,
+						statusLast,
+						errLast,
+						cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
+						cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
+						cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
+
+			UDEBUG("");
+			std::vector<cv::KeyPoint> lastKpts, newKpts;
+			/*cv::KeyPoint::convert(lastCornersKept, lastKpts);
+			cv::KeyPoint::convert(newCornersKept, newKpts);
+			std::vector<cv::DMatch> good_matches(lastKpts.size());
+			for(unsigned int i=0; i<good_matches.size(); ++i)
+			{
+				good_matches[i].trainIdx = i;
+				good_matches[i].queryIdx = i;
+			}
+
+			cv::drawMatches( lastFrame_, lastKpts, newLeftFrame, newKpts,
+						   good_matches, imgMatches_, cv::Scalar::all(-1), cv::Scalar::all(-1),
+						   std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
+			UDEBUG("");*/
+
+			std::vector<unsigned char> statusNew;
+			std::vector<float> errNew;
+			std::vector<cv::Point2f> newCornersKeptRight;
+			cv::calcOpticalFlowPyrLK(
+						newLeftFrame,
+						newRightFrame,
+						newCornersKept,
+						newCornersKeptRight,
+						statusNew,
+						errNew,
+						cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
+						cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
+						cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
+
+			UDEBUG("Getting correspondences begin");
+			// Get 3D correspondences
+			pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesLast(new pcl::PointCloud<pcl::PointXYZ>);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesNew(new pcl::PointCloud<pcl::PointXYZ>);
+			correspondencesLast->resize(statusLast.size());
+			correspondencesNew->resize(statusLast.size());
+			int oi = 0;
+			lastKpts.resize(statusLast.size());
+			newKpts.resize(statusLast.size());
+			for(unsigned int i=0; i<statusLast.size(); ++i)
+			{
+				if(statusLast[i] && statusNew[i])
+				{
+					float lastDisparity = lastCornersKept[i].x - lastCornersKeptRight[i].x;
+					float newDisparity = newCornersKept[i].x - newCornersKeptRight[i].x;
+					if(lastDisparity > 0.0f && newDisparity > 0.0f)
+					{
+						pcl::PointXYZ lastPt3D = util3d::projectDisparityTo3d(
+								lastCornersKept[i],
+								lastDisparity,
+								data.cx(), data.cy(), data.fx(), data.baseline());
+						pcl::PointXYZ newPt3D = util3d::projectDisparityTo3d(
+								newCornersKept[i],
+								newDisparity,
+								data.cx(), data.cy(), data.fx(), data.baseline());
+
+						if(pcl::isFinite(lastPt3D) && uIsInBounds(lastPt3D.z, 0.0f, this->getMaxDepth()) &&
+						   pcl::isFinite(newPt3D) && uIsInBounds(newPt3D.z, 0.0f, this->getMaxDepth()))
+						{
+							//Add 3D correspondences!
+							lastPt3D = util3d::transformPoint(lastPt3D, data.localTransform());
+							newPt3D = util3d::transformPoint(newPt3D, data.localTransform());
+							correspondencesLast->at(oi) = lastPt3D;
+							correspondencesNew->at(oi) = newPt3D;
+							lastKpts[oi].pt = lastCornersKept[i];
+							newKpts[oi].pt = newCornersKept[i];
+							++oi;
+						}
+					}
+				}
+			}// end loop
+			correspondencesLast->resize(oi);
+			correspondencesNew->resize(oi);
+			lastKpts.resize(oi);
+			newKpts.resize(oi);
+			correspondences = oi;
+			lastCorners3D_ = correspondencesNew;
+			UDEBUG("Getting correspondences end, kept %d/%d", correspondences, (int)statusLast.size());
+
+			/*good_matches.resize(lastKpts.size());
+			for(unsigned int i=0; i<good_matches.size(); ++i)
+			{
+				good_matches[i].trainIdx = i;
+				good_matches[i].queryIdx = i;
+			}
+
+			cv::Mat imgInliers;
+			cv::drawMatches( lastFrame_, lastKpts, newLeftFrame, newKpts,
+						   good_matches, imgInliers, cv::Scalar::all(-1), cv::Scalar::all(-1),
+						   std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
+			imgMatches_.push_back(imgInliers);
+			UDEBUG("");*/
+
+			if(correspondences >= this->getMinInliers())
+			{
+				std::vector<int> inliersV;
+				UTimer timerRANSAC;
+				output = util3d::transformFromXYZCorrespondences(
+						correspondencesNew,
+						correspondencesLast,
+						this->getInlierDistance(),
+						this->getIterations(),
+						this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
+						&inliersV);
+				UDEBUG("time RANSAC = %fs", timerRANSAC.ticks());
+
+				inliers = (int)inliersV.size();
+				if(quality)
+				{
+					*quality = inliers;
+				}
+
+				if(inliers < this->getMinInliers())
+				{
+					output.setNull();
+					UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
+				}
+
+				/*if(correspondencesLast->size() >= 6)
+				{
+					UWARN("saved pcd");
+					pcl::io::savePCDFile("last.pcd", *correspondencesLast);
+					pcl::io::savePCDFile("new.pcd", *correspondencesNew);
+					correspondencesNew = util3d::transformPointCloud(correspondencesNew, output);
+					pcl::io::savePCDFile("new2.pcd", *correspondencesNew);
+				}*/
+			}
+			else
+			{
+				UWARN("Not enough correspondences (%d)", correspondences);
+			}
+		}
+	}
+	else
+	{
+		//return Identity
+		output = Transform::getIdentity();
+	}
+
+	newCorners.clear();
+	if(!output.isNull())
+	{
+		// Update frame, reset saved last transform
+		savedLastRefFrameTransform_.setNull();
+
+		// Copy or generate new keypoints
+		if(data.keypoints().size())
+		{
+			newCorners.resize(data.keypoints().size());
+			for(unsigned int i=0; i<data.keypoints().size(); ++i)
+			{
+				newCorners[i] = data.keypoints().at(i).pt;
+			}
+		}
+		else
+		{
+			// generate kpts
+			std::vector<cv::KeyPoint> newKtps;
+			cv::Rect roi = Feature2D::computeRoi(newLeftFrame, this->getRoiRatios());
+			newKtps = feature2D_->generateKeypoints(newLeftFrame, this->getMaxFeatures(), roi);
+			Feature2D::limitKeypoints(newKtps, this->getMaxFeatures());
+
+			if(newKtps.size())
+			{
+				cv::KeyPoint::convert(newKtps, newCorners);
+
+				if(subPixWinSize_ > 0 && subPixIterations_ > 0)
+				{
+					UDEBUG("cv::cornerSubPix() begin");
+					cv::cornerSubPix(newLeftFrame, newCorners,
+						cv::Size( subPixWinSize_, subPixWinSize_ ),
+						cv::Size( -1, -1 ),
+						cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations_, subPixEps_ ) );
+					UDEBUG("cv::cornerSubPix() end");
+				}
+			}
+		}
+
+		if(lastCorners_.size() && newCorners.size() < (unsigned int)(this->getFeaturesRatio() * float(lastCorners_.size())))
+		{
+			UWARN("At least %f%% keypoints of the last image required. New=%d last=%d",
+					this->getFeaturesRatio()*100.0f, newCorners.size(), lastCorners_.size());
+		}
+		else if((int)newCorners.size() > this->getMinInliers())
+		{
+			lastFrame_ = newLeftFrame;
+			lastRightFrame_ = newRightFrame;
+			lastCorners_ = newCorners;
+		}
+		else
+		{
+			UWARN("Too low 2D corners (%d), ignoring new frame...",
+					(int)newCorners.size());
+		}
+	}
+	else if(!output.isNull())
+	{
+		output.setNull();
+	}
+
+	UINFO("Odom update time = %fs inliers=%d/%d, new corners=%d, transform accepted=%s",
+			timer.elapsed(),
+			inliers,
+			correspondences,
+			(int)newCorners.size(),
+			!output.isNull()?"true":"false");
+
+	return output;
+}
+
+Transform OdometryOpticalFlow::computeTransformRGBD(
+		const SensorData & data,
+		int * quality,
+		int * features)
+{
+	UTimer timer;
+	Transform output;
+
+	int inliers = 0;
+	int correspondences = 0;
+	imgMatches_ = cv::Mat();
+
+	cv::Mat newFrame;
+	// convert to grayscale
+	if(data.image().channels() > 1)
+	{
+		cv::cvtColor(data.image(), newFrame, cv::COLOR_BGR2GRAY);
+	}
+	else
+	{
+		newFrame = data.image().clone();
+	}
+
+	float updatePixels = 0.0f;
+	bool updateFrame = false;
+
+	std::vector<cv::Point2f> newCorners;
+	if(!lastFrame_.empty() && lastCorners_.size() && lastCorners3D_->size())
+	{
+		std::vector<unsigned char> status;
+		std::vector<float> err;
+		UDEBUG("cv::calcOpticalFlowPyrLK() begin");
+		cv::calcOpticalFlowPyrLK(
+				lastFrame_,
+				newFrame,
+				lastCorners_,
+				newCorners,
+				status,
+				err,
+				cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
+				cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
+				cv::OPTFLOW_LK_GET_MIN_EIGENVALS, 1e-4);
+		UDEBUG("cv::calcOpticalFlowPyrLK() end");
+
+		pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesLast(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::PointCloud<pcl::PointXYZ>::Ptr correspondencesNew(new pcl::PointCloud<pcl::PointXYZ>);
+		correspondencesLast->resize(lastCorners_.size());
+		correspondencesNew->resize(lastCorners_.size());
+		int oi=0;
+
+		std::vector<cv::KeyPoint> lastKpts(lastCorners_.size());
+		std::vector<cv::KeyPoint> newKpts(lastCorners_.size());
+
+		UASSERT(lastCorners_.size() == lastCorners3D_->size());
+		UDEBUG("lastCorners3D_ = %d", lastCorners3D_->size());
+		float sumSqrdDistance = 0.0f;
+		int flowInliers = 0;
+		for(unsigned int i=0; i<status.size(); ++i)
+		{
+			if(status[i] && pcl::isFinite(lastCorners3D_->at(i)) &&
+				uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols-1)) &&
+				uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows-1)))
+			{
+				pcl::PointXYZ pt = util3d::getDepth(data.depth(), newCorners[i].x, newCorners[i].y,
+						data.cx(), data.cy(), data.fx(), data.fy(), true);
+				if(pcl::isFinite(pt) &&
+					uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) &&
+					uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) &&
+					uIsInBounds(pt.z, 0.0f, this->getMaxDepth()))
+				{
+					pt = util3d::transformPoint(pt, data.localTransform());
+					correspondencesLast->at(oi) = lastCorners3D_->at(i);
+					correspondencesNew->at(oi) = pt;
+
+					cv::Point2f diff = newCorners[i]-lastCorners_[i];
+					sumSqrdDistance += diff.x*diff.x + diff.y*diff.y;
+
+					lastKpts[oi].pt = lastCorners_[i];
+					newKpts[oi].pt = newCorners[i];
+
+					++oi;
+				}
+				++flowInliers;
+			}
+			else if(status[i])
+			{
+				++flowInliers;
+			}
+		}
+		UDEBUG("Flow inliers = %d, added inliers=%d", flowInliers, oi);
+		float meanPixel = -1;
+		if(oi)
+		{
+			float meanPixel = sumSqrdDistance/(float)oi;
+			if(meanPixel >= updatePixels*updatePixels)
+			{
+				updateFrame = true;
+			}
+		}
+		UDEBUG("mean pixel distance = %f", meanPixel);
+
+		lastKpts.resize(oi);
+		newKpts.resize(oi);
+		correspondencesLast->resize(oi);
+		correspondencesNew->resize(oi);
+		correspondences = oi;
+		if(correspondences >= this->getMinInliers())
+		{
+			std::vector<int> inliersV;
+			UTimer timerRANSAC;
+			output = util3d::transformFromXYZCorrespondences(
+					correspondencesNew,
+					correspondencesLast,
+					this->getInlierDistance(),
+					this->getIterations(),
+					this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
+					&inliersV);
+			UDEBUG("time RANSAC = %fs", timerRANSAC.ticks());
+
+			inliers = (int)inliersV.size();
+			if(quality)
+			{
+				*quality = inliers;
+			}
+
+			if(inliers < this->getMinInliers())
+			{
+				output.setNull();
+				UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
+			}
+
+			/*std::vector<cv::DMatch> good_matches(lastKpts.size());
+			for(unsigned int i=0; i<good_matches.size(); ++i)
+			{
+				good_matches[i].trainIdx = i;
+				good_matches[i].queryIdx = i;
+			}
+
+			cv::drawMatches( lastFrame_, lastKpts, newFrame, newKpts,
+						   good_matches, imgMatches_, cv::Scalar::all(-1), cv::Scalar::all(-1),
+						   std::vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );*/
+		}
+		else
+		{
+			UWARN("Not enough correspondences (%d)", correspondences);
+		}
+	}
+	else
+	{
+		//return Identity
+		output = Transform::getIdentity();
+		updateFrame = true;
+	}
+
+	newCorners.clear();
+	if(!output.isNull() && updateFrame)
+	{
+		// Copy or generate new keypoints
+		if(data.keypoints().size())
+		{
+			newCorners.resize(data.keypoints().size());
+			for(unsigned int i=0; i<data.keypoints().size(); ++i)
+			{
+				newCorners[i] = data.keypoints().at(i).pt;
+			}
+		}
+		else
+		{
+			// generate kpts
+			std::vector<cv::KeyPoint> newKtps;
+			cv::Rect roi = Feature2D::computeRoi(newFrame, this->getRoiRatios());
+			newKtps = feature2D_->generateKeypoints(newFrame, this->getMaxFeatures(), roi);
+			Feature2D::filterKeypointsByDepth(newKtps, data.depth(), this->getMaxDepth());
+			Feature2D::limitKeypoints(newKtps, this->getMaxFeatures());
+
+			if(newKtps.size())
+			{
+				cv::KeyPoint::convert(newKtps, newCorners);
+
+				if(subPixWinSize_ > 0 && subPixIterations_ > 0)
+				{
+					cv::cornerSubPix(newFrame, newCorners,
+						cv::Size( subPixWinSize_, subPixWinSize_ ),
+						cv::Size( -1, -1 ),
+						cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, subPixIterations_, subPixEps_ ) );
+				}
+			}
+		}
+
+		if(lastCorners_.size() && newCorners.size() < (unsigned int)(this->getFeaturesRatio() * float(lastCorners_.size())))
+		{
+			UWARN("At least %f%% keypoints of the last image required. New=%d last=%d",
+					this->getFeaturesRatio()*100.0f, newCorners.size(), lastCorners_.size());
+		}
+		else if((int)newCorners.size() > this->getMinInliers())
+		{
+			// get 3D corners for the extracted 2D corners (not the ones refined by Optical Flow)
+			pcl::PointCloud<pcl::PointXYZ>::Ptr newCorners3D(new pcl::PointCloud<pcl::PointXYZ>);
+			newCorners3D->resize(newCorners.size());
+			std::vector<cv::Point2f> newCornersFiltered(newCorners.size());
+			int oi=0;
+			for(unsigned int i=0; i<newCorners.size(); ++i)
+			{
+				if(uIsInBounds(newCorners[i].x, 0.0f, float(data.depth().cols)-1.0f) &&
+				   uIsInBounds(newCorners[i].y, 0.0f, float(data.depth().rows)-1.0f))
+				{
+					pcl::PointXYZ pt = util3d::getDepth(data.depth(), newCorners[i].x, newCorners[i].y,
+							data.cx(), data.cy(), data.fx(), data.fy(), true);
+					if(pcl::isFinite(pt) &&
+						uIsInBounds(pt.x, -this->getMaxDepth(), this->getMaxDepth()) &&
+						uIsInBounds(pt.y, -this->getMaxDepth(), this->getMaxDepth()) &&
+						uIsInBounds(pt.z, 0.0f, this->getMaxDepth()))
+					{
+						pt = util3d::transformPoint(pt, data.localTransform());
+						newCorners3D->at(oi) = pt;
+						newCornersFiltered[oi] = newCorners[i];
+						++oi;
+					}
+				}
+			}
+			newCornersFiltered.resize(oi);
+			newCorners3D->resize(oi);
+			if((int)newCornersFiltered.size() > this->getMinInliers())
+			{
+				lastFrame_ = newFrame;
+				lastCorners_ = newCornersFiltered;
+				lastCorners3D_ = newCorners3D;
+			}
+			else
+			{
+				UWARN("Too low 3D corners (%d/%d, minCorners=%d), ignoring new frame...",
+						(int)newCornersFiltered.size(), (int)lastCorners3D_->size(), this->getMinInliers());
+			}
+		}
+		else
+		{
+			UWARN("Too low 2D corners (%d), ignoring new frame...",
+					(int)newCorners.size());
+		}
+	}
+	else if(!output.isNull())
+	{
+		output = Transform::getIdentity();
+	}
+
+	UINFO("Odom update time = %fs inliers=%d/%d, new corners=%d, transform accepted=%s",
+			timer.elapsed(),
+			inliers,
+			correspondences,
+			(int)newCorners.size(),
+			updateFrame||output.isNull()?"true":"false");
+	return output;
+}
+
 // OdometryICP
 OdometryICP::OdometryICP(int decimation,
 		float voxelSize,
@@ -444,10 +1061,10 @@ Transform OdometryICP::computeTransform(const SensorData & data, int * quality,
 	{
 		pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
 						data.depth(),
-						data.depthFx(),
-						data.depthFy(),
-						data.depthCx(),
-						data.depthCy(),
+						data.fx(),
+						data.fy(),
+						data.cx(),
+						data.cy(),
 						_decimation,
 						this->getMaxDepth(),
 						_voxelSize,
@@ -618,7 +1235,7 @@ void OdometryThread::mainLoop()
 
 void OdometryThread::addData(const SensorData & data)
 {
-	if(data.image().empty() || data.depth().empty() || data.depthFx() == 0.0f || data.depthFy() == 0.0f)
+	if(data.image().empty() || data.depth().empty() || data.fx() == 0.0f || data.fy() == 0.0f)
 	{
 		ULOGGER_ERROR("image empty !?");
 		return;

corelib/src/Rtabmap.cpp

@@ -1581,7 +1581,6 @@ bool Rtabmap::process(const SensorData & data)
 
 			if(_publishImage)
 			{
-				std::map<int, int> mapIds;
 				std::map<int, std::vector<unsigned char> > images;
 				std::map<int, std::vector<unsigned char> > depths;
 				std::map<int, std::vector<unsigned char> > depth2ds;
@@ -1629,7 +1628,6 @@ bool Rtabmap::process(const SensorData & data)
 						im = _memory->getImage(ids[i]);
 					}
 					UASSERT(_memory->getSignature(ids[i]) != 0);
-					mapIds.insert(std::make_pair(ids[i], _memory->getSignature(ids[i])->mapId()));
 					if(!im.empty())
 					{
 						images.insert(std::make_pair(ids[i], im));
@@ -1641,7 +1639,6 @@ bool Rtabmap::process(const SensorData & data)
 					UWARN("getting data[%d] time = %fs", (int)ids.size(), tmpTimer.ticks());
 				}
 
-				statistics_.setMapIds(mapIds);
 				statistics_.setImages(images);
 				statistics_.setDepths(depths);
 				statistics_.setDepth2ds(depth2ds);
@@ -1756,6 +1753,13 @@ bool Rtabmap::process(const SensorData & data)
 		//Poses, place this after Transfer! (_optimizedPoses may change)
 		if(_rgbdSlamMode)
 		{
+			std::map<int, int> mapIds;
+			for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
+			{
+				mapIds.insert(std::make_pair(iter->first, _memory->getMapId(iter->first)));
+			}
+
+			statistics_.setMapIds(mapIds);
 			statistics_.setPoses(_optimizedPoses);
 			statistics_.setConstraints(_constraints);
 			statistics_.setMapCorrection(_mapCorrection);
@@ -2345,6 +2349,10 @@ void Rtabmap::get3DMap(std::map<int, std::vector<unsigned char> > & images,
 			mapIds.insert(std::make_pair(*iter, _memory->getMapId(*iter)));
 		}
 	}
+	else if(_memory->getStMem().size() || _memory->getWorkingMem().size())
+	{
+		UERROR("Last working signature is null!?");
+	}
 }
 
 void Rtabmap::getGraph(
@@ -2378,6 +2386,10 @@ void Rtabmap::getGraph(
 			mapIds.insert(std::make_pair(*iter, _memory->getMapId(*iter)));
 		}
 	}
+	else if(_memory->getStMem().size() || _memory->getWorkingMem().size())
+	{
+		UERROR("Last working signature is null!?");
+	}
 }
 
 void Rtabmap::readParameters(const std::string & configFile, ParametersMap & parameters)

corelib/src/SensorData.cpp

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 
 #include "rtabmap/core/SensorData.h"
+#include "rtabmap/utilite/ULogger.h"
 
 namespace rtabmap
 {
@@ -38,7 +39,7 @@ SensorData::SensorData() :
 	_image(cv::Mat()),
 	_id(0),
 	_fx(0.0f),
-	_fy(0.0f),
+	_fyOrBaseline(0.0f),
 	_cx(0.0f),
 	_cy(0.0f),
 	_localTransform(Transform::getIdentity())
@@ -50,18 +51,20 @@ SensorData::SensorData(const cv::Mat & image,
 	_image(image),
 	_id(id),
 	_fx(0.0f),
-	_fy(0.0f),
+	_fyOrBaseline(0.0f),
 	_cx(0.0f),
 	_cy(0.0f),
 	_localTransform(Transform::getIdentity())
 {
+	UASSERT(image.type() == CV_8UC1 || // Mono
+			image.type() == CV_8UC3);  // RGB
 }
 
 	// Metric constructor
 SensorData::SensorData(const cv::Mat & image,
-		  const cv::Mat & depth,
+		  const cv::Mat & depthOrRightImage,
 		  float fx,
-		  float fy,
+		  float fyOrBaseline,
 		  float cx,
 		  float cy,
 		  const Transform & pose,
@@ -69,22 +72,29 @@ SensorData::SensorData(const cv::Mat & image,
 		  int id) :
 	_image(image),
 	_id(id),
-	_depth(depth),
+	_depthOrRightImage(depthOrRightImage),
 	_fx(fx),
-	_fy(fy),
+	_fyOrBaseline(fyOrBaseline),
 	_cx(cx),
 	_cy(cy),
 	_pose(pose),
 	_localTransform(localTransform)
 {
+	UASSERT(image.type() == CV_8UC1 || // Mono
+			image.type() == CV_8UC3);  // RGB
+	UASSERT(depthOrRightImage.type() == CV_32FC1 || // Depth in meter
+			depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre
+			depthOrRightImage.type() == CV_8U);     // Right stereo image
+	UASSERT(!depthOrRightImage.empty() && _fx>0.0f && _fyOrBaseline>0.0f && _cx>=0.0f && _cy>=0.0f);
+	UASSERT(!_localTransform.isNull());
 }
 
 	// Metric constructor + 2d depth
 SensorData::SensorData(const cv::Mat & image,
-		  const cv::Mat & depth,
+		  const cv::Mat & depthOrRightImage,
 		  const cv::Mat & depth2d,
 		  float fx,
-		  float fy,
+		  float fyOrBaseline,
 		  float cx,
 		  float cy,
 		  const Transform & pose,
@@ -92,15 +102,22 @@ SensorData::SensorData(const cv::Mat & image,
 		  int id) :
 	_image(image),
 	_id(id),
-	_depth(depth),
+	_depthOrRightImage(depthOrRightImage),
 	_depth2d(depth2d),
 	_fx(fx),
-	_fy(fy),
+	_fyOrBaseline(fyOrBaseline),
 	_cx(cx),
 	_cy(cy),
 	_pose(pose),
 	_localTransform(localTransform)
 {
+	UASSERT(image.type() == CV_8UC1 || // Mono
+			image.type() == CV_8UC3);  // RGB
+	UASSERT(depthOrRightImage.type() == CV_32FC1 || // Depth in meter
+			depthOrRightImage.type() == CV_16UC1 || // Depth in millimetre
+			depthOrRightImage.type() == CV_8U);     // Right stereo image
+	UASSERT(!depthOrRightImage.empty() && _fx>0.0f && _fyOrBaseline>0.0f && _cx>=0.0f && _cy>=0.0f);
+	UASSERT(!_localTransform.isNull());
 }
 
 bool SensorData::empty() const

corelib/src/util3d.cpp

@@ -443,13 +443,18 @@ pcl::PointXYZ getDepth(
 		bool smoothing,
 		float maxZError)
 {
+	UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
+
 	pcl::PointXYZ pt;
 	float bad_point = std::numeric_limits<float>::quiet_NaN ();
 
-	if(!(int(x) >=0 && int(x)<depthImage.cols && int(y) >=0 && int(y)<depthImage.rows))
+	int u = int(x+0.5f);
+	int v = int(y+0.5f);
+
+	if(!(u >=0 && u<depthImage.cols && v >=0 && v<depthImage.rows))
 	{
-		UERROR("!(x >=0 && x<depthImage.cols && y >=0 && y<depthImage.rows) cond failed! returning bad point. (x=%f, y=%f, cols=%d, rows=%d)",
-				x,y,depthImage.cols, depthImage.rows);
+		UERROR("!(x >=0 && x<depthImage.cols && y >=0 && y<depthImage.rows) cond failed! returning bad point. (x=%f (u=%d), y=%f (v=%d), cols=%d, rows=%d)",
+				x,u,y,v,depthImage.cols, depthImage.rows);
 		pt.x = pt.y = pt.z = bad_point;
 		return pt;
 	}
@@ -462,8 +467,6 @@ pcl::PointXYZ getDepth(
 	//  | 1 | 2 | 1 |
 	//  | 2 | 4 | 2 |
 	//  | 1 | 2 | 1 |
-	int u = int(x+0.5f);
-	int v = int(y+0.5f);
 	int u_start = std::max(u-1, 0);
 	int v_start = std::max(v-1, 0);
 	int u_end = std::min(u+1, depthImage.cols-1);
@@ -721,6 +724,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 		float fx, float fy,
 		int decimation)
 {
+	UASSERT(imageRgb.rows == imageDepth.rows && imageRgb.cols == imageDepth.cols);
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 	if(decimation < 1)
 	{
@@ -775,6 +779,129 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 	return cloud;
 }
 
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
+		const cv::Mat & imageRgb,
+		const cv::Mat & imageDisparity,
+		float cx, float cy,
+		float fx, float baseline,
+		int decimation)
+{
+	UASSERT(imageRgb.rows == imageDisparity.rows &&
+			imageRgb.cols == imageDisparity.cols &&
+			(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1));
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+	if(decimation < 1)
+	{
+		return cloud;
+	}
+
+	bool mono;
+	if(imageRgb.channels() == 3) // BGR
+	{
+		mono = false;
+	}
+	else if(imageRgb.channels() == 1) // Mono
+	{
+		mono = true;
+	}
+	else
+	{
+		return cloud;
+	}
+
+	//cloud.header = cameraInfo.header;
+	cloud->height = imageRgb.rows/decimation;
+	cloud->width  = imageRgb.cols/decimation;
+	cloud->is_dense = false;
+	cloud->resize(cloud->height * cloud->width);
+
+	for(int h = 0; h < imageRgb.rows && h/decimation < (int)cloud->height; h+=decimation)
+	{
+		for(int w = 0; w < imageRgb.cols && w/decimation < (int)cloud->width; w+=decimation)
+		{
+			pcl::PointXYZRGB & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));
+			if(!mono)
+			{
+				pt.b = imageRgb.at<cv::Vec3b>(h,w)[0];
+				pt.g = imageRgb.at<cv::Vec3b>(h,w)[1];
+				pt.r = imageRgb.at<cv::Vec3b>(h,w)[2];
+			}
+			else
+			{
+				unsigned char v = imageRgb.at<unsigned char>(h,w);
+				pt.b = v;
+				pt.g = v;
+				pt.r = v;
+			}
+
+			float disp = imageDisparity.type()==CV_16SC1?float(imageDisparity.at<short>(h,w))/16.0f:imageDisparity.at<float>(h,w);
+			pcl::PointXYZ ptXYZ = projectDisparityTo3d(cv::Point2f(w, h), disp, cx, cy, fx, baseline);
+			pt.x = ptXYZ.x;
+			pt.y = ptXYZ.y;
+			pt.z = ptXYZ.z;
+		}
+	}
+	return cloud;
+}
+
+cv::Mat disparityFromStereoImages(const cv::Mat & leftImage, const cv::Mat & rightImage)
+{
+	UASSERT(!leftImage.empty() && !rightImage.empty() &&
+			leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
+			leftImage.cols == rightImage.cols &&
+			leftImage.rows == rightImage.rows);
+	cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET, 160, 15);
+	cv::Mat disparity;
+	stereo(leftImage, rightImage, disparity, CV_16S);
+	cv::filterSpeckles(disparity, 0, 1000, 16);
+	return disparity;
+}
+
+// inspired from ROS image_geometry/src/stereo_camera_model.cpp
+pcl::PointXYZ projectDisparityTo3d(
+		const cv::Point2f & pt,
+		float disparity,
+		float cx, float cy, float fx, float baseline)
+{
+	if(disparity > 0.0f && baseline > 0.0f && fx > 0.0f)
+	{
+		float W = disparity/baseline;// + (right_.cx() - left_.cx()) / Tx;
+		return pcl::PointXYZ((pt.x - cx)/W, (pt.y - cy)/W, fx/W);
+	}
+	float bad_point = std::numeric_limits<float>::quiet_NaN ();
+	return pcl::PointXYZ(bad_point, bad_point, bad_point);
+}
+
+cv::Mat depthFromDisparity(const cv::Mat & disparity,
+		float cx, float cy, float fx, float baseline,
+		int type)
+{
+	UASSERT(disparity.type() == CV_32FC1 || disparity.type() == CV_16S);
+	UASSERT(type == CV_32FC1 || type == CV_16U);
+	cv::Mat depth = cv::Mat::zeros(disparity.rows, disparity.cols, type);
+	for (int i = 0; i < disparity.rows; i++)
+	{
+		for (int j = 0; j < disparity.cols; j++)
+		{
+			float disparity_value = disparity.type() == CV_16S?float(disparity.at<short>(i,j))/16.0f:disparity.at<float>(i,j);
+			if (disparity_value > 0.0f)
+			{
+				// baseline * focal / disparity
+				float d = baseline * fx / disparity_value;
+				if(depth.type() == CV_32FC1)
+				{
+					depth.at<float>(i,j) = d;
+				}
+				else
+				{
+					depth.at<unsigned short>(i,j) = (unsigned short)(d*1000.0f);
+				}
+			}
+		}
+	}
+	return depth;
+}
+
 cv::Mat depth2DFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
 {
 	cv::Mat depth2d(1, (int)cloud.size(), CV_32FC2);
@@ -1131,59 +1258,168 @@ Transform transformFromXYZCorrespondences(
 		const pcl::PointCloud<pcl::PointXYZ>::ConstPtr & cloud2,
 		double inlierThreshold,
 		int iterations,
-		int * inliers)
+		bool refineModel,
+		double refineModelSigma,
+		int refineModelIterations,
+		std::vector<int> * inliersOut)
 {
+	//NOTE: this method is a mix of two methods:
+	//  - getRemainingCorrespondences() in pcl/registration/impl/correspondence_rejection_sample_consensus.hpp
+	//  - refineModel() in pcl/sample_consensus/sac.h
+
 	Transform transform;
-	if(cloud1->size() && cloud1->size() == cloud2->size())
+	if(cloud1->size() >=3 && cloud1->size() == cloud2->size())
 	{
-		// Robust to outliers RANSAC
-		pcl::CorrespondencesPtr correspondences(new pcl::Correspondences);
-		for(unsigned int i = 0; i<cloud1->size(); ++i)
+		// RANSAC
+		UDEBUG("iterations=%d inlierThreshold=%f", iterations, inlierThreshold);
+		std::vector<int> source_indices (cloud2->size());
+		std::vector<int> target_indices (cloud1->size());
+
+		// Copy the query-match indices
+		for (size_t i = 0; i < cloud1->size(); ++i)
 		{
-			correspondences->push_back(pcl::Correspondence(i, i, pcl::euclideanDistance(cloud2->at(i), cloud1->at(i))));
+			source_indices[i] = i;
+			target_indices[i] = i;
 		}
 
-		pcl::registration::CorrespondenceRejectorSampleConsensus<pcl::PointXYZ> crsc;
-		crsc.setInputCorrespondences(correspondences);
-		crsc.setInputSource(cloud2);
-		crsc.setInputTarget(cloud1);
-		crsc.setMaximumIterations(iterations);
-		crsc.setInlierThreshold(inlierThreshold);
-		crsc.setRefineModel(true);
-		pcl::Correspondences correspondencesInliers;
-		crsc.getCorrespondences(correspondencesInliers);
-		UDEBUG("RANSAC inliers=%d outliers=%d", (int)correspondencesInliers.size(), (int)correspondences->size()-(int)correspondencesInliers.size());
-		transform = util3d::transformFromEigen4f(crsc.getBestTransformation());
+		// From the set of correspondences found, attempt to remove outliers
+		// Create the registration model
+		pcl::SampleConsensusModelRegistration<pcl::PointXYZ>::Ptr model;
+		model.reset(new pcl::SampleConsensusModelRegistration<pcl::PointXYZ>(cloud2, source_indices));
+		// Pass the target_indices
+		model->setInputTarget (cloud1, target_indices);
+		// Create a RANSAC model
+		pcl::RandomSampleConsensus<pcl::PointXYZ> sac (model, inlierThreshold);
+		sac.setMaxIterations(iterations);
 
-		/*UDEBUG("RANSAC=%s", transform.prettyPrint().c_str());
-
-		pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> trans_est;
-		Eigen::Matrix4f transform_svd;
-		trans_est.estimateRigidTransformation (*cloud2, *cloud1, correspondencesInliers, transform_svd);
-		transform = util3d::transformFromEigen4f(transform_svd);
-		UDEBUG("SVD=%s", transform.prettyPrint().c_str());*/
-
-		if(correspondencesInliers.size() == correspondences->size() && transform.isIdentity())
+		// Compute the set of inliers
+		if(sac.computeModel())
 		{
-			//Wrong transform
-			UDEBUG("Wrong transform: identity with full inliers");
-			transform.setNull();
-		}
+			std::vector<int> inliers;
+			Eigen::VectorXf model_coefficients;
 
-		if(inliers)
+			sac.getInliers(inliers);
+			sac.getModelCoefficients (model_coefficients);
+
+			if (refineModel)
+			{
+				double inlier_distance_threshold_sqr = inlierThreshold * inlierThreshold;
+				double error_threshold = inlierThreshold;
+				double sigma_sqr = refineModelSigma * refineModelSigma;
+				int refine_iterations = 0;
+				bool inlier_changed = false, oscillating = false;
+				std::vector<int> new_inliers, prev_inliers = inliers;
+				std::vector<size_t> inliers_sizes;
+				Eigen::VectorXf new_model_coefficients = model_coefficients;
+				do
+				{
+					// Optimize the model coefficients
+					model->optimizeModelCoefficients (prev_inliers, new_model_coefficients, new_model_coefficients);
+					inliers_sizes.push_back (prev_inliers.size ());
+
+					// Select the new inliers based on the optimized coefficients and new threshold
+					model->selectWithinDistance (new_model_coefficients, error_threshold, new_inliers);
+					UDEBUG("RANSAC refineModel: Number of inliers found (before/after): %zu/%zu, with an error threshold of %g.",
+							prev_inliers.size (), new_inliers.size (), error_threshold);
+
+					if (new_inliers.empty ())
+					{
+						++refine_iterations;
+						if (refine_iterations >= refineModelIterations)
+						{
+							break;
+						}
+						continue;
+					}
+
+					// Estimate the variance and the new threshold
+					double variance = model->computeVariance ();
+					error_threshold = sqrt (std::min (inlier_distance_threshold_sqr, sigma_sqr * variance));
+
+					UDEBUG ("RANSAC refineModel: New estimated error threshold: %g on iteration %d out of %d.",
+						  error_threshold, refine_iterations, refineModelIterations);
+					inlier_changed = false;
+					std::swap (prev_inliers, new_inliers);
+
+					// If the number of inliers changed, then we are still optimizing
+					if (new_inliers.size () != prev_inliers.size ())
+					{
+						// Check if the number of inliers is oscillating in between two values
+						if (inliers_sizes.size () >= 4)
+						{
+							if (inliers_sizes[inliers_sizes.size () - 1] == inliers_sizes[inliers_sizes.size () - 3] &&
+							inliers_sizes[inliers_sizes.size () - 2] == inliers_sizes[inliers_sizes.size () - 4])
+							{
+								oscillating = true;
+								break;
+							}
+						}
+						inlier_changed = true;
+						continue;
+					}
+
+					// Check the values of the inlier set
+					for (size_t i = 0; i < prev_inliers.size (); ++i)
+					{
+						// If the value of the inliers changed, then we are still optimizing
+						if (prev_inliers[i] != new_inliers[i])
+						{
+							inlier_changed = true;
+							break;
+						}
+					}
+				}
+				while (inlier_changed && ++refine_iterations < refineModelIterations);
+
+				// If the new set of inliers is empty, we didn't do a good job refining
+				if (new_inliers.empty ())
+				{
+					UWARN ("RANSAC refineModel: Refinement failed: got an empty set of inliers!");
+				}
+
+				if (oscillating)
+				{
+					UDEBUG("RANSAC refineModel: Detected oscillations in the model refinement.");
+				}
+
+				std::swap (inliers, new_inliers);
+				model_coefficients = new_model_coefficients;
+			}
+
+			if (inliers.size() >= 3)
+			{
+				if(inliersOut)
+				{
+					*inliersOut = inliers;
+				}
+
+				// get best transformation
+				Eigen::Matrix4f bestTransformation;
+				bestTransformation.row (0) = model_coefficients.segment<4>(0);
+				bestTransformation.row (1) = model_coefficients.segment<4>(4);
+				bestTransformation.row (2) = model_coefficients.segment<4>(8);
+				bestTransformation.row (3) = model_coefficients.segment<4>(12);
+
+				transform = util3d::transformFromEigen4f(bestTransformation);
+				UDEBUG("RANSAC inliers=%zu/%zu tf=%s", inliers.size(), cloud1->size(), transform.prettyPrint().c_str());
+
+				return transform.inverse(); // inverse to get actual pose transform (not correspondences transform)
+			}
+			else
+			{
+				UDEBUG("RANSAC: Model with inliers < 3");
+			}
+		}
+		else
 		{
-			*inliers = (int)correspondencesInliers.size();
+			UDEBUG("RANSAC: Failed to find model");
 		}
-
-		//std::cout << "transformMatrix: " << transformMatrix << std::endl;
-
-		//std::cout << "quality: " << float(correspondencesRej.size()) / float(correspondences->size());
 	}
 	else
 	{
-		UDEBUG("not enough points to compute the transform");
+		UDEBUG("Not enough points to compute the transform");
 	}
-	return transform.inverse(); // inverse to get actual pose transform (not correspondences transform)
+	return Transform();
 }
 
 // return transform from source to target (All points must be finite!!!)