adjustment

BundleAdjustment简述

在SFM（structurefrommotion）的计算中BA（BundleAdjustment）作为最后⼀步优化具有很重要的作⽤，在近⼏年兴起的基于图的

SLAM（simultaneouslocalizationandmapping）算法⾥⾯使⽤了图优化替代了原来的滤波器，这⾥所谓的图优化其实也是指BA。其

实很多经典的⽂献对于BA都有深深浅浅的介绍，如果想对BA的全过程做⼀个全⾯的更深层次的了解，推荐阅读BundleAdjustment—A

ModernSynthesis，但是BA的内容确实太多太杂了，刚对其了解的时候往往会陷⼊其局部的计算中不能⾃拔，因此笔者准备对其进⾏⼀

个⽐较全局⼀点的介绍，希望读者可以⽐较轻松的⼀览BA的全过程⽽不是陷⼊其局部的繁琐的计算中，同时也会尽量对其需要的数学⼯具

介绍全⾯，如有错误和遗漏还望指正。

如果读者对以下内容有基本了解那可就太棒棒了！

射影相机⼏何模型

对极⼏何

凸优化

矩阵理论

这才是真的开始

BundleAdjustment（之后在不引起歧义的情况下⽤BA代替，你问为什么？笔者懒啊-。-），⼤概似乎也许好像有近百年的历史了吧（没

错，可以称为state-of-art的视觉SLAM在⼏年前才⽤上将近上百岁的算法），中⽂译为光束法平差，⼤概⼤家看到更多的翻译可能为束调

整、捆集调整或者捆绑调整等等。这么多翻译笔者最喜欢的还是光束法平差，⼀看就⽐其它的更专业逼格更⾼嘛，其它的翻译都太直译了。

当然最重要的是光束法平差完美的表达了BA的来源、原理和计算过程，⽽其他的只是强调了将很多数据放在⼀起进⾏优化计算这个事。不

信？那我们来分析⼀下嘛。

所谓bundle，来源于bundleoflight，其本意就是指的光束，这些光束指的是三维空间中的点投影到像平⾯上的光束，⽽重投影误差（后

⾯会讲这到底是个什么⿁）正是利⽤这些光束来构建的，因此称为光束法强调光束也正是描述其优化模型是如何建⽴的。剩下的就是平差，

那什么是平差呢？借⽤⼀下百度词条测量平差中的解释吧。

由于测量仪器的精度不完善和⼈为因素及外界条件的影响，测量误差总是不可避免的。为了提⾼成果的质量，处理好这些测量中存在

的误差问题，观测值的个数往往要多于确定未知量所必须观测的个数，也就是要进⾏多余观测。有了多余观测，势必在观测结果之间

产⽣⽭盾，测量平差的⽬的就在于消除这些⽭盾⽽求得观测量的最可靠结果并评定测量成果的精度。测量平差采⽤的原理就是“最⼩

⼆乘法”。

平差也就正好表述了为什么需要BA以及BA这个优化过程到底是怎么进⾏的。

BA模型到底是怎么来的？

感觉前⾯废话说了⼀⼤堆，解释了半天BA的中⽂翻译，那么BA到底是⼲嘛的呢？经过前⾯的铺垫，⽤⼀句话来描述BA那就是，BA的本质

是⼀个优化模型，其⽬的是最⼩化重投影误差

本质是⼀个优化模型应该很容易理解，那么什么是重投影误差呢？投影⾃然是个⼏何的问题，既然是⼏何问题那这个时候来个图⾃然就是最

棒棒了！

看！这些五颜六⾊的线就是我们讲的光束啦！那现在就该说下什么叫重投影误差了，重投影也就是指的第⼆次投影，那到底是怎么投影的

呢？我们来整理⼀下吧：

其实第⼀次投影指的就是相机在拍照的时候三维空间点投影到图像上

然后我们利⽤这些图像对⼀些特征点进⾏三⾓定位（triangulation，很多地⽅翻译为三⾓化或者三⾓剖分等等，当然笔者最喜欢的还是

三⾓定位，显然是利⽤⼏何信息构建三⾓形来确定三维空间点的位置嘛，相关内容请参考对极⼏何）

最后利⽤我们计算得到的三维点的坐标（注意不是真实的）和我们计算得到的相机矩阵（当然也不是真实的）进⾏第⼆次投影，也就是

重投影

现在我们知道什么是重投影了，那重投影误差到底是什么样的误差呢？这个误差是指的真实三维空间点在图像平⾯上的投影（也就是图像上

