一、单目视觉定位与测距的技术背景

单目相机姿态估计与测距是计算机视觉领域的核心问题，其通过单幅图像或视频序列实现相机空间位姿（位置与朝向）的精确计算，并推导场景中物体的三维坐标。相较于双目或RGB-D方案，单目系统具有硬件成本低、适用场景广的优势，但面临尺度模糊、特征匹配误差等挑战。本文聚焦Python实现，结合OpenCV与NumPy库，系统阐述从相机标定到三维重建的全流程技术。

1.1 技术原理概述

单目视觉定位的核心是透视几何投影模型，即通过二维图像点与三维空间点的映射关系，反推相机外参（旋转矩阵R与平移向量t）及内参（焦距、主点坐标）。测距过程则依赖已知特征点的空间坐标或假设的地面平面模型，通过解算投影方程获取深度信息。典型应用场景包括无人机导航、机器人SLAM、增强现实等。

1.2 Python技术栈选择

• OpenCV：提供相机标定、特征检测、PnP解算等核心算法
• NumPy：高效矩阵运算与线性代数支持
• SciPy：优化算法与统计工具
• Matplotlib：可视化调试与结果展示

二、相机标定：奠定精度基础

相机标定是姿态估计与测距的前提，其目的是确定相机的内参矩阵（焦距fx/fy、主点cx/cy）和畸变系数（k1,k2,p1,p2）。标定精度直接影响后续解算的稳定性。

2.1 标定板选择与图像采集

推荐使用棋盘格或圆形阵列标定板，因其特征点易于精确检测。采集时需覆盖不同角度与距离（建议15-20张），确保涵盖图像边缘区域。示例代码：

import cv2
import numpy as np
import glob
# 设置棋盘格尺寸（内部角点数）
pattern_size = (9, 6)  # 9x6的内部角点
square_size = 25.0     # 实际物理尺寸（mm）
# 准备对象点（世界坐标系中的3D点）
objp = np.zeros((pattern_size[0]*pattern_size[1], 3), np.float32)
objp[:, :2] = np.mgrid[0:pattern_size[0], 0:pattern_size[1]].T.reshape(-1, 2) * square_size
# 存储对象点与图像点
objpoints = []  # 3D世界坐标
imgpoints = []  # 2D图像坐标
# 读取标定图像
images = glob.glob('calibration_images/*.jpg')
for fname in images:
    img = cv2.imread(fname)
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    # 查找棋盘格角点
    ret, corners = cv2.findChessboardCorners(gray, pattern_size, None)
    if ret:
        objpoints.append(objp)
        # 亚像素级角点优化
        criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
        corners_refined = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)
        imgpoints.append(corners_refined)

2.2 标定参数求解

使用cv2.calibrateCamera()函数计算内参与畸变系数：

ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
    objpoints, imgpoints, gray.shape[::-1], None, None)
print("内参矩阵:\n", mtx)
print("畸变系数:\n", dist)

关键参数说明：

• mtx：3x3内参矩阵，包含fx,fy,cx,cy
• dist：5项畸变系数（k1,k2,p1,p2,k3）
• rvecs/tvecs：每幅标定图的外参（旋转向量与平移向量）

三、单目相机姿态精准估计

姿态估计的核心是Perspective-n-Point (PnP)问题，即已知n个三维点及其二维投影，求解相机外参。Python中可通过OpenCV的solvePnP()实现。

3.1 特征点匹配与三维坐标准备

需提前获取场景中特征点的三维坐标（如通过SLAM重建或人工标记）。以Aruco标记为例：

# 检测Aruco标记并获取角点
aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
parameters = cv2.aruco.DetectorParameters()
corners, ids, _ = cv2.aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
# 假设已知标记的3D坐标（单位：米）
object_points = np.array([
    [0, 0, 0],    # 标记中心
    [0.1, 0, 0],  # 标记角点1
    [0.1, 0.1, 0],
    [0, 0.1, 0]
], dtype=np.float32)

3.2 PnP解算与姿态优化

# 假设已获取图像中的2D点（需与object_points顺序对应）
image_points = np.array([
    [100, 200],   # 中心
    [150, 200],   # 角点1
    [150, 250],
    [100, 250]
], dtype=np.float32)
# 使用EPnP算法求解（适用于n>=4）
ret, rvec, tvec = cv2.solvePnP(
    object_points, image_points, mtx, dist, flags=cv2.SOLVEPNP_EPNP)
# 将旋转向量转换为旋转矩阵
rmat, _ = cv2.Rodrigues(rvec)
print("旋转矩阵:\n", rmat)
print("平移向量:\n", tvec.flatten())

优化策略：

