欧拉角转旋转矩阵

在和机器人相关或者3D图像相关的领域经常会碰到欧拉角到旋转矩阵的变换。基本的原理本文不做介绍，仅仅提供两种实现方法

方法一：a,b,c为绕着xyz三个轴转动的角度，代码中的x,y,z为平移量，你可以全部置为0.程序中会先转换为弧度，最后计算出4x4的变换矩阵

float a=20,b=30,c=40;
double angle2radian=M_PI/180.0;
double sin_a = sin(a*angle2radian),sin_b=sin(b*angle2radian),sin_c=sin(c*angle2radian);
double cos_a= cos(a*angle2radian),cos_b = cos(b*angle2radian),cos_c = cos(c*angle2radian);

// the 4x4 transform mat T_l2h(transform mat from local tool coord to home tool coord)
double T_l2h[4][4] = 
{{cos_b*cos_c,    cos_c*sin_a*sin_b-cos_a*sin_c,  sin_a*sin_c+ cos_a*cos_c*sin_b, x},
 { cos_b*sin_c,    cos_a*cos_c+sin_a*sin_b*sin_c,  cos_a*sin_b*sin_c-cos_c*sin_a, y}, 
 { -sin_b,         cos_b*sin_a,                    cos_a*cos_b,                   z},
 {0,              0,                              0,                              1}};

方法二：eigen实现更为简单(下面的代码将旋转和平移统一做)

void xyzabc2Mat4f(const Eigen::Vector3f& abc,const Eigen::Vector3f& xyz,Eigen::Matrix4f& transform_mat){
    float angle2radian=M_PI/180.0;
    Eigen::AngleAxisf r_x ( abc[0]*angle2radian, Eigen::Vector3f::UnitX() );
    Eigen::AngleAxisf r_y ( abc[1]*angle2radian, Eigen::Vector3f::UnitY() );
    Eigen::AngleAxisf r_z ( abc[2]*angle2radian, Eigen::Vector3f::UnitZ() );

    Eigen::Matrix3f rot_mat;
    rot_mat = r_z*r_y*r_x;
    transform_mat.block(0,0,3,3) = rot_mat;
    transform_mat(0,3) = xyz[0];
    transform_mat(1,3) = xyz[1];
    transform_mat(2,3) = xyz[2];
    return;
}