#include "estimator.h"

Estimator::Estimator() : f_manager{Rs} {
  ROS_INFO("init begins");
  clearState();
}

void Estimator::setParameter() {
  for (int i = 0; i < NUM_OF_CAM; i++) {
    tic[i] = TIC[i];
    ric[i] = RIC[i];
  }
  f_manager.setRic(ric);

  ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
  ProjectionTdFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();

  td = TD;
}

void Estimator::clearState() {
  for (int i = 0; i < WINDOW_SIZE + 1; i++) {
    Rs[i].setIdentity();
    Ps[i].setZero();
    Vs[i].setZero();
    Bas[i].setZero();
    Bgs[i].setZero();
    dt_buf[i].clear();
    linear_acceleration_buf[i].clear();
    angular_velocity_buf[i].clear();

    if (pre_integrations[i] != nullptr) delete pre_integrations[i];
    pre_integrations[i] = nullptr;
  }

  for (int i = 0; i < NUM_OF_CAM; i++) {
    tic[i] = Vector3d::Zero();
    ric[i] = Matrix3d::Identity();
  }

  for (auto &it : all_image_frame) {
    if (it.second.pre_integration != nullptr) {
      delete it.second.pre_integration;
      it.second.pre_integration = nullptr;
    }
  }

  solver_flag = INITIAL;
  first_imu = false;
  frame_count = 0;
  solver_flag = INITIAL;
  initial_timestamp = 0;
  all_image_frame.clear();
  td = TD;

  if (tmp_pre_integration != nullptr) delete tmp_pre_integration;
  if (last_marginalization_info != nullptr) delete last_marginalization_info;

  tmp_pre_integration = nullptr;
  last_marginalization_info = nullptr;
  last_marginalization_parameter_blocks.clear();

  f_manager.clearState();

  failure_occur = 0;
  relocalization_info = 0;

  drift_correct_r = Matrix3d::Identity();
  drift_correct_t = Vector3d::Zero();
}

void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity) {
  if (!first_imu) {
    first_imu = true;
    acc_0 = linear_acceleration;
    gyr_0 = angular_velocity;
  }

  if (!pre_integrations[frame_count]) {
    pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
  }

  if (frame_count != 0) {
    // 预积分
    pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
    // if(solver_flag != NON_LINEAR)
    tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);

    dt_buf[frame_count].push_back(dt);
    linear_acceleration_buf[frame_count].push_back(linear_acceleration);
    angular_velocity_buf[frame_count].push_back(angular_velocity);

    int j = frame_count;

    // 积分 R P V
    Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
    Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];
    Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();
    Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;
    Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
    Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
    Vs[j] += dt * un_acc;
  }

  acc_0 = linear_acceleration;
  gyr_0 = angular_velocity;
}

void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image,
                             const std_msgs::Header &header) {
  ROS_DEBUG("new image coming ------------------------------------------");
  ROS_DEBUG("Adding feature points %lu", image.size());

  // 基于视差来选择关键帧(经过旋转补偿)，以此来选择边缘化的方式
  if (f_manager.addFeatureCheckParallax(frame_count, image, td))
    marginalization_flag = MARGIN_OLD;  // Keyframe
  else
    marginalization_flag = MARGIN_SECOND_NEW;  // Non-Keyframe

  ROS_DEBUG("this frame is--------------------%s", marginalization_flag ? "reject" : "accept");
  ROS_DEBUG("%s", marginalization_flag ? "Non-keyframe" : "Keyframe");
  ROS_DEBUG("Solving %d", frame_count);
  ROS_DEBUG("number of feature: %d", f_manager.getFeatureCount());

  Headers[frame_count] = header;

  // 分别将读到的features和imu_mea加入到各自的序列当中
  ImageFrame imageframe(image, header.stamp.toSec());
  imageframe.pre_integration = tmp_pre_integration;
  all_image_frame.insert(make_pair(header.stamp.toSec(), imageframe));

  tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};

#pragma region  // Cam与IMU之间的相对旋转
  if (ESTIMATE_EXTRINSIC == 2) {
    ROS_INFO("calibrating extrinsic param, rotation movement is needed");
    if (frame_count != 0) {
      // 选取两帧之间共有的Features
      vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);

      // 校准相机与IMU之间的旋转
      Matrix3d calib_ric;
      if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric)) {
        ROS_WARN("initial extrinsic rotation calib success");
        ROS_WARN_STREAM("initial extrinsic rotation: " << endl << calib_ric);
        ric[0] = calib_ric;
        RIC[0] = calib_ric;
        ESTIMATE_EXTRINSIC = 1;
      }
    }
  }
#pragma endregion

  if (solver_flag == INITIAL) {
    if (frame_count == WINDOW_SIZE) {  // 滑窗中的Keyframe达到指定大小的时候，才开始优化
      bool result = false;
      if (ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1) {
        result = initialStructure();
        initial_timestamp = header.stamp.toSec();
      }
      if (result) {
        solver_flag = NON_LINEAR;

        solveOdometry();

        slideWindow();
        f_manager.removeFailures();

        printf("[cggos %s] Initialization finish!\n", __FUNCTION__);

        // for failure
        last_R = Rs[WINDOW_SIZE];
        last_P = Ps[WINDOW_SIZE];
        last_R0 = Rs[0];
        last_P0 = Ps[0];
      } else
        slideWindow();
    } else
      frame_count++;
  } else {
    TicToc t_solve;
    solveOdometry();
    ROS_DEBUG("solver costs: %fms", t_solve.toc());

    if (failureDetection()) {
      ROS_WARN("failure detection!");
      failure_occur = 1;
      clearState();
      setParameter();
      ROS_WARN("system reboot!");
      return;
    }

    TicToc t_margin;
    slideWindow();
    f_manager.removeFailures();
    ROS_DEBUG("marginalization costs: %fms", t_margin.toc());

    // prepare output of VINS
    key_poses.clear();
    for (int i = 0; i <= WINDOW_SIZE; i++) key_poses.push_back(Ps[i]);

    // for failure
    last_R = Rs[WINDOW_SIZE];
    last_P = Ps[WINDOW_SIZE];
    last_R0 = Rs[0];
    last_P0 = Ps[0];
  }
}

