urbste / MultiCol-SLAM

This repository contains a multi-fisheye camera SLAM. The underlying SLAM system is based on ORB-SLAM.
613 stars 221 forks source link

Hom2Cayley Produces same results for negated angles #47

Open soroushseifi opened 3 years ago

soroushseifi commented 3 years ago

Hi, I have an omnidirectional camera system with 4 cameras in it. There is almost a 90 degree rotation between each camera pair. In case we set CAM 0 (Left camera) as the reference, then CAM 1 (Front camera) and CAM 3 (Back camera) have 90 and -90 rotations around the Y axis according to CAM 0. The strange thing is that the Cayley transform, maps these two rotations to same values ! This results the orientation of Camera 3 and Camera 1 to be the same in SLAM map-viewer which is wrong !

Map viewer with these 3 cameras in it: https://drive.google.com/file/d/1jFVdt5FLhZR9UPOMFgLGRfUZZU6qmJmP/view?usp=sharing

In the link above Camera 3 (blue frame to the left) should face the opposite direction (180 degree around Y).

This is the code I use to convert the transformation matrix to Cayley in python:

import numpy as np
def rot2cayley(R):
    eyeM=np.eye(3)
    C1=R-eyeM
    C2=R+eyeM
    C= C1*np.linalg.inv(C2)
    cayley=np.array([[-C[1,2]],[C[0,2]],[-C[0,1]]])
    return cayley

def hom2cayley(M):
    R=np.array([[M[0,0],M[0,1],M[0,2]],[M[1,0],M[1,1],M[1,2]],[M[2,0],M[2,1],M[2,2]]])
    C=rot2cayley(R)
    answer=np.array([[C[0,0]],[C[1,0]],[C[2,0]],[M[0,3]],[M[1,3]],[M[2,3]]])
    return answer

Then converting 90 and -90 degree rotations around Y (expressed with quaternions) to Cayley parameters:

from scipy.spatial.transform import Rotation as R
import numpy as np
r=R.from_quat([0.0,0.707,0.0,0.707])
print(hom2cayley_r(r.as_matrix()))
r=R.from_quat([0.0,-0.707,0.0,0.707])
print(hom2cayley_r(r.as_matrix()))

Result in : array([[-0. ], [-0.5], [ 0. ]])

array([[ 0. ], [-0.5], [ 0. ]])

Could anyone suggest what am I doing wrong or is this a property of the Cayley transform ?