如何将3D圆投影到针孔相机的2D图像平面？（Python实现）

实现思路 #

要将3D圆投影到相机图像平面，我们需要完成以下核心步骤：

1. 将3D圆的中心和平面法向量从坐标系C转换到相机坐标系K。
2. 在相机坐标系中生成圆上的3D点。
3. 使用相机内参将这些3D点投影到图像平面。
4. 根据投影结果拟合出对应的椭圆、线段或点集，并处理边缘情况。

完整Python实现 #

import numpy as np

def transform_circle_to_camera_frame(c_center, c_normal, kTc):
    """将圆的中心和法向量从坐标系C转换到相机坐标系K"""
    # 转换中心（齐次坐标）
    center_c_hom = np.array([*c_center, 1.0])
    center_k_hom = kTc @ center_c_hom
    center_k = center_k_hom[:3] / center_k_hom[3]  # 处理非刚性变换的情况
    
    # 转换法向量（仅应用旋转部分）
    R = kTc[:3, :3]
    normal_k = R @ c_normal
    normal_k = normal_k / np.linalg.norm(normal_k)  # 确保单位向量
    
    return center_k, normal_k

def create_plane_basis(normal):
    """生成平面的正交基向量"""
    if abs(normal[0]) < abs(normal[1]) and abs(normal[0]) < abs(normal[2]):
        temp = np.array([1.0, 0.0, 0.0])
    elif abs(normal[1]) < abs(normal[2]):
        temp = np.array([0.0, 1.0, 0.0])
    else:
        temp = np.array([0.0, 0.0, 1.0])
    
    u = np.cross(normal, temp)
    u = u / np.linalg.norm(u)
    v = np.cross(normal, u)
    
    return u, v

def generate_3d_circle_points(center, radius, u, v, num_points=100):
    """生成3D圆上的点集"""
    theta = np.linspace(0, 2 * np.pi, num_points)
    circle_points = center[None, :] + radius * (
        np.cos(theta)[:, None] * u[None, :] + 
        np.sin(theta)[:, None] * v[None, :]
    )
    return circle_points

def project_3d_to_image(points_3d, intrinsic):
    """将相机坐标系中的3D点投影到图像平面"""
    points_hom = intrinsic @ points_3d.T
    z = points_hom[2, :]
    
    # 过滤掉相机后方的点（z<=0）
    valid_mask = z > 1e-6
    points_img = (points_hom[:2, valid_mask] / z[valid_mask]).T
    
    return points_img, valid_mask

def are_points_colinear(points, tol=1e-3):
    """判断点集是否共线"""
    if len(points) < 3:
        return True
    
    vecs = points[1:] - points[0]
    cross = vecs[:, 0] * vecs[0, 1] - vecs[:, 1] * vecs[0, 0]
    return np.all(np.abs(cross) < tol)

def fit_line_segment(points):
    """拟合线段，返回端点和边界"""
    min_u = np.min(points[:, 0])
    max_u = np.max(points[:, 0])
    min_v = np.min(points[:, 1])
    max_v = np.max(points[:, 1])
    
    # 找到距离最远的两个端点
    distances = np.linalg.norm(points - points[0], axis=1)
    idx1 = np.argmax(distances)
    distances = np.linalg.norm(points - points[idx1], axis=1)
    idx2 = np.argmax(distances)
    
    return {
        'endpoint1': points[idx1],
        'endpoint2': points[idx2],
        'min_u': min_u,
        'max_u': max_u,
        'min_v': min_v,
        'max_v': max_v
    }

