一、问题描述

三维重建旨在将三维物体表示为在计算机中易于处理的数学模型，近年来由于计算机硬件的快速发展而被大规模应用于自动驾驶、VR／AR、医疗、古建筑数字化保护等领域。其中，点云数据作为最常用的数学模型，指一组向量在一个三维坐标系下的集合，这些向量通常以X、Y、Z三维坐标的形式表示，通常代表一个物体的外表面几何形状，除此之外点云数据还可以附带RGB信息，即每个坐标点的颜色信息，或者是其他的信息。深度相机通常被应用于获取场景深度信息，以Kinect一二代为主的主动式传感器和Realsense D435为主的被动式传感器被广泛应用。本文主从主动式RGBD传感器出发，分别介绍针孔相机模型和RGBD相机的测距原理，并基于TUM数据集中一对匹配的RGBD图像来实现点云PCD生成。

二、传感器模型

1.针孔相机模型

二维相机将三维坐标点映射到二维图像平面，其中针孔相机模型是最常用且最有效的模型，它描述三维坐标点在相机坐标系和像素坐标系下表征之间的转换关系，其示意图和数学表示如下：

针孔相机模型

?数学模型

其中，(u,v)和(X,Y,Z)分别表示空间点在像素坐标系和相机坐标系下的坐标，公式中间的矩阵被称之为相机的内参数矩阵K，由公式可知，点的深度在投影过程中丢失了，即根据二维图像中的像素坐标和相机内参无法推导出该点的三维空间坐标，因此需要结合深度测距和双目测距技术来获取深度信息。

2.深度测距

RGBD相机采用深度测距技术来获取空间点和相机坐标系之间的距离信息并生成深度图像，同时结合传感器标定技术来对彩色图像和深度图像进行对齐，以便后续点云生成及定位建图方面的应用。目前的RGBD相机按照原理可分为以Kinect 1代为代表的红外结构光(Structured Light)原理相机和以Kinect 2代为代表的飞行时间(Time-of-Flight, TOF)原理相机。

红外结构光测距

无论是哪种类型的测距原理，RGB-D相机都需要向探测目标发射一束光线，区别在于结构光相机通过返回的结构光图案来计算物体距离，而TOF相机根据脉冲光从发射到返回的的光束飞行时间来感知深度。

代码实现

本文的代码实现环境为Ubuntu 18.04 + OpenCV 3.4.13 + PCL 1.12.1 + CMake 3.10.2，基于TUM数据集中的RGBD图像生成一帧点云数据。TUM数据集的下载地址为：

Computer Vision Group - Dataset Download (tum.de)https://vision.in.tum.de/data/datasets/rgbd-dataset/download对应的彩色图和深度图文件为fr3/cabinet序列下的1341841599.918690.png和1341841599.918730.png。

算法步骤：

1.读取彩色图、深度图与相机内参

2.遍历深度图中的每个像素点，通过内参转换为相机坐标系下的三维坐标

3.将对应的RGB数值赋值给每一个三维坐标

4.将三维空间点集保存为pcd格式数据

注：由于TUM数据集中的RGBD已经完成了配准，图像的坐标系和分辨率均一致，无需进行额外的像素匹配。

#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <pcl/point_types.h> 
#include <pcl/io/pcd_io.h> 
using namespace std;

int main( int argc, char** argv )
{
    // 数据读取
    cv::Mat rgb, depth;
    rgb = cv::imread("../data/rgb.png");
    depth = cv::imread("../data/depth.png", -1);
    
    // 相机内参与缩放系数 
    double cx = 319.5;
    double cy = 239.5;
    double fx = 525.0;
    double fy = 525.0;
    double depthScale = 5000.0;
    
    // 定义点云使用的格式：这里用的是XYZRGB
    typedef pcl::PointXYZRGB PointT; 
    typedef pcl::PointCloud<PointT> PointCloud;
    
    // 计算每个点对应的XYZRGB值
    PointCloud::Ptr pointCloud(new PointCloud);
    for ( int v=0; v<rgb.rows; v++ )
        for (int u=0; u<rgb.cols; u++)
        {
            unsigned int d = depth.ptr<unsigned short>(v)[u];
            if (d==0)
                continue;
            PointT p;
            p.z = double(d)/depthScale;
            p.x = (u-cx)*p.z/fx;
            p.y = (v-cy)*p.z/fy;
            p.b = rgb.data[v*rgb.step+u*rgb.channels()];
            p.g = rgb.data[v*rgb.step+u*rgb.channels()+1];
            p.r = rgb.data[v*rgb.step+u*rgb.channels()+2];
            pointCloud->points.push_back(p);
        }
        
    // 点云保存
    pointCloud->is_dense = false;
    cout << "点云共有" << pointCloud->size() << "个点." << endl;
    pcl::io::savePCDFileBinary("../data/cabinet.pcd", *pointCloud );
    return 0;
}

rgbd2pcd.cpp

# cmake版本及工程名
cmake_minimum_required( VERSION 2.8 )
project( rgbd2pcd )

# 设置opencv库
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_INCLUDE_DIRS} )

# 设置pcl库 
find_package( PCL REQUIRED)
include_directories( ${PCL_INCLUDE_DIRS} )

# 编译与链接
add_executable( rgbd2pcd rgbd2pcd.cpp )
target_link_libraries( rgbd2pcd ${OpenCV_LIBS} ${PCL_LIBRARIES} )