bool Estimator::initialStructure() {
  TicToc t_sfm;

#pragma region  // check imu observibility
  {
    // 计算均值
    map<double, ImageFrame>::iterator frame_it;
    Vector3d sum_g;
    for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++) {
      double sum_dt = frame_it->second.pre_integration->sum_dt;
      Vector3d tmp_g = frame_it->second.pre_integration->delta_v / sum_dt;
      sum_g += tmp_g;
    }
    Vector3d aver_g = sum_g * 1.0 / ((int)all_image_frame.size() - 1);

    // 计算方差
    double var = 0;
    for (frame_it = all_image_frame.begin(), frame_it++; frame_it != all_image_frame.end(); frame_it++) {
      double sum_dt = frame_it->second.pre_integration->sum_dt;
      Vector3d tmp_g = frame_it->second.pre_integration->delta_v / sum_dt;
      var += (tmp_g - aver_g).transpose() * (tmp_g - aver_g);
    }

    // 计算标准差
    var = sqrt(var / ((int)all_image_frame.size() - 1));
    // ROS_WARN("IMU variation %f!", var);
    if (var < 0.25)  //! 以标准差判断可观性
    {
      ROS_INFO("IMU excitation not enouth!");
      // return false;
    }
  }
#pragma endregion

  //! 滑窗内全局的SFM (global sfm)

  // 遍历滑窗内所有的Features，以vector<SFMFeature>形式保存滑窗内所有特征点
  vector<SFMFeature> sfm_f;
  for (auto &it_per_id : f_manager.feature) {
    int imu_j = it_per_id.start_frame - 1;

    SFMFeature tmp_feature;
    tmp_feature.state = false;
    tmp_feature.id = it_per_id.feature_id;
    for (auto &it_per_frame : it_per_id.feature_per_frame) {
      imu_j++;
      Vector3d pts_j = it_per_frame.point;
      tmp_feature.observation.push_back(make_pair(imu_j, Eigen::Vector2d{pts_j.x(), pts_j.y()}));
    }
    sfm_f.push_back(tmp_feature);
  }

  // 在滑窗内寻找与当前帧的匹配特征点数较多的关键帧作为参考帧,
  // 并求变换矩阵 T:  当前帧到共视帧  cur --> l
  Matrix3d relative_R;
  Vector3d relative_T;
  int l;
  if (!relativePose(relative_R, relative_T, l)) {
    ROS_INFO("Not enough features or parallax; Move device around");
    return false;
  }
  std::cout << "[CGGOS] " << __FUNCTION__ << " " << __LINE__ << ": " << "初始坐标系 l = " << l << std::endl;

  // 三角化恢复滑窗内的Features
  // solve pnp: Tli
  GlobalSFM sfm;
  Quaterniond Q[frame_count + 1];
  Vector3d T[frame_count + 1];
  map<int, Vector3d> sfm_tracked_points;
  if (!sfm.construct(frame_count + 1, Q, T, l, relative_R, relative_T, sfm_f, sfm_tracked_points)) {
    ROS_DEBUG("global SFM failed!");
    marginalization_flag = MARGIN_OLD;
    return false;
  }

  // solve pnp for all frame
  map<int, Vector3d>::iterator it;
  map<double, ImageFrame>::iterator frame_it = all_image_frame.begin();
  for (int i = 0; frame_it != all_image_frame.end(); frame_it++) {
    // provide initial guess
    if ((frame_it->first) == Headers[i].stamp.toSec()) {
      frame_it->second.is_key_frame = true;
      frame_it->second.R = Q[i].toRotationMatrix() * RIC[0].transpose();  // Tclci * Tcb -> Tclbi
      frame_it->second.T = T[i];
      i++;
      continue;
    }
    if ((frame_it->first) > Headers[i].stamp.toSec()) {
      i++;
    }

    // 将滑窗内第i帧的变换矩阵当做初始值
    Matrix3d R_inital = (Q[i].inverse()).toRotationMatrix();
    Vector3d P_inital = -R_inital * T[i];
    cv::Mat rvec, t, tmp_r;
    cv::eigen2cv(R_inital, tmp_r);
    cv::Rodrigues(tmp_r, rvec);
    cv::eigen2cv(P_inital, t);

    frame_it->second.is_key_frame = false;

    vector<cv::Point3f> pts_3_vector;
    vector<cv::Point2f> pts_2_vector;
    for (auto &id_pts : frame_it->second.points) {
      int feature_id = id_pts.first;
      for (auto &i_p : id_pts.second) {
        it = sfm_tracked_points.find(feature_id);
        if (it != sfm_tracked_points.end()) {
          Vector3d world_pts = it->second;
          cv::Point3f pts_3(world_pts(0), world_pts(1), world_pts(2));
          pts_3_vector.push_back(pts_3);

          Vector2d img_pts = i_p.second.head<2>();
          cv::Point2f pts_2(img_pts(0), img_pts(1));
          pts_2_vector.push_back(pts_2);
        }
      }
    }
    if (pts_3_vector.size() < 6) {
      cout << "pts_3_vector size " << pts_3_vector.size() << endl;
      ROS_DEBUG("Not enough points for solve pnp !");
      return false;
    }

    cv::Mat K = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, 1, 0, 0, 0, 1);

    cv::Mat D;
    if (!cv::solvePnP(pts_3_vector, pts_2_vector, K, D, rvec, t, true)) {
      ROS_DEBUG("solve pnp fail!");
      return false;
    }

    // Tcicl^{-1} -> Tclci
    MatrixXd R_pnp;
    MatrixXd T_pnp;
    {
      cv::Mat r;
      cv::Rodrigues(rvec, r);
      MatrixXd tmp_R_pnp;
      cv::cv2eigen(r, tmp_R_pnp);
      R_pnp = tmp_R_pnp.transpose();

      cv::cv2eigen(t, T_pnp);
      T_pnp = R_pnp * (-T_pnp);
    }

    // Rclci * Rcb -> Rclbi
    // tclci
    frame_it->second.R = R_pnp * RIC[0].transpose();
    frame_it->second.T = T_pnp;
  }

  if (visualInertialAlign())
    return true;
  else {
    ROS_INFO("misalign visual structure with IMU");
    return false;
  }
}

