ROS 订阅雷达话题，获取点云数据，可视化，生成鸟瞰图

图像与点云坐标

点云的坐标轴与图像中的坐标轴具有完全不同的含义。下图显示了蓝色的图像轴和橙色的点云轴。

除此之外，对于图像来说：1）图像中的坐标值始终为正值； 2）原点位于左上角； 3）坐标是整数值。而对于点云来说：1）点云中的坐标值可以是正值或负值； 2）坐标可以采用实数值； 3）正x轴代表向前； 4）正y轴代表左边； 5）正z轴代表向上。
鸟瞰视图转换
为了创建鸟瞰视图，需要对点云数据的X轴和Y轴进行转换。

创建点云数据的鸟瞰视图
设定感兴趣区域
图像通常是矩形张量，而点云则是稀疏的点集。创建鸟瞰视图首先得限定一个点云的特定矩形区域，此外，还需要创建一个过滤器将不属于这个区域的点云滤除。
本实验中将感兴趣的矩形设置为距离原点任意一侧10m，前面20m。

side_range=(-10, 10)
fwd_range=(0, 20)

接下来，创建一个过滤器，只保留实际位于指定的矩形内的点。

x_points = points[:, 0]
y_points = points[:, 1]
z_points = points[:, 2]

f_filt = np.logical_and((x_points > fwd_range[0]), (x_points < fwd_range[1]))
s_filt = np.logical_and((y_points > -side_range[1]), (y_points < -side_range[0]))
filter = np.logical_and(f_filt, s_filt)
indices = np.argwhere(filter).flatten()

x_points = x_points[indices]
y_points = y_points[indices]
z_points = z_points[indices]

映射点位置到像素位置
点云数据通常是浮点数，而图像数据通常是整数，首先得将浮点数映射到整数位置，如果直接将所有的x和y值转换为整数，可能会失去很多分辨率。如果这些点的度量单位是米，则每个像素代表点云中1x1米的矩形，并且会丢失比这更小的一些细节。如果是一个类似山景的点云，这可能没问题。但是，如果要识别人类，汽车或甚至更小的东西，那么这种方法就不是很好。所以本实验在将类型转换为整数之前先对数据进行缩放。这里得到5厘米的分辨率，同时将x轴和y轴交换并且方向反转：

res = 0.05
x_img = (-y_points / res).astype(np.int32)
y_img = (-x_points / res).astype(np.int32)

移动到新的原点
目前，原点还是原来点云的原点，还会存在负值，所以我们需要移动数据使（0,0）成为最小值。

x_img -= int(np.floor(side_range[0] / res))
y_img += int(np.ceil(fwd_range[1] / res))

像素值填充
之前使用点数据来指定图像中的x和y位置。现在需要做的是指定这些像素位置填充的值。一种可能性是用高度数据填充它。需要注意的两件事是：
•像素值应该是整数。
•像素值应该在0-255范围内。
像素值可以从数据中获取最小和最大高度值，并重新调整该范围到0-255的范围。另一种方法，这里将使用的一种方法是设置想要关注的高度值，并且高于或低于该范围的任何数值都会被剪切到最小值和最大值。

height_range = (-2, 0.5) 
pixel_values = np.clip(a = z_points,
                           a_min=height_range[0],
                           a_max=height_range[1])

def scale_to_255(a, min, max, dtype=np.uint8):
    return (((a - min) / float(max - min)) * 255).astype(dtype)

pixel_values = scale_to_255(pixel_values, min=height_range[0], max=height_range[1])

创建图像并查看
首先初始化一个数组，然后，使用以转换为像素位置的x和y点值来指定数组中的索引，并将这些索引分配像素值。

x_max = 1+int((side_range[1] - side_range[0])/res)
y_max = 1+int((fwd_range[1] - fwd_range[0])/res)
im = np.zeros([y_max, x_max], dtype=np.uint8)

im[y_img, x_img] = pixel_values

图像被存储为一个numpy数组。可以将其转换为PIL图像并查看它。

from PIL import Image
im2 = Image.fromarray(im)
im2.show()

结果如图所示：

除了PIL外，还可以使用matplotlib来查看图像。

import matplotlib.pyplot as plt
plt.imshow(im, cmap="spectral", vmin=0, vmax=255)
plt.show()

结果如图所示：

以下为代码

#!/usr/bin/python
# -*- coding: utf-8 -*-

import numpy as np
import rospy
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2
import scipy.misc

global saved
saved = 0

def scale_to_255(a, min, max, dtype=np.uint8):
    return (((a - min) / float(max - min)) * 255).astype(dtype)

