手眼标定中利用Eigen求解AX=XB问题

1.背景

在手眼标定中，求解AX=XB基本是个绕不过去的问题，其中A、B、X都是4*4的变换矩阵。不过还好，网上有不少现成的代码。

2.各种解法

经过初步检测，部分代码得到的结果良好，现在把我经过测试基本符合要求的发出来，仅供参考
注意，下面的代码基本都要用到【Eigen】这个库，请先安装配置好这个库再使用下面的代码。

2.1.解法1（chatgpt给的，效果一般）

void AXXBTest()
{
    // 定义矩阵A和B
    Eigen::Matrix4d A, B;

    // 设置A和B的值

    A << 0.9951070418405665, 0.09833447156013869, -0.009607652278522231, -0.1834429495359904,
        0.0986618674759368, -0.9941695948028406, 0.04350462821029899, -0.07756771360648877,
        -0.005273631147264981, -0.04423967080058273, -0.9990070271734722, 0.4807538282250369,
        0,                     0,                    0,                   1;


    B << 0.9943477554988256, 0.09927144759666691, -0.03765263372607622, -0.1563855785254715,
        0.1005067113133437, -0.9944065212933074, 0.03246646562715001, -0.08892271309585634,
        -0.03421903147992392, -0.03606729961343019, -0.9987633392266513, 0.4951639944945874,
        0,                     0,                    0,                   1;


    Eigen::Matrix4d X;

    /***方法1***/
    //    // 通过对系数矩阵进行LU分解，将原问题转化为两个子问题：一个是求解Ly = b，另一个是求解Ux = y，其中L和U分别代表系数矩阵的下三角和上三角部分。
    //    // 需要注意的是，使用LU分解求解线性方程组的复杂度是O(n^3)，因此在处理较大规模的问题时可能会比QR分解慢一些。此外，如果系数矩阵A的奇异性（singularity）很高，
    //    // 那么在使用LU分解时可能会遇到数值上的问题。
    //    {
    //        // 求解线性方程组Ax=xB
    //        X = A.lu().solve(B);
    //    }

    /***方法2***/
    // 关于解决AX=XB的问题，一个较为高效的方法是使用矩阵分解来求解。可以将A和B分别分解成QR分解或LU分解等形式，并将X写成一些未知量的乘积，然后代入方程，进而得到未知量的值。
    // 以下是给出一个基于QR分解的示例代码：
    // 在这个方法中，QR分解可以确保有解，且求解速度较快。同时，使用矩阵分解的方法不需要显式地计算 Kronecker 积，因此可以更好地避免数值上的问题。
    {
        // 对 A、B 进行 QR 分解
        HouseholderQR<Matrix4d> qrA(A);
        HouseholderQR<Matrix4d> qrB(B);
        // 构造 R1 和 R2 矩阵
        Matrix4d R1 = qrA.matrixQR().triangularView<Upper>();
        Matrix4d R2 = qrB.matrixQR().triangularView<Upper>();
        // 构造 Q1 和 Q2 矩阵
        Matrix4d Q1 = qrA.householderQ();
        Matrix4d Q2 = qrB.householderQ();

        // 求解 X
        X = Q1.transpose() * Q2;
        X = R1.inverse() * X;
        X = X * R2.inverse();
    }

    // 输出解矩阵X
    std::cout << "The solution is:\n" << X << std::endl;

    // 验证一下结果
    std::cout << "A*X:" << A*X << std::endl;
    std::cout << "X*B:" << X*B << std::endl;
}

输出的结果：

2.2.解法2

在github上，有个人基于Eigen搞了个计算AX=XB的库【SolveAXXB】，测试了一下，效果比解法1的效果要好。
我注释了他源码的部分代码，使其能够仅使用一组AB数据。

下面这个是测试代码：

#include "axxb/andreffextendedaxxbsolver.h"
#include "axxb/conventionalaxxbsvdsolver.h"
#include "axxb/extendedaxxbelilambdasvdsolver.h"
void AXXBTest()
{
    // 定义矩阵A和B
    Eigen::Matrix4d A, B;

    // 设置A和B的值
    A << 0.9951070418405665, 0.09833447156013869, -0.009607652278522231, -0.1834429495359904,
        0.0986618674759368, -0.9941695948028406, 0.04350462821029899, -0.07756771360648877,
        -0.005273631147264981, -0.04423967080058273, -0.9990070271734722, 0.4807538282250369,
        0,                     0,                    0,                   1;

    B << 0.9943477554988256, 0.09927144759666691, -0.03765263372607622, -0.1563855785254715,
        0.1005067113133437, -0.9944065212933074, 0.03246646562715001, -0.08892271309585634,
        -0.03421903147992392, -0.03606729961343019, -0.9987633392266513, 0.4951639944945874,
        0,                     0,                    0,                   1;
    
    // 需要计算的X
    Eigen::Matrix4d X;
    
    // 调用这个人的算法，本来是需要两组以上的数据的。但是我这里为了演示方便，只用一组了。
    // 在实际的手眼标定项目中，取得两组以上的A、B应该不是太难的事
    std::vector<Pose> posA;
    std::vector<Pose> posB;
    posA.push_back(A);
    posB.push_back(B);
    
    // 这里有三个计算器，可以分别测试一下，看看哪个会好一点。
    // AndreffExtendedAXXBSolver solver(posA, posB);
    ConventionalAXXBSVDSolver solver(posA, posB); // 这个效果最好
    // ExtendedAXXBEliLambdaSVDSolver solver(posA, posB);

    X = solver.SolveX();

    // 输出解矩阵X
    std::cout << "The solution is:\n" << X << std::endl;

    // 验证一下结果
    std::cout << "A*X:" << A*X << std::endl;
    std::cout << "X*B:" << X*B << std::endl;
}

