SLAM十四讲---十一、位姿图优化

首先先学习最小二乘问题

https://blog.csdn.net/weixin_45634390/article/details/137337145?csdn_share_tail=%7B%22type%22%3A%22blog%22%2C%22rType%22%3A%22article%22%2C%22rId%22%3A%22137337145%22%2C%22source%22%3A%22weixin_45634390%22%7D

目标优化函数

那么问题就变成了求解目标函数，待优化函数

在这个位姿图优化问题中，优化的是一个非线性最小二乘问题，目的是最小化所有观测误差的加权和。这个误差是由位姿图中所有边上的测量误差构成的。每条边表示两个节点（即位姿）之间的相对位姿测量，误差是测量值与根据当前位姿估计计算出的预期值之间的差异。

具体地，优化的目标函数形式如下：

$$\underset{\xi }{\min }\frac{1}{2}\sum _{(i,j)\in E}{e}_{ij}(\xi {)}^T{\Sigma }_{ij}^{-1}{e}_{ij}(\xi )$$

其中，

• $E$ 是所有边的集合。
• $e_{ij}(\xi )$ 是边 $(i,j)$ 上的误差向量，表示节点 $i$ 和节点 $j$ 之间的相对位姿测量误差。在SLAM中，这个误差通常包括位置和方向的差异。
• ${\Sigma }_{ij}^{-1}$ 是边 $(i,j)$ 上的信息矩阵（即测量误差的协方差矩阵的逆），表示测量的置信度。
• $\xi$ 表示所有节点位姿的集合，是优化问题的决策变量。

误差 $e_{ij}(\xi )$ 的计算依赖于两个节点当前的位姿估计，并且与边 $(i,j)$ 关联的观测值。在实践中，这个误差通常通过比较边上的观测位姿（例如，来自传感器测量）与根据两个节点当前估计位姿计算出的相对位姿之间的差异来计算。

优化问题的关键在于找到一组位姿 $\xi$，使得上述目标函数最小。这通常通过迭代方法完成，如高斯-牛顿法、Levenberg-Marquardt法或Dogleg法。每次迭代都会计算当前估计位姿下的误差及其关于位姿的导数（即雅可比矩阵），然后更新位姿估计以减少总误差。这个过程重复进行，直到达到一定的收敛条件，例如迭代次数、位姿更新量或目标函数改善量。

代码实现

1、
computeError函数用于计算位姿图优化问题中一条边的误差

    // 误差计算与书中推导一致
    virtual void computeError()
    {
        Sophus::SE3 v1 = (static_cast<VertexSE3LieAlgebra*> (_vertices[0]))->estimate();
        Sophus::SE3 v2 = (static_cast<VertexSE3LieAlgebra*> (_vertices[1]))->estimate();
        _error = (_measurement.inverse()*v1.inverse()*v2).log();//(11.2)(11.3)
    }

对应书中推导：

2、

    
    // 雅可比计算
    virtual void linearizeOplus()
    {
        Sophus::SE3 v1 = (static_cast<VertexSE3LieAlgebra*> (_vertices[0]))->estimate();
        Sophus::SE3 v2 = (static_cast<VertexSE3LieAlgebra*> (_vertices[1]))->estimate();
        Matrix6d J = JRInv(SE3::exp(_error));
        // 尝试把J近似为I？
        _jacobianOplusXi = - J* v2.inverse().Adj();//(11.8)
        _jacobianOplusXj = J*v2.inverse().Adj();//(11.9)
    }

对应：

3、
代入1、2就得到了目标函数