如何用OpenCV实现3D坐标系及多相机已知位姿坐标系的可视化

嘿，这个问题问到点子上了！我之前做SLAM可视化的时候刚好折腾过这些，来给你一步步讲清楚：

一、基础3D坐标系可视化 #

OpenCV本身是2D图像库，所以3D坐标系的可视化核心是把3D点投影到2D平面，用cv2.projectPoints()这个函数就能搞定。具体步骤如下：

1. 定义3D坐标系的关键节点：原点+三个轴的单位端点
2. 准备相机的内参矩阵和畸变系数（如果没有畸变就用零矩阵）
3. 设定坐标系的位姿（比如默认在世界原点，外参用单位旋转矩阵+零平移向量）
4. 把3D点投影到2D图像，然后用画线函数连接原点和各轴端点

举个Python的代码例子：

import cv2
import numpy as np

# 1. 定义3D坐标系点：原点(0,0,0)，X轴(1,0,0)，Y轴(0,1,0)，Z轴(0,0,1)
axis_3d = np.float32([[0,0,0], [1,0,0], [0,1,0], [0,0,1]]).reshape(-1,3)

# 2. 相机内参（示例值，替换成你的实际内参）
camera_matrix = np.array([[800, 0, 320],
                          [0, 800, 240],
                          [0, 0, 1]], dtype=np.float32)
# 畸变系数（无畸变时用零矩阵）
dist_coeffs = np.zeros((4,1), dtype=np.float32)

# 3. 坐标系的位姿：旋转矩阵R和平移向量t（这里默认在世界原点）
R = np.eye(3, dtype=np.float32)
t = np.zeros((3,1), dtype=np.float32)

# 4. 投影3D点到2D图像
axis_2d, _ = cv2.projectPoints(axis_3d, R, t, camera_matrix, dist_coeffs)
axis_2d = axis_2d.astype(np.int32)

# 5. 创建空白图像并绘制坐标轴
img = np.zeros((480, 640, 3), dtype=np.uint8)
# X轴红色，Y轴绿色，Z轴蓝色
cv2.line(img, tuple(axis_2d[0].ravel()), tuple(axis_2d[1].ravel()), (0,0,255), 2)
cv2.line(img, tuple(axis_2d[0].ravel()), tuple(axis_2d[2].ravel()), (0,255,0), 2)
cv2.line(img, tuple(axis_2d[0].ravel()), tuple(axis_2d[3].ravel()), (255,0,0), 2)

cv2.imshow('3D Coordinate System', img)
cv2.waitKey(0)
cv2.destroyAllWindows()

二、多相机坐标系的可视化 #

当多台相机的位姿（外参R、t，即相机在世界坐标系中的旋转和平移）已知时，核心是把每个相机自身的坐标系转换到世界坐标系，再统一投影到同一个2D视角：

1. 对每个相机，定义它自身坐标系的3D点（原点+三个轴端点，和上面的axis_3d一样）
2. 用相机的外参，把相机坐标系的点转换到世界坐标系：P_world = R @ P_cam.T + t（注意矩阵维度，建议用齐次坐标处理更稳妥）
3. 选择一个参考视角（比如用第一个相机的内参和外参，或者直接用世界视角的虚拟相机），把所有世界坐标系下的点投影到2D
4. 给每个相机的坐标轴用不同颜色或标注区分，避免混淆

代码示例（假设我们有3台相机的外参）：

import cv2
import numpy as np

# 定义相机自身坐标系的3D点
axis_3d = np.float32([[0,0,0], [1,0,0], [0,1,0], [0,0,1]]).reshape(-1,3)

# 相机内参（统一用同一个内参，实际如果相机不同要对应替换）
camera_matrix = np.array([[800, 0, 320],
                          [0, 800, 240],
                          [0, 0, 1]], dtype=np.float32)
dist_coeffs = np.zeros((4,1), dtype=np.float32)

# 示例：3台相机的外参（R是相机到世界的旋转，t是平移）
camera_poses = [
    # 相机1：在世界原点
    (np.eye(3), np.zeros((3,1))),
    # 相机2：在世界坐标系(2,0,0)位置，绕Y轴旋转90度
    (cv2.Rodrigues(np.array([0, np.pi/2, 0]))[0], np.array([[2],[0],[0]])),
    # 相机3：在世界坐标系(0,2,0)位置，绕X轴旋转-90度
    (cv2.Rodrigues(np.array([-np.pi/2, 0, 0]))[0], np.array([[0],[2],[0]]))
]

# 选择参考视角：用世界视角（外参为单位矩阵+零平移）
ref_R = np.eye(3)
ref_t = np.zeros((3,1))

# 创建空白图像
img = np.zeros((480, 640, 3), dtype=np.uint8)
colors = [(0,0,255), (0,255,0), (255,0,0)]  # 每个相机的坐标轴颜色

for idx, (R_cam, t_cam) in enumerate(camera_poses):
    # 1. 把相机坐标系的点转换到世界坐标系
    # 方法：P_world = R_cam * P_cam + t_cam
    axis_world = []
    for point in axis_3d:
        point_world = R_cam @ point.reshape(3,1) + t_cam
        axis_world.append(point_world.ravel())
    axis_world = np.float32(axis_world)
    
    # 2. 投影到参考视角的2D图像
    axis_2d, _ = cv2.projectPoints(axis_world, ref_R, ref_t, camera_matrix, dist_coeffs)
    axis_2d = axis_2d.astype(np.int32)
    
    # 3. 绘制坐标轴
    cv2.line(img, tuple(axis_2d[0].ravel()), tuple(axis_2d[1].ravel()), colors[idx], 2)
    cv2.line(img, tuple(axis_2d[0].ravel()), tuple(axis_2d[2].ravel()), colors[idx], 2)
    cv2.line(img, tuple(axis_2d[0].ravel()), tuple(axis_2d[3].ravel()), colors[idx], 2)
    # 标注相机ID
    cv2.putText(img, f'Cam {idx+1}', tuple(axis_2d[0].ravel()), cv2.FONT_HERSHEY_SIMPLEX, 0.5, colors[idx], 1)

cv2.imshow('Multi-Camera Coordinate Systems', img)
cv2.waitKey(0)
cv2.destroyAllWindows()

三、有没有现成的实现方案？ #

OpenCV官方没有提供一键绘制相机坐标系的内置函数，但社区里有很多成熟的复用方案：

• OpenCV官方的姿态估计示例中包含了3D点投影的逻辑，可以直接扩展使用
• 很多SLAM项目（比如ORB-SLAM、LIO-SAM）的可视化模块里都有相机位姿的绘制代码，你可以参考它们的实现逻辑
• 如果需要更直观的3D可视化，你可以结合matplotlib的3D绘图功能，把所有相机坐标系的点在3D空间中绘制出来，不过纯OpenCV的话，基于2D投影的方式已经足够满足大多数工程需求

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