vSLAMNet（二）-三维旋转变换

发表于 | 分类于 ，

1. 概述

  • 欧拉角
  • 旋转矩阵
  • 四元数

2. 欧拉角与旋转矩阵相互转换

2.1 旋转矩阵转欧拉角

  • C++
1
2
3
4
5
6
7
8
9
10
11
12
13
void rotationMatrixToEuler(Eigen::Matrix3d rotation, Eigen::Vector3d& euler) {
    float sy = static_cast<float>(sqrt(rotation(0, 0) * rotation(0, 0) + rotation(1, 0) * rotation(1, 0)));
    bool singular = sy < 1e-6;
    if (!singular) {
        euler[0] = atan2(rotation(2, 1) , rotation(2, 2));
        euler[1] = atan2(-rotation(2, 0), sy);
        euler[2] = atan2(rotation(1, 0), rotation(0, 0));
    } else {
        euler[0] = atan2(-rotation(1, 2), rotation(1, 1));
        euler[1] = atan2(-rotation(2, 0), sy);
        euler[2] = 0;
    }
}
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
void GetEulerAngles(const ARFrame* frame, Eigen::Vector3d& euler) {
    //first we get the quaternion from m00...m22
    //see http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.html
    float qw = sqrt(1 + frame.camera.transform.columns[0][0] + frame.camera.transform.columns[1][1] + frame.camera.transform.columns[2][2]) / 2.0;
    float qx = (frame.camera.transform.columns[2][1] - frame.camera.transform.columns[1][2]) / (qw * 4.0);
    float qy = (frame.camera.transform.columns[0][2] - frame.camera.transform.columns[2][0]) / (qw * 4.0);
    float qz = (frame.camera.transform.columns[1][0] - frame.camera.transform.columns[0][1]) / (qw * 4.0);
    
    //then we deduce euler angles with some cosines
    //see https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
    // roll (x-axis rotation)
    float sinr = +2.0 * (qw * qx + qy * qz);
    float cosr = +1.0 - 2.0 * (qx * qx + qy * qy);
    float roll = atan2(sinr, cosr);
    
    // pitch (y-axis rotation)
    float sinp = +2.0 * (qw * qy - qz * qx);
    float pitch;
    if (fabs(sinp) >= 1) {
        pitch = copysign(M_PI/2, sinp);
    } else {
        pitch = asin(sinp);
    }

    // yaw (z-axis rotation)
    float siny = +2.0 * (qw * qz + qx * qy);
    float cosy = +1.0 - 2.0 * (qy * qy + qz * qz);
    float yaw = atan2(siny, cosy);
    euler[0] = roll;
    euler[1] = pitch;
    euler[2] = yaw;

}
  • python
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
def rotationToEuler(rotation):
    euler = []
    for i in range(len(rotation)):
        sy = math.sqrt(rotation[i][0]*rotation[i][0] + rotation[i][3]*rotation[i][3])
        singular = sy < 1e-6
        eulers = []
        if ~singular:
            eulers.append(-math.atan2(rotation[i][7], rotation[i][8]))
            eulers.append(-math.atan2(-rotation[i][6], sy))
            eulers.append(-math.atan2(rotation[i][3], rotation[i][0]))
        else:
            eulers.append(-math.atan2(-rotation[i][5], rotation[i][4]))
            eulers.append(-math.atan2(-rotation[i][6], sy))
            eulers.append(0)
        eulers = np.array(eulers)
        euler.append(eulers)
    euler = np.array(euler)
    return euler

2.2 欧拉角转旋转矩阵

  • Python
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
# yaw pitch roll
def EularToRotationMatrix(angle):
    y = angle[0]
    p = angle[1]
    r = angle[2]
    Rx = random.rand(3, 3)
    Ry = random.rand(3, 3)
    Rz = random.rand(3, 3)
    Rz[0, 0] = math.cos(y)
    Rz[0, 1] = -math.sin(y)
    Rz[0, 2] = 0.0
    Rz[1, 0] = math.sin(y)
    Rz[1, 1] = math.cos(y)
    Rz[1, 2] = 0.0
    Rz[2, 0] = 0.0
    Rz[2, 1] = 0.0
    Rz[2, 2] = 1.0
    Ry[0, 0] = math.cos(p)
    Ry[0, 1] = 0.0
    Ry[0, 2] = math.sin(p)
    Ry[1, 0] = 0.0
    Ry[1, 1] = 1.0
    Ry[1, 2] = 0.0
    Ry[2, 0] = -math.sin(p)
    Ry[2, 1] = 0.0
    Ry[2, 2] = math.cos(p)
    Rx[0, 0] = 1.0
    Rx[0, 1] = 0.0
    Rx[0, 2] = 0.0
    Rx[1, 0] = 0.0
    Rx[1, 1] = math.cos(r)
    Rx[1, 2] = -math.sin(r)
    Rx[2, 0] = 0.0
    Rx[2, 1] = math.sin(r)
    Rx[2, 2] = math.cos(r)
    return np.dot(np.dot(Rz, Ry), Rx)

