Stereo! The memory can now handle directly stereo images. Disparity can be computed on the fly by keeping left and right images, so for features extraction (and for re-extraction on loop closure), we can compute 3D points precisely. New parameters can be found under "RGB-D Mapping->Stereo". Full image disparity is reconstructed in the GUI (not the core).

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1861 f169173b-cf89-36c8-b27e-44dbe73f0c83
2026-09-01 17:10:26 +08:00 · 2014-10-16 00:14:23 +00:00
parent 8f451029e2
commit fcd3301665
18 changed files with 1230 additions and 260 deletions
--- a/corelib/src/Features2d.cpp
+++ b/corelib/src/Features2d.cpp
@@ -114,6 +114,73 @@ void Feature2D::filterKeypointsByDepth(
 	}
 }

+void Feature2D::filterKeypointsByDisparity(
+		std::vector<cv::KeyPoint> & keypoints,
+		const cv::Mat & disparity,
+		float minDisparity)
+{
+	cv::Mat descriptors;
+	filterKeypointsByDisparity(keypoints, descriptors, disparity, minDisparity);
+}
+
+void Feature2D::filterKeypointsByDisparity(
+		std::vector<cv::KeyPoint> & keypoints,
+		cv::Mat & descriptors,
+		const cv::Mat & disparity,
+		float minDisparity)
+{
+	if(!disparity.empty() && minDisparity > 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;
+		for(unsigned int i=0; i<keypoints.size(); ++i)
+		{
+			int u = int(keypoints[i].pt.x+0.5f);
+			int v = int(keypoints[i].pt.y+0.5f);
+			if(u >=0 && u<disparity.cols && v >=0 && v<disparity.rows)
+			{
+				float d = disparity.type() == CV_16SC1?float(disparity.at<short>(v,u))/16.0f:disparity.at<float>(v,u);
+				if(d!=0.0f && uIsFinite(d) && d >= minDisparity)
+				{
+					output[oi++] = keypoints[i];
+					indexes[i] = 1;
+				}
+			}
+		}
+		output.resize(oi);
+		keypoints = output;
+
+		if(!descriptors.empty() && (int)keypoints.size() != descriptors.rows)
+		{
+			if(keypoints.size() == 0)
+			{
+				descriptors = cv::Mat();
+			}
+			else
+			{
+				cv::Mat newDescriptors(keypoints.size(), descriptors.cols, descriptors.type());
+				int di = 0;
+				for(unsigned int i=0; i<indexes.size(); ++i)
+				{
+					if(indexes[i] == 1)
+					{
+						if(descriptors.type() == CV_32FC1)
+						{
+							memcpy(newDescriptors.ptr<float>(di++), descriptors.ptr<float>(i), descriptors.cols*sizeof(float));
+						}
+						else // CV_8UC1
+						{
+							memcpy(newDescriptors.ptr<char>(di++), descriptors.ptr<char>(i), descriptors.cols*sizeof(char));
+						}
+					}
+				}
+				descriptors = newDescriptors;
+			}
+		}
+	}
+}
+
 void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints)
 {
 	cv::Mat descriptors;
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -98,7 +98,16 @@ Memory::Memory(const ParametersMap & parameters) :
 	_icp2MaxIterations(Parameters::defaultLccIcp2Iterations()),
 	_icp2MaxFitness(Parameters::defaultLccIcp2MaxFitness()),
 	_icp2CorrespondenceRatio(Parameters::defaultLccIcp2CorrespondenceRatio()),
-	_icp2VoxelSize(Parameters::defaultLccIcp2VoxelSize())
+	_icp2VoxelSize(Parameters::defaultLccIcp2VoxelSize()),
+
+	_stereoFlowWinSize(Parameters::defaultStereoWinSize()),
+	_stereoFlowIterations(Parameters::defaultStereoIterations()),
+	_stereoFlowEpsilon(Parameters::defaultStereoEps()),
+	_stereoFlowMaxLevel(Parameters::defaultStereoMaxLevel()),
+
+	_stereoSubPixWinSize(Parameters::defaultStereoSubPixWinSize()),
+	_stereoSubPixIterations(Parameters::defaultStereoSubPixIterations()),
+	_stereoSubPixEps(Parameters::defaultStereoSubPixEps())
 {
 	_vwd = new VWDictionary(parameters);
 	this->parseParameters(parameters);
@@ -383,6 +392,15 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kLccIcp2CorrespondenceRatio(), _icp2CorrespondenceRatio);
 	Parameters::parse(parameters, Parameters::kLccIcp2VoxelSize(), _icp2VoxelSize);

+	//stereo
+	Parameters::parse(parameters, Parameters::kStereoWinSize(), _stereoFlowWinSize);
+	Parameters::parse(parameters, Parameters::kStereoIterations(), _stereoFlowIterations);
+	Parameters::parse(parameters, Parameters::kStereoEps(), _stereoFlowEpsilon);
+	Parameters::parse(parameters, Parameters::kStereoMaxLevel(), _stereoFlowMaxLevel);
+	Parameters::parse(parameters, Parameters::kStereoSubPixWinSize(), _stereoSubPixWinSize);
+	Parameters::parse(parameters, Parameters::kStereoSubPixIterations(), _stereoSubPixIterations);
+	Parameters::parse(parameters, Parameters::kStereoSubPixEps(), _stereoSubPixEps);
+
 	UASSERT_MSG(_bowMinInliers >= 1, uFormat("value=%d", _bowMinInliers).c_str());
 	UASSERT_MSG(_bowInlierDistance > 0.0f, uFormat("value=%f", _bowInlierDistance).c_str());
 	UASSERT_MSG(_bowIterations > 0, uFormat("value=%d", _bowIterations).c_str());
@@ -498,7 +516,10 @@ bool Memory::update(const SensorData & data, Statistics * stats)
 	//============================================================
 	if(_incrementalMemory)
 	{
-		this->rehearsal(signature, stats);
+		if(_similarityThreshold < 1.0f)
+		{
+			this->rehearsal(signature, stats);
+		}
 		t=timer.ticks()*1000;
 		if(stats) stats->addStatistic(Statistics::kTimingMemRehearsal(), t);
 		UDEBUG("time rehearsal=%f ms", t);
@@ -1848,72 +1869,79 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
 		cv::Mat newDepth = ctNew.getUncompressedData();
 		if(!oldDepth.empty() && !newDepth.empty())
 		{
-			pcl::PointCloud<pcl::PointXYZ>::Ptr oldCloudXYZ = util3d::getICPReadyCloud(
-					oldDepth,
-					oldS.getDepthFx(),
-					oldS.getDepthFy(),
-					oldS.getDepthCx(),
-					oldS.getDepthCy(),
-					_icpDecimation,
-					_icpMaxDepth,
-					_icpVoxelSize,
-					_icpSamples,
-					oldS.getLocalTransform());
-			pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
-					newDepth,
-					newS.getDepthFx(),
-					newS.getDepthFy(),
-					newS.getDepthCx(),
-					newS.getDepthCy(),
-					_icpDecimation,
-					_icpMaxDepth,
-					_icpVoxelSize,
-					_icpSamples,
-					guess * newS.getLocalTransform());
-
-			pcl::PointCloud<pcl::PointNormal>::Ptr oldCloud = util3d::computeNormals(oldCloudXYZ);
-			pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ);
-
-			std::vector<int> indices;
-			newCloud = util3d::removeNaNNormalsFromPointCloud(newCloud);
-			oldCloud = util3d::removeNaNNormalsFromPointCloud(oldCloud);
-
-			// 3D
-			double fitness = 0;
-			bool hasConverged = false;
-			Transform icpT;
-			if(newCloud->size() && oldCloud->size())
+			if(oldDepth.type() == CV_8UC1 || newDepth.type() == CV_8UC1)
 			{
-				icpT = util3d::icpPointToPlane(newCloud,
-						oldCloud,
-					   _icpMaxCorrespondenceDistance,
-					   _icpMaxIterations,
-					   hasConverged,
-					   fitness);
-
-				//pcl::io::savePCDFile("old.pcd", *oldCloudXYZ);
-				//pcl::io::savePCDFile("newguess.pcd", *newCloudXYZ);
-				//newCloudXYZ = util3d::transformPointCloud(newCloudXYZ, icpT);
-				//pcl::io::savePCDFile("newicp.pcd", *newCloudXYZ);
-
-				UDEBUG("fitness=%f", fitness);
-
-				if(!icpT.isNull() && hasConverged && (_icpMaxFitness == 0 || fitness < _icpMaxFitness))
-				{
-					transform = icpT * guess;
-					transform = transform.inverse();
-				}
-				else
-				{
-					msg = uFormat("Cannot compute transform (hasConverged=%s fitness=%f/%f)",
-							hasConverged?"true":"false", fitness, _icpMaxFitness);
-					UINFO(msg.c_str());
-				}
+				UERROR("ICP 3D cannot be done on stereo images!");
 			}
 			else
 			{
-				msg = "Clouds empty ?!?";
-				UWARN(msg.c_str());
+				pcl::PointCloud<pcl::PointXYZ>::Ptr oldCloudXYZ = util3d::getICPReadyCloud(
+						oldDepth,
+						oldS.getDepthFx(),
+						oldS.getDepthFy(),
+						oldS.getDepthCx(),
+						oldS.getDepthCy(),
+						_icpDecimation,
+						_icpMaxDepth,
+						_icpVoxelSize,
+						_icpSamples,
+						oldS.getLocalTransform());
+				pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
+						newDepth,
+						newS.getDepthFx(),
+						newS.getDepthFy(),
+						newS.getDepthCx(),
+						newS.getDepthCy(),
+						_icpDecimation,
+						_icpMaxDepth,
+						_icpVoxelSize,
+						_icpSamples,
+						guess * newS.getLocalTransform());
+
+				pcl::PointCloud<pcl::PointNormal>::Ptr oldCloud = util3d::computeNormals(oldCloudXYZ);
+				pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ);
+
+				std::vector<int> indices;
+				newCloud = util3d::removeNaNNormalsFromPointCloud(newCloud);
+				oldCloud = util3d::removeNaNNormalsFromPointCloud(oldCloud);
+
+				// 3D
+				double fitness = 0;
+				bool hasConverged = false;
+				Transform icpT;
+				if(newCloud->size() && oldCloud->size())
+				{
+					icpT = util3d::icpPointToPlane(newCloud,
+							oldCloud,
+						   _icpMaxCorrespondenceDistance,
+						   _icpMaxIterations,
+						   hasConverged,
+						   fitness);
+
+					//pcl::io::savePCDFile("old.pcd", *oldCloudXYZ);
+					//pcl::io::savePCDFile("newguess.pcd", *newCloudXYZ);
+					//newCloudXYZ = util3d::transformPointCloud(newCloudXYZ, icpT);
+					//pcl::io::savePCDFile("newicp.pcd", *newCloudXYZ);
+
+					UDEBUG("fitness=%f", fitness);
+
+					if(!icpT.isNull() && hasConverged && (_icpMaxFitness == 0 || fitness < _icpMaxFitness))
+					{
+						transform = icpT * guess;
+						transform = transform.inverse();
+					}
+					else
+					{
+						msg = uFormat("Cannot compute transform (hasConverged=%s fitness=%f/%f)",
+								hasConverged?"true":"false", fitness, _icpMaxFitness);
+						UINFO(msg.c_str());
+					}
+				}
+				else
+				{
+					msg = "Clouds empty ?!?";
+					UWARN(msg.c_str());
+				}
 			}
 		}
 		else