bool Estimator::visualInertialAlign() {
  VectorXd x;

  // solve scale
  bool result = VisualIMUAlignment(all_image_frame, Bgs, g, x);  // g是在C0坐标系下
  if (!result) {
    ROS_DEBUG("solve g failed!");
    return false;
  }

  // Ps & Rs
  // tclci, Rclbi
  for (int i = 0; i <= frame_count; i++) {
    Matrix3d Ri = all_image_frame[Headers[i].stamp.toSec()].R;
    Vector3d Pi = all_image_frame[Headers[i].stamp.toSec()].T;
    Ps[i] = Pi;
    Rs[i] = Ri;
    all_image_frame[Headers[i].stamp.toSec()].is_key_frame = true;
  }

  VectorXd dep = f_manager.getDepthVector();
  for (int i = 0; i < dep.size(); i++) dep[i] = -1;
  f_manager.clearDepth(dep);

  // triangulat on cam pose , no tic
  Vector3d TIC_TMP[NUM_OF_CAM];
  for (int i = 0; i < NUM_OF_CAM; i++) TIC_TMP[i].setZero();
  ric[0] = RIC[0];
  f_manager.setRic(ric);
  f_manager.triangulate(Ps, &(TIC_TMP[0]), &(RIC[0]));

  double s = (x.tail<1>())(0);

  for (int i = 0; i <= WINDOW_SIZE; i++) pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);

  // Ps: tbib0 = tclbi - tclb0
  for (int i = frame_count; i >= 0; i--) Ps[i] = s * Ps[i] - Rs[i] * TIC[0] - (s * Ps[0] - Rs[0] * TIC[0]);

  // Vs: vi = Rclbi * vi
  int kv = -1;
  map<double, ImageFrame>::iterator frame_i;
  for (frame_i = all_image_frame.begin(); frame_i != all_image_frame.end(); frame_i++) {
    if (frame_i->second.is_key_frame) {
      kv++;
      Vs[kv] = frame_i->second.R * x.segment<3>(kv * 3);
    }
  }

  // scale depth
  for (auto &it_per_id : f_manager.feature) {
    it_per_id.used_num = it_per_id.feature_per_frame.size();
    if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) continue;
    it_per_id.estimated_depth *= s;
  }

  // 旋转 g、Ps、Rs、Vs 到水平坐标系

  // Rwcl(Roll, Pitch)
  Matrix3d R0 = Utility::g2R(g);
  {
    double yaw = Utility::R2ypr(R0 * Rs[0]).x();  // Rwcl * Rclb0
    R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0;
  }
  g = R0 * g;
  // Matrix3d rot_diff = R0 * Rs[0].transpose();
  Matrix3d rot_diff = R0;
  for (int i = 0; i <= frame_count; i++) {
    Ps[i] = rot_diff * Ps[i];
    Rs[i] = rot_diff * Rs[i];
    Vs[i] = rot_diff * Vs[i];
  }

  std::cout << "[cggos " << __FUNCTION__ << "] Bgs[0]: " << Bgs[0].transpose() << std::endl;
  std::cout << "[cggos " << __FUNCTION__ << "] s: " << s << std::endl;
  std::cout << "[cggos " << __FUNCTION__ << "] G (param): " << G.transpose() << std::endl;
  std::cout << "[cggos " << __FUNCTION__ << "] g in world: " << g.transpose() << std::endl;
  std::cout << "[cggos " << __FUNCTION__ << "] Rs[0] ypr: " << Utility::R2ypr(Rs[0]).transpose() << std::endl;

  return true;
}

bool Estimator::relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l) {
  // find previous frame which contians enough correspondance and parallex with newest frame

  // 在滑窗内寻找与最新的关键帧共视点超过20(像素点)的关键帧
  for (int i = 0; i < WINDOW_SIZE; i++) {
    vector<pair<Vector3d, Vector3d>> corres;
    corres = f_manager.getCorresponding(i, WINDOW_SIZE);

    if (corres.size() > 20) {
      // 求取匹配的特征点在图像上的视差和(归一化平面上)
      double sum_parallax = 0;
      for (int j = 0; j < int(corres.size()); j++) {
        Vector2d pts_0(corres[j].first(0), corres[j].first(1));
        Vector2d pts_1(corres[j].second(0), corres[j].second(1));
        double parallax = (pts_0 - pts_1).norm();
        sum_parallax = sum_parallax + parallax;
      }
      double average_parallax = 1.0 * sum_parallax / int(corres.size());

      // 视差大于一定阈值，并且能够有效地求解出变换矩阵
      if (average_parallax * 460 > 30 && m_estimator.solveRelativeRT(corres, relative_R, relative_T)) {
        l = i;
        ROS_DEBUG(
            "average_parallax %f choose l %d and newest frame to "
            "triangulate the whole structure",
            average_parallax * 460, l);
        return true;
      }
    }
  }
  return false;
}