3. 欧拉角与四元数相互转换

3.1 欧拉角转四元数

  • C++
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
// euler angles: roll pitch yaw
void Utility::EulerAngleToQuat(const Eigen::Vector3d euler, Eigen::Quaterniond& quat)
{
    float fCosHRoll = cos(euler[0] * .5f);
    float fSinHRoll = sin(euler[0] * .5f);
    float fCosHPitch = cos(euler[1] * .5f);
    float fSinHPitch = sin(euler[1] * .5f);
    float fCosHYaw = cos(euler[2] * .5f);
    float fSinHYaw = sin(euler[2] * .5f);
    
    /// Cartesian coordinate System
    float w = fCosHRoll * fCosHPitch * fCosHYaw + fSinHRoll * fSinHPitch * fSinHYaw;
    float x = fSinHRoll * fCosHPitch * fCosHYaw - fCosHRoll * fSinHPitch * fSinHYaw;
    float y = fCosHRoll * fSinHPitch * fCosHYaw + fSinHRoll * fCosHPitch * fSinHYaw;
    float z = fCosHRoll * fCosHPitch * fSinHYaw - fSinHRoll * fSinHPitch * fCosHYaw;
    
//    float w = fCosHRoll * fCosHPitch * fCosHYaw + fSinHRoll * fSinHPitch * fSinHYaw;
//    float x = fCosHRoll * fSinHPitch * fCosHYaw + fSinHRoll * fCosHPitch * fSinHYaw;
//    float y = fCosHRoll * fCosHPitch * fSinHYaw - fSinHRoll * fSinHPitch * fCosHYaw;
//    float z = fSinHRoll * fCosHPitch * fCosHYaw - fCosHRoll * fSinHPitch * fSinHYaw;
    
    quat.coeffs().w() = w;
    quat.coeffs().x() = x;
    quat.coeffs().y() = y;
    quat.coeffs().z() = z;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
EulerAngle Quaternion::GetEulerAngle() const
{
	EulerAngle ea;

	/// Cartesian coordinate System 
	//ea.m_fRoll  = atan2(2 * (w * x + y * z) , 1 - 2 * (x * x + y * y));
	//ea.m_fPitch = asin(2 * (w * y - z * x));
	//ea.m_fYaw   = atan2(2 * (w * z + x * y) , 1 - 2 * (y * y + z * z));

	ea.m_fRoll  = atan2(2 * (w * z + x * y) , 1 - 2 * (z * z + x * x));
	ea.m_fPitch = asin(CLAMP(2 * (w * x - y * z) , -1.0f , 1.0f));
	ea.m_fYaw   = atan2(2 * (w * y + z * x) , 1 - 2 * (x * x + y * y));

	return ea;
}
  • Java
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
    public Quaternion eulerAngleToQuaternion(Vector3f euler, Quaternion quat) {

        float fCosHRoll = (float) Math.cos(euler.getX() * .5f);
        float fSinHRoll = (float) Math.sin(euler.getX() * .5f);
        float fCosHPitch = (float) Math.cos(euler.getY() * .5f);
        float fSinHPitch = (float) Math.sin(euler.getY() * .5f);
        float fCosHYaw = (float) Math.cos(euler.getZ() * .5f);
        float fSinHYaw = (float) Math.sin(euler.getZ() * .5f);

        /// Cartesian coordinate System
        float w = fCosHRoll * fCosHPitch * fCosHYaw + fSinHRoll * fSinHPitch * fSinHYaw;
        float x = fSinHRoll * fCosHPitch * fCosHYaw - fCosHRoll * fSinHPitch * fSinHYaw;
        float y = fCosHRoll * fSinHPitch * fCosHYaw + fSinHRoll * fCosHPitch * fSinHYaw;
        float z = fCosHRoll * fCosHPitch * fSinHYaw - fSinHRoll * fSinHPitch * fCosHYaw;

//    float w = fCosHRoll * fCosHPitch * fCosHYaw + fSinHRoll * fSinHPitch * fSinHYaw;
//    float x = fCosHRoll * fSinHPitch * fCosHYaw + fSinHRoll * fCosHPitch * fSinHYaw;
//    float y = fCosHRoll * fCosHPitch * fSinHYaw - fSinHRoll * fSinHPitch * fCosHYaw;
//    float z = fSinHRoll * fCosHPitch * fCosHYaw - fCosHRoll * fSinHPitch * fSinHYaw;

        quat.setW(w);
        quat.setX(x);
        quat.setY(y);
        quat.setZ(z);

        return quat;
    }
  • Eigen
1
2
3
4
5
6
7
8
9
10
11
Eigen::Vector3d euler;
euler[0] = 0;
euler[1] = 0;
euler[2] = M_PI/4.0f;

Eigen::AngleAxisd rollAngle(euler[0], Eigen::Vector3d::UnitX());
Eigen::AngleAxisd pitchAngle(euler[1], Eigen::Vector3d::UnitY());
Eigen::AngleAxisd yawAngle(euler[2], Eigen::Vector3d::UnitZ());
Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle;

std::cout << "q = " << q.coeffs().w() << " " << q.coeffs().x() << " " << q.coeffs().y() << " " << q.coeffs().z() << std::endl;

4. 四元数与旋转矩阵相互转换

4.1 旋转矩阵转四元数

  • python
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
def rotation_matrix_to_quaternion(rotation):
    quat = quaternion.quaternion(1, *[0, 0, 0])
    trace = rotation[0, 0] + rotation[1, 1] + rotation[2, 2]
    if trace > 0:
        s = 0.5 / math.sqrt(trace + 1.0)
        quat.w = 0.25 / s
        quat.x = (rotation[2, 1] - rotation[1, 2]) * s
        quat.y = (rotation[0, 2] - rotation[2, 0]) * s
        quat.z = (rotation[1, 0] - rotation[0, 1]) * s
    else:
        if rotation[0, 0] > rotation[1, 1] and rotation[0, 0] > rotation[2, 2]:
            s = 2.0 * math.sqrt(1.0 + rotation[0, 0] - rotation[1, 1] - rotation[2, 2])
            quat.w = (rotation[2, 1] - rotation[1, 2]) / s
            quat.x = 0.25 * s
            quat.y = (rotation[0, 1] + rotation[1, 0]) / s
            quat.z = (rotation[0, 2] + rotation[2, 0]) / s
        elif rotation[1, 1] > rotation[2, 2]:
            s = 2.0 * math.sqrt(1.0 + rotation[1, 1] - rotation[0, 0] - rotation[2, 2])
            quat.w = (rotation[0, 2] - rotation[2, 0]) / s
            quat.x = (rotation[0, 1] + rotation[1, 0]) / s
            quat.y = 0.25 * s
            quat.z = (rotation[1, 2] + rotation[2, 1]) / s
        else:
            s = 2.0 * math.sqrt(1.0 + rotation[2, 2] - rotation[0, 0] - rotation[1, 1])
            quat.w = (rotation[1, 0] - rotation[0, 1]) / s
            quat.x = (rotation[0, 2] + rotation[2, 0]) / s
            quat.y = (rotation[1, 2] + rotation[2, 1]) / s
            quat.z = 0.25 * s
    quat_array = [quat.w, quat.x, quat.y, quat.z]
    return np.array(quat_array)

4.2 四元数转旋转矩阵

  • python
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
# qw, qx, qy, qz
def QuatToRotationMatrix(q):
    R = np.eye(3, 3)
    w = q[0]
    x = q[1]
    y = q[2]
    z = q[3]
    x2 = x * x
    y2 = y * y
    z2 = z * z
    xy = x * y
    xz = x * z
    yz = y * z
    wx = w * x
    wy = w * y
    wz = w * z
    R[0, 0] = 1.0 - 2.0 * (y2 + z2)
    R[0, 1] = 2.0 * (xy - wz)
    R[0, 2] = 2.0 * (xz + wy)
    R[1, 0] = 2.0 * (xy + wz)
    R[1, 1] = 1.0 - 2.0 * (x2 + z2)
    R[1, 2] = 2.0 * (yz - wx)
    R[2, 0] = 2.0 * (xz - wy)
    R[2, 1] = 2.0 * (yz + wx)
    R[2, 2] = 1.0 - 2.0 * (x2 + y2)
    return R

5. 其他变换

5.1 静止检测

  • Python
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
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
def GetMotionState(values):
    state = []
    isMoving = false
    moveBeg = false
    xPositive = false
    xNegative = false
    yPositive = false
    yNegative = false
    zPositive = false
    zNegative = false
    userAccel = max(max(abs(values[0]), abs(values[1])), abs(values[2]))
    if userAccel > 0.07:
        isMoving = True
    if isMoving :
        moveBeg = True
        if userAccel == fabs(values[0]):
            if values[0] > 0:
                xPositive = True
            else:
                xNegative = True
        elif userAccel == fabs(values[1]):
            if values[1] > 0:
                yPositive = True
            else:
                yNegative = True
        elif userAccel == fabs(values[2]):
            if values[2] > 0:
                zPositive = True
            else:
                zNegative = True
        state = 1
    else:
        if moveBeg:
            changeSymbol = false
            if xPositive or xNegative:
                if xPositive and xNegative:
                    changeSymbol = True
            elif yPositive or yNegative:
                if yPositive and yNegative:
                    changeSymbol = True
            elif zPositive or zNegative:
                if zPositive and zNegative:
                    changeSymbol = True
            xPositive = false
            xNegative = false
            yPositive = false
            yNegative = false
            zPositive = false
            zNegative = false
            if ~changeSymbol:
                state = 1
            else:
                state = 0
            moveBeg = false
        else:
            state = 0
    moveBeg = false
    return state

def GetMotionStateVar(values):
    state = -1
    global values_buf
    value = sqrt(pow(values[0], 2) + pow(values[1], 2) + pow(values[2], 2))
    values_buf.append(value)
    print len(values_buf)
    if len(values_buf) >= 120*0.3 and len(values_buf) <= 120*2:
        var = VarCalculation(values_buf)
        print var
        if var >= 0.0003:
            state = 1
        else:
            state = 0
        del(values_buf[0])
    elif len(values_buf) < 120*0.3:
        state = 0
    else:
        del(values_buf[0])
        state = -1
    return state

def VarCalculation(values_buf):
    sum = 0
    m = len(values_buf)
    for i in range(m):
        sum += values_buf[i]
    dAve = sum/m;
    dVar = 0
    for i in range(m):
        dVar += (values_buf[i]-dAve)*(values_buf[i]-dAve);
    return dVar/m;

def LowPassFilter(values, time):
    print values
    global output
    global startTime
    global timestamp
    global dt
    global alpha
    global count
    if startTime == 0:
        startTime = time*1000
    timestamp = time*1000+0.0000001
    count += 1
    dt = 1.0 / (count/(float((timestamp-startTime))/1000.0))
    alpha = 0.05 / (0.05 + dt)
#     print(startTime, timestamp, count, dt, alpha)
    if count > 5:
        output[0] = alpha * output[0] + (1-alpha)*values[0]
        output[1] = alpha * output[1] + (1-alpha)*values[1]
        output[2] = alpha * output[2] + (1-alpha)*values[2]
    else:
        output[0] = values[0]
        output[1] = values[1]
        output[2] = values[2]
    print output
    return output

5.2 数据点对齐

  • Python
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
41
42
43
44
45
46
47
48
49
50
51
52
def searchInsert(nums, target):
    start = 0
    end = len(nums) - 1
    while start <= end:
        mid = (start + end) // 2
        if nums[mid] == target:
            return mid
        elif nums[mid] < target:
            start = mid + 1
        else:
            end = mid - 1
    return end + 1

# 给定一个元素，在制定list中查找最相邻的元素
def rIndex(nums, target):
    n = len(nums)
    if n == 0: return -1
    mid = searchInsert(nums, target)
    rlist = []  # 保持索引
    i, j = -1, n
    left, rigth = 0, 0  # 左右扩展的标志
    mxg = float('-inf')
    if 0 < mid < n:  # 如果找到了
        i, j = mid-1, mid
        mxg = min(abs(nums[i] - target), abs(nums[j] - target))
        left, rigth = 1, 1
    elif mid == 0:  # 小于最左边的数字
        j = mid
        mxg = abs(nums[j] - target)
        left, rigth = 0, 1
    elif mid == n:  # 大于最右边的数字
        i = mid-1
        mxg = abs(nums[i] - target)
        left, rigth = 1, 0
    while left == 1 or rigth == 1:  # 两边查找
        if i == -1: left = 0
        if j == n: rigth = 0
        if left == 1 and i >= 0:
            le = abs(nums[i] - target)
            if le == mxg:
                rlist = [i] + rlist
                i -= 1
            else:
                left = 0
        if rigth == 1 and j < len(nums):
            ri = abs(nums[j] - target)
            if mxg == ri:
                rlist = rlist + [j]
                j += 1
            else:
                rigth = 0
    return rlist

5.3 向量转四元数

  • Python
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
def quaternion_from_two_vectors(v1, v2):
    """
    Compute quaternion from two vectors. v1 and v2 need not be normalized.

    :param v1: starting vector
    :param v2: ending vector
    :return Quaternion representation of rotation that rotate v1 to v2.
    """
    v1n = v1 / np.linalg.norm(v1)
    #     print(v1n)
    v2n = v2 / np.linalg.norm(v2)
    #     print(v2n)
    w = np.cross(v1n, v2n)
    #     print("w = {}".format(w))
    q = np.array([1.0 + np.dot(v1n, v2n), *w])
    #     print("q = {}".format(q))
    q /= np.linalg.norm(q)
    #     print("q_normlizaed = {}".format(q))
    return quaternion.quaternion(*q)