文章目录
- 前言
- 一、环境配置
- 二、代码
- 三、效果图
前言
- 一般我们把采集的原始数据放在 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 库 网络不稳定有可能中断 多安装几次就好了
二、代码
整体代码很简单,直接上代码。
import cv2
import numpy as np
import rosbag
import sensor_msgs.point_cloud2 as pc2def 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 = ptv = imgH - (x - minX) * ratioHu = imgW - (y - minY) * ratioWu, 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 imgdef get_iter(path, topic):bag_data = rosbag.Bag(path, "r")iter_points_data = bag_data.read_messages(topics=topic)return iter_points_dataif __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