How to find the angle of rotation and translation using opencv if I know the homogeneous transformation matrix in 3d?

Question

I found several post about this subject, but none of the solutions are using opencv.

I am wondering if OpenCv has any function, or class that could help on this subject?

I have a 4*4 affine transformation in opencv and I am looking to find the rotation, translation assuming that scaling is 1 and there is no other transformation in matrix.

Is there any function in OpenCV to help finding these parameters?

@berak: Thanks. Are you suggesting to get the 3X3 rotation matrix and use the above function to get angles and find translation by using 4X4 matrix? — mans
yes. if you chop off the last row-vec from the 4x4, that's your translation. if you chop off the last col-vec from the remaining 3x4, you get the 3x3 rotation mat. assuming you wanted the angles (i'm no more sure), feed that into Rodrigues. (or keep it as is , if you wanted the Mat) — berak
@berak Thanks. I need angles. What is the output of Rodrigues? As I can read from documentation it is a 3X1 matrix. Is it angle around X,Y,Z in radians? — mans

mrcointreau mrcointreau · Accepted Answer · 2014-10-20T07:30:57

The problem you're facing is known as a matrix decomposition problem.

You can retrieve your desired matrices following these steps:

Compute the scaling factors as the magnitudes of the first three basis vectors (columns or rows) of the matrix
Divide the first three basis vectors by these values (thus normalizing them)
The upper-left 3x3 part of the matrix now represents the rotation (you can use this as is, or convert it to quaternion form)
The translation is the fourth basis vector of the matrix (in homogeneous coordinates - it'll be the first three elements that you're interested in)

In your case, being your scaling factor 1, you can skip the first two steps.
To retrieve the rotation matrix axis and angle (in radians) I suggest you to port the following Java algorithm in OpenCV (source: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/).

/**
This requires a pure rotation matrix 'm' as input.
*/
public axisAngle toAxisAngle(matrix m) {
  double angle,x,y,z; // variables for result
    double epsilon = 0.01; // margin to allow for rounding errors
    double epsilon2 = 0.1; // margin to distinguish between 0 and 180 degrees
    // optional check that input is pure rotation, 'isRotationMatrix' is defined at:
    // http://www.euclideanspace.com/maths/algebra/matrix/orthogonal/rotation/
    assert isRotationMatrix(m) : "not valid rotation matrix" ;// for debugging
    if ((Math.abs(m[0][1]-m[1][0])< epsilon)
      && (Math.abs(m[0][2]-m[2][0])< epsilon)
      && (Math.abs(m[1][2]-m[2][1])< epsilon)) {
        // singularity found
        // first check for identity matrix which must have +1 for all terms
        //  in leading diagonaland zero in other terms
        if ((Math.abs(m[0][1]+m[1][0]) < epsilon2)
          && (Math.abs(m[0][2]+m[2][0]) < epsilon2)
          && (Math.abs(m[1][2]+m[2][1]) < epsilon2)
          && (Math.abs(m[0][0]+m[1][1]+m[2][2]-3) < epsilon2)) {
            // this singularity is identity matrix so angle = 0
            return new axisAngle(0,1,0,0); // zero angle, arbitrary axis
        }
        // otherwise this singularity is angle = 180
        angle = Math.PI;
        double xx = (m[0][0]+1)/2;
        double yy = (m[1][1]+1)/2;
        double zz = (m[2][2]+1)/2;
        double xy = (m[0][1]+m[1][0])/4;
        double xz = (m[0][2]+m[2][0])/4;
        double yz = (m[1][2]+m[2][1])/4;
        if ((xx > yy) && (xx > zz)) { // m[0][0] is the largest diagonal term
            if (xx< epsilon) {
                x = 0;
                y = 0.7071;
                z = 0.7071;
            } else {
                x = Math.sqrt(xx);
                y = xy/x;
                z = xz/x;
            }
        } else if (yy > zz) { // m[1][1] is the largest diagonal term
            if (yy< epsilon) {
                x = 0.7071;
                y = 0;
                z = 0.7071;
            } else {
                y = Math.sqrt(yy);
                x = xy/y;
                z = yz/y;
            }   
        } else { // m[2][2] is the largest diagonal term so base result on this
            if (zz< epsilon) {
                x = 0.7071;
                y = 0.7071;
                z = 0;
            } else {
                z = Math.sqrt(zz);
                x = xz/z;
                y = yz/z;
            }
        }
        return new axisAngle(angle,x,y,z); // return 180 deg rotation
    }
    // as we have reached here there are no singularities so we can handle normally
    double s = Math.sqrt((m[2][1] - m[1][2])*(m[2][1] - m[1][2])
        +(m[0][2] - m[2][0])*(m[0][2] - m[2][0])
        +(m[1][0] - m[0][1])*(m[1][0] - m[0][1])); // used to normalise
    if (Math.abs(s) < 0.001) s=1; 
        // prevent divide by zero, should not happen if matrix is orthogonal and should be
        // caught by singularity test above, but I've left it in just in case
    angle = Math.acos(( m[0][0] + m[1][1] + m[2][2] - 1)/2);
    x = (m[2][1] - m[1][2])/s;
    y = (m[0][2] - m[2][0])/s;
    z = (m[1][0] - m[0][1])/s;
   return new axisAngle(angle,x,y,z);
}

How to find the angle of rotation and translation using opencv if I know the homogeneous transformation matrix in 3d?

1 Answers