kitti数据可视化

数据下载

这里以 2011_09_26_drive_0005 (0.6 GB)数据为参考，下载[synced+rectified data] [calibration] 数据。

下载完毕之后解压，然后将calibration文件解压后的结果放在如下目录下，

下载kitti2bag包

python 复制代码

pip install kitti2bag

如果后期出现如下错误：

ImportError: dynamic module does not define module export function (PyInit__tf2)

则执行如下重新安装

python 复制代码

pip2 install kitti2bag

进入到2011_09_26文件夹的同级目录中，执行如下代码进行转换：

bash 复制代码

kitti2bag -t 2011_09_26 -r 0005 raw_synced

运行截图如下：

运行结束之后，会在当前目录下生成一个bag文件。

启动ROS，并进行播放

bash 复制代码

roscore

进入到刚刚生成bag文件的目录下，执行如下代码：

bash 复制代码

rqt_bag kitti_2011_09_26_drive_0005_synced.bag

生成如下截图：

选择其中一个图像话题，右键，选择view -》选择image，然后点击play进行播放。

发布图片

创建工作空间，并创建功能包

bash 复制代码

~/catkin_ws/src$ catkin_create_pkg kitti_tutorial rospy

编写kitti.py代码

python 复制代码

#!/usr/bin/env python

import cv2
import os

import rospy
from sensor_msgs.msg import Image
from cv_bridge import CvBridge

DATA_PATH = "/home/d501/kitti_data/2011_09_26/2011_09_26_drive_0005_sync/2011_09_26/2011_09_26_drive_0005_sync"

if __name__ == "__main__":
    frame = 0
    rospy.init_node("kitti_node", anonymous=True)
    cam_pub = rospy.Publisher('kitti_cam', Image, queue_size=10)
    bridge = CvBridge()

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        img = cv2.imread(os.path.join(DATA_PATH, 'image_02/data/%010d.png'%frame))
        cam_pub.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))
        rospy.loginfo("camera image published")
        rate.sleep()
        frame += 1
        frame %= 154

添加可执行权限

bash 复制代码

sudo chmod +x kitti.py

编译工作空间

bash 复制代码

~/catkin_ws$ catkin_make
~/catkin_ws$ source devel/setup.bash

在使用pycharm编写python程序时，

问题：在pycharm中使用ros时候，遇到"No module named rospy"

pycharm中无法加载rospy_pycharm no module named rospy-CSDN博客

将解释器更换为opt下的python2.7

启动rviz

bash 复制代码

rviz

启动ros节点

bash 复制代码

~/catkin_ws$ rosrun kitti_tutorial kitti.py

添加topic

如图所示：

发布点云数据

修改kitti.py代码

python 复制代码

#!/usr/bin/env python
import numpy as np

import cv2
import os
import numpy as np
import rospy
from std_msgs.msg import Header
from sensor_msgs.msg import Image, PointCloud2
import sensor_msgs.point_cloud2 as pcl2
from cv_bridge import CvBridge

DATA_PATH = "/home/d501/kitti_data/2011_09_26/2011_09_26_drive_0005_sync/2011_09_26/2011_09_26_drive_0005_sync"

if __name__ == "__main__":
    frame = 0
    rospy.init_node("kitti_node", anonymous=True)
    cam_pub = rospy.Publisher('kitti_cam', Image, queue_size=10)
    pcl_pub = rospy.Publisher('kitti_point_cloud', PointCloud2, queue_size=10)
    bridge = CvBridge()

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        img = cv2.imread(os.path.join(DATA_PATH, 'image_02/data/%010d.png'%frame))
        point_cloud = np.fromfile(os.path.join(DATA_PATH, 'velodyne_points/data/%010d.bin'%frame), dtype=np.float32).reshape(-1, 4)
        cam_pub.publish(bridge.cv2_to_imgmsg(img, 'bgr8'))

        header = Header()
        header.stamp = rospy.Time.now()
        header.frame_id = 'map'
        pcl_pub.publish(pcl2.create_cloud_xyz32(header, point_cloud[:,:3]))
        rospy.loginfo("published")
        rate.sleep()
        frame += 1
        frame %= 154

重新添加topic之后，如下：

调整size大小

将rviz存档，以后开启它就不用重新添加topic

点击File->Save Config As->选择包下的某个路径，保存为xxxx.rviz文件，下次启动时，使用rviz -d xxxx.rviz即可

添加照相机视野

重构代码data_utils.py

python 复制代码

#!/usr/bin/env python

import cv2
import numpy as np

def read_camera(path):
    return cv2.imread(path)

def read_point_cloud(path):
    return np.fromfile(path, dtype=np.float32).reshape(-1, 4)

publish_utils.py

python 复制代码

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Point
from std_msgs.msg import Header
from sensor_msgs.msg import Image, PointCloud2
import sensor_msgs.point_cloud2 as pcl2
from cv_bridge import CvBridge
from visualization_msgs.msg import Marker

FRAME_ID = 'map'

def publish_camera(cam_pub, bridge, image):
    cam_pub.publish(bridge.cv2_to_imgmsg(image, 'bgr8'))

def publish_point_cloud(pcl_pub, point_cloud):
    header = Header()
    header.stamp = rospy.Time.now()
    header.frame_id = FRAME_ID
    pcl_pub.publish(pcl2.create_cloud_xyz32(header, point_cloud[:, :3]))

