前言
- 一般我们把采集的原始数据放在 rosbag 中。我们要可视化需要对 bag 包解析
- 二维图像可视化点云思路：在二维图片显示投影的点云就行了
一、环境配置
- 我用的 python 3.7
- pip install --extra-index-url https://rospypi.github.io/simple/ rosbag ---- 安装 rosbag 库
- pip install sensor_msgs --extra-index-url https://rospypi.github.io/simple/ ---- 安装 sensor_msgs 库 网络不稳定有可能中断 多安装几次就好了
二、代码
整体代码很简单，直接上代码。
代码语言：javascript复制import cv2
import numpy as np
import rosbag
import sensor_msgs.point_cloud2 as pc2
def pointsToBev(points, imgW=540, imgH=1080, minY=-16, maxY=16, minX=-20, maxX=34, show=False):
img = np.zeros((imgH, imgW, 3), np.uint8)
ratioW = imgW / (maxY - minY)
ratioH = imgH / (maxX - minX)
for i, pt in enumerate(points):
x, y, *other = pt
v = imgH - (x - minX) * ratioH
u = imgW - (y - minY) * ratioW
u, v = int(u), int(v)
if (u >= 0 and u < imgW) and (v >= 0 and v < imgH):
img[v, u] = [255, 255, 255] # 点云显示白色
if show:
cv2.imwrite("test.jpg", img)
cv2.imshow("bev", img)
cv2.waitKey(0)
return img
def get_iter(path, topic):
bag_data = rosbag.Bag(path, "r")
iter_points_data = bag_data.read_messages(topics=topic)
return iter_points_data
if __name__ == '__main__':
bag_path = "test.bag" # bag包路径
topic_name = "/lidar_points" # 话题名
points_iter = get_iter(bag_path, topic_name) # 生成器 可迭代对象
while True:
try:
topic, msg, t = next(points_iter) # 每次取一帧，节省内存空间
pcd = pc2.read_points(msg)
pcd = np.array(list(pcd))
pcd = list(np.delete(pcd, np.where(np.isnan(pcd))[0], axis=0))
data = np.array(pcd)
pointsToBev(data, show=True)
except StopIteration:
# 遇到 StopIteration 停止迭代
print("over")
break三、效果图
