数据预处理（17）

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