可以读取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_frame = line_data[1]
frame = line_data[2]
x = float(line_data[3])
y = float(line_data[4])
