前言

本篇记录Python将RGBD图像转点云的方法

原理

通过针孔相机投影原理：
$$p=\frac{1}{Z}KP$$
逆投影：
$$\begin{gathered} P=Z{K}^{-1}p \\ p:归一化像素坐标 \\ P:相机坐标 \\ K:相机内参矩阵 \end{gathered}$$

思路

首先建立与RGBD图像同shape的像素坐标矩阵，随后逆投影公式将像素坐标矩阵的每个元素转换为相机坐标。

补充：由于上面的逆投影公式使用了矩阵逆和矩阵乘法，因此运算速度较慢。可以使用下面另一种思路：
$$\begin{gathered} u=f_x\frac{X}{Z}+cx \\ v=f_y\frac{Y}{Z}+cy \end{gathered}$$
可以看出像素横纵坐标u,v分别只与相机x,y坐标（以及Z）相关，因此可以使用下面的公式：
$$X=\frac{u-cx}{f_x}Z$$
这样就可以避免矩阵运算

代码

矩阵法：

import cv2
import numpy as np
import open3d
import matplotlib.pyplot as plt
import timedef create_pcd_from_rgbd_using_matrix(rgb_img, depth_img):t1 = time.time()K = np.array([[927.16973877, 0, 651.31506348],[0, 927.36688232, 349.62133789],[0, 0, 1]])xlin = np.arange(rgb_img.shape[1])ylin = np.arange(rgb_img.shape[0])xmap, ymap = np.meshgrid(xlin, ylin)pixel_map = np.stack([xmap, ymap, np.ones_like(xmap)], axis=-1)cam_map = depth_img[:, :, np.newaxis] * (np.linalg.inv(K) @ pixel_map[:, :, :, np.newaxis]).squeeze()pcd_rgb = np.concatenate([cam_map, rgb_img[:, :, ::-1]], axis=-1).reshape([-1, 6])t2 = time.time()print('time: {}'.format(t2 - t1))return pcd_rgb

简化法：

import cv2
import numpy as np
import open3d
import matplotlib.pyplot as plt
import timedef create_pcd_from_rgbd(rgb_img, depth_img):"""*Compute point cloud from depth*"""t1 = time.time()depth = depth_imgrgb = rgb_img[:, :, ::-1]xmap = np.arange(1280)ymap = np.arange(720)xmap, ymap = np.meshgrid(xmap, ymap)points_z = depthpoints_x = (xmap - 651.31506348) * points_z / 927.16973877points_y = (ymap - 349.62133789) * points_z / 927.36688232pcd = np.stack([points_x, points_y, points_z], axis=-1)pcd_rgb = np.concatenate([pcd, rgb], axis=-1).reshape([-1, 6])t2 = time.time()print('time: {}'.format(t2 - t1))return pcd_rgb