如果我知道3D中的齐次变换矩阵，如何使用opencv查找旋转和平移角度？

Question

我发现了几篇有关此主题的文章，但没有一种解决方案使用opencv。

我想知道OpenCv是否有任何功能或类可以在此主题上提供帮助？

我在opencv中有一个4 * 4的仿射变换，我想查找旋转，假设缩放比例为1，矩阵中没有其他变换。

OpenCV中是否有任何功能可以帮助您找到这些参数？

Answer 1

您面临的问题称为矩阵分解问题。

您可以按照以下步骤检索所需的矩阵：

1. 将比例因子计算为矩阵的前三个基向量（列或行）的大小
2. 将前三个基向量除以这些值（从而对其进行归一化）
3. 矩阵左上角的3x3部分现在代表旋转（您可以原样使用，也可以将其转换为四元数形式）
4. 翻译是矩阵的第四个基向量（在齐次坐标中-它是您感兴趣的前三个元素）

在您的情况下，作为缩放因子1，您可以跳过前两个步骤。
要检索旋转矩阵的轴和角度（以弧度为单位），建议您在OpenCV中移植以下Java算法（来源： 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);
}