窥探PTAM之姿态估计

4 姿态估计

在斑点搜索之后，我们有一堆新找到的点集在当前视图的投影，由于测量误差的关系，点集的座标位置并不是精准的，我们需要从这些不太准确的数据中估算新的摄像机姿态和位置。把这群点集的座标值作为观测值，把运动模型计算的点位置作为估计值。求某个运动参数向量，使估计值与观测值的残差平方和最小，即为我们要计算的姿态更新增量。
姿态估计分为粗搜索和精搜索两个阶段。粗搜索搜索范围更广，使用点数较少。精搜索搜索范围较窄，使用上千个点样本。两个阶段解决的一样的问题，但精搜索样本大对计算资源消耗极大，因此引入一些优化方法。

4.1 姿态更新

斑块搜索中得到的斑块第0层位置u,v作为独立的测量值，把j组测量值线性组合成一个向量：Pj={u1,v1,u2,v2,...uj,vj} 。而运动模型估计的u,v为：

[û j,v̂ j]T=CamProj(exp(μ)Ecwpj)

用 ^ 区分观测值和估计值。ej 代表重投影误差向量。CamProj函数可以将摄像机座标系中的值转到图像座标系，则测量残差为：

r2j=uj−û j;r2j+1=vj−v̂ j

那么解∑iri=min 就是一个最小二乘问题。把参数向量μ=[α,β,γ,Tx,Ty,Tz] 作为待估计参数。试着把uj^(pj),vj^(pj) 展开会发现这是一个非线性方程(组)。即估计运动参数的实质就是求解非线性最优解问题。下面介绍用高斯牛顿法求解非线性最小二乘问题。

4.2 最小二乘

(ref:使用Math.NET求解线性和非线性最小二乘问题 | 赵毅力的视觉研究博客 )

最小二乘问题出现在许多应用中。它可以被看作是一个最优化问题（optimization problem）。例如，我们搜集了一些数据，然后想用这些数据对一个模型进行拟合，并使得这些数据和求解到的模型在某种意义上达到最佳拟合。 一般可以将最小二乘问题紧凑地表示为：

f(x)=12∑j=1mr2j(x)

其中x是模型参数组成的向量，r被称为残差向量（residual vector），可以表示预期值和测量值之间的差异，是由单独的残差所组成的列向量：

rj(x)=y(x̃ j|x)–ỹ j

y(α|x)=x1X1(α)+x2X2(α)+…+xnXn(α)

函数y是我们的模型函数（或期望模型），它根据给定的样本点x̃_j以及模型参数x返回预期值。这个值再和ỹ j 进行比较，这里ỹ j 为观测值。波浪线标记(~)用来区分从模型参数(x)和模型函数(y)获获取的数据点。

4.2.1设计矩阵

最小二乘问题可以用矩阵表示，引入设计矩阵A：

Aji=Xi(x̃ j)=∂rj∂xi=∂∂xi[y(x̃ j|x)–ỹ j]=Xi(x̃ j)

这里x是要拟合模型的参数向量，X是基函数（basis functions）。基函数可以是参数向量x的任何函数。把最小二乘化为矩阵形式：

∥r∥2=rTr=(Ax–ỹ )T(Ax–ỹ )=xTATAx–2xTATỹ +ỹ Tỹ 

要对∥r∥2 进行最优化求解。可以通过计算上述表达式对于参数x的偏导数，并令它等于零：

2ATAx–2ATỹ =0

求解得到：

x=(ATA)−1ATỹ 

4.2.2 非线性最小二乘

对于线性最小二乘问题，我们将问题求解归结到对方程组Ax=b进行求解，而对于非线性最小二乘问题，我们将会有诸如A(x)x=b形式的方程组，即设计矩阵也依赖于模型参数。 在非线性的情况下能够使用的最好方法是为参数向量提供一个初始值，然后让这个初始值逐步趋近于正确的解向量。这种方法会在每一步对参数向量进行更新，并重复这个过程直到收敛，因此是一种迭代算法。 在线性最小二乘问题中称为设计矩阵的矩阵A在非线性最小二乘问题中被称为雅可比矩阵J(x)。它的定义和设计矩阵的定义相同：

