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

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

#include "ch3/imu_integration.h"
#include "common/io_utils.h"
#include "tools/ui/pangolin_window.h"

DEFINE_string(imu_txt_path, "./data/ch3/10.txt", "数据文件路径");
DEFINE_bool(with_ui, true, "是否显示图形界面");

/// 本程序演示如何对IMU进行直接积分
/// 该程序需要输入data/ch3/下的文本文件，同时它将状态输出到data/ch3/state.txt中，在UI中也可以观察到车辆运动
int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc, &argv, true);

    if (FLAGS_imu_txt_path.empty()) {
        return -1;
    }

    sad::TxtIO io(FLAGS_imu_txt_path);

    // 该实验中，我们假设零偏已知
    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);

    //IMU积分类初始化（使用重力加速度，陀螺仪零偏，加速度计零偏）
    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调用Go函数运行
    io.SetIMUProcessFunc([&imu_integ, &save_result, &fout, &ui](const sad::IMU& 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);
    }

    if (ui) {
        ui->Quit();
    }

    return 0;
}

    // 增加imu读数
    // 【注意这里】重力的方向不会受到物体的旋转影响，但是自身加速矢量会受到本身旋转的影响。
    void AddIMU(const IMU& imu) {
        double dt = imu.timestamp_ - timestamp_;
        if (dt > 0 && dt < 0.1) {
            // 假设IMU时间间隔在0至0.1以内
            //假设模型为匀加速直线运动
            //p_ 初始位姿
            //v_ * dt 匀速直线运动部分
            //0.5 * gravity_ * dt * dt 重力加速部分（固定值） 
            //0.5 * (R_ * (imu.acce_ - ba_)) * dt * dt //受旋转影响的直线加速部分
            p_ = p_ + v_ * dt + 0.5 * gravity_ * dt * dt + 0.5 * (R_ * (imu.acce_ - ba_)) * dt * dt;

            // v_ 初始速度
            // R_ * (imu.acce_ - ba_) * dt 受旋转影响的加速部分
            // gravity_ * dt 受重力影响的加速部分
            v_ = v_ + R_ * (imu.acce_ - ba_) * dt + gravity_ * dt;
            
            //R_ 初始旋转姿态
            //Sophus::SO3d::exp((imu.gyro_ - bg_) * dt) //变换的旋转姿态
            //cartographer还用到了IMU的线速度，慢速情况下IMU的线速度其实是对重力的测量，cartographer其实是背包模型，一般情况下都是慢速。
            //这里没有用到imu的线速度，应该是更准确一些的
            R_ = R_ * Sophus::SO3d::exp((imu.gyro_ - bg_) * dt);
        }
        // 更新时间
        timestamp_ = imu.timestamp_;
    }

其他：
cartographer使用pose_extrapolator来融合IMU、odom、激光数据来推断位姿

关于cartographer使用IMU的注释

// Keeps track of the orientation using angular velocities and linear
// accelerations from an IMU. Because averaged linear acceleration (assuming
// slow movement) is a direct measurement of gravity, roll/pitch does not drift,
// though yaw does.