在开发的过程中,通常会遇到坐标转换的问题,比如从传感器坐标系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进行转换,也可以通过数学坐标变换关系完成。下一节会对这两种方式作介绍。