Jj,i(x)=[∂∂xirj]

但与设计矩阵不同的是，它依赖于模型参数x。由定义可知，我们可以直接从我们的模型函数构造雅可比矩阵：

J(x)=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢∂∂x1y(x̃ 1|x)∂∂x1y(x̃ 2|x)⋮∂∂x1y(x̃ m|x)∂∂x2y(x̃ 1|x)∂∂x2y(x̃ 2|x)⋮∂∂x2y(x̃ n|x)⋯⋯⋱⋯∂∂xny(x̃ 1|x)∂∂xny(x̃ 2|x)⋮∂∂xny(x̃ m|x)⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥

每一种迭代算法在开始计算时都需要提供参数向量x的一个初始估计值x(0)，并用下面的形式计算在当前迭代步骤中新的参数估计值：

x(k+1)=x(k)+p(k)

pk 表示第k次迭代的修正因子，x（k）表示第k次迭代的值。此外，目标函数的函数值应该是在每一步变得更小：

f(x+p)<f(x)

通过对不等式左边进行一阶泰勒展开我们有：

f(x)+g(x)Tpg(x)Tp<<f(x)0

∇2f(x)≈J(x)TJ(x)=H(x)

上式中：

• g(x)表示目标函数的梯度向量，即目标函数，或当前对目标函数的近似，相对于模型参数的一阶导数的向量。它描述了目标函数在某点x的斜率（slopes）。
• 用H(x)表示目标函数的海森矩阵，即目标函数（或当前对目标函数的近似）相对于每一种参数组合的二阶偏导数矩阵。它描述了目标函数在某点x的曲率（curvature）。目标函数的二阶偏导可以用hessian矩阵近似表达。

这样就给出了以目标函数的梯度表示的减小条件。

4.3 高斯牛顿法

应用高斯牛顿法可以解决非线性最小二乘问题。由于的fi(x)非线性性，此时按梯度等于 0,得到的是一个非线性方程组，求解困难。常用的基本思想是用一系列线性最小二乘问题求解该非线性最小二乘问题：设x(k)是解的第k次近似， 在x(k)处将函数fi(x)线性化，把问题化为线性最小二乘问题，求出第k+1 次近似解x(k+1);再从x(k+1) 出发，重复此过程，直到达到迭代终止准则。
首先原方程可以化解为二次式最优解的问题：

f(x+p)≈f(x)+pT∇f(x)+12pT∇2f(x)p=min

尝试用牛顿法得到牛顿法迭代步骤公式，实际就是上式对向量p求导令等于0,然后变形，得到牛顿迭代定义：( ref：矩阵求导.pdf):

p(k)=–∇2f−1(k)∇f(k)=–H−1(k)g(k)

此时按牛顿法的思路用f的hessian矩阵计算上式结果太复杂。如上节所述，用雅可比矩阵近似替代目标函数的hessian矩阵。所以上式也可以写为：

HkpGNkJTkJkpGNk==–gk–JTkrk

对上式求解P即为高斯牛顿法迭代定义。完整的迭代过程如下：
1. 取初始点 x (1) ,置精度要求 ε ,令 k=1;
2. 计算雅可比矩阵Jk ；
3. 若||Jkr||≤ε ,则停止计算，x(k)为所求k的极小点;否则解以上线性方程组。得到 Gauss-Newton 方向 pGNk ；||表示取范数。
4. 令x(k+1)=x(k)+pGNk,k=k+1 转2;

由迭代步骤可知解非线性最小二乘最优化问题仅需要雅可比矩阵和残差向量。实际编程中使用TooN库的wls类解姿态更新的最小二乘问题。

4.4 图像雅可比

现在我们的问题就变成前一节所说的纯数学问题：求出哪些摄像机运动参数能让目标函数达到最小值，参数向量μ 相当于上一节的x,观测值û j 相当于y(xj^,x) ,而xj^ 表示一组观测输入，比如点所在的世界座标系xw,yw,zw 。
解最小二乘需要雅可比矩阵，首先Camproj函数分别对参数求偏导。用X代表该参数。
对X 求偏导：

