基于单目相机的姿态估计与测距：Python实现指南

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

单目相机因成本低、部署灵活，广泛应用于机器人导航、增强现实（AR）、自动驾驶等领域。其核心挑战在于通过单张图像或连续帧数据，精准估计相机在三维空间中的姿态（旋转矩阵R和平移向量t），并进一步实现场景中物体的测距。这一过程涉及计算机视觉中的多个关键技术：特征点检测与匹配、相机标定、PnP（Perspective-n-Point）问题求解以及深度估计。

传统方法依赖立体视觉或多传感器融合，而单目方案通过几何约束与数学优化，仅需单个相机即可完成姿态与距离的联合估计。本文将围绕Python实现，分步骤解析技术原理与代码实践。

二、单目相机姿态精准估计的Python实现

1. 相机标定：获取内参矩阵

相机标定是姿态估计的基础，通过拍摄棋盘格图像计算内参矩阵（焦距、主点坐标）和畸变系数。OpenCV提供了完整的标定工具：

import cv2
import numpy as np
import glob
# 准备棋盘格图像路径
images = glob.glob('calibration_images/*.jpg')
square_size = 2.5  # 棋盘格方格边长（单位：cm）
pattern_size = (9, 6)  # 棋盘格内角点数量（宽×高）
# 初始化对象点与图像点列表
obj_points = []  # 三维空间点
img_points = []  # 二维图像点
# 生成棋盘格三维坐标
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
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:
        obj_points.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)
        img_points.append(corners_refined)
# 执行标定
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, gray.shape[::-1], None, None)
print("内参矩阵:\n", mtx)
print("畸变系数:\n", dist)

关键点：内参矩阵mtx包含焦距（fx, fy）和主点（cx, cy），是后续姿态估计和测距的核心参数。

2. 特征点提取与匹配

姿态估计需已知场景中3D点与图像中2D点的对应关系。常用方法包括：

• 人工标记：手动选择特征点并记录其三维坐标。
• 自动检测：使用SIFT、ORB等算法提取特征点，并通过描述子匹配建立对应关系。

以ORB特征为例：

def extract_features(image):
    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
    orb = cv2.ORB_create()
    keypoints, descriptors = orb.detectAndCompute(gray, None)
    return keypoints, descriptors
# 读取两帧图像（假设第一帧为参考帧）
img1 = cv2.imread('frame1.jpg')
img2 = cv2.imread('frame2.jpg')
kp1, des1 = extract_features(img1)
kp2, des2 = extract_features(img2)
# 暴力匹配
bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
matches = bf.match(des1, des2)
matches = sorted(matches, key=lambda x: x.distance)
# 提取匹配点坐标
pts1 = np.float32([kp1[m.queryIdx].pt for m in matches]).reshape(-1, 1, 2)
pts2 = np.float32([kp2[m.trainIdx].pt for m in matches]).reshape(-1, 1, 2)

3. PnP算法求解相机姿态

给定3D-2D点对应关系，PnP问题可通过EPnP、DLT或RANSAC优化求解旋转矩阵R和平移向量t。OpenCV的solvePnP函数支持多种方法：

# 假设已知3D点坐标（如参考帧中的物体坐标）
object_points = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1]], np.float32)  # 示例3D点
image_points = pts1[:4]  # 对应图像点（需与3D点数量一致）
# 使用EPnP + RANSAC求解
ret, rvec, tvec, inliers = cv2.solvePnPRansac(
    object_points, image_points, mtx, dist, flags=cv2.SOLVEPNP_EPNP
)
# 将旋转向量转换为旋转矩阵
R, _ = cv2.Rodrigues(rvec)
print("旋转矩阵:\n", R)
print("平移向量:\n", tvec)

优化建议：

• 增加匹配点数量以提高精度。
• 使用RANSAC剔除异常点。
• 对多帧结果进行滤波（如卡尔曼滤波）。

三、单目相机测距原理与实现

单目测距基于三角测量原理：已知物体在图像中的像素坐标（u, v）、相机内参（fx, fy, cx, cy）以及物体在相机坐标系下的深度（Z），可通过以下公式计算：
[
X = \frac{(u - c_x) \cdot Z}{f_x}, \quad Y = \frac{(v - c_y) \cdot Z}{f_y}
]
但单目相机无法直接获取深度Z，需通过以下方法间接估计：

1. 基于已知尺寸物体的测距

若物体实际尺寸（如宽度W）已知，可通过其在图像中的像素宽度（w）计算距离：
[
Z = \frac{f_x \cdot W}{w}
]
代码实现：

def calculate_distance(pixel_width, actual_width, focal_length):
    return (focal_length * actual_width) / pixel_width
# 示例：物体实际宽度为0.5m，图像中像素宽度为50px，焦距为800px
distance = calculate_distance(50, 0.5, 800)
print("估计距离:", distance, "米")

2. 基于深度学习的测距

近年来，深度学习模型（如MonoDepth、MiDaS）可通过单张图像预测深度图。以下为MiDaS的示例代码：

import torch
from midas.model_loader import load_model
# 加载预训练模型
device = torch.device("cuda" if torch.cuda.is_available() else "cpu")
model, transform, net_w, net_h = load_model(device, "dpt_large")
# 输入图像预处理
img = cv2.imread('scene.jpg')
img_input = transform({"image": img})["image"]
# 预测深度
with torch.no_grad():
    sample = torch.from_numpy(img_input).to(device).unsqueeze(0)
    prediction = model.forward(sample)
    prediction = torch.nn.functional.interpolate(
        prediction.unsqueeze(1),
        size=img.shape[:2],
        mode="bicubic",
        align_corners=False,
    ).squeeze()
depth = prediction.cpu().numpy()

优缺点对比：

• 几何方法：无需训练数据，但依赖已知物体尺寸或特征点匹配。
• 深度学习方法：泛化能力强，但需大量标注数据训练。

四、实际应用中的挑战与解决方案

1. 动态场景适配：运动模糊或光照变化会导致特征点丢失。
   解决方案：结合光流法（如Lucas-Kanade）或使用事件相机。

2. 累积误差：多帧姿态估计中，误差会随时间累积。
   解决方案：引入IMU进行传感器融合，或使用SLAM框架（如ORB-SLAM3）。

3. 低纹理场景：光滑表面特征点稀少。
   解决方案：使用主动照明（如结构光）或切换至红外相机。

五、总结与展望

单目相机姿态精准估计与测距技术已从实验室走向实际应用，其核心在于几何约束与数据驱动方法的结合。未来发展方向包括：

• 轻量化模型：优化深度学习模型以适应嵌入式设备。
• 多模态融合：结合激光雷达或IMU提升鲁棒性。
• 实时性优化：通过并行计算（如CUDA加速）满足低延迟需求。

开发者可通过OpenCV、PyTorch等工具快速实现基础功能，并根据具体场景调整算法参数。掌握单目视觉技术，将为机器人、AR/VR等领域的产品开发提供关键支持。