Pose Graph的构建
图构建好，就需要进行优化
- 用一个图来表示SLAM问题
- 每一个节点都是位姿（x,y,theta）
- 两个节点之间的边，表示两个节点之间的空间约束
- 帧间边：表示时间连续的两个点
- 回环边：两个时间是不连续的两个点
- 一旦出现了回环边，就需要进行优化这个图。进行误差消除
- 里程积分的相对位姿视为预测值
- 回环计算的相对位姿视为观测值
- 调整回环的误差，即观测和预测之间的误差最小！非线性最小二乘优化
图的构建— 帧间边
- 里程计测量从$x_i$到$x_{i+1}$的关系
- 相邻节点之间的位姿关系可以有里程计、IMU、帧间匹配计算得到
回环检测:
- 节点i和节点j在空间上相邻，但是时间上不相邻
- 用帧间匹配算法算一个相对位姿
一个简单的回环检测方法
- 把节点分为active和inactive的两部分，时间比较进的是active，有点源的是inactive
- 找到当前节点周围一定范围内所有inactive的节点，作为回环候选帧
- 当前节点和回环候选镇进行匹配，根据得分判断是否形成回环
一旦回环检测成功，图就已经建立完成了，接下来就需要对图进行优化
图优化-非线性最小二乘的问题
此过程就是一个非线性最小二乘的问题
要解决的问题
给定一个系统，其状态方程为：$f(x)=z$
- x表示系统的状态向量-即需要估计的值
- z表示系统的观测值，可以通过传感器进行直接观测
- $f(x)$表示一个非线性的映射函数，状态向量x可以通过非线性函数$f(x)$映射得到z
给定该系统的n个混有噪声的观测值$(z_1,…,z_n)$，估计状态向量x，使得其经过$f(x)$映射之后的预测值和观测值的误差最小。
和下行二乘基本相同，只是状态方程$f(x)$是一个非线性函数
目标最小话预测和观测的差 ，因此误差即为预测和观测的差：
假设误差服从高斯分布$e_i(x)$~$N(0,\Omega_i)$,$\Omega_i$位对应的信息矩阵。
…
…
非线性最小二乘的目标函数为
也是一个加权的最小二乘法
如果此时$F(x)$是一个凸函数，可以直接求导等于0，求解方程
但是对于非凸函数来说，通常采用基于梯度的优化方法，需要把非线性函数进行线性化-泰勒展开
把$e_i(x)$泰勒展开带入式子（2），构建线性系统，求解线性系统
更新解，并且不断迭代直到收敛
向g2o、ceres等等库整体的主干流程都是符合上面的。
在图优化SLAM 中的应用
误差函数
- 观测值为匹配计算得到节点i和节点j的相对位姿，$Z_{ij}^\prime$
- 预测值为里程积分得到的当前节点i和节点j的相对位姿，$Z_{ij}$
- 求两个位姿之间的相对位姿$e_{ij}(x)=T2V(Z_{ij}^{-1}Z^\prime_{ij})$
预测值
已知$X_i$和$X_j$为两个位置的位姿。那么预测值应该是（也是先表示$X_i$先做反变换，然后变换到$X_j$上去）
若$X_i=\begin{bmatrix}R_i&t_i\\0&1\end{bmatrix}$，则$Z_{ij}=\begin{bmatrix}R_i^TR_j&R_i^T(t_j-t_i)\\0 &1\end{bmatrix}$。
那么如果求向量则为$z_{ij}=T2V(Z_{ij})=\left( R_i^T(t_j-t_i),\theta_j-\theta_i \right)^T$
现在去求它和观测值的一个相对位姿。因此结果为
这也就是求从预测值和观测值的一个位姿变换，也可以求相应的jacobian矩阵
接下来对误差函数进行线性化，求导的时候，注意 ，他只有和$x_i$和$x_j$项求导有值，其他的都是0
因为b，H求导的时候，这些都是非常稀疏的，因此用稀疏矩阵去求才可以实时
图之间都是两两之间的相对位置约束，即使移动了整个图，也没有关系，因此解是不唯一的，满足相对位姿约束的解有无穷多组。因此，我们只需要固定一个位姿。一般选择第一个位姿，
关于信息矩阵
信息矩阵用来代表边的不确定度，信息矩阵越大代表这条边在优化的过程中越重要。