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

韭菜钟

已于 2024-04-30 10:38:35 修改

阅读量984

点赞数

分类专栏： ros 文章标签： ROS2 Eigen

于 2023-05-08 10:11:31 首次发布

本文链接：https://blog.csdn.net/joyopirate/article/details/130553098

版权

ros 专栏收录该内容

30 篇文章 34 订阅

订阅专栏

文章目录

1.背景
2.各种解法
3.总结

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的库，测试了一下，效果比解法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;
}