void Estimator::solveOdometry() {
  if (frame_count < WINDOW_SIZE) return;

  if (solver_flag == NON_LINEAR) {
    TicToc t_tri;
    f_manager.triangulate(Ps, tic, ric);
    ROS_DEBUG("triangulation costs %f", t_tri.toc());

    optimization();
  }
}

void Estimator::vector2double() {
  for (int i = 0; i <= WINDOW_SIZE; i++) {
    para_Pose[i][0] = Ps[i].x();
    para_Pose[i][1] = Ps[i].y();
    para_Pose[i][2] = Ps[i].z();

    Quaterniond q{Rs[i]};
    para_Pose[i][3] = q.x();
    para_Pose[i][4] = q.y();
    para_Pose[i][5] = q.z();
    para_Pose[i][6] = q.w();

    para_SpeedBias[i][0] = Vs[i].x();
    para_SpeedBias[i][1] = Vs[i].y();
    para_SpeedBias[i][2] = Vs[i].z();

    para_SpeedBias[i][3] = Bas[i].x();
    para_SpeedBias[i][4] = Bas[i].y();
    para_SpeedBias[i][5] = Bas[i].z();

    para_SpeedBias[i][6] = Bgs[i].x();
    para_SpeedBias[i][7] = Bgs[i].y();
    para_SpeedBias[i][8] = Bgs[i].z();
  }
  for (int i = 0; i < NUM_OF_CAM; i++) {
    para_Ex_Pose[i][0] = tic[i].x();
    para_Ex_Pose[i][1] = tic[i].y();
    para_Ex_Pose[i][2] = tic[i].z();

    Quaterniond q{ric[i]};
    para_Ex_Pose[i][3] = q.x();
    para_Ex_Pose[i][4] = q.y();
    para_Ex_Pose[i][5] = q.z();
    para_Ex_Pose[i][6] = q.w();
  }

  VectorXd dep = f_manager.getDepthVector();
  for (int i = 0; i < f_manager.getFeatureCount(); i++) para_Feature[i][0] = dep(i);

  if (ESTIMATE_TD) para_Td[0][0] = td;
}

void Estimator::double2vector() {
  Vector3d origin_R0 = Utility::R2ypr(Rs[0]);
  Vector3d origin_P0 = Ps[0];

  if (failure_occur) {
    origin_R0 = Utility::R2ypr(last_R0);
    origin_P0 = last_P0;
    failure_occur = 0;
  }

  Vector3d origin_R00 = Utility::R2ypr(
      Quaterniond(para_Pose[0][6], para_Pose[0][3], para_Pose[0][4], para_Pose[0][5]).toRotationMatrix());

  // Tww' = Tw0 * Tw'0,  Twi = Tww' * Tw'i

  double y_diff = origin_R0.x() - origin_R00.x();
  // TODO
  Matrix3d rot_diff = Utility::ypr2R(Vector3d(y_diff, 0, 0));
  if (abs(abs(origin_R0.y()) - 90) < 1.0 || abs(abs(origin_R00.y()) - 90) < 1.0) {
    ROS_DEBUG("euler singular point!");
    rot_diff =
        Rs[0] *
        Quaterniond(para_Pose[0][6], para_Pose[0][3], para_Pose[0][4], para_Pose[0][5]).toRotationMatrix().transpose();
  }

  for (int i = 0; i <= WINDOW_SIZE; i++) {
    Rs[i] =
        rot_diff *
        Quaterniond(para_Pose[i][6], para_Pose[i][3], para_Pose[i][4], para_Pose[i][5]).normalized().toRotationMatrix();

    Ps[i] = rot_diff * Vector3d(para_Pose[i][0] - para_Pose[0][0], para_Pose[i][1] - para_Pose[0][1],
                                para_Pose[i][2] - para_Pose[0][2]) +
            origin_P0;

    Vs[i] = rot_diff * Vector3d(para_SpeedBias[i][0], para_SpeedBias[i][1], para_SpeedBias[i][2]);

    Bas[i] = Vector3d(para_SpeedBias[i][3], para_SpeedBias[i][4], para_SpeedBias[i][5]);

    Bgs[i] = Vector3d(para_SpeedBias[i][6], para_SpeedBias[i][7], para_SpeedBias[i][8]);
  }

  for (int i = 0; i < NUM_OF_CAM; i++) {
    tic[i] = Vector3d(para_Ex_Pose[i][0], para_Ex_Pose[i][1], para_Ex_Pose[i][2]);
    ric[i] =
        Quaterniond(para_Ex_Pose[i][6], para_Ex_Pose[i][3], para_Ex_Pose[i][4], para_Ex_Pose[i][5]).toRotationMatrix();
  }

  VectorXd dep = f_manager.getDepthVector();
  for (int i = 0; i < f_manager.getFeatureCount(); i++) dep(i) = para_Feature[i][0];
  f_manager.setDepth(dep);

  if (ESTIMATE_TD) td = para_Td[0][0];

  // relative info between two loop frame
  if (relocalization_info) {
    Matrix3d relo_r;
    Vector3d relo_t;
    relo_r =
        rot_diff * Quaterniond(relo_Pose[6], relo_Pose[3], relo_Pose[4], relo_Pose[5]).normalized().toRotationMatrix();
    relo_t = rot_diff * Vector3d(relo_Pose[0] - para_Pose[0][0], relo_Pose[1] - para_Pose[0][1],
                                 relo_Pose[2] - para_Pose[0][2]) +
             origin_P0;
    double drift_correct_yaw;
    drift_correct_yaw = Utility::R2ypr(prev_relo_r).x() - Utility::R2ypr(relo_r).x();
    drift_correct_r = Utility::ypr2R(Vector3d(drift_correct_yaw, 0, 0));
    drift_correct_t = prev_relo_t - drift_correct_r * relo_t;
    relo_relative_t = relo_r.transpose() * (Ps[relo_frame_local_index] - relo_t);
    relo_relative_q = relo_r.transpose() * Rs[relo_frame_local_index];
    relo_relative_yaw =
        Utility::normalizeAngle(Utility::R2ypr(Rs[relo_frame_local_index]).x() - Utility::R2ypr(relo_r).x());
    // cout << "vins relo " << endl;
    // cout << "vins relative_t " << relo_relative_t.transpose() << endl;
    // cout << "vins relative_yaw " <<relo_relative_yaw << endl;
    relocalization_info = 0;
  }
}

