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

[Disa]

Добрый день

Есть задача - по 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 градусов).

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

[Igors]

Углы Эйлера здесь совершенно не нужны.

Код

C++ (Qt)
// По первым 3 точкам строим матрицу поворота "из локальной в мир" (column-major)
void BuildLocal2World( const Point p[3], Matrix & m )
{
 Vector v01 = (p[1] - p[0]).normalized(); 
 Vector v02 = (p[2] - p[0]).normalized(); 
 Vector nor = cross(v01, v02).normalized();
 v01 = cross(v02, nor);
 m.SetColumn(0, v01); 
 m.SetColumn(1, v02); 
 m.SetColumn(2, nor); 
 m.translate(p[0]);
} 
 
// По первым 3 точкам строим матрицу поворота "из мира в локальную" (column-major)
void BuildWorld2Local( const Point p[3], Matrix & m )
{
 Vector v01 = (p[1] - p[0]).normalized(); 
 Vector v02 = (p[2] - p[0]).normalized(); 
 Vector nor = cross(v01, v02).normalized();
 v01 = cross(v02, nor);
 
 m.SetRow(0, v01); 
 m.SetRow(1, v02); 
 m.SetRow(2, nor); 
 
 Matrix m2;
 m2.translate(-p[0]);
 m = m2 * m;  // см ниже 
} 

Примечание: оператор * для матриц делают всяко, хз какое преобразование первое.  По смыслу должно быть: "из локальной в мир" - сначала поворачиваем, потом добавляем центр локальной.  И наоборот, "из мира в локальную" - сначала отнимаем центр локальной, потом крутим.

Ну и просто множим первую на вторую, опять-таки  с учетом оператора * (первое преобразование "из локальной")

[Disa]

О! Спасибо, сейчас попробую. А, да. Углы, конечно, не нужны, они только для отладки, чтоб можно было руками подогнать в рендере и понять насколько ошибся