Поиск матрицы трансляции и поворота

Добрый день

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

Я переписал алгоритм отсюда - http://nghiaho.com/?page_id=671 с питоновского скрипта на плюсах с помощью eigen. При отображении вычисления делаю при помощи glm (ну и в шейдере model матрицу умножаю на view и projection)

Вот код алгоритма:

Код:

Eigen::MatrixXf matrixA(3, 3);
Eigen::MatrixXf matrixB(3, 3);

... // инициализация матриц

Eigen::MatrixXf centroidA(3, 3);
Eigen::MatrixXf centroidB(3, 3);

// считаем центроид
for (int i = 0; i < 3; i++)
{
    centroidA(i, 0) = (matrixA(0, i) + matrixA(1, i) + matrixA(2, i)) / 3;
    centroidB(i, 0) = (matrixB(0, i) + matrixB(1, i) + matrixB(2, i)) / 3;

    centroidA(i, 1) = 0;
    centroidB(i, 1) = 0;

    centroidA(i, 2) = 0;
    centroidB(i, 2) = 0;
}

Eigen::MatrixXf matrixAA(3, 3);
Eigen::MatrixXf matrixBB(3, 3);

// сдвигаем СК
for (int i = 0; i < 3; i++)
{
    for (int j = 0; j < 3; j++)
    {
        matrixAA(j, i) = matrixA(j, i) - centroidA(i, 0);
        matrixBB(j, i) = matrixB(j, i) - centroidB(i, 0);
    }
}

// вычисляем поврото через сингулярное разложение и через поворот вычисляем трансляцию
Eigen::MatrixXf matrixH = matrixAA.transpose() * matrixBB;
Eigen::JacobiSVD<Eigen::MatrixXf> svd(matrixH, Eigen::ComputeThinU | Eigen::ComputeThinV);

Eigen::MatrixXf rotationMatrix = svd.matrixV() * svd.matrixU().transpose();
Eigen::MatrixXf translationMatrix = -rotationMatrix * centroidA + centroidB;

// задаём поворот
position->x = translationMatrix(0, 0);
position->y = translationMatrix(1, 0);
position->z = translationMatrix(2, 0);

// в углы эйлера
float thetaX = atan2(rotationMatrix(2, 1), rotationMatrix(2, 2));
float thetaY = atan2(-rotationMatrix(2, 0), sqrt(rotationMatrix(2, 1) * rotationMatrix(2, 1) + rotationMatrix(2, 2) * rotationMatrix(2, 2)));
float thetaZ = atan2(rotationMatrix(1, 0), rotationMatrix(0, 0));

// задаём углы
orientation->x = thetaX;
orientation->y = thetaY;
orientation->z = thetaZ;

// вычисляем ошибку
Eigen::MatrixXf translationMatrixFilled(3, 3);
for (int i = 0; i < 3; i++)
{
    translationMatrixFilled(0, i) = translationMatrix(0);
    translationMatrixFilled(1, i) = translationMatrix(1);
    translationMatrixFilled(2, i) = translationMatrix(2);
}

Eigen::Matrix3f b1 = rotationMatrix * matrixA.transpose() + translationMatrixFilled;
Eigen::Matrix3f err = matrixB - b1.transpose();

Вот результат (как видно из результата, матрицы найдены не супер, но верно (ошибка в 2-3 знаке после запятой). На подогнанных данных (так точки с физических датчиков) ошибка очень маленькая (R матрица поворота, T - вектор трансляции, thetas - углы Эйлера из R)

Но в итоге, когда я отображаю итоговое положение
(если быть конкретнее, то вот вычисление model матрицы)

Код:

RotationMatrix = glm::eulerAngleXYZ(Orientation.x, Orientation.y, Orientation.z);
TranslationMatrix = translate(glm::mat4(), Position);
        
M = TranslationMatrix * RotationMatrix;
VP = viewProjection;
MVP = VP * M;

То итоговая ошибка в углах и трансляции сильно заметна (например, по одной оси трансляция может различаться в 2-3 раза от нужного значения, а поворот на 10-20 градусов).

Буду рад любой помощи, спасибо за подсказки!