def fit_ellipse(points):
    """拟合椭圆，返回标准参数"""
    x = points[:, 0]
    y = points[:, 1]
    
    # 构建最小二乘问题
    A = np.vstack([x**2, x*y, y**2, x, y, np.ones_like(x)]).T
    U, S, Vt = np.linalg.svd(A)
    params = Vt[-1, :]
    A_ell, B_ell, C_ell, D_ell, E_ell, F_ell = params
    
    # 计算中心坐标
    denominator = 4 * A_ell * C_ell - B_ell**2
    cx = (B_ell * E_ell - 2 * C_ell * D_ell) / denominator
    cy = (B_ell * D_ell - 2 * A_ell * E_ell) / denominator
    
    # 计算旋转角度
    phi = 0.5 * np.arctan2(B_ell, A_ell - C_ell)
    
    # 计算半轴长度
    numerator = 2 * (
        A_ell * E_ell**2 + C_ell * D_ell**2 + F_ell * B_ell**2 -
        B_ell * D_ell * E_ell - A_ell * C_ell * F_ell
    )
    term = np.sqrt((A_ell - C_ell)**2 + B_ell**2)
    denominator1 = (A_ell * C_ell - (B_ell/2)**2) * (A_ell + C_ell + term)
    denominator2 = (A_ell * C_ell - (B_ell/2)**2) * (A_ell + C_ell - term)
    
    a = np.sqrt(numerator / denominator1)
    b = np.sqrt(numerator / denominator2)
    
    # 确保长半轴 >= 短半轴
    if a < b:
        a, b = b, a
        phi += np.pi / 2
    
    # 归一化旋转角度到[-π/2, π/2]
    phi = np.mod(phi, np.pi)
    if phi > np.pi/2:
        phi -= np.pi
    
    return {
        'center': (cx, cy),
        'semi_major': a,
        'semi_minor': b,
        'rotation': phi,
        'general_params': (A_ell, B_ell, C_ell, D_ell, E_ell, F_ell)
    }

def project_3d_circle_to_image(c_center, c_normal, radius, kTc, intrinsic, num_points=100):
    """主函数：将3D圆投影到图像平面"""
    # 1. 转换到相机坐标系
    center_k, normal_k = transform_circle_to_camera_frame(c_center, c_normal, kTc)
    
    # 2. 生成平面正交基
    u, v = create_plane_basis(normal_k)
    
    # 3. 生成3D圆上的点
    circle_points_k = generate_3d_circle_points(center_k, radius, u, v, num_points)
    
    # 4. 投影到图像平面
    points_img, valid_mask = project_3d_to_image(circle_points_k, intrinsic)
    
    # 5. 处理结果
    if len(points_img) < 2:
        return {'type': 'none', 'message': '无可见点'}
    elif are_points_colinear(points_img):
        segment_params = fit_line_segment(points_img)
        return {'type': 'line_segment', 'params': segment_params}
    elif len(points_img) >=5:
        ellipse_params = fit_ellipse(points_img)
        return {'type': 'ellipse', 'params': ellipse_params}
    else:
        return {'type': 'points', 'points': points_img}

# ------------------------------
# 示例用法
# ------------------------------
if __name__ == "__main__":
    # 相机内参矩阵 (fx=500, fy=500, 主点(320,240))
    intrinsic = np.array([
        [500, 0, 320],
        [0, 500, 240],
        [0, 0, 1]
    ])
    
    # 坐标系变换矩阵kTc：C系原点在K系中(0,0,10)，无旋转
    kTc = np.eye(4)
    kTc[2, 3] = 10.0
    
    # C系中的圆参数：中心(0,0,0)，半径2，法向量(0,0,1)
    c_center = (0, 0, 0)
    c_normal = (0, 0, 1)
    radius = 2
    
    # 执行投影
    result = project_3d_circle_to_image(c_center, c_normal, radius, kTc, intrinsic)
    
    # 打印结果
    print("投影结果类型:", result['type'])
    if result['type'] == 'ellipse':
        print("椭圆中心:", result['params']['center'])
        print("长半轴:", result['params']['semi_major'])
        print("短半轴:", result['params']['semi_minor'])
        print("旋转角度(弧度):", result['params']['rotation'])

关键说明 #

1. 坐标系转换：使用齐次坐标转换中心，仅用旋转矩阵转换法向量（平移不影响方向）。
2. 点生成：通过平面正交基生成圆上的均匀点集，确保覆盖整个圆。
3. 投影过滤：仅保留相机前方（z>0）的点，避免无效投影。
4. 结果拟合：根据投影点的分布情况，自动选择拟合椭圆、线段或返回原始点集，处理边缘情况。

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