结果可以输出绝对姿态吗？就是相对于ENU坐标系（或者称为导航坐标系）

[zhao-zhibo]

您好，之前请教过您一个gps轨迹和LIO里程计轨迹分开的问题（https://github.com/JokerJohn/LIO_SAM_6AXIS/issues/57 ），已经得到解决。因为我在适配我的数据（gnss，lidar，imu），我看计算的结果里面包含优化的gps轨迹，里程计的轨迹，以及优化的里程计轨迹。如果我想得到绝对位置（大地坐标系，也就是平时说的纬经高或者ECEF坐标系）和绝对姿态（相对于ENU坐标系），那我是不是就只能去从优化的gps轨迹进行输出了，另外绝对姿态可以输出吗？

[JokerJohn]

这里有段代码已经给出了演示。

[JokerJohn]

在使用GPS factor后因子图优化后的输出轨迹就是ENU系下的绝对位姿。关于这一点，你推导下GPS factor的雅可比矩阵就明白了，简单的说 GPS. factor 中的残差 e = Tp.translation - p_gps. Tp代表系统融合位姿，p_gps代表GPS的ENU 观测。那么GPS factor的目标就是最小化融合位姿对GPS观测的残差，进而优化融合位姿Tp。残差为3维位置差，优化6维位姿，那么其雅可比矩阵H2就为 36，并且残差对旋转维度没有任何约束。考虑到因子图中位姿使用SE3，GTSAM中是SE3整体右扰动求导，那么这个雅可比在求的时候，需要链式法则，即先融合位姿对平移求导得到（36），然后整体雅可比就是H2，变成代码一行就解决了。从这个过程看，最终因子图优化就是让残差和最小，那么融合后的位姿自然是在ENU坐标系下了。

[zhao-zhibo]

@JokerJohn 非常感谢您的讲解和回答，并且您的回答非常高效和及时。我正在适配我的数据，数据没有闭环。

• [ ] 1.我看了您上面截图的代码，在datasaver.cpp中已经将大地坐标系和在enu系下的绝对姿态输出，但是输出的频率是0.4s一个结果，能不能将输出结果频率调高一点呢，调到10HZ？我看了配置文件，但是还找到合适的参数。看了其他人的issue，应该是这三个参数吧，surroundingkeyframeAddingAngleThreshold surroundingkeyframeAddingDistThreshold mappingProcessInterval？
• [ ] 2. 我用evo评价了大地坐标系下ape和rpe，将组合导航的结果作为真值，ape的平均误差为0.119米，最大为0.246米，rpe的平均误差为0.684，最大误差为1.167。一般rpe误差都这么大吗？我真值的频率是100HZ，里程计融合输出频率是0.4s，我不确定这个是不是跟二者的输出频率有关，因为一个是100HZ，一个输出是0.4s。
• [ ] 3. 我在您的代码mapOptmizationGps.cpp中加了以下代码，可是后者earth1不能构造成功，我很纳闷的是我看其他人GitHub（https://github.com/mavlink/mavros/issues/1520 ）上都是这么用的，并且 GeographicLib::Geocentric有这样的构造函数。我只能用前者构成成功earth，不知道您有什么见解？

  GeographicLib::Geocentric earth = GeographicLib::Geocentric(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
  // GeographicLib::Geocentric earth1(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());

• [ ] 4. 您在这次回答中提到，那么GPS factor的目标就是最小化融合位姿对GPS观测的残差，进而优化融合位姿Tp，我能从感性上理解。但是之前没有具体推导过因子图优化这部分内容，用过UKF，EKF。所以对于因子图优化这块儿，不知道您是否有什么推荐的学习部分？

[JokerJohn]

1.选取关键帧是SLAM的基本问题，自己搜下。2.融合精度与你的GNSS观测精度有关系，你设置的斜方差都一样大，意味着你认为加入的每个GNSS观测都同样可信，这与实际情况明显不同。另外融合精度与你的数据距离有关系，1公里ATE误差0.1米，我觉得很不错了，RPE误差0.6米说明你的相对位姿很不准。3.没用过 4.不管因子图，EKF UKF，对于具体同一个观测，其误差和雅可比推导方式都是一样的。你推导下雅可比，就明白了。这与你用什么优化框架无关。