Python: RGBD转点云

前言

本篇记录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 time


def 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 time


def create_pcd_from_rgbd(rgb_img, depth_img):
    """
    *Compute point cloud from depth*
    """
    t1 = time.time()
    depth = depth_img
    rgb = rgb_img[:, :, ::-1]
    xmap = np.arange(1280)
    ymap = np.arange(720)
    xmap, ymap = np.meshgrid(xmap, ymap)
    points_z = depth
    points_x = (xmap - 651.31506348) * points_z / 927.16973877
    points_y = (ymap - 349.62133789) * points_z / 927.36688232
    pcd = 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