读取g2o 文件的python实现

可以读取2D 和 3D的 g2o 文件，并可以把四元数的位姿转换为节点和边数据；

import argparse
import numpy as np
import pyquaternion

####################
## File Format
####################
# Vertex
### 2D Robot Pose
##### VERTEX_SE2 i x y theta
# Edges: 
##### EDGE_SE2 i j x y theta info(x, y, theta)

### 3D Robot Pose
##### VERTEX_SE3:QUAT i x y z qx qy qz qw

##### EDGE_SE3:QUAT i j x y z qx qy qz qw Omega11 Omega12 .. Omega16 Omega22 .. Omega26 Omega33 .. Omega36 Omega44 .. Omega46 Omega55 .. Omega56 Omega66
####################

anchor_frame_line = 'FIX'
edge_line_2d = "EDGE_SE2"
vertex_line_2d = "VERTEX_SE2"
landmark_line_2d = "VERTEX_XY"
edge_quat_3d = "EDGE_SE3:QUAT"
vertex_quat_3d = "VERTEX_SE3:QUAT"

def parse_2d_edge_line(line_data):
    ref_fra