一、麻烦的边缘化
维持一个固定数量的滑窗的目的:
• 控制运算量;• 选择删除次新帧或最老帧,是为了让滑窗中的帧有足够多的视差量;• 删除最老帧时,要以边缘化约束的形式,保留最老帧对其他帧的约束 ( 理解这句话的意思 );
如何选择是删除次新帧还是最老帧?
计算相邻帧的视差量
![](https://img-blog.csdnimg.cn/20210917215657121.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
二、根据删除最老帧还是次新帧,对滑窗进行更新:slideWindow()
![](https://img-blog.csdnimg.cn/20210917215807621.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
三、slideWindow():更新滑窗
![](https://img-blog.csdnimg.cn/20210917215933836.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
![](https://img-blog.csdnimg.cn/20210917220005160.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
注意: 分清什么是
优化变量,什么是
需要边缘化掉的变量;
![](https://img-blog.csdnimg.cn/20210917220028514.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
四、边缘化的大管家:MarginalizationInf
![](https://img-blog.csdnimg.cn/20210917220113498.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
五、MarginalizationInfo中的几个变量
![](https://img-blog.csdnimg.cn/20210917220153271.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
![](https://img-blog.csdnimg.cn/20210917220213814.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
![](https://img-blog.csdnimg.cn/20210917220238457.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
![](https://img-blog.csdnimg.cn/20210917220259277.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
六、继续刚才的例子
![](https://img-blog.csdnimg.cn/20210917220344697.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
七、addResidualBlockInfo:
(1)添加一个视觉因子
![](https://img-blog.csdnimg.cn/20210917220445356.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
(2)添加IMU因子
![](https://img-blog.csdnimg.cn/20210917220527179.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
八、MarginalizationInfo::preMarginalize
![](https://img-blog.csdnimg.cn/20210917220603555.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
![](https://img-blog.csdnimg.cn/20210917220625840.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
九、如何构建边缘化的Hessian矩阵?
将每个误差项关于相应变量的 Jacobian 相乘,放到对应的 H 矩阵块中即可
![](https://img-blog.csdnimg.cn/2021091722073787.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
如何构建边缘化的Hessian矩阵?先建立个空的Hessian矩阵
![](https://img-blog.csdnimg.cn/20210917220803931.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
![](https://img-blog.csdnimg.cn/20210917220837743.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_18,color_FFFFFF,t_70,g_se,x_16)
如何构建边缘化的Hessian矩阵?一个视觉因子
![](https://img-blog.csdnimg.cn/20210917220927129.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
![](https://img-blog.csdnimg.cn/2021091722094723.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
![](https://img-blog.csdnimg.cn/20210917221023871.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
![](https://img-blog.csdnimg.cn/20210917221052306.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
十、得到H矩阵后,进行Schur补,得到marg因子
![](https://img-blog.csdnimg.cn/20210917221127843.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
十一、MarginalizationFactor长什么模样?
![](https://img-blog.csdnimg.cn/20210917221217826.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
![](https://img-blog.csdnimg.cn/20210917221240987.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
![](https://img-blog.csdnimg.cn/20210917221300900.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
十二、如何构建边缘化的Hessian矩阵?
![](https://img-blog.csdnimg.cn/20210917221355809.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)
![](https://img-blog.csdnimg.cn/20210917221413327.png?x-oss-process=image/watermark,type_ZHJvaWRzYW5zZmFsbGJhY2s,shadow_50,text_Q1NETiBA6LS15Zyo5Z2a5oyB77yM5LiN5b-Y5Yid5b-D,size_20,color_FFFFFF,t_70,g_se,x_16)