OKVIS 里的 marginalization

个人看法，很可能是错的
待优化问题通过化简：Hδχ=b
定义将被边缘化掉的变量：xμ
和将被边缘化变量关联的变量：xλ
其余变量：xp
原文如下：
Due to conditional independence, we can simplify the marginalization step and only apply it to a sub-problem。这里不管 xp ，待优化问题写成关于 xλ和 xμ 表达式：

执行舍而补边缘化消去 xμ：

作者原话：
In our keyframe-based approach, we must apply the marginalization step repeatedly and incorporate the resulting information as a prior in our optimization while our state estimate continues to change. Hence, we fix the linearization point around x0 , the value of x at the time of marginalization.
因为在边缘化的时候为了提高效率需要将 pose 和 landmark 分开，边缘化操作迭代进行，在分开边缘化的时候采用first estimate Jacobians算法，对全部将被边缘化掉的变量和边缘化变量牵扯到的变量，只在迭代的最开始计算一次雅各比矩阵，后续边缘化的时候都用这个雅各比矩阵，不再会因变量值改变重新计算雅各比矩阵。

关于 marginalization 和 Schur complement 建议阅读贺一家大神的博客：
http://blog.csdn.net/heyijia0327/article/details/52822104

定义执行边缘化变量更新值： Δχ:=Φ−1(log(x¯⊕x−10)，这里 x¯ 是当前变量的估计值。

所以优化和边缘化整个过程变量更新如下：

上式中 Δχ 表示整个边缘化操作更新（实际 marginalize 是迭代进行的），δχ 表示优化对变量的更新。

对于 b 近似：

边缘化后对 b 更新（这里因为 xμ 被边缘化掉，公式 25 中的 xμ 代指下一次将会被边缘化掉的变量）。

按照公式 24 执行公式 25 更新的过程，就是根据边缘化变量的更新，更新 b 的过程。

所以 b 总的更新表达式为：

其中 b∗λ1,0 是边缘化时消去 xμ 对 b 的更新，后面 −H∗λ1λ1Δχλ1 是根据边缘化结果按照公式 24 对 b 做更新。其中：Δχλ1 由 H∗λ1λ1∗Δχλ1=b∗λ1 计算得到。

由于雅各比矩阵只会计算一次，在后续边缘化时保持不变，所以 H 矩阵在边缘化后根据变量更新 b 值时，H 矩阵仍然保持边缘化消去 xμ 时的值。

所以整个 marginalization 的过程如下所示：

边缘化掉 xμ：
H∗λ1λ1:=Hλ1λ1−Hλ1μ∗H−1μμHμλ1

b∗λ1,0:=bλ1−Hλ1μH−1μμbμ

计算边缘化变量更新：H∗λ1λ1Δχλ1=b∗λ1,0

将变量更新作用到 b：b∗λ1:=b∗λ1,0−H∗λ1λ1Δχλ1

边缘化后新的 H 矩阵和 b 为 H∗λ1λ1 和 b∗λ1。

版权声明：本文为博主原创文章，未经博主允许不得转载。