使用g2opy 做一个简单的三维点数据回环优化 Slam（附python代码）

三维的点用二维的图画出来的，Z高度值都赋值0 ，根据自己的需要修改就行

import numpy as np
import g2o



# wechat 394467238


import matplotlib.pyplot as plt


gt = {  '0': {"state_vector": [0,0,0], "connected_frame":['world', '1']},
            '1': {"state_vector": [1,0,np.pi/2], "connected_frame":['0', '2']},
            '2': {"state_vector": [1,1,np.pi], "connected_frame":['1', '3']},
            '3': {"state_vector": [0,1,-np.pi/2], "connected_frame":['2', '4']},
            '4': {"state_vector": [0,0,0], "connected_frame":['3', '0']}    }


odometry = [
            [0, 0, 0],
            [1.0404914245326837, 0.032285132