边缘化实操
博客目的：在优化中使用边缘化
边缘化目的：保留先验(其他一些博客说明边缘化的已经很多很详细了)
FEJ:不同残差对同一状态变量求Jacobian矩阵要在同一个线性化点 在vins的策略是，使用近似的边缘化残差，但是残差的雅可比矩阵不变
实验过程
问题：
给定一组相对旋转和相对平移进行优化，如果不把第一帧固定住或者加入边缘化策略，则轨迹是会发生漂动的，自然优化出来也是有问题的。设定一个点可以观察到周围2个点的数据，例如P3点的观测数据是P1、P2、P4、P5在P3坐标系下的位置和姿态。如果在优化的时候不选择固定住第一帧，则会发生明显的漂移另一个办法就是加入边缘化因子，用历史的信息固定住状态
简化问题：
1、每次只边缘化老帧（忽略边缘化新帧这一个情况）
2、滑动窗口固定住为10帧
优化参数
每个点在世界坐标系下的位置和姿态（同时提供了，第一个点的位置和姿态）
残差定义
1、位置残差
e r r p = r i T p j − r i T p i − t i j m err_p = r_i^T p_j - r_i^T p_i - t^m_{ij} errp=riTpj−riTpi−tijm 观测即j在i坐标系下位置值
2、旋转残差
e r r r = ( r i T r j ) T δ r i j m err_r = (r_i^T r_j)^T \delta r^m_{ij} errr=(riTrj)Tδrijm 观测是j在i坐标系下的姿态值
雅可比矩阵
1、位置残差对 p i p_i pi的导数
∂ e r r p ∂ p i = − r i T \frac{ \partial err_p}{ \partial p_i} = - r_i^T ∂pi∂errp=−riT
2、位置残差对 r i r_i ri的导数 (×是反对称矩阵)
∂ e r r p ∂ r i = [ r i T ∗ ( t j − t i ) ] × \frac{ \partial err_p}{ \partial r_i} = [r_i^T *(tj - ti)]^× ∂ri∂errp=[riT∗(tj−ti)]×
3、位置残差对 p j p_j pj的导数
∂ e r r p ∂ p j = r i T \frac{ \partial err_p}{ \partial p_j} = r_i^T ∂pj∂errp=riT
4、位置残差对 r j r_j rj的导数为0
5、旋转残差对 p x p_x px 任一位置的导数都是0
6、旋转惨差对 r i r_i ri的导数
∂ e r r r ∂ r i = J r − 1 ( e r r r ) ∗ ( δ r i j m ) T \frac{ \partial err_r}{ \partial r_i} = J^{-1}_{r}(err_r) * (\delta r^m_{ij})^T ∂ri∂errr=Jr−1(errr)∗(δrijm)T
7、旋转惨差对 r j r_j rj的导数
∂ e r r r ∂ r i = − J r − 1 ( e r r r ) ∗ ( e r r r ) T \frac{ \partial err_r}{ \partial r_i} = -J^{-1}_{r}(err_r) * (err_r)^T ∂ri∂errr=−Jr−1(errr)∗(errr)T
边缘化实现的几个步骤
1、求解参数的雅可比矩阵以及残差
2、按照前后顺序排放H阵和B（边缘化帧放前面这里只是第一帧）
3、利用H和B返向分解出J和res
4、下一次优化的时候加入J和res
总体实现方式也是看了vins的方式自己写了一下，具体步骤流程见gitee开源代码。
全局BA结果
没有固定第一帧的位姿
固定第一帧的位姿
可以看到在全局BA中只有固定住第一帧优化出来才是对的，这也是与理论对应的。
滑动窗口BA结果
滑动窗口不固定第一帧
可以看到如果不固定第一帧有优化出来的是有问题的。
固定滑动窗口第一帧的位姿
边缘化策略结果
实验开源代码
https://gitee.com/rbopen-source/marge_ceres
5370
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
