Added util3d::laserScanFomrDepthImage() and some refactoring

corelib/src/util3d.cpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UMath.h>
 #include <pcl/io/pcd_io.h>
+#include <pcl/common/transforms.h>
 #include <opencv2/imgproc/imgproc.hpp>
 
 namespace rtabmap
@@ -723,6 +724,143 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
 	return cloud;
 }
 
+pcl::PointCloud<pcl::PointXYZ> laserScanFromDepthImage(
+					const cv::Mat & depthImage,
+					float fx,
+					float fy,
+					float cx,
+					float cy,
+					float maxDepth,
+					const Transform & localTransform)
+{
+	UASSERT(depthImage.type() == CV_16UC1 || depthImage.type() == CV_32FC1);
+	UASSERT(!localTransform.isNull());
+
+	pcl::PointCloud<pcl::PointXYZ> scan;
+	int middle = depthImage.rows/2;
+	if(middle)
+	{
+		scan.resize(depthImage.cols);
+		int oi = 0;
+		for(int i=0; i<depthImage.cols; ++i)
+		{
+			pcl::PointXYZ pt = util3d::projectDepthTo3D(depthImage, i, middle, cx, cy, fx, fy, false);
+			if(pcl::isFinite(pt) && (maxDepth == 0 || pt.z < maxDepth))
+			{
+				if(!localTransform.isIdentity())
+				{
+					pt = util3d::transformPoint(pt, localTransform);
+				}
+				scan[oi++] = pt;
+			}
+		}
+		scan.resize(oi);
+	}
+	return scan;
+}
+
+
+cv::Mat cvtDepthFromFloat(const cv::Mat & depth32F)
+{
+	UASSERT(depth32F.empty() || depth32F.type() == CV_32FC1);
+	cv::Mat depth16U;
+	if(!depth32F.empty())
+	{
+		depth16U = cv::Mat(depth32F.rows, depth32F.cols, CV_16UC1);
+		for(int i=0; i<depth32F.rows; ++i)
+		{
+			for(int j=0; j<depth32F.cols; ++j)
+			{
+				float depth = (depth32F.at<float>(i,j)*1000.0f);
+				unsigned short depthMM = 0;
+				if(depth <= (float)USHRT_MAX)
+				{
+					depthMM = (unsigned short)depth;
+				}
+				depth16U.at<unsigned short>(i, j) = depthMM;
+			}
+		}
+	}
+	return depth16U;
+}
+
+cv::Mat cvtDepthToFloat(const cv::Mat & depth16U)
+{
+	UASSERT(depth16U.empty() || depth16U.type() == CV_16UC1);
+	cv::Mat depth32F;
+	if(!depth16U.empty())
+	{
+		depth32F = cv::Mat(depth16U.rows, depth16U.cols, CV_32FC1);
+		for(int i=0; i<depth16U.rows; ++i)
+		{
+			for(int j=0; j<depth16U.cols; ++j)
+			{
+				float depth = float(depth16U.at<unsigned short>(i,j))/1000.0f;
+				depth32F.at<float>(i, j) = depth;
+			}
+		}
+	}
+	return depth32F;
+}
+
+cv::Mat laserScanFromPointCloud(const pcl::PointCloud<pcl::PointXYZ> & cloud)
+{
+	cv::Mat laserScan(1, (int)cloud.size(), CV_32FC2);
+	for(unsigned int i=0; i<cloud.size(); ++i)
+	{
+		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)
+{
+	UASSERT(laserScan.empty() || laserScan.type() == CV_32FC2);
+
+	pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
+	output->resize(laserScan.cols);
+	for(int i=0; i<laserScan.cols; ++i)
+	{
+		output->at(i).x = laserScan.at<cv::Vec2f>(i)[0];
+		output->at(i).y = laserScan.at<cv::Vec2f>(i)[1];
+	}
+	return output;
+}
+
+pcl::PointCloud<pcl::PointXYZ>::Ptr cvMat2Cloud(
+		const cv::Mat & matrix,
+		const Transform & tranform)
+{
+	UASSERT(matrix.type() == CV_32FC2 || matrix.type() == CV_32FC3);
+	UASSERT(matrix.rows == 1);
+
+	Eigen::Affine3f t = tranform.toEigen3f();
+	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
+	cloud->resize(matrix.cols);
+	if(matrix.channels() == 2)
+	{
+		for(int i=0; i<matrix.cols; ++i)
+		{
+			cloud->at(i).x = matrix.at<cv::Vec2f>(0,i)[0];
+			cloud->at(i).y = matrix.at<cv::Vec2f>(0,i)[1];
+			cloud->at(i).z = 0.0f;
+			cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
+		}
+	}
+	else // channels=3
+	{
+		for(int i=0; i<matrix.cols; ++i)
+		{
+			cloud->at(i).x = matrix.at<cv::Vec3f>(0,i)[0];
+			cloud->at(i).y = matrix.at<cv::Vec3f>(0,i)[1];
+			cloud->at(i).z = matrix.at<cv::Vec3f>(0,i)[2];
+			cloud->at(i) = pcl::transformPoint(cloud->at(i), t);
+		}
+	}
+	return cloud;
+}
+
 // inspired from ROS image_geometry/src/stereo_camera_model.cpp
 pcl::PointXYZ projectDisparityTo3D(
 		const cv::Point2f & pt,