单目视觉定位与测距：Python实现与精度优化

一、单目相机姿态估计与测距的技术背景

单目视觉定位技术通过单个摄像头获取的2D图像信息，结合计算机视觉算法，实现相机在三维空间中的姿态（位置和方向）估计以及目标物体的距离测量。相较于双目或深度相机，单目方案具有硬件成本低、部署灵活的优势，广泛应用于机器人导航、AR/VR、自动驾驶等领域。

核心挑战在于单目视觉的尺度不确定性：仅通过单张图像无法直接获取真实世界的物理尺度，需通过已知尺寸的参考物体或运动恢复结构（Structure from Motion, SfM）技术解决。本文将重点介绍基于已知参考物体的姿态估计与测距方法，并提供完整的Python实现。

二、技术原理与数学基础

1. 相机成像模型

单目相机成像遵循针孔相机模型，三维空间点 ( P(X,Y,Z) ) 投影到二维图像平面 ( p(u,v) ) 的关系为：
[
s \begin{bmatrix} u \ v \ 1 \end{bmatrix} = K \begin{bmatrix} R & t \end{bmatrix} \begin{bmatrix} X \ Y \ Z \ 1 \end{bmatrix}
]
其中：

• ( K ) 为相机内参矩阵（焦距、主点坐标）
• ( R ) 为旋转矩阵（3×3）
• ( t ) 为平移向量（3×1）
• ( s ) 为尺度因子

2. PnP问题求解

给定至少4个三维空间点及其对应的二维投影点，可通过Perspective-n-Point（PnP）算法求解相机姿态 ( (R, t) )。常用方法包括：

• EPnP：高效PnP算法，适用于任意数量的点
• DLT：直接线性变换，需至少6个点
• RANSAC+P3P：鲁棒性强的组合方法

3. 测距原理

通过已知尺寸的参考物体（如棋盘格），建立三维坐标系与图像像素的映射关系。测距时，利用相似三角形原理或深度估计网络计算目标物体距离。

三、Python实现流程（基于OpenCV）

1. 环境准备

import cv2
import numpy as np
import glob
# 相机标定参数（需预先标定）
camera_matrix = np.array([[fx, 0, cx],
                           [0, fy, cy],
                           [0,  0,  1]])  # 替换为实际标定值
dist_coeffs = np.zeros(5)  # 畸变系数，若无则设为0

2. 棋盘格检测与三维点定义

# 棋盘格参数
pattern_size = (9, 6)  # 内部角点数量
square_size = 0.025    # 每个方格的实际尺寸（米）
# 生成三维世界坐标系点
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 = []  # 三维点
imgpoints = []  # 二维点
# 读取标定图像并检测角点
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)

3. 姿态估计（solvePnP）

def estimate_pose(img, objp, camera_matrix, dist_coeffs):
    gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
    ret, corners = cv2.findChessboardCorners(gray, (9,6), None)
    if not ret:
        return None
    # 使用EPnP算法求解姿态
    ret, rvec, tvec = cv2.solvePnP(objp, corners, camera_matrix, dist_coeffs, flags=cv2.SOLVEPNP_EPNP)
    # 将旋转向量转换为旋转矩阵
    rmat, _ = cv2.Rodrigues(rvec)
    return rmat, tvec
# 示例：在实时视频中估计姿态
cap = cv2.VideoCapture(0)
while True:
    ret, frame = cap.read()
    if not ret: break
    pose = estimate_pose(frame, objp, camera_matrix, dist_coeffs)
    if pose is not None:
        rmat, tvec = pose
        # 可视化：绘制坐标轴
        axis = np.float32([[0.1,0,0], [0,0.1,0], [0,0,-0.1]]).reshape(-1,3)
        imgpts, _ = cv2.projectPoints(axis, rvec, tvec, camera_matrix, dist_coeffs)
        frame = draw_axis(frame, imgpts)  # 需自定义绘制函数
    cv2.imshow('Pose Estimation', frame)
    if cv2.waitKey(1) == 27: break

4. 单目测距实现

def measure_distance(pixel_coords, rmat, tvec, obj_height_real):
    """
    通过已知物体高度和图像中的像素高度计算距离
    :param pixel_coords: 物体底部和顶部的像素坐标 [(u1,v1), (u2,v2)]
    :param rmat: 旋转矩阵
    :param tvec: 平移向量
    :param obj_height_real: 物体实际高度（米）
    :return: 距离（米）
    """
    # 提取平移向量的Z分量（相机到物体的距离）
    # 更精确的方法需结合相似三角形原理
    # 简化版：假设物体垂直于相机光轴
    pixel_height = np.abs(pixel_coords[0][1] - pixel_coords[1][1])
    focal_length = camera_matrix[0,0]
    # 相似三角形：真实高度 / 像素高度 = 距离 / 焦距
    distance = (obj_height_real * focal_length) / pixel_height
    return distance
# 示例：测量已知高度物体的距离
target_pixel_coords = [(320, 240), (320, 300)]  # 替换为实际检测值
obj_real_height = 0.5  # 物体实际高度（米）
distance = measure_distance(target_pixel_coords, rmat, tvec, obj_real_height)
print(f"Estimated distance: {distance:.2f} meters")

四、精度优化策略

1. 标定精度提升

• 使用高分辨率棋盘格（建议10×7以上）
• 采集不同角度和距离的标定图像（>20张）
• 确保棋盘格平整无变形

2. 姿态估计优化

• 结合IMU数据进行视觉惯性融合
• 使用非线性优化（如g2o）对多帧姿态进行全局优化
• 增加特征点数量（如结合ARUCO标记）

3. 测距误差补偿

• 考虑相机安装高度和倾斜角
• 建立像素-距离的查表模型
• 使用深度学习补充单目深度估计

五、应用场景与扩展

1. 机器人导航：结合SLAM实现室内定位
2. AR应用：虚拟物体与真实场景的精准对齐
3. 工业检测：零件尺寸测量与缺陷定位
4. 自动驾驶：交通标志识别与车辆测距

扩展建议：

• 尝试使用ORB-SLAM3等开源框架实现更复杂的定位
• 结合深度学习模型（如MonoDepth2）提升测距鲁棒性
• 开发ROS节点实现机器人集成

六、总结与代码资源

本文详细介绍了单目相机姿态估计与测距的完整流程，包括相机标定、PnP求解、姿态可视化和距离测量。完整代码示例已提供关键模块，读者可根据实际需求调整参数和扩展功能。

推荐学习资源：

• OpenCV官方文档（solvePnP部分）
• 《Multiple View Geometry in Computer Vision》
• GitHub开源项目：https://github.com/ethz-asl/kalibr（高级标定工具）

通过系统实践上述方法，开发者可快速构建高精度的单目视觉定位系统，满足机器人、AR等领域的核心需求。