bool Estimator::failureDetection() {
  if (f_manager.last_track_num < 2) {
    ROS_INFO(" little feature %d", f_manager.last_track_num);
    // return true;
  }
  if (Bas[WINDOW_SIZE].norm() > 2.5) {
    ROS_INFO(" big IMU acc bias estimation %f", Bas[WINDOW_SIZE].norm());
    return true;
  }
  if (Bgs[WINDOW_SIZE].norm() > 1.0) {
    ROS_INFO(" big IMU gyr bias estimation %f", Bgs[WINDOW_SIZE].norm());
    return true;
  }
  /*
  if (tic(0) > 1)
  {
      ROS_INFO(" big extri param estimation %d", tic(0) > 1);
      return true;
  }
  */
  Vector3d tmp_P = Ps[WINDOW_SIZE];
  if ((tmp_P - last_P).norm() > 5) {
    ROS_INFO(" big translation");
    return true;
  }
  if (abs(tmp_P.z() - last_P.z()) > 1) {
    ROS_INFO(" big z translation");
    return true;
  }
  Matrix3d tmp_R = Rs[WINDOW_SIZE];
  Matrix3d delta_R = tmp_R.transpose() * last_R;
  Quaterniond delta_Q(delta_R);
  double delta_angle;
  delta_angle = acos(delta_Q.w()) * 2.0 / 3.14 * 180.0;
  if (delta_angle > 50) {
    ROS_INFO(" big delta_angle ");
    // return true;
  }
  return false;
}

void Estimator::optimization() {
  TicToc t_whole;

  ceres::Problem problem;

  ceres::LossFunction *loss_function;
  loss_function = new ceres::CauchyLoss(1.0);  // new ceres::HuberLoss(1.0);

#pragma region  // AddParameterBlock

  for (int i = 0; i < WINDOW_SIZE + 1; i++) {
    ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
    problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);  // [p, q] 7
    problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);                // [v, ba, bg] 9
  }

  for (int i = 0; i < NUM_OF_CAM; i++) {
    ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
    problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);  // [p, q] 7
    if (!ESTIMATE_EXTRINSIC) {
      problem.SetParameterBlockConstant(para_Ex_Pose[i]);  // fix extinsic param
    }
  }

  if (ESTIMATE_TD) {
    problem.AddParameterBlock(para_Td[0], 1);  // td 1
                                               // problem.SetParameterBlockConstant(para_Td[0]);
  }

#pragma endregion

  vector2double();

#pragma region  // AddResidualBlock

  // Marginalization residual: 1个
  // IMU residual: WINDOW_SIZE个(总长度WINDOW_SIZE+1),
  // 每相邻两个Pose之间一个IMU residual项 feature residual: 观测数大于2的特征,
  // 首次观测与后面的每次观测之间各一个residual项

  // Marginalization factor
  if (last_marginalization_info) {  // construct new marginlization_factor
    auto *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
    problem.AddResidualBlock(marginalization_factor, NULL, last_marginalization_parameter_blocks);
  }

  // IMU factor
  for (int i = 0; i < WINDOW_SIZE; i++) {
    int j = i + 1;
    if (pre_integrations[j]->sum_dt > 10.0) continue;
    auto *imu_factor = new IMUFactor(pre_integrations[j]);
    problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
  }

  // Vision factor
  int f_m_cnt = 0;
  int feature_index = -1;
  for (auto &it_per_id : f_manager.feature) {  // add residual for per feature to per frame
    it_per_id.used_num = it_per_id.feature_per_frame.size();

    if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) continue;

    ++feature_index;

    int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;  // 得到观测到该特征点的首帧

    Vector3d pts_i = it_per_id.feature_per_frame[0].point;  // 得到首帧观测到的特征点

    for (auto &it_per_frame : it_per_id.feature_per_frame) {
      imu_j++;
      if (imu_i == imu_j) continue;

      // 得到第二个特征点
      Vector3d pts_j = it_per_frame.point;

      if (ESTIMATE_TD) {
        auto *f_td =
            new ProjectionTdFactor(pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity,
                                   it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td,
                                   it_per_id.feature_per_frame[0].uv.y(), it_per_frame.uv.y());
        problem.AddResidualBlock(f_td, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0],
                                 para_Feature[feature_index], para_Td[0]);
        /*
        double **para = new double *[5];
        para[0] = para_Pose[imu_i];
        para[1] = para_Pose[imu_j];
        para[2] = para_Ex_Pose[0];
        para[3] = para_Feature[feature_index];
        para[4] = para_Td[0];
        f_td->check(para);
        */
      } else {
        auto *f = new ProjectionFactor(pts_i, pts_j);
        problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0],
                                 para_Feature[feature_index]);
      }

      f_m_cnt++;
    }
  }

  ROS_DEBUG("visual measurement count: %d", f_m_cnt);

#pragma endregion

