python SO3 & so3 BCH近似计算

import numpy as np

def skew(x):
    x_skew = np.zeros((3, 3)) 
    x_skew[0, 1] = -x[2]
    x_skew[0, 2] = x[1]
    x_skew[1, 0] = x[2]
    x_skew[1, 2] = -x[0]
    x_skew[2, 0] = -x[1]
    x_skew[2, 1] = x[0]
    return x_skew

def so3_to_SO3(so3):
    theta = np.linalg.norm(so3)
    axis = so3/theta
    Askew = skew(axis)

    R = np.cos(theta)*np.eye(3) + np.sin(theta) * Askew + (1.0 - np.cos(theta)) \
      * np.multiply(np.array([axis]).T, axis)
    return R


def SO3_to_so3_hat(R):
    theta = np.arccos((np.trace(R) - 1) / 2)
    if abs(theta) < 1e-6:
        omega_hat = np.zeros((3, 3)) 
    else:
        omega_hat = (theta / (2 * np.sin(theta))) * (R - R.T)
    return omega_hat

def SO3_to_so3(R):
    ''' 
    theta = np.arccos((np.trace(R) - 1) / 2)
    if abs(theta) < 1e-6:
        omega = np.zeros(3)
    else:
        omega = (theta / (2 * np.sin(theta))) * np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]])
    '''
    skew_omega = SO3_to_so3_hat(R)
    omega = np.array([skew_omega[2,1], skew_omega[0,2], skew_omega[1,0]])
    return omega

#so3 = np.random.normal(0.0, 0.0001, 3);
so3 = np.random.normal(1.0, 1, 3); 
print("so3:\n", so3)

so3_skew = skew(so3)
print("so3 skew:\n", so3_skew)

R = so3_to_SO3(so3)
print("R:\n", R)

omega_hat = SO3_to_so3_hat(R)

print("omega hat:\n", omega_hat)

omega = SO3_to_so3(R)

print("omega:\n", omega)
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

BCH近似公式：

其中：