#include "initial_ex_rotation.h"

InitialEXRotation::InitialEXRotation() {
  frame_count = 0;
  Rc.push_back(Matrix3d::Identity());
  Rc_g.push_back(Matrix3d::Identity());
  Rimu.push_back(Matrix3d::Identity());
  ric = Matrix3d::Identity();
}

bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres,
                                              Quaterniond delta_q_imu,
                                              Matrix3d &calib_ric_result) {
  // 滑窗内的帧数加1
  frame_count++;

  Rc.push_back(solveRelativeR(corres));
  Rimu.push_back(delta_q_imu.toRotationMatrix());
  Rc_g.push_back(ric.inverse() * delta_q_imu * ric);  // 计算两帧之间的旋转矩阵

  Eigen::MatrixXd A(frame_count * 4, 4);
  A.setZero();
  for (int i = 1; i <= frame_count; i++) {
    double huber = 1.0;
    {
      Quaterniond r1(Rc[i]);
      Quaterniond r2(Rc_g[i]);

      // 求取估计出的相机与IMU之间旋转的残差
      double angular_distance = 180 / M_PI * r1.angularDistance(r2);
      ROS_DEBUG("%d %f", i, angular_distance);

      // 计算外点剔除的权重
      huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;
    }

    Matrix4d L, R;

    // 相机对极关系得到的旋转q的左乘
    double w = Quaterniond(Rc[i]).w();
    Vector3d q = Quaterniond(Rc[i]).vec();
    L.block<3, 3>(0, 0) = w * Matrix3d::Identity() + Utility::skewSymmetric(q);
    L.block<3, 1>(0, 3) = q;
    L.block<1, 3>(3, 0) = -q.transpose();
    L(3, 3) = w;

    // 由IMU预积分得到的旋转q的右乘
    Quaterniond R_ij(Rimu[i]);
    w = R_ij.w();
    q = R_ij.vec();
    R.block<3, 3>(0, 0) = w * Matrix3d::Identity() - Utility::skewSymmetric(q);
    R.block<3, 1>(0, 3) = q;
    R.block<1, 3>(3, 0) = -q.transpose();
    R(3, 3) = w;

    A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R);
  }

  // 通过SVD分解，求取相机与IMU的相对旋转，解为系数矩阵A的右奇异向量V中最小奇异值对应的特征向量
  JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);

  Matrix<double, 4, 1> x = svd.matrixV().col(3);
  Quaterniond estimated_R(x);
  ric = estimated_R.toRotationMatrix().inverse();

  // 第二小的奇异值是否大于某个阈值,若大于则其零空间的秩为 1,反之秩大于 1, 相对旋转的精度不够,校准不成功
  Vector3d ric_cov = svd.singularValues().tail<3>();
  if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25) {
    calib_ric_result = ric;
    return true;
  } else
    return false;
}

Matrix3d InitialEXRotation::solveRelativeR(const vector<pair<Vector3d, Vector3d>> &corres) {
  if (corres.size() >= 9) {
    vector<cv::Point2f> ll, rr;
    for (int i = 0; i < int(corres.size()); i++) {
      ll.push_back(cv::Point2f(corres[i].first(0), corres[i].first(1)));
      rr.push_back(cv::Point2f(corres[i].second(0), corres[i].second(1)));
    }

    cv::Mat E = cv::findFundamentalMat(ll, rr);
    cv::Mat_<double> R1, R2, t1, t2;
    decomposeE(E, R1, R2, t1, t2);

    if (determinant(R1) + 1.0 < 1e-09) {
      E = -E;
      decomposeE(E, R1, R2, t1, t2);
    }

    double ratio1 = max(testTriangulation(ll, rr, R1, t1), testTriangulation(ll, rr, R1, t2));
    double ratio2 = max(testTriangulation(ll, rr, R2, t1), testTriangulation(ll, rr, R2, t2));

    cv::Mat_<double> ans_R_cv = ratio1 > ratio2 ? R1 : R2;

    Matrix3d ans_R_eigen;
    for (int i = 0; i < 3; i++)
      for (int j = 0; j < 3; j++) ans_R_eigen(j, i) = ans_R_cv(i, j);

    return ans_R_eigen;
  }
  return Matrix3d::Identity();
}

double InitialEXRotation::testTriangulation(const vector<cv::Point2f> &l,
                                            const vector<cv::Point2f> &r,
                                            cv::Mat_<double> R,
                                            cv::Mat_<double> t) {
  cv::Mat pointcloud;
  cv::Matx34f P = cv::Matx34f(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0);
  cv::Matx34f P1 =
      cv::Matx34f(R(0, 0), R(0, 1), R(0, 2), t(0), R(1, 0), R(1, 1), R(1, 2), t(1), R(2, 0), R(2, 1), R(2, 2), t(2));
  cv::triangulatePoints(P, P1, l, r, pointcloud);

  int front_count = 0;
  for (int i = 0; i < pointcloud.cols; i++) {
    double normal_factor = pointcloud.col(i).at<float>(3);

    cv::Mat_<double> p_3d_l = cv::Mat(P) * (pointcloud.col(i) / normal_factor);
    cv::Mat_<double> p_3d_r = cv::Mat(P1) * (pointcloud.col(i) / normal_factor);
    if (p_3d_l(2) > 0 && p_3d_r(2) > 0) front_count++;
  }
  ROS_DEBUG("MotionEstimator: %f", 1.0 * front_count / pointcloud.cols);
  return 1.0 * front_count / pointcloud.cols;
}

void InitialEXRotation::decomposeE(
    cv::Mat E, cv::Mat_<double> &R1, cv::Mat_<double> &R2, cv::Mat_<double> &t1, cv::Mat_<double> &t2) {
  cv::SVD svd(E, cv::SVD::MODIFY_A);
  cv::Matx33d W(0, -1, 0, 1, 0, 0, 0, 0, 1);
  cv::Matx33d Wt(0, 1, 0, -1, 0, 0, 0, 0, 1);
  R1 = svd.u * cv::Mat(W) * svd.vt;
  R2 = svd.u * cv::Mat(Wt) * svd.vt;
  t1 = svd.u.col(2);
  t2 = -svd.u.col(2);
}
