SLMA-雷达点如何转变为range image图像以及range image图像和球坐标系的关系；IROS 2020 REMOVERT动态SLAM论文解析

文章目录

雷达点如何转变到range image图像
球体坐标关联

雷达点如何转变到range image图像

使用激光雷达采样得到的点一般包含x y z 坐标以及intensity、time、ring属性。参考如下：

namespace velodyne_ros {struct EIGEN_ALIGN16 Point {PCL_ADD_POINT4D;float intensity;float time;uint16_t ring;EIGEN_MAKE_ALIGNED_OPERATOR_NEW};
}POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point,(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(float, time, time)(uint16_t, ring, ring)
)void cloudProcessing::velodyneHandler(const sensor_msgs::PointCloud2::ConstPtr &msg, double &dt_offset)
{pcl::PointCloud<velodyne_ros::Point> raw_cloud;pcl::fromROSMsg(*msg, raw_cloud);
}

通过上述操作可以将激光雷达采集到的点在雷达回调函数中从ros的msg格式转化为自定义格式。获取到自定义雷达数据后，一般需要将一帧雷达数据中的所有点转换到range image中。
range image的横坐标是雷达的垂直分辨率，纵坐标是雷达的水平分辨率。此外每个图像中的雷达点需要包含一个强度信息。这三个属性在下面会介绍如何和球体坐标进行关联。
一个常见的将每一帧雷达点编码为range image伪代码如下所示：

          int row_id, col_id;double vertical_angle, horizon_angle, yaw_angle, range;if (given_ring){row_id = raw_cloud.points[i].ring;assert(row_id >= 0 && row_id < N_SCANS);}else{vertical_angle = atan2(point_temp.raw_point.z(), sqrt(point_temp.raw_point.x() * point_temp.raw_point.x() + point_temp.raw_point.y() * point_temp.raw_point.y())) * 180 / M_PI;row_id = (vertical_angle + ang_bottom) / ang_res_y;if (row_id < 0 || row_id >= N_SCANS) continue;}yaw_angle = atan2(point_temp.raw_point.y(), point_temp.raw_point.x()) * 57.2957;if (is_first[row_id]){horizon_angle = atan2(point_temp.raw_point.x(), point_temp.raw_point.y()) * 180 / M_PI;col_id = -round((horizon_angle - 90.0)/ang_res_x) + Horizon_SCANS/2;if (col_id >= Horizon_SCANS)col_id -= Horizon_SCANS;if (col_id < 0 || col_id >= Horizon_SCANS) continue;range = sqrt(point_temp.raw_point.x() * point_temp.raw_point.x() + point_temp.raw_point.y() * point_temp.raw_point.y() + point_temp.raw_point.z() * point_temp.raw_point.z());if (range < blind) continue;point_temp.range = range;yaw_first_point[row_id] = yaw_angle;is_first[row_id] = false;if (given_offset_time){point_temp.timestamp = point_temp.relative_time / double(1000) + msg->header.stamp.toSec();point_temp.alpha_time = point_temp.relative_time / dt_last_point;}elsepoint_temp.relative_time = 0.0;range_mat.at<float>(row_id, col_id) = range;num_full_points++;full_cloud[col_id  + row_id * Horizon_SCANS] = point_temp;continue;}

上面的二维图像表示并没有真正使用二维图像，而是将二维图像一维划，这个也是雷达点云处理非常普遍的处理方式。