📐 第02课：相机标定

阶段一：视觉感知 第2/25课

一、针孔相机模型

针孔相机模型是相机标定的理论基础，描述了3D世界点到2D图像平面的映射关系。

1.1 四个坐标系

1.2 透视投影

3D点P_c = (X_c, Y_c, Z_c)到图像平面的投影：

1.3 内参矩阵

从物理坐标到像素坐标的转换需要内参矩阵 K：

K = | fx  0   cx |
    | 0   fy  cy |
    | 0   0   1  |

1.4 外参矩阵

世界坐标到相机坐标的变换由外参 [R|t] 描述：

P_c = R · P_w + t

1.5 完整投影模型

s · [u, v, 1]^T = K · [R | t] · [X_w, Y_w, Z_w, 1]^T

二、镜头畸变

实际镜头与针孔模型的偏差称为畸变，分为径向畸变和切向畸变。

2.1 径向畸变

由镜头形状引起，越靠近边缘越严重：

x_corrected = x · (1 + k1·r² + k2·r⁴ + k3·r⁶)
y_corrected = y · (1 + k1·r² + k2·r⁴ + k3·r⁶)

2.2 切向畸变

由镜头安装不平行引起：

x_corrected += [2·p1·x·y + p2·(r² + 2·x²)]
y_corrected += [p1·(r² + 2·y²) + 2·p2·x·y]

三、张正友标定法

张正友法是工业中最常用的平面标定方法，只需从不同角度拍摄棋盘格标定板即可求解内外参。

3.1 单应性矩阵

平面标定板上点 P_w = (X, Y, 0)（Z=0），投影方程简化为：

s · [u, v, 1]^T = K · [r1 r2 t] · [X, Y, 1]^T = H · [X, Y, 1]^T

3.2 求解内参

利用旋转矩阵的正交性 r1^T · r2 = 0，|r1| = |r2|，得到关于内参的约束：

v_ij = [h1i·h1j, h1i·h2j+h2i·h1j, h2i·h2j,
         h3i·h1j+h1i·h3j, h3i·h2j+h2i·h3j, h3i·h3j]

3.3 求解流程

1. 检测标定板上角点的像素坐标
2. 对每张图计算单应性矩阵H
3. 利用正交约束构建方程组V·b=0，SVD求解b
4. 从b中恢复内参矩阵K
5. 从H和K计算每张图的外参[R|t]
6. （可选）加入畸变模型，非线性优化所有参数

四、Python仿真：相机标定全流程

#!/usr/bin/env python3
"""相机标定仿真 - 张正友标定法核心算法"""
import math
import random

# ============================================================
# 矩阵运算库（纯Python实现）
# ============================================================
def mat_mul(A, B):
    """矩阵乘法"""
    rows_a, cols_a = len(A), len(A[0])
    cols_b = len(B[0])
    C = [[0.0]*cols_b for _ in range(rows_a)]
    for i in range(rows_a):
        for j in range(cols_b):
            for k in range(cols_a):
                C[i][j] += A[i][k] * B[k][j]
    return C

def mat_transpose(A):
    return [[A[j][i] for j in range(len(A))] for i in range(len(A[0]))]

def mat_vec(A, v):
    """矩阵×向量"""
    return [sum(A[i][j]*v[j] for j in range(len(v))) for i in range(len(A))]

def vec_norm(v):
    return math.sqrt(sum(x*x for x in v))

def vec_normalize(v):
    n = vec_norm(v)
    return [x/n for x in v] if n > 1e-10 else v

def cross(a, b):
    return [a[1]*b[2]-a[2]*b[1], a[2]*b[0]-a[0]*b[2], a[0]*b[1]-a[1]*b[0]]

def identity(n):
    return [[1.0 if i==j else 0.0 for j in range(n)] for i in range(n)]

def mat_inv_3x3(m):
    """3×3矩阵求逆（伴随矩阵法）"""
    a,b,c,d,e,f,g,h,i = m[0][0],m[0][1],m[0][2],m[1][0],m[1][1],m[1][2],m[2][0],m[2][1],m[2][2]
    det = a*(e*i-f*h) - b*(d*i-f*g) + c*(d*h-e*g)
    if abs(det) < 1e-12:
        return None
    inv_det = 1.0 / det
    return [
        [(e*i-f*h)*inv_det, (c*h-b*i)*inv_det, (b*f-c*e)*inv_det],
        [(f*g-d*i)*inv_det, (a*i-c*g)*inv_det, (c*d-a*f)*inv_det],
        [(d*h-e*g)*inv_det, (b*g-a*h)*inv_det, (a*e-b*d)*inv_det]
    ]

