深度图像转换为点云数据计算原理及代码实现

1.开发环境

-Visual Studio2017
-PCL1.9.0
关于VS2017下配置PCL相关环境的方法可以参考文章：
链接: VS2017配置PCL1.9（win10环境)

2. 深度图转点云计算原理

深度图转为点云说白了其实就是坐标系的变换：图像坐标系转换为世界坐标系。变换的约束条件就是相机内参，公式很简单：

其中x,y,z是点云坐标系，x’,y’是图像坐标系，Ｄ为深度值。下面介绍其推导的过程。
首先，要了解下世界坐标到图像的映射过程，考虑世界坐标点M(Xw,Yw,Zw)映射到图像点m(u,v)的过程，如下图所示：

形式化表示如下：

u,v分别为图像坐标系下的任意坐标点。u0,v0分别为图像的中心坐标。xw,yw,zw表示世界坐标系下的三维坐标点。zc表示相机坐标的z轴值，即目标到相机的距离。R,T分别为外参矩阵的3x3旋转矩阵和3x1平移矩阵。

对外参矩阵的设置：由于世界坐标原点和相机原点是重合的，即没有旋转和平移，所以:

相机坐标系和世界坐标系的坐标原点重合，因此相机坐标和世界坐标下的同一个物体具有相同的深度，即zc=zw.于是公式可进一步简化为：

从以上的变换矩阵公式，可以计算得到图像点[u,v]T 到世界坐标点[xw,yw,zw]T的变换公式:

3.代码实现

3.1 头文件Depth_TO_PointCloud.h

#include <iostream>
#include <string>

#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui/highgui.hpp"

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
using namespace std;
using namespace cv;

namespace DepthToPCL
{
	int Depth_TO_PointCloud();
}

3.2Depth_TO_PointCloud.cpp

#include "Depth_TO_PointCloud.h"
using namespace DepthToPCL;

// 定义点云类型
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloud;

// 相机内参
const double camera_factor = 1000;
const double camera_cx = 623.827;// 325.5;
const double camera_cy = 354.457;// 253.5;
const double camera_fx = 699.751;// 518.0;
const double camera_fy = 699.751;// 519.0;

// 主函数
int DepthToPCL::Depth_TO_PointCloud()
{
	// 读取./data/rgb.png和./data/depth.png，并转化为点云

	// 图像矩阵
	cv::Mat rgb, depth;
	// 使用cv::imread()来读取图像
	// API: http: //docs.opencv.org/modules/highgui/doc/reading_and_writing_images_and_video.html?highlight=imread#cv2.imread
	rgb = cv::imread("RGB/0.335200.png");
	// rgb 图像是8UC3的彩色图像
	// depth 是16UC1的单通道图像，注意flags设置-1,表示读取原始数据不做任何修改
	depth = cv::imread("Depth/0.335166.png", -1);

	// 点云变量
	// 使用智能指针，创建一个空点云。这种指针用完会自动释放。
	PointCloud::Ptr cloud(new PointCloud);
	// 遍历深度图
	for (int m = 0; m < depth.rows; m++)
		for (int n = 0; n < depth.cols; n++)
		{
			// 获取深度图中(m,n)处的值
			unsigned short d = depth.ptr<unsigned short>(m)[n];
			// d 可能没有值，若如此，跳过此点
			if (d == 0)
				continue;
			// d 存在值，则向点云增加一个点
			PointT p;

			// 计算这个点的空间坐标
			p.z = double(d) / camera_factor;
			p.x = (n - camera_cx) * p.z / camera_fx;
			p.y = (m - camera_cy) * p.z / camera_fy;

			// 从rgb图像中获取它的颜色
			// rgb是三通道的BGR格式图，所以按下面的顺序获取颜色
			p.b = rgb.ptr<uchar>(m)[n * 3];
			p.g = rgb.ptr<uchar>(m)[n * 3 + 1];
			p.r = rgb.ptr<uchar>(m)[n * 3 + 2];

			// 把p加入到点云中
			cloud->points.push_back(p);
		}
	// 设置并保存点云
	cloud->height = 1;
	cloud->width = cloud->points.size();
	cout << "point cloud size = " << cloud->points.size() << endl;
	cloud->is_dense = false;
	pcl::io::savePCDFile("PCD/0.335166.pcd", *cloud);
	// 清除数据并退出
	cloud->points.clear();
	cout << "Point cloud saved." << endl;
}