的像素点）和重投影（其实是⽤我们的计算值得到的虚拟的像素点）的差值，因为种种原因计算得到的值和实际情况不会完全相符，也就是

这个差值不可能恰好为0，此时也就需要将这些差值的和最⼩化获取最优的相机参数及三维空间点的坐标。

进⼊数学模式！

感觉像写⼩说⼀样写了⼀堆堆的⽂字，既然BA是个数学问题，不⽤数学讲讲好像不太⾏，接下来就看看BA的数学模型是怎么构建的吧。

对BA有点了解的同学可能知道BA是⼀个图优化模型，那⾸先肯定要构造⼀个图模型了（没学过图论也没事，后⾯还是会回到⼀般的优化模

型）。既然是图模型那⾃然就有节点和边了，这个图模型的节点由相机PiPi和三维空间点构成XjXj构成，如果点XjXj投影到相机PiPi的图像

上则将这两个节点连接起来。还是来张图吧。

这样就⼀⽬了然了。那么我们现在就可以通过这个图来构造优化模型了。

令点XjXj在相机PiPi拍摄到的图像归⼀化坐标系上的坐标为k(uTij,1)T=K−1ixijk(uijT,1)T=Ki−1xij，其重投影后的图像归⼀化坐标系下坐

标为k′(vTij,1)T=K−1iPiXjk′(vijT,1)T=Ki−1PiXj，其中K−1iKi−1是为了在计算时能不受相机内参影响kk和k′k′是将齐次坐标转换

为⾮齐次坐标的常数项，可以得到该重投影误差为

eij=∥uij−vij∥eij=‖uij−vij‖

BA是要将所有重投影误差的和最⼩化，那么这⾥⾃然就要开始求和了。

minRi,ti,Xj∑i,jσij∥uij−vij∥minRi,ti,Xj∑i,jσij‖uij−vij‖

其中当点XjXj在相机PiPi中有投影时σij=1σij=1，否则为σij=0σij=0。

到此我们就得到了BA优化模型的数学形式了。

接下来就应该开始计算了！

既然是优化模型，那⾃然就应该⽤各种优化算法来进⾏计算了。这⾥先⼩⼩的剧透⼀下，BA现在基本都是利⽤LM（Levenberg-

Marquardt）算法并在此基础上利⽤BA模型的稀疏性质来进⾏计算的，LM算法是最速下降法（梯度下降法）和Gauss-Newton的结合

体，⾄于是怎么结合的接下来就来慢慢介绍了。

最速下降法

如果你对梯度⽐较熟悉的话，那你应该知道梯度⽅向是函数上升最快的⽅向，⽽此时我们需要解决的问题是让函数最⼩化。你应该想到了，

那就顺着梯度的负⽅向去迭代寻找使函数最⼩的变量值就好了嘛。梯度下降法就是⽤的这种思想，⽤数学表达的话⼤概就是这样

xk=xk−1−λ∇f(xk−1)xk=xk−1−λ∇f(xk−1)

其中λλ为步长。

最速下降法保证了每次迭代函数都是下降的，在初始点离最优点很远的时候刚开始下降的速度⾮常快，但是最速下降法的迭代⽅向是折线形

的导致了收敛⾮常⾮常的慢。

Newton型⽅法

现在先回顾⼀下中学数学，给定⼀个开⼝向上的⼀元⼆次函数，如何知道该函数何处最⼩？这个应该很容易就可以答上来了，对该函数求

导，导数为00处就是函数最⼩处。

Newton型⽅法也就是这种思想，⾸先将函数利⽤泰勒展开到⼆次项：

f(x+δx)≈φ(δx)=f(x)+J(x)δx+12δxTH(x)δxf(x+δx)≈φ(δx)=f(x)+J(x)δx+12δxTH(x)δx

其中JJ为Jacobi矩阵，对矩阵函数求⼀次偏导⽽来，梯度也是对向量函数求⼀次偏导⽽来。将标量考虑为1×11×1的矩阵，将向量考虑为

n×1n×1的矩阵，其实这些求导都是求Jacobi矩阵。HH为Hessian矩阵，也就是⼆次偏导矩阵。

也就是说Newton型⽅法将函数局部近似成⼀个⼆次函数进⾏迭代，然后令xx在δxδx⽅向上迭代直⾄收敛，接下来⾃然就对这个函数求导

了：

