旋轉矩陣 轉 尤拉角 (Conversion Euler To Matrix)

我寫出了屬於我自己的兩種版本, 如下

版本一

參考來源

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
/** this conversion uses conventions as described on page:
*   https://www.euclideanspace.com/maths/geometry/rotations/euler/index.htm
*   Coordinate System: right hand
*   Positive angle: right hand
*   Order of euler angles: heading first, then attitude, then bank
*   matrix row column ordering:
*   [m00 m01 m02]
*   [m10 m11 m12]
*   [m20 m21 m22]*/
public final void rotate(matrix  m) {
    // Assuming the angles are in radians.
	if (m.m10 > 0.998) { // singularity at north pole
		heading = Math.atan2(m.m02,m.m22);
		attitude = Math.PI/2;
		bank = 0;
		return;
	}
	if (m.m10 < -0.998) { // singularity at south pole
		heading = Math.atan2(m.m02,m.m22);
		attitude = -Math.PI/2;
		bank = 0;
		return;
	}
	heading = Math.atan2(-m.m20,m.m00);
	bank = Math.atan2(-m.m12,m.m11);
	attitude = Math.asin(m.m10);
}

轉成 opencv 的版本, 且對應到我的座標系 (x, z 對調), 最後 x y 軸還加上負號.
bank(x) -> attitude(x)
heading(y) -> heading(y)
attitude(z) -> bank(z)
euclideanspace -> 我的座標系(OpennGL)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
Vec3f matrix2Euler(const Mat &R) {
    // Assuming the angles are in radians.
	float heading, attitude, bank;
	if (R.at<double>(1,0) > 0.998) { // singularity at north pole
		heading = atan2(R.at<double>(0,2),R.at<double>(2,2));
		attitude = M_PI/2;
		bank = 0;
		return Vec3f(bank, heading, attitude);
	}
	if (R.at<double>(1,0) < -0.998) { // singularity at south pole
		heading = atan2(R.at<double>(0,2),R.at<double>(2,2));
		attitude = -M_PI/2;
		bank = 0;
		return Vec3f(bank, heading, attitude);
	}
	heading = atan2(-R.at<double>(2,0),R.at<double>(0,0));
	bank = atan2(-R.at<double>(1,2),R.at<double>(1,1));
	attitude = asin(R.at<double>(1,0));

	return Vec3f(-bank, -heading, attitude);
}

版本二

參考來源

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
// Checks if a matrix is a valid rotation matrix.
static bool isRotationMatrix(const Mat &R)
{
    Mat Rt;
    transpose(R, Rt);
    Mat shouldBeIdentity = Rt * R;
    Mat I = Mat::eye(3,3, shouldBeIdentity.type());

    return norm(I, shouldBeIdentity) < 1e-6;
}

/** rotation matrix to euler angles
*   Coordinate System: right hand
*   Positive angle: right hand
*   Order of euler angles: heading first, then attitude, then bank
*   matrix row column ordering:
*   [m00 m01 m02]
*   [m10 m11 m12]
*   [m20 m21 m22]*/
Vec3f matrix2Euler(const Mat &R)
{
    assert(isRotationMatrix(R));
    float sy = sqrt(R.at<double>(0,0) * R.at<double>(0,0) +  R.at<double>(1,0) * R.at<double>(1,0) );
    bool singular = sy < 1e-6; // If

    float x, y, z;
    if (!singular)
    {
        x = atan2(R.at<double>(2,1) , R.at<double>(2,2));
        y = atan2(-R.at<double>(2,0), sy);
        z = atan2(R.at<double>(1,0), R.at<double>(0,0));
    }
    else
    {
        x = atan2(-R.at<double>(1,2), R.at<double>(1,1));
        y = atan2(-R.at<double>(2,0), sy);
        z = 0;
    }
    return Vec3f(x, y, z);
}

轉成 我的座標系 (z 軸加上負號).

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
Vec3f matrix2Euler(const Mat &R)
{
    assert(isRotationMatrix(R));
    float sy = sqrt(R.at<double>(0,0) * R.at<double>(0,0) +  R.at<double>(1,0) * R.at<double>(1,0) );
    bool singular = sy < 1e-6; // If

    float x, y, z;
    if (!singular)
    {
        x = atan2(R.at<double>(2,1) , R.at<double>(2,2));
        y = atan2(-R.at<double>(2,0), sy);
        z = atan2(R.at<double>(1,0), R.at<double>(0,0));
    }
    else
    {
        x = atan2(-R.at<double>(1,2), R.at<double>(1,1));
        y = atan2(-R.at<double>(2,0), sy);
        z = 0;
    }
    return Vec3f(x, y, -z);
}