首先我们分析一下 H = J T ∗ J H = J^T * J H=JT∗J H矩阵大家都知道是一个如下形状,但实际值是多少呢?
假设我们有两个相机位姿C1 C2 都看到一个顶点P1 ,即我们有两条边,则我们的J可以写成
J
=
A
1
0
A
2
0
A
3
A
4
J = \begin{matrix} A1 & 0 & A2 \ \ 0 & A3 & A4 \end{matrix}
J=A10A2 0A3A4
其中A1是第一条边对C1的雅克比,A2是第一条边对P1的雅克比,A3是第二条边对C2的雅克比,A4是第二条边对P1的雅克比。
H
=
J
T
J
=
A
1
T
0
0
A
3
T
A
2
T
A
4
T
∗
A
1
0
A
2
0
A
3
A
4
H = J^TJ =\begin{matrix} A1^T & 0 \\ 0 & A3^T \\ A2^T & A4^T\end{matrix} * \begin{matrix} A1 & 0 & A2 \\ 0 & A3 & A4 \end{matrix}
H=JTJ=A1T0A2T0A3TA4T∗A100A3A2A4
H
=
A
1
T
A
1
0
A
1
T
A
2
0
A
3
T
A
3
A
3
T
A
4
A
2
T
A
1
A
4
T
A
3
A
2
T
A
2
+
A
4
T
A
4
H = \begin{matrix}A1^TA1 &0 & A1^TA2 \\ 0 & A3^TA3 & A3^TA4 \\ A2^TA1 & A4^TA3 & A2^TA2+A4^TA4 \end{matrix}
H=A1TA10A2TA10A3TA3A4TA3A1TA2A3TA4A2TA2+A4TA4
观测H矩阵可知,在对角线元素上,是误差边对对应顶点hessian的累积和,而非对角元素则是每条边对两个顶点的雅克比乘积。
因此,对于H的对角线元素,我们可以单独存放在顶点类里面,每个顶点对应的元素等于
J
j
i
=
∂
f
j
(
x
i
)
∂
x
i
H
[
i
,
i
]
=
∑
j
J
j
i
T
∗
J
j
i
J_{ji} = \frac{\partial{f_j(x_i)}}{\partial{x_i}} \\ H[i,i] = \sum_j{J_{ji} ^T * J_{ji} }
Jji=∂xi∂fj(xi)H[i,i]=j∑JjiT∗Jji
而非对角线元素,必须存储在边的类里面,一条边对应一个元素(对称应该是两个)。
g2o正是这样的存储方式。