单目视觉位姿估算：正交迭代算法原理与工程实践

简介：这份资源面向计算机视觉学习者与嵌入式视觉开发者，聚焦单目视觉位姿估算中的正交迭代（OI）算法实现，帮助读者理解如何由单目图像推断相机在三维空间中的位置与姿态，适用于机器人导航、增强现实等场景的算法验证与学习。压缩包共14个文件，全部为m脚本，整体约8KB，涵盖主程序与test测试程序，以及欧拉角与旋转矩阵互转、四元数处理、投影变换、随机旋转矩阵生成、绝对位姿求解等辅助函数，构成从特征检测、匹配、三角化到迭代优化的完整流程。已有2573人学习下载，说明该主题在视觉定位方向具有较高关注度。读者可据此复现OI算法的迭代优化过程，对比不同迭代次数下的估算精度，并与其他位姿估计方法进行性能评估，为轻量级视觉系统的算法选型与调试提供可运行的参考代码。

1. 单目视觉位姿估算：一只眼睛如何算出自己站在哪

手机举起来扫脸解锁，机器人伸爪子去抓桌上的杯子，无人机贴着走廊往前飞——这些场景背后都藏着一个共同的问题：只靠一个摄像头拍到的画面，怎么知道自己相对于目标物体在什么位置、朝哪个方向？这就是单目视觉位姿估算要回答的事。通俗地说，位姿就是“位置 + 姿态”的合称，位置是你在哪，姿态是你朝哪。单目意味着只用一个镜头，没有双目立体视觉那种左右眼配合的深度信息，也没有激光雷达直接测距，难度自然上了一个台阶。正交迭代（Orthogonal Iteration，常缩写为 OI）就是解决这个问题的一类经典算法思路，它把“找位姿”转化成“让投影误差最小”的迭代过程。搞懂它，你就能明白为什么很多视觉定位系统在只有一个相机时依然能跑得挺稳，也能分清它和那些听起来相近的概念到底差在哪。

2. 为什么单目也能算位姿：从投影关系说起

2.1 一个相机、一张图，信息到底够不够

先想一个日常困惑：闭上一只眼睛，你还能判断杯子离你多远吗？能，但不太准。单目视觉面临的就是这个处境。相机把三维世界压扁成二维图像，深度信息在投影那一刻就丢了。可位姿估算偏偏要反着来——从二维像素倒推三维关系。这听起来像是不可能完成的任务，但有个关键前提救了场：我们通常已经知道目标物体上若干特征点的三维坐标，比如一个标定板上几个角点的实际间距是已知的。有了这组“三维点 + 它们在图像上的二维像素位置”的对应关系，未知量就只剩下相机相对物体的旋转和平移，一共六个自由度。六个未知数，只要有足够多的对应点，方程就能解。单目位姿估算的本质，就是拿已知的三维结构和观测到的二维投影，去反推那六个自由度。信息不是凭空来的，是“已知模型 + 图像观测”共同凑出来的。

2.2 把“找位姿”变成“让误差最小”

直接解方程在理想情况下可行，但真实图像有噪声，像素坐标总有偏差，方程不会完美成立。于是思路转成优化：先猜一个位姿，把已知的三维点按这个位姿投影到图像上，看看投影点和实际观测点差多少，然后调整位姿让总误差变小，反复迭代。这个误差有个专门的名字叫重投影误差，就是“按当前位姿算出来的像素位置”和“实际拍到的像素位置”之间的距离。正交迭代的巧妙之处在于，它每一轮都把问题拆成两步：先固定当前位姿算出每个点的投影，再在满足正交约束的条件下求一个更优的旋转，如此往复。所谓正交，指的是旋转矩阵本身要满足的正交性——转完之后坐标系不能拉伸变形，这是刚体旋转的数学底线。用类比说，就像调收音机旋钮，你没法一次拧到最清晰的频道，但可以每次根据杂音大小微调一点，几轮下来就逼近了。

2.3 迭代为什么能收敛，靠的是什么

很多人第一次接触迭代算法会犯嘀咕：万一越调越偏怎么办？正交迭代能收敛，靠的是它每一步都在让重投影误差单调不增。具体来说，在给定旋转的前提下，平移有闭式解，可以直接算出来；在给定平移和投影点的前提下，旋转又能通过一个正交约束下的最小二乘问题求解。两块交替优化，目标函数一路往下走，不会反弹。这跟梯度下降那种“步长没选好可能震荡”的情况不同，它的结构决定了稳定性比较好。当然，收敛速度受初值影响，初值离真值太远时迭代次数会变多，极端情况下也可能停在局部极小。所以工程上常配一个粗略的初值估计，比如用几组点先算个近似解，再交给正交迭代精修。理解这一点很关键：它不是玄学，是有明确下降保证的交替优化，但也不是万能钥匙，初值和质量差的特征点照样能让它翻车。

