基于OpenCV的2D相机轨迹恢复优化问题咨询

我之前在做KITTI数据集的单目相机轨迹恢复时，也踩过不少类似的坑，咱们针对你的问题逐一分析，再给你一些实用的优化建议：

1. 8-24个特征点是否会导致recoverPose出错？ #

绝对会有很大影响！recoverPose依赖足够数量的内点来计算稳定的旋转矩阵R和平移向量t，8-24个特征点哪怕全是内点，样本量也太小了——很容易被少量噪声点干扰，导致R和t的估计出现偏差，进而让轨迹出现晃动甚至倒退。

尤其是KITTI的户外场景，光线变化、物体遮挡都可能带来噪声，建议你至少保证每帧有50个以上的可靠特征点。如果用的是物体边界框角点，那尽量多检测一些静态物体（比如建筑物、路牌），每个物体贡献4个角点，这样更容易凑够数量。

2. 异常值占比的合理限制是多少？ #

这个得结合你的特征点类型来看：你用的是静态物体的边界框角点，理论上大部分点都应该是内点。我个人的经验是：

• 如果RANSAC输出的内点占比低于60%，直接丢弃这一帧的计算结果，不要累积到轨迹里；
• 如果连续3帧以上内点占比都很低，建议重新初始化轨迹（回到之前的可靠帧重新开始计算）；
• 同时可以把findEssentialMat的RANSAC置信度调高到0.999，让算法更严格地筛选可靠的内点模型。

3. 位置计算方式是否正确？ #

这很可能是你轨迹倒退的核心原因！首先要明确cv2.recoverPose输出的R和t的含义：它给出的是从第i帧相机到第i+1帧相机的相对变换，即归一化相机坐标下满足 x_{i+1} = R @ x_i + t。

而相机在世界坐标系中的位置计算，正确的逻辑应该是这样的：
假设我们以第0帧相机为世界原点，第i帧的世界到相机的旋转矩阵为R_total，平移向量为t_total，那么相机的位置P_i = -R_total.T @ t_total（因为t_total是世界原点到相机原点的向量在相机坐标系下的表示，转换到世界坐标系需要逆旋转）。

你的计算方式C = np.dot(R, C)、p[i+1] = p[i] + np.dot(np.linalg.inv(C), t)明显混淆了坐标系变换的逻辑，建议改成正确的累积方式：

# 初始化：第0帧的位姿
R_total = np.eye(3)
t_total = np.zeros((3, 1))
trajectory = [np.zeros((3, 1))]  # 第0帧位置在原点

# 处理第i帧到i+1帧的相对变换R, t
R_total = R @ R_total
t_total = R @ t_total + t
# 计算当前相机在世界中的位置
current_pos = -R_total.T @ t_total
trajectory.append(current_pos)

4. 其他可优化方向 #

除了你已经尝试的方法，还有这些思路可以试试：

• 校准内参：KITTI数据集提供了官方的相机内参文件（比如calib.txt），一定要用对应相机的内参，不要用OpenCV的默认值——内参错误会直接导致本质矩阵计算偏差，进而让R和t完全不准；
• 过滤动态物体：KITTI里有很多动态目标（车、行人），哪怕你用Faster R-CNN检测，也要通过数据集的标注过滤掉这些动态物体，只用静态物体的角点来计算；
• 特征点跟踪替代重新检测：不要每帧都用Faster R-CNN重新检测角点，改用光流（比如cv2.calcOpticalFlowPyrLK）跟踪上一帧的角点，这样能保证特征点的连续性，减少匹配错误；
• 后端优化（BA）：单目相机的轨迹不可避免会有累积误差，你可以用g2o或者Ceres Solver做Bundle Adjustment，把所有帧的位姿和特征点的3D位置一起优化，大幅减少轨迹漂移；
• 归一化特征点：在调用cv2.findEssentialMat之前，一定要把像素坐标转换成归一化相机坐标（即减去主点坐标，再除以对应轴的焦距），这是本质矩阵计算的必要前提；
• 关键帧选择：不要处理每相邻帧，而是选择关键帧（比如当相机移动一定距离或旋转一定角度时才计算），减少无效计算和误差累积。

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