fixed laserScanFromDepthImage order (counterclockwise)

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,

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