VINS ---视觉约束雅克比推导
1.注
高博SLAM 十四讲 P186 中介绍了点的视觉重投影误差相对各个优化变量的雅克比
这儿做一个大概的公式介绍
1.1 投影误差公式
- 我们选择定义：（与高博书上相反，VINS确是这样定义的，为了与代码对应，我们也这样）
误差 = 理论预测值 - 观测值 - 十四讲 是使用像素平面下的像素坐标做差，我们也为了与VINS对应，使用归一化坐标系的点做差
我们记世界坐标系下一点 P w = [ X Y Z ] P_w=\begin{bmatrix}X\\Y\\Z\end{bmatrix} Pw=⎣⎡XYZ⎦⎤ 通过 T w c T_{wc} Twc转到相机坐标 P c = [ x y z ] P_c=\begin{bmatrix}x\\y\\z\end{bmatrix} Pc=⎣⎡xyz⎦⎤ ，再转到归一化坐标系 p p p， p p p为理论预测值:
P
c
=
R
c
w
(
P
w
+
t
c
w
)
=
[
x
y
z
]
P_c=R_{cw}(P_w+t_{cw})=\begin{bmatrix}x\\y\\z\end{bmatrix}
Pc=Rcw(Pw+tcw)=⎣⎡xyz⎦⎤
p
=
[
x
/
z
y
/
z
1
]
p\;=\;\begin{bmatrix}x/z\\y/z\\1\end{bmatrix}
p=⎣⎡x/zy/z1⎦⎤
且在相机归一化坐标系下有
p
p
p 对应的观测值
p
′
=
[
x
′
/
z
′
y
′
/
z
′
1
]
p^{'} = \begin{bmatrix}x^{'}/z^{'}\\y^{'}/z^{'}\\1\end{bmatrix}
p′=⎣⎡x′/z′y′/z′1⎦⎤
投影误差即为：
e
=
[
x
/
z
y
/
z
]
−
[
x
′
/
z
′
y
′
/
z
′
]
e=\begin{bmatrix}x/z\\y/z\end{bmatrix} - \begin{bmatrix}x^{'}/z^{'}\\y^{'}/z^{'}\end{bmatrix}
e=[x/zy/z]−[x′/z′y′/z′]
这样的观测点
p
p
p与路标点
P
w
P_w
Pw点对，就能构成一项投影残差，实际情况下，就是有很多对这样的点对，组成多项投影误差，然后我们通过调整相机位姿
T
w
c
T_{wc}
Twc使得整体的误差最小化，这个时候的位姿也就是最优化的位姿，这就是构建最小二乘问题去优化变量。
1.2 投影误差相对于待优化变量的雅克比
待优化的变量即为 相机的位姿 T w c T_{wc} Twc 由 旋转与平移构成，可以用李代数 ξ \xi ξ 6维向量表示，也可以用四元数与平移量表示 [ t Q ] \begin{bmatrix}t&Q\end{bmatrix} [tQ]
-
十四讲使用的是 李代数表示，误差相对于李代数的雅克比为：
∂ e ∂ δ ξ = ∂ e ∂ P c ∂ P c ∂ δ ξ \frac{\partial e}{\partial\delta\xi}=\frac{\displaystyle\partial e}{\displaystyle\partial P_c}\frac{\displaystyle\partial P_c}{\displaystyle\partial\delta\xi} ∂δξ∂e=∂Pc∂e∂δξ∂Pc
∂ e ∂ P c = [ ∂ ( x / z ) ∂ x ∂ ( x / z ) ∂ y ∂ ( x / z ) ∂ z ∂ ( y / z ) ∂ x ∂ ( y / z ) ∂ y ∂ ( y / z ) ∂ z ] 2 × 3 = [ 1 / z 0 − x / z 2 0 1 / z − y / z 2 ] 2 × 3 \frac{\displaystyle\partial e}{\displaystyle\partial P_c}={\begin{bmatrix}\frac{\partial(x/z)}{\partial x}&\frac{\displaystyle\partial(x/z)}{\displaystyle\partial y}&\frac{\displaystyle\partial(x/z)}{\displaystyle\partial z}\\\frac{\displaystyle\partial(y/z)}{\displaystyle\partial x}&\frac{\displaystyle\partial(y/z)}{\displaystyle\partial y}&\frac{\displaystyle\partial(y/z)}{\displaystyle\partial z}\end{bmatrix}}_{2\times3}={\begin{bmatrix}1/z&0&-x/z^2\\0&1/z&-y/z^2\end{bmatrix}}_{2\times3} ∂Pc∂e=⎣⎢⎡∂x∂(x/z)∂x∂(y/z)∂y∂(x/z)∂y∂(y/z)∂z∂(x/z)∂z∂(y/z)⎦⎥⎤2×3=[1/z001/z−x/z2−y/z2]2×3
∂ P c ∂ δ ξ = [ I 3 × 3 − P c ^ ] 3 × 6 \frac{\displaystyle\partial P_c}{\displaystyle\partial\delta\xi}={\begin{bmatrix}I_{3\times3}&-\hat{P_c}\end{bmatrix}}_{3\times6} ∂δξ∂Pc=[I3×3−Pc^]3×6注： ∂ P c ∂ δ ξ \frac{\displaystyle\partial P_c}{\displaystyle\partial\delta\xi} ∂δξ∂Pc 投影点（投影到相机坐标系）关于位姿的雅克比，见书本推导 ；误差函数中 [ x ′ / z ′ y ′ / z ′ ] \begin{bmatrix}x^{'}/z^{'}\\y^{'}/z^{'}\end{bmatrix} [x′/z′y′/z′] 为观测值，是常数。
-
VINS中的位姿使用的是四元数与平移量表示，误差相对于四元数与平移量的雅克比为：
∂ e ∂ T = ( ∂ e ∂ P c ∂ P c ∂ T ) 2 × 7 \frac{\displaystyle\partial e}{\displaystyle\partial T}={\begin{pmatrix}\frac{\displaystyle\partial e}{\displaystyle\partial P_c}\frac{\displaystyle\partial P_c}{\displaystyle\partial T}\end{pmatrix}}_{2\times7}\; ∂T∂e=(∂Pc∂e∂T∂Pc)2×7
维度变成 2 × 7 2\times7 2×7, ∂ e ∂ P c \frac{\displaystyle\partial e}{\displaystyle\partial P_c} ∂Pc∂e 在上面已经算出
∂ P c ∂ T 3 × 7 = [ ∂ P c ∂ t ∂ P c ∂ Q ] = [ R c w − P c ^ 3 × 3 ] 3 × 7 {\frac{\displaystyle\partial P_c}{\displaystyle\partial T}}_{3\times7}=\;\begin{bmatrix}\frac{\displaystyle\partial P_c}{\displaystyle\partial t}&\frac{\displaystyle\partial P_c}{\displaystyle\partial Q}\end{bmatrix}={\begin{bmatrix}R_{cw}&-{\hat{P_{c\;}}}_{3\times3}\end{bmatrix}}_{3\times7} ∂T∂Pc3×7=[∂t∂Pc∂Q∂Pc]=[Rcw−Pc^3×3]3×7
∂ P c ∂ T 3 × 7 {\frac{\displaystyle\partial P_c}{\displaystyle\partial T}}_{3\times7} ∂T∂Pc3×7 的雅克比矩阵维度为 3 × 7 3\times7 3×7，但是最后一列全为0 。
这样就与VINS 对应上了，只不过VINS 中包含IMU 坐标系，还要注意的是这儿讲的是投影误差，并不是重投影误差。
1.3重投影误差公式
通过对上面的投影误差介绍，我们接下来介绍 VINS 中的重投影误差，投影的起点不是从世界坐标系开始，而是从相机i 的归一化平面一点 P i P_i Pi 开始，重投影到相机j 的归一化平面下为 P i ′ P_i^{'} Pi′ ，与相机j下的观测值 P j P_j Pj 做差。
1.4重投影误差相对各个待优化变量的雅克比
整个过程中涉及到的变量有：
相机i的位姿，相机j的位姿，相机与IMU的外参，
P
i
P_i
Pi 对应的逆深度
雅克比计算过程和上面的类似，VINs在代码中，预先计算了
∂
e
∂
P
c
\frac{\displaystyle\partial e}{\displaystyle\partial P_c}
∂Pc∂e,所有的雅克比都是通过
P
c
P_c
Pc作为桥梁，进行链式求导。
并且在对重投影误差乘上了一个协方差
（具体推导后续有空补上）