(02)Cartographer源码无死角解析-(18) SensorBridge→landmark与Imu的数据处理

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

上一篇博客，对 SensorBridge::HandleOdometryMessage() 函数进行了比较细致的分析。

//所有里程计 topic回调函数
Node::HandleOdometryMessage()  =  SensorBridge::HandleOdometryMessage()
1
2

其主要的核心内容就是如何把里程计 odom 数据由原来的 child_frame_id(footprint) 坐标系转换到 imu_link(tracking_frame_) 坐标系下。最后还可以看到如下一段代码：

    trajectory_builder_->AddSensorData(
        sensor_id,
        carto::sensor::OdometryData{odometry_data->time, odometry_data->pose});
1
2
3

该部分具体类容后续再进行讲解了，接下来看看 SensorBridge 中，关于 landmark 与 Imu 的数据处理是如何的。也就是如下两个函数：

// 处理Landmark数据, 先转成carto的格式,再传入trajectory_builder_
void SensorBridge::HandleLandmarkMessage(const std::string& sensor_id,const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) 

// 调用trajectory_builder_的AddSensorData进行数据的处理
void SensorBridge::HandleImuMessage(const std::string& sensor_id,const sensor_msgs::Imu::ConstPtr& msg)

// 将ros格式的gps数据转换成相对坐标系下的坐标,再调用trajectory_builder_的AddSensorData进行数据的处理
void SensorBridge::HandleNavSatFixMessage(const std::string& sensor_id, const sensor_msgs::NavSatFix::ConstPtr& msg)
1
2
3
4
5
6
7
8

他们都位于 src/cartographer_ros/cartographer_ros/cartographer_ros/sensor_bridge.cc 文件夹中。
 

二、HandleLandmarkMessage

如果需要调试 HandleLandmarkMessage，需要使用前面提供的运行指令：

roslaunch cartographer_ros landmark_mir_100.launch
1

否则的化，无法执行到 HandleLandmarkMessage 这个程序，该函数的主要功能为处理Landmark数据, 先转成carto的格式,再传入trajectory_builder_，先来看一下主体分析，然后再进行细节分析：

