Search code examples
c++opencvperspectivecamera

How can I find the angle between two chessboard planes?

I have two chessboard poses obtained with solvePnp:

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

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

How can I check if the planes of the poses are parallel, or find the angle between these planes?

More info

I tried obtaining Euler angles and computing the difference between each alpha, beta and gamma but that only tells me relative rotation for each axis I think:

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

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

I used the getEulerAngles implementation from 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);
}

Edit

In my case a rotation-translation pair (R, T) gives the correspondence between a coordinate system where the camera is at (0,0,0) (the camera coordinate system) to a coordinate system where (0,0,0) is something I defined in the first two parameters of solvePnp (the world coordinate system). So I have two world coordinate systems relative to the same camera coordinate system. If I could switch from coord. system 2 to coord. system 1 I could use the Z=0 planes for each one to find the normals and solve my problem.

I think that for example switching from coord. system 2 to camera system should be done like in this post:

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

Then if Pw = [X Y Z] is a point in world coord. system 2 I can get its camera system coords.with:

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

Following the same logic again I can get the coordinates of Pc relative to coord. system 1:

Pw1 = [ R1 T1] * Pc

Should I normalize Pc or just normalize Pw1 at the end?

Solution

  • I found how to translate points between coordinate systems in this OpenCV Demo.

    The explanation from "Demo 3: Homography from the camera displacement" (the section spanning from title right until the first lines of code) shows how to translate between coordinate systems using matrix multiplication. I just had to apply it to my situation (I had CMO1 and CMO2 and needed to find O1MO2).

    This way I can get two planes in the same coord. system, get their normals and find the angle between them.

    Also it helped to realize that the extrinsic matrix [R T] translates a 3D point from the world coord. system to the camera coord. system (where camera is at (0,0,0)), not the other way around.