《自动驾驶与机器人中的SLAM技术》第七章代码分析——松耦合里程计

0. 引言

本文主要关注松耦合里程计的数据处理部分，原因如下：

1. 该代码相对复杂，存在很多lamda表达式，类型的嵌套等程序技法
2. 该代码的架构清晰适合学习
3. 该代码的框架是后续的章节的基础，后续章节的算法有变化，但框架大部分继承自此

1. 关注问题

1. 雷达、IMU等数据的读入
2. 雷达、IMU等数据的处理
3. 后端的估计器（ESKF）经历了哪些流程？

2.雷达、IMU数据的读入

总体来看，数据读入流程如下：

1. 声明了一个sad::RosbagIO，该类的作用是读入各类传感器消息。
2. 通过callback把一帧雷达数据和这帧雷达扫描时间内的IMU数据打成一个包，称为measureGroup

	//1.声明了一个sad::RosbagIO，该类的作用是读入各类传感器消息。
	sad::RosbagIO rosbag_io(fLS::FLAGS_bag_path, sad::Str2DatasetType(FLAGS_dataset_type));
	//2. 通过sad::RosbagIO声明各类消息的callback和函数，callback函数则指定了各类消息的使用方式。
    rosbag_io
        .AddAutoPointCloudHandle([&](sensor_msgs::PointCloud2::Ptr cloud) -> bool {
            sad::common::Timer::Evaluate([&]() { lm.PCLCallBack(cloud); }, "loosely lio");
            return true;
        })
        .AddLivoxHandle([&](const livox_ros_driver::CustomMsg::ConstPtr& msg) -> bool {
            sad::common::Timer::Evaluate([&]() { lm.LivoxPCLCallBack(msg); }, "loosely lio");
            return true;
        })
        .AddImuHandle([&](IMUPtr imu) {
            lm.IMUCallBack(imu);
            return true;
        })
        .Go();

然后看各个callback。

2.1 callback函数

下面是各种callback函数

void LooselyLIO::PCLCallBack(const sensor_msgs::PointCloud2::ConstPtr &msg) { sync_->ProcessCloud(msg); }

void LooselyLIO::LivoxPCLCallBack(const livox_ros_driver::CustomMsg::ConstPtr &msg) { sync_->ProcessCloud(msg); }

void LooselyLIO::IMUCallBack(IMUPtr msg_in) { sync_->ProcessIMU(msg_in); }

可以看到，他们都使用了sync_成员的某些内部函数，因此继续看sync_，这也是这个代码初看比较麻烦的地方——层层嵌套。

2.2sync_内部的各类处理函数

sync_是LooselyLIO的成员，它的类型是MessageSync，这是对观测进行同步和处理的类，在MessageSync中包含了对各种传感器数据的处理函数，这里主要关注点云和IMU处理。

2.2.1处理IMU

    void ProcessIMU(IMUPtr imu) {
        double timestamp = imu->timestamp_;
        if (timestamp < last_timestamp_imu_) {
            LOG(WARNING) << "imu loop back, clear buffer";
            imu_buffer_.clear();
        }

        last_timestamp_imu_ = timestamp;
        imu_buffer_.emplace_back(imu);
    }

这里的函数主要就是把imu放到队列中。

2.2.2处理点云

    void ProcessCloud(const sensor_msgs::PointCloud2::ConstPtr &msg) {
        if (msg->header.stamp.toSec() < last_timestamp_lidar_) {
            LOG(ERROR) << "lidar loop back, clear buffer";
            lidar_buffer_.clear();
        }

        FullCloudPtr cloud(new FullPointCloudType());
        conv_->Process(msg, cloud);
        lidar_buffer_.push_back(cloud);
        time_buffer_.push_back(msg->header.stamp.toSec());
        last_timestamp_lidar_ = msg->header.stamp.toSec();

        Sync();
    }

点云的处理就比较复杂：

1. 将ROS消息转换成pcl点云
2. 把点云的时间戳和pcl点云放到buffer里
3. 调用函数sync（）把一帧雷达和该帧雷达扫描时间内的IMU数据打成一个包。
   至此IMU和雷达点云的数据读入就完成了，sync()函数的内部这里不展开，具体原理其实和fastlio是一样的，可以在github上找FAST-LIO的注释版

3.雷达、IMU等数据的处理

数据处理的入口在sync（）函数内部的一行代码：

   if (callback_) {
        callback_(measures_);
    }

所以要品牌搞清楚callback_哪里来的

3.1callback_哪来的？

下面截取了相关的代码：

//声明部分
using Callback = std::function<void(const MeasureGroup &)>;
Callback callback_;                             // 同步数据后的回调函数
MessageSync(Callback cb) : callback_(cb), conv_(new CloudConvert) {}
//
sync_ = std::make_shared<MessageSync>([this](const MeasureGroup &m) { ProcessMeasurements(m); });

可以看到callback_函数是在定义sync_的时候传入的，传入的是ProcessMeasurements()，而处理对象就是MeasureGroup，即前面提到的一帧雷达数据和这帧雷达扫描时间内的IMU数据打包组成了MeasureGroup