// 处理Landmark数据, 先转成carto的格式,再传入trajectory_builder_
void SensorBridge::HandleLandmarkMessage(
    const std::string& sensor_id,
    const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) {
  // 将在ros中自定义的LandmarkList类型的数据, 转成LandmarkData
  auto landmark_data = ToLandmarkData(*msg);

  // 获取 tracking_frame到landmark的frame 的坐标变换
  auto tracking_from_landmark_sensor = tf_bridge_.LookupToTracking(
      landmark_data.time, CheckNoLeadingSlash(msg->header.frame_id));

  // 将数据转到tracking_frame下
  if (tracking_from_landmark_sensor != nullptr) {
    for (auto& observation : landmark_data.landmark_observations) {
      observation.landmark_to_tracking_transform =
          *tracking_from_landmark_sensor *
          observation.landmark_to_tracking_transform;
    }
  }
  // 调用trajectory_builder_的AddSensorData进行数据的处理
  trajectory_builder_->AddSensorData(sensor_id, landmark_data);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

1、参数

从上面可以看到可以看到两个参数，第一个参数 sensor_id 其是就是 node_constants.h 文件中定义的 kLandmarkTopic(有可能经过重映射之后改变，且还与传感器的个数相关)，本人这里的 sensor_id=“landmark”, 另外一个参数就是：

const cartographer_ros_msgs::LandmarkList::ConstPtr& msg
1

终端执行指令 rosmsg show LandmarkList 可以该消息的定义如下：

std_msgs/Header header //消息头
  uint32 seq //消息系列号
  time stamp //发布该消息的时间
  string frame_id //数据来源的坐标系，本人为 "camera"
  
//自定义的消息类型，实现于 src/cartographer_ros/cartographer_ros_msgs/msg/LandmarkEntry.msg 文件中
cartographer_ros_msgs/LandmarkEntry[] landmarks 
  string id //暂时不清楚
  //tracking_frame到landmark的frame 的坐标变换，也可以理解为landmark数据再tracking_frame坐标系下的位姿
  geometry_msgs/Pose tracking_from_landmark_transform //从
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
  float64 translation_weight //平移的权重
  float64 rotation_weight //旋转的权重
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

然后会调用 ToLandmarkData() 函数把 msg 转换成 struct LandmarkObservation 数据格式，赋值给 landmark_data。

2、变换关系

landmark_data 的数据是基于 msg->header.frame_id=“camera” 坐标系的，根据前面上一篇博客的介绍知道，可以使用
tf_bridge_.LookupToTracking() 函数找到 frame_id=“camera” 坐标系到 tracking_frame=“imu_link” 坐标系的变换关系。

  // 获取 tracking_frame到landmark的frame 的坐标变换
  auto tracking_from_landmark_sensor = tf_bridge_.LookupToTracking(
      landmark_data.time, CheckNoLeadingSlash(msg->header.frame_id));
1
2
3

tf_bridge_.LookupToTracking 函数在上一篇博客中，做了十分详细的介绍，这里就不再重复进行讲解了。总而言之，tracking_from_landmark_sensor 表示的就是 frame_id=“camera” 坐标系的坐标到 tracking_frame=“imu_link” 坐标系的变换矩阵。

3、数据转换

获得了转换关系 tracking_from_landmark_sensor 之后，下一步就是对数据进行转换了，注意，对于 landmark_data 数据中，其可能存在多个 landmark，所以其使用的是一个 for 循环，把所有的 landmark 都进行坐标系的转换。然后再调用 trajectory_builder_->AddSensorData() 对数据进行进一步的处理。变换的核心代码为：

      observation.landmark_to_tracking_transform =
          *tracking_from_landmark_sensor *
          observation.landmark_to_tracking_transform;
1
2
3

对应的变换公式可以表示为：
$$P^{imu}={\mathbf{T}}_{camera}^{imu}\ast P^{camera}\text{\hspace{1em}(01)}$$

 

三、HandleImuMessage

该函数的代码结构十分简单，因为其本人就是 tracking_frame=“imu_link” 坐标系下的数据，所以就不需要查询数据，然后再做数据坐标系的变变换了，代码注释如下：

// 调用trajectory_builder_的AddSensorData进行数据的处理
void SensorBridge::HandleImuMessage(const std::string& sensor_id,
                                    const sensor_msgs::Imu::ConstPtr& msg) {
  std::unique_ptr<carto::sensor::ImuData> imu_data = ToImuData(msg);
  if (imu_data != nullptr) {
    trajectory_builder_->AddSensorData(
        sensor_id,
        carto::sensor::ImuData{imu_data->time, imu_data->linear_acceleration,
                               imu_data->angular_velocity});
  }
}
1
2
3
4
5
6
7
8
9
10
11

1、参数

从上面可以看到可以看到两个参数，第一个参数 sensor_id 其是就是 node_constants.h 文件中定义的 kLandmarkTopic(有可能经过重映射之后改变，且还与传感器的个数相关)，本人这里的 sensor_id=“imu”, 另外一个参数就是：

const sensor_msgs::Imu::ConstPtr& msg
1

终端执行指令 rosmsg show Imu 可以该消息的定义如下：

std_msgs/Header header //消息头
  uint32 seq //消息序列号
  time stamp //时间戳
  string frame_id //该数据基于的坐标系
geometry_msgs/Quaternion orientation //旋转
  float64 x
  float64 y
  float64 z
  float64 w
float64[9] orientation_covariance //旋转协方差
geometry_msgs/Vector3 angular_velocity //角速度
  float64 x
  float64 y
  float64 z
float64[9] angular_velocity_covariance //角速度协方差
geometry_msgs/Vector3 linear_acceleration //线速度
  float64 x
  float64 y
  float64 z
float64[9] linear_acceleration_covariance //线速度协方差
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

2、数据格式变换

调用 ToImuData(msg) 函数，sensor_msgs::Imu::ConstPtr 类型转换成 carto::sensor::ImuData 格式，也就是 cartographer 核算算法处理 Imu数据时需要的格式。然后再通过 trajectory_builder_->AddSensorData() 函数把数据送给核心算法。ToImuData() 函数注释如下：

// 进行数据类型转换与坐标系的转换
std::unique_ptr<carto::sensor::ImuData> SensorBridge::ToImuData(
    const sensor_msgs::Imu::ConstPtr& msg) {
  // 检查是否存在线性加速度与角速度
  CHECK_NE(msg->linear_acceleration_covariance[0], -1)
      << "Your IMU data claims to not contain linear acceleration measurements "
         "by setting linear_acceleration_covariance[0] to -1. Cartographer "
         "requires this data to work. See "
         "http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html.";
  CHECK_NE(msg->angular_velocity_covariance[0], -1)
      << "Your IMU data claims to not contain angular velocity measurements "
         "by setting angular_velocity_covariance[0] to -1. Cartographer "
         "requires this data to work. See "
         "http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html.";

  const carto::common::Time time = FromRos(msg->header.stamp);
  const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
      time, CheckNoLeadingSlash(msg->header.frame_id));
  if (sensor_to_tracking == nullptr) {
    return nullptr;
  }
  // 推荐将imu的坐标系当做tracking frame
  CHECK(sensor_to_tracking->translation().norm() < 1e-5)
      << "The IMU frame must be colocated with the tracking frame. "
         "Transforming linear acceleration into the tracking frame will "
         "otherwise be imprecise.";
  // 进行坐标系的转换
  return absl::make_unique<carto::sensor::ImuData>(carto::sensor::ImuData{
      time, sensor_to_tracking->rotation() * ToEigen(msg->linear_acceleration),
      sensor_to_tracking->rotation() * ToEigen(msg->angular_velocity)});
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31

总来说，就是进行了了一个数据的检查，如线速度、角速度为-1说明没有数据，则报错。可以看到其调用了 tf_bridge_.LookupToTracking() 函数，查询了 Imu->Imu的坐标变换(因为 tracking_frame=“imu_link” )，所以该变换的平移应该为0，故执行了如下代码：

  // 推荐将imu的坐标系当做tracking frame
  CHECK(sensor_to_tracking->translation().norm() < 1e-5)
      << "The IMU frame must be colocated with the tracking frame. "
         "Transforming linear acceleration into the tracking frame will "
         "otherwise be imprecise.";
1
2
3
4
5

也就是 sensor_to_tracking 变量中的旋转为单位矩阵，平移为0。
 

四、HandleNavSatFixMessage

相对于 landmark、Imu 的预处理，大家可能觉得 GPS 的预处理复杂一些(因为代码多一些)，如下所示：

// 将ros格式的gps数据转换成相对坐标系下的坐标,再调用trajectory_builder_的AddSensorData进行数据的处理
void SensorBridge::HandleNavSatFixMessage(
    const std::string& sensor_id, const sensor_msgs::NavSatFix::ConstPtr& msg) {
  const carto::common::Time time = FromRos(msg->header.stamp);
  // 如果不是固定解,就加入一个固定的空位姿
  if (msg->status.status == sensor_msgs::NavSatStatus::STATUS_NO_FIX) {
    trajectory_builder_->AddSensorData(
        sensor_id,
        carto::sensor::FixedFramePoseData{time, absl::optional<Rigid3d>()});
    return;
  }

  // 确定ecef原点到局部坐标系的坐标变换
  if (!ecef_to_local_frame_.has_value()) {
    ecef_to_local_frame_ =
        ComputeLocalFrameFromLatLong(msg->latitude, msg->longitude);
    LOG(INFO) << "Using NavSatFix. Setting ecef_to_local_frame with lat = "
              << msg->latitude << ", long = " << msg->longitude << ".";
  }

  // 通过这个坐标变换 乘以 之后的gps数据,就相当于减去了一个固定的坐标,从而得到了gps数据间的相对坐标变换
  trajectory_builder_->AddSensorData(
      sensor_id, carto::sensor::FixedFramePoseData{
                     time, absl::optional<Rigid3d>(Rigid3d::Translation(//T_le*T_ce为把e坐标系下的数据变换到l坐标系
                               ecef_to_local_frame_.value() *   //T_le 表示ecef坐标系到local(第一帧)坐标系的变换
                               LatLongAltToEcef(msg->latitude, msg->longitude,  //Tce表示基于ecef坐标系的数据
                                                msg->altitude)))});
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

大家的直觉没有错，确实复杂了一些，不过没有关系，一步步进行分析即可。如果想要调试GPS，记得对 .lua 文件进行调整，需要设置 use_nav_sat = true, 除此之外，还要修改 .launch 文件，把 “bag_filename” 修改成又GPS消息发布的tag包。

1、参数

从上面可以看到可以看到两个参数，第一个参数 sensor_id 其是就是 node_constants.h 文件中定义的 kLandmarkTopic(有可能经过重映射之后改变，且还与传感器的个数相关)，本人这里的 sensor_id=“fix”, 另外一个参数就是：

const sensor_msgs::NavSatFix::ConstPtr& msg
1

终端执行指令 rosmsg show NavSatFix 可以该消息的定义如下：

std_msgs/Header header //消息头
  uint32 seq //消息序列号
  time stamp //时间戳
  string frame_id //该数据来源的坐标系
sensor_msgs/NavSatStatus status //GPS数据状态
  int8 STATUS_NO_FIX=-1 //非固定解，无法确定位置
  int8 STATUS_FIX=0 //未能精准定位
  int8 STATUS_SBAS_FIX=1 //通过卫星增强
  int8 STATUS_GBAS_FIX=2 //利用地面增强
  uint16 SERVICE_GPS=1  //GPS导航系统
  uint16 SERVICE_GLONASS=2 //GLONASS导航系统
  uint16 SERVICE_COMPASS=4 //中国北斗卫星导航系统
  uint16 SERVICE_GALILEO=8 //# 伽利略导航系统
  int8 status //该次数据的状态
  uint16 service //该次数据的导航系统
float64 latitude //纬度→ 正数位于赤道以北； 负面是南方。
float64 longitude //经度→正数位于本初子午线以东； 负面是西方 
float64 altitude //海拔高度→正值高于WGS 84椭球（如果没有可用的海拔高度，则为NaN）
float64[9] position_covariance //位置协方差

//If the covariance of the fix is known, fill it in completely. If the
//GPS receiver provides the variance of each measurement, put them
//along the diagonal. If only Dilution of Precision is available,
//estimate an approximate covariance from that.
//3 - 如果已知修正的协方差，请完全填写。
//2 - 如果GPS接收器提供了每次测量的方差，请将其沿对角线放置。
//1 - 如果只有“精度稀释”可用，请据此估计近似协方差。

uint8 COVARIANCE_TYPE_UNKNOWN = 0
uint8 COVARIANCE_TYPE_APPROXIMATED = 1 
uint8 COVARIANCE_TYPE_DIAGONAL_KNOWN = 2
uint8 COVARIANCE_TYPE_KNOWN = 3

uint8 position_covariance_type //协方差类型
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34

2、不可用数据

如果GPS数据并非固定解（msg->status.status != sensor_msgs::NavSatStatus::STATUS_NO_FIX()），则表示该数据是不值得信任的，可以直接理解为不可用如以下都是可能的情况：
        ①GPS导航仪没有连接有效的GPS天线；
        ②CGPS定位误差明显偏大；
        ③GPS卫星出现故障；
        ④GPS内部电池的电量不足；
        ⑤过桥或者隧道的时候。
对于并非固定解的情况，源码中会丢弃这些数据，然后传递一个给空值(absl::optional())给cartographer算法。另外可以做更多的判断，比如协方差较大，也表示该数据不可靠。

3、求变换矩阵

进行数据筛选之后，就是进行数据坐标系的变换了。 先查查看变量 Rigid3d ecef_to_local_frame_ 是否有值，有值就表示之前求其进行初始化了，其存储的是 ecef 坐标系到 local坐标系 的变换，这里的 local 可以理解为第一帧数据的坐标系。

//确定ecef(地心坐标系)原点(地球的质心)到局部坐标系的坐标变换
ecef_to_local_frame_ =ComputeLocalFrameFromLatLong(msg->latitude, msg->longitude);
1
2

该函数的注释如下：

/**
 * @brief 计算ECEF坐标系到第一帧GPS数据坐标的变换矩阵, 用这个变换矩阵,乘以之后的GPS数据
 * 就得到了之后GPS数据相对于第一帧GPS数据的位姿
 * 
 * @param[in] latitude 维度数据
 * @param[in] longitude 经度数据
 * @return cartographer::transform::Rigid3d ECEF坐标系到第一帧GPS数据坐标的变换矩阵
 */
cartographer::transform::Rigid3d ComputeLocalFrameFromLatLong(
    const double latitude, const double longitude) {
  //将经纬度数据转换成ecef坐标系下的坐标,也就是相对于ecef原点的平移,
  //虽然海拔altitude设置为0,但是会根据公式计算出海拔高度
  const Eigen::Vector3d translation = LatLongAltToEcef(latitude, longitude, 0.);

  //这里的 rotation 表示为第一帧GPS数据(locata)坐标系到ECEF坐标系旋转矩阵的逆
  //等价于ECEF坐标系到第一帧GPS数据(locata)坐标系的旋转
  const Eigen::Quaterniond rotation = 
      //绕着Y轴旋转就是调整经度
      Eigen::AngleAxisd(cartographer::common::DegToRad(latitude - 90.),
                        Eigen::Vector3d::UnitY()) *  //(1,0,0)
      绕Z轴旋转就是调整纬度
      Eigen::AngleAxisd(cartographer::common::DegToRad(-longitude),
                        Eigen::Vector3d::UnitZ()); //(0,0,1)

  //返回的是第一帧GPS数据(locata)坐标系到ECEF坐标系变换的逆。
  //等价于ECEF坐标系到第一帧GPS数据(locata)坐标的变换
  return cartographer::transform::Rigid3d(rotation * -translation, rotation);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

该函数的主要作用是 →计算ECEF坐标系到第一帧GPS数据坐标系的变换, 后续用这个坐标变换乘以之后的GPS数据
就得到了之后的GPS数据相对于第一帧GPS数据的相对坐标变换，先来看下图：

根据图示，该函数的核心思想就是为了实现红色字体的功能(求得ECEF坐标系到local坐标的变换关系)。

$(1)：$ 其首先调用 LatLongAltToEcef(latitude, longitude, 0.) 函数，将经纬度数据转换到ECEF坐标系下，其公式是固定的，有兴趣的朋友可以通过
https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates
进行详细的了解。也就是说 translation 表示ECEF坐标系原到 (latitude, longitude) 位置的平移。

$(2)：$ 计算ECEF坐标系下的坐标到 local 坐标系的旋转矩阵源码中记为 rotation，该旋转矩阵表示先绕Y轴调整经度，然后再绕Z轴调整纬度。

$(3)：$ 求得ECEF坐标到local坐标的变换矩阵T(源码对应变量ecef_to_local_frame_.value())，推导公式如下：
$$\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ & \\ 0& 1\end{array}\right]设{\mathbf{T}}^{-1}=\left[\begin{array}{cc}\mathbf{A}& \mathbf{b}\\ & \\ c& d\end{array}\right]由于:\mathbf{T}{\mathbf{T}}^{-1}=\mathbf{E}\text{\hspace{1em}(02)}$$$$所以\{\begin{array}{l}\mathbf{R}\mathbf{A}+c\mathbf{t}=\mathbf{E}\\ c=0\\ \mathbf{R}\mathbf{b}+d\mathbf{t}=0\\ d=1\end{array}得:\{\begin{array}{l}\mathbf{A}={\mathbf{R}}^{-1}\\ \\ \mathbf{t}=-{\mathbf{R}}^{-1}\mathbf{t}\end{array}所以：{\mathbf{T}}^{-1}=\left[\begin{array}{cc}{\mathbf{R}}^{-1}& -{\mathbf{R}}^{-1}\mathbf{t}\\ & \\ 0& 1\end{array}\right]\text{\hspace{1em}(03)}$$

4、数据坐标系变换

求得变换矩阵之后就比较简单了，之后把所有获得的GPS数据，先转换成ECEF坐标系下的数据，然后左乘ecef_to_local_frame_.value()，即可以把坐标变换到 local(第一帧GPS数据) 坐标系下。
 

五、结语

通过该篇博客，了解到了 landmark、Imu 的预处理过程，也就是把传感器数据都转换到 tracking_frame=“imu_link” 坐标系下。GPS 是把后续的数据都转换到第一帧坐标系下。剩下的还有点云数据还没有进行讲解，也就是接下来的主要内容了。