def point_cloud_2_birdseye(points,
                           res=0.1,
                           side_range=(-50, 50),
                           fwd_range = (-50, 50),
                           height_range=(-2., 2.),
                           ):
    x_points = points[:, 0]
    y_points = points[:, 1]
    z_points = points[:, 2]
    f_filt = np.logical_and((x_points > fwd_range[0]), (x_points < fwd_range[1]))
    s_filt = np.logical_and((y_points > -side_range[1]), (y_points < -side_range[0]))
    filter = np.logical_and(f_filt, s_filt)
    indices = np.argwhere(filter).flatten()

    x_points = x_points[indices]
    y_points = y_points[indices]
    z_points = z_points[indices]

    x_img = (-y_points / res).astype(np.int32)
    y_img = (-x_points / res).astype(np.int32)

    x_img -= int(np.floor(side_range[0] / res))
    y_img += int(np.ceil(fwd_range[1] / res))

    pixel_values = np.clip(a=z_points,
                           a_min=height_range[0],
                           a_max=height_range[1])

    pixel_values = scale_to_255(pixel_values,
                                    min=height_range[0],
                                    max=height_range[1])

    x_max = 1 + int((side_range[1] - side_range[0]) / res)
    y_max = 1 + int((fwd_range[1] - fwd_range[0]) / res)
    im = np.zeros([y_max, x_max], dtype=np.uint8)

    im[y_img, x_img] = pixel_values

    return im

def callback(lidar):
    global saved
    if saved == 0:
        lidar = pc2.read_points(lidar)
        points = np.array(list(lidar))
        im = point_cloud_2_birdseye(points)
        scipy.misc.imsave('/home/richard/projects/lidar_ws_python/src/cloud_subscribe/save/lidar.png', im)
        saved = 1

def cloud_subscribe():
    rospy.init_node('cloud_subscribe_node')
    rospy.Subscriber("/lslidar_point_cloud", PointCloud2, callback)
    rospy.spin()

if __name__ == '__main__':
    cloud_subscribe()

这是简化后版本

import numpy as np
import rospy
from sensor_msgs.msg import PointCloud2
import sensor_msgs.point_cloud2 as pc2
import scipy.misc
import os
def point_cloud_2_birdseye(points):
    x_points = points[:, 0]
    y_points = points[:, 1]
    z_points = points[:, 2]
    
    f_filt = np.logical_and((x_points > -50), (x_points < 50))
    s_filt = np.logical_and((y_points > -50), (y_points < 50))
    filter = np.logical_and(f_filt, s_filt)
    indices = np.argwhere(filter)
    
    x_points = x_points[indices]
    y_points = y_points[indices]
    z_points = z_points[indices]

    x_img = (-y_points*10).astype(np.int32)+500
    y_img = (-x_points *10).astype(np.int32)+500      

    pixel_values = np.clip(z_points,-2,2)
    pixel_values = ((pixel_values +2) / 4.0) * 255

    im = np.zeros([1001, 1001], dtype=np.uint8)
    im[y_img, x_img] = pixel_values
    return im

def callback(lidar):
    lidar = pc2.read_points(lidar)
    points = np.array(list(lidar))
    im = point_cloud_2_birdseye(points)
    scipy.misc.imsave('./lidar.png', im)
    os._exit(0)       

def cloud_subscribe():
    rospy.init_node('cloud_subscribe_node')
    rospy.Subscriber("/velodyne_points", PointCloud2, callback)
    rospy.spin()
cloud_subscribe()

文章出自人工智能制造赛项的工程师（侵删）

16线激光雷达数据包

链接：https://pan.baidu.com/s/15_a6PKv5vbQPFiME6rVv8A
提取码：a2cw

我的其他有关ros 的博客,不妨一看
Ubuntu 安装 ros linux 验证安装成功
http://hotdog29.com/index.php/2019/06/22/ubuntu-安装-ros-kinetic-官方教程-下载-linux/
ROS 订阅雷达话题，获取点云数据，可视化，生成鸟瞰图
http://hotdog29.com/index.php/2019/06/22/ros-订阅雷达话题，获取点云数据，可视化，生成鸟/
ROS订阅雷达话题　获取坐标
http://hotdog29.com/index.php/2019/06/22/ros订阅雷达话题　获取坐标-bag数据包/
快速安装依赖包
https://blog.csdn.net/weixin_41600500/article/details/88432283
.bag 包转化成.pcd文件
https://blog.csdn.net/weixin_41600500/article/details/88432257
ros 播放激光雷达数据包，rviz可视化
http://hotdog29.com/index.php/2019/06/22/ros-播放激光雷达数据包，rviz可视化/
ROS 基础操作
https://blog.csdn.net/weixin_41600500/article/details/86213794
ros 基础操作２
https://blog.csdn.net/weixin_41600500/article/details/88412954