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

corelib/include/rtabmap/core/Features2d.h

@@ -62,6 +62,7 @@ public:
 		kFeatureBrisk=7};
 
 	static Feature2D * create(Feature2D::Type & type, const ParametersMap & parameters);
+
 	static void filterKeypointsByDepth(
 				std::vector<cv::KeyPoint> & keypoints,
 				const cv::Mat & depth,
@@ -72,6 +73,16 @@ public:
 			const cv::Mat & depth,
 			float maxDepth);
 
+	static void filterKeypointsByDisparity(
+				std::vector<cv::KeyPoint> & keypoints,
+				const cv::Mat & disparity,
+				float minDisparity);
+	static void filterKeypointsByDisparity(
+			std::vector<cv::KeyPoint> & keypoints,
+			cv::Mat & descriptors,
+			const cv::Mat & disparity,
+			float minDisparity);
+
 	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints);
 	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints);
 

corelib/include/rtabmap/core/Memory.h

@@ -161,15 +161,6 @@ public:
 	const VWDictionary * getVWDictionary() const;
 	std::multimap<int, cv::KeyPoint> getWords(int signatureId) const;
 	Feature2D::Type getFeatureType() const {return _featureType;}
-	void extractKeypointsAndDescriptors(
-			const cv::Mat & image,
-			std::vector<cv::KeyPoint> & keypoints,
-			cv::Mat & descriptors) const;
-	void extractKeypointsAndDescriptors(
-			const cv::Mat & image,
-			const cv::Mat & depth,
-			std::vector<cv::KeyPoint> & keypoints,
-			cv::Mat & descriptors) const;
 
 	// RGB-D stuff
 	void getMetricConstraints(
@@ -276,6 +267,16 @@ private:
 	float _icp2MaxFitness;
 	float _icp2CorrespondenceRatio;
 	float _icp2VoxelSize;
+
+	// Stereo stuff
+	int _stereoFlowWinSize;
+	int _stereoFlowIterations;
+	double _stereoFlowEpsilon;
+	int _stereoFlowMaxLevel;
+
+	int _stereoSubPixWinSize;
+	int _stereoSubPixIterations;
+	double _stereoSubPixEps;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Parameters.h

@@ -269,7 +269,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Odom, MaxDepth,               float, 4.0, 	"Max depth of the words (0 means no limit).");
 	RTABMAP_PARAM(Odom, ResetCountdown,         int, 0,         "Automatically reset odometry after X consecutive images on which odometry cannot be computed (value=0 disables auto-reset).");
 	RTABMAP_PARAM_STR(Odom, RoiRatios,          "0.0 0.0 0.0 0.0", "Region of interest ratios [left, right, top, bottom].");
