高翔《自动驾驶中的slam技术》第3章代码-run_eskf_gins.cc

COCO_PEAK_NOODLE

1142人浏览 · 2023-06-26 10:13:24

COCO_PEAK_NOODLE · 2023-06-26 10:13:24 发布

eskf 不太懂的看看高博的《视觉SLAM14讲》和和高博的简明ESKF
说实话，我看了4-5遍了，只能浅浅领悟，我感觉很多东西被公式化了，不要看到一大堆公式就被吓倒，这些公式一一简化就是+ - * /。
其实如果能有一本数学的科普书，或者视频教程就好了。把一些什么协方差，滤波，偏导，指数，反对称矩阵，旋转矩阵，扰动，旋转向量，四元数，以直观的形式表达。
所以到现在我还是只能浅浅理解。阶段性记录，与君共享。

ESKF与KF 、EKF有什么不同？一个是过滤当前值
过滤你可以理解成平均，1号评委打分9.7，2号评委打分9.5，那选手的得分过滤后是9.6=（9.7+9.5）/ 2，这里可以理解为KF和EKF的方式
ESKF与KF、EKF有什么不同呢？还是用评委打分的方式，1号评委打分为9.7（基础得分9.6+印象分0.1），2号评委打分为9.5（基础得分9.6+印象分-0.1），那使用ESKF如何计算选手的最终得分呢？
最终的分：9.6+（0.1+ （-0.1））/2 = 9.6
有的同学说如果2个评委的基础得分不一样怎么办？那9.6是不是也要平均一下？
这个就是算法，你想怎么处理都可以，公式是死的，人是活的，也许人家的公式也要优化与改进。
ESKF只是一个数据融合的方式，可能比KF、EKF稳定一些吧。

//
// Created by xiang on 2021/11/11.
//

#include "ch3/eskf.hpp"
#include "ch3/static_imu_init.h"
#include "common/io_utils.h"
#include "tools/ui/pangolin_window.h"
#include "utm_convert.h"

#include <gflags/gflags.h>
#include <glog/logging.h>
#include <fstream>
#include <iomanip>

DEFINE_string(txt_path, "/home/peak/slam_in_autonomous_driving-master/data/ch3/10.txt", "数据文件路径");
DEFINE_double(antenna_angle, 12.06, "RTK天线安装偏角（角度）");
DEFINE_double(antenna_pox_x, -0.17, "RTK天线安装偏移X");
DEFINE_double(antenna_pox_y, -0.20, "RTK天线安装偏移Y");
DEFINE_bool(with_ui, true, "是否显示图形界面");
DEFINE_bool(with_odom, false, "是否加入轮速计信息");

/**
 * 本程序演示使用RTK+IMU进行组合导航
 */
int main(int argc, char** argv) {
    //谷歌的日志库
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc, &argv, true);

    if (fLS::FLAGS_txt_path.empty()) {
        return -1;
    }

    // 初始化器
    sad::StaticIMUInit imu_init;  // 使用默认配置
    //eskf类
    sad::ESKFD eskf;

    sad::TxtIO io(FLAGS_txt_path);
    //GPS显示的位置
    Vec2d antenna_pos(FLAGS_antenna_pox_x, FLAGS_antenna_pox_y);
    //这是一个函数，用来保存数据，怎么用？看下面，
    //【】是什么意思，说实话我又忘记了，还有【=】【&】，
    // 如果真的不习惯这种用法，把他当成函数指针。【】【=】【&】不去管他（换汤不换药，也许以后用多了就习惯了，）
    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() << " ";
    };

    auto save_result = [&save_vec3, &save_quat](std::ofstream& fout, const sad::NavStated& save_state) {
        fout << std::setprecision(18) << save_state.timestamp_ << " " << std::setprecision(9);
        save_vec3(fout, save_state.p_);
        save_quat(fout, save_state.R_.unit_quaternion());
        save_vec3(fout, save_state.v_);
        save_vec3(fout, save_state.bg_);
        save_vec3(fout, save_state.ba_);
        fout << std::endl;
    };

    std::ofstream fout("./data/ch3/gins.txt");
    bool imu_inited = false, gnss_inited = false;

    //共享指针，C++要自己回收内存的诞生产品，感觉做的还有漏洞，所以又诞生了另一个指针类型weak_ptr;把它当成一个指针就好了。
    std::shared_ptr<sad::ui::PangolinWindow> ui = nullptr;
    if (FLAGS_with_ui) {
        ui = std::make_shared<sad::ui::PangolinWindow>();
        ui->Init();
    }

    /// 设置各类回调函数
    bool first_gnss_set = false;
    Vec3d origin = Vec3d::Zero();

    //这个代码不要懵，换汤不换药，相当于分步执行以下4步。
    //  io.SetIMUProcessFunc();
    //  io.SetGNSSProcessFunc();
    //  io.SetOdomProcessFunc();
    //  io.Go();
    // 为什么要这样写？你看不懂是我最大的愿望，哈哈，我的理解（有学过javaweb的同学就能很好的理解了，IDEA可以方便生成一个类的Get和Set，其实就是给客户开放了便捷的操作接口）
    // 那高博连起来写是作什么？他其实就是想让你看不懂，没有什么其他，哈哈！原理同stdout的 << 符号。
    io.SetIMUProcessFunc([&](const sad::IMU& imu) {
          /// IMU 处理函数
          if (!imu_init.InitSuccess()) {
              imu_init.AddIMU(imu);
              return;
          }

          /// 需要IMU初始化
          if (!imu_inited) {
              // 读取初始零偏，设置ESKF
              sad::ESKFD::Options options;
              // 噪声由初始化器估计
              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_inited = true;
              return;
          }

          if (!gnss_inited) {
              /// 等待有效的RTK数据
              return;
          }

          /// GNSS 也接收到之后，再开始进行预测
          eskf.Predict(imu);

          /// predict就会更新ESKF，所以此时就可以发送数据
          auto state = eskf.GetNominalState();
          if(ui){
              ui->UpdateNavState(state);
          }

          /// 记录数据以供绘图
          save_result(fout, state);

          usleep(1e3);
      })
        .SetGNSSProcessFunc([&](const sad::GNSS& gnss) {
            /// GNSS 处理函数
            if (!imu_inited) {
                return;
            }

            sad::GNSS gnss_convert = gnss;
            if (!sad::ConvertGps2UTM(gnss_convert, antenna_pos, FLAGS_antenna_angle) || !gnss_convert.heading_valid_) {
                return;
            }

            /// 去掉原点
            if (!first_gnss_set) {
                origin = gnss_convert.utm_pose_.translation();
                first_gnss_set = true;
            }
            gnss_convert.utm_pose_.translation() -= origin;

            // 要求RTK heading有效，才能合入ESKF
            eskf.ObserveGps(gnss_convert);

            auto state = eskf.GetNominalState();
            if (ui) {
                ui->UpdateNavState(state);
            }
            save_result(fout, state);

            gnss_inited = true;
        })
        .SetOdomProcessFunc([&](const sad::Odom& odom) {
            /// Odom 处理函数，本章Odom只给初始化使用
            imu_init.AddOdom(odom);
            if (FLAGS_with_odom && imu_inited && gnss_inited) {
                eskf.ObserveWheelSpeed(odom);
            }
        })
        .Go();

    while (ui && !ui->ShouldQuit()) {
        usleep(1e5);
    }
    if (ui) {
        ui->Quit();
    }
    return 0;
}