前言

最近遇到一个数据集的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 的变换关系来获得

使用TF工具包获取

使用ros里面的工具包，来监听两个坐标系之间的变换

rosrun tf tf_echo base_link d400_imu

播放数据集之后得到：

直接从这个TF读出来的感觉还是有挺大误差的，因为那个欧拉角旋转应该理论上接近90°才比较正常。下面，我们通过手动计算出来看看有多大差别

使用Eigen库计算

#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左右，最大偏差也减少了不少。