∂u∂X=ΔuΔX;X=μ[n]

由针孔摄像机模型可知：

p′c=MEcwpw=Mpc

M为4x3运动矩阵。需要注意的，M可以分解。例如第l+1次迭代时，M可以分解为

⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢r11r21r310r12r22r320r13r23r330∑jTxj∑jTyj∑jTzj1⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥=[Rl,Tl]...[R2,T2][R1,T1]

pl+1=Mpc=[Rl,Tl]pl

即第l+1次迭代时，p′c 可以通过上一次迭代结果作为初值。

记p′c=[x′c,y′c,z′c] 向量p′c 求微分有：

Δmotion=[Δx,Δy,Δz]T=∂p′c∂XΔX

将Δmotion 的Δx,Δz 分量代入[4]式，得û  对变量X求偏导的通用公式：

∂û ∂X=λzc∂x′c∂X+xc∂z′c∂Xz2

λ 为x方向的焦距λ=fx 。记motion=[∂y′c∂X,∂x′c∂X,∂z′c∂X] ,物理意义上，motion可以理解为摄像机做某个微分运动时造成该点在摄像机座标系下的运动量。摄像机微分运动分为微分平移和微分旋转。

当X为Tx时，P′c 对Tx求偏导得

∂x′c∂Tx=∂(m11x+m12y+m13z+Tx)∂Tx=1

∂y′c∂Tx=0

∂z′c∂Tx=0

motionTx=[1,0,0] ,同理得motionTy=[0,1,0],motionTz=[0,0,1]
当X为绕x轴的旋转量α 时，矩阵Mpc 对α 求偏导：

motionα=(∂R∂α+∂T∂α)pc

第二项为0,第一项为绕x轴旋转的微分旋转矩阵。因为微分旋转量很小，所以有：

sin(δα)=0,cos(δα)=1

微分旋转矩阵rot(x,α) 为：

∂R∂α=⎡⎣⎢⎢⎢⎢00000sin(δα)cos(δα)00−cos(δα)sin(δα)00001⎤⎦⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢000000100−1000001⎤⎦⎥⎥⎥⎥

代入求偏导式得：

motionα=[0,−z,y]

同理可得，对绕y轴旋转量β 求偏导：

motionβ=[z,0,−x]

对绕z轴旋转γ 求偏导：

motionγ=[−y,x,0]

将motion代入求偏导通用公式即得到û j 对参数向量[Tx,Ty,Tz,α,β,γ] 求偏导的结果，v̂ j 对参数向量求偏导的的过程也类似。遍历所有点对参数向量求导便得到一个雅可比矩阵：

⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢∂û 1∂Tx∂v̂ 1∂Tx...∂û j∂Tx∂v̂ j∂Tx∂û 1∂Ty∂v̂ 1∂Ty...∂û j∂Ty∂v̂ j∂Ty∂û 1∂Tz∂v̂ 1∂Tz...∂û j∂Tz∂v̂ j∂Tz∂û 1∂α∂v̂ 1∂α...∂û 1∂α∂v̂ j∂α∂û 1∂β∂v̂ 1∂β...∂û j∂β∂v̂ j∂β∂û 1∂γ∂v̂ 1∂γ...∂û j∂γ∂v̂ j∂γ⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥

将此雅可比矩阵和残差rj 代入高斯牛顿迭代定义式，求得修正因子PN 。编程实现上，采用TooN库的wls类可以简化以上操作。使用修正因子修正μj+1=μj+PN 即完成一次迭代。之后再次运行上面的过程，投影获得估计值，计算雅可比矩阵，代入高斯牛顿迭代定义式。直到修正因子收敛，完成一次姿态更新。

4.5 精搜索时使用线性最小二乘