-	RTABMAP_PARAM(Odom, RefineIterations,       int, 10,        "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
+	RTABMAP_PARAM(Odom, RefineIterations,       int, 5,        "Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
 	RTABMAP_PARAM(Odom, FeaturesRatio,          float, 0.5, 	"Minimum ratio of keypoints between the current image and the last image to compute odometry.");
 
 	// Odometry Bag-of-words
@@ -315,6 +315,15 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(LccIcp2, CorrespondenceRatio, float, 0.7, 		"ICP 2D: Ratio of matching correspondences to accept the transform.");
 	RTABMAP_PARAM(LccIcp2, VoxelSize,       float, 0.005, 			"Voxel size to be used for ICP computation.");
 
+	// Stereo disparity
+	RTABMAP_PARAM(Stereo, WinSize,               int, 9,        "See cv::calcOpticalFlowPyrLK().");
+	RTABMAP_PARAM(Stereo, Iterations,            int, 20,       "See cv::calcOpticalFlowPyrLK().");
+	RTABMAP_PARAM(Stereo, Eps,                   double, 0.02,  "See cv::calcOpticalFlowPyrLK().");
+	RTABMAP_PARAM(Stereo, MaxLevel,              int, 4,        "See cv::calcOpticalFlowPyrLK().");
+	RTABMAP_PARAM(Stereo, SubPixWinSize,         int, 3,        "See cv::cornerSubPix().");
+	RTABMAP_PARAM(Stereo, SubPixIterations,      int, 0,        "See cv::cornerSubPix(). 0 disables sub pixel refining.");
+	RTABMAP_PARAM(Stereo, SubPixEps,             double, 0.02,  "See cv::cornerSubPix().");
+
 public:
 	virtual ~Parameters();
 

corelib/include/rtabmap/core/util3d.h

@@ -103,8 +103,8 @@ void RTABMAP_EXP rgbdFromCloud(
 cv::Mat RTABMAP_EXP cvtDepthFromFloat(const cv::Mat & depth32F);
 cv::Mat RTABMAP_EXP cvtDepthToFloat(const cv::Mat & depth16U);
 
-std::multimap<int, pcl::PointXYZ> RTABMAP_EXP generateWords3(
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDepth(
-		const std::multimap<int, cv::KeyPoint> & words,
+		const std::vector<cv::KeyPoint> & keypoints,
 		const cv::Mat & depth,
 		float fx,
 		float fy,
@@ -112,11 +112,34 @@ std::multimap<int, pcl::PointXYZ> RTABMAP_EXP generateWords3(
 		float cy,
 		const Transform & transform);
 
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP generateKeypoints3DDisparity(
+		const std::vector<cv::KeyPoint> & keypoints,
+		const cv::Mat & disparity,
+		float fx,
+		float baseline,
+		float cx,
+		float cy,
+		const Transform & transform);
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP 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 = Transform::getIdentity(),
+		int flowWinSize = 9,
+		int flowMaxLevel = 4,
+		int flowIterations = 20,
+		double flowEps = 0.02);
+
 std::multimap<int, cv::KeyPoint> RTABMAP_EXP aggregate(
 		const std::list<int> & wordIds,
 		const std::vector<cv::KeyPoint> & keypoints);
 
-pcl::PointXYZ RTABMAP_EXP getDepth(
+pcl::PointXYZ RTABMAP_EXP projectDepthTo3D(
 		const cv::Mat & depthImage,
 		float x, float y,
 		float cx, float cy,
@@ -193,17 +216,62 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDisparityRGB(
 		float fx, float baseline,
 		int decimation);
 
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromStereoImages(
+		const cv::Mat & imageLeft,
+		const cv::Mat & imageRight,
+		float cx, float cy,
+		float fx, float fy,
+		int decimation);
+
 cv::Mat RTABMAP_EXP disparityFromStereoImages(
 		const cv::Mat & leftImage,
 		const cv::Mat & rightImage);
 
-pcl::PointXYZ RTABMAP_EXP projectDisparityTo3d(
+cv::Mat RTABMAP_EXP disparityFromStereoImages(
+		const cv::Mat & leftImage,
+		const cv::Mat & rightImage,
+		const std::vector<cv::Point2f> & leftCorners,
+		int flowWinSize = 9,
+		int flowMaxLevel = 4,
+		int flowIterations = 20,
+		double flowEps = 0.02);
+
+cv::Mat RTABMAP_EXP depthFromStereoImages(
+		const cv::Mat & leftImage,
+		const cv::Mat & rightImage,
+		const std::vector<cv::Point2f> & leftCorners,
+		float fx,
+		float baseline,
+		int flowWinSize = 9,
+		int flowMaxLevel = 4,
+		int flowIterations = 20,
+		double flowEps = 0.02);
+
+cv::Mat RTABMAP_EXP disparityFromStereoCorrespondences(
+		const cv::Mat & leftImage,
+		const std::vector<cv::Point2f> & leftCorners,
+		const std::vector<cv::Point2f> & rightCorners,
+		const std::vector<unsigned char> & mask);
+
+cv::Mat RTABMAP_EXP 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);
+
+pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D(
 		const cv::Point2f & pt,
 		float disparity,
 		float cx, float cy, float fx, float baseline);
 
+pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D(
+		const cv::Point2f & pt,
+		const cv::Mat & disparity,
+		float cx, float cy, float fx, float baseline);
+
 cv::Mat RTABMAP_EXP depthFromDisparity(const cv::Mat & disparity,
-		float cx, float cy, float fx, float baseline,
+		float fx, float baseline,
 		int type = CV_32FC1);
 
 cv::Mat RTABMAP_EXP depth2DFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud);

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;

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(
+			if(oldDepth.type() == CV_8UC1 || newDepth.type() == CV_8UC1)
-					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,
+				UERROR("ICP 3D cannot be done on stereo images!");
-						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 ?!?";
+				pcl::PointCloud<pcl::PointXYZ>::Ptr oldCloudXYZ = util3d::getICPReadyCloud(
-				UWARN(msg.c_str());
+						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
+		if(_wordsPerImageTarget >= 0)
-		cv::Mat imageMono;
-		// convert to grayscale
-		if(data.image().channels() > 1)
 		{
-			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();
+			UDEBUG("_wordsPerImageTarget(%d)<0 so don't extract any descriptors...", _wordsPerImageTarget);
-		}
-
-		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();
 		}
 	}
 	else
@@ -3156,10 +3238,72 @@ Signature * Memory::createSignature(const SensorData & data, bool keepRawData)
 		keypoints = data.keypoints();
 		descriptors = data.descriptors().clone();
 
-		Feature2D::filterKeypointsByDepth(keypoints, descriptors,
+		// filter by depth
-				data.depth(),
+		if(!data.rightImage().empty())
-				_wordsMaxDepth);
+		{
-		Feature2D::limitKeypoints(keypoints, descriptors, _wordsPerImageTarget);
+			//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()));
 	}

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(),

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));

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(
+pcl::PointCloud<pcl::PointXYZ>::Ptr generateKeypoints3DDepth(
-		const std::multimap<int, cv::KeyPoint> & words,
+		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;
+	UASSERT(!depth.empty() && (depth.type() == CV_32FC1 || depth.type() == CV_16UC1));
-	for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
+	pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3d(new pcl::PointCloud<pcl::PointXYZ>);
+	if(!depth.empty())
 	{
-		pcl::PointXYZ pt = util3d::getDepth(
+		keypoints3d->resize(keypoints.size());
-				depth,
+		for(unsigned int i=0; i!=keypoints.size(); ++i)
-				iter->second.pt.x,
+		{
-				iter->second.pt.y,
+			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,
+				baseline);
-				true);
 
-		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 pt1 = projectDepthTo3D(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 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);

examples/RGBDMapping/MapBuilder.h

@@ -46,7 +46,9 @@ class MapBuilder : public QWidget, public UEventsHandler
 {
 	Q_OBJECT
 public:
-	MapBuilder()
+	MapBuilder() :
+		_processingStatistics(false),
+		_lastOdometryProcessed(true)
 	{
 		this->setWindowFlags(Qt::Dialog);
 		this->setWindowTitle(tr("3D Map"));
@@ -60,7 +62,7 @@ public:
 		this->setLayout(layout);
 
 		qRegisterMetaType<rtabmap::Statistics>("rtabmap::Statistics");
-		qRegisterMetaType<rtabmap::SensorData>("rtabmap::Image");
+		qRegisterMetaType<rtabmap::SensorData>("rtabmap::SensorData");
 	}
 
 	virtual ~MapBuilder()
@@ -128,15 +130,14 @@ private slots:
 			}
 		}
 		cloudViewer_->render();
+
+		_lastOdometryProcessed = true;
 	}
 
 
 	void processStatistics(const rtabmap::Statistics & stats)
 	{
-		if(!this->isVisible())
+		_processingStatistics = true;
-		{
-			return;
-		}
 
 		const std::map<int, Transform> & poses = stats.poses();
 		QMap<std::string, Transform> clouds = cloudViewer_->getAddedClouds();
@@ -198,6 +199,8 @@ private slots:
 		}
 
 		cloudViewer_->render();
+
+		_processingStatistics = false;
 	}
 
 protected:
@@ -208,19 +211,30 @@ protected:
 			RtabmapEvent * rtabmapEvent = (RtabmapEvent *)event;
 			const Statistics & stats = rtabmapEvent->getStats();
 			// Statistics must be processed in the Qt thread
-			QMetaObject::invokeMethod(this, "processStatistics", Q_ARG(rtabmap::Statistics, stats));
+			if(this->isVisible())
+			{
+				QMetaObject::invokeMethod(this, "processStatistics", Q_ARG(rtabmap::Statistics, stats));
+			}
 		}
 		else if(event->getClassName().compare("OdometryEvent") == 0)
 		{
 			OdometryEvent * odomEvent = (OdometryEvent *)event;
 			// Odometry must be processed in the Qt thread
-			QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::SensorData, odomEvent->data()));
+			if(this->isVisible() &&
+			   _lastOdometryProcessed &&
+			   !_processingStatistics)
+			{
+				_lastOdometryProcessed = false; // if we receive too many odometry events!
+				QMetaObject::invokeMethod(this, "processOdometry", Q_ARG(rtabmap::SensorData, odomEvent->data()));
+			}
 		}
 	}
 
 private:
 	CloudViewer * cloudViewer_;
 	Transform lastOdomPose_;
