From 991a6036485ab0d6e0e1ed89b977b7394dc7cf79 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Thu, 26 Nov 2015 14:34:06 -0500
Subject: [PATCH] Added util3d::laserScan2dFromPointCloud()

---
 corelib/include/rtabmap/core/util3d.h |  3 +++
 corelib/src/util3d.cpp                | 23 +++++++++++++++++++++++
 2 files changed, 26 insertions(+)

diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h
index 491707a7..a0b9857a 100644
--- a/corelib/include/rtabmap/core/util3d.h
+++ b/corelib/include/rtabmap/core/util3d.h
@@ -127,7 +127,10 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
 					float maxDepth = 0,
 					const Transform & localTransform = Transform::getIdentity());
 
+// return CV_32FC3
 cv::Mat RTABMAP_EXP laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
+// return CV_32FC2
+cv::Mat RTABMAP_EXP laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform = Transform());
 pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform = Transform());
 
 pcl::PointXYZ RTABMAP_EXP projectDisparityTo3D(
diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp
index 2f01cdf8..5afe8b08 100644
--- a/corelib/src/util3d.cpp
+++ b/corelib/src/util3d.cpp
@@ -815,6 +815,29 @@ cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, co
 	return laserScan;
 }
 
+cv::Mat laserScan2dFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud, const Transform & transform)
+{
+	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
+	bool nullTransform = transform.isNull();
+	Eigen::Affine3f transform3f = transform.toEigen3f();
+	for(unsigned int i=0; i<cloud.size(); ++i)
+	{
+		if(!nullTransform)
+		{
+			pcl::PointXYZ pt = pcl::transformPoint(cloud.at(i), transform3f);
+			laserScan.at<cv::Vec2f>(i)[0] = pt.x;
+			laserScan.at<cv::Vec2f>(i)[1] = pt.y;
+		}
+		else
+		{
+			laserScan.at<cv::Vec2f>(i)[0] = cloud.at(i).x;
+			laserScan.at<cv::Vec2f>(i)[1] = cloud.at(i).y;
+		}
+
+	}
+	return laserScan;
+}
+
 pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const cv::Mat & laserScan, const Transform & transform)
 {
 	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3);