φ′(δx)=J+Hδx=0⟹δx=−H−1Jφ′(δx)=J+Hδx=0⟹δx=−H−1J

Newton型⽅法收敛的时候特别快，尤其是对于⼆次函数⽽⾔⼀步就可以得到结果。但是该⽅法有个最⼤的缺点就是Hessian矩阵计算实在

是太复杂了，并且Newton型⽅法的迭代并不像最速下降法⼀样保证每次迭代都是下降的。

Gauss-Newton⽅法

既然Newton型⽅法计算Hessian矩阵太困难了，那有没有什么⽅法可以不计算Hessian矩阵呢？将泰勒展开式的⼆次项也去掉好像就可以

避免求Hessian矩阵了吧，就像这样：

f(x+δx)≈f(x)+J(x)δxf(x+δx)≈f(x)+J(x)δx

这好像变成了⼀个线性函数了啊，线性函数如果要最⼩化的话好像是需要增加其他的约束条件的啊。那这⾥有没有其他的约束条件呢？仔细

思考⼀下，我们需要最⼩化的是重投影误差eij=∥uij−vij∥eij=‖uij−vij‖，它的最⼩值是什么呢？理想状态下当然是等于00了。所以这个

时候就不应该求导了，⽽是直接令函数为00。此时，令f(x)=εf(x)=ε有

ε+Jδx=0⟹JTJδx=−JTεx=x+δxε+Jδx=0⟹JTJδx=−JTεx=x+δx

由此xx在δxδx⽅向上迭代直⾄∥ε∥‖ε‖最⼩。

Gauss-Newton⽅法就避免了求Hessian矩阵，并且在收敛的时候依旧很快。但是依旧⽆法保证每次迭代的时候函数都是下降的。

LM⽅法

LM⽅法就是在以上⽅法基础上的改进，通过参数的调整使得优化能在最速下降法和Gauss-Newton法之间⾃由的切换，在保证下降的同时

也能保证快速收敛。

Gauss-Newton最后需要求解的⽅程为

JTJδx=−JTεJTJδx=−JTε

LM算法在此基础上做了更改，变成了

(JTJ+λI)δx=−JTε(JTJ+λI)δx=−JTε

通过参数λλ的调节在最速下降法和Gauss-Newton法之间切换。做个不很数学的直观分析吧，当λλ很⼩时，显然和Gauss-Newton法是

⼀样的；当λλ很⼤时，就变成了这样：

λIδx=−JTϵ⟹δx=−λ−1JTϵλIδx=−JTϵ⟹δx=−λ−1JTϵ

然后再看看前⾯的最速下降法？

这⾥还存在⼀个问题，当λλ取某个值的时候可能会导致JJ+λIJJ+λI不可逆，所以这⾥变成了

(JTJ+λdiag(JTJ))δx=−JTε(JTJ+λdiag(JTJ))δx=−JTε

其实LM算法的具体形式就笔者看到的就有很多种，但是本质都是通过参数λλ在最速下降法和Gauss-Newton法之间切换。这⾥选⽤的是

上的形式。

LM算法就由此保证了每次迭代都是下降的，并且可以快速收敛。

还没完呢！别忘了还要解⽅程

LM算法主体就是⼀个⽅程的求解，也是其计算量最⼤的部分。当其近似于最速下降法的时候没有什么好讨论的，但是当其近似于Gauss-

Newton法的时候，这个最⼩⼆乘解的问题就该好好讨论⼀下了。以下的讨论就利⽤Gauss-Newton的形式来求解。

稠密矩阵的最⼩⼆乘解

对于形如Ax=bAx=b的超定参数⽅程⽽⾔，有很多求解⽅式，伪逆、QR分解、SVD等等，这⾥不展开谈，想具体了解的可以去查阅矩阵理

论相关资料。这些⽅式都有⼀个共同的特点，我们都是将AA看作⼀般的稠密矩阵，主要得到的解⾃然⾮常鲁棒，都是计算量却是和维数的

三次⽅成正⽐（O(n3)O(n3)）。⾯对BA这种超⼤规模的优化似乎有点不太实⽤。

稀疏矩阵的Cholesky分解

稠密矩阵计算起来那么复杂，如果是稀疏矩阵的话利⽤其稀疏的性质可以⼤幅减少计算量，对于稀疏矩阵的Cholesky分解就是这样。其分

解形式为⼀个上三⾓矩阵的转置乘上⾃⾝：

