激光雷达点云数据从laser坐标系转换成全局坐标

激光雷达的位置如下图所示，车体右侧面激光雷达： 静态TF：

<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser4" args="0.0 -0.3 0.7 -1.57 0.0 0.0 /base_link /ydlidar_right_frame 40" />

1.方法一：利用tf进行坐标变换

// trans pos in raser to global Raser_3dPoint get_global_pos(Raser_3dPoint &pos) { std::cout<<"pos in laser"<<" x:"<<pos.x<<" y:"<<pos.z<<" z:"<<pos.y<<std::endl; Raser_3dPoint res; //创建TF监听器 tf::TransformListener listener; geometry_msgs::PointStamped laser_point; laser_point.header.frame_id = "ydlidar_right_frame"; //使用最近可用的转换 laser_point.header.stamp = ros::Time(); laser_point.point.x = pos.x; laser_point.point.y = pos.z; laser_point.point.z = 0.0; geometry_msgs::PointStamped world_point; try { //监听获取tf变换 listener.waitForTransform("odom", "ydlidar_right_frame", ros::Time(0), ros::Duration(3.0)); world_point.header.frame_id = "odom"; listener.transformPoint("odom", laser_point, world_point); } catch(tf::TransformException& ex) { ROS_ERROR("%s", ex.what()); ros::Duration(1.0).sleep(); } res.x = world_point.point.x; res.y = world_point.point.y; res.z = world_point.point.z; std::cout<<"pos in world"<<" x:"<<res.x<<" y:"<<res.y<<" z:"<<res.z<<std::endl; return res; }

得到的变化结果如下：

target pos to laser:x 0.0840537 y 0 z 0.498975 valid points 138 pos in laser x:0.0840537 y:0.498975 z:0 pos in base x:0.499042 y:-0.383656 z:0.7

方法二：直接利用变换矩阵进行坐标变换 首先需要订阅得到当前的小车Pose，即里程计信息，以下是忽略激光和车体之间的变化的情况，默认重合。 如果激光里程计和车体存在旋转和平移，需要先将激光点云数据转换从laser frame转换到base link下。

Eigen::Vector3d robotPose(robotPose_x,robotPose_y,robotPose_theta); //计算得到该激光点的世界坐标系的坐标 double theta = -robotPose(2); // 激光雷达逆时针转，角度取反 //double laser_x = dist * cos(angle); //double laser_y = dist * sin(angle); double laser_x = pos.x; double laser_y = pos.z; //double laser_z = pos.z; double world_x = cos(theta) * laser_x - sin(theta) * laser_y + robotPose(0); double world_y = sin(theta) * laser_x + cos(theta) * laser_y + robotPose(1); res.x = world_x; res.y = world_y; res.z = 0.7; return res;

实际上在使用时，使用TF变化坐标延迟太高，建议自己转换。