Use Eigen to solve the AX=XB problem in hand-eye calibration

In hand-eye calibration, solving AX=XB is basically an unavoidable problem, where A, B, and X are all 4*4 transformation matrices. Fortunately, there are many ready-made codes on the Internet. After preliminary testing, the results are good. Now I will send out the ones that I tested and basically meet the requirements:

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;
}

Output results:

As you can see from the results, use the solved X to calculate and compare AX and

---

Reference:
[C++'s Eigen library writes matrix solving code for AX=XB]
ChatGPT