输出的结果：

效果看上去相当好。尤其这个是只用了一组数据的，假如有多组数据，效果不知道会不会更好一些。

2.3.解法3

opencv也有个类似的函数cv::calibrateHandEye，简单测试了一下，效果不太好评价：

int axxbTest()
{
    using namespace cv;
    using namespace std;

    // 设置A和B的值
    double dataA[16] = {0.9951070418405665, 0.09833447156013869, -0.009607652278522231, -0.1834429495359904,
                       0.0986618674759368, -0.9941695948028406, 0.04350462821029899, -0.07756771360648877,
                       -0.005273631147264981, -0.04423967080058273, -0.9990070271734722, 0.4807538282250369,
                       0,                     0,                    0,                   1};
    double dataB[16] = {0.9943477554988256, 0.09927144759666691, -0.03765263372607622, -0.1563855785254715,
                       0.1005067113133437, -0.9944065212933074, 0.03246646562715001, -0.08892271309585634,
                       -0.03421903147992392, -0.03606729961343019, -0.9987633392266513, 0.4951639944945874,
                       0,                     0,                    0,                   1};

    Mat A(4, 4, CV_64FC1, dataA);
    Mat B(4, 4, CV_64FC1, dataB);
    Mat X(4, 4, CV_64FC1, Scalar(0));

    Mat A_R = A(Range(0, 3), Range(0, 3));
    Mat A_T = A(Range(0, 3), Range(3, 4));

    Mat B_R = B(Range(0, 3), Range(0, 3));
    Mat B_T = B(Range(0, 3), Range(3, 4));

    Mat X_R, X_T;

    vector<Mat> ARVec, ATVec, BRVec, BTVec;

    // calibrateHandEye要求至少3组数据
    for(int i = 0; i < 3; i++)
    {
        ARVec.push_back(A_R);
        ATVec.push_back(A_T);

        BRVec.push_back(B_R);
        BTVec.push_back(B_T);
    }

    // CALIB_HAND_EYE_TSAI         = 0, //!< A New Technique for Fully Autonomous and Efficient 3D Robotics Hand/Eye Calibration @cite Tsai89
    // CALIB_HAND_EYE_PARK         = 1, //!< Robot Sensor Calibration: Solving AX = XB on the Euclidean Group @cite Park94
    // CALIB_HAND_EYE_HORAUD       = 2, //!< Hand-eye Calibration @cite Horaud95
    // CALIB_HAND_EYE_ANDREFF      = 3, //!< On-line Hand-Eye Calibration @cite Andreff99
    // CALIB_HAND_EYE_DANIILIDIS   = 4  //!< Hand-Eye Calibration Using Dual Quaternions @cite Daniilidis98
    calibrateHandEye(ARVec, ATVec, BRVec, BTVec, X_R, X_T, CALIB_HAND_EYE_TSAI);

    X_R.copyTo(X(Range(0, 3), Range(0, 3)));
    X_T.copyTo(X(Range(0, 3), Range(3, 4)));
    X.at<double>(3, 3) = 1;

    cout << "solution:\r\n" << X << endl;

    cout << "A*X:\r\n" << A*X << endl;

    cout << "X*B:\r\n" << X*B << endl;

    return 0;
}

可以看到，计算出来的旋转矩阵R是个单位矩阵，也就是代表两个坐标系之间没有旋转。我忘记我A、B这两个数据是从哪里得到的了。
$$X=\left[\begin{array}{cccc}1& 0& 0& 0.5530169888693617\\ 0& 1& 0& -7.861745752782662\\ 0& 0& 1& 2.518087287639135\\ 0& 0& 0& 1\end{array}\right]$$

3.总结

从结果可以看到，利用解出来的X计算对比一下AX、XB，虽然AX与BX没有绝对完全相等，但是基本符合要求，先将就着用。

---

参考：
【c++ 的 Eigen库写 AX=XB的矩阵求解代码】
ChatGPT
【 zhixy/SolveAXXB】