使用python做因子图优化（二维因子图优化1，附代码）

红色是真值轨迹，绿色是有偏差的里程计轨迹，蓝色是优化后的轨迹，可以看见，回环优化后，蓝色轨迹相对于绿色轨迹有了很大的改善；

我们使用了 python的因子图优化库 g2opy;  具体代码如下：

import numpy as np
import g2o

class PoseGraphOptimization(g2o.SparseOptimizer):
    def __init__(self):
        super().__init__()
        solver = g2o.BlockSolverX(g2o.LinearSolverCholmodX())
        # solver = g2o.BlockSolverSE3(g2o.LinearSolverCholmodSE3())
        solver = g2o.OptimizationAlgorithmLevenberg(solver)
        super().set_algorithm(solver)
        super().set_verbose(True)

    def optimize(self, max_iterations=20):
        print('num vertices:', len(super().vertices()))
        print('num edges:', len(super().edges()), end='\n\n')
        super().initialize_optimizatio