#pragma region  // Relocalization Info

  if (relocalization_info) {
    printf("[cggos %s] set relocalization factor!\n", __FUNCTION__);

    ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
    problem.AddParameterBlock(relo_Pose, SIZE_POSE, local_parameterization);

    int retrive_feature_index = 0;
    int feature_index = -1;
    for (auto &it_per_id : f_manager.feature) {
      it_per_id.used_num = it_per_id.feature_per_frame.size();
      if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) continue;
      ++feature_index;
      int start = it_per_id.start_frame;
      if (start <= relo_frame_local_index) {
        while ((int)match_points[retrive_feature_index].z() < it_per_id.feature_id) {
          retrive_feature_index++;
        }
        if ((int)match_points[retrive_feature_index].z() == it_per_id.feature_id) {
          Vector3d pts_j =
              Vector3d(match_points[retrive_feature_index].x(), match_points[retrive_feature_index].y(), 1.0);
          Vector3d pts_i = it_per_id.feature_per_frame[0].point;

          ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
          problem.AddResidualBlock(f, loss_function, para_Pose[start], relo_Pose, para_Ex_Pose[0],
                                   para_Feature[feature_index]);

          retrive_feature_index++;
        }
      }
    }
  }

#pragma endregion

#pragma region  // ceres::Solve

  ceres::Solver::Options options;
  options.linear_solver_type = ceres::DENSE_SCHUR;
  // options.num_threads = 2;
  options.trust_region_strategy_type = ceres::DOGLEG;
  options.max_num_iterations = NUM_ITERATIONS;
  // options.use_explicit_schur_complement = true;
  // options.minimizer_progress_to_stdout = true;
  // options.use_nonmonotonic_steps = true;
  if (marginalization_flag == MARGIN_OLD)
    options.max_solver_time_in_seconds = SOLVER_TIME * 4.0 / 5.0;
  else
    options.max_solver_time_in_seconds = SOLVER_TIME;

  TicToc t_solver;
  ceres::Solver::Summary summary;
  ceres::Solve(options, &problem, &summary);
  // cout << summary.BriefReport() << endl;
  ROS_DEBUG("Iterations : %d", static_cast<int>(summary.iterations.size()));
  ROS_DEBUG("solver costs: %f", t_solver.toc());

#pragma endregion

  double2vector();

  /**
   * sliding windows bounding了优化问题中pose的个数, 从而防止pose和特征的个数随时间不断增加,
   * 使得优化问题始终在一个有限的复杂度内, 不会随时间不断增长
   *
   * 然而, 将pose移出windows时, 有些约束会被丢弃掉, 这样势必会导致求解的精度下降, 
   * 而且当MAV进行一些退化运动(如: 匀速运动)时, 没有历史信息做约束的话是无法求解的. 
   * 所以, 在移出位姿或特征的时候, 需要将相关联的约束转变成一个约束项作为prior放到优化问题中.
   * 这就是marginalization要做的事情
   *
   * MARGIN_OLD:
   * 如果次新帧是关键帧，则丢弃滑动窗口内最老的图像帧，同时对与该图像帧关联的约束项进行边缘化处理。
   * 这里需要注意的是，如果该关键帧是观察到某个地图点的第一帧，则需要把该地图点的深度转移到后面的图像帧中去。
   *
   * MARGIN_NEW:
   * 如果次新帧不是关键帧，则丢弃当前帧的前一帧。因为判定当前帧不是关键帧的条件就是当前帧与前一帧视差很小，也就是说当前帧和前一帧很相似，
   * 这种情况下直接丢弃前一帧，然后用当前帧代替前一帧。为什么这里可以不对前一帧进行边缘化，而是直接丢弃，原因就是当前帧和前一帧很相似，
   * 因此当前帧与地图点之间的约束和前一帧与地图点之间的约束是很接近的，直接丢弃并不会造成整个约束关系丢失信息。这里需要注意的是，
   * 要把当前帧和前一帧之间的 IMU 预积分转换为当前帧和前二帧之间的 IMU预积分。
   *
   * 在悬停等运动较小的情况下, 会频繁的MARGIN_NEW, 这样也就保留了那些比较旧但是视差比较大的pose.
   * 这种情况如果一直MARGIN_OLD的话, 视觉约束不够强, 状态估计会受IMU积分误差影响, 具有较大的累积误差
   */

  TicToc t_whole_marginalization;
  if (marginalization_flag == MARGIN_OLD) {
#pragma region  // Marginalization MARGIN_OLD

    auto *marginalization_info = new MarginalizationInfo();

    vector2double();

    // 先验误差会一直保存，而不是只使用一次
    // 要边缘化的参数块是 para_Pose[0] para_SpeedBias[0] 以及 para_Feature[feature_index](滑窗内的第feature_index个点的逆深度)
    if (last_marginalization_info) {
      vector<int> drop_set;
      for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++) {
        // 查询待估计参数中是首帧状态量的序号
        if (last_marginalization_parameter_blocks[i] == para_Pose[0] ||
            last_marginalization_parameter_blocks[i] == para_SpeedBias[0])
          drop_set.push_back(i);
      }

      auto *marginalization_factor = new MarginalizationFactor(last_marginalization_info);

      auto *residual_block_info =
          new ResidualBlockInfo(marginalization_factor, nullptr, last_marginalization_parameter_blocks, drop_set);

      marginalization_info->addResidualBlockInfo(residual_block_info);
    }

    // 添加第0帧和第1帧之间的IMU预积分值以及第0帧和第1帧相关优化变量
    if (pre_integrations[1]->sum_dt < 10.0) {
      auto *imu_factor = new IMUFactor(pre_integrations[1]);
      auto *residual_block_info = new ResidualBlockInfo(
          imu_factor, nullptr,
          vector<double *>{para_Pose[0], para_SpeedBias[0], para_Pose[1], para_SpeedBias[1]},  // 7, 9, 7, 9
          vector<int>{0, 1});
      marginalization_info->addResidualBlockInfo(residual_block_info);
    }

    // 添加视觉的先验，只添加起始帧是旧帧且观测次数大于2的Features
    {
      int feature_index = -1;
      for (auto &it_per_id : f_manager.feature) {
        it_per_id.used_num = it_per_id.feature_per_frame.size();  // 该特征点被观测到的次数

        if (!(it_per_id.used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2)) continue;

        ++feature_index;

        int imu_i = it_per_id.start_frame, imu_j = imu_i - 1;
        // 如果第一个观察帧不是第一帧就不进行考虑，因此后面用来构建marg矩阵的都是和第一帧有共视关系的滑窗帧
        if (imu_i != 0) continue;

        Vector3d pts_i = it_per_id.feature_per_frame[0].point;  // 得到该Feature在起始下的归一化坐标

        for (auto &it_per_frame : it_per_id.feature_per_frame) {
          imu_j++;
          if (imu_i == imu_j)  // 不需要起始观测帧
            continue;

          Vector3d pts_j = it_per_frame.point;

          if (ESTIMATE_TD) {
            auto *f_td =
                new ProjectionTdFactor(pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity,
                                       it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td,
                                       it_per_id.feature_per_frame[0].uv.y(), it_per_frame.uv.y());
            auto *residual_block_info =
                new ResidualBlockInfo(f_td, loss_function,
                                      vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0],
                                                       para_Feature[feature_index], para_Td[0]},  // 7, 7, 7, 1, 1
                                      vector<int>{0, 3});

            marginalization_info->addResidualBlockInfo(residual_block_info);
          } else {
            auto *f = new ProjectionFactor(pts_i, pts_j);

            // 为0和3的原因是，para_Pose[imu_i]是第一帧的位姿，需要marg掉，而3是para_Feature[feature_index]，是和第一帧相关的特征点，需要marg掉
            // 不仅仅会边缘化第一帧相关的优化变量，还会边缘化掉以第一帧为起始观察帧的路标点
            auto *residual_block_info =
                new ResidualBlockInfo(f, loss_function,
                                      vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0],
                                                       para_Feature[feature_index]},  // 7, 7, 7, 1
                                      vector<int>{0, 3});

            marginalization_info->addResidualBlockInfo(residual_block_info);
          }
        }
      }
    }

    TicToc t_pre_margin;
    marginalization_info->preMarginalize();
    ROS_DEBUG("pre marginalization %f ms", t_pre_margin.toc());

    TicToc t_margin;
    marginalization_info->marginalize();
    ROS_DEBUG("marginalization %f ms", t_margin.toc());

    // 滑窗预移动
    // 将滑窗里关键帧位姿移位，这里是保存了所有状态量的信息，为什么没有保存逆深度的状态量呢
    std::unordered_map<long, double *> addr_shift;
    for (int i = 1; i <= WINDOW_SIZE; i++) {
      addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];
      addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
    }
    for (int i = 0; i < NUM_OF_CAM; i++) addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i];
    if (ESTIMATE_TD) addr_shift[reinterpret_cast<long>(para_Td[0])] = para_Td[0];

    if (last_marginalization_info) delete last_marginalization_info;
    last_marginalization_info = marginalization_info;
    last_marginalization_parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);

