SLAM8-后端的其他方法，BA与图优化，Pose Graph优化的理论与公式详解、因子图优化

1、BA方法
Bundle Adjustment翻译过来是“光束调整”，意思是每个特征反射的光束，通过调整它们的空间位置和相机姿态，使它们都汇聚到相机光心，这个过程叫BA。
假定世界坐标系下的点p,通过K,T之后转换为了像素坐标，把这个过程用方程描述为z=h(x,y)其中x是位姿T，y是路标点p，误差函数写为：

通过同时把T,P作为优化量来进行调整，对误差函数进行求解，就是所谓的BA
2、BA的求解
由于误差函数不是线性的，所以我们想到用增量△x的方法，逐步迭代。当给误差函数一个增量△x时，函数变为：

对相机姿态的偏导数记为F，对路标点的偏导数记为E，把所有的位姿作为一个变量xc，所有的空间点作为一个变量xp，进一步化简为：

由于变量xc,xp很多，而且后面还要求误差函数对于△x的导数为零来求解△x，进而引发H△x=g求解△x的问题，这里的H矩阵就会非常大。
例如，高斯牛顿法里面：

3、化简H
我们知道H具有了一定的特殊结构，而这个结构是因为JT*J的雅可比矩阵引起的。雅可比矩阵如下图，每一行例如第四行J14，就是第一个相机位置拍摄的图片中的第四个特征点，根据外参畸变内参计算到像素坐标的时候产生的误差对于相机的位姿和特征点产生的导数。

进一步导致H矩阵如下

把H矩阵进行分块划分，那么方程H△x=g可以进一步改写为：


把方程逐步化简


这样会得到两个方程，先解开第一个方程，获得△xc,再把△xc带入第二个方程求出来△xp，这个过程叫做Schur消元或者边缘化
4、核函数
有时候误差函数会出现错误，因为有可能某个相机没有观察到这个点，但是却加了上去，所以我们要利用核函数改写一下误差函数。常用的核函数例如Huber函数，把误差限制在一个范围内才进行二次增长，超过阈值就变成一次增长，以防一个误匹配使劲优化，把整体都给带歪了。

5、Pose Graph优化：
BA和图优化，是把位姿和空间点放在一块，进行优化。特征点非常多，机器人轨迹越走越长，特征点增长的也很快。因此位姿图优化的意义在于：在优化几次以后把特征点固定住不再优化，只当做位姿估计的约束，之后主要优化位姿。

三角形的位姿T1,T2。。。是通过和世界坐标系的比较而得到的，蓝色的边是两个位姿之间相对运动的估计。
如何获得T1,T2：世界坐标系的路标点有了，然后根据图像坐标系下的点，估计世界坐标系与图像坐标系下的变换关系，解得位姿，这里的位置是有误差的。
如何估计它们的相对运动，也就是蓝色的线：
数学方式算出来的是：T1的逆乘以T2。
匹配方法是：通过T1和T2所在的图像进行匹配，比如单独算一个2D-2D之间的位姿变换或者光流法等，这样根据图像算出的位姿相对变换很有可能是和T1与T2用数学方式计算得到的位姿是有差异的
如何减小两种方法的差异：
相对运动公式如下，左边是根据匹配获得的，右边是根据数学公式获得的，两者是近似相等，这里公式稍微有问题

这个△的意思就是蓝色的边，两个位姿i和j之间的变换ij。之后左右两边同时乘以△Tij的逆，再求对数。如果Ti和Tj没有误差，那左右都应该是0，但是由于右边Ti,Tj有误差，所以要想办法调整Ti,Tj,使得右边的（eij）尽可能接近零
想求误差对于位姿Ti的偏导数和位姿Tj的偏导数来调整Ti,Tj，但是求导很麻烦，我们仍然想使用高斯牛顿法，求出增量，然后进行迭代。
从李代数的角度出发，给俩左扰动，一个是i的小扰动δξi，一个是j的δξj。不过Ti在误差函数里是逆矩阵，求了逆以后，i的小扰动δξi就去了右边：



把两个exp合并起来，然后泰勒展开，保留一阶项：



求出关于Ti,Tj的雅可比矩阵之后，由于雅可比矩阵比较复杂，通常取近似，

得到J，然后计算H，第三，根据H△x=g得到扰动量△x，并对原先俩位姿进行更新，实现优化。
把所有的顶点和边都考虑进来，总的目标函数就是：

6、因子图
因子图怎么看：

这图的意思是，路标l和位姿x1，在状态方程h的作用下一起决定了一个观测数据z。

位姿x1根据运动方程f和噪声u2一起构成了下一步的位姿x2，这个过程逐步递推。红框表示一次观测，蓝框表示一次运动。
蓝框可以看成是先验，红框是似然，在似然和先验的共同作用下，才能得到后验

因子图优化是否就是普通图优化换了种说法？不是。

区别在于：因子图是增量的处理后端优化。啥意思呢，机器人是运动的，不同的边和节点会被不断加入图中。每加入一个点，普通图优化是对整个图进行优化，所以比较麻烦和耗时。因子图的话相当于是保留中间结果，每加入一个点，对不需要重新计算的就直接用之前的中间结果，对需要重新计算的再去计算。以这种方式加速计算，避免冗余计算。

相机和路标点信息存储在problem-16-22106-pre.txt文件中，可以使用common.h中定义的BALProblem类读入文件内容，然后使用ceres或者g2o来解决BA问题.
rotation.h定义旋转变换所需要的函数,这里的旋转使用的是旋转向量
SnavelyReprojectionError.h定义：根据文件中的相机信息和路标点信息预测路标的归一化平面点的坐标，与实际拍摄到的图片的路标点坐标之差，作为误差。
代码1:使用ceres
https://blog.csdn.net/Summer_star_summer/article/details/107466298
代码2：使用g2o
https://blog.csdn.net/Summer_star_summer/article/details/107467107

代码3：pose graph优化:使用g2o自带的默认SE3顶点和边
https://blog.csdn.net/Summer_star_summer/article/details/107472214

代码4：pose graph优化:使用g2o自定义的顶点和边
https://blog.csdn.net/Summer_star_summer/article/details/107472615