3.2ProcessMeasurements()

这边主要四步操作：

1. 初始化IMU
2. IMU预测
3. 去畸变
4. 进行点云配准，并用配准结果对状态进行更新。

void LooselyLIO::ProcessMeasurements(const MeasureGroup &meas) {
    LOG(INFO) << "call meas, imu: " << meas.imu_.size() << ", lidar pts: " << meas.lidar_->size();
    measures_ = meas;

    if (imu_need_init_) {
        // 初始化IMU系统
        TryInitIMU();
        return;
    }

    // 利用IMU数据进行状态预测
    Predict();

    // 对点云去畸变
    Undistort();

    // 配准
    Align();
}

4. 后端的估计器（ESKF）经历了哪些流程

ESKF的代码流程分为三步：

1. 初始化
2. 预测
3. 更新
   下面分开探究

4.1 初始化

代码中的初始化是一种静止的初始化，用于估计bias以及重力

1. 截取一段时间的IMU数据放到deque中
2. 计算加速度计和和陀螺仪的均值和方差
3. 估计重力，重新计算均值和协方差，使用均值作为初始的测量噪声和零偏
4. 读入初始化配置，比如测量模型的方差等等

//
bool StaticIMUInit::TryInit() {
    if (init_imu_deque_.size() < 10) {
        return false;
    }

    // 2.计算加速度计和和陀螺仪的均值和方差
    Vec3d mean_gyro, mean_acce;
    math::ComputeMeanAndCovDiag(init_imu_deque_, mean_gyro, cov_gyro_, [](const IMU& imu) { return imu.gyro_; });
    math::ComputeMeanAndCovDiag(init_imu_deque_, mean_acce, cov_acce_, [this](const IMU& imu) { return imu.acce_; });
	//3. 估计重力，估计测量噪声和零偏
    gravity_ = -mean_acce / mean_acce.norm() * options_.gravity_norm_;

    //4. 重新计算均值和协方差
    math::ComputeMeanAndCovDiag(init_imu_deque_, mean_acce, cov_acce_,
                                [this](const IMU& imu) { return imu.acce_ + gravity_; });

    // 估计测量噪声和零偏
    init_bg_ = mean_gyro;
    init_ba_ = mean_acce;
    
    init_success_ = true;
    return true;
}

原代码的ComputeMeanAndCovDiag使用了lambda表达式和accumulation等新特性，换成for循环方便理解：

void ComputeMeanAndCovDiag(const C& data, D& mean, D& cov_diag, Getter&& getter) {
    size_t len = data.size();
    assert(len > 1);

    // 初始化均值和协方差对角线为零向量
    mean = D::Zero();
    cov_diag = D::Zero();

    // 计算均值
    for (auto it = data.begin(); it != data.end(); ++it) {
        mean += getter(*it);
    }
    mean /= len;

    // 计算协方差
    for (auto it = data.begin(); it != data.end(); ++it) {
        auto diff = getter(*it) - mean;  // 计算当前数据项与均值的差
        cov_diag += diff.cwiseAbs2();    // 计算差的平方并累加到 cov_diag
    }
    cov_diag /= (len - 1);
}

下面是配置部分

    if (imu_init_.InitSuccess()) {
        // 读取初始零偏，设置ESKF
        sad::ESKFD::Options options;
        // 这部分代码阿其实就是把刚才IMU静态初始化的结果给到ESKF内部
        options.gyro_var_ = sqrt(imu_init_.GetCovGyro()[0]);
        options.acce_var_ = sqrt(imu_init_.GetCovAcce()[0]);
        eskf_.SetInitialConditions(options, imu_init_.GetInitBg(), imu_init_.GetInitBa(), imu_init_.GetGravity());
        imu_need_init_ = false;

        LOG(INFO) << "IMU初始化成功";
    }

sad::ESKFD::Options options;规定了eskf的配置参数，如下所示

    struct Options {
        Options() = default;

        /// IMU 测量与零偏参数
        double imu_dt_ = 0.01;  // IMU测量间隔
        // NOTE IMU噪声项都为离散时间，不需要再乘dt，可以由初始化器指定IMU噪声
        double gyro_var_ = 1e-5;       // 陀螺测量标准差
        double acce_var_ = 1e-2;       // 加计测量标准差
        double bias_gyro_var_ = 1e-6;  // 陀螺零偏游走标准差
        double bias_acce_var_ = 1e-4;  // 加计零偏游走标准差

        /// 里程计参数
        double odom_var_ = 0.5;
        double odom_span_ = 0.1;        // 里程计测量间隔
        double wheel_radius_ = 0.155;   // 轮子半径
        double circle_pulse_ = 1024.0;  // 编码器每圈脉冲数

        /// RTK 观测参数
        double gnss_pos_noise_ = 0.1;                   // GNSS位置噪声
        double gnss_height_noise_ = 0.1;                // GNSS高度噪声
        double gnss_ang_noise_ = 1.0 * math::kDEG2RAD;  // GNSS旋转噪声

        /// 其他配置
        bool update_bias_gyro_ = true;  // 是否更新陀螺bias
        bool update_bias_acce_ = true;  // 是否更新加计bias
    };