def publish_ego_car(ego_car_pub):
    """
    publish left and right 45 degree FOV lines and ego car model mesh
    :param ego_car_pub:
    :return:
    """
    marker = Marker()

    marker.header.frame_id = FRAME_ID
    marker.header.stamp = rospy.Time.now()

    marker.id = 0
    marker.action = Marker.ADD
    marker.lifetime = rospy.Duration()
    marker.type = Marker.LINE_STRIP

    marker.color.r = 0.0
    marker.color.g = 1.0
    marker.color.b = 0.0
    marker.color.a = 1.0
    marker.scale.x = 0.2

    marker.points = []
    marker.points.append(Point(10, -10, 0))
    marker.points.append(Point(0, 0, 0))
    marker.points.append(Point(10, 10, 0))

    ego_car_pub.publish(marker)

kitti.py

python 复制代码

#!/usr/bin/env python
import numpy as np

import rospy

from data_utils import *
from publish_utils import *
import os

DATA_PATH = "/home/d501/kitti_data/2011_09_26/2011_09_26_drive_0005_sync/2011_09_26/2011_09_26_drive_0005_sync"

if __name__ == "__main__":
    frame = 0
    rospy.init_node("kitti_node", anonymous=True)
    cam_pub = rospy.Publisher('kitti_cam', Image, queue_size=10)
    pcl_pub = rospy.Publisher('kitti_point_cloud', PointCloud2, queue_size=10)
    ego_pub = rospy.Publisher('kitti_ego_car', Marker, queue_size=10)

    bridge = CvBridge()

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        image = read_camera(os.path.join(DATA_PATH, 'image_02/data/%010d.png'%frame))
        point_cloud = read_point_cloud(os.path.join(DATA_PATH, 'velodyne_points/data/%010d.bin'%frame))

        publish_camera(cam_pub, bridge, image)
        publish_point_cloud(pcl_pub, point_cloud)
        publish_ego_car(ego_pub)

        rospy.loginfo("published")
        rate.sleep()
        frame += 1
        frame %= 154

画出汽车模型

下载汽车模型Low-Poly Car Free 3D Model - .3ds .obj .dae .blend .fbx .mtl - Free3D

解压之后，将Car.dae文件放入到包下的某个文件夹中，这里直接使用原生解压的文件夹目录。

在publish_utils.py中添加如下代码

python 复制代码

def publish_car_model(car_model_pub):
    mech_marker = Marker()
    mech_marker.header.frame_id = "map"
    mech_marker.header.stamp = rospy.Time.now()
    # id必须不一样，且只能是整数
    mech_marker.id = -1
    mech_marker.type = Marker.MESH_RESOURCE
    mech_marker.action = Marker.ADD
    mech_marker.lifetime = rospy.Duration()
    mech_marker.mesh_resource = "package://kitti_tutorials/am7f3psa0agw-Car-Model/Car-Model/Car.dae"  # .dae模型文件的相对地址

    # 位置主要看官方提供的位置图
    # 因为自车的velodyne激光雷达相对于map的位置是（0，0，0），看设备安装图上velodyne的位置是（0，0，1.73），显而易见，在rviz中自车模型位置应该是（0，0，-1.73）
    mech_marker.pose.position.x = 0.0
    mech_marker.pose.position.y = 0.0
    mech_marker.pose.position.z = -1.73

    # 这边和原作者的代码不一样，因为tf结构发生了一些改变，四元数需要自己根据模型做响应的调整，笔者这边是调整好的模型四元数
    q = transformations.quaternion_from_euler(np.pi, np.pi, -np.pi / 2.0)
    mech_marker.pose.orientation.x = q[0]
    mech_marker.pose.orientation.y = q[1]
    mech_marker.pose.orientation.z = q[2]
    mech_marker.pose.orientation.w = q[3]

    mech_marker.color.r = 1.0
    mech_marker.color.g = 1.0
    mech_marker.color.b = 1.0
    mech_marker.color.a = 1.0  # 透明度

    # 设置车辆大小
    mech_marker.scale.x = 1.0
    mech_marker.scale.y = 1.0
    mech_marker.scale.z = 1.0

    car_model_pub.publish(mech_marker)

修改kitti.py代码

python 复制代码

#!/usr/bin/env python
import numpy as np

import rospy

from data_utils import *
from publish_utils import *
import os

DATA_PATH = "/home/d501/kitti_data/2011_09_26/2011_09_26_drive_0005_sync/2011_09_26/2011_09_26_drive_0005_sync"

if __name__ == "__main__":
    frame = 0
    rospy.init_node("kitti_node", anonymous=True)
    cam_pub = rospy.Publisher('kitti_cam', Image, queue_size=10)
    pcl_pub = rospy.Publisher('kitti_point_cloud', PointCloud2, queue_size=10)
    ego_pub = rospy.Publisher('kitti_ego_car', Marker, queue_size=10)
    model_pub = rospy.Publisher('kitti_car_model', Marker, queue_size=10)

    bridge = CvBridge()

    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        image = read_camera(os.path.join(DATA_PATH, 'image_02/data/%010d.png'%frame))
        point_cloud = read_point_cloud(os.path.join(DATA_PATH, 'velodyne_points/data/%010d.bin'%frame))

        publish_camera(cam_pub, bridge, image)
        publish_point_cloud(pcl_pub, point_cloud)
        publish_ego_car(ego_pub)
        publish_car_model(model_pub)

        rospy.loginfo("published")
        rate.sleep()
        frame += 1
        frame %= 154

重新运行之后，出现错误：