[C++] Quaternion to Euler 상호 변환

Robotics 분야에서 각도 표현에 자주 사용되는 것이 1) Quaternion과 2) Euler이다.

일반적으로 roll, pitch, yaw로 표현되는 Euler 각도에 다들 익숙할 것인데, 짐벌락이라는 한계때문에 각도는 Quaternion으로 계산하곤 한다. 거두절미하고 바로 코드를 보자

 

1. Quaternion to Euler 코드

#include <tf/tf.h>

tf::Quaternion q(
        msg->pose.pose.orientation.x,
        msg->pose.pose.orientation.y,
        msg->pose.pose.orientation.z,
        msg->pose.pose.orientation.w);
tf::Matrix3x3 m(q);
double roll, pitch, yaw;
m.getRPY(roll, pitch, yaw);

반대도 알아야겠죠~~

 

2. Euler to Quaternion 코드

#include <tf2/LinearMath/Quaternion.h>

// input : 오일러 각(roll, pitch, yaw)
tf2::Quaternion quat;
quat.setRPY(roll, pitch, yaw);
quat.normalize();