+	bool _processingStatistics;
+	bool _lastOdometryProcessed;
 };
 
 

examples/RGBDMapping/main.cpp

@@ -36,12 +36,35 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "MapBuilder.h"
 
+void showUsage()
+{
+	printf("\nUsage:\n"
+			"rtabmap-rgbd_mapping driver\n"
+			"  driver       Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS\n\n");
+	exit(1);
+}
+
 using namespace rtabmap;
 int main(int argc, char * argv[])
 {
 	ULogger::setType(ULogger::kTypeConsole);
 	ULogger::setLevel(ULogger::kWarning);
 
+	int driver = 0;
+	if(argc < 2)
+	{
+		showUsage();
+	}
+	else
+	{
+		driver = atoi(argv[argc-1]);
+		if(driver < 0 || driver > 4)
+		{
+			UERROR("driver should be between 0 and 4.");
+			showUsage();
+		}
+	}
+
 	// GUI stuff, there the handler will receive RtabmapEvent and construct the map
 	QApplication app(argc, argv);
 	MapBuilder mapBuilder;
@@ -51,8 +74,49 @@ int main(int argc, char * argv[])
 
 	// Create the OpenNI camera, it will send a CameraEvent at the rate specified.
 	// Set transform to camera so z is up, y is left and x going forward
-	CameraRGBD * camera = new CameraOpenni("", 10, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
+	CameraRGBD * camera = 0;
-	//CameraRGBD * camera = new CameraFreenect(0, 10, rtabmap::Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0));
+	Transform opticalRotation(0,0,1,0, -1,0,0,0, 0,-1,0,0);
+	if(driver == 1)
+	{
+		if(!CameraOpenNI2::available())
+		{
+			UERROR("Not built with OpenNI2 support...");
+			exit(-1);
+		}
+		camera = new CameraOpenNI2(0, opticalRotation);
+	}
+	else if(driver == 2)
+	{
+		if(!CameraFreenect::available())
+		{
+			UERROR("Not built with Freenect support...");
+			exit(-1);
+		}
+		camera = new CameraFreenect(0, 0, opticalRotation);
+	}
+	else if(driver == 3)
+	{
+		if(!CameraOpenNICV::available())
+		{
+			UERROR("Not built with OpenNI from OpenCV support...");
+			exit(-1);
+		}
+		camera = new CameraOpenNICV(false, 0, opticalRotation);
+	}
+	else if(driver == 4)
+	{
+		if(!CameraOpenNICV::available())
+		{
+			UERROR("Not built with OpenNI from OpenCV support...");
+			exit(-1);
+		}
+		camera = new CameraOpenNICV(true, 0, opticalRotation);
+	}
+	else
+	{
+		camera = new rtabmap::CameraOpenni("", 0, opticalRotation);
+	}
+
 	CameraThread cameraThread(camera);
 	if(!cameraThread.init())
 	{
@@ -61,7 +125,7 @@ int main(int argc, char * argv[])
 	}
 
 	// Create an odometry thread to process camera events, it will send OdometryEvent.
-	OdometryThread odomThread(new OdometryBOW()); // 0=SURF 1=SIFT
+	OdometryThread odomThread(new OdometryBOW());
 
 
 	// Create RTAB-Map to process OdometryEvent

guilib/src/DatabaseViewer.cpp

@@ -1410,19 +1410,9 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA;
 		if(depthA.type() == CV_8UC1)
 		{
-			cv::Mat leftImg;
+			cloudA = rtabmap::util3d::cloudFromStereoImages(
-			if(imageA.channels() == 3)
-			{
-				cv::cvtColor(imageA, leftImg, CV_BGR2GRAY);
-			}
-			else
-			{
-				leftImg = imageA;
-			}
-			cv::Mat disparity = util3d::disparityFromStereoImages(leftImg, depthA);
-			cloudA = rtabmap::util3d::cloudFromDisparityRGB(
 					imageA,
-					disparity,
+					depthA,
 					cxA, cyA,
 					fxA, fyA,
 					1);
@@ -1443,18 +1433,9 @@ void DatabaseViewer::updateConstraintView(const rtabmap::Link & link,
 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
 		if(depthB.type() == CV_8UC1)
 		{
-			cv::Mat leftImg;
+			cloudB = rtabmap::util3d::cloudFromStereoImages(
-			if(imageB.channels() == 3)
-			{
-				cv::cvtColor(imageB, leftImg, CV_BGR2GRAY);
-			}
-			else
-			{
-				leftImg = imageB;
-			}
-			cloudB = rtabmap::util3d::cloudFromDisparityRGB(
 					imageB,
-					util3d::disparityFromStereoImages(leftImg, depthB),
+					depthB,
 					cxB, cyB,
 					fxB, fyB,
 					1);
@@ -1786,6 +1767,13 @@ void DatabaseViewer::refineConstraint(int from, int to)
 		cv::Mat depthA = rtabmap::util3d::uncompressImage(depthBytesA);
 		cv::Mat depthB = rtabmap::util3d::uncompressImage(depthBytesB);
 
+		if(depthA.type() == CV_8UC1 || depthB.type() == CV_8UC1)
+		{
+			QMessageBox::critical(this, tr("ICP failed"), tr("ICP cannot be done on stereo images!"));
+			UERROR("ICP 3D cannot be done on stereo images!");
+			return;
+		}
+
 		cloudA = util3d::getICPReadyCloud(depthA,
 						fxA, fyA, cxA, cyA,
 						ui_->spinBox_icp_decimation->value(),

guilib/src/LoopClosureViewer.cpp

@@ -147,12 +147,24 @@ void LoopClosureViewer::updateView(const Transform & transform)
 
 			//cloud 3d
 			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudA;
-			cloudA = util3d::cloudFromDepthRGB(
+			if(depthA.type() == CV_8UC1)
-					imageA,
+			{
-					depthA,
+				cloudA = util3d::cloudFromStereoImages(
-					sA_->getDepthCx(), sA_->getDepthCy(),
+						imageA,
-					sA_->getDepthFx(), sA_->getDepthFy(),
+						depthA,
-					decimation);
+						sA_->getDepthCx(), sA_->getDepthCy(),
+						sA_->getDepthFx(), sA_->getDepthFy(),
+						decimation);
+			}
+			else
+			{
+				cloudA = util3d::cloudFromDepthRGB(
+						imageA,
+						depthA,
+						sA_->getDepthCx(), sA_->getDepthCy(),
+						sA_->getDepthFx(), sA_->getDepthFy(),
+						decimation);
+			}
 
 			cloudA = util3d::removeNaNFromPointCloud(cloudA);
 
@@ -167,12 +179,24 @@ void LoopClosureViewer::updateView(const Transform & transform)
 			cloudA = util3d::transformPointCloud(cloudA, sA_->getLocalTransform());
 
 			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudB;
-			cloudB = util3d::cloudFromDepthRGB(
+			if(depthB.type() == CV_8UC1)
-					imageB,
+			{
-					depthB,
+				cloudB = util3d::cloudFromStereoImages(
-					sB_->getDepthCx(), sB_->getDepthCy(),
+						imageB,
-					sB_->getDepthFx(), sB_->getDepthFy(),
+						depthB,
-					decimation);
+						sB_->getDepthCx(), sB_->getDepthCy(),
+						sB_->getDepthFx(), sB_->getDepthFy(),
+						decimation);
+			}
+			else
+			{
+				cloudB = util3d::cloudFromDepthRGB(
+						imageB,
+						depthB,
+						sB_->getDepthCx(), sB_->getDepthCy(),
+						sB_->getDepthFx(), sB_->getDepthFy(),
+						decimation);
+			}
 
 			cloudB = util3d::removeNaNFromPointCloud(cloudB);
 

guilib/src/MainWindow.cpp

@@ -3766,12 +3766,25 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr MainWindow::createCloud(
 		float maxDepth) const
 {
 	UTimer timer;
-	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(
+	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-			rgb,
+	if(depth.type() == CV_8UC1)
-			depth,
+	{
-		    cx, cy,
+		cloud = util3d::cloudFromStereoImages(
-		    fx, fy,
+				rgb,
-		    decimation);
+				depth,
+				cx, cy,
+				fx, fy,
+				decimation);
+	}
+	else
+	{
+		cloud = util3d::cloudFromDepthRGB(
+				rgb,
+				depth,
+				cx, cy,
+				fx, fy,
+				decimation);
+	}
 
 	if(cloud->size())
 	{

guilib/src/OdometryViewer.cpp

@@ -98,12 +98,24 @@ void OdometryViewer::processData()
 		// visualization: buffering the clouds
 		// Create the new cloud
 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
-		cloud = util3d::cloudFromDepthRGB(
+		if(data.depth().type() == CV_8UC1)
-				data.image(),
+		{
-				data.depth(),
+			cloud = util3d::cloudFromStereoImages(
-				data.cx(), data.cy(),
+					data.image(),
-			    data.fx(), data.fy(),
+					data.depth(),
-			    decimation_);
+					data.cx(), data.cy(),
+					data.fx(), data.fy(),
+					decimation_);
+		}
+		else
+		{
+			cloud = util3d::cloudFromDepthRGB(
+					data.image(),
+					data.depth(),
+					data.cx(), data.cy(),
+					data.fx(), data.fy(),
+					decimation_);
+		}
 
 		if(voxelSize_ > 0.0f)
 		{

guilib/src/PreferencesDialog.cpp

@@ -482,6 +482,15 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->odom_flow_subpix_iterations->setObjectName(Parameters::kOdomFlowSubPixIterations().c_str());
 	_ui->odom_flow_subpix_eps->setObjectName(Parameters::kOdomFlowSubPixEps().c_str());
 
+	//Stereo
+	_ui->stereo_winSize->setObjectName(Parameters::kStereoWinSize().c_str());
+	_ui->stereo_maxLevel->setObjectName(Parameters::kStereoMaxLevel().c_str());
+	_ui->stereo_iterations->setObjectName(Parameters::kStereoIterations().c_str());
+	_ui->stereo_eps->setObjectName(Parameters::kStereoEps().c_str());
+	_ui->stereo_subpix_winSize->setObjectName(Parameters::kStereoSubPixWinSize().c_str());
+	_ui->stereo_subpix_iterations->setObjectName(Parameters::kStereoSubPixIterations().c_str());
+	_ui->stereo_subpix_eps->setObjectName(Parameters::kStereoSubPixEps().c_str());
+
 	setupSignals();
 	// custom signals
 	connect(_ui->doubleSpinBox_kp_roi0, SIGNAL(valueChanged(double)), this, SLOT(updateKpROI()));

guilib/src/ui/preferencesDialog.ui

@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>9</number>
+                <number>22</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -5833,6 +5833,248 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                  </item>
                 </layout>
                </widget>
+               <widget class="QWidget" name="page_29">
+                <layout class="QVBoxLayout" name="verticalLayout_57">
+                 <item>
+                  <widget class="QGroupBox" name="groupBox_stereo2">
+                   <property name="title">
+                    <string>Stereo</string>
+                   </property>
+                   <layout class="QVBoxLayout" name="verticalLayout_56">
+                    <item>
+                     <widget class="QLabel" name="label_187">
+                      <property name="text">
+                       <string>These parameters are used to find the 3D positions of visual words using disparity between the left and right images. Optical flow is used to find corresponding corners in the right image of words extracted from the left image. Sub pixel corners may not be used for features like SURF/SIFT, which are already sub pixel.</string>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                     </widget>
+                    </item>
+                    <item>
+                     <widget class="QGroupBox" name="groupBox_10">
+                      <property name="title">
+                       <string>calcOpticalFlowPyrLK()</string>
+                      </property>
+                      <layout class="QGridLayout" name="gridLayout_33" columnstretch="0,1">
+                       <item row="0" column="0">
+                        <widget class="QSpinBox" name="stereo_winSize">
+                         <property name="minimum">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <number>999999</number>
+                         </property>
+                         <property name="singleStep">
+                          <number>1</number>
+                         </property>
+                         <property name="value">
+                          <number>21</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="0" column="1">
+                        <widget class="QLabel" name="label_205">
+                         <property name="text">
+                          <string>Window size.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="0">
+                        <widget class="QSpinBox" name="stereo_iterations">
+                         <property name="minimum">
+                          <number>1</number>
+                         </property>
+                         <property name="maximum">
+                          <number>999999</number>
+                         </property>
+                         <property name="singleStep">
+                          <number>1</number>
+                         </property>
+                         <property name="value">
+                          <number>30</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="1">
+                        <widget class="QLabel" name="label_206">
+                         <property name="text">
+                          <string>Iterations.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="2" column="0">
+                        <widget class="QDoubleSpinBox" name="stereo_eps">
+                         <property name="decimals">
+                          <number>3</number>
+                         </property>
+                         <property name="minimum">
+                          <double>0.001000000000000</double>
+                         </property>
+                         <property name="maximum">
+                          <double>0.100000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.010000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>0.010000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="2" column="1">
+                        <widget class="QLabel" name="label_207">
+                         <property name="text">
+                          <string>Epsilon.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="3" column="1">
+                        <widget class="QLabel" name="label_208">
+                         <property name="text">
+                          <string>Max level.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="3" column="0">
+                        <widget class="QSpinBox" name="stereo_maxLevel">
+                         <property name="minimum">
+                          <number>0</number>
+                         </property>
+                         <property name="maximum">
+                          <number>999999</number>
+                         </property>
+                         <property name="singleStep">
+                          <number>1</number>
+                         </property>
+                         <property name="value">
+                          <number>3</number>
+                         </property>
+                        </widget>
+                       </item>
+                      </layout>
+                     </widget>
+                    </item>
+                    <item>
+                     <widget class="QGroupBox" name="groupBox_9">
+                      <property name="title">
+                       <string>cornerSubPix()</string>
+                      </property>
+                      <layout class="QGridLayout" name="gridLayout_32" columnstretch="0,1">
+                       <item row="0" column="0">
+                        <widget class="QSpinBox" name="stereo_subpix_winSize">
+                         <property name="minimum">
+                          <number>0</number>
+                         </property>
+                         <property name="maximum">
+                          <number>999999</number>
+                         </property>
+                         <property name="singleStep">
+                          <number>1</number>
+                         </property>
+                         <property name="value">
+                          <number>5</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="0" column="1">
+                        <widget class="QLabel" name="label_200">
+                         <property name="text">
+                          <string>Window size for sub pixel estimation.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="0">
+                        <widget class="QSpinBox" name="stereo_subpix_iterations">
+                         <property name="minimum">
+                          <number>0</number>
+                         </property>
+                         <property name="maximum">
+                          <number>999999</number>
+                         </property>
+                         <property name="singleStep">
+                          <number>1</number>
+                         </property>
+                         <property name="value">
+                          <number>20</number>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="1" column="1">
+                        <widget class="QLabel" name="label_203">
+                         <property name="text">
+                          <string>Iterations for sub pixel estimation. 0 disables sub pixel refining.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="2" column="0">
+                        <widget class="QDoubleSpinBox" name="stereo_subpix_eps">
+                         <property name="decimals">
+                          <number>3</number>
+                         </property>
+                         <property name="minimum">
+                          <double>0.001000000000000</double>
+                         </property>
+                         <property name="maximum">
+                          <double>0.100000000000000</double>
+                         </property>
+                         <property name="singleStep">
+                          <double>0.010000000000000</double>
+                         </property>
+                         <property name="value">
+                          <double>0.030000000000000</double>
+                         </property>
+                        </widget>
+                       </item>
+                       <item row="2" column="1">
+                        <widget class="QLabel" name="label_204">
+                         <property name="text">
+                          <string>Epsilon for sub pixel estimation.</string>
+                         </property>
+                         <property name="wordWrap">
+                          <bool>true</bool>
+                         </property>
+                        </widget>
+                       </item>
+                      </layout>
+                     </widget>
+                    </item>
+                   </layout>
+                  </widget>
+                 </item>
+                 <item>
+                  <spacer name="verticalSpacer_29">
+                   <property name="orientation">
+                    <enum>Qt::Vertical</enum>
+                   </property>
+                   <property name="sizeHint" stdset="0">
+                    <size>
+                     <width>20</width>
+                     <height>464</height>
+                    </size>
+                   </property>
+                  </spacer>
+                 </item>
+                </layout>
+               </widget>
                <widget class="QWidget" name="page_12">
                 <layout class="QVBoxLayout" name="verticalLayout_32">
                  <item>

tools/OdometryViewer/main.cpp

@@ -44,8 +44,8 @@ void showUsage()
 			"odometryViewer [options]\n"
 			"Options:\n"
 			"  -driver #                 Driver number to use: 0=OpenNI-PCL, 1=OpenNI2, 2=Freenect, 3=OpenNI-CV, 4=OpenNI-CV-ASUS\n"
-			"  -o #                      Odometry type (default 0): 0=SURF, 1=SIFT, 2=ORB, 3=FAST/FREAK, 4=FAST/BRIEF, 5=GFTT/FREAK, 6=GFTT/BRIEF, 7=BRISK\n"
+			"  -o #                      Odometry type (default 6): 0=SURF, 1=SIFT, 2=ORB, 3=FAST/FREAK, 4=FAST/BRIEF, 5=GFTT/FREAK, 6=GFTT/BRIEF, 7=BRISK\n"
-			"  -nn #                     Nearest neighbor strategy (default 1): kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4\n"
+			"  -nn #                     Nearest neighbor strategy (default 3): kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4\n"
 			"  -nndr #                   Nearest neighbor distance ratio (default 0.7)\n"
 			"  -icp                      Use ICP odometry\n"
 			"  -flow                     Use optical flow odometry.\n"
@@ -55,17 +55,17 @@ void showUsage()
 			"  -clouds #                 Maximum clouds shown (default 10, zero means inf)\n"
 			"  -sec #.#                  Delay (seconds) before reading the database (if set)\n"
 			"\n"
-			"  -in #.#                   Inliers maximum distance, features/ICP (default 0.005 m)\n"
+			"  -in #.#                   Inliers maximum distance, features/ICP (default 0.01 m)\n"
 			"  -max #                    Max features used for matching (default 0=inf)\n"
 			"  -min #                    Minimum inliers to accept the transform (default 20)\n"
 			"  -depth #.#                Maximum features depth (default 5.0 m)\n"
-			"  -i #                      RANSAC/ICP iterations (default 100)\n"
+			"  -i #                      RANSAC/ICP iterations (default 30)\n"
 			"  -r #.#                    Words ratio (default 0.5)\n"
 			"  -lu #                     Linear update (default 0.0 m)\n"
 			"  -au #                     Angular update (default 0.0 radian)\n"
 			"  -reset #                  Reset countdown (default 0 = disabled)\n"
 			"  -gpu                      Use GPU\n"
-			"  -lh #                     Local history (default 0)\n"
+			"  -lh #                     Local history (default 1000)\n"
 			"\n"
 			"  -brief_bytes #        BRIEF bytes (default 32)\n"
 			"  -fast_thr #           FAST threshold (default 30)\n"
@@ -84,7 +84,7 @@ void showUsage()
 			"  odometryViewer -odom 4 -nn 2 -lh 1000                FAST/BRIEF example\n"
 			"  odometryViewer -odom 3 -nn 2 -lh 1000                FAST/FREAK example\n"
 			"  odometryViewer -icp -in 0.05 -i 30                   ICP example\n"
-			"  odometryViewer -flow                                 Optical flow example\n");
+			"  odometryViewer -flow -in 0.02                        Optical flow example\n");
 	exit(1);
 }
 
@@ -97,17 +97,17 @@ int main (int argc, char * argv[])
 	float rate = 0.0;
 	std::string inputDatabase;
 	int driver = 0;
-	int odomType = 0;
+	int odomType = 6;
 	bool icp = false;
 	bool flow = false;
-	int nnType =1;
+	int nnType =3;
 	float nndr = 0.7f;
-	float distance = 0.005;
+	float distance = 0.01;
 	int maxWords = 0;
 	int minInliers = 20;
 	float wordsRatio = 0.5;
 	float maxDepth = 5.0f;
-	int iterations = 100;
+	int iterations = 30;
 	float linearUpdate = 0.0f;
 	float angularUpdate = 0.0f;
 	int resetCountdown = 0;
@@ -120,7 +120,7 @@ int main (int argc, char * argv[])
 	int fastThr = 30;
 	float sec = 0.0f;
 	bool gpu = false;
-	int localHistory = 0;
+	int localHistory = 1000;
 	bool p2p = false;
 
 	for(int i=1; i<argc; ++i)
@@ -683,6 +683,7 @@ int main (int argc, char * argv[])
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomInlierDistance(), uNumber2Str(distance)));
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomMinInliers(), uNumber2Str(minInliers)));
 			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomIterations(), uNumber2Str(iterations)));
+			parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOdomFeatureType(), uNumber2Str(odomType)));
 			odom = new rtabmap::OdometryOpticalFlow(parameters);
 		}
 		else