// Copyright (c), ETH Zurich and UNC Chapel Hill.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//
//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
//       its contributors may be used to endorse or promote products derived
//       from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

#include "colmap/estimators/solvers/generalized_relative_pose.h"

#include "colmap/estimators/solvers/poselib_utils.h"
#include "colmap/geometry/essential_matrix.h"
#include "colmap/math/random.h"
#include "colmap/util/eigen_alignment.h"
#include "colmap/util/logging.h"

#include <Eigen/Dense>
#include <PoseLib/solvers/gen_relpose_6pt.h>

namespace colmap {

void GR6PEstimator::Estimate(const std::vector<X_t>& points1,
                             const std::vector<Y_t>& points2,
                             std::vector<M_t>* rigs2_from_rigs1) {
  THROW_CHECK_EQ(points1.size(), 6);
  THROW_CHECK_EQ(points2.size(), 6);
  THROW_CHECK_NOTNULL(rigs2_from_rigs1);

  rigs2_from_rigs1->clear();

  std::vector<Eigen::Vector3d> origins_in_rig1(6);
  std::vector<Eigen::Vector3d> origins_in_rig2(6);
  std::vector<Eigen::Vector3d> rays_in_rig1(6);
  std::vector<Eigen::Vector3d> rays_in_rig2(6);
  for (int i = 0; i < 6; ++i) {
    origins_in_rig1[i] = points1[i].cam_from_rig.TgtOriginInSrc();
    origins_in_rig2[i] = points2[i].cam_from_rig.TgtOriginInSrc();
    rays_in_rig1[i] =
        points1[i].cam_from_rig.rotation().inverse() * points1[i].ray_in_cam;
    rays_in_rig2[i] =
        points2[i].cam_from_rig.rotation().inverse() * points2[i].ray_in_cam;
  }

  std::vector<poselib::CameraPose> poses;
  poselib::gen_relpose_6pt(
      origins_in_rig1, rays_in_rig1, origins_in_rig2, rays_in_rig2, &poses);

  rigs2_from_rigs1->reserve(poses.size());
  for (const poselib::CameraPose& pose : poses) {
    rigs2_from_rigs1->emplace_back(ConvertPoseLibPoseToRigid3d(pose));
  }
}

void GR6PEstimator::Residuals(const std::vector<X_t>& points1,
                              const std::vector<Y_t>& points2,
                              const M_t& rig2_from_rig1,
                              std::vector<double>* residuals) {
  THROW_CHECK_EQ(points1.size(), points2.size());
  residuals->resize(points1.size());
  for (size_t i = 0; i < points1.size(); ++i) {
    const Rigid3d cam2_from_cam1 = points2[i].cam_from_rig * rig2_from_rig1 *
                                   Inverse(points1[i].cam_from_rig);
    const Eigen::Matrix3d E = EssentialMatrixFromPose(cam2_from_cam1);
    const Eigen::Vector3d epipolar_line1 = E * points1[i].ray_in_cam;
    const double num = points2[i].ray_in_cam.dot(epipolar_line1);
    const Eigen::Vector4d denom(points2[i].ray_in_cam.dot(E.col(0)),
                                points2[i].ray_in_cam.dot(E.col(1)),
                                epipolar_line1.x(),
                                epipolar_line1.y());
    (*residuals)[i] = num * num / denom.squaredNorm();
  }
}

namespace {

void ComposePlueckerData(const Rigid3d& rig_from_cam,
                         const Eigen::Vector3d& ray_in_cam,
                         Eigen::Vector3d* origin_in_rig,
                         Eigen::Vector6d* pluecker) {
  const Eigen::Vector3d ray_in_rig =
      (rig_from_cam.rotation() * ray_in_cam).normalized();
  *origin_in_rig = rig_from_cam.translation();
  *pluecker << ray_in_rig, rig_from_cam.translation().cross(ray_in_rig);
}

Eigen::Matrix3d CayleyToRotationMatrix(const Eigen::Vector3d& cayley) {
  const double cayley0_sqr = cayley[0] * cayley[0];
  const double cayley1_sqr = cayley[1] * cayley[1];
  const double cayley2_sqr = cayley[2] * cayley[2];
  const double cayley01 = cayley[0] * cayley[1];
  const double cayley12 = cayley[1] * cayley[2];
  const double cayley02 = cayley[0] * cayley[2];

  const double scale = 1 + cayley0_sqr + cayley1_sqr + cayley2_sqr;
  const double inv_scale = 1.0 / scale;

  Eigen::Matrix3d R;

  R(0, 0) = inv_scale * (1 + cayley0_sqr - cayley1_sqr - cayley2_sqr);
  R(0, 1) = inv_scale * (2 * (cayley01 - cayley[2]));
  R(0, 2) = inv_scale * (2 * (cayley02 + cayley[1]));
  R(1, 0) = inv_scale * (2 * (cayley01 + cayley[2]));
  R(1, 1) = inv_scale * (1 - cayley0_sqr + cayley1_sqr - cayley2_sqr);
  R(1, 2) = inv_scale * (2 * (cayley12 - cayley[0]));
  R(2, 0) = inv_scale * (2 * (cayley02 - cayley[1]));
  R(2, 1) = inv_scale * (2 * (cayley12 + cayley[0]));
  R(2, 2) = inv_scale * (1 - cayley0_sqr - cayley1_sqr + cayley2_sqr);

  return R;
}

Eigen::Vector3d RotationMatrixToCaley(const Eigen::Matrix3d& R) {
  const Eigen::Matrix3d C1 = R - Eigen::Matrix3d::Identity();
  const Eigen::Matrix3d C2 = R + Eigen::Matrix3d::Identity();
  const Eigen::Matrix3d C = C1 * C2.inverse();
  return Eigen::Vector3d(-C(1, 2), C(0, 2), -C(0, 1));
}

Eigen::Vector3d ComputeRotationBetweenPoints(
    const std::vector<Eigen::Vector6d>& plueckers1,
    const std::vector<Eigen::Vector6d>& plueckers2) {
  THROW_CHECK_EQ(plueckers1.size(), plueckers2.size());

  // Compute the center of all observed points.
  Eigen::Vector3d points_center1 = Eigen::Vector3d::Zero();
  Eigen::Vector3d points_center2 = Eigen::Vector3d::Zero();
  for (size_t i = 0; i < plueckers1.size(); i++) {
    points_center1 += plueckers1[i].head<3>();
    points_center2 += plueckers2[i].head<3>();
  }
  points_center1 = points_center1 / plueckers1.size();
  points_center2 = points_center2 / plueckers1.size();

  Eigen::Matrix3d Hcross = Eigen::Matrix3d::Zero();
  for (size_t i = 0; i < plueckers1.size(); i++) {
    const Eigen::Vector3d f1 = plueckers1[i].head<3>() - points_center1;
    const Eigen::Vector3d f2 = plueckers2[i].head<3>() - points_center2;
    Hcross += f2 * f1.transpose();
  }

  const Eigen::JacobiSVD<Eigen::Matrix3d> svd(
      Hcross, Eigen::ComputeFullU | Eigen::ComputeFullV);
  const Eigen::Matrix3d& V = svd.matrixV();
  const Eigen::Matrix3d& U = svd.matrixU();

  Eigen::Matrix3d R = V * U.transpose();
  if (R.determinant() < 0) {
    Eigen::Matrix3d V_prime;
    V_prime.col(0) = V.col(0);
    V_prime.col(1) = V.col(1);
    V_prime.col(2) = -V.col(2);
    R = V_prime * U.transpose();
  }

  return RotationMatrixToCaley(R);
}

Eigen::Matrix4d ComposeG(const Eigen::Matrix3d& xxF,
                         const Eigen::Matrix3d& yyF,
                         const Eigen::Matrix3d& zzF,
                         const Eigen::Matrix3d& xyF,
                         const Eigen::Matrix3d& yzF,
                         const Eigen::Matrix3d& zxF,
                         const Eigen::Matrix<double, 3, 9>& x1P,
                         const Eigen::Matrix<double, 3, 9>& y1P,
                         const Eigen::Matrix<double, 3, 9>& z1P,
                         const Eigen::Matrix<double, 3, 9>& x2P,
                         const Eigen::Matrix<double, 3, 9>& y2P,
                         const Eigen::Matrix<double, 3, 9>& z2P,
                         const Eigen::Matrix<double, 9, 9>& m11P,
                         const Eigen::Matrix<double, 9, 9>& m12P,
                         const Eigen::Matrix<double, 9, 9>& m22P,
                         const Eigen::Vector3d& rotation) {
  const Eigen::Matrix3d R = CayleyToRotationMatrix(rotation);

  Eigen::Matrix<double, 1, 9> R_rows;
  R_rows << R.row(0), R.row(1), R.row(2);

  Eigen::Matrix<double, 9, 1> R_cols;
  R_cols << R.col(0), R.col(1), R.col(2);

  const Eigen::Vector3d xxFr1t = xxF * R.row(1).transpose();
  const Eigen::Vector3d yyFr0t = yyF * R.row(0).transpose();
  const Eigen::Vector3d zzFr0t = zzF * R.row(0).transpose();
  const Eigen::Vector3d yzFr0t = yzF * R.row(0).transpose();
  const Eigen::Vector3d xyFr1t = xyF * R.row(1).transpose();
  const Eigen::Vector3d xyFr2t = xyF * R.row(2).transpose();
  const Eigen::Vector3d zxFr1t = zxF * R.row(1).transpose();
  const Eigen::Vector3d zxFr2t = zxF * R.row(2).transpose();

  const Eigen::Vector3d x1PC = x1P * R_cols;
  const Eigen::Vector3d y1PC = y1P * R_cols;
  const Eigen::Vector3d z1PC = z1P * R_cols;

  const Eigen::Vector3d x2PR = x2P * R_rows.transpose();
  const Eigen::Vector3d y2PR = y2P * R_rows.transpose();
  const Eigen::Vector3d z2PR = z2P * R_rows.transpose();

  Eigen::Matrix4d G;

  G(0, 0) = R.row(2) * yyF * R.row(2).transpose();
  G(0, 0) += -2.0 * R.row(2) * yzF * R.row(1).transpose();
  G(0, 0) += R.row(1) * zzF * R.row(1).transpose();

  G(0, 1) = R.row(2) * yzFr0t;
  G(0, 1) += -1.0 * R.row(2) * xyFr2t;
  G(0, 1) += -1.0 * R.row(1) * zzFr0t;
  G(0, 1) += R.row(1) * zxFr2t;

  G(0, 2) = R.row(2) * xyFr1t;
  G(0, 2) += -1.0 * R.row(2) * yyFr0t;
  G(0, 2) += -1.0 * R.row(1) * zxFr1t;
  G(0, 2) += R.row(1) * yzFr0t;

  G(1, 1) = R.row(0) * zzFr0t;
  G(1, 1) += -2.0 * R.row(0) * zxFr2t;
  G(1, 1) += R.row(2) * xxF * R.row(2).transpose();

  G(1, 2) = R.row(0) * zxFr1t;
  G(1, 2) += -1.0 * R.row(0) * yzFr0t;
  G(1, 2) += -1.0 * R.row(2) * xxFr1t;
  G(1, 2) += R.row(0) * xyFr2t;

  G(2, 2) = R.row(1) * xxFr1t;
  G(2, 2) += -2.0 * R.row(0) * xyFr1t;
  G(2, 2) += R.row(0) * yyFr0t;

  G(1, 0) = G(0, 1);
  G(2, 0) = G(0, 2);
  G(2, 1) = G(1, 2);

  G(0, 3) = R.row(2) * y1PC;
  G(0, 3) += R.row(2) * y2PR;
  G(0, 3) += -1.0 * R.row(1) * z1PC;
  G(0, 3) += -1.0 * R.row(1) * z2PR;

  G(1, 3) = R.row(0) * z1PC;
  G(1, 3) += R.row(0) * z2PR;
  G(1, 3) += -1.0 * R.row(2) * x1PC;
  G(1, 3) += -1.0 * R.row(2) * x2PR;

  G(2, 3) = R.row(1) * x1PC;
  G(2, 3) += R.row(1) * x2PR;
  G(2, 3) += -1.0 * R.row(0) * y1PC;
  G(2, 3) += -1.0 * R.row(0) * y2PR;

  G(3, 3) = -1.0 * R_cols.transpose() * m11P * R_cols;
  G(3, 3) += -1.0 * R_rows * m22P * R_rows.transpose();
  G(3, 3) += -2.0 * R_rows * m12P * R_cols;

  G(3, 0) = G(0, 3);
  G(3, 1) = G(1, 3);
  G(3, 2) = G(2, 3);

  return G;
}

Eigen::Vector4d ComputeEigenValue(const Eigen::Matrix3d& xxF,
                                  const Eigen::Matrix3d& yyF,
                                  const Eigen::Matrix3d& zzF,
                                  const Eigen::Matrix3d& xyF,
                                  const Eigen::Matrix3d& yzF,
                                  const Eigen::Matrix3d& zxF,
                                  const Eigen::Matrix<double, 3, 9>& x1P,
                                  const Eigen::Matrix<double, 3, 9>& y1P,
                                  const Eigen::Matrix<double, 3, 9>& z1P,
                                  const Eigen::Matrix<double, 3, 9>& x2P,
                                  const Eigen::Matrix<double, 3, 9>& y2P,
                                  const Eigen::Matrix<double, 3, 9>& z2P,
                                  const Eigen::Matrix<double, 9, 9>& m11P,
                                  const Eigen::Matrix<double, 9, 9>& m12P,
                                  const Eigen::Matrix<double, 9, 9>& m22P,
                                  const Eigen::Vector3d& rotation) {
  const Eigen::Matrix4d G = ComposeG(xxF,
                                     yyF,
                                     zzF,
                                     xyF,
                                     yzF,
                                     zxF,
                                     x1P,
                                     y1P,
                                     z1P,
                                     x2P,
                                     y2P,
                                     z2P,
                                     m11P,
                                     m12P,
                                     m22P,
                                     rotation);

  // Compute the roots in closed-form.
  // const double G00_2 = G(0,0) * G(0,0);
  const double G01_2 = G(0, 1) * G(0, 1);
  const double G02_2 = G(0, 2) * G(0, 2);
  const double G03_2 = G(0, 3) * G(0, 3);
  // const double G11_2 = G(1,1) * G(1,1);
  const double G12_2 = G(1, 2) * G(1, 2);
  const double G13_2 = G(1, 3) * G(1, 3);
  // const double G22_2 = G(2,2) * G(2,2);
  const double G23_2 = G(2, 3) * G(2, 3);
  // const double G33_2 = G(3,3) * G(3,3);

  const double B = -G(3, 3) - G(2, 2) - G(1, 1) - G(0, 0);
  const double C = -G23_2 + G(2, 2) * G(3, 3) - G13_2 - G12_2 +
                   G(1, 1) * G(3, 3) + G(1, 1) * G(2, 2) - G03_2 - G02_2 -
                   G01_2 + G(0, 0) * G(3, 3) + G(0, 0) * G(2, 2) +
                   G(0, 0) * G(1, 1);
  const double D =
      G13_2 * G(2, 2) - 2.0 * G(1, 2) * G(1, 3) * G(2, 3) + G12_2 * G(3, 3) +
      G(1, 1) * G23_2 - G(1, 1) * G(2, 2) * G(3, 3) + G03_2 * G(2, 2) +
      G03_2 * G(1, 1) - 2.0 * G(0, 2) * G(0, 3) * G(2, 3) + G02_2 * G(3, 3) +
      G02_2 * G(1, 1) - 2.0 * G(0, 1) * G(0, 3) * G(1, 3) -
      2.0 * G(0, 1) * G(0, 2) * G(1, 2) + G01_2 * G(3, 3) + G01_2 * G(2, 2) +
      G(0, 0) * G23_2 - G(0, 0) * G(2, 2) * G(3, 3) + G(0, 0) * G13_2 +
      G(0, 0) * G12_2 - G(0, 0) * G(1, 1) * G(3, 3) -
      G(0, 0) * G(1, 1) * G(2, 2);
  const double E =
      G03_2 * G12_2 - G03_2 * G(1, 1) * G(2, 2) -
      2.0 * G(0, 2) * G(0, 3) * G(1, 2) * G(1, 3) +
      2.0 * G(0, 2) * G(0, 3) * G(1, 1) * G(2, 3) + G02_2 * G13_2 -
      G02_2 * G(1, 1) * G(3, 3) + 2.0 * G(0, 1) * G(0, 3) * G(1, 3) * G(2, 2) -
      2.0 * G(0, 1) * G(0, 3) * G(1, 2) * G(2, 3) -
      2.0 * G(0, 1) * G(0, 2) * G(1, 3) * G(2, 3) +
      2.0 * G(0, 1) * G(0, 2) * G(1, 2) * G(3, 3) + G01_2 * G23_2 -
      G01_2 * G(2, 2) * G(3, 3) - G(0, 0) * G13_2 * G(2, 2) +
      2.0 * G(0, 0) * G(1, 2) * G(1, 3) * G(2, 3) - G(0, 0) * G12_2 * G(3, 3) -
      G(0, 0) * G(1, 1) * G23_2 + G(0, 0) * G(1, 1) * G(2, 2) * G(3, 3);

  const double B_pw2 = B * B;
  const double B_pw3 = B_pw2 * B;
  const double B_pw4 = B_pw3 * B;
  const double alpha = -0.375 * B_pw2 + C;
  const double beta = B_pw3 / 8.0 - B * C / 2.0 + D;
  const double gamma = -0.01171875 * B_pw4 + B_pw2 * C / 16.0 - B * D / 4.0 + E;
  const double alpha_pw2 = alpha * alpha;
  const double alpha_pw3 = alpha_pw2 * alpha;
  const double p = -alpha_pw2 / 12.0 - gamma;
  const double q = -alpha_pw3 / 108.0 + alpha * gamma / 3.0 - beta * beta / 8.0;
  const double helper1 = -p * p * p / 27.0;
  const double theta2 = std::pow(helper1, (1.0 / 3.0));
  const double theta1 =
      std::sqrt(theta2) *
      std::cos((1.0 / 3.0) * std::acos((-q / 2.0) / std::sqrt(helper1)));
  const double y = -(5.0 / 6.0) * alpha -
                   ((1.0 / 3.0) * p * theta1 - theta1 * theta2) / theta2;
  const double w = std::sqrt(alpha + 2.0 * y);

  Eigen::Vector4d roots;
  roots(0) = -B / 4.0 + 0.5 * w +
             0.5 * std::sqrt(-3.0 * alpha - 2.0 * y - 2.0 * beta / w);
  roots(1) = -B / 4.0 + 0.5 * w -
             0.5 * std::sqrt(-3.0 * alpha - 2.0 * y - 2.0 * beta / w);
  roots(2) = -B / 4.0 - 0.5 * w +
             0.5 * std::sqrt(-3.0 * alpha - 2.0 * y + 2.0 * beta / w);
  roots(3) = -B / 4.0 - 0.5 * w -
             0.5 * std::sqrt(-3.0 * alpha - 2.0 * y + 2.0 * beta / w);
  return roots;
}

double ComputeCost(const Eigen::Matrix3d& xxF,
                   const Eigen::Matrix3d& yyF,
                   const Eigen::Matrix3d& zzF,
                   const Eigen::Matrix3d& xyF,
                   const Eigen::Matrix3d& yzF,
                   const Eigen::Matrix3d& zxF,
                   const Eigen::Matrix<double, 3, 9>& x1P,
                   const Eigen::Matrix<double, 3, 9>& y1P,
                   const Eigen::Matrix<double, 3, 9>& z1P,
                   const Eigen::Matrix<double, 3, 9>& x2P,
                   const Eigen::Matrix<double, 3, 9>& y2P,
                   const Eigen::Matrix<double, 3, 9>& z2P,
                   const Eigen::Matrix<double, 9, 9>& m11P,
                   const Eigen::Matrix<double, 9, 9>& m12P,
                   const Eigen::Matrix<double, 9, 9>& m22P,
                   const Eigen::Vector3d& rotation,
                   const int step) {
  THROW_CHECK_GE(step, 0);
  THROW_CHECK_LE(step, 1);

  const Eigen::Vector4d roots = ComputeEigenValue(xxF,
                                                  yyF,
                                                  zzF,
                                                  xyF,
                                                  yzF,
                                                  zxF,
                                                  x1P,
                                                  y1P,
                                                  z1P,
                                                  x2P,
                                                  y2P,
                                                  z2P,
                                                  m11P,
                                                  m12P,
                                                  m22P,
                                                  rotation);

  if (step == 0) {
    return roots[2];
  } else if (step == 1) {
    return roots[3];
  }

  return 0;
}

Eigen::Vector3d ComputeJacobian(const Eigen::Matrix3d& xxF,
                                const Eigen::Matrix3d& yyF,
                                const Eigen::Matrix3d& zzF,
                                const Eigen::Matrix3d& xyF,
                                const Eigen::Matrix3d& yzF,
                                const Eigen::Matrix3d& zxF,
                                const Eigen::Matrix<double, 3, 9>& x1P,
                                const Eigen::Matrix<double, 3, 9>& y1P,
                                const Eigen::Matrix<double, 3, 9>& z1P,
                                const Eigen::Matrix<double, 3, 9>& x2P,
                                const Eigen::Matrix<double, 3, 9>& y2P,
                                const Eigen::Matrix<double, 3, 9>& z2P,
                                const Eigen::Matrix<double, 9, 9>& m11P,
                                const Eigen::Matrix<double, 9, 9>& m12P,
                                const Eigen::Matrix<double, 9, 9>& m22P,
                                const Eigen::Vector3d& rotation,
                                const double current_cost,
                                const int step) {
  Eigen::Vector3d jacobian;
  constexpr double kStepSize = 1e-8;
  for (int j = 0; j < 3; j++) {
    Eigen::Vector3d cayley_j = rotation;
    cayley_j[j] += kStepSize;
    const double cost_j = ComputeCost(xxF,
                                      yyF,
                                      zzF,
                                      xyF,
                                      yzF,
                                      zxF,
                                      x1P,
                                      y1P,
                                      z1P,
                                      x2P,
                                      y2P,
                                      z2P,
                                      m11P,
                                      m12P,
                                      m22P,
                                      cayley_j,
                                      step);
    jacobian(j) = cost_j - current_cost;
  }
  return jacobian;
}

}  // namespace

void GR8PEstimator::Estimate(const std::vector<X_t>& points1,
                             const std::vector<Y_t>& points2,
                             std::vector<M_t>* rigs2_from_rigs1) {
  THROW_CHECK_GE(points1.size(), 6);
  THROW_CHECK_EQ(points1.size(), points2.size());
  THROW_CHECK(rigs2_from_rigs1 != nullptr);

  rigs2_from_rigs1->clear();

  std::vector<Eigen::Vector3d> origins_in_rig1(points1.size());
  std::vector<Eigen::Vector3d> origins_in_rig2(points1.size());
  std::vector<Eigen::Vector6d> plueckers1(points1.size());
  std::vector<Eigen::Vector6d> plueckers2(points1.size());
  for (size_t i = 0; i < points1.size(); ++i) {
    ComposePlueckerData(Inverse(points1[i].cam_from_rig),
                        points1[i].ray_in_cam,
                        &origins_in_rig1[i],
                        &plueckers1[i]);
    ComposePlueckerData(Inverse(points2[i].cam_from_rig),
                        points2[i].ray_in_cam,
                        &origins_in_rig2[i],
                        &plueckers2[i]);
  }

  Eigen::Matrix3d xxF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d yyF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d zzF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d xyF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d yzF = Eigen::Matrix3d::Zero();
  Eigen::Matrix3d zxF = Eigen::Matrix3d::Zero();

  Eigen::Matrix<double, 3, 9> x1P = Eigen::Matrix<double, 3, 9>::Zero();
  Eigen::Matrix<double, 3, 9> y1P = Eigen::Matrix<double, 3, 9>::Zero();
  Eigen::Matrix<double, 3, 9> z1P = Eigen::Matrix<double, 3, 9>::Zero();
  Eigen::Matrix<double, 3, 9> x2P = Eigen::Matrix<double, 3, 9>::Zero();
  Eigen::Matrix<double, 3, 9> y2P = Eigen::Matrix<double, 3, 9>::Zero();
  Eigen::Matrix<double, 3, 9> z2P = Eigen::Matrix<double, 3, 9>::Zero();

  Eigen::Matrix<double, 9, 9> m11P = Eigen::Matrix<double, 9, 9>::Zero();
  Eigen::Matrix<double, 9, 9> m12P = Eigen::Matrix<double, 9, 9>::Zero();
  Eigen::Matrix<double, 9, 9> m22P = Eigen::Matrix<double, 9, 9>::Zero();

  for (size_t i = 0; i < points1.size(); ++i) {
    const Eigen::Vector3d f1 = plueckers1[i].head<3>();
    const Eigen::Vector3d f2 = plueckers2[i].head<3>();
    const Eigen::Vector3d& t1 = origins_in_rig1[i];
    const Eigen::Vector3d& t2 = origins_in_rig2[i];

    const Eigen::Matrix3d F = f2 * f2.transpose();
    xxF += f1[0] * f1[0] * F;
    yyF += f1[1] * f1[1] * F;
    zzF += f1[2] * f1[2] * F;
    xyF += f1[0] * f1[1] * F;
    yzF += f1[1] * f1[2] * F;
    zxF += f1[2] * f1[0] * F;

    Eigen::Matrix<double, 9, 1> ff1;
    ff1(0) = f1[0] * (f2[1] * t2[2] - f2[2] * t2[1]);
    ff1(1) = f1[1] * (f2[1] * t2[2] - f2[2] * t2[1]);
    ff1(2) = f1[2] * (f2[1] * t2[2] - f2[2] * t2[1]);
    ff1(3) = f1[0] * (f2[2] * t2[0] - f2[0] * t2[2]);
    ff1(4) = f1[1] * (f2[2] * t2[0] - f2[0] * t2[2]);
    ff1(5) = f1[2] * (f2[2] * t2[0] - f2[0] * t2[2]);
    ff1(6) = f1[0] * (f2[0] * t2[1] - f2[1] * t2[0]);
    ff1(7) = f1[1] * (f2[0] * t2[1] - f2[1] * t2[0]);
    ff1(8) = f1[2] * (f2[0] * t2[1] - f2[1] * t2[0]);

    x1P += f1[0] * f2 * ff1.transpose();
    y1P += f1[1] * f2 * ff1.transpose();
    z1P += f1[2] * f2 * ff1.transpose();

    Eigen::Matrix<double, 9, 1> ff2;
    ff2(0) = f2[0] * (f1[1] * t1[2] - f1[2] * t1[1]);
    ff2(1) = f2[1] * (f1[1] * t1[2] - f1[2] * t1[1]);
    ff2(2) = f2[2] * (f1[1] * t1[2] - f1[2] * t1[1]);
    ff2(3) = f2[0] * (f1[2] * t1[0] - f1[0] * t1[2]);
    ff2(4) = f2[1] * (f1[2] * t1[0] - f1[0] * t1[2]);
    ff2(5) = f2[2] * (f1[2] * t1[0] - f1[0] * t1[2]);
    ff2(6) = f2[0] * (f1[0] * t1[1] - f1[1] * t1[0]);
    ff2(7) = f2[1] * (f1[0] * t1[1] - f1[1] * t1[0]);
    ff2(8) = f2[2] * (f1[0] * t1[1] - f1[1] * t1[0]);

    x2P += f1[0] * f2 * ff2.transpose();
    y2P += f1[1] * f2 * ff2.transpose();
    z2P += f1[2] * f2 * ff2.transpose();

    m11P -= ff1 * ff1.transpose();
    m22P -= ff2 * ff2.transpose();
    m12P -= ff2 * ff1.transpose();
  }

  const Eigen::Vector3d initial_rotation =
      ComputeRotationBetweenPoints(plueckers1, plueckers2);

  const double kMinLambda = 0.00001;
  const double kMaxLambda = 0.08;
  const double kLambdaModifier = 2.0;
  const int kMaxNumIterations = 50;
  const bool kDisableIncrements = true;

  double perturbation_amplitude = 0.3;
  int num_random_trials = 0;

  Eigen::Vector3d rotation;
  while (num_random_trials < 5) {
    if (num_random_trials > 2) {
      perturbation_amplitude = 0.6;
    }

    if (num_random_trials == 0) {
      rotation = initial_rotation;
    } else {
      const Eigen::Vector3d perturbation(
          RandomUniformReal<double>(-perturbation_amplitude,
                                    perturbation_amplitude),
          RandomUniformReal<double>(-perturbation_amplitude,
                                    perturbation_amplitude),
          RandomUniformReal<double>(-perturbation_amplitude,
                                    perturbation_amplitude));
      rotation = initial_rotation + perturbation;
    }

    double lambda = 0.01;
    int num_iterations = 0;
    double smallest_eigen_value = ComputeCost(xxF,
                                              yyF,
                                              zzF,
                                              xyF,
                                              yzF,
                                              zxF,
                                              x1P,
                                              y1P,
                                              z1P,
                                              x2P,
                                              y2P,
                                              z2P,
                                              m11P,
                                              m12P,
                                              m22P,
                                              rotation,
                                              1);

    for (int iter = 0; iter < kMaxNumIterations; ++iter) {
      const Eigen::Vector3d jacobian = ComputeJacobian(xxF,
                                                       yyF,
                                                       zzF,
                                                       xyF,
                                                       yzF,
                                                       zxF,
                                                       x1P,
                                                       y1P,
                                                       z1P,
                                                       x2P,
                                                       y2P,
                                                       z2P,
                                                       m11P,
                                                       m12P,
                                                       m22P,
                                                       rotation,
                                                       smallest_eigen_value,
                                                       1);

      const Eigen::Vector3d normalized_jacobian = jacobian.normalized();

      Eigen::Vector3d sampling_point = rotation - lambda * normalized_jacobian;
      double sampling_eigen_value = ComputeCost(xxF,
                                                yyF,
                                                zzF,
                                                xyF,
                                                yzF,
                                                zxF,
                                                x1P,
                                                y1P,
                                                z1P,
                                                x2P,
                                                y2P,
                                                z2P,
                                                m11P,
                                                m12P,
                                                m22P,
                                                sampling_point,
                                                1);

      if (num_iterations == 0 || !kDisableIncrements) {
        while (sampling_eigen_value < smallest_eigen_value) {
          smallest_eigen_value = sampling_eigen_value;
          if (lambda * kLambdaModifier > kMaxLambda) {
            break;
          }
          lambda *= kLambdaModifier;
          sampling_point = rotation - lambda * normalized_jacobian;
          sampling_eigen_value = ComputeCost(xxF,
                                             yyF,
                                             zzF,
                                             xyF,
                                             yzF,
                                             zxF,
                                             x1P,
                                             y1P,
                                             z1P,
                                             x2P,
                                             y2P,
                                             z2P,
                                             m11P,
                                             m12P,
                                             m22P,
                                             sampling_point,
                                             1);
        }
      }

      while (sampling_eigen_value > smallest_eigen_value) {
        lambda /= kLambdaModifier;
        sampling_point = rotation - lambda * normalized_jacobian;
        sampling_eigen_value = ComputeCost(xxF,
                                           yyF,
                                           zzF,
                                           xyF,
                                           yzF,
                                           zxF,
                                           x1P,
                                           y1P,
                                           z1P,
                                           x2P,
                                           y2P,
                                           z2P,
                                           m11P,
                                           m12P,
                                           m22P,
                                           sampling_point,
                                           1);
      }

      rotation = sampling_point;
      smallest_eigen_value = sampling_eigen_value;

      if (lambda < kMinLambda) {
        break;
      }
    }

    if (rotation.norm() < 0.01) {
      const double eigen_value2 = ComputeCost(xxF,
                                              yyF,
                                              zzF,
                                              xyF,
                                              yzF,
                                              zxF,
                                              x1P,
                                              y1P,
                                              z1P,
                                              x2P,
                                              y2P,
                                              z2P,
                                              m11P,
                                              m12P,
                                              m22P,
                                              rotation,
                                              0);
      if (eigen_value2 > 0.001) {
        num_random_trials += 1;
      } else {
        break;
      }
    } else {
      break;
    }
  }

  const Eigen::Matrix3d R = CayleyToRotationMatrix(rotation).transpose();

  const Eigen::Matrix4d G = ComposeG(xxF,
                                     yyF,
                                     zzF,
                                     xyF,
                                     yzF,
                                     zxF,
                                     x1P,
                                     y1P,
                                     z1P,
                                     x2P,
                                     y2P,
                                     z2P,
                                     m11P,
                                     m12P,
                                     m22P,
                                     rotation);

  const Eigen::EigenSolver<Eigen::Matrix4d> eigen_solver_G(G, true);
  const Eigen::Matrix4cd V = eigen_solver_G.eigenvectors();
  const Eigen::Matrix3x4d VV = V.real().colwise().hnormalized();

  rigs2_from_rigs1->resize(4);
  for (int i = 0; i < 4; ++i) {
    (*rigs2_from_rigs1)[i] = Rigid3d(Eigen::Quaterniond(R), -R * VV.col(i));
  }
}

void GR8PEstimator::Residuals(const std::vector<X_t>& points1,
                              const std::vector<Y_t>& points2,
                              const M_t& rig2_from_rig1,
                              std::vector<double>* residuals) {
  GR6PEstimator::Residuals(points1, points2, rig2_from_rig1, residuals);
}

}  // namespace colmap
