4. 深度图与彩色图对齐

任务目标

将ToF相机输出的深度图与RGB相机输出的彩色图进行像素级对齐，生成同一视角下深度与颜色一一对应的图像对，为后续点云着色或三维重建提供对齐后的输入数据。

核心原理与实现思路

深度图与彩色图对齐的实质是：利用两个相机各自的内参以及二者之间的外参（旋转矩阵R、平移向量t），将深度图中的每个像素反投影到三维空间，再通过外参变换到RGB相机坐标系下，最后用RGB相机内参投影到彩色图像平面，得到该深度像素对应的彩色像素坐标。由于投影坐标通常为浮点数，需通过双线性插值采样彩色值，并对超出彩色图边界的深度像素做无效标记。

依赖与环境

• Python 3.8+，NumPy，OpenCV（用于图像读写与插值）
• 输入数据：深度图（16bit PNG，单位mm）、彩色图（8bit BGR）、相机参数文件（JSON格式）

关键完整代码

import numpy as np
import cv2
import json

def align_depth_to_color(depth, color, K_depth, K_color, R, t):
    """
    将深度图对齐到彩色图视角
    depth: (Hd, Wd) uint16, 单位mm
    color: (Hc, Wc, 3) uint8, BGR
    K_depth, K_color: 3x3 内参矩阵
    R: 3x3, 从深度相机到彩色相机的旋转
    t: (3,), 从深度相机到彩色相机的平移
    返回: aligned_depth (Hc, Wc) float32, 无效像素为0
          aligned_color (Hc, Wc, 3) uint8, 无效像素为黑色
    """
    Hd, Wd = depth.shape
    Hc, Wc = color.shape[:2]

    # 1. 生成深度图像素网格
    v, u = np.meshgrid(np.arange(Hd), np.arange(Wd), indexing='ij')
    z = depth.astype(np.float32) / 1000.0  # mm -> m
    valid = z > 0

    # 2. 反投影到深度相机三维坐标
    x = (u - K_depth[0, 2]) * z / K_depth[0, 0]
    y = (v - K_depth[1, 2]) * z / K_depth[1, 1]
    pts_depth = np.stack([x, y, z], axis=-1)  # (Hd, Wd, 3)

    # 3. 变换到彩色相机坐标系
    pts_color = pts_depth @ R.T + t  # (Hd, Wd, 3)

    # 4. 投影到彩色图像平面
    xc = pts_color[..., 0] / pts_color[..., 2]
    yc = pts_color[..., 1] / pts_color[..., 2]
    uc = xc * K_color[0, 0] + K_color[0, 2]
    vc = yc * K_color[1, 1] + K_color[1, 2]

    # 5. 判断投影后是否在彩色图范围内
    in_bounds = (uc >= 0) & (uc < Wc - 1) & (vc >= 0) & (vc < Hc - 1) & valid & (pts_color[..., 2] > 0)

    # 6. 双线性插值采样彩色值
    map_x = uc.astype(np.float32)
    map_y = vc.astype(np.float32)
    sampled_color = cv2.remap(color, map_x, map_y, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT, borderValue=(0, 0, 0))

    # 7. 生成对齐后的深度图（在彩色图分辨率下）
    aligned_depth = np.zeros((Hc, Wc), dtype=np.float32)
    # 使用最近邻将深度值映射到彩色分辨率
    uc_int = np.round(uc).astype(int)
    vc_int = np.round(vc).astype(int)
    mask = in_bounds & (uc_int >= 0) & (uc_int < Wc) & (vc_int >= 0) & (vc_int < Hc)
    aligned_depth[vc_int[mask], uc_int[mask]] = depth[mask]

    # 无效区域置零
    aligned_color = sampled_color.copy()
    aligned_color[~in_bounds] = 0

    return aligned_depth, aligned_color

# ========== 使用示例 ==========
if __name__ == "__main__":
    # 加载相机参数（示例结构，实际以标定结果为准）
    with open("camera_params.json") as f:
        params = json.load(f)
    K_depth = np.array(params["K_depth"])
    K_color = np.array(params["K_color"])
    R = np.array(params["R"])
    t = np.array(params["t"])

    # 读取图像
    depth = cv2.imread("depth.png", cv2.IMREAD_UNCHANGED)
    color = cv2.imread("color.png", cv2.IMREAD_COLOR)

    aligned_depth, aligned_color = align_depth_to_color(depth, color, K_depth, K_color, R, t)

    # 保存结果
    cv2.imwrite("aligned_depth.png", (aligned_depth * 1000).astype(np.uint16))
    cv2.imwrite("aligned_color.png", aligned_color)

运行与验证

1. 准备数据：将深度图、彩色图及标定得到的相机参数（内参、外参）放在同一目录下，按代码中路径命名。
2. 执行：运行脚本 python align_depth_color.py，生成 aligned_depth.png 与 aligned_color.png。
3. 验证方法：用图像查看工具打开两张对齐结果，检查以下两点：
   • 对齐后的彩色图中，物体边缘与深度图边缘是否基本重合（无严重错位）；
   • 在深度有效区域，彩色图与深度图内容是否对应同一场景视角。
4. 定量验证（可选）：选取场景中一个已知距离的平面区域，检查对齐后深度值是否与真实距离一致（误差应在传感器标称精度范围内）。

常见问题

• 对齐后出现黑色条纹：通常由深度图无效像素（值为0）或投影超出彩色图边界导致，属正常现象，可通过深度图预处理（如空洞填充）改善。
• 边缘重影：多因深度图与彩色图分辨率差异大或外参标定精度不足，建议先确认标定参数准确性。
• 运行速度慢：当前实现为逐像素向量化计算，若需实时处理，可改用GPU或降低处理分辨率。