红色是真值轨迹,绿色是有偏差的里程计轨迹,蓝色是优化后的轨迹,可以看见,回环优化后,蓝色轨迹相对于绿色轨迹有了很大的改善;
我们使用了 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