From cf6478b6335459c1a71289c22d5fa5c47ea1ad87 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Mon, 18 Sep 2017 21:49:57 -0400
Subject: [PATCH] util3d::transformLaserScan(): supporting 7 channels

---
 corelib/src/CameraThread.cpp      |  6 +++---
 corelib/src/util3d_transforms.cpp | 21 +++++++++++++++++++--
 2 files changed, 22 insertions(+), 5 deletions(-)

diff --git a/corelib/src/CameraThread.cpp b/corelib/src/CameraThread.cpp
index ce20bfd2..fa4151d3 100644
--- a/corelib/src/CameraThread.cpp
+++ b/corelib/src/CameraThread.cpp
@@ -298,7 +298,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 			UASSERT(_scanDecimation >= 1);
 			UTimer timer;
 			pcl::IndicesPtr validIndices(new std::vector<int>);
-			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
 					data,
 					_scanDecimation,
 					_scanMaxDepth,
@@ -317,7 +317,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 				}
 				else if(!cloud->is_dense)
 				{
-					pcl::PointCloud<pcl::PointXYZ>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZ>);
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 					pcl::copyPointCloud(*cloud, *validIndices, *denseCloud);
 					cloud = denseCloud;
 				}
@@ -328,7 +328,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 					{
 						Eigen::Vector3f viewPoint(baseToScan.x(), baseToScan.y(), baseToScan.z());
 						pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, _scanNormalsRadius, viewPoint);
-						pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
+						pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 						pcl::concatenateFields(*cloud, *normals, *cloudNormals);
 						scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse());
 					}
diff --git a/corelib/src/util3d_transforms.cpp b/corelib/src/util3d_transforms.cpp
index 9cd97f6a..cd3d3501 100644
--- a/corelib/src/util3d_transforms.cpp
+++ b/corelib/src/util3d_transforms.cpp
@@ -38,7 +38,7 @@ namespace util3d
 
 cv::Mat transformLaserScan(const cv::Mat & laserScan, const Transform & transform)
 {
-	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6));
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2 || laserScan.type() == CV_32FC3 || laserScan.type() == CV_32FC(4) || laserScan.type() == CV_32FC(5) || laserScan.type() == CV_32FC(6) || laserScan.type() == CV_32FC(7));
 
 	cv::Mat output = laserScan.clone();
 
@@ -81,7 +81,7 @@ cv::Mat transformLaserScan(const cv::Mat & laserScan, const Transform & transfor
 				out[3] = pt.normal_y;
 				out[4] = pt.normal_z;
 			}
-			else // 6 and 7 channels
+			else if(laserScan.type() == CV_32FC(6))
 			{
 				pcl::PointNormal pt;
 				pt.x=ptr[0];
@@ -98,6 +98,23 @@ cv::Mat transformLaserScan(const cv::Mat & laserScan, const Transform & transfor
 				out[4] = pt.normal_y;
 				out[5] = pt.normal_z;
 			}
+			else // 7 channels
+			{
+				pcl::PointNormal pt;
+				pt.x=ptr[0];
+				pt.y=ptr[1];
+				pt.z=ptr[2];
+				pt.normal_x=ptr[4];
+				pt.normal_y=ptr[5];
+				pt.normal_z=ptr[6];
+				pt = util3d::transformPoint(pt, transform);
+				out[0] = pt.x;
+				out[1] = pt.y;
+				out[2] = pt.z;
+				out[4] = pt.normal_x;
+				out[5] = pt.normal_y;
+				out[6] = pt.normal_z;
+			}
 		}
 	}
 	return output;