@@ -3011,49 +3039,6 @@ void Memory::copyData(const Signature * from, Signature * to)
 	UDEBUG("Merging time = %fs", timer.ticks());
 }

-void Memory::extractKeypointsAndDescriptors(
-		const cv::Mat & image,
-		std::vector<cv::KeyPoint> & keypoints,
-		cv::Mat & descriptors) const
-{
-	extractKeypointsAndDescriptors(image, cv::Mat(), keypoints, descriptors);
-}
-
-void Memory::extractKeypointsAndDescriptors(
-		const cv::Mat & image,
-		const cv::Mat & depth,
-		std::vector<cv::KeyPoint> & keypoints,
-		cv::Mat & descriptors) const
-{
-	if(_wordsPerImageTarget >= 0)
-	{
-		UTimer timer;
-		if(_feature2D)
-		{
-			cv::Rect roi = Feature2D::computeRoi(image, _roiRatios);
-			keypoints = _feature2D->generateKeypoints(image, 0, roi);
-			UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), timer.ticks());
-
-			Feature2D::filterKeypointsByDepth(keypoints, depth, _wordsMaxDepth);
-			Feature2D::limitKeypoints(keypoints, _wordsPerImageTarget);
-		}
-		else
-		{
-			UWARN("feature2D not set!");
-		}
-
-		if(keypoints.size())
-		{
-			descriptors = _feature2D->generateDescriptors(image, keypoints);
-			UDEBUG("time descriptors (%d) = %fs", descriptors.rows, timer.ticks());
-		}
-	}
-	else
-	{
-		UDEBUG("_wordsPerImageTarget(%d)<0 so don't extract any descriptors...", _wordsPerImageTarget);
-	}
-}
-
 class PreUpdateThread : public UThreadNode
 {
 public:
@@ -3076,6 +3061,7 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 	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);
+	UASSERT(_feature2D != 0);

 	PreUpdateThread preUpdateThread(_vwd);

