euler 와 quarternion 의 관계

 0. 참고자료

- 블로그 형

위 블로그만 읽어도 왠만한건 정리가 된다.

- atan2 함수

 배경지식으로 atan2 를 먼저 소개한다. 

 tan 의 역함수인 arc tangent 는 삼각형을 이루는 변의 비율이다.

 위의 그림처럼 말이다. 

 그런데 b/a 랑 (-b)/(-a) 랑 값은 같은데 결과값은 부호가 다른걸 어떻게 구분하냐?

 그래서 인자를 두개 나눠서 넣어 부호를 구분할 수 있게 하는 함수가 atan2 다.

1. Quaternion 과 Matrix 의 표현관계

$p\ =\ \left(x,\ y,\ z,\ 0\right)$p = (x, y, z, 0)​

$q\ =\ \left(\sin \left(\frac{\theta }{2}\right)u,\ \cos \left(\frac{\theta }{2}\right)\right)=\ (\sin \left(\frac{\theta }{2}\right)\combi{\cdot u}_x,\ \sin \left(\frac{\theta }{2}\right)\cdot \combi{u}_y,\ \sin \left(\frac{\theta }{2}\right)\cdot \combi{u}_z,\ \cos \left(\frac{\theta }{2}\right))$q = (sin(θ2​)u, cos(θ2​))= (sin(θ2​)·ux​, sin(θ2​)·uy​, sin(θ2​)·uz​, cos(θ2​))​

위는 우리가 사용할 사원수이다. 

p 는 우리가 회전시킬 좌표고, q 는 회전할 각도에 따른 quaternion 이다.

먼저 회전이 아니라, 단순히 Quaternion q 와 정검 p 의 곱, pq을 살펴보자

(

qw​	qz​	−qy​	qx​
−qz​	qw​	qx​	qy​
qy​	−qx​	qw​	qz​
−qx​	−qy​	−qz​	qw​

)(

px​

py​

pz​

pw​

)​

$\left(\begin{matrix}p_w&-p_z&p_y&p_x\\p_z&p_w&-p_x&p_y\\-p_y&p_x&p_w&p_z\\-p_x&-p_y&-p_z&p_w\end{matrix}\right)\left(\begin{matrix}q_x\\q_y\\q_z\\q_w\end{matrix}\right)$(

pw​	−pz​	py​	px​
pz​	pw​	−px​	py​
−py​	px​	pw​	pz​
−px​	−py​	−pz​	pw​

)(

qx​

qy​

qz​

qw​

)

그렇다. pq는 위처럼 행렬의 곱으로 나타낼 수 있다.

그리고 우리가 회전을 계산하려면 qpq* 를 계산하면 된다.

이때 q* 는 w 값 외의 값의 부호를 반전시킨 값이다.

그런데 qpq* 는

(qp)(q*)    -> 결합법칙이 통해서 그냥 괄호친거

M(qp)      ->  qp 도 quaternion 이고, q* 도 quaternion 이니까 q* 를 행렬로 뺌

MNp       => q 는 quaternion 이니까 행렬로 뺌.

한번의 회전은 위처럼 행렬로 뺄 수 있다. 그리고 다시 회전의 연결도 그렇다.

r(qpq*)r*    ->  위의 qpq* 에 r 로 회전시킴

(rq)p(rq)*   ->  quaternion 은 결합법칙이 통하고 *로 묶으면 순서 바뀜

Qp           -> 위에서 봤듯이 quaternion 이 양쪽에 붙으면 하나의 행렬로 표현 가능

즉 quaternion 끼리 먼저 계산 끝내고 정점 계산에 들어가도 된다는 결론이다.

2. Euler 와 Yaw Pitch Roll

쿼터니온은 축 중심으로 회전하는 Euler 를 쓸 경우의 문제점을 해소하기 위해 만들어졌다.

예를들어 보간이 Euler 의 경우는 어색하게 되는데 Quaternion 은 부드럽게 된다.

이런 Euler 는 회전하는 축의 순서에 따라서 결과값이 달라지게 된다.

상식적으로 머리속에서 회전시켜봐도 달라지는건 명백한데,

Euler 시뮬레이터 에서 XYZ order 부분을 바꾸면서 돌려보면서 확인할 수 있다.

이때 사용하는 단위가 Yaw, Pitch, Roll 이다. 

비행기 조종 관련해서 용어가 많이 사용되는데, Yaw(Z), Pitch(Y), Roll(X) 축 중심 회전.

이렇게 외우는게 편하다. 

비행기 관련 비유에서는 비행기가 앞을 보는게 x 축의 양의 방향으로,

Pitch(끄덕끄덕), Row(갸우뚱), Yaw(도리도리) 이렇게 말하기도 하는데 걍 축 외우는게 낫다.

