visual slam的BA问题中的雅克比推导过程
这里用了计算图的方式：
1
f = [ f 1 , f 2 ] ⊤ \mathbf{f}=[f_1, f_2]^\top f=[f1,f2]⊤, z = [ z 1 , z 2 ] ⊤ \mathbf{z}=[z_1, z_2]^\top z=[z1,z2]⊤, p = [ p 1 , p 2 ] ⊤ \mathbf{p}=[p_1, p_2]^\top p=[p1,p2]⊤:
f = z − p \mathbf{f} = \mathbf{z} - \mathbf{p} f=z−p
∂ f ∂ p = [ − 1 0 0 − 1 ] \frac{\partial \mathbf{f}}{\partial \mathbf{p}} = \begin{bmatrix} -1 &0 \\ 0 &-1 \\ \end{bmatrix} ∂p∂f=[−100−1]
2
c = [ c 1 , c 2 , c 3 ] ⊤ \mathbf{c} = [c_1, c_2, c_3]^\top c=[c1,c2,c3]⊤:
{ p 1 = f ⋅ c 1 c 3 + c x p 2 = f ⋅ c 2 c 3 + c y \begin{cases} p_1 = f \cdot \frac{c_1}{c_3} + c_x \\ p_2 = f \cdot \frac{c_2}{c_3} + c_y \\ \end{cases} {p1=f⋅c3c1+cxp2=f⋅c3c2+cy
∂ p ∂ c = [ f c 3 0 − f c 1 c 3 2 0 f c 3 − f c 2 c 3 2 ] \frac{\partial \mathbf{p}}{\partial \mathbf{c}} = \begin{bmatrix} \frac{f}{c_3} &0 &-\frac{f c_1}{c_3^2} \\ 0 &\frac{f}{c_3} &-\frac{f c_2}{c_3^2} \\ \end{bmatrix} ∂c∂p=[c3f00c3f−c32fc1−c32fc2]
3
y = [ y 1 , y 2 , y 3 ] ⊤ \mathbf{y} = [y_1, y_2, y_3]^\top y=[y1,y2,y3]⊤:
[ c 1 ] = T [ y 1 ] \begin{bmatrix} \mathbf{c} \\ 1 \\ \end{bmatrix} = \mathbf{T} \begin{bmatrix} \mathbf{y} \\ 1 \\ \end{bmatrix} [c1]=T[y1]
T = [ R t 0 1 ] = exp ( x ∧ ) \mathbf{T} = \begin{bmatrix} \mathbf{R} &\mathbf{t} \\ \mathbf{0} &1 \\ \end{bmatrix} = \exp(\mathbf{x}^{\wedge}) T=[R0t1]=exp(x∧)
∂ c ∂ y = R \frac{\partial \mathbf{c}}{\partial \mathbf{y}} = \mathbf{R} ∂y∂c=R
∂ c ∂ x = [ − c ∧ , I ] \frac{\partial \mathbf{c}}{\partial \mathbf{x}} = [-\mathbf{c}^{\wedge}, \mathbf{I}] ∂x∂c=[−c∧,I]
于是
∂ f ∂ y = [ − f c 3 0 f c 1 c 3 2 0 − f c 3 f c 2 c 3 2 ] ⋅ R \frac{\partial \mathbf{f}}{\partial \mathbf{y}} = \begin{bmatrix} -\frac{f}{c_3} &0 &\frac{f c_1}{c_3^2} \\ 0 &-\frac{f}{c_3} &\frac{f c_2}{c_3^2} \\ \end{bmatrix} \cdot \mathbf{R} ∂y∂f=[−c3f00−c3fc32fc1c32fc2]⋅R
∂ f ∂ x = [ − f c 3 0 f c 1 c 3 2 0 − f c 3 f c 2 c 3 2 ] ⋅ [ − c ∧ , I ] = [ f c 1 c 2 c 3 2 − f ( 1 + c 1 2 c 3 2 ) f c 2 c 3 − f c 3 0 f c 1 c 3 2 f ( 1 + c 2 2 c 3 2 ) − f c 1 c 2 c 3 2 − f c 1 c 3 0 − f c 3 f c 2 c 3 2 ] \begin{aligned} \frac{\partial \mathbf{f}}{\partial \mathbf{x}} &= \begin{bmatrix} -\frac{f}{c_3} &0 &\frac{f c_1}{c_3^2} \\ 0 &-\frac{f}{c_3} &\frac{f c_2}{c_3^2} \\ \end{bmatrix} \cdot [-\mathbf{c}^{\wedge}, \mathbf{I}] \\ &= \begin{bmatrix} \frac{f c_1 c_2}{c_3^2} &-f(1 + \frac{c_1^2}{c_3^2}) &\frac{f c_2}{c_3} &-\frac{f}{c_3} &0 &\frac{f c_1}{c_3^2} \\ f(1 + \frac{c_2^2}{c_3^2}) &-\frac{f c_1 c_2}{c_3^2} &-\frac{f c_1}{c_3} &0 &-\frac{f}{c_3} &\frac{f c_2}{c_3^2} \\ \end{bmatrix} \end{aligned} ∂x∂f=[−c3f00−c3fc32fc1c32fc2]⋅[−c∧,I]=⎣⎡c32fc1c2f(1+c32c22)−f(1+c32c12)−c32fc1c2c3fc2−c3fc1−c3f00−c3fc32fc1c32fc2⎦⎤