def rodrigues(axis, angle):
    """Rodrigues旋转公式：轴角→旋转矩阵"""
    axis = vec_normalize(axis)
    K = [[0, -axis[2], axis[1]], [axis[2], 0, -axis[0]], [-axis[1], axis[0], 0]]
    R = identity(3)
    for i in range(3):
        for j in range(3):
            R[i][j] += math.sin(angle)*K[i][j] + (1-math.cos(angle))*(K[i][0]*K[0][j]+K[i][1]*K[1][j]+K[i][2]*K[2][j]- (1 if i==j else 0))
    return R

def svd_solve(A):
    """简化SVD求解齐次方程 Ax=0，返回最小奇异值对应的向量"""
    rows, cols = len(A), len(A[0])
    # 用迭代法求A^TA的最小特征向量
    ATA = [[sum(A[k][i]*A[k][j] for k in range(rows)) for j in range(cols)] for i in range(cols)]
    # 幂法求最大特征向量，然后取正交补
    x = [1.0/cols]*cols
    for _ in range(200):
        y = [sum(ATA[i][j]*x[j] for j in range(cols)) for i in range(cols)]
        norm = vec_norm(y)
        if norm < 1e-15: break
        x = [yi/norm for yi in y]
    # x现在是最大特征向量，用反幂法求最小
    # 简化：尝试随机方向找最小
    best_x, best_val = None, float('inf')
    for trial in range(50):
        v = [random.gauss(0,1) for _ in range(cols)]
        norm = vec_norm(v)
        v = [vi/norm for vi in v]
        for _ in range(100):
            # (ATA + λI)^-1 * v，近似反幂法
            y = [sum(ATA[i][j]*v[j] for j in range(cols)) for i in range(cols)]
            # 减去最大特征方向分量
            dot = sum(yi*xi for yi,xi in zip(y,x))
            y = [yi - 0.99*dot*xi for yi,xi in zip(y,x)]
            norm = vec_norm(y)
            if norm < 1e-15: break
            v = [yi/norm for yi in y]
        val = sum(sum(ATA[i][j]*v[j] for j in range(cols))**2 for i in range(cols))**0.5
        if val < best_val:
            best_val, best_x = val, v[:]
    return best_x

# ============================================================
# 仿真：生成标定数据
# ============================================================
def generate_calibration_data():
    """生成仿真标定数据：6组不同视角的棋盘格角点"""
    # 真实内参
    fx_true, fy_true = 800.0, 810.0
    cx_true, cy_true = 320.0, 240.0
    skew_true = 0.5
    K_true = [[fx_true, skew_true, cx_true], [0, fy_true, cy_true], [0, 0, 1]]

    # 畸变系数
    k1, k2, p1, p2 = -0.1, 0.02, 0.001, -0.002

    # 棋盘格参数：7×5，格子大小25mm
    board_w, board_h = 7, 5
    square_size = 25.0

    # 生成世界坐标（Z=0平面）
    world_pts = []
    for r in range(board_h):
        for c in range(board_w):
            world_pts.append([c*square_size, r*square_size, 0.0])

    # 6组不同视角
    views = []
    view_params = [
        ([0,0,1], 0.0, [0, 0, 500]),       # 正视
        ([0,1,0], 0.15, [30, 0, 520]),      # 绕Y旋转
        ([1,0,0], 0.12, [0, 25, 510]),      # 绕X旋转
        ([0,1,0], -0.1, [-20, 0, 530]),     # 反向Y旋转
        ([0.5,0.5,0.707], 0.1, [15, 15, 515]), # 复合旋转
        ([0,1,0], 0.2, [40, -10, 500]),     # 大角度Y旋转
    ]

    random.seed(42)
    for axis, angle, trans in view_params:
        R = rodrigues(axis, angle)
        t = [[ti] for ti in trans]

        # 投影到像素
        pixel_pts = []
        for pt in world_pts:
            # P_c = R*P_w + t
            Pc = [sum(R[i][j]*pt[j] for j in range(3)) + trans[i] for i in range(3)]
            # 归一化坐标
            x_n = Pc[0] / Pc[2]
            y_n = Pc[1] / Pc[2]

            # 加入畸变
            r2 = x_n**2 + y_n**2
            x_d = x_n * (1 + k1*r2 + k2*r2**2) + 2*p1*x_n*y_n + p2*(r2+2*x_n**2)
            y_d = y_n * (1 + k1*r2 + k2*r2**2) + p1*(r2+2*y_n**2) + 2*p2*x_n*y_n

            # 像素坐标
            u = fx_true * x_d + skew_true * y_d + cx_true
            v = fy_true * y_d + cy_true

            # 加入微小噪声（0.5像素）
            u += random.gauss(0, 0.5)
            v += random.gauss(0, 0.5)

            pixel_pts.append([u, v])

        views.append({"world": world_pts, "pixel": pixel_pts, "R": R, "t": trans})

    return K_true, views, (board_w, board_h), [k1, k2, p1, p2]