3. Euler to Quaternion

$rotate\left(Yaw\right)rotate\left(Pitch\right)rotate\left(Roll\right)\ order\ \left(z->y->x\right)$rotate(Yaw)rotate(Pitch)rotate(Roll) order (z−>y−>x)​

$column\ Major\ caculation$column Major caculation​

$\begin{bmatrix}0\\0\\\cos \left(Roll\right)\\\sin \left(Roll\right)\end{bmatrix}\begin{bmatrix}0\\\cos \left(Pitch\right)\\0\\\sin \left(Pitch\right)\end{bmatrix}\begin{bmatrix}\cos \left(Yaw\right)\\0\\0\\\sin \left(Yaw\right)\end{bmatrix}$[

0

0

sin(Yaw)

cos(Yaw)

][

0

sin(Pitch)

0

cos(Pitch)

][

sin(Roll)

0

0

cos(Roll)

]​

$=\begin{bmatrix}\sin \left(Roll\right)\cdot \cos \left(Pitch\right)\cdot \cos \left(Yaw\right)-\cos \left(Roll\right)\cdot \sin \left(Pitch\right)\cdot \sin \left(Yaw\right)\\\cos \left(Roll\right)\cdot \sin \left(Pitch\right)\cdot \cos \left(Yaw\right)+\sin \left(Roll\right)\cdot \cos \left(Pitch\right)\cdot \sin \left(Yaw\right)\\\cos \left(Roll\right)\cdot \cos \left(Pitch\right)\cdot \sin \left(Yaw\right)-\sin \left(Roll\right)\cdot \sin \left(Pitch\right)\cdot \cos \left(Yaw\right)\\\cos \left(Roll\right)\cdot \cos \left(Pitch\right)\cdot \cos \left(Yaw\right)+\cos \left(Roll\right)\cdot \cos \left(Pitch\right)\cdot \cos \left(Yaw\right)\end{bmatrix}$=[

sin(Roll)·cos(Pitch)·cos(Yaw)−cos(Roll)·sin(Pitch)·sin(Yaw)

cos(Roll)·sin(Pitch)·cos(Yaw)+sin(Roll)·cos(Pitch)·sin(Yaw)

cos(Roll)·cos(Pitch)·sin(Yaw)−sin(Roll)·sin(Pitch)·cos(Yaw)

cos(Roll)·cos(Pitch)·cos(Yaw)+sin(Roll)·sin(Pitch)·sin(Yaw)

]​

주의사항은 위에서도 언급한 Yaw, Pitch, Roll 을 곱한 순서랑 Column Major 이다.

곱한 순서가 z->y->x 축이고 column major 이면 위처럼 표기가 된다.

곱한 순서와 행/열우선을 꼭 확인하자!

Euler 를 Quaternion 으로 바꾸는건 위의 결과값 그대로 쓰면 된다.

4. Quaternion to Euler

$rotate\left(Yaw\right)rotate\left(Pitch\right)rotate\left(Roll\right)\ order\ \left(z->y->x\right)$rotate(Yaw)rotate(Pitch)rotate(Roll) order (z−>y−>x)​

$column\ Major\ caculation$column Major caculation​

$\begin{bmatrix}x"\\y"\\z"\end{bmatrix}=R\left(Yaw\right)R\left(Pitch\right)R\left(Roll\right)\begin{bmatrix}x\\y\\z\end{bmatrix}$[

x′

y′

z′

]=R(Yaw)R(Pitch)R(Roll)[

x

y

z

]​

$=\begin{bmatrix}\cos \left(Y\right)&-\sin \left(Y\right)&0\\\sin \left(Y\right)&\cos \left(Y\right)&0\\0&0&1\end{bmatrix}\begin{bmatrix}\cos \left(P\right)&0&\sin \left(P\right)\\0&1&0\\-\sin \left(P\right)&0&\cos \left(P\right)\end{bmatrix}\begin{bmatrix}1&0&0\\0&\cos \left(R\right)&-\sin \left(R\right)\\0&\sin \left(R\right)&\cos \left(R\right)\end{bmatrix}\begin{bmatrix}x\\y\\z\end{bmatrix}$=[

cos(Y)	−sin(Y)	0
sin(Y)	cos(Y)	0
0	0	1

][

cos(P)	0	sin(P)
0	1	0
−sin(P)	0	cos(P)

][

1	0	0
0	cos(R)	−sin(R)
0	sin(R)	cos(R)

][

x

y

z

]​

$=\begin{bmatrix}\cos \left(P\right)\cos \left(Y\right)&-\cos \left(R\right)\sin \left(Y\right)+\sin \left(R\right)\sin \left(P\right)\cos \left(Y\right)&\sin \left(R\right)\sin \left(Y\right)+\cos \left(R\right)\sin \left(P\right)\cos \left(Y\right)\\\sin \left(P\right)\cos \left(Y\right)&\cos \left(R\right)\cos \left(Y\right)+\sin \left(R\right)\sin \left(P\right)\sin \left(Y\right)&-\sin \left(R\right)\cos \left(Y\right)+\cos \left(R\right)\sin \left(P\right)\sin \left(Y\right)\\-\sin \left(P\right)&\sin \left(R\right)\cos \left(P\right)&\cos \left(R\right)\cos \left(P\right)\end{bmatrix}$=[

cos(P)cos(Y)	−cos(R)sin(Y)+sin(R)sin(P)cos(Y)	sin(R)sin(Y)+cos(R)sin(P)cos(Y)
sin(P)cos(Y)	cos(R)cos(Y)+sin(R)sin(P)sin(Y)	−sin(R)cos(Y)+cos(R)sin(P)sin(Y)
−sin(P)	sin(R)cos(P)	cos(R)cos(P)

]​

$R=\begin{bmatrix}\combi{\combi{q}_x}^2-\combi{\combi{q}_y}^2-\combi{\combi{q}_z}^2+\combi{\combi{q}_w}^2&2\left(\combi{q}_1\combi{q}_2-\combi{q}_0\combi{q}_3\right)&2\left(\combi{q}_x\combi{q}_z+\combi{q}_y\combi{q}_w\right)\\2\left(\combi{q}_1\combi{q}_2+\combi{q}_0\combi{q}_3\right)&\combi{\combi{-q}_x}^2+\combi{\combi{q}_y}^2-\combi{\combi{q}_z}^2+\combi{\combi{q}_w}^2&2\left(\combi{q}_x\combi{q}_w-\combi{q}_y\combi{q}_z\right)\\2\left(\combi{q}_x\combi{q}_z-\combi{q}_y\combi{q}_w\right)&2\left(\combi{q}_x\combi{q}_w+\combi{q}_y\combi{q}_z\right)&\combi{\combi{-q}_x}^2-\combi{\combi{q}_y}^2+\combi{\combi{q}_z}^2+\combi{\combi{q}_w}^2\end{bmatrix}$R=[

qx​2−qy​2−qz​2+qw​2	2(qx​qy​−qz​qw​)	2(qx​qz​+qy​qw​)
2(qz​qw​+qx​qy​)	−qx​2+qy​2−qz​2+qw​2	2(qx​qw​−qy​qz​)
2(qx​qz​−qy​qw​)	2(qx​qw​+qy​qz​)	−qx​2−qy​2+qz​2+qw​2

]​

위는 Y, P, R (Yaw, Pitch, Roll) 순서로 Euler 회전을 시킨 결과이고,

아래는 그냥 quaternion 회전을 matrix 로 만든 결과이다.

Pitch 는 _31 간의 비교로 바로 얻을 수 있다.

Pitch = arc sin( 2 * ( qyqw - qxqz))  

Roll 은 _32 에 _33 를 나누면 sin/cos = tan 을 이용해 얻을 수 있다.

tan(Roll) = 2(qxqw + qyqz) / ( -qx2 -qy2 + qz2 + qw2)

그런데 quaternion 은 xyzw 값들의 제곱의 합이 1이다. 

따라서 qz2 + qw2 = 1 - qx2 - qy2 이므로

tan(Roll) = 2(qxqw + qyqz) / (1 - 2(qx2 + qy2)) 이다.

따라서 arcTan2(2(qxqw + qyqz), 1 - 2(qx * qx + qy * qy)) 이다.

Yaw 은 _11 에 _21 를 나누면 sin/cos = tan 을 이용해 얻을 수 있다.

tan(Yaw) = 2(qxqy + qzqw) / ( qx2 + qw2 - qy2 - qz2)

그런데 quaternion 은 xyzw 값들의 제곱의 합이 1이다. 

따라서 qx2 + qw2 = 1 - qy2 - qz2 이므로

tan(Roll) = 2(qxqy + qzqw) / (1 - 2(qy2 + qz2)) 이다.

따라서 arcTan2(2(qxqy + qzqw), 1 - 2(qy * qy + qz * qz)) 이다.

그런데 여기에 주의사항이 있다.

축 순서를 바꾸면 -tan 이런 꼴로 나오는데 분모가 음수냐 분자가 음수냐 라는,

대단한 문제에 우리는 마주치게 되고. 

잘못하면 계속 위아래나 좌우가 반전되는 사태에 도달한다.

그럴 땐 제곱이 없는 분자에 해주면 되는데, 그 이유는 모르겠다.

이건 몇번 강조해도 문제가 없어보인다.