Блог

У меня есть тип публикации-подписки узла, который получает информацию о позе (положение и ориентацию) из подписанного потока данных, и он должен вычислить обратную и опубликовать.

Для этого я создаю однородную матрицу преобразования 4 на 4 из исходных данных pose.

Инвертируйте его, используя собственную библиотеку шаблонов C , преобразуйте матрицу преобразования обратно в форму положения и ориентации и опубликуйте ее.

Когда я строил опубликованный поток данных, я заметил некоторый шум, поэтому в итоге я также опубликовал исходные данные для сравнения, вот что я сделал:

1. преобразовать original_pose в матрицу TF, названную как original_TF
2. преобразовать original_TF обратно в pose, названный как original_pose_
3. опубликовать original_pose_
4. обратный original_TF присваивает inverted_TF
5. преобразовать inverted_TF в pose, названный как inverted_pose_
6. опубликовать inverted_pose_

Когда я строю поля положения X, Y, Z, я вижу значительное количество шума (пики и вырезы на изображении ниже) в данных перевернутой позы. Поскольку я использую те же функции для преобразования исходной позы в TF и обратно, я знаю, что эти уравнения не являются источником шума.

Синий — это оригинал, тогда как красный — инвертированный.

Вот код. На самом деле ничего экстраординарного.

 bool inverse_matrix(std::vector<std::vector<double> > amp; input, std::vector<std::vector<double> > amp; 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)
 (-0.992614,0.121312)
(-0.992614,-0.121312)
                (1,0)
  

— Правка 2 —

Я не совсем понимаю, что вы замышляете. Это original_pose.{X,Y,Z} и inverted_pose.{X,Y,Z}? Тогда «пики» действительно будут зависеть от ориентации-части матрицы.

Я строю original_pose_{position.x, position.y, position.z} график и inverted_pose_{position.x, position.y, position.z} где находятся полные опубликованные данные <variable_name>{position.x, position.y, position.z, orientation.w, orientation.x, orientation.y, orientation.z} .

Не могли бы вы подробнее рассказать о «шипах», которые действительно будут зависеть от ориентации матрицы».?

Кроме того, как ваше описание связано с фрагментом кода? (Я не вижу никаких совпадающих имен переменных).

Я определил, что источником шума является инверсия, которая является элементом под номером 4 в моем описании: обратное original_TF присвоение inverted_TF .Чтобы связать друг с другом, я вызываю функцию следующим образом:

 isSuccess = inverse_matrix(original_TF, inverted_TF);
  

Как вы храните «poses» (это вектор <вектор> в вашем фрагменте)?

Да, я сохраняю их в 2-мерных векторах типа double.

В какой-либо момент вы используете Eigen::Transform для хранения преобразований или просто Eigen::Matrix4d?

Нет, я использую только Eigen::Matrix4d локально в функции inverse_matrix, чтобы иметь возможность использовать собственную библиотеку для вычислений.