4.2预测

这里主要根据公式进行代码review

bool ESKF<S>::Predict(const IMU& imu) {
    assert(imu.timestamp_ >= current_time_);

    double dt = imu.timestamp_ - current_time_;

    // nominal state 递推,见公式3.16
    VecT new_p = p_ + v_ * dt + 0.5 * (R_ * (imu.acce_ - ba_)) * dt * dt + 0.5 * g_ * dt * dt;
    VecT new_v = v_ + R_ * (imu.acce_ - ba_) * dt + g_ * dt;
    SO3 new_R = R_ * SO3::exp((imu.gyro_ - bg_) * dt);

    R_ = new_R;
    v_ = new_v;
    p_ = new_p;
    // 其余状态维度不变

    // error state 递推
    // 计算运动过程雅可比矩阵 F，见(3.47)
    // F实际上是稀疏矩阵，也可以不用矩阵形式进行相乘而是写成散装形式，这里为了教学方便，使用矩阵形式
    Mat18T F = Mat18T::Identity();                                                 // 主对角线
    F.template block<3, 3>(0, 3) = Mat3T::Identity() * dt;                         // p 对 v
    F.template block<3, 3>(3, 6) = -R_.matrix() * SO3::hat(imu.acce_ - ba_) * dt;  // v对theta
    F.template block<3, 3>(3, 12) = -R_.matrix() * dt;                             // v 对 ba
    F.template block<3, 3>(3, 15) = Mat3T::Identity() * dt;                        // v 对 g
    F.template block<3, 3>(6, 6) = SO3::exp(-(imu.gyro_ - bg_) * dt).matrix();     // theta 对 theta
    F.template block<3, 3>(6, 9) = -Mat3T::Identity() * dt;                        // theta 对 bg

    // mean and cov prediction
    dx_ = F * dx_;  // 这行其实没必要算，dx_在重置之后应该为零，因此这步可以跳过，但F需要参与Cov部分计算，所以保留
    cov_ = F * cov_.eval() * F.transpose() + Q_;
    current_time_ = imu.timestamp_;
    return true;
}

4.3 更新

以下代码处罚更新：

eskf_.ObserveSE3(pose_of_lo_, 1e-2, 1e-2);

以下是ObsereSE3的内容

template <typename S>
bool ESKF<S>::ObserveSE3(const SE3& pose, double trans_noise, double ang_noise) {
    /// 既有旋转，也有平移
    /// 观测状态变量中的p, R，H为6x18，其余为零
    //公式3.50 ， 3.66 , 3.70
    Eigen::Matrix<S, 6, 18> H = Eigen::Matrix<S, 6, 18>::Zero();
    H.template block<3, 3>(0, 0) = Mat3T::Identity();  // P部分
    H.template block<3, 3>(3, 6) = Mat3T::Identity();  // R部分（3.66)

    // 卡尔曼增益和更新过程
    Vec6d noise_vec;
    noise_vec << trans_noise, trans_noise, trans_noise, ang_noise, ang_noise, ang_noise;

    Mat6d V = noise_vec.asDiagonal();
    //3.51a
    Eigen::Matrix<S, 18, 6> K = cov_ * H.transpose() * (H * cov_ * H.transpose() + V).inverse();

    // 更新x和cov,公式3.65 3.69
    Vec6d innov = Vec6d::Zero();
    innov.template head<3>() = (pose.translation() - p_);          // 平移部分
    innov.template tail<3>() = (R_.inverse() * pose.so3()).log();  // 旋转部分(3.67)
    //3.51b , 3.51d
    dx_ = K * innov;
    cov_ = (Mat18T::Identity() - K * H) * cov_;

    UpdateAndReset();
    return true;
}

以下是UpdateAndReset()，代表了ESKF的后续更新操作

    void UpdateAndReset() {
        //3.55
        p_ += dx_.template block<3, 1>(0, 0);
        v_ += dx_.template block<3, 1>(3, 0);
        R_ = R_ * SO3::exp(dx_.template block<3, 1>(6, 0));

        if (options_.update_bias_gyro_) {
            bg_ += dx_.template block<3, 1>(9, 0);
        }

        if (options_.update_bias_acce_) {
            ba_ += dx_.template block<3, 1>(12, 0);
        }

        g_ += dx_.template block<3, 1>(15, 0);
        //3.63
        ProjectCov();
        //3.57
        dx_.setZero();
    }

5.总结

本文梳理了第七章的整体代码框架。重点看了数据处理，以及ESKF部分。并按照书中公式走了一遍ESKF的流程。
关于数据处理部分，首先把一帧雷达点云和其间的IMU数据打包，做成一个MeasureGroup，完成读入操作，难点是其中的架构。
对于ESKF部分，ESKF主要经历了以下步骤

1. IMU初始化，计算均值和方差，并以此估计重力，并以均值作为IMU的噪声
2. 利用IMU预测
3. 利用lidar匹配结果进行更新
   2、3部分具体的代码和推导注释可查看文中代码
   值的注意的是，整个系统在IMU的坐标系下进行融合。