(02)Cartographer源码无死角解析-(18) SensorBridge→landmark、Imu、GPS 数据预处理
本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
文 末 正 下 方 中 心 提 供 了 本 人 联 系 方 式 ， 点 击 本 人 照 片 即 可 显 示 W X → 官 方 认 证 {\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式，\color{blue}点击本人照片即可显示WX→官方认证} 文末正下方中心提供了本人联系方式，点击本人照片即可显示WX→官方认证
一、前言
上一篇博客，对 SensorBridge::HandleOdometryMessage() 函数进行了比较细致的分析。
//所有里程计 topic回调函数
Node::HandleOdometryMessage() = SensorBridge::HandleOdometryMessage()
其主要的核心内容就是如何把里程计 odom 数据由原来的 child_frame_id(footprint) 坐标系转换到 imu_link(tracking_frame_) 坐标系下。最后还可以看到如下一段代码：
trajectory_builder_->AddSensorData(
sensor_id,
carto::sensor::OdometryData{odometry_data->time, odometry_data->pose});
该部分具体类容后续再进行讲解了，接下来看看 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)
他们都位于 src/cartographer_ros/cartographer_ros/cartographer_ros/sensor_bridge.cc 文件夹中。
二、HandleLandmarkMessage
如果需要调试 HandleLandmarkMessage，需要使用前面提供的运行指令：
roslaunch cartographer_ros landmark_mir_100.launch
否则的化，无法执行到 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、参数
从上面可以看到可以看到两个参数，第一个参数 sensor_id 其是就是 node_constants.h 文件中定义的 kLandmarkTopic(有可能经过重映射之后改变，且还与传感器的个数相关)，本人这里的 sensor_id=“landmark”, 另外一个参数就是：
const cartographer_ros_msgs::LandmarkList::ConstPtr& msg
终端执行指令 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 //旋转的权重
然后会调用 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));
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() 对数据进行进一步的处理。
三、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、参数
从上面可以看到可以看到两个参数，第一个参数 sensor_id 其是就是 node_constants.h 文件中定义的 kLandmarkTopic(有可能经过重映射之后改变，且还与传感器的个数相关)，本人这里的 sensor_id=“imu”, 另外一个参数就是：
const sensor_msgs::Imu::ConstPtr& msg
终端执行指令 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 //线速度协方差
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说明没有数据，则报错。可以看到其调用了 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.";
也就是 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)))});
}
大家的直觉没有错，确实复杂了一些，不过没有关系，一步步进行分析即可。如果想要调试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
终端执行指令 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 //协方差类型
2、不可用数据
如果GPS数据并非固定解（msg->status.status != sensor_msgs::NavSatStatus::STATUS_NO_FIX()），则表示该数据是不值得信任的，可以直接理解为不可用如以下都是可能的情况：
①GPS导航仪没有连接有效的GPS天线；
②CGPS定位误差明显偏大；
③GPS卫星出现故障；
④GPS内部电池的电量不足；
⑤过桥或者隧道的时候。
对于并非固定解的情况，源码中会丢弃这些数据，然后传递一个给空值(absl::optional
3、求变换矩阵
进行数据筛选之后，就是进行数据坐标系的变换了。 先查查看变量 Rigid3d ecef_to_local_frame_ 是否有值，有值就表示之前求其进行初始化了，其存储的是 ecef 坐标系到 local坐标系 的变换，这里的 local 可以理解为第一帧数据的坐标系。
//确定ecef(地心坐标系)原点(地球的质心)到局部坐标系的坐标变换
ecef_to_local_frame_ =ComputeLocalFrameFromLatLong(msg->latitude, msg->longitude);
该函数的注释如下：
/**
* @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);
}
该函数的主要作用是 →计算ECEF坐标系到第一帧GPS数据坐标系的变换, 后续用这个坐标变换乘以之后的GPS数据
就得到了之后的GPS数据相对于第一帧GPS数据的相对坐标变换，先来看下图：
根据图示，该函数的核心思想就是为了实现红色字体的功能(求得ECEF坐标系到local坐标的变换关系)。
( 1 ) ： \color{blue}(1)： (1)： 其首先调用 LatLongAltToEcef(latitude, longitude, 0.) 函数，将经纬度数据转换到ECEF坐标系下，其公式是固定的，有兴趣的朋友可以通过
https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#From_geodetic_to_ECEF_coordinates
进行详细的了解。也就是说 translation 表示ECEF坐标系原到 (latitude, longitude) 位置的平移。
( 2 ) ： \color{blue}(2)： (2)： 计算ECEF坐标系到 local 坐标系的旋转矩阵记为 rotation，该旋转矩阵表示先绕Y轴调整经度，然后再绕Z轴调整纬度。
( 3 ) ： \color{blue}(3)： (3)： 求得ECEF坐标系到local坐标的变换矩阵T(源码对应变量ecef_to_local_frame_.value())，推导公式如下：
T = [ R t 0 1 ] 设 T − 1 = [ A b c d ] 由 于 : T T − 1 = E (01) \color{Green} \tag{01} \mathbf T =\begin{bmatrix} \mathbf R& \mathbf t\\ \\ 0 & 1 \end{bmatrix}~~~~~~~~~~~~~~~设 \mathbf T^{-1}=\begin{bmatrix} \mathbf A& \mathbf b\\ \\ c & d \end{bmatrix} ~~~~~~~~~~~~由于:~~\mathbf T \mathbf T^{-1}=\mathbf E T=⎣⎡R0t1⎦⎤ 设T−1=⎣⎡Acbd⎦⎤ 由于: TT−1=E(01) 所 以 { R A + c t = E c = 0 R b + d t = 0 d = 1 得 : { A = R − 1 t = − R − 1 t 所 以 ： T − 1 = [ R − 1 − R − 1 t 0 1 ] (02) \color{Green} \tag{02}所以 \begin{cases} \mathbf R \mathbf A + c\mathbf t=\mathbf E\\ c=0\\ \mathbf R \mathbf b+d \mathbf t=0\\ d=1 \end{cases}~~~~~~~~得: \begin{cases} \mathbf A=\mathbf R^{-1} \\ \\ \mathbf t=-\mathbf R^{-1}\mathbf t\\ \end{cases}~~~~~~~所以：\mathbf T^{-1}=\begin{bmatrix} \mathbf R^{-1}& -\mathbf R^{-1}\mathbf t\\ \\ 0 & 1 \end{bmatrix} 所以⎩⎪⎪⎪⎨⎪⎪⎪⎧RA+ct=Ec=0Rb+dt=0d=1 得:⎩⎪⎨⎪⎧A=R−1t=−R−1t 所以：T−1=⎣⎡R−10−R−1t1⎦⎤(02)
4、数据坐标系变换
求得变换矩阵之后就比较简单了，之后把所有获得的GPS数据，先转换成ECEF坐标系下的数据，然后左乘ecef_to_local_frame_.value()，即可以把坐标变换到 local(第一帧GPS数据) 坐标系下。
五、结语
通过该篇博客，了解到了 landmark、Imu、GPS 的预处理过程，也就是把传感器数据都转换到 tracking_frame=“imu_link” 坐标系下(GPS 是转换到第一帧坐标系)。剩下的还有点云数据还没有进行讲解，也就是接下来的主要内容了。