EIGEN,TF等，方便自己CV大法

1.利用tf函数从四元数求解航向角（eigen里的出错，原因不知道）

tf::Quaternion q(x,y,z,w);

double yaw = tf::getYaw(q);

2.已知角度和位移，求变换矩阵Eigen::Matrix4f

Eigen::AngleAxisf rotation_x(roll, Eigen::Vector3f::UnitX());

Eigen::AngleAxisf rotation_y(pitch, Eigen::Vector3f::UnitY());

Eigen::AngleAxisf rotation_z(yaw, Eigen::Vector3f::UnitZ());

Eigen::Translation3f translation(x,y,z);

Eigen::Matrix4f transform = (translation * rotation_y * rotation_z * rotation_x).matrix();

3.tf::StampedTransform转Eigen::Matrix4f

tf::StampedTransform tf_transform;

Eigen::Matrix4f eigen_transform;

pcl_ros::transformAsMatrix(tf_transform, eigen_transform);