@@ -3126,29 +3112,125 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 		preUpdateThread.start();
 	}

+	pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3D(new pcl::PointCloud<pcl::PointXYZ>);
 	if(data.keypoints().size() == 0)
 	{
-		// Extract features
-		cv::Mat imageMono;
-		// convert to grayscale
-		if(data.image().channels() > 1)
+		if(_wordsPerImageTarget >= 0)
 		{
-			cv::cvtColor(data.image(), imageMono, cv::COLOR_BGR2GRAY);
+			// Extract features
+			cv::Mat imageMono;
+			// convert to grayscale
+			if(data.image().channels() > 1)
+			{
+				cv::cvtColor(data.image(), imageMono, cv::COLOR_BGR2GRAY);
+			}
+			else
+			{
+				imageMono = data.image();
+			}
+			cv::Rect roi = Feature2D::computeRoi(imageMono, _roiRatios);
+
+			if(!data.rightImage().empty())
+			{
+				//stereo
+				cv::Mat disparity;
+				keypoints = _feature2D->generateKeypoints(imageMono, 0, roi);
+				UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), timer.ticks());
+
+				std::vector<cv::Point2f> leftCorners;
+				cv::KeyPoint::convert(keypoints, leftCorners);
+				if(_stereoSubPixWinSize > 0 && _stereoSubPixIterations > 0)
+				{
+					cv::cornerSubPix( imageMono, leftCorners,
+							cv::Size( _stereoSubPixWinSize, _stereoSubPixWinSize ),
+							cv::Size( -1, -1 ),
+							cv::TermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, _stereoSubPixIterations, _stereoSubPixEps ) );
+					UDEBUG("time subpix left kpts=%fs", timer.ticks());
+				}
+
+				//generate a disparity map
+				disparity = util3d::disparityFromStereoImages(
+						imageMono,
+						data.rightImage(),
+						leftCorners,
+						_stereoFlowWinSize,
+						_stereoFlowMaxLevel,
+						_stereoFlowIterations,
+						_stereoFlowEpsilon);
+				UDEBUG("generate disparity = %fs", timer.ticks());
+
+				if(_wordsMaxDepth > 0.0f)
+				{
+					// disparity = baseline * fx / depth;
+					float minDisparity = data.baseline() * data.fx() / _wordsMaxDepth;
+					Feature2D::filterKeypointsByDisparity(keypoints, disparity, minDisparity);
+					UDEBUG("time filter keypoints by disparity (%d) = %fs", (int)keypoints.size(), timer.ticks());
+				}
+
+				if(_wordsPerImageTarget && (int)keypoints.size() > _wordsPerImageTarget)
+				{
+					Feature2D::limitKeypoints(keypoints, _wordsPerImageTarget);
+					UDEBUG("time limit keypoints max (%d) = %fs", _wordsPerImageTarget, timer.ticks());
+				}
+
+				if(keypoints.size())
+				{
+					descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
+					UDEBUG("time descriptors (%d) = %fs", descriptors.rows, timer.ticks());
+
+					keypoints3D = util3d::generateKeypoints3DDisparity(keypoints, disparity, data.fx(), data.baseline(), data.cx(), data.cy(), data.localTransform());
+					UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), timer.ticks());
+				}
+			}
+			else if(!data.depth().empty())
+			{
+				//depth
+				keypoints = _feature2D->generateKeypoints(imageMono, 0, roi);
+				UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), timer.ticks());
+
+				if(_wordsMaxDepth > 0.0f)
+				{
+					Feature2D::filterKeypointsByDepth(keypoints, data.depth(), _wordsMaxDepth);
+					UDEBUG("time filter keypoints by depth (%d) = %fs", (int)keypoints.size(), timer.ticks());
+				}
+
+				if(_wordsPerImageTarget && (int)keypoints.size() > _wordsPerImageTarget)
+				{
+					Feature2D::limitKeypoints(keypoints, _wordsPerImageTarget);
+					UDEBUG("time limit keypoints max (%d) = %fs", _wordsPerImageTarget, timer.ticks());
+				}
+
+				if(keypoints.size())
+				{
+					descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
+					UDEBUG("time descriptors (%d) = %fs", descriptors.rows, timer.ticks());
+
+					keypoints3D = util3d::generateKeypoints3DDepth(keypoints, data.depth(), data.fx(), data.fy(), data.cx(), data.cy(), data.localTransform());
+					UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), timer.ticks());
+				}
+			}
+			else
+			{
+				//RGB only
+				keypoints = _feature2D->generateKeypoints(imageMono, _wordsPerImageTarget, roi);
+				UDEBUG("time keypoints (%d) = %fs", (int)keypoints.size(), timer.ticks());
+
+				if(keypoints.size())
+				{
+					descriptors = _feature2D->generateDescriptors(imageMono, keypoints);
+					UDEBUG("time descriptors (%d) = %fs", descriptors.rows, timer.ticks());
+				}
+			}
+
+			UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
+			if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
+			{
+				descriptors = cv::Mat();
+			}
 		}
 		else
 		{
-			imageMono = data.image();
-		}
-
-		this->extractKeypointsAndDescriptors(imageMono,
-				data.depth(),
-				keypoints,
-				descriptors);
-
-		UDEBUG("ratio=%f, meanWordsPerLocation=%d", _badSignRatio, meanWordsPerLocation);
-		if(descriptors.rows && descriptors.rows < _badSignRatio * float(meanWordsPerLocation))
-		{
-			descriptors = cv::Mat();
+			UDEBUG("_wordsPerImageTarget(%d)<0 so don't extract any descriptors...", _wordsPerImageTarget);
 		}
 	}
 	else