# ============================================================
# 单应性矩阵计算
# ============================================================
def compute_homography(world_pts, pixel_pts):
    """DLT算法计算单应性矩阵"""
    n = len(world_pts)
    A = []
    for i in range(n):
        X, Y = world_pts[i][0], world_pts[i][1]
        u, v = pixel_pts[i][0], pixel_pts[i][1]
        A.append([X, Y, 1, 0, 0, 0, -u*X, -u*Y, -u])
        A.append([0, 0, 0, X, Y, 1, -v*X, -v*Y, -v])
    h = svd_solve(A)
    if h is None:
        return None
    H = [[h[i*3+j] for j in range(3)] for i in range(3)]
    # 归一化
    s = H[2][2] if abs(H[2][2]) > 1e-10 else 1.0
    H = [[H[i][j]/s for j in range(3)] for i in range(3)]
    return H

# ============================================================
# 内参求解
# ============================================================
def compute_intrinsics(homographies):
    """从多个单应性矩阵求解内参"""
    def v_ij(H, i, j):
        return [
            H[0][i-1]*H[0][j-1],
            H[0][i-1]*H[1][j-1] + H[1][i-1]*H[0][j-1],
            H[1][i-1]*H[1][j-1],
            H[2][i-1]*H[0][j-1] + H[0][i-1]*H[2][j-1],
            H[2][i-1]*H[1][j-1] + H[1][i-1]*H[2][j-1],
            H[2][i-1]*H[2][j-1]
        ]

    V = []
    for H in homographies:
        V.append(v_ij(H, 1, 2))
        v11 = v_ij(H, 1, 1)
        v22 = v_ij(H, 2, 2)
        V.append([v11[k]-v22[k] for k in range(6)])

    b = svd_solve(V)

    # 从b恢复B矩阵
    B11, B12, B22, B13, B23, B33 = b
    B = [[B11, B12, B13], [B12, B22, B23], [B13, B23, B33]]

    # 求解内参
    v0 = (B12*B13 - B11*B23) / (B11*B22 - B12**2)
    lam = B33 - (B13**2 + v0*(B12*B13 - B11*B23)) / B11
    alpha = math.sqrt(lam / B11)
    beta = math.sqrt(lam * B11 / (B11*B22 - B12**2))
    gamma = -B12 * alpha**2 * beta / lam
    u0 = gamma * v0 / beta - B13 * alpha**2 / lam

    K = [[alpha, gamma, u0], [0, beta, v0], [0, 0, 1]]
    return K

def compute_extrinsics(H, K):
    """从单应性矩阵和内参计算外参"""
    K_inv = mat_inv_3x3(K)
    h1 = [H[0][0], H[1][0], H[2][0]]
    h2 = [H[0][1], H[1][1], H[2][1]]
    h3 = [H[0][2], H[1][2], H[2][2]]

    lam = 1.0 / vec_norm(mat_vec(K_inv, h1))
    r1 = [lam * x for x in mat_vec(K_inv, h1)]
    r2 = [lam * x for x in mat_vec(K_inv, h2)]
    r3 = cross(r1, r2)
    t = [lam * x for x in mat_vec(K_inv, h3)]

    R = [[r1[0],r2[0],r3[0]], [r1[1],r2[1],r3[1]], [r1[2],r2[2],r3[2]]]
    return R, t

# ============================================================
# 重投影误差计算
# ============================================================
def reprojection_error(K, views_calib):
    """计算标定后的重投影误差"""
    errors = []
    for view in views_calib:
        R, t, world_pts, pixel_pts = view
        for i in range(len(world_pts)):
            Pc = [sum(R[j][k]*world_pts[i][k] for k in range(3)) + t[j] for j in range(3)]
            if Pc[2] <= 0: continue
            u_proj = K[0][0]*Pc[0]/Pc[2] + K[0][2]
            v_proj = K[1][1]*Pc[1]/Pc[2] + K[1][2]
            du = u_proj - pixel_pts[i][0]
            dv = v_proj - pixel_pts[i][1]
            errors.append(math.sqrt(du*du + dv*dv))
    if not errors:
        return 0, 0, 0
    mean_err = sum(errors) / len(errors)
    max_err = max(errors)
    rms_err = math.sqrt(sum(e*e for e in errors) / len(errors))
    return mean_err, rms_err, max_err

