数据预处理（17）_坐标转换，tf::StampedTransform =」 Eigen::Matrix4f

在开发的过程中，通常会遇到坐标转换的问题，比如从传感器坐标系src，到车体坐标系ref。通常可以分为3步：
第一步：监听TF

bool getTF(const ros::Time&query_time,const std::string& ref,const std::string& src,tf::StampedTransform& transform)
{
	    try 
	    {
	        tf::TransformListener* tf_=new tf::TransformListener();
	       	tf_->waitForTransform(ref,src,query_time,ros::Duration(0.01));
	        tf_->.lookupTransform(ref,src,query_time,transform);
	        return true;
    	} 
    	catch (tf::TransformException &ex) 
    	{
        	return false;
    	}
}

第二步：tf::StampedTransform => Eigen::Matrix4f 转换

bool TFListener::TransformToMatrix(const tf::StampedTransform& transform, Eigen::Matrix4f& transform_matrix) 
{
    Eigen::Translation3f tl_btol(
    transform.getOrigin().getX(), 
    transform.getOrigin().getY(), 
    transform.getOrigin().getZ());
    double roll, pitch, yaw;
    tf::Matrix3x3(transform.getRotation()).getEulerYPR(yaw, pitch, roll);
    Eigen::AngleAxisf rot_x_btol(roll, Eigen::Vector3f::UnitX());
    Eigen::AngleAxisf rot_y_btol(pitch, Eigen::Vector3f::UnitY());
    Eigen::AngleAxisf rot_z_btol(yaw, Eigen::Vector3f::UnitZ());
    transform_matrix = (tl_btol * rot_z_btol * rot_y_btol * rot_x_btol).matrix();
    return true;
}

第三步：坐标变换，以点云为例。
将source_cloud经过transform_matrix转换矩阵得到新的点云filtered_pc

pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_pc(new pcl::PointCloud<pcl::PointXYZI>）
pcl::tranformPointCloud(*source_cloud,*filtered_pc,transform_matrix);

当然这里只是其中一种针对点云批量转换的写法，如果只是想转换传感器坐标系下的一个点（比如传感器坐标系下的一个障碍物），可以直接通过transform进行转换，也可以通过数学坐标变换关系完成。下一节会对这两种方式作介绍。