數據預處理（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進行轉換，也可以通過數學座標變換關係完成。下一節會對這兩種方式作介紹。