#pragma once
#include <ros/ros.h>
#include <eigen3/Eigen/Dense>
#include <iostream>
#include <map>
#include "../factor/imu_factor.h"
#include "../feature_manager.h"
#include "../utility/utility.h"

using namespace Eigen;
using namespace std;

class ImageFrame {
 public:
  ImageFrame(){};
  ImageFrame(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &_points, double _t)
      : t{_t}, is_key_frame{false} {
    points = _points;
  };
  map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> points;
  double t;
  Matrix3d R;
  Vector3d T;
  IntegrationBase *pre_integration;
  bool is_key_frame;
};

/**
 * @brief 视觉与IMU对齐，主要解决三个问题:
 *        1) 修正陀螺仪的bias
 *        2）初始化速度、重力向量g和尺度因子(Metric scale)
 *        3) 改进重力向量的量值
 *
 * @param all_image_frame
 * @param Bgs
 * @param g
 * @param x
 * @return
 */
bool VisualIMUAlignment(map<double, ImageFrame> &all_image_frame, Vector3d *Bgs, Vector3d &g, VectorXd &x);