如何用OpenCV结合3台相机实现姿态关键点的3D三角化？

问题背景 #

使用3台同步固定相机记录有限空间内的个体运动，通过预训练模型检测每帧20余个姿态关键点，目标是用OpenCV将2D关键点转换为统一坐标系下的3D点云。目前已实现双相机三角化，但两两相机得到的3D点集因坐标系不同无法合并，尝试用cv2.recoverPose()和cv2.solvePnP()合并失败。

核心解决方案 #

1. 统一所有相机的外参到同一世界坐标系 #

选择其中一台相机（如相机1）作为世界坐标系原点，计算另外两台相机相对该原点的外参矩阵，确保所有投影矩阵都基于同一坐标系：

• 相机1的外参：M1 = np.hstack((np.eye(3), np.zeros((3,1))))（自身为原点）
• 相机2相对相机1的外参：通过cv2.recoverPose()得到的R2, t2，即M2 = np.hstack((R2, t2))
• 相机3相对相机1的外参：直接用相机1和3的匹配点计算R3, t3，避免通过相机2中转带来的累积误差。

2. 多视角融合的3D点三角化 #

双相机三角化是线性解法，多相机场景下用最小二乘优化融合三个视角的信息，能得到更稳定、一致的3D点，原理是最小化每个3D点投影到各相机2D点的重投影误差。

修正后的代码实现 #

import cv2
import numpy as np

# 计算相机相对基准相机的投影矩阵
def get_relative_projection(pts_ref, pts_target, K_ref, K_target):
    # 归一化关键点
    pts_ref_norm = cv2.undistortPoints(np.expand_dims(pts_ref, axis=1), K_ref, None)
    pts_target_norm = cv2.undistortPoints(np.expand_dims(pts_target, axis=1), K_target, None)
    # 计算本质矩阵并恢复姿态（使用相机实际内参的焦距和主点）
    E, mask = cv2.findEssentialMat(pts_ref_norm, pts_target_norm, focal=K_ref[0,0], pp=(K_ref[0,2], K_ref[1,2]), 
                                   method=cv2.RANSAC, prob=0.999, threshold=1.0)
    _, R, t, mask = cv2.recoverPose(E, pts_ref_norm, pts_target_norm)
    # 构建目标相机的外参矩阵（相对基准相机）
    M_target = np.hstack((R, t))
    P_target = K_target @ M_target
    # 基准相机的投影矩阵
    M_ref = np.hstack((np.eye(3), np.zeros((3,1))))
    P_ref = K_ref @ M_ref
    return P_ref, P_target, R, t

# 多视角最小二乘三角化
def multi_view_triangulate(points_2d_list, K_list, M_list):
    """
    points_2d_list: 每个相机的2D关键点列表，形状为 [N, 2]，N为关键点数量
    K_list: 每个相机的内参矩阵列表
    M_list: 每个相机的外参矩阵列表（相对同一世界坐标系）
    """
    num_cameras = len(points_2d_list)
    num_points = points_2d_list[0].shape[0]
    points_3d = []
    
    for i in range(num_points):
        A = []
        for cam_idx in range(num_cameras):
            u, v = points_2d_list[cam_idx][i]
            K = K_list[cam_idx]
            M = M_list[cam_idx]
            # 构建投影方程的行
            row1 = u * M[2,:] - M[0,:]
            row2 = v * M[2,:] - M[1,:]
            A.append(row1)
            A.append(row2)
        A = np.array(A)
        # SVD求解最小二乘
        _, _, Vt = np.linalg.svd(A)
        point_4d = Vt[-1]
        point_3d = point_4d[:3] / point_4d[3]
        points_3d.append(point_3d)
    return np.array(points_3d)

# ---------------------- 主流程 ----------------------
# 假设已有的数据：
# K1, K2, K3: 三个相机的内参矩阵（需提前标定）
# ptpL1, ptpL2, ptpL3: 三个相机的匹配2D关键点，形状为 [N, 2]
# getSpecificLandMarks, marksOfInterest 等函数保持原有逻辑

# 1. 计算所有相机相对相机1的外参和投影矩阵
# 相机1→相机2
P1, P2, R2, t2 = get_relative_projection(ptpL1, ptpL2, K1, K2)
# 相机1→相机3
P1, P3, R3, t3 = get_relative_projection(ptpL1, ptpL3, K1, K3)

# 整理外参矩阵列表（都相对相机1的世界坐标系）
M_list = [
    np.hstack((np.eye(3), np.zeros((3,1)))),  # 相机1外参
    np.hstack((R2, t2)),                      # 相机2外参
    np.hstack((R3, t3))                       # 相机3外参
]
K_list = [K1, K2, K3]

# 2. 处理每帧的关键点
marksOfInterest = [mp.solutions.pose.PoseLandmark.NOSE]
posList = []
for markOfInterest in marksOfInterest:
    marks1 = getSpecificLandMarks(landMarks1, markOfInterest)
    marks2 = getSpecificLandMarks(landMarks2, markOfInterest)
    marks3 = getSpecificLandMarks(landMarks3, markOfInterest)
    
    for idFrame, e1 in enumerate(marks1):
        e2 = marks2[idFrame]
        e3 = marks3[idFrame]
        if e1 is not None and e2 is not None and e3 is not None:
            # 转换为像素坐标
            p1 = np.array([[e1.x*width, e1.y*height]])
            p2 = np.array([[e2.x*width, e2.y*height]])
            p3 = np.array([[e3.x*width, e3.y*height]])
            
            # 多视角三角化
            points_2d_list = [p1, p2, p3]
            point_3d = multi_view_triangulate(points_2d_list, K_list, M_list)[0]
            
            # 此时point_3d就是统一坐标系下的3D点
            posList.append(point_3d)

关键修正点说明 #

1. 外参统一：所有相机的外参都基于相机1的坐标系，彻底解决两两三角化的坐标系差异问题。
2. 多视角优化：用最小二乘替代双相机三角化，融合三个视角的信息，提升3D点的精度和一致性。
3. 本质矩阵计算修正：之前的findEssentialMat传入的focal和pp参数错误，现在使用相机内参的实际焦距和主点，这是外参计算不准确的核心原因之一。

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