【解析rosbag可视化】二维图像可视化点云

读书猿

发布于 2024-02-05 15:25:53

1130

发布于 2024-02-05 15:25:53

文章被收录于专栏：无人驾驶感知无人驾驶感知

前言

一般我们把采集的原始数据放在 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 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