投影过程： 世界坐标系下的点 p p p， 1.将世界坐标系下的点转换为相机坐标系，为相机坐标系(R,t): P ′ = R p t = [ X ′ , Y ′ , Z ′ ] T P' = Rp t = [X',Y',Z']^T P′=Rp t=[X′,Y′,Z′]T 2.将 P ′ P' P′ 投至归一化平面，得到归一化坐标： P c = [ u c , v c , 1 ] T = [ X ′ / Z ′ , Y ′ / Z ′ , 1 ] T P_c = [u_c,v_c,1]^T = [X'/Z',Y'/Z',1]^T Pc​=[uc​,vc​,1]T=[X′/Z′,Y′/Z′,1]T 3.考虑到归一化坐标的畸变情况，得到去畸变前的原始像素坐标(例如只考虑径向畸变时)： { u c ′ = u c ( 1 + k 1 r c 2 + k 2 r c 4 ) v c ′ = v c ( 1 + k 1 r c 2 + k 2 r c 4 ) \left\{\begin{array}{l} u_{c}^{\prime}=u_{c}\left(1+k_{1} r_{c}^{2}+k_{2} r_{c}^{4}\right) \\ \\ v_{c}^{\prime}=v_{c}\left(1+k_{1} r_{c}^{2}+k_{2} r_{c}^{4}\right) \end{array}\right. ⎩⎨⎧​uc′​=uc​(1+k1​rc2​+k2​rc4​)vc′​=vc​(1+k1​rc2​+k2​rc4​)​ 4.根据相机内参模型，计算像素坐标： { u s = f x u c ′ + c x v s = f y v c ′ + c y \left\{\begin{array}{l} u_{s}=f_{x} u_{c}^{\prime}+c_{x} \\ \\ v_{s}=f_{y} v_{c}^{\prime}+c_{y} \end{array}\right. ⎩⎨⎧​us​=fx​uc′​+cx​vs​=fy​vc′​+cy​​ 这一系列计算流程看似复杂，下面用流程图形象地表示整个过程。这个过程就是前面讲的观测方程。 之前把它抽象地记为： z = h ( x , y ) z = h(x,y) z=h(x,y) 上面步骤中给出了它详细的参数化过程。具体来说，这里的 x x x 代表相机的位姿，即外参 R , t R,t R,t，它对应的李群为 T T T，李代数为 ξ \xi ξ。路标 y y y 即这里的三维点 p p p，而观测数据则是像素坐标 z = [ u s , v s ] T z = [u_s,v_s]^T z=[us​,vs​]T。以最小二乘的角度来考虑，那么可以列写关于此次观测的误差： e = z − h ( T , p ) e = z - h(T,p) e=z−h(T,p) 然后，把其他时刻的观测量也考虑进来，可以给误差添加一个下标。这 z i j z_{ij} zij​ 为在位姿 T i T_i Ti​ 时刻观察路标 p j p_j pj​ 产生的数据，那么整体的代价函数为： 1 2 ∑ i = 1 m ∑ j = 1 n ∥ e i j ∥ 2 = 1 2 ∑ i = 1 m ∑ j = 1 n ∥ z i j − h ( T i , p j ) ∥ 2 \frac{1}{2} \sum_{i=1}^{m} \sum_{j=1}^{n}\left\|e_{i j}\right\|^{2}=\frac{1}{2} \sum_{i=1}^{m} \sum_{j=1}^{n}\left\|z_{i j}-h\left(T_{i}, p_{j}\right)\right\|^{2} 21​i=1∑m​j=1∑n​∥eij​∥2=21​i=1∑m​j=1∑n​∥zij​−h(Ti​,pj​)∥2

观察到上面说的 h ( T , p ) h(T,p) h(T,p)，很容易看出该函数不是线性函数，所以希望使用非线性优化的手段来优化它。根据非线性优化的思想，应该从某个初始值开始，不断地寻找下降的方向 Δ x \Delta x Δx，来找到目标函数的最优解，即不断地求解增量方程 H Δ x = g H \Delta x = g HΔx=g 中的增量 Δ x \Delta x Δx。 尽管误差项都是针对单个位姿和路标点的，但是在整体 BA 目标函数上，应该把自变量定义成所有待优化的变量： x = [ T 1 , T 2 , . . . , T m , p 1 , p 2 , . . . , p n ] T x = [T_1,T_2,...,T_m,p_1,p_2,...,p_n]^T x=[T1​,T2​,...,Tm​,p1​,p2​,...,pn​]T 相应地，增量方程中的 Δ x \Delta x Δx 则是对整体自变量的增量。在这个意义下，当给自变量一个增量时，目标函数在 Δ x \Delta x Δx 附近进行泰勒展开为： 1 2 ∥ f ( x + Δ x ) ∥ 2 ≈ 1 2 ∑ i = 1 m ∑ j = 1 n ∥ e i j + F i j Δ ξ i + E i j Δ p j ∥ 2 \frac{1}{2} \left \| f(x+\Delta x) \right \|^2\approx \frac{1}{2}\sum_{i=1}^{m} \sum_{j=1}^{n} \left \| e_{ij}+F_{ij}\Delta \xi _i + E_{ij}\Delta p_j\right \|^2 2