因子图前世今生

因子图最初理论的发展，是一系列摸索的过程：ref三川小哥

1、塞巴斯蒂安最开始就是研究图优化的，只不过换了一种模型表达方式，可能单纯为了水文章吧，叫做“平方根平滑建图”[1]（建议阅读一下），这篇文章提出的概念就是批量更新。这里上帝埋下一个伏笔，因为后来他们发现这个模型竟然有一个的彩蛋。因为他们通过平方根的方法构建好了矩阵后，新添加的变量都是从最后一行。

2、后来呢？Michael觉得，每个矩阵才添加了几行就得更新，能不能简化一点呢？这时候那个彩蛋就打开了，因为每次都是在最后面添加的，所以只要把后面的那几行“搞搞”就行了，就找到了一种Givens 旋转的方法就可以消元了（别说你没听过，数值方法必考知识点）。这时候作者就发了第二篇文章“iSAM”（增量平滑建图）[2]，这里面呢，作者用的思路是，正常情况下，用Givens增量消元，如果误差太大，那就固定隔几分钟来个全部优化（又用到了论文[1]）。

3、又过了一段时间，作者发现，为什么要分“增量优化”和“全局优化”两种模式呢？（作者大脑开始开挂了） 有没有这样一种方法呢，局部更新如果没有影响全局图，就更新局部；如果影响了全局，就更新全局？或者说有没有一种方法让我影响了多大区域就更新多大面积呢？

4、后来他就想到树结构（妙啊），从树的根部开始，受影响了多大范围，我就更新多大范围，嘻嘻嘻嘻。这个树该怎么构建呢？

作者又想了无数个夜晚，接近崩溃的时候，找到了一种贝叶斯树的结构。这次呢就不用分“批量”和“增量”两种模式了，而是智能调节，所有的数据都在树结构上，新加进来的放在树根部，受影响的区域肯定是在刚加进来的地方。把受影响的区域提取出来。

main step like the pic，这就成了最终的贝叶斯树构建的因子图优化了，即iSAM2[3].

5、作者 Michal 自己还开源了个简单的因子图软件，知道的人不多，其实如果想看源代码，这个更好懂一点，代码量很少，[源码：ori-drs/isam]。他担心大家看不懂，就把这三篇论文写成了一本书《Incremental Smoothing and Mapping》

【下载：http://www.cs.cmu.edu/~kaess/pub/Kaess08tro.pdf】

6、最后，佐治亚州立大学的这些教授，觉得算法不错呀，干脆封装成一个框架吧，就是GTSAM了[https://github.com/borglab/gtsam]，这个库编译好了以后就能出来一个matlab 的toolbox。

7、最后的最后，才出现了很多以这个框架为基础的应用性论文比如"SLAM ++"[4]和优化型论文“AprilSAM”[5]。

in order speed up ,reorder the variable and
总结：这些论文，建议 平方根sam[1],iSAM[2],iSAM2[3]这三篇原文阅读一下，体会下什么叫“平地起高楼”。其他两篇创新的，你可以借鉴看看他们怎么改进的。

二、书本和视频资源

1. 这套理论综合整理成的一本书《Incremental Smoothing and Mapping》值得一读原文
   【下载：http://www.cs.cmu.edu/~kaess/pub/Kaess08tro.pdf】

2. 作者M. Kaess的公开课视频，这个视频里最关键的是有一个演示动画，演示了贝叶斯树原来是这样动态构建的.

【B站：https://b23.tv/BV1gk4y1R7Lb 】
【油管：https://www.youtube.com/watch?v=_W3Ua1Yg2fk&t=2170s】

三、论文下载地址
GTSAM/iSAM1/2资源整理