#pragma endregion
  } else {
#pragma region  // Marginalization MARGIN_NEW

    if (last_marginalization_info &&
        std::count(std::begin(last_marginalization_parameter_blocks), std::end(last_marginalization_parameter_blocks),
                   para_Pose[WINDOW_SIZE - 1])) {
      auto *marginalization_info = new MarginalizationInfo();

      vector2double();

      if (last_marginalization_info) {
        vector<int> drop_set;
        for (int i = 0; i < static_cast<int>(last_marginalization_parameter_blocks.size()); i++) {
          // 寻找倒数第二帧的位姿
          ROS_ASSERT(last_marginalization_parameter_blocks[i] != para_SpeedBias[WINDOW_SIZE - 1]);
          if (last_marginalization_parameter_blocks[i] == para_Pose[WINDOW_SIZE - 1]) drop_set.push_back(i);
        }

        // construct new marginlization_factor
        auto *marginalization_factor = new MarginalizationFactor(last_marginalization_info);

        auto *residual_block_info =
            new ResidualBlockInfo(marginalization_factor, NULL, last_marginalization_parameter_blocks, drop_set);

        marginalization_info->addResidualBlockInfo(residual_block_info);
      }

      TicToc t_pre_margin;
      ROS_DEBUG("begin marginalization");
      marginalization_info->preMarginalize();
      ROS_DEBUG("end pre marginalization, %f ms", t_pre_margin.toc());

      TicToc t_margin;
      ROS_DEBUG("begin marginalization");
      marginalization_info->marginalize();
      ROS_DEBUG("end marginalization, %f ms", t_margin.toc());

      std::unordered_map<long, double *> addr_shift;
      for (int i = 0; i <= WINDOW_SIZE; i++) {
        if (i == WINDOW_SIZE - 1)
          continue;
        else if (i == WINDOW_SIZE) {
          addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i - 1];
          addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i - 1];
        } else {
          addr_shift[reinterpret_cast<long>(para_Pose[i])] = para_Pose[i];
          addr_shift[reinterpret_cast<long>(para_SpeedBias[i])] = para_SpeedBias[i];
        }
      }
      for (int i = 0; i < NUM_OF_CAM; i++) addr_shift[reinterpret_cast<long>(para_Ex_Pose[i])] = para_Ex_Pose[i];
      if (ESTIMATE_TD) addr_shift[reinterpret_cast<long>(para_Td[0])] = para_Td[0];

      if (last_marginalization_info) delete last_marginalization_info;
      last_marginalization_info = marginalization_info;
      last_marginalization_parameter_blocks = marginalization_info->getParameterBlocks(addr_shift);
    }

#pragma endregion
  }
  ROS_DEBUG("whole marginalization costs: %f", t_whole_marginalization.toc());

  ROS_DEBUG("whole time for ceres: %f", t_whole.toc());
}