A≈RTRRTRx=bx=R−1R−TbA≈RTRRTRx=bx=R−1R−Tb

为什么说我们的矩阵是稀疏的

⽤⼀个⾮常简单的例⼦来解释吧，考虑有两个相机矩阵P1P1和P2P2、两个空间点X1X1和X2X2，其中X1X1只在P2P2中有投

影，X2X2在两个相机（或视⾓）中都有投影。令优化函数为f(P1,P2,X1,X2)f(P1,P2,X1,X2)，此时Jacobi矩阵为

J=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢∂f∂P1∂f∂P2∂f∂P2∂f∂X1∂f∂X2∂f∂X2⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥J=[∂f∂P1∂f∂X2∂f∂P2∂f∂X1∂f∂P2∂f∂X2]

考虑相机位置（图像数量）和空间点都⾮常多的情况，不难想象Jacobi矩阵不光是⼀个稀疏矩阵⽽且还可以写成形如[A|B][A|B]的分块矩

阵。接下来就该利⽤这些性质正式进⼊计算了！

开始计算吧！

现在再回到Gauss-Newton最后的超定参数⽅程吧。既然Jacobi矩阵可以分块那我们就先分块，分块可以有效降低需要计算的矩阵的维度并

以此减少计算量。

JTJδx=−JTεJ=[A|B][A|B]T[A|B]δx=−[A|B]TεJTJδx=−JTεJ=[A|B][A|B]T[A|B]δx=−[A|B]Tε

[ATABTAATBBTB][δxAδxB]=−[ATεBTε][ATAATBBTABTB][δxAδxB]=−[ATεBTε]

[UWTWV][δxAδxB]=[εAεB][UWWTV][δxAδxB]=[εAεB]

[I0−WV−1I][UWTWV][δxAδxB]=[I0−WV−1I][εAεB][I−WV−10I][UWWTV][δxAδxB]=[I−WV−10I][εAεB]

[U−WV−1WTWT0V][δxAδxB]=[εA−WV−1εBεB][U−WV−1WT0WTV][δxAδxB]=[εA−WV−1εBεB]

{(U−WV−1WT)δA=εA−WV−1εBWTδA+VδB=εB{(U−WV−1WT)δA=εA−WV−1εBWTδA+VδB=εB

由此我们可以先求出δAδA，然后代回求出δBδB。其中U−WV−1WTU−WV−1WT被称为舒尔补（Schurcomplement）。分块降维

之后的计算就可以利⽤稀疏的Cholesky分解进⾏计算了。

注意事项！

以上就基本将BA的整个过程进⾏了介绍，当然这只是最基础的思路，接下来⼀些遗漏点进⾏补充。

李群及李代数

不知道有没有⼈注意到，在优化迭代的过程中，我们求的值为δxδx，然后利⽤x+δxx+δx来更新xx的值。这⾥就应该出现⼀个问题了，

对于空间点的坐标和平移向量这么处理⾃然没有什么问题，但是对于旋转矩阵呢？难道⽤R+δRR+δR来更新RR的值吗？好像不太对吧。

对于旋转矩阵RR⽽⾔是不存在加法的，按理讲应该⽤RδRRδR来更新RR的值，但是优化算法的迭代过程⼜不能是乘法，这就出现了⽭

盾。

这⾥旋转矩阵及相关运算属于李群，此时将旋转矩阵变换到其对应的李代数上进⾏计算，然后再变回李群。打个不是那么恰当的⽐⽅，在计

算卷积的时候常常通过傅⾥叶变换计算乘积然后再反变换回来就是要求的卷积了，这个也是转换到李代数上计算然后再变回李群。具体的推

导可以参看李群及李代数相关内容。

协⽅差矩阵

在我们的推导中是求解⽅程

JTJδx=−JTεJTJδx=−JTε

但常常加⼊信息矩阵（协⽅差矩阵的逆），令求解⽅程变为

JTΣ−1xJδx=−JTΣ−1xεJTΣx−1Jδx=−JTΣx−1ε

其中ΣxΣx为协⽅差矩阵，令其为分块对⾓阵表⽰所有观测向量都不相关。

参考⽂献

TriggsB,McLauchlanPF,HartleyRI,adjustment—amodernsynthesis[C]//Internationalworkshopon

erBerlinHeidelberg,1999:298-372.

HartleyR,leviewgeometryincomputervision[M].Cambridgeuniversitypress,2003.

STIMATIONFORROBOTICS[J].2017.