From 3cf21a7e7f0faa2495ac97a82175ba5dd4076150 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Fri, 24 Feb 2017 15:26:42 -0500
Subject: [PATCH] fixed laserScanFromDepthImage order (counterclockwise)

---
 corelib/include/rtabmap/core/util3d.h | 8 ++++++++
 corelib/src/util3d.cpp                | 4 ++--
 2 files changed, 10 insertions(+), 2 deletions(-)

diff --git a/corelib/include/rtabmap/core/util3d.h b/corelib/include/rtabmap/core/util3d.h
index aa5c9c9c..95a3d2ac 100644
--- a/corelib/include/rtabmap/core/util3d.h
+++ b/corelib/include/rtabmap/core/util3d.h
@@ -167,6 +167,9 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 		const ParametersMap & stereoParameters = ParametersMap(),
 		const std::vector<float> & roiRatios = std::vector<float>()); // ignored for stereo
 
+/**
+ * Simulate a laser scan rotating counterclockwise, using middle line of the depth image.
+ */
 pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
 					const cv::Mat & depthImage,
 					float fx,
@@ -176,6 +179,11 @@ pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImage(
 					float maxDepth = 0,
 					float minDepth = 0,
 					const Transform & localTransform = Transform::getIdentity());
+/**
+ * Simulate a laser scan rotating counterclockwise, using middle line of the depth images.
+ * The last value of the scan is the most left value of the first depth image. The first value of the scan is the most right value of the last depth image.
+ *
+ */
 pcl::PointCloud<pcl::PointXYZ> RTABMAP_EXP laserScanFromDepthImages(
 		const cv::Mat & depthImages,
 		const std::vector<CameraModel> & cameraModels,
diff --git a/corelib/src/util3d.cpp b/corelib/src/util3d.cpp
index 340ab835..82b8cff3 100644
--- a/corelib/src/util3d.cpp
+++ b/corelib/src/util3d.cpp
@@ -1109,7 +1109,7 @@ pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
 	{
 		scan.resize(depthImage.cols);
 		int oi = 0;
-		for(int i=0; i<depthImage.cols; ++i)
+		for(int i=depthImage.cols-1; i>=0; --i)
 		{
 			pcl::PointXYZ pt = util3d::projectDepthTo3D(depthImage, i, middle, cx, cy, fx, fy, false);
 			if(pcl::isFinite(pt) && pt.z >= minDepth && (maxDepth == 0 || pt.z < maxDepth))
@@ -1135,7 +1135,7 @@ pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImages(
 	pcl::PointCloud<pcl::PointXYZ> scan;
 	UASSERT(int((depthImages.cols/cameraModels.size())*cameraModels.size()) == depthImages.cols);
 	int subImageWidth = depthImages.cols/cameraModels.size();
-	for(unsigned int i=0; i<cameraModels.size(); ++i)
+	for(int i=(int)cameraModels.size()-1; i>=0; --i)
 	{
 		if(cameraModels[i].isValidForProjection())
 		{