使用Eigen C ++库进行矩阵求逆会引入噪声

如何解决使用Eigen C ++库进行矩阵求逆会引入噪声

我有一个节点的发布-订阅类型，该节点从订阅的数据流中接收姿势信息（位置和方向），它应该计算逆并发布。

为此，我要根据原始姿势数据创建一个4×4的均匀变换矩阵。

使用Eigen C ++模板库将其求逆，将转换矩阵转换回位置和方向形式并发布。

当我绘制已发布的数据流时，我注意到了一些噪音，因此我最终也发布了原始数据以进行比较，这是我所做的：

将original_pose转换为TF矩阵，命名为original_TF
将original_TF转换回姿势，命名为original_pose_
发布original_pose_
分配给inverted_TF的反向original_TF
将inverted_TF转换为姿势，命名为inverted_pose_
发布inverted_pose_

当我绘制X，Y，Z位置字段时，在反向姿势数据中看到大量的噪声（在下面的视觉效果中为尖峰和凹口）。由于我使用相同的函数将原始姿态转换为TF并返回，因此我知道这些方程式并不是噪声的来源。

蓝色是原始颜色，而红色是倒置颜色。

这是代码。真的没什么特别的。

bool inverse_matrix(std::vector<std::vector<double> > & input,std::vector<std::vector<double> > & output)
{
    // TODO: Currently only supports 4-by-4 matrices,I can make this configurable.
    // see https://eigen.tuxfamily.org/dox/group__TutorialMatrixClass.html

    Eigen::Matrix4d input_matrix;
    Eigen::Matrix4d output_matrix;
    Eigen::VectorXcd input_eivals;
    Eigen::VectorXcd output_eivals;

    input_matrix << input[0][0],input[0][1],input[0][2],input[0][3],input[1][0],input[1][1],input[1][2],input[1][3],input[2][0],input[2][1],input[2][2],input[2][3],input[3][0],input[3][1],input[3][2],input[3][3];
    cout << "Here is the matrix input:\n" << input_matrix << endl;

    input_eivals = input_matrix.eigenvalues();
    cout << "The eigenvalues of the input_eivals are:" << endl << input_eivals << endl;

    if(input_matrix.determinant() == 0) { return false; }
    output_matrix = input_matrix.inverse();
    cout << "Here is the matrix output:\n" << output_matrix << endl;

    output_eivals = output_matrix.eigenvalues();
    cout << "The eigenvalues of the output_eivals are:" << endl << output_eivals << endl;

    // Copy output_matrix to output
    for (int i = 0; i < 16; ++i)
    {
        int in = i/4;
        int im = i%4;
        output[in][im] = output_matrix(in,im);
    }
    return true;
}

-编辑1-

我打印出了inverse_matrix函数的输入和输出矩阵的特征值。

Here is the matrix input:
    0.99916 -0.00155684  -0.0409514    0.505506
 0.00342358   -0.992614    0.121267     0.19625
 -0.0408377   -0.121305   -0.991775     1.64257
          0           0           0           1
The eigenvalues of the input_eivals are:
                (1,0)
 (-0.992614,0.121312)
(-0.992614,-0.121312)
                (1,0)
Here is the matrix output:
    0.99916  0.00342358  -0.0408377   -0.438674
-0.00155684   -0.992614   -0.121305     0.39484
 -0.0409514    0.121267   -0.991775     1.62597
         -0          -0           0           1
The eigenvalues of the output_eivals are:
                (1,0)

-编辑2-

我不太了解您要绘制的内容。是original_pose。{X，Y，Z}和倒立姿势。{X，Y，Z}？那么“尖峰”实际上将取决于矩阵的方向部分。

我正在绘制original_pose_{position.x,position.y,position.z}和inverted_pose_{position.x,position.z}，其中已发布的完整数据为<variable_name>{position.x,position.z,orientation.w,orientation.x,orientation.y,orientation.z}。

您能否详细说明“峰值”确实取决于矩阵的方向部分。”

此外，您的描述与代码段有何关系？（我看不到任何匹配的变量名）。

我已经确定噪声的来源是倒置，这是我描述中的项目编号4：倒数original_TF分配给inverted_TF。一个，我按如下方式调用该函数：

isSuccess = inverse_matrix(original_TF,inverted_TF);

如何存储“姿势”（片段中的vector ）？

是的，我将它们存储在double类型的二维向量中。

在任何时候，您使用Eigen :: Transform来存储转换，还是仅使用普通Eigen :: Matrix4d？

不，我只在inverse_matrix函数中本地使用Eigen::Matrix4d才能使用Eigen库进行计算。

使用Eigen C ++库进行矩阵求逆会引入噪声

如何解决使用Eigen C ++库进行矩阵求逆会引入噪声

相关推荐