Eigen中旋转矩阵和欧拉角的转换

Eigen中旋转矩阵和欧拉角的转换

在计算机视觉研究中常常用到物体的姿态转换，一般在计算中常用旋转矩阵和四元数，然而二者在表示物体位姿时并不直观，通常采用欧拉角去表示物体的姿态。所幸，Eigen中已经给出了现成的欧拉角与旋转矩阵的转换函数。

Eigen中欧拉角转换旋转矩阵

在计算中常常遇到已知两坐标系三个轴的角度转换关系，求解二者之间的旋转矩阵。下面举例来进行说明，已知坐标系A和坐标系B，其中B是由A先通过Z轴旋转$\gamma$度，再在旋转后坐标的基础上沿Y轴旋转$\beta$度，在此基础上再沿X轴旋转$\alpha$度，则两坐标系之间的旋转矩阵$R_{AB}$可由下式求得：
$$R_{AB}=R_z(\gamma )\ast R_y(\beta )\ast R_x(\alpha )$$
上式用Eigen代码表示如下：

int main(int argc, char **argv)
{
Eigen::Vector3d rpy_raw, ypr;
    rpy_raw << 30, 60, 150;
    rpy_raw = rpy_raw * M_PI / 180;
 
    Eigen::Matrix3d R_AB;
 
    //输入部分为YPR,所以转换的输出也是YPR
    R_AB = Eigen::AngleAxisd(rpy_raw[2], Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(rpy_raw[1], Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(rpy_raw[0], Eigen::Vector3d::UnitX());
    ypr = R_AB.eulerAngles(2, 1, 0);
 
    cout << ypr.transpose() * 180 / M_PI << endl;
 
    return 0;
}