@@ -3156,10 +3238,72 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 		keypoints = data.keypoints();
 		descriptors = data.descriptors().clone();

-		Feature2D::filterKeypointsByDepth(keypoints, descriptors,
-				data.depth(),
-				_wordsMaxDepth);
-		Feature2D::limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
+		// filter by depth
+		if(!data.rightImage().empty())
+		{
+			//stereo
+			cv::Mat imageMono;
+			// convert to grayscale
+			if(data.image().channels() > 1)
+			{
+				cv::cvtColor(data.image(), imageMono, cv::COLOR_BGR2GRAY);
+			}
+			else
+			{
+				imageMono = data.image();
+			}
+			//generate a disparity map
+			std::vector<cv::Point2f> leftCorners;
+			cv::KeyPoint::convert(keypoints, leftCorners);
+			cv::Mat disparity = util3d::disparityFromStereoImages(
+					imageMono,
+					data.rightImage(),
+					leftCorners,
+					_stereoFlowWinSize,
+					_stereoFlowMaxLevel,
+					_stereoFlowIterations,
+					_stereoFlowEpsilon);
+			UDEBUG("generate disparity = %fs", timer.ticks());
+
+			if(_wordsMaxDepth)
+			{
+				// disparity = baseline * fx / depth;
+				float minDisparity = data.baseline() * data.fx() / _wordsMaxDepth;
+				Feature2D::filterKeypointsByDisparity(keypoints, descriptors, disparity, minDisparity);
+			}
+
+			if(_wordsPerImageTarget && (int)keypoints.size() > _wordsPerImageTarget)
+			{
+				Feature2D::limitKeypoints(keypoints, _wordsPerImageTarget);
+				UDEBUG("time limit keypoints max (%d) = %fs", _wordsPerImageTarget, timer.ticks());
+			}
+
+			keypoints3D = util3d::generateKeypoints3DDisparity(keypoints, disparity, data.fx(), data.baseline(), data.cx(), data.cy(), data.localTransform());
+			UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), timer.ticks());
+		}
+		else if(!data.depth().empty())
+		{
+			//depth
+			if(_wordsMaxDepth)
+			{
+				Feature2D::filterKeypointsByDepth(keypoints, descriptors, _wordsMaxDepth);
+				UDEBUG("time filter keypoints by depth (%d) = %fs", (int)keypoints.size(), timer.ticks());
+			}
+
+			if(_wordsPerImageTarget && (int)keypoints.size() > _wordsPerImageTarget)
+			{
+				Feature2D::limitKeypoints(keypoints, _wordsPerImageTarget);
+				UDEBUG("time limit keypoints max (%d) = %fs", _wordsPerImageTarget, timer.ticks());
+			}
+
+			keypoints3D = util3d::generateKeypoints3DDepth(keypoints, data.depth(), data.fx(), data.fy(), data.cx(), data.cy(), data.localTransform());
+			UDEBUG("time keypoints 3D (%d) = %fs", (int)keypoints3D->size(), timer.ticks());
+		}
+		else
+		{
+			// RGB only
+			Feature2D::limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
+		}
 	}

 	if(_parallelized)
@@ -3188,23 +3332,22 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 	}

 	std::multimap<int, cv::KeyPoint> words;
+	std::multimap<int, pcl::PointXYZ> words3D;
 	if(wordIds.size() > 0)
 	{
 		UASSERT(wordIds.size() == keypoints.size());
+		UASSERT(keypoints3D->size() == 0 || keypoints3D->size() == wordIds.size());
 		unsigned int i=0;
 		for(std::list<int>::iterator iter=wordIds.begin(); iter!=wordIds.end() && i < keypoints.size(); ++iter, ++i)
 		{
 			words.insert(std::pair<int, cv::KeyPoint>(*iter, keypoints[i]));
+			if(keypoints3D->size())
+			{
+				words3D.insert(std::pair<int, pcl::PointXYZ>(*iter, keypoints3D->at(i)));
+			}
 		}
 	}