void Estimator::slideWindow() {
  if (marginalization_flag == MARGIN_OLD) {
    double t_0 = Headers[0].stamp.toSec();
    back_R0 = Rs[0];
    back_P0 = Ps[0];

    if (frame_count == WINDOW_SIZE) {
      // 删去最后一帧，则滑窗每一个帧向后挪动一个位置
      for (int i = 0; i < WINDOW_SIZE; i++) {
        Rs[i].swap(Rs[i + 1]);

        std::swap(pre_integrations[i], pre_integrations[i + 1]);

        dt_buf[i].swap(dt_buf[i + 1]);
        linear_acceleration_buf[i].swap(linear_acceleration_buf[i + 1]);
        angular_velocity_buf[i].swap(angular_velocity_buf[i + 1]);

        Headers[i] = Headers[i + 1];
        Ps[i].swap(Ps[i + 1]);
        Vs[i].swap(Vs[i + 1]);
        Bas[i].swap(Bas[i + 1]);
        Bgs[i].swap(Bgs[i + 1]);
      }

      // 最新帧
      {
        // 让最新帧等于次新帧
        Headers[WINDOW_SIZE] = Headers[WINDOW_SIZE - 1];
        Ps[WINDOW_SIZE] = Ps[WINDOW_SIZE - 1];
        Vs[WINDOW_SIZE] = Vs[WINDOW_SIZE - 1];
        Rs[WINDOW_SIZE] = Rs[WINDOW_SIZE - 1];
        Bas[WINDOW_SIZE] = Bas[WINDOW_SIZE - 1];
        Bgs[WINDOW_SIZE] = Bgs[WINDOW_SIZE - 1];

        delete pre_integrations[WINDOW_SIZE];
        pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};

        dt_buf[WINDOW_SIZE].clear();
        linear_acceleration_buf[WINDOW_SIZE].clear();
        angular_velocity_buf[WINDOW_SIZE].clear();
      }

      if (true || solver_flag == INITIAL) {
        map<double, ImageFrame>::iterator it_0;
        it_0 = all_image_frame.find(t_0);

        // 删除 最老帧 及其之前的 
        
        // 预积分
        delete it_0->second.pre_integration;
        it_0->second.pre_integration = nullptr;
        for (map<double, ImageFrame>::iterator it = all_image_frame.begin(); it != it_0; ++it) {
          if (it->second.pre_integration) delete it->second.pre_integration;
          it->second.pre_integration = nullptr;
        }

        // 视觉观测
        all_image_frame.erase(all_image_frame.begin(), it_0);
        all_image_frame.erase(t_0);
      }

      // 把老帧的信息传给新帧
      slideWindowOld();
    }
  } else {
    if (frame_count == WINDOW_SIZE) {
      // 如果要删去次新帧(不是最新帧,是次新帧)
      // 次新帧 预积分 继续积分 当前帧测量
      for (unsigned int i = 0; i < dt_buf[frame_count].size(); i++) {
        double tmp_dt = dt_buf[frame_count][i];
        Vector3d tmp_linear_acceleration = linear_acceleration_buf[frame_count][i];
        Vector3d tmp_angular_velocity = angular_velocity_buf[frame_count][i];

        pre_integrations[frame_count - 1]->push_back(tmp_dt, tmp_linear_acceleration, tmp_angular_velocity);

        dt_buf[frame_count - 1].push_back(tmp_dt);
        linear_acceleration_buf[frame_count - 1].push_back(tmp_linear_acceleration);
        angular_velocity_buf[frame_count - 1].push_back(tmp_angular_velocity);
      }
      // 让次新帧的信息等于新帧
      Headers[frame_count - 1] = Headers[frame_count];
      Ps[frame_count - 1] = Ps[frame_count];
      Vs[frame_count - 1] = Vs[frame_count];
      Rs[frame_count - 1] = Rs[frame_count];
      Bas[frame_count - 1] = Bas[frame_count];
      Bgs[frame_count - 1] = Bgs[frame_count];

      // 处理 最新帧
      delete pre_integrations[WINDOW_SIZE];
      pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]};

      dt_buf[WINDOW_SIZE].clear();
      linear_acceleration_buf[WINDOW_SIZE].clear();
      angular_velocity_buf[WINDOW_SIZE].clear();

      slideWindowNew();
    }
  }
}

void Estimator::slideWindowNew() { f_manager.removeFront(frame_count); }

void Estimator::slideWindowOld() {
  bool shift_depth = solver_flag == NON_LINEAR ? true : false;
  if (shift_depth) {
    Matrix3d R0, R1;
    Vector3d P0, P1;
    R0 = back_R0 * ric[0];
    R1 = Rs[0] * ric[0];
    P0 = back_P0 + back_R0 * tic[0];
    P1 = Ps[0] + Rs[0] * tic[0];
    f_manager.removeBackShiftDepth(R0, P0, R1, P1);  // 把老帧的信息传给新帧
  } else
    f_manager.removeBack();
}

void Estimator::setReloFrame(
    double _frame_stamp, int _frame_index, vector<Vector3d> &_match_points, Vector3d _relo_t, Matrix3d _relo_r) {
  relo_frame_stamp = _frame_stamp;
  relo_frame_index = _frame_index;
  match_points.clear();
  match_points = _match_points;
  prev_relo_t = _relo_t;
  prev_relo_r = _relo_r;

  for (int i = 0; i < WINDOW_SIZE; i++) {
    if (relo_frame_stamp == Headers[i].stamp.toSec()) {
      relo_frame_local_index = i;
      relocalization_info = 1;
      for (int j = 0; j < SIZE_POSE; j++) relo_Pose[j] = para_Pose[i][j];
    }
  }
}
