如何使用pykinect完成已标定Kinect的深度图与彩色图融合

依赖准备 #

• 已标定参数：深度相机内参(Fx_d, Fy_d, Cx_d, Cy_d)、彩色相机内参(Fx_rgb, Fy_rgb, Cx_rgb, Cy_rgb)、深度坐标系到彩色坐标系的旋转矩阵R、平移向量T，若你获取的R/T是彩色到深度的变换，需要先求逆得到反向变换参数
• 第三方库：安装pykinect2（适配Kinect v2，若为v1版本安装对应pykinect包）、numpy、opencv-python

核心实现流程 #

1. 同步获取深度帧与彩色帧 #

通过pykinect接口同步拉取对齐时间戳的深度帧、彩色帧，避免帧间运动导致的配准误差。Kinect v2默认输出深度帧分辨率为512×424、彩色帧为1920×1080，深度值单位为毫米，后续计算需转为米。
示例初始化代码：

import numpy as np
import cv2
from pykinect2 import PyKinectV2
from pykinect2.PyKinectRuntime import PyKinectRuntime

# 初始化运行时，同时启用深度和彩色流
kinect = PyKinectRuntime(PyKinectV2.FrameSourceTypes_Depth | PyKinectV2.FrameSourceTypes_Color)
depth_width, depth_height = kinect.depth_frame_desc.Width, kinect.depth_frame_desc.Height
color_width, color_height = kinect.color_frame_desc.Width, kinect.color_frame_desc.Height

2. 像素坐标映射计算 #

对深度图上的每一个有效像素点，依次完成三次坐标变换：

1. 深度像素转深度相机三维坐标
   对于深度图坐标(u_d, v_d)，对应深度值为z（毫米转米：z = depth_val / 1000）：

X_d = (u_d - Cx_d) * z / Fx_d
Y_d = (v_d - Cy_d) * z / Fy_d
Z_d = z

2. 深度坐标系转彩色坐标系
   利用标定得到的R、T做刚体变换：

point_d = np.array([X_d, Y_d, Z_d]).reshape(3,1)
point_rgb = R.dot(point_d) + T.reshape(3,1)
X_r, Y_r, Z_r = point_rgb.flatten()

3. 彩色坐标系三维点转彩色图像素坐标

u_r = int(round((X_r * Fx_rgb / Z_r) + Cx_rgb))
v_r = int(round((Y_r * Fy_rgb / Z_r) + Cy_rgb))

3. 生成融合图像 #

遍历所有深度像素后，过滤掉超出彩色图边界的无效点，即可生成对齐后的融合结果：

• 若需要和彩色图同分辨率的对齐深度图：创建和彩色图同尺寸的空矩阵，将计算得到的(u_r, v_r)位置赋值对应深度值
• 若需要带深度信息的彩色图：创建和深度图同尺寸的空RGB矩阵，将(u_d, v_d)位置赋值彩色图(u_r, v_r)处的RGB值
  示例映射代码：

# 这里替换为你自己标定的参数
Fx_d, Fy_d, Cx_d, Cy_d = 367.23, 367.23, 256.12, 209.45 # 示例深度内参
Fx_rgb, Fy_rgb, Cx_rgb, Cy_rgb = 1081.37, 1081.37, 959.5, 539.5 # 示例彩色内参
R = np.array([[0.9999, 0.0087, -0.0052], [-0.0087, 0.9999, 0.0013], [0.0052, -0.0012, 0.9999]]) # 示例R
T = np.array([0.052, -0.0002, -0.011]) # 示例T，单位米

while True:
    if kinect.has_new_depth_frame() and kinect.has_new_color_frame():
        # 获取深度帧、彩色帧
        depth_frame = kinect.get_last_depth_frame().reshape(depth_height, depth_width)
        color_frame = kinect.get_last_color_frame().reshape(color_height, color_width, 4)[:,:,:3] # 丢弃alpha通道
        color_frame = cv2.cvtColor(color_frame, cv2.COLOR_BGRA2BGR)

        # 初始化对齐后的彩色图（和深度同分辨率）
        aligned_color = np.zeros((depth_height, depth_width, 3), dtype=np.uint8)

        # 遍历深度像素
        for v_d in range(depth_height):
            for u_d in range(depth_width):
                z = depth_frame[v_d, u_d]
                if z == 0: # 跳过无效深度点
                    continue
                z_m = z / 1000.0
                # 深度像素转三维
                X_d = (u_d - Cx_d) * z_m / Fx_d
                Y_d = (v_d - Cy_d) * z_m / Fy_d
                # 转彩色坐标系
                point_rgb = R @ np.array([X_d, Y_d, z_m]) + T
                X_r, Y_r, Z_r = point_rgb
                if Z_r <= 0: # 跳过负深度无效点
                    continue
                # 转彩色像素
                u_r = int(round((X_r * Fx_rgb / Z_r) + Cx_rgb))
                v_r = int(round((Y_r * Fy_rgb / Z_r) + Cy_rgb))
                # 边界判断
                if 0 <= u_r < color_width and 0 <= v_r < color_height:
                    aligned_color[v_d, u_d] = color_frame[v_r, u_r]
        
        # 显示结果
        cv2.imshow('Aligned RGB-D', aligned_color)
        if cv2.waitKey(1) == ord('q'):
            break

cv2.destroyAllWindows()
kinect.close()

优化与问题排查 #

• 实时性能优化：上述逐像素遍历的方式速度较慢，可改用numpy向量化运算替代循环，运行速度可提升10倍以上
• 错位问题排查：确认R/T的变换方向是否为深度到彩色，内参对应的相机是否正确，深度值单位是否统一为米
• 边缘空洞问题：深度相机视场小于彩色相机时边缘会出现无RGB值的空洞，可通过形态学填充或插值优化

内容的提问来源于stack exchange，提问作者Lee Edition