-	//3d words
-	std::multimap<int, pcl::PointXYZ> words3;
-	if(!data.depth().empty() && data.fx() && data.fy())
-	{
-		words3 = util3d::generateWords3(words, data.depth(), data.fx(), data.fy(), data.cx(), data.cy(), data.localTransform());
-	}
-
 	Signature * s;
 	if(keepRawData)
 	{
@@ -3235,7 +3378,7 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 		s = new Signature(id,
 			_idMapCount,
 			words,
-			words3,
+			words3D,
 			data.pose(),
 			util3d::compressData(data.depth2d()),
 			imageBytes,
@@ -3246,14 +3389,14 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 			data.cy(),
 			data.localTransform());
 		s->setImageRaw(data.image());
-		s->setDepthRaw(data.depth());
+		s->setDepthRaw(depthOrRightImage);
 	}
 	else
 	{
 		s = new Signature(id,
 			_idMapCount,
 			words,
-			words3,
+			words3D,
 			data.pose(),
 			util3d::compressData(data.depth2d()));
 	}
--- a/corelib/src/Odometry.cpp
+++ b/corelib/src/Odometry.cpp
@@ -152,6 +152,29 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
 	customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
 	customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(featureType)));

+	// Memory's stereo parameters, copy from Odometry
+	int flowWinSize = Parameters::defaultOdomFlowWinSize();
+	int flowIterations_ = Parameters::defaultOdomFlowIterations();
+	double flowEps_ = Parameters::defaultOdomFlowEps();
+	int flowMaxLevel_ = Parameters::defaultOdomFlowMaxLevel();
+	int subPixWinSize_ = Parameters::defaultOdomFlowSubPixWinSize();
+	int subPixIterations_ = Parameters::defaultOdomFlowSubPixIterations();
+	double subPixEps_ = Parameters::defaultOdomFlowSubPixEps();
+	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_);
+	customParameters.insert(ParametersPair(Parameters::kStereoWinSize(), uNumber2Str(flowWinSize)));
+	customParameters.insert(ParametersPair(Parameters::kStereoIterations(), uNumber2Str(flowIterations_)));
+	customParameters.insert(ParametersPair(Parameters::kStereoEps(), uNumber2Str(flowEps_)));
+	customParameters.insert(ParametersPair(Parameters::kStereoMaxLevel(), uNumber2Str(flowMaxLevel_)));
+	customParameters.insert(ParametersPair(Parameters::kStereoSubPixWinSize(), uNumber2Str(subPixWinSize_)));
+	customParameters.insert(ParametersPair(Parameters::kStereoSubPixIterations(), uNumber2Str(subPixIterations_)));
+	customParameters.insert(ParametersPair(Parameters::kStereoSubPixEps(), uNumber2Str(subPixEps_)));
+
 	// add only feature stuff
 	for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
 	{
@@ -434,7 +457,7 @@ OdometryOpticalFlow::OdometryOpticalFlow(const ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomFlowSubPixEps(), subPixEps_);

 	ParametersMap::const_iterator iter;
-	Feature2D::Type detectorStrategy = Feature2D::kFeatureUndef;
+	Feature2D::Type detectorStrategy = (Feature2D::Type)Parameters::defaultOdomFeatureType();
 	if((iter=parameters.find(Parameters::kOdomFeatureType())) != parameters.end())
 	{
 		detectorStrategy = (Feature2D::Type)std::atoi((*iter).second.c_str());
@@ -531,7 +554,6 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 			{
 				lastCornersKept[ki] = lastCorners_[i];
 				newCornersKept[ki] = newCorners[i];
-				cv::Point2f pt = lastCorners_[i] - newCorners[i];
 				++ki;
 			}
 		}
@@ -602,11 +624,11 @@ Transform OdometryOpticalFlow::computeTransformStereo(
 					float newDisparity = newCornersKept[i].x - newCornersKeptRight[i].x;
 					if(lastDisparity > 0.0f && newDisparity > 0.0f)
 					{
-						pcl::PointXYZ lastPt3D = util3d::projectDisparityTo3d(
+						pcl::PointXYZ lastPt3D = util3d::projectDisparityTo3D(
 								lastCornersKept[i],
 								lastDisparity,
 								data.cx(), data.cy(), data.fx(), data.baseline());
-						pcl::PointXYZ newPt3D = util3d::projectDisparityTo3d(
+						pcl::PointXYZ newPt3D = util3d::projectDisparityTo3D(
 								newCornersKept[i],
 								newDisparity,
 								data.cx(), data.cy(), data.fx(), data.baseline());
@@ -828,7 +850,7 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 				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,
+				pcl::PointXYZ pt = util3d::projectDepthTo3D(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()) &&
@@ -971,7 +993,7 @@ Transform OdometryOpticalFlow::computeTransformRGBD(
 				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,
+					pcl::PointXYZ pt = util3d::projectDepthTo3D(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()) &&
@@ -1059,6 +1081,12 @@ Transform OdometryICP::computeTransform(const SensorData & data, int * quality,
 	unsigned int minPoints = 100;
 	if(!data.depth().empty())
 	{
+		if(data.depth().type() == CV_8UC1)
+		{
+			UERROR("ICP 3D cannot be done on stereo images!");
+			return output;
+		}
+
 		pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
 						data.depth(),
 						data.fx(),
--- a/corelib/src/toro3d/transformation3.hxx
+++ b/corelib/src/toro3d/transformation3.hxx
@@ -216,7 +216,7 @@ inline Vector3<T> Quaternion<T>::toAngles() const{
 	T n = this->norm();
 	T s = n > 0?2./(n*n):0.;
 	
-	T m00, m01, m02, m10, m11, m12, m20, m21, m22;
+	T m00, m10, m20, m21, m22;
 	T phi,theta,psi;
 	
 	
@@ -238,17 +238,17 @@ inline Vector3<T> Quaternion<T>::toAngles() const{
 	T zz = this->z*zs;
 	
 	m00 = 1.0 - (yy + zz);
-	m11 = 1.0 - (xx + zz);
+	//m11 = 1.0 - (xx + zz);
 	m22 = 1.0 - (xx + yy);
 	
 	
 	m10 = xy + wz;
-	m01 = xy - wz;
+	//m01 = xy - wz;
 	
 	m20 = xz - wy;
-	m02 = xz + wy;
+	//m02 = xz + wy;
 	m21 = yz + wx;
-	m12 = yz - wx;
+	//m12 = yz - wx;
 	
 	phi   = atan2(m21,m22);
 	theta = atan2(-m20,sqrt(m21*m21 + m22*m22));
--- a/corelib/src/util3d.cpp
+++ b/corelib/src/util3d.cpp
@@ -44,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include <opencv2/nonfree/features2d.hpp>
 #include <opencv2/calib3d/calib3d.hpp>
+#include <opencv2/video/tracking.hpp>
 #include <rtabmap/core/VWDictionary.h>
 #include <cmath>
 #include <stdio.h>
@@ -318,8 +319,8 @@ cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
 	return depth32F;
 }

-std::multimap<int, pcl::PointXYZ> generateWords3(
-		const std::multimap<int, cv::KeyPoint> & words,
+pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
+		const std::vector<cv::KeyPoint> & keypoints,
 		const cv::Mat & depth,
 		float fx,
 		float fy,
@@ -327,26 +328,133 @@ std::multimap<int, pcl::PointXYZ> generateWords3(
 		float cy,
 		const Transform & transform)
 {
-	std::multimap<int, pcl::PointXYZ> words3;
-	for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
+	UASSERT(!depth.empty() && (depth.type() == CV_32FC1 || depth.type() == CV_16UC1));
+	pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
+	if(!depth.empty())
 	{
-		pcl::PointXYZ pt = util3d::getDepth(
-				depth,
-				iter->second.pt.x,
-				iter->second.pt.y,
+		keypoints3d->resize(keypoints.size());
+		for(unsigned int i=0; i!=keypoints.size(); ++i)
+		{
+			pcl::PointXYZ pt = util3d::projectDepthTo3D(
+					depth,
+					keypoints[i].pt.x,
+					keypoints[i].pt.y,
+					cx,
+					cy,
+					fx,
+					fy,
+					true);
+
+			if(!transform.isNull() && !transform.isIdentity())
+			{
+				pt = pcl::transformPoint(pt, util3d::transformToEigen3f(transform));
+			}
+			keypoints3d->at(i) = pt;
+		}
+	}
+	return keypoints3d;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDisparity(
+		const std::vector<cv::KeyPoint> & keypoints,
+		const cv::Mat & disparity,
+		float fx,
+		float baseline,
+		float cx,
+		float cy,
+		const Transform & transform)
+{
+	UASSERT(!disparity.empty() && (disparity.type() == CV_16SC1 || disparity.type() == CV_32F));
+	pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
+	keypoints3d->resize(keypoints.size());
+	for(unsigned int i=0; i!=keypoints.size(); ++i)
+	{
+		pcl::PointXYZ pt = util3d::projectDisparityTo3D(
+				keypoints[i].pt,
+				disparity,
 				cx,
 				cy,
 				fx,
-				fy,
-				true);
+				baseline);

-		if(!transform.isNull() && !transform.isIdentity())
+		if(pcl::isFinite(pt) && !transform.isNull() && !transform.isIdentity())
 		{
 			pt = pcl::transformPoint(pt, util3d::transformToEigen3f(transform));
 		}
-		words3.insert(std::make_pair(iter->first, pt));
+		keypoints3d->at(i) = pt;
 	}
-	return words3;
+	return keypoints3d;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DStereo(
+		const std::vector<cv::KeyPoint> & keypoints,
+		const cv::Mat & leftImage,
+		const cv::Mat & rightImage,
+		float fx,
+		float baseline,
+		float cx,
+		float cy,
+		const Transform & transform,
+		int flowWinSize,
+		int flowMaxLevel,
+		int flowIterations,
+		double flowEps)
+{
+	UASSERT(!leftImage.empty() && !rightImage.empty() &&
+			leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
+			leftImage.rows == rightImage.rows && leftImage.cols == rightImage.cols);
+
+	std::vector<cv::Point2f> leftCorners;
+	cv::KeyPoint::convert(keypoints, leftCorners);
+
+	// Find features in the new left image
+	std::vector<unsigned char> status;
+	std::vector<float> err;
+	std::vector<cv::Point2f> rightCorners;
+	UDEBUG("cv::calcOpticalFlowPyrLK() begin");
+	cv::calcOpticalFlowPyrLK(
+			leftImage,
+			rightImage,
+			leftCorners,
+			rightCorners,
+			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 keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
+	keypoints3d->resize(keypoints.size());
+	float bad_point = std::numeric_limits<float>::quiet_NaN ();
+	UASSERT(status.size() == keypoints.size());
+	for(unsigned int i=0; i<status.size(); ++i)
+	{
+		pcl::PointXYZ pt(bad_point, bad_point, bad_point);
+		if(status[i])
+		{
+			float disparity = leftCorners[i].x - rightCorners[i].x;
+			if(disparity > 0.0f)
+			{
+				pcl::PointXYZ tmpPt = util3d::projectDisparityTo3D(
+						leftCorners[i],
+						disparity,
+						cx, cy, fx, baseline);
+
+				if(pcl::isFinite(tmpPt))
+				{
+					pt = tmpPt;
+					if(!transform.isNull() && !transform.isIdentity())
+					{
+						pt = pcl::transformPoint(pt, util3d::transformToEigen3f(transform));
+					}
+				}
+			}
+		}
+
+		keypoints3d->at(i) = pt;
+	}
+	return keypoints3d;
 }

 std::multimap<int, cv::KeyPoint> aggregate(
@@ -435,7 +543,7 @@ void findCorrespondences(
 	inliers2.resize(oi);
 }

-pcl::PointXYZ getDepth(
+pcl::PointXYZ projectDepthTo3D(
 		const cv::Mat & depthImage,
 		float x, float y,
 		float cx, float cy,
@@ -685,6 +793,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 		float fx, float fy,
 		int decimation)
 {
+	UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 	if(decimation < 1)
 	{
@@ -706,7 +815,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDepth(
 		{
 			pcl::PointXYZ & pt = cloud->at((h/decimation)*cloud->width + (w/decimation));

-			pcl::PointXYZ ptXYZ = getDepth(imageDepth, w, h, cx, cy, fx, fy, false);
+			pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, cx, cy, fx, fy, false);
 			pt.x = ptXYZ.x;
 			pt.y = ptXYZ.y;
 			pt.z = ptXYZ.z;
@@ -725,6 +834,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 		int decimation)
 {
 	UASSERT(imageRgb.rows == imageDepth.rows && imageRgb.cols == imageDepth.cols);
+	UASSERT(!imageDepth.empty() && (imageDepth.type() == CV_16UC1 || imageDepth.type() == CV_32FC1));
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 	if(decimation < 1)
 	{
@@ -770,7 +880,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 				pt.r = v;
 			}

-			pcl::PointXYZ ptXYZ = getDepth(imageDepth, w, h, cx, cy, fx, fy, false);
+			pcl::PointXYZ ptXYZ = projectDepthTo3D(imageDepth, w, h, cx, cy, fx, fy, false);
 			pt.x = ptXYZ.x;
 			pt.y = ptXYZ.y;
 			pt.z = ptXYZ.z;
@@ -835,7 +945,7 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
 			}

 			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);
+			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;
@@ -844,7 +954,35 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
 	return cloud;
 }

-cv::Mat disparityFromStereoImages(const cv::Mat & leftImage, const cv::Mat & rightImage)
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
+		const cv::Mat & imageLeft,
+		const cv::Mat & imageRight,
+		float cx, float cy,
+		float fx, float fy,
+		int decimation)
+{
+	UASSERT(imageRight.type() == CV_8UC1);
+
+	cv::Mat leftMono;
+	if(imageLeft.channels() == 3)
+	{
+		cv::cvtColor(imageLeft, leftMono, CV_BGR2GRAY);
+	}
+	else
+	{
+		leftMono = imageLeft;
+	}
+	return rtabmap::util3d::cloudFromDisparityRGB(
+			imageLeft,
+			util3d::disparityFromStereoImages(leftMono, imageRight),
+			cx, cy,
+			fx, fy,
+			decimation);
+}
+
+cv::Mat disparityFromStereoImages(
+		const cv::Mat & leftImage,
+		const cv::Mat & rightImage)
 {
 	UASSERT(!leftImage.empty() && !rightImage.empty() &&
 			leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
@@ -852,13 +990,132 @@ cv::Mat disparityFromStereoImages(const cv::Mat & leftImage, const cv::Mat & rig
 			leftImage.rows == rightImage.rows);
 	cv::StereoBM stereo(cv::StereoBM::BASIC_PRESET, 160, 15);
 	cv::Mat disparity;
-	stereo(leftImage, rightImage, disparity, CV_16S);
+	stereo(leftImage, rightImage, disparity, CV_16SC1);
 	cv::filterSpeckles(disparity, 0, 1000, 16);
 	return disparity;
 }

+cv::Mat disparityFromStereoImages(
+		const cv::Mat & leftImage,
+		const cv::Mat & rightImage,
+		const std::vector<cv::Point2f> & leftCorners,
+		int flowWinSize,
+		int flowMaxLevel,
+		int flowIterations,
+		double flowEps)
+{
+	UASSERT(!leftImage.empty() && !rightImage.empty() &&
+			leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
+			leftImage.cols == rightImage.cols &&
+			leftImage.rows == rightImage.rows);
+
+	// Find features in the new left image
+	std::vector<unsigned char> status;
+	std::vector<float> err;
+	std::vector<cv::Point2f> rightCorners;
+	UDEBUG("cv::calcOpticalFlowPyrLK() begin");
+	cv::calcOpticalFlowPyrLK(
+			leftImage,
+			rightImage,
+			leftCorners,
+			rightCorners,
+			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");
+
+	return disparityFromStereoCorrespondences(leftImage, leftCorners, rightCorners, status);
+}
+
+cv::Mat depthFromStereoImages(
+		const cv::Mat & leftImage,
+		const cv::Mat & rightImage,
+		const std::vector<cv::Point2f> & leftCorners,
+		float fx,
+		float baseline,
+		int flowWinSize,
+		int flowMaxLevel,
+		int flowIterations,
+		double flowEps)
+{
+	UASSERT(!leftImage.empty() && !rightImage.empty() &&
+			leftImage.type() == CV_8UC1 && rightImage.type() == CV_8UC1 &&
+			leftImage.cols == rightImage.cols &&
+			leftImage.rows == rightImage.rows);
+	UASSERT(fx > 0.0f && baseline > 0.0f);
+
+	// Find features in the new left image
+	std::vector<unsigned char> status;
+	std::vector<float> err;
+	std::vector<cv::Point2f> rightCorners;
+	UDEBUG("cv::calcOpticalFlowPyrLK() begin");
+	cv::calcOpticalFlowPyrLK(
+			leftImage,
+			rightImage,
+			leftCorners,
+			rightCorners,
+			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");
+
+	return depthFromStereoCorrespondences(leftImage, leftCorners, rightCorners, status, fx, baseline);
+}
+
+cv::Mat disparityFromStereoCorrespondences(
+		const cv::Mat & leftImage,
+		const std::vector<cv::Point2f> & leftCorners,
+		const std::vector<cv::Point2f> & rightCorners,
+		const std::vector<unsigned char> & mask)
+{
+	UASSERT(!leftImage.empty() && leftCorners.size() == rightCorners.size());
+	UASSERT(mask.size() == 0 || mask.size() == leftCorners.size());
+	cv::Mat disparity = cv::Mat::zeros(leftImage.rows, leftImage.cols, CV_32FC1);
+	for(unsigned int i=0; i<leftCorners.size(); ++i)
+	{
+		if(mask.size() == 0 || mask[i])
+		{
+			float d = leftCorners[i].x - rightCorners[i].x;
+			if(d > 0.0f)
+			{
+				disparity.at<float>(int(leftCorners[i].y+0.5f), int(leftCorners[i].x+0.5f)) = d;
+			}
+		}
+	}
+	return disparity;
+}
+
+cv::Mat depthFromStereoCorrespondences(
+		const cv::Mat & leftImage,
+		const std::vector<cv::Point2f> & leftCorners,
+		const std::vector<cv::Point2f> & rightCorners,
+		const std::vector<unsigned char> & mask,
+		float fx, float baseline)
+{
+	UASSERT(!leftImage.empty() && leftCorners.size() == rightCorners.size());
+	UASSERT(mask.size() == 0 || mask.size() == leftCorners.size());
+	cv::Mat depth = cv::Mat::zeros(leftImage.rows, leftImage.cols, CV_32FC1);
+	for(unsigned int i=0; i<leftCorners.size(); ++i)
+	{
+		if(mask.size() == 0 || mask[i])
+		{
+			float disparity = leftCorners[i].x - rightCorners[i].x;
+			if(disparity > 0.0f)
+			{
+				float d = baseline * fx / disparity;
+				depth.at<float>(int(leftCorners[i].y+0.5f), int(leftCorners[i].x+0.5f)) = d;
+			}
+		}
+	}
+	return depth;
+}
+
 // inspired from ROS image_geometry/src/stereo_camera_model.cpp
-pcl::PointXYZ projectDisparityTo3d(
+pcl::PointXYZ projectDisparityTo3D(
 		const cv::Point2f & pt,
 		float disparity,
 		float cx, float cy, float fx, float baseline)
@@ -872,18 +1129,36 @@ pcl::PointXYZ projectDisparityTo3d(
 	return pcl::PointXYZ(bad_point, bad_point, bad_point);
 }

+pcl::PointXYZ projectDisparityTo3D(
+		const cv::Point2f & pt,
+		const cv::Mat & disparity,
+		float cx, float cy, float fx, float baseline)
+{
+	UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
+	int u = int(pt.x+0.5f);
+	int v = int(pt.y+0.5f);
+	float bad_point = std::numeric_limits<float>::quiet_NaN ();
+	if(uIsInBounds(u, 0, disparity.cols-1) &&
+	   uIsInBounds(v, 0, disparity.rows-1))
+	{
+		float d = disparity.type() == CV_16SC1?float(disparity.at<short>(v,u))/16.0f:disparity.at<float>(v,u);
+		return projectDisparityTo3D(pt, d, cx, cy, fx, baseline);
+	}
+	return pcl::PointXYZ(bad_point, bad_point, bad_point);
+}
+
 cv::Mat depthFromDisparity(const cv::Mat & disparity,
-		float cx, float cy, float fx, float baseline,
+		float fx, float baseline,
 		int type)
 {
-	UASSERT(disparity.type() == CV_32FC1 || disparity.type() == CV_16S);
+	UASSERT(!disparity.empty() && (disparity.type() == CV_32FC1 || disparity.type() == CV_16SC1));
 	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);
+			float disparity_value = disparity.type() == CV_16SC1?float(disparity.at<short>(i,j))/16.0f:disparity.at<float>(i,j);
 			if (disparity_value > 0.0f)
 			{
 				// baseline * focal / disparity
@@ -1132,8 +1407,8 @@ void extractXYZCorrespondences(const std::list<std::pair<cv::Point2f, cv::Point2
 		iter!=correspondences.end();
 		++iter)
 	{
-		pcl::PointXYZ pt1 = getDepth(depthImage1, iter->first.x, iter->first.y, cx, cy, fx, fy, true);
-		pcl::PointXYZ pt2 = getDepth(depthImage2, iter->second.x, iter->second.y, cx, cy, fx, fy, true);
+		pcl::PointXYZ pt1 = projectDepthTo3D(depthImage1, iter->first.x, iter->first.y, cx, cy, fx, fy, true);
+		pcl::PointXYZ pt2 = projectDepthTo3D(depthImage2, iter->second.x, iter->second.y, cx, cy, fx, fy, true);
 		if(pcl::isFinite(pt1) && pcl::isFinite(pt2) &&
 		   (maxDepth <= 0 || (pt1.z <= maxDepth && pt2.z<=maxDepth)))
 		{
@@ -1691,6 +1966,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr getICPReadyCloud(
 		int samples,
 		const Transform & transform)
 {
+	UASSERT(!depth.empty() && (depth.type() == CV_16UC1 || depth.type() == CV_32FC1));
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
 	cloud = cloudFromDepth(
 			depth,
@@ -1767,7 +2043,7 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr get3DFASTKpts(
 	pcl::PointCloud<pcl::PointXYZ>::Ptr points(new pcl::PointCloud<pcl::PointXYZ>);
 	for(unsigned int i=0; i<kpts.size(); ++i)
 	{
-		pcl::PointXYZ pt = getDepth(imageDepth, kpts[i].pt.x, kpts[i].pt.y, 0, 0, 1.0f/constant, 1.0f/constant, true);
+		pcl::PointXYZ pt = projectDepthTo3D(imageDepth, kpts[i].pt.x, kpts[i].pt.y, 0, 0, 1.0f/constant, 1.0f/constant, true);
 		if(uIsFinite(pt.z) && (maxDepth <= 0 || pt.z <= maxDepth))
 		{
 			points->push_back(pt);