1.构建posegraph 顶点结构包含路标id和位姿vector3d。比如我有6个位姿节点: VERTEX 0 0 0 0 VERTEX 1 10.5 0.5 2.093 VERTEX 2 11.5 12.5 -2.617 VERTEX 3 -1 8.0 -1.221 VERTEX 4 -9.5