search

CathIAS / TLIO

Tight Learned Inertial Odometry
Other
277 stars 69 forks source link

function rot_2vec(a,b) R_ba calculating #24

Closed wanXiao1984 closed 3 years ago

wanXiao1984 commented 3 years ago

hi, CathIAS

Why _Rba is calculated like this ?
Now that we know the rotation angle and the axis, why not directly substitute the Rodrigues formula to solve the rotation matrix ? image

Funciton path: src/utils/math_utils.py

@jit(nopython=True, parallel=False, cache=True) def rot_2vec(a, b): assert a.shape == (3, 1) assert b.shape == (3, 1)

def hat(v):
    v = v.flatten()
    R = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
    return R

a_n = np.linalg.norm(a)
b_n = np.linalg.norm(b)
a_hat = a / a_n
b_hat = b / b_n
omega = np.cross(a_hat.T, b_hat.T).T
c = 1.0 / (1 + np.dot(a_hat.T, b_hat))
**_R_ba = np.eye(3) + hat(omega) + c * hat(omega) @ hat(omega)_**
return R_ba
wanXiao1984 commented 3 years ago

Get it . Two methods are the same results.