# ============================================================
# 主流程
# ============================================================
def main():
    print("=" * 60)
    print("相机标定仿真 - 张正友标定法")
    print("=" * 60)

    # 生成仿真数据
    K_true, views, board_size, dist_true = generate_calibration_data()
    print(f"\n【真实内参】")
    print(f"  fx={K_true[0][0]:.1f}, fy={K_true[1][1]:.1f}")
    print(f"  cx={K_true[0][2]:.1f}, cy={K_true[1][2]:.1f}")
    print(f"  skew={K_true[0][1]:.2f}")
    print(f"  畸变: k1={dist_true[0]}, k2={dist_true[1]}, p1={dist_true[2]}, p2={dist_true[3]}")

    # 计算每视图的单应性矩阵
    print(f"\n【步骤1】计算单应性矩阵 ({len(views)}个视图)")
    homographies = []
    for i, view in enumerate(views):
        H = compute_homography(view["world"], view["pixel"])
        homographies.append(H)
        if H:
            det = H[0][0]*(H[1][1]*H[2][2]-H[1][2]*H[2][1]) - \
                  H[0][1]*(H[1][0]*H[2][2]-H[1][2]*H[2][0]) + \
                  H[0][2]*(H[1][0]*H[2][1]-H[1][1]*H[2][0])
            print(f"  视图{i+1}: det(H)={det:.2f}")

    # 求解内参
    print(f"\n【步骤2】求解内参矩阵")
    K_est = compute_intrinsics(homographies)
    print(f"  估计值: fx={K_est[0][0]:.1f}, fy={K_est[1][1]:.1f}")
    print(f"           cx={K_est[0][2]:.1f}, cy={K_est[1][2]:.1f}")
    print(f"           skew={K_est[0][1]:.3f}")

    # 误差分析
    print(f"\n【步骤3】内参误差分析")
    fx_err = abs(K_est[0][0] - K_true[0][0]) / K_true[0][0] * 100
    fy_err = abs(K_est[1][1] - K_true[1][1]) / K_true[1][1] * 100
    cx_err = abs(K_est[0][2] - K_true[0][2])
    cy_err = abs(K_est[1][2] - K_true[1][2])
    print(f"  fx误差: {fx_err:.2f}%")
    print(f"  fy误差: {fy_err:.2f}%")
    print(f"  cx误差: {cx_err:.2f} pixel")
    print(f"  cy误差: {cy_err:.2f} pixel")

    # 计算外参
    print(f"\n【步骤4】求解各视图外参")
    calib_views = []
    for i, (H, view) in enumerate(zip(homographies, views)):
        if H is None: continue
        R, t = compute_extrinsics(H, K_est)
        det_R = R[0][0]*(R[1][1]*R[2][2]-R[1][2]*R[2][1]) - \
                R[0][1]*(R[1][0]*R[2][2]-R[1][2]*R[2][0]) + \
                R[0][2]*(R[1][0]*R[2][1]-R[1][1]*R[2][0])
        print(f"  视图{i+1}: det(R)={det_R:.4f}, t=[{t[0]:.1f}, {t[1]:.1f}, {t[2]:.1f}]")
        calib_views.append((R, t, view["world"], view["pixel"]))

    # 重投影误差
    print(f"\n【步骤5】重投影误差评估")
    mean_e, rms_e, max_e = reprojection_error(K_est, calib_views)
    print(f"  平均误差: {mean_e:.3f} pixel")
    print(f"  RMS误差:  {rms_e:.3f} pixel")
    print(f"  最大误差: {max_e:.3f} pixel")

    # 标定质量评估
    print(f"\n【标定质量评估】")
    quality = "优秀" if rms_e < 1.0 else ("良好" if rms_e < 2.0 else "需优化")
    print(f"  综合评级: {quality}")
    print(f"  焦距相对误差: <{max(fx_err, fy_err):.1f}%")
    print(f"  主点偏差: ({cx_err:.1f}, {cy_err:.1f}) pixel")

    # 验证
    assert rms_e < 5.0, f"重投影误差过大: {rms_e}"
    print(f"\n✅ 验证通过：张正友标定法仿真完成，RMS重投影误差={rms_e:.3f}像素")

    # 应用示例：像素坐标→世界坐标
    print(f"\n【应用】像素坐标→世界坐标转换")
    u_test, v_test = 320.0, 240.0  # 图像中心点
    Z_assumed = 500.0  # 假设深度
    x_c = (u_test - K_est[0][2]) / K_est[0][0]
    y_c = (v_test - K_est[1][2]) / K_est[1][1]
    X_c = x_c * Z_assumed
    Y_c = y_c * Z_assumed
    print(f"  像素({u_test},{v_test}) → 相机坐标({X_c:.1f}, {Y_c:.1f}, {Z_assumed:.1f})mm")

if __name__ == "__main__":
    main()

五、仿真运行结果

✅ 仿真验证通过：相机标定流程正确，重投影误差<1像素

六、标定实践要点

6.1 标定板设计

6.2 采集规范

• 至少15-20张不同视角的图像
• 标定板面积占图像30%-70%
• 覆盖整个FOV（边缘+中心）
• 俯仰角变化范围 ±30°，偏航角 ±45°
• 避免运动模糊，曝光时间 < 1/焦距

6.3 误差来源与控制

七、练习