• 使用RANSAC剔除异常点（flags=cv2.SOLVEPNP_RANSAC）
• 结合非线性优化（如cv2.solvePnPRansac()的reprojectionError参数）
• 多帧数据融合（通过卡尔曼滤波平滑姿态）

四、单目相机测距实现

测距的核心是三角测量，即通过已知基线（如特征点间距）与图像中的像素位移计算深度。

4.1 基于已知尺寸的测距

若场景中存在已知尺寸的物体（如标准尺寸的标记板），可通过相似三角形原理计算距离：

def calculate_distance(pixel_width, real_width, focal_length):
    """
    pixel_width: 物体在图像中的像素宽度
    real_width: 物体的实际物理宽度
    focal_length: 相机焦距（像素单位）
    返回: 物体到相机的距离（米）
    """
    return (real_width * focal_length) / pixel_width
# 示例：已知标记板宽度为0.2米，图像中检测到宽度为50像素，焦距为800像素
distance = calculate_distance(50, 0.2, 800)
print("估计距离:", distance, "米")

4.2 基于地面平面的测距

假设场景中存在已知高度的地面点（如地面标记），可通过消隐点约束计算深度：

def ground_plane_distance(pt2d, height, mtx):
    """
    pt2d: 地面点的图像坐标（u,v）
    height: 地面点相对于相机的高度（米）
    mtx: 相机内参矩阵
    返回: 地面点到相机的水平距离（米）
    """
    fx = mtx[0, 0]
    cx = mtx[0, 2]
    u = pt2d[0]
    # 解算方程：height = (u - cx) * Z / fx => Z = height * fx / (u - cx)
    Z = height * fx / (u - cx)
    return Z
# 示例：地面点高度为0米（与相机光心同高），图像坐标为(320,240)，焦距800像素，主点(320,240)
distance = ground_plane_distance((320, 240), 0, np.array([[800,0,320],[0,800,240],[0,0,1]]))
print("水平距离:", distance, "米")  # 输出应为无穷大（因高度为0）

五、性能优化与误差控制

5.1 常见误差来源

• 标定误差：内参不准确导致投影模型偏差
• 特征匹配误差：低纹理区域或重复模式导致误匹配
• 尺度模糊：单目系统无法直接获取绝对尺度

5.2 优化策略

• 多帧融合：通过IMU或轮式编码器提供初始姿态，减少PnP解算迭代次数
• 深度学习辅助：使用MonoDepth等网络预测密集深度图，替代稀疏特征点
• 硬件优化：采用全局快门相机减少运动模糊，提高特征检测稳定性

六、完整代码示例

以下是一个结合标定、姿态估计与测距的完整流程：

import cv2
import numpy as np
# 1. 相机标定（假设已执行，加载预存参数）
mtx = np.array([[800, 0, 320], [0, 800, 240], [0, 0, 1]])  # 示例内参
dist = np.zeros(5)  # 假设无畸变
# 2. 加载测试图像与特征点
img = cv2.imread('test_image.jpg')
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# 假设检测到4个Aruco标记
aruco_dict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_6X6_250)
corners, ids, _ = cv2.aruco.detectMarkers(gray, aruco_dict)
if len(corners) >= 4:
    # 3. 准备3D-2D对应点（假设标记ID对应预定义的3D坐标）
    object_points = np.array([
        [0, 0, 0],    # ID=0
        [0.1, 0, 0],  # ID=1
        [0.1, 0.1, 0],# ID=2
        [0, 0.1, 0]   # ID=3
    ], dtype=np.float32)
    image_points = []
    for i, corner in enumerate(corners[:4]):  # 取前4个标记
        image_points.append(corner[0][0])  # 每个标记的第一个角点
    image_points = np.array(image_points, dtype=np.float32)
    # 4. PnP解算
    ret, rvec, tvec = cv2.solvePnP(object_points, image_points, mtx, dist)
    # 5. 测距示例（计算ID=0标记的距离）
    if ret:
        pixel_u = image_points[0][0]
        real_width = 0.1  # 标记边长0.1米
        # 假设标记中心在图像中宽度为50像素（需实际测量）
        estimated_dist = calculate_distance(50, real_width, mtx[0,0])
        print("标记0的估计距离:", estimated_dist, "米")

七、总结与展望

本文系统阐述了单目相机姿态精准估计与测距的Python实现方法，覆盖相机标定、PnP解算、三角测量等核心技术。实际应用中需注意：

1. 标定质量是精度的基础，建议使用高精度标定板与多角度图像
2. 特征选择影响解算稳定性，优先使用人工标记或高区分度自然特征
3. 尺度问题可通过先验信息（如已知物体尺寸）或多传感器融合解决

未来研究方向包括深度学习与几何方法的融合、动态场景下的实时定位等。通过持续优化算法与硬件，单目视觉系统将在自动驾驶、机器人等领域发挥更大价值。