用对偶四元数求解 AX=XB

最近接触手眼标定算法，其核心是求解 AX=XB 方程，参考公开的MATLAB程序，采用 C++ 重写。

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <iostream>
#include <utility>

struct Calibration {
  Calibration(Eigen::MatrixXd A, Eigen::MatrixXd B)
      : A(std::move(A)), B(std::move(B)) {}

  Eigen::Matrix4d daniilidis() const {
    // Konstantinos1999: Hand-Eye Calibration Using Dual Quaternions
    // http://faculty.cooper.edu/mili/Calibration/AXXB/daniilidis.m
    //      Step1 -> Get T
    assert(A.rows() % 4 == 0);
    assert(B.rows() % 4 == 0);
    assert(A.cols() == B.cols());
    auto n = A.rows() / 4;

    Eigen::MatrixXd T = Eigen::MatrixXd::Zero(6 * n, 8);
    for (int i = 0; i < n; ++i) {
      Eigen::Matrix4d A1 = A.block<4, 4>(4 * i, 0);
      Eigen::Matrix4d B1 = B.block<4, 4>(4 * i, 0);
      auto a_pair = hom2dual(A1);
      auto b_pair = hom2dual(B1);

      Eigen::Vector3d a = {a_pair.first.x(), a_pair.first.y(),
                           a_pair.first.z()};
      Eigen::Vector3d a_prime = {a_pair.second.x(), a_pair.second.y(),
                                 a_pair.second.z()};
      Eigen::Vector3d b = {b_pair.first.x(), b_pair.first.y(),
                           b_pair.first.z()};
      Eigen::Vector3d b_prime = {b_pair.second.x(), b_pair.second.y(),
                                 b_pair.second.z()};

      Eigen::MatrixXd t = Eigen::MatrixXd::Zero(6, 8);
      t.block<3, 1>(0, 0) = a - b;
      t.block<3, 3>(0, 1) = skew(Eigen::Vector3d{a + b});
      t.block<3, 1>(3, 0) = a_prime - b_prime;
      t.block<3, 3>(3, 1) = skew(Eigen::Vector3d{a_prime + b_prime});
      t.block<3, 1>(3, 4) = a - b;
      t.block<3, 3>(3, 5) = skew(Eigen::Vector3d{a + b});

      T.middleRows<6>(6 * i) = t;
    }

    //      Step2 -> SVD T = UΣV_8x8^T
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(T, Eigen::ComputeFullV);
    auto V_8x8 = svd.matrixV();
    Eigen::Vector4d u1 = V_8x8.block<4, 1>(0, 6);
    Eigen::Vector4d v1 = V_8x8.block<4, 1>(4, 6);
    Eigen::Vector4d u2 = V_8x8.block<4, 1>(0, 7);
    Eigen::Vector4d v2 = V_8x8.block<4, 1>(4, 7);
    //      Step3
    double a = u1.dot(v1);
    double b = u1.dot(v2) + u2.dot(v1);
    double c = u2.dot(v2);

    auto s1 = (-b + std::sqrt(b * b - 4 * a * c)) / (2.0 * a);
    auto s2 = (-b - std::sqrt(b * b - 4 * a * c)) / (2.0 * a);
    //      Step4
    auto val1 = s1 * s1 * u1.dot(u1) + 2 * s1 * u1.dot(u2) + u2.dot(u2);
    auto val2 = s2 * s2 * u1.dot(u1) + 2 * s2 * u1.dot(u2) + u2.dot(u2);

    auto val = (val1 > val2) ? val1 : val2;
    auto s = (val1 > val2) ? s1 : s2;
    auto lambda2 = std::sqrt(1 / val);
    auto lambda1 = s * lambda2;
    //      Step5
    Eigen::VectorXd q = lambda1 * V_8x8.col(6) + lambda2 * V_8x8.col(7);
    Eigen::Quaterniond q_r{q(0), q(1), q(2), q(3)};
    Eigen::Quaterniond q_d{q(4), q(5), q(6), q(7)};
    auto X = dual2hom(std::make_pair(q_r, q_d));
    return X;
  }

  static std::pair<Eigen::Quaterniond, Eigen::Quaterniond> hom2dual(
      Eigen::Matrix4d H) {
    //    Converts H4x4 to a dual quaternion dq = q_r + q_d * eps
    Eigen::Quaterniond q_r{H.block<3, 3>(0, 0)};
    q_r.normalize();
    Eigen::Quaterniond q_d{0, H(0, 3), H(1, 3), H(2, 3)};
    q_d = q_d * q_r;
    return std::make_pair(
        q_r, Eigen::Quaterniond{0.5 * q_d.w(), 0.5 * q_d.x(), 0.5 * q_d.y(),
                                0.5 * q_d.z()});
  }

  static Eigen::Matrix4d dual2hom(
      const std::pair<Eigen::Quaterniond, Eigen::Quaterniond>& dq) {
    //    Converts dual quaternion dq = q_r + q_d * eps to a H4x4
    Eigen::Matrix4d H = Eigen::Matrix4d::Identity();
    H.block<3, 3>(0, 0) = dq.first.toRotationMatrix();
    auto q_d = dq.second * dq.first.conjugate();
    H.block<3, 1>(0, 3) = 2 * Eigen::Vector3d{q_d.x(), q_d.y(), q_d.z()};
    return H;
  }

  static Eigen::Matrix3d skew(Eigen::Matrix<double, 3, 1> v) {
    Eigen::Matrix3d m;
    m << 0, -v(2), v(1), v(2), 0, -v(0), -v(1), v(0), 0;
    return m;
  }

  Eigen::MatrixXd A;
  Eigen::MatrixXd B;
};

int main() {
  Eigen::Matrix4d A1, A2, B1, B2;
  // Example "Robot sensor calibration: solving AX=XB on the Euclidean group"
  // clang-format off
  A1 << -0.9900, -0.1411, 0.0, 0.0,
         0.1411, -0.9900, 0.0, 0.0,
         0.0,     0.0,    1.0, 0.0,
         0.0,     0.0,    0.0, 1.0;
  B1 << -0.9900, -0.1383, 0.0280, -26.9559,
         0.1383, -0.9114, 0.3875, -96.1332,
        -0.0280,  0.3875, 0.9215,  19.4872,
         0.0,     0.0,    0.0,      1.0;
  A2 << 0.0707,   0.0,    0.9975,-400.0,
        0.0,      1.0,    0.0,      0.0,
       -0.9975,   0.0,    0.0707, 400.0,
        0.0,      0.0,    0.0,      1.0;
  B2 << 0.0707,   0.1982, 0.9976,-309.543,
       -0.1982,   0.9633,-0.1809,  59.0244,
       -0.9776,  -0.1809, 0.1074, 291.177,
        0.0,      0.0,    0.0,       1.0;
  // clang-format on
  Eigen::MatrixXd A(A1.rows() + A2.rows(), A1.cols());
  A << A1, A2;
  Eigen::MatrixXd B(B1.rows() + B2.rows(), B1.cols());
  B << B1, B2;
  std::cout << "A = \n" << A << std::endl;
  std::cout << "B = \n" << B << std::endl;
  Calibration hand_eye_cal(A, B);
  auto X = hand_eye_cal.daniilidis();
  std::cout << "X_estimate = \n" << X << std::endl;
  return 0;
}