Frame간의 Transformation matrix

SLAM분야에서는 Calibration등의 과정에서 두 프레임이 주어졌을때 transformation matrix를 구해야하는 상황이 흔히 발생한다.
하지만 transformation의 기준, 예를들어 Transformation from camera to body-frame이 어떤것을 나타내는지는 오픈소스의 저자마다 axis기준인지 point기준인지 다를수 있어 주의가 필요하다.

1. Concept

VINS-Mono의 config를 보면

#Rotation from camera frame to imu frame, imu^R_cam
extrinsicRotation: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 0.9995004,   0.03139202,  0.00367391,
          -0.03143884,  0.99941494,  0.01346765,
          -0.00324898, -0.01357642,  0.99990256]
#Translation from camera frame to imu frame, imu^T_cam
extrinsicTranslation: !!opencv-matrix
   rows: 3
   cols: 1
   dt: d
   data: [0.00500934, -0.00600253, 0.04180872]

\(T^i_c\)를 나타내고 있으며, Kalibr가 내놓은 결과의 T_ic와 동일하다

하지만 ORB-SLAM3의 config를 보면

# Transformation from body-frame (imu) to left camera 
IMU.T_b_c1: !!opencv-matrix # 
   rows: 4
   cols: 4
   dt: f
   data: [-0.999926331416774, -0.004903922475695, 0.011103300578872, 0.010699210334022,
          0.004934127578069, -0.999984196594937, 0.002694614150709, -0.000052795150761,
          0.011089910930019, 0.002749200743896, 0.999934725755053, -0.000118662002204,
          0.0, 0.0, 0.0, 1.0]

이 역시 \(T^i_c\)를 나타내는데, 설명하고 있는 문구가 다른 것을 볼 수 있다.

T_b_c1과 같이 앞에 오는 것이 보통 기준이 되는 프레임, 즉 위의 프레임이므로 문구보다는 변수명으로 보는것이 좋다.

추가적으로, rostopic에서 주는 tf 메세지는 frame_id, child_frame_id가 있는데 frame_id가 기준이 되는 즉 변수에서 위에 오는 부분이 된다.

한편, \(T_b^c\)를 구하기 위해 Axis를 기준으로 생각해 볼 때는 아래와 같이 rotation matrix를 계산한다.

[Body frame] = R * [Camera frame]

Translation은 어떤 frame을 기준으로 움직이느냐에 따라 추가하면 된다.

예를들어 body frame기준으로 x=1m가 camera frame까지 차이가 난다면,

[Camera frame] = T * [Body frame]에서 Body frame의 T matrix의 translation 부분의 x값을 1m 입력해주면 된다.

이를 종합하여 볼 때, axis를 기준으로 할때 translation이 어떤 frame기준인지 먼저 잘 살펴보고, rotation의 기준도 동일하게 정해주는 것이 좋다.

2. Rotation Matrix

roll : x축 기준 회전

Untitled

pitch : y축 기준 회전

Untitled

yaw : z축 기준 회전

Untitled

\[R = R_{yaw} * R_{pitch} * R_{roll}\]

roll → pitch → yaw순으로 회전한다.

3. Example

Camera frame TF :

Untitled

IMU frame TF :

Untitled

위 두 frame의 Translation이 body기준으로 1m차이 난다고 주어졌다고 가정하자.

만약 Transformation Camera to body(IMU)을 구해야 하더라도, body기준으로 값이 주어졌기 때문에 Transformation body to Camera를 먼저 구하고 inverse해주는 것이 더 편리하다.

Body기준으로 roll 방향 90도, pitch방향 -90도 회전하면 camera frame을 구할 수 있으므로,
(counterclockwise 기준, 왼손법칙)

Transformation from body to camera-frame (axis 기준)는 다음과 같이 구할 수 있다.

\[T_c^b = \begin{pmatrix} 0 & -1 & 0 & 1 \\ 0 & 0 & -1 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{pmatrix}\]

따라서 원래 구하려던 Transformation from camera to body-frame (axis 기준)를 구하려면 이를 inverse해주면 된다.

\[T_b^c= \begin{pmatrix} 0 & 0 & 1 & 0 \\ -1 & 0 & 0 & 1 \\ 0 & -1 & 0 & 0 \\ 0 & 0 & 0 & 1 \end{pmatrix}\]

※ 계산해주는 tools site : https://www.andre-gaschler.com/rotationconverter/

transformation matrix를 구할 때 위 site를 이용하여 Rotation의 각도 차이를 적어주어 구하면 편리하다.

위 사이트에서 예시를 적용하기 위해서는 Euler angles of multiple axis rotations (radians)의 ZYX순서로 rotation을 계산하여야 한다.