1.投影模型和BA代价模型##
对于观测模型而言,我们可以简单的用以下的模型进行表示:z=h(x,y)
上图中,P点是世界坐标系的点,中间的畸变模块满足: r c 2 = u c 2 + v c 2 r_{c}^{2}=u_{c}^{2}+v_{c}^{2} rc2=uc2+vc2
把观测返程抽象出来,有:z=h(x,y) ,这里的x指代此时相机的位姿,即外参R,t,对应的李代数为 ξ \xi ξ。路边y即为三维点p,若观测数据(实际观测数据)为z,那么此次观测误差为: e = z − h ( ξ , p ) e=z-h(\xi ,p) e=z−h(ξ,p),那么代价函数为:
1 2 ∑ i = 1 m ∑ j = 1 n ∣ ∣ e i j ∣ ∣ 2 = 1 2 ∑ i = 1 m ∑ j = 1 n ∣ ∣ z i j − h ( ξ i , p j ) ∣ ∣ 2 \frac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{n}|| e_{ij}||^{2}= \frac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{n}|| z_{ij} -h(\xi_{i},p_{j} )||^{2} 21i=1∑mj=1∑n∣∣eij∣∣2=21i=1∑mj=1∑n∣∣zij−h(ξi,pj)∣∣2
其中 z i j z_{ij} zij 为姿态i观察路标点j所产生的数据。
使用非线性优化来优化这个代价函数,当把所有的自变量定义成所有待优化的变量时,即:
x = [ ξ 1 , ⋯ , ξ m , ρ 1 , ⋯ , ρ n ] x=[\xi_{1} ,\cdots ,\xi_{m},\rho_{1} ,\cdots ,\rho_{n}] x=[ξ1,⋯,ξm,ρ1,⋯,ρn]
那么整个代价函数就可以变成:
1 2 ∣ ∣ f ( x + Δ x ) ∣ ∣ 2 ≈ 1 2 ∑ i = 1 m ∑ j = 1 n ∣ ∣ e i j + F i j Δ ξ i + E i j Δ ρ j ∣ ∣ 2 \frac{1}{2}||f(x+\Delta x)||^{2}\approx \frac{1}{2}\sum_{i=1}^{m}\sum_{j=1}^{n}||e_{ij}+F_{ij}\Delta \xi _{i}+E_{ij}\Delta\rho_{j}||^{2} 21∣∣f(x+Δx)∣∣2≈2