最近遇到一个数据集的ground true参考坐标和vSLAM输出的位姿的参考坐标不一样的问题，记录一下。
在之前参加的一个PRCV 2022的多传感器融合SLAM挑战赛中也同样遇到类似的问题：
因为之前一直忙于其他事情没有记录，这里记录一下，其实解决办法很简单，就是在最后输出的位姿做一个坐标变换即可。
最近需要使用到一个由清华大学一个团队构建的数据集 OpenLORIS-Scene Dataset 这个数据是一个做动态场景SLAM比较合适的数据集，里面包含很多行人干扰的场景。它的数据集采集平台是一个类似于服务机器人的平台：
它上面传感器的坐标系如图所示：
因为数据集中的真值是机器人base_link到gt_map坐标系的仿射变换，但是使用vinsfusion得到的位姿的载体是D435i的IMU的，所以我们在最后输出位姿的时候需要转换到base_link坐标系下。
数据集中的各个传感器之间的静态TF变换在话题/tf_static中进行了发布，但是这里没有直接d400_imu到base_link的TF变换。下面是echo这个话题的输出
transforms:
-
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: "base_link"
child_frame_id: "laser"
transform:
translation:
x: 0.35
y: 0.0
z: 1.28
rotation:
x: -0.009737
y: 0.05002
z: -0.004493
w: 0.9986
-
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: "laser"
child_frame_id: "d400_color"
transform:
translation:
x: 0.0559649
y: 0.0311189
z: -0.0756753
rotation:
x: -0.52531
y: 0.494111
z: -0.469561
w: 0.50933
-
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: "d400_color"
child_frame_id: "d400_depth"
transform:
translation:
x: 0.0147083755582571
y: 3.83682090614457e-05
z: 0.000288475974230096
rotation:
x: 0.000504782
y: 0.00444093
z: -0.00264019
w: 0.999987
-
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: "d400_color"
child_frame_id: "d400_imu"
transform:
translation:
x: 0.0200968869030476
y: -0.0050785536877811
z: -0.0115051260218024
rotation:
x: -0.000504782
y: 0.00444093
z: -0.00264019
w: 0.999987
-
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: "base_link"
child_frame_id: "t265_fisheye1"
transform:
translation:
x: 0.873
y: 0.026
z: 0.68
rotation:
x: -0.395
y: 0.406
z: -0.6
w: 0.565
-
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: "t265_fisheye1"
child_frame_id: "t265_fisheye2"
transform:
translation:
x: 0.0640782490372658
y: 0.000390329543733969
z: -0.000322926469380036
rotation:
x: 0.00186088
y: 0.0031785
z: 0.000162728
w: 0.999993
-
header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: "t265_fisheye1"
child_frame_id: "t265_imu"
transform:
translation:
x: 0.0106999985873699
y: 7.27595761418343e-12
z: -2.91038304567337e-11
rotation:
x: 0.00554841
y: 0.00136098
z: 0.99998062
w: -0.00245956
---
既然没有直接的结果，那我们通过 d400_imu ——> d400_color ——> laser ——> base_link 的变换关系来获得
使用ros里面的工具包，来监听两个坐标系之间的变换
rosrun tf tf_echo base_link d400_imu
播放数据集之后得到：
直接从这个TF读出来的感觉还是有挺大误差的，因为那个欧拉角旋转应该理论上接近90°才比较正常。下面，我们通过手动计算出来看看有多大差别
#include <iostream>
#include <vector>
#include <algorithm>
#include <Eigen/Core>
#include <Eigen/Geometry>
int convert()
{
Eigen::Quaterniond q_ci(0.999987, -0.000504782, 0.00444093, -0.00264019);
Eigen::Vector3d t_ci(0.0200968869030476, -0.0050785536877811, -0.0115051260218024);
Isometry3d T_ci(q_ci);
T_ci.pretranslate(t_ci);
Eigen::Quaterniond q_lc(0.50933, -0.52531, 0.494111, -0.469561);
Eigen::Vector3d t_lc(0.0559649, 0.0311189, -0.0756753);
Isometry3d T_lc(q_lc);
T_lc.pretranslate(t_lc);
Eigen::Quaterniond q_bl(0.9986, -0.009737, 0.05002, -0.004493);
Eigen::Vector3d t_bl(0.35, 0.0, 1.28);
Isometry3d T_bl(q_bl);
T_bl.pretranslate(t_bl);
Isometry3d T_bi = T_bl * T_lc * T_ci;
Eigen::Quaterniond q_bi(T_bi.rotation());
cout <<"Rotation Matrix: \n" << T_bi.matrix() << endl;
cout << "Quaternion " << q_bi.coeffs().transpose() << endl;
cout << "Translation " << T_bi.translation().transpose() << endl;
cout << "RPY (degree): " << T_bi.rotation().eulerAngles(2, 1, 0).transpose()*180/M_PI << endl;
return 0;
};
输出如下：
可以看到，这里我们手动计算得到的结果和直接从TF监听得到的结果还是有差别的，主要在于TF监听得到的结果的精度是比较低的，他舍去了小数点后一些位，导致在反复累乘过程中积累了一定的误差。后面我们还是直接使用我们手动计算出来的结果。
在代码中添加一个变换函数
static Eigen::Isometry3d transformBaselink(const Eigen::Quaterniond& q, const Eigen::Vector3d& t) {
Eigen::Isometry3d Twi(q);
Twi.pretranslate(t);
Eigen::Isometry3d Tbi(Eigen::Quaterniond(0.550447, -0.51761, 0.453209, -0.472959));
Tbi.pretranslate(Eigen::Vector3d(0.388943, 0.0084219, 1.20522));
Eigen::Isometry3d Twb = Tbi * Twi * Tbi.inverse();
return Twb;
}
然后在保存位姿之前做一次转换即可
下面我们跑同一个数据集输出用输出的位姿和真值做评估，对比一下做了这个转换和没做这个转换看看有多大的差别
从上面结果来看，确实是会有一定的影响，平均误差降低了不少，在没做转换之后平均误差在1.3m左右，转换为真值参考坐标之后平均误差在1.1m左右，最大偏差也减少了不少。