在Pose-Graph的过程中，如果使用G2O优化函数库，那么似乎是不用自己编写代价函数（也就是优化目标函数）的，因为G2O有封装好的SE3等格式，使得Pose-Graph的过程变得简单了，即只需要设置好vertex（优化变量），以及edge（vertex与vertex之间的trans）,然后调用G2O的优化器，即可完成对Pose的优化；其实PoseGraph中的边本质上就是一个代价函数，只不过是G2O封装了，一旦我们使用Geres优化库，边就得自己构建，也就是需要自己构建优化目标函数。
其中，在构建姿态角的优化目标函数的时候，不能用RPY做差，用旋转矩阵，结合罗德里格斯公式，推导如下：