Как я могу найти угол между двумя плоскостями шахматной доски?

Question

У меня есть две позы на шахматной доске, полученные с solvePnp:

Mat rotationVector1, translationVector1;
solvePnP(chess1WorldPoints, chess1ImagePoints, intrinsicMatrix, distortCoefficients, rotationVector1, translationVector1);

Mat rotationVector2, translationVector2;
solvePnP(chess2WorldPoints, chess2ImagePoints, intrinsicMatrix, distortCoefficients, rotationVector2, translationVector2);

Как я могу проверить, параллельны ли плоскости поз или найти угол между этими плоскостями?

Больше информации

Я попытался получить углы Эйлера и вычислить разницу между каждой альфа, бета и гаммой, но это говорит мне только об относительном вращении для каждой оси, я думаю:

Vec3d eulerAnglesPose1; 
Mat rotationMatrix1;
Rodrigues(rotationVector1, rotationMatrix1);
getEulerAngles(rotationMatrix1, eulerAngles1);

Vec3d eulerAnglesPose2;
Mat rotationMatrix2;
Rodrigues(rotationVector2, rotationMatrix2);
getEulerAngles(rotationMatrix2, eulerAngles2);

Я использовал getEulerAngles реализация от Camera Rotation SolvePnp:

void getEulerAngles(Mat &rotCamerMatrix, Vec3d &eulerAngles) 
{
    Mat cameraMatrix, rotMatrix, transVect, rotMatrixX, rotMatrixY, rotMatrixZ;
    double* _r = rotCamerMatrix.ptr<double>();
    double projMatrix[12] = 
    { 
     _r[0],_r[1],_r[2],0,
     _r[3],_r[4],_r[5],0,
     _r[6],_r[7],_r[8],0 
    };

    decomposeProjectionMatrix(Mat(3, 4, CV_64FC1, projMatrix), cameraMatrix, rotMatrix, transVect, rotMatrixX, rotMatrixY, rotMatrixZ, eulerAngles);
}

редактировать

В моем случае пара вращение-перемещение (R, T) дает соответствие между системой координат, где камера находится в точке (0,0,0) (система координат камеры), системе координат, где (0,0,0) это то, что я определил в первых двух параметрах solvePnp (мировой системы координат). Таким образом, у меня есть две мировые системы координат относительно одной и той же системы координат камеры. Если бы я мог переключиться с координации. система 2 по согласованию. Система 1 Я мог бы использовать плоскости Z=0 для каждой, чтобы найти нормали и решить мою проблему.

Я думаю, что, например, переход от скоординат. Система 2 для системы камеры должна быть сделана как в этом посте:

Rinv = R' (just the transpose as it's a rotation matrix)
Tinv = -Rinv * T (T is 3x1 column vector)

Тогда, если Pw = [X Y Z] - точка в мировой координате. Система 2 Я могу получить его систему координат камеры.

Pc = [ Rinv Tinv] * [X Y Z 1] transposed.
Pc looks like [a b c d]

Следуя той же логике, я могу получить координаты Pc относительно координаты. система 1:

Pw1 = [ R1 T1] * Pc

Должен ли я нормализовать Pc или просто нормализовать Pw1 в конце?

Answer 1

Я нашел, как переводить точки между системами координат в этой демонстрации OpenCV.

Объяснение из "Демонстрации 3: Гомография от смещения камеры" (раздел, охватывающий от заголовка справа до первых строк кода) показывает, как переводить между системами координат с использованием умножения матриц. Я просто должен был применить это к моей ситуации (у меня были ^C M_O1 и ^C M_O2, и мне нужно было найти ^O1 M_O2).

Таким образом, я могу получить две плоскости с одинаковыми координатами. Система, получить их нормали и найти угол между ними.

Также это помогло понять, что внешняя матрица [R T] переводит трехмерную точку из мировой координаты. Система координат камеры. система (где камера в (0,0,0)), а не наоборот.