From 8e55d7847f4509c3da0e76676e7713d650a9f2f3 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@f169173b-cf89-36c8-b27e-44dbe73f0c83>
Date: Wed, 22 Oct 2014 20:33:26 +0000
Subject: [PATCH] added util3d::cloudFromDisparity()

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1915 f169173b-cf89-36c8-b27e-44dbe73f0c83
---
 corelib/include/rtabmap/core/util3d.h |  6 ++++
 corelib/src/util3d.cpp                | 44 +++++++++++++++++++++++++++
 2 files changed, 50 insertions(+)

diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h
index e1cbefed..89492472 100644
--- a/corelib/include/rtabmap/core/util3d.h
+++ b/corelib/include/rtabmap/core/util3d.h
@@ -210,6 +210,12 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDepthRGB(
 		float fx, float fy,
 		int decimation = 1);
 
+pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromDisparity(
+		const cv::Mat & imageDisparity,
+		float cx, float cy,
+		float fx, float baseline,
+		int decimation);
+
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudFromDisparityRGB(
 		const cv::Mat & imageRgb,
 		const cv::Mat & imageDisparity,
diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp
index 95082cd8..017a1190 100644
--- a/corelib/src/util3d.cpp
+++ b/corelib/src/util3d.cpp
@@ -894,6 +894,50 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDepthRGB(
 	return cloud;
 }
 
+pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
+		const cv::Mat & imageDisparity,
+		float cx, float cy,
+		float fx, float baseline,
+		int decimation)
+{
+	UASSERT(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1);
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	if(decimation < 1)
+	{
+		return cloud;
+	}
+
+	//cloud.header = cameraInfo.header;
+	cloud->height = imageDisparity.rows/decimation;
+	cloud->width  = imageDisparity.cols/decimation;
+	cloud->is_dense = false;
+	cloud->resize(cloud->height * cloud->width);
+
+	if(imageDisparity.type()==CV_16SC1)
+	{
+		for(int h = 0; h < imageDisparity.rows && h/decimation < (int)cloud->height; h+=decimation)
+		{
+			for(int w = 0; w < imageDisparity.cols && w/decimation < (int)cloud->width; w+=decimation)
+			{
+				float disp = float(imageDisparity.at<short>(h,w))/16.0f;
+				cloud->at((h/decimation)*cloud->width + (w/decimation)) = projectDisparityTo3D(cv::Point2f(w, h), disp, cx, cy, fx, baseline);
+			}
+		}
+	}
+	else
+	{
+		for(int h = 0; h < imageDisparity.rows && h/decimation < (int)cloud->height; h+=decimation)
+		{
+			for(int w = 0; w < imageDisparity.cols && w/decimation < (int)cloud->width; w+=decimation)
+			{
+				float disp = imageDisparity.at<float>(h,w);
+				cloud->at((h/decimation)*cloud->width + (w/decimation)) = projectDisparityTo3D(cv::Point2f(w, h), disp, cx, cy, fx, baseline);
+			}
+		}
+	}
+	return cloud;
+}
+
 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
 		const cv::Mat & imageRgb,
 		const cv::Mat & imageDisparity,