3. 正交迭代到底在算什么：三个核心环节拆开看

3.1 三维点、二维像素与那六个自由度

要把正交迭代讲透，得先把参与运算的角色认全。假设目标物体上有 n 个特征点，每个点的三维坐标在世界坐标系里是已知的，记作 P_i。相机拍下它们，在图像上得到对应的像素坐标 p_i。相机相对物体的位姿用一个旋转矩阵 R 和一个平移向量 t 描述，R 管朝向，t 管位置，合起来六个自由度。投影过程就是：先把三维点用 R、t 变换到相机坐标系，再除以深度做透视投影，落到归一化平面上，最后乘内参变成像素。内参是相机自己的焦距、主点这些固定参数，通常提前标定好，不参与位姿求解。所以整个问题里，已知的是 P_i、p_i 和内参，未知的是 R 和 t。正交迭代每一轮都在回答：在当前估计下，每个点的投影落在哪，和真实像素差多少，然后据此更新 R 和 t。这个“投影—比较—更新”的循环，就是算法的心跳。

3.2 正交约束：旋转矩阵不能随便乱来

初学者容易忽略的一点是，旋转矩阵不是随便九个数字填进去就行。它必须满足 R 转置乘 R 等于单位矩阵，行列式为 1。这个约束叫正交约束，物理含义是旋转不改变长度和角度，刚体转动就该这样。正交迭代在更新旋转时，会专门在这个约束下求解，常见做法是对某个矩阵做奇异值分解，再拼回一个合法的旋转矩阵。打个比方，普通最小二乘像是让一根橡皮筋随便拉伸去贴合数据，而正交约束相当于给橡皮筋套了个刚性框架，只能转不能拉。正是这个约束让解出来的 R 有物理意义，不会出现“镜子里的自己”那种反射变换。很多位姿估算翻车的案例，追根溯源就是旋转矩阵没守住正交性，算出来的姿态看着数值挺小，实际已经变形了。

3.3 一轮迭代的完整流程与参数含义

把一轮迭代摊开看，顺序大致是这样：先用当前位姿把所有三维点投影到图像平面，算出每个点的重投影误差；然后根据这些误差构造一个加权矩阵，权重通常和点的深度或误差大小有关；接着在正交约束下求解新的旋转矩阵；旋转定了之后，平移向量有直接的闭式表达，一步算出来；最后判断误差下降量是否小于阈值，小于就停，否则进入下一轮。这里几个参数值得留意：最大迭代次数是防止死循环的保险丝，误差收敛阈值决定什么时候算“够好了”，权重策略影响对噪声点的敏感程度。阈值设太松，结果粗糙；设太紧，可能白跑很多轮还在原地打转。实际使用中这些值要结合场景调，没有一套放之四海皆准的数字。下面这段伪代码把流程骨架示意出来，重点看注释里的角色分工，不是让你照抄运行。

# 正交迭代的流程骨架（示意，非完整实现） R, t = initial_guess() # 初值：粗略位姿，影响收敛速度 for step in range(max_iters): # max_iters 是迭代上限，防死循环 projected = project_points(P, R, t, K) # 三维点按当前位姿投到像素 errors = projected - p_observed # 重投影误差，逐点计算 weights = compute_weights(errors, depth) # 权重：抑制噪声点影响 R_new = solve_rotation_orthogonal(P, p_observed, weights) # 正交约束下更新旋转 t_new = solve_translation(P, p_observed, R_new, weights) # 旋转定了，平移闭式求解 if error_drop(errors) < converge_threshold: # 收敛阈值判断 break R, t = R_new, t_new

这段骨架里，project_points负责透视投影，solve_rotation_orthogonal是正交约束求解的核心，converge_threshold控制精度和耗时的平衡。真实实现还要处理内参矩阵、归一化坐标、异常点剔除等细节，但主干逻辑就是“投影—算误差—更新旋转—更新平移—判收敛”这个循环。看懂这个循环，再看任何一篇讲正交迭代的资料，都不会被公式吓住，因为公式只是在把每一步写严谨。

4. 易混概念辨析：别把正交迭代和这些搞混

4.1 易混点一：正交迭代与普通最小二乘迭代

最容易混的一对，是把正交迭代当成“带迭代的最小二乘”。两者确实都用最小化重投影误差的思路，但差别在于约束的处理方式。普通最小二乘迭代往往把旋转矩阵的九个元素当自由变量直接优化，跑完再想办法凑成合法旋转，或者干脆不管正交性，结果在数值上“差不多能用”。正交迭代则每一轮都把正交约束焊死在求解里，出来的旋转天生合法。为什么容易混？因为名字里都有“迭代”，目标函数也长得像。区分方法很简单：看它有没有在旋转更新那一步显式处理正交约束，有就是正交迭代的路子，没有就是普通非线性优化。这个区别在噪声大、点数少的时候尤其明显，前者不容易解出畸形旋转，后者可能要靠后处理补救。

