g2o库简单入门

整体框架：

is-a：相当于一个等号

has-a：表示包含一个，这个包含的属于必备的组件

has-many：和has-a差不多，至少要有一个

        先从第4步SparseOptimizer这个类开始看(顺序4->3->2->1)，它需要一个优化器OptimazationAlgorithm，优化器需要优化算法支撑，g2o中提供了三种优化算法可供选择，Gauss-Newton（高斯牛顿法，简称GN）、Levernberg-Marquardt（简称LM法）、Powell’s DogLeg。而优化算法OptimazationWithHessian需要求解器(Solver)，同样的可以有多种求解器来选择，Solver是由一个BlockSolver组成的。这个BlockSolver有两个部分，一个是SparseBlockMatrix，用于计算稀疏的雅可比和Hessian矩阵；一个是线性方程的求解器（LinearSolver），它用于计算迭代过程中最关键的一步HΔx=−b，LinearSolver有几种方法可以选择：PCG, CSparse, Choldmod。

        再回到第4步(顺序4->5)，类似的可以看到SparseOptimizer就是一个超图(HyperGraph)，它由多个边和多个顶点组成。

---

关于图优化和g2o库：

        所谓的图优化，就是把一个常规的优化问题，以图(Graph)的形式来表述。图优化适用于无约束优化问题，并且保证该优化的目标函数的海森矩阵存在且是稀疏的。

        g2o是一个优秀的图优化求解库，数据接收主要包括三种数据类型：

        第一种是边，第二种是顶点(vertex)，第三种是求解器。

        在使用g2o库的过程中，通常首先是定义优化器，包括三个过程：

                第一步：定义一个稀疏线性方程求解器，如CG，CSparse，Cholmod等，用于计算雅克比和海森矩阵的解析形式。

                第二步：定义所使用的优化算法，通常为GN或LM算法。

                第三步：定义优化器。之后定义图的顶点和边，将其加入优化器，最终求解得到优化结果。

---

顶点和边：

g2o中，对于优化问题都统统抽象成边和节点

        顶点：待优化的变量

        边：顶点之间的变换关系，常用误差表示

---

文件结构： 

---

 举个🌰，曲线拟合：

         以上就是一个最简单的SparseOptimizer对象的构造方法，有了这个优化器，然后再添加边和顶点：

        下面两张图就定义完成了，曲线拟合任务优化的顶点和边

 然后就需要将顶点和边添加到优化器中：

        最终使得红色线能够以最小的误差跟踪样本中的点。以上就是一个完整的g2o优化方法的使用流程。

        鲁棒核函数：g2o中提供了鲁棒核函数来抑制某些误差特别大的点，避免拉偏整个优化结果。(鲁棒核函数不是g2o独有的，这是非线性优化方法中的一种常用手段)

        信息矩阵：现在来考虑另一种情况，比方说在一次优化中，对于某一次测量，我们有十足的把握，它非常的准确，所以优化时我们希望对于这次测量给予更高的权重。

        如上图，假设认为右上角那个异常点是一个比较正确的点，希望拟合的曲线尽量往这个点偏移。那么就这可以设置这次测量边的权重更大。代码如下：

        e->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 10);

        因为测量值的维度为1，所以信息矩阵也为1。如果我们把每一条边的信息矩阵都设置为一样，那么在优化时将认为所有边的优化权重是一样的，将不会对某一条边执行过多的优化！对于那个异常点设置权重为别的点的10倍，则曲线会往右上角那个点靠。最终的结果如下图：

        一般情况下，信息矩阵和鲁棒核函数都会一起使用！ 

引用：非线性优化库g2o使用教程，探索一些常见的用法，以及信息矩阵、鲁棒核函数对于优化的结果的影响_一点儿也不萌的萌萌的博客-CSDN博客_g2o教程