Oak-D相机深度图与彩色图配准问题求助

你之前使用单应矩阵H = K2 * R * inv(K1)的方法只适用于平面场景，无法处理3D场景中的平移影响——因为单应性假设所有点都在同一个平面上，而深度图的每个像素对应不同深度的3D点，平移会让不同深度的点投影到彩色图的位置偏移量不同，必须通过3D点重投影来完成配准。

核心原理步骤 #

• 将深度图的像素坐标转换为深度相机坐标系下的3D点
• 通过外参矩阵将深度相机的3D点变换到彩色相机坐标系
• 将变换后的3D点投影到彩色图像平面，得到对齐后的深度像素坐标
• 插值填充深度值，生成与彩色图对齐的深度图

修改后的完整代码 #

import cv2
import numpy as np
from google.colab.patches import cv2_imshow

# 彩色相机内参
K2 = np.array([[553.0378462082966, 0, 355.449626693058],
              [0, 555.6255112161116, 197.16318449210289],
              [0, 0, 1]])

# 深度相机（右相机）内参
K1 = np.array([[426.6990467952769, 0, 319.5763569688426],
              [0, 428.2569124831735, 203.66385041327385],
              [0, 0, 1]])

# 彩色相机畸变系数
distortion_coeffs = np.array([-0.01015686833772626, 0.018782266649409993, 0.0015606228293145892, -0.00019459459264923275])

# 深度相机到彩色相机的齐次变换矩阵（D: 右相机→彩色相机）
D = np.array([[0.9999971496369501, 0.00012642174287522421, -0.0023842683365914922, -0.07501121652962993/2],
            [-0.00012887613805504113, 0.9999994619796904, -0.0010292867693778803, -4.5329260960026137e-05],
            [0.002384136929579421, 0.0010295911108322208, 0.9999966279109286, -0.0005594943499687595],
            [0, 0, 0, 1]])

# 加载图像
depth = cv2.imread("/content/drive/MyDrive/RGBD_images/img_00001_dep.png", cv2.IMREAD_GRAYSCALE)
color = cv2.imread("/content/drive/MyDrive/RGBD_images/img_00001_rgb.jpg")

# 彩色图像去畸变
height_color, width_color = color.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(K2, distortion_coeffs, (width_color, height_color), 0, (width_color, height_color))
color_undistorted = cv2.undistort(color, K2, distortion_coeffs, None, newcameramtx)

# 深度图尺寸
height_depth, width_depth = depth.shape

# --------------------- 核心配准逻辑 ---------------------
# 1. 生成深度图的像素网格坐标（u, v）
u_depth, v_depth = np.meshgrid(np.arange(width_depth), np.arange(height_depth))

# 2. 将深度图像素转换为深度相机坐标系下的3D点（单位：米，假设深度图单位是毫米，需转换）
depth_values = depth.astype(np.float32) / 1000.0  # 若深度单位是毫米则除以1000，根据实际调整
fx1, fy1, cx1, cy1 = K1[0,0], K1[1,1], K1[0,2], K1[1,2]

X1 = (u_depth - cx1) * depth_values / fx1
Y1 = (v_depth - cy1) * depth_values / fy1
Z1 = depth_values

# 3. 将3D点转换为齐次坐标，并用外参矩阵变换到彩色相机坐标系
points_homogeneous = np.stack([X1.flatten(), Y1.flatten(), Z1.flatten(), np.ones_like(X1.flatten())], axis=0)
points_color_cam = D @ points_homogeneous
X2, Y2, Z2 = points_color_cam[0,:], points_color_cam[1,:], points_color_cam[2,:]

# 4. 将彩色相机坐标系的3D点投影到彩色图像平面
fx2, fy2, cx2, cy2 = K2[0,0], K2[1,1], K2[0,2], K2[1,2]
u_color = (fx2 * X2 / Z2) + cx2
v_color = (fy2 * Y2 / Z2) + cy2

# 5. 生成映射表，用于重映射深度图
map_x = u_color.reshape(height_depth, width_depth).astype(np.float32)
map_y = v_color.reshape(height_depth, width_depth).astype(np.float32)

# 6. 重映射深度图到彩色图像尺寸，使用双线性插值
aligned_depth = cv2.remap(depth, map_x, map_y, interpolation=cv2.INTER_LINEAR, borderMode=cv2.BORDER_CONSTANT, borderValue=0)
# 调整深度图尺寸到彩色图大小（如果需要）
aligned_depth = cv2.resize(aligned_depth, (width_color, height_color))

# --------------------- 可视化与保存 ---------------------
depth_colormap = cv2.applyColorMap(cv2.convertScaleAbs(aligned_depth, alpha=0.03), cv2.COLORMAP_JET)
aligned_image = cv2.addWeighted(color_undistorted, 0.5, depth_colormap, 0.5, 0)

# 显示结果
cv2_imshow(color_undistorted)
cv2_imshow(depth)
cv2_imshow(aligned_image)

# 保存对齐后的图像
cv2.imwrite("/content/drive/MyDrive/selected_RGBD_images/aligned_depth_0000.jpg", aligned_image)

关键说明 #

• 深度单位：代码中假设深度图单位是毫米，需根据你的实际数据调整（如果是米则去掉除以1000的步骤）
• 外参矩阵方向：确认D是深度相机（右相机）到彩色相机的变换矩阵，若方向相反则需要取逆矩阵
• 插值方式：使用cv2.INTER_LINEAR双线性插值保证深度图平滑，也可以根据需求改用INTER_NEAREST
• 去畸变：彩色图像已经做了去畸变，深度图如果有畸变也需要先做去畸变处理（如果你的深度图是未畸变的则跳过）

内容的提问来源于stack exchange，提问作者Tiago Rodrigues