高翔-自动驾驶与机器人中的SLAM技术第三章代码解析（1）

实验一： IMU 数据进行轨迹的推算

实验代码

ch3/imu_integration.h

实现了一个 IMU（惯性测量单元）积分功能，用于根据 IMU 数据更新载体（比如无人机、小车 ）的位置 p、速度 v和姿态 R

#ifndef SLAM_IN_AUTO_DRIVING_IMU_INTEGRATION_H
#define SLAM_IN_AUTO_DRIVING_IMU_INTEGRATION_H

#include "common/eigen_types.h"
#include "common/imu.h"
#include "common/nav_state.h"

namespace sad {

// IMU积分器类，封装状态和操作，方便模块化使用
class IMUIntegration {
   public:
    IMUIntegration(const Vec3d& gravity, const Vec3d& init_bg, const Vec3d& init_ba)
        : gravity_(gravity), bg_(init_bg), ba_(init_ba) {}

    // 处理 IMU 数据并更新状态
    void AddIMU(const IMU& imu) {
        double dt = imu.timestamp_ - timestamp_;
        if (dt > 0 && dt < 0.1) {
            // 假设IMU时间间隔在0至0.1以内
            p_ = p_ + v_ * dt + 0.5 * gravity_ * dt * dt + 0.5 * (R_ * (imu.acce_ - ba_)) * dt * dt;
            v_ = v_ + R_ * (imu.acce_ - ba_) * dt + gravity_ * dt;
            R_ = R_ * Sophus::SO3d::exp((imu.gyro_ - bg_) * dt);
        }

        // 更新时间
        timestamp_ = imu.timestamp_;
    }

    /// 状态获取接口
    NavStated GetNavState() const { return NavStated(timestamp_, R_, p_, v_, bg_, ba_); }

    SO3 GetR() const { return R_; }
    Vec3d GetV() const { return v_; }
    Vec3d GetP() const { return p_; }

   private:
    // 累计量
    SO3 R_;
    Vec3d v_ = Vec3d::Zero();
    Vec3d p_ = Vec3d::Zero();

    double timestamp_ = 0.0;

    // 零偏，由外部设定
    Vec3d bg_ = Vec3d::Zero();
    Vec3d ba_ = Vec3d::Zero();

    Vec3d gravity_ = Vec3d(0, 0, -9.8);  // 重力
};

}  // namespace sad

#endif  // SLAM_IN_AUTO_DRIVING_IMU_INTEGRATION_H

ch3/run_imu_integration.cc

IMU 数据积分程序的主函数，功能是读取 IMU 传感器数据，通过积分计算出载体（如小车、无人机）的位置、速度和姿态，并可实时可视化运动状态，同时将结果保存到文件中。

#include <glog/logging.h>
#include <iomanip>
#include "ch3/imu_integration.h"
#include "common/io_utils.h"
#include "tools/ui/pangolin_window.h"
// 定义命令行参数：IMU数据文件路径，默认值为"./data/ch3/10.txt"
DEFINE_string(imu_txt_path, "./data/ch3/10.txt", "数据文件路径");
// 定义命令行参数：是否显示图形界面，默认值为true
DEFINE_bool(with_ui, true, "是否显示图形界面");
/// 本程序演示如何对IMU进行直接积分
/// 该程序需要输入data/ch3/下的文本文件，同时它将状态输出到data/ch3/state.txt中，在UI中也可以观察到车辆运动
int main(int argc, char** argv) {
      google::InitGoogleLogging(argv[0]);  // 初始化Google日志库
    FLAGS_stderrthreshold = google::INFO;  // 设置日志输出级别为INFO及以上
    FLAGS_colorlogtostderr = true;         // 日志输出到控制台时带颜色
    google::ParseCommandLineFlags(&argc, &argv, true);  // 解析命令行参数（更新上面定义的FLAGS_xxx）

       if (FLAGS_imu_txt_path.empty()) {  // 检查IMU数据文件路径是否为空
        return -1;  // 路径为空则退出程序，返回错误码
    }

    sad::TxtIO io(FLAGS_imu_txt_path);  // 创建TxtIO对象，用于读取指定路径的IMU文本数据

    // 该实验中，我们假设零偏已知
    Vec3d gravity(0, 0, -9.8);  // 重力方向
    Vec3d init_bg(00.000224886, -7.61038e-05, -0.000742259);
    Vec3d init_ba(-0.165205, 0.0926887, 0.0058049);

    sad::IMUIntegration imu_integ(gravity, init_bg, init_ba);

      std::shared_ptr<sad::ui::PangolinWindow> ui = nullptr;  // 声明可视化窗口智能指针
    if (FLAGS_with_ui) {  // 如果命令行参数指定显示界面
        ui = std::make_shared<sad::ui::PangolinWindow>();  // 创建可视化窗口对象
        ui->Init();  // 初始化窗口（如设置窗口大小、坐标系等）
    }

      /// 记录结果
    auto save_result = [](std::ofstream& fout, double timestamp, const Sophus::SO3d& R, const Vec3d& v,
                          const Vec3d& p) {
        auto save_vec3 = [](std::ofstream& fout, const Vec3d& v) { fout << v[0] << " " << v[1] << " " << v[2] << " "; };
        auto save_quat = [](std::ofstream& fout, const Quatd& q) {
            fout << q.w() << " " << q.x() << " " << q.y() << " " << q.z() << " ";
        };

        fout << std::setprecision(18) << timestamp << " " << std::setprecision(9);
        save_vec3(fout, p);    // 保存位置
        save_quat(fout, R.unit_quaternion());  // 保存姿态（转换为四元数）
        save_vec3(fout, v);    // 保存速度
        fout << std::endl;
    };

    std::ofstream fout("./data/ch3/state.txt");
    io.SetIMUProcessFunc([&imu_integ, &save_result, &fout, &ui](const sad::IMU& imu) {
          imu_integ.AddIMU(imu);
          save_result(fout, imu.timestamp_, imu_integ.GetR(), imu_integ.GetV(), imu_integ.GetP());
          if (ui) {
              ui->UpdateNavState(imu_integ.GetNavState());
              usleep(1e2);
          }
      }).Go();

      // 打开了可视化的话，等待界面退出
    while (ui && !ui->ShouldQuit()) {
        usleep(1e4);  // 休眠10毫秒，等待用户关闭可视化窗口
    }

    if (ui) {
        ui->Quit();  // 关闭可视化窗口，释放资源
    }

    return 0;  // 程序正常退出
}

运行结果