4.2 易混点二：位姿估算与相机标定

第二个高频混淆是位姿估算和相机标定。标定解决的是“相机自己是谁”的问题——焦距多少、主点在哪、镜头有没有畸变，这些是内参，一次标定反复用。位姿估算解决的是“相机此刻在哪、朝哪”的问题，是外参，每帧都可能变。为什么混？因为两者都涉及投影模型，都用到三维点和二维点的对应关系，初学时常被放在同一章讲。区分的关键看未知量：标定求的是相机内部属性，位姿求的是相机相对物体的空间关系。实际流程里通常先标定拿内参，再做位姿估算，顺序反了或者把内参当未知量一起优化，问题会变得又慢又不稳。记住一句话：标定是认识自己，位姿是认识关系。

4.3 易混点三：单目位姿与双目、深度相机方案

第三个容易糊涂的地方，是单目和双目、深度相机之间的边界。双目靠两个镜头的视差直接算深度，深度相机靠结构光或飞行时间直接给距离，它们拿到的深度信息是“测”出来的。单目没有这个便利，深度要靠已知三维结构或者多帧运动间接推。为什么混？因为最终输出都是位姿，上层应用看起来一样。区分要看输入：如果系统里只有一路图像，且依赖已知目标模型，那就是单目位姿估算的范畴；如果有两路图像做立体匹配，或者有深度图直接喂进来，那是另一套体系。单目的优势是硬件简单、成本低、标定负担小，劣势是尺度模糊和深度精度依赖先验。正交迭代这类算法正是为了在“只有单目”的约束下把位姿算稳而存在的，别把它和立体视觉的方案混为一谈。

4.4 易混点四：正交迭代与直接线性变换

还有一个常被并列提起的概念是直接线性变换。直接线性变换把投影方程整理成线性方程组，一步解出位姿，速度快、实现简单，但它解出来的旋转矩阵不保证正交，通常要做一次正交化修补，修补又会引入偏差。正交迭代可以看作在直接线性变换之后继续打磨的那一层：先用简单方法拿初值，再用迭代把正交约束和重投影误差同时照顾好。为什么混？因为很多资料把直接线性变换当作正交迭代的初始化步骤，两者出现在同一段流程里。区分看角色：直接线性变换是“快速粗解”，正交迭代是“精细修正”。指望直接线性变换一步到位，在噪声小的理想数据上或许凑合，真实场景里往往精度不够；而正交迭代单独冷启动、没有好初值，也会多花不少轮次。

5. 什么时候该用、什么时候别硬上

5.1 适用场景与不适用场景的边界

正交迭代这类单目位姿估算方法，最适合的场景有几个共同特征：目标物体上有一批三维坐标已知的特征点，图像里能稳定提取到对应的二维点，相机内参已经标定好，实时性要求不算极端苛刻。比如工业装配里对准一个已知工装，或者增强现实里把虚拟物体贴到标定板坐标系上，都属于它的舒适区。反过来，如果目标物体没有先验三维模型、特征点提取抖动严重、场景光照变化剧烈导致对应关系大量错配，硬上正交迭代就会陷入“初值差—迭代多—误差大”的循环。还有一种情况要避开：需要绝对尺度且没有任何先验的场景，单目本身就有尺度模糊，这不是算法能凭空补上的，得靠额外信息。判断标准可以收成一张简表：

这张表不是绝对判决书，而是帮你快速判断“值不值得走这条路”。表里任何一项落在右边，都该先想办法补条件，而不是指望换算法救场。

5.2 一个模拟项目的踩坑记录

某开发者做过一个模拟项目，用单目相机估算一块标定板相对相机的位姿，最初图省事，直接线性变换一步出结果，静态下看着还行，一旦标定板稍微倾斜、图像有点模糊，姿态角就开始跳。后来换成正交迭代，初值仍用直接线性变换给，迭代上限设了二十轮，收敛阈值按像素误差卡。第一版跑下来精度上去了，但偶尔有几帧结果突然偏掉。排查发现是特征点里混进了两个误匹配，权重策略没把它们压住，迭代被带偏。加了异常点剔除、把权重和重投影误差挂钩之后，跳动基本消失。这段血泪经验说明两件事：正交迭代本身稳，但喂给它的对应点得先洗干净；初值和异常处理不是可选项，是流程的一部分。从那以后我每次做单目位姿估算，都强制先把对应点质量过一遍、初值来源标清楚，再让迭代去精修，宁可多花一步检查，也不愿事后对着跳变的数据找后悔药。希望这些能帮到你，下次再看到“单目视觉位姿估算”和“正交迭代”这两个词，心里能有个清晰的轮廓，而不是一团公式。

本文还有配套的精品资源，点击获取