解A*w=b的最小二乘QR-分解

Question

我正在尝试实现‘m 24-算法，以校准法兰/工具和机器人/世界从这个纸由弗洛里斯恩斯特(2012年)。

我需要解一个方程M_i*X - Y*N_i = 0，其中M_i和N_i是已知的，I从1到测量数，X和Y是未知矩阵。

在本文中，他们把这个方程组合成一个线性方程组A*w = b，其中A由12*行和24列组成，所以我有一个有24个参数的线性方程组，其中我至少需要两个测量来求解这个系统。

要解决这个方程，我需要使用QR-因子在最小二乘意义上，因为有更多的测量，这个系统有更多的方程比参数。

我正在使用Apache库中的OLSMultipleLinearRegression来求解方程系统：

OLSMultipleLinearRegression regression = new OLSMultipleLinearRegression();
regression.setNoIntercept(true);
regression.newSampleData(B.toArray(), A.getData());
RealVector w = new ArrayRealVector(regression.estimateRegressionParameters());

RealVector w现在应该包含未知矩阵X和Y的条目(没有最后一行，因为这些矩阵都是齐次转换矩阵，所以总是[0 0 0 1] )。

我用手工制作了一些测试测量数据，因为我目前无法访问我想要使用的机器人和跟踪系统，因为电晕。

然而，我得到的结果X和Y矩阵(向量w)总是那么荒谬，而且离我期望的结果很远。例如，当我使用没有任何平移或旋转误差的精确变换矩阵时(除了计算机上的计算误差)，我得到矩阵的旋转部分值大于10^14 (这显然不可能是真的)和平移部分大于10^17，而不是预期的100左右。

当我把一些测量误差加到我的矩阵中(例如旋转+-0.01°，平移部分是+-0.01°)时，我不会得到那些超高的值，但是旋转部分的值不可能是真的。

你知不知道，为什么这些值是如此的错误，或任何建议，如何使用QR-因子分解的最起码的平方意义上的这个库？

下面还有我的代码，使用M_i和N_i度量来创建A的每个条目/子矩阵Ai：

private RealMatrix createAi(RealMatrix m,RealMatrix n, boolean invert) {
    RealMatrix M = new Array2DRowRealMatrix();
    if(invert) {
         M = new QRDecomposition(m).getSolver().getInverse();
    }else {
        M = m.copy();
    }
    // getRot is a method i wrote to get the rotational part of a matrix
    RealMatrix RM = getRot(M);
    
    RealMatrix N = n.copy();
    // 12 equations per Measurement and 24 parameters to solve for
    RealMatrix Ai = new Array2DRowRealMatrix(12,24);
    RealMatrix Zero = new Array2DRowRealMatrix(3,3);
    RealMatrix Identity12 = MatrixUtils.createRealIdentityMatrix(12);
    
    // first column
    Ai.setSubMatrix(RM.scalarMultiply(N.getEntry(0, 0)).getData(), 0, 0);
    Ai.setSubMatrix(RM.scalarMultiply(N.getEntry(0, 1)).getData(), 3, 0);
    Ai.setSubMatrix(RM.scalarMultiply(N.getEntry(0, 2)).getData(), 6, 0);
    Ai.setSubMatrix(RM.scalarMultiply(N.getEntry(0, 3)).getData(), 9, 0);
                                                       
    // secondcolumn
    Ai.setSubMatrix(RM.scalarMultiply(N.getEntry(1, 0)).getData(), 0, 3);
    Ai.setSubMatrix(RM.scalarMultiply(N.getEntry(1, 1)).getData(), 3, 3);
    Ai.setSubMatrix(RM.scalarMultiply(N.getEntry(1, 2)).getData(), 6, 3);
    Ai.setSubMatrix(RM.scalarMultiply(N.getEntry(1, 3)).getData(), 9, 3);

    // third column
    Ai.setSubMatrix(RM.scalarMultiply(N.getEntry(2, 0)).getData(), 0, 6);
    Ai.setSubMatrix(RM.scalarMultiply(N.getEntry(2, 1)).getData(), 3, 6);
    Ai.setSubMatrix(RM.scalarMultiply(N.getEntry(2, 2)).getData(), 6, 6);
    Ai.setSubMatrix(RM.scalarMultiply(N.getEntry(2, 3)).getData(), 9, 6);
    
    // fourth column
    Ai.setSubMatrix(Zero.getData(), 0, 9);
    Ai.setSubMatrix(Zero.getData(), 3, 9);
    Ai.setSubMatrix(Zero.getData(), 6, 9);
    Ai.setSubMatrix(RM.getData(), 9, 9);
    
    // fifth column
    Ai.setSubMatrix(Identity12.scalarMultiply(-1d).getData(), 0, 12);
    return Ai;
}

下面是我的代码，使用M_i度量来创建b的每个条目/子向量bi：

private RealVector createBEntry(RealMatrix m, boolean invert) {
    RealMatrix bi = new Array2DRowRealMatrix(1,12);
    RealMatrix negative_M = new Array2DRowRealMatrix();
    // getTrans is a method i wrote to get the translational part of a matrix
    if(invert) {
        negative_M = getTrans(new QRDecomposition(m).getSolver().getInverse()).scalarMultiply(-1d); 
    }else {
        negative_M = getTrans(m).scalarMultiply(-1d);
    }       
    bi.setSubMatrix(negative_M.getData(), 0, 9);
    
    return bi.getRowVector(0);
}

Answer 1

我能够找到解决我的问题的办法，我想和你分享它。该问题不是编程误差，而是给出了求解线性方程组所需的不正确矩阵( Ai矩阵)。我试图利用齐次变换矩阵和旋转矩阵的特性，自行从M*X - Y*N = 0中提取一个线性方程组。我想出了以下解决方案：

​

哪里

文中给出的矢量bi是很好的。

由于恩斯特教授在我的大学任教，我要和他一起上一门课，我会设法让他意识到这个错误。