PTAM跟踪系统中，第一次搜索仅选取至多40个跟踪点，在大范围内搜索以更新姿态。第二次为精搜索，在精搜索中，我们要计算数千个点。如果每次迭代都重新投影这些点，重新计算它们组成的非线性方程组的雅可比矩阵。那么消耗的计算资源是巨大的。PTAM采用的策略是先非线性计算一次，再线性计算两次，作为一个周期。通常三个周期后修复因子已经到了10−7 精度的极小值。最后再做一次非线性计算，完成一次精搜索的姿态更新。
在前面的非线性解最优解中，提到了目标方程是非线性的，以其中求u变量为例：

uj=Camproj(μ,pc)=u0+fuxczc

[xc,yc,zc]T=MEcwpwj,M=exp(μ)

显然Camproj关于μ 的方程是非线性方程。用F表示Camproj(p1…j) 方程组，x 表示μ 参数向量，J为该方程组的雅可比矩阵。F是个多元方程，F(x)在p 处的多元一次泰勒展开为:

F(x)≈F̃ (x)=F(p)+JF(p)⋅(x–p)

这是个线性方程，变形一下得到：

F̃ (x)−F(p)=JF(p)⋅PN

上式展示了用近似的线性方程逼近结果。等式左边可以是Δuj^ 或Δvj^ 。其逼近的方向是上一次非线性逼近时计算的各个梯度方向，增量也是上一次非线性迭代计算的PN 。这样的逼近的结果是近似的，但是有可能不准甚至远离结果，这取决于得到的修正引子Δμ ,当它取极小值时，得到的结果和重计算投影公式结果相同。取值越大，越偏离F(p+Pn) 的结果。所以不能做多次近似逼近。以一个二元方程为例演示：

伪代码如下：

4.6 稳健回归(M估计)方法简介

由于测量中系统可能存在粗差。个别异常大的残差的出现会导致平方和迅速增大。为了达到平方和为最小的目的，估值必然要迁就那些离群值[13]。如果用增长较慢的残差函数代替平方和函数，也许就能获得一定抗粗差性的估计值。M估计正是基于这样的想法，在60年代由Huber提出来。
M-estimators是一类广泛估计函数[8]，定义为所给数据上的最小和函数。最小平方估计和极大似然估计都是M估计法。M估计法由鲁棒的数据作为运行保证。一般地，一个M-estimation定义为一个估计函数为0的情况。这个估计函数经常是一些统计函数。比如令一个由参数定义的极大似然函数为0，因此一个极大似然估计值往往是一个能量函数取得极值得点。在很多应用中，这样的M估计可以用于参数估计。

在最小二乘中，观测方程为：
rTPr=min
其中权重矩阵P=diag(p1,p2...) 。也就是每一项多乘以一个权重因子。pi 为权重引子，由权函数计算获得。权函数是关于残差的函数。在PTAM中，我们使用tukey发明的biweight权函数做迭权迭代：

wi={0(1−∣ui/2∣)2 if ∣ui/c∣>1 if ∣ui/c∣≤1

式中c一般4.685。uI=eI/s=0.6745⋅ei/med(∣ei−med(ei)∣) . med()为中位数，s为残差尺度。在跟踪系统中，wls库已经封装好了计算最小二乘时加入权重的方法。综合上一节，只需要迭代每个点时这样添加数据：

wls.add_mJ(v2[0], TD.dSqrtInvNoise * m26Jac[0], dWeight);

参数分别是上一节计算得到的残差，雅可比矩阵的第i个方程的梯度向量，权重。添加完后调用wls.compute()得到结果。其实质就是求解高斯牛顿迭代定义式中的修正因子向量。

4.7 重定位简介

PTAM使用比较简单的重定位方法。在跟踪系统中，当判定跟踪丢失时启动重定位。重定位使用低分辨率的模糊的关键帧做处理。首先在构建线程每次加入关键帧时保留图像金字塔最上层的图像，并做高斯模糊处理，生成图像和地图数据一起保存。当判定跟踪丢失时，给当前帧画面也生成一个小尺寸模糊图像。直接对地图数据中所有关键帧比较处理，选择最接近的关键帧做姿态估计。