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

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

使用g2opy 做一个简单的三维点数据回环优化 Slam(附python代码)_第1张图片






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

你可能感兴趣的:(点云-激光雷达处理代码合集,python为主,python,开发语言,c++,算法,点云)