Coordinate System간의 Transform(Euler, DCM, Quaternions)

LiDAR를 통해 Object Detection을 하게 되면, 결과 값은 LiDAR의 Body Frame 기준의 값으로 얻어집니다. 이 값을 UTM이라는 Local Frame의 값으로 변환하기 위해서는 Coordinate Frame(= Coordinate System) 간의 변환 관계를 계산해야 합니다. 따라서 Transform에 대해 설명드리고, 어떻게 좌표계간 데이터를 변환할 수 있는지 설명드리겠습니다. (Object Detection 관점이 아니더라도 Transform에 대해 이해할 수 있도록 글을 작성하였습니다.)

 

먼저 Coordinate System에 대한 이해가 필요합니다. 이전 포스팅을 먼저 확인해주시길 바랍니다. (해당 포스팅에서 Frame과 Coordinate System을 혼용해서 사용하겠습니다.)

2022.07.03 - [Perception Engineering/Object Detection] - Object Detection의 Coordinate System(WGS84, ENU, UTM, RFU) 정리

Object Detection의 Coordinate System(WGS84, ENU, UTM, RFU) 정리

 

Transform

아래 그림에서 Transform에 관한 몇 가지 insight를 얻을 수 있습니다. 빨간 화살표와 파란 화살표는 각 각 Frame 간의 관계인 Transform을 나타냅니다. Frame 간 관계는 3차원 공간 상에서 Translation(평행 이동), Rotation(회전 이동)으로 나타낼 수 있습니다. U Frame에서 D Frame까지의 Transform은 빨간 화살표를 따라 계산할 수도 있고 파란 화살표를 따라 계산할 수도 있습니다. 

Frame간 Transform (translation + rotation)

 

3차원 회전 : Euler, DCM, Quaternions

Euler Angle에 해당하는 Roll, Pitch, Yaw로 3차원 회전을 표현하는 것은 사람이 이해하기에 직관적이지만 짐벌락 문제가 있습니다. 따라서 짐벌락 문제가 없는 Quaternions 표현으로 변환하여 Transform의 Rotation 값으로 사용합니다. Rotation 값을 이용해서 좌표변환을 하기 위해서는 DCM(Direct Cosine Matrix) 표현식으로 변환해야 합니다. 즉, 코드 상에서 Transform을 이용할 때, 3 by 3 Matrix로 표현해야 Point 좌표(x, y, z)와 행렬 곱셈을 할 수 있습니다. Quaternions와 DCM은 직관적으로 이해가 되지 않기 때문에 Euler Angle로 변환하여 이해합니다.

https://www.andre-gaschler.com/rotationconverter/

3D Rotation Converter

해당 사이트에서 표현식 간의 변환을 아래와 같이 확인해 볼 수 있습니다.

Rotation 표현식간 변환

Euler Angle 관련하여 2가지를 설정해야 합니다. 먼저 Radians로 표현할지, Degrees로 표현할지 결정해야 하고 다음으로 XYZ 순서를 결정해야 합니다. Roll, Pitch, Yaw를 적용하는 순서에 따라 결과가 달라지기 때문입니다. (Rotation Matrix가 달라지는 것을 확인해볼 수 있습니다.) 

위 사진에서 Input에 이해가 되지 않는 Quaternion 값을 입력했을 때, Output의 Euler Angle을 보면 대략  Yaw 방향으로 90도 회전하는 변환임을 알 수 있습니다. 

 

Transform과 좌표 변환

A Frame을 B Frame으로 바꾸기 위한 translation, rotation을 구하는 것은 A to B Transform을 구하는 것을 의미합니다. B Frame에서 본 좌표를 A Frame에서 봤을 때, 어떤 값 일지 구해야 하는 경우 아래와 같이 계산할 수 있습니다. (3가지 식 모두 같은 의미이다.) 

A to B Transform을 알 때, B Frame와 좌표를 A Frame 좌표로 변환

조금 더 쉽게 표현하면 다음과 같이 표현할 수 있습니다.

A Frame의 point = (A to B Transform) * B Frame의 point

만약 A Frame에서 본 좌표를 B Frame에서 본 좌표로 변환하려면 A to B Transform의 Inverse를 구해야 합니다. 위 사진의 첫 번째 식을 보면, A Frame의 Point에 먼저 translation을 빼고 양변의 왼쪽에 rotation의 inverse를 곱해주면 B Frame의 Point가 되는 것을 알 수 있습니다. (참고로 rotation matrix의 inverse는 transpose와 같습니다.) 이를 코드로 구현하면 다음과 같습니다.

//std::cout << "pose matrix" << std::endl; 2번째 식의 3x4 Matrix
//std::cout  << pose(0,0) << " " << pose(0,1) << " " << pose(0,2) << " " << pose(0,3) << std::endl;
//std::cout  << pose(1,0) << " " << pose(1,1) << " " << pose(1,2) << " " << pose(1,3) << std::endl;
//std::cout  << pose(2,0) << " " << pose(2,1) << " " << pose(2,2) << " " << pose(2,3) << std::endl;

void Inverse(float in_x, float in_y, float in_z,\
             float& out_x, float& out_y, float& out_z,\
             Eigen::Affine3d& pose)
{
  out_x= pose(0, 0) * (in_x - pose(0, 3)) + pose(1, 0) * (in_y-pose(1,3) +
         pose(2, 0) * (in_z - pose(2,3)));
  
  out_y= pose(0, 1) * (in_x - pose(0, 3)) + pose(1, 1) * (in_y-pose(1,3)) +
         pose(2, 1) * (in_z - pose(2,3));

  out_z= (pose(0, 2) * (in_x - pose(0, 3)) + pose(1, 2) * (in_y-pose(1,3)) +  
         pose(2, 2) * (in_z - pose(2,3));
}

---