Python: RGBD转点云

前言

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

原理

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

思路

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

补充：由于上面的逆投影公式使用了矩阵逆和矩阵乘法，因此运算速度较慢。可以使用下面另一种思路： u = f x X Z + c x   v = f y Y Z + c y u=f_x\frac{X}{Z}+cx \\ \ \\ v=f_y\frac{Y}{Z}+cy u=fx​ZX​+cx v=fy​ZY​+cy 可以看出像素横纵坐标u,v分别只与相机x,y坐标（以及Z）相关，因此可以使用下面的公式： X = u − c x f x Z X=\frac{u-cx}{f_x}Z X=fx​u−cx​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