kitti之ros可视化_学习笔记--第11课:无人车本身轨迹添加

Posted 鸿_H

tags:

篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了kitti之ros可视化_学习笔记--第11课:无人车本身轨迹添加相关的知识,希望对你有一定的参考价值。

环境:ubuntu16.04,ros-kinetic,python2,vscode,opencv,rviz

概要:这节课笔记,新增展示的是,给无人车本身添加运动轨迹显示。

源码是承接之前课程新增内容所得的,操作以及解析可以参考之前blog~

资料准备及预处理可参考博客,https://blog.csdn.net/qq_45701501/article/details/116447770

tracking资料准备:https://blog.csdn.net/qq_45701501/article/details/116586427

1、思路

包存储位置、创建、编译、运行这些参考本人这系列前面的博客。

思路:
0)源码里面计算坐标,比如两帧的平移量、旋转角,来源数据集里面的imu资料;

1)坐标转换时,注意只是考虑俯视图情况,也就是将3d情况转为2d情况来降低思考难度;

2)无人车过去所有帧的坐标都变为当前帧和上一帧的坐标系考虑,最后都是转为当前帧坐标系表示;

3)坐标转换时,对于既有平移又有旋转情况,先旋转再平移,也就是先乘以一个旋转矩阵,再乘以一个平移矩阵,达到所有坐标都转为当前坐标系表示。

注意:这里可能说得比较抽象,建议参考文末参考老师油管帐号课程链接或者b站搬运的视频https://www.bilibili.com/video/BV1qV41167d2?p=1

2、源码

四个文件:读取资料文件data_utils.py,发布函数文件publish_utils.py,将3d侦测框从相机坐标系转为雷达坐标系显示文件kitti_utils.py,主函数文件p20_kitti.py.

data_utils.py:

#!/usr/bin/env python
# -*- coding:utf8 -*-

import cv2
import numpy as np
import os
import pandas as pd #用于读取imu资料

IMU_COLUMN_NAMES = ['lat','lon','alt','roll','pitch','yaw','vn','ve','vf','vl','vu',
                    'ax','ay','az','af','al','au','wx','wy','wz','wf','wl','wu',
                    'posacc','velacc','navstat','numsats','posmode','velmode','orimode'
                    ]#根据kitti数据集中的名称进行定义的,个人理解是对照c里面的宏定义

TRACKING_COLUMN_NAMES=['frame', 'track_id', 'type', 'truncated', 'occluded', 'alpha', 
                'bbox_left', 'bbox_top','bbox_right', 'bbox_bottom', 'height', 
                'width', 'length', 'pos_x', 'pos_y', 'pos_z', 'rot_y']#tracking数据单位


#读取图片路径函数
def read_camera(path):
    return cv2.imread(path)

#读取点云路径函数
def read_point_cloud(path):
    return np.fromfile(path,dtype=np.float32).reshape(-1,4)

#读取imu资料
def read_imu(path):
    df=pd.read_csv(path,header=None,sep=' ')#读取数据
    df.columns=IMU_COLUMN_NAMES#给数据赋予单位
    return df

#读取trackiing资料
def read_tracking(path):
    df=pd.read_csv(path,header=None,sep=' ')#读取tracking资料
    df.columns=TRACKING_COLUMN_NAMES#给资料数据添加单位
    df.loc[df.type.isin(['Truck','Van','Tram']),'type']='Car'#将这三种车子,统一定义为Car
    df=df[df.type.isin(['Car','Pedestrian','Cyclist'])]#只是获取数据集中类型为指定的数据,注意car为重定义类型
    return df#返回读取的资料

publish_utils.py:

#!/usr/bin/env python
# -*- coding:utf8 -*-

import rospy
from std_msgs.msg import Header
from visualization_msgs.msg import Marker,MarkerArray#Marker绘制相机视野指示线模块,MarkerArray解决Marker带来发布的不同步问题
from sensor_msgs.msg import Image,PointCloud2,Imu,NavSatFix
from geometry_msgs.msg import Point#Point来自ros包定义,所以需要定义;若不清楚,则需要到ros官网上面查看具体那个包
import sensor_msgs.point_cloud2 as pcl2
from cv_bridge import CvBridge
import numpy as np
import tf
import cv2

FRAME_ID='map'
DETECTION_COLOR_DICT = {'Car':(255,255,0),'Pedestrian':(0,226,255),'Cyclist':(141,40,255)}#颜色字典

#车头朝前,左上点为0,顺时针,0,1,2,3四个点,顶部同样顺时针,依次为(0顶部)4,5,6,7
#侦测盒资料,连线顺序
LINES = [[0, 1], [1, 2], [2, 3], [3, 0]] # lower face
LINES+= [[4, 5], [5, 6], [6, 7], [7, 4]] #upper face
LINES+= [[4, 0], [5, 1], [6, 2], [7, 3]] #connect lower face and upper face
LINES+= [[4, 1], [5, 0]] #front face 对角线表示叉叉以表示正前方

#侦测盒存在的时长
LIFETIME = 0.1

#发布图片函数
def publish_camera(cam_pub,bridge,image,boxes,types):#增加参数boxes、types
    #绘制框框到图片中
    for typ,box in zip(types,boxes):#给对应类型每个box绘制对应颜色图线
        top_left=int(box[0]),int(box[1])#box的左上角点,像素为整数,所以需要转换int类型
        bottom_right=int(box[2]),int(box[3])#box的右下角点
        #绘制框框,依次指定图片、左上角点、右下角点、根据类型不同给的颜色(bgr)、线粗细
        cv2.rectangle(image,top_left,bottom_right,DETECTION_COLOR_DICT[typ],2)
    cam_pub.publish(bridge.cv2_to_imgmsg(image,"bgr8"))

#发布点云函数
def publish_point_cloud(pcl_pub,point_clond):
    header=Header()
    header.stamp=rospy.Time.now()
    header.frame_id=FRAME_ID
    pcl_pub.publish(pcl2.create_cloud_xyz32(header,point_clond[:,:3]))

#发布相机视野以及车子模型marker函数
def publish_ego_car(ego_car_pub):
#publish left and right 45 degree FOV lines and ego car model mesh
    
    marker_array=MarkerArray()#解决marker发布不同步问题

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

    marker.id=0#每个marker只能有一个id,有重复的id,只会显示一个
    marker.action=Marker.ADD#表示添加marker
    marker.lifetime=rospy.Duration()#lifetime表示marker在画面中显示的时长;Duration()函数,不给任何参数时,表示一直存在
    marker.type=Marker.LINE_STRIP#所发布marker的类型

    #设定指示线颜色
    marker.color.r=0.0
    marker.color.g=1.0
    marker.color.b=0.0
    marker.color.a=1.0#透明度,1表示完全不透明
    marker.scale.x=0.2#大小,这里表示线的粗细

    #根据激光点云的坐标系来定义2号相机的视野范围
    marker.points=[]
    marker.points.append(Point(10,-10,0))#Point,属于ros的资料包里面的定义,所以需要导入
    marker.points.append(Point(0,0,0))
    marker.points.append(Point(10,10,0))

    marker_array.markers.append(marker)#将指示线marker放到MarkerArray中

    #发布车子外形函数
    mesh_marker=Marker()
    mesh_marker.header.frame_id=FRAME_ID
    mesh_marker.header.stamp=rospy.Time.now()

    mesh_marker.id=-1#id只能设置整数,不能设置带有小数的
    mesh_marker.lifetime=rospy.Duration()
    mesh_marker.type=Marker.MESH_RESOURCE#这里的MESH_RESOURCE表示导入的是3d模型
    mesh_marker.mesh_resource="package://kitti_tutorial/Audi R8/Models/Audi R8.dae"#下载的dae模型存在问题,只是显示部分

    #设定模型位置
    mesh_marker.pose.position.x=0.0
    mesh_marker.pose.position.y=0.0
    mesh_marker.pose.position.z=-1.73#这里负数,是因为以激光雷达坐标系而定义的,1.73是根据官方发布的位置定义所取的

    #设计车子模型的旋转量
    q=tf.transformations.quaternion_from_euler(0,0,np.pi/2)#(np.pi/2,0,np.pi)这里根据下载的车子模型进行调整
    mesh_marker.pose.orientation.x=q[0]
    mesh_marker.pose.orientation.y=q[1]
    mesh_marker.pose.orientation.z=q[2]
    mesh_marker.pose.orientation.w=q[3]

    #设置车子模型的颜色
    mesh_marker.color.r=1.0
    mesh_marker.color.g=1.0
    mesh_marker.color.b=1.0
    mesh_marker.color.a=1.0

    #设置车子模型的大小
    mesh_marker.scale.x=0.6
    mesh_marker.scale.y=0.6
    mesh_marker.scale.z=0.6

    marker_array.markers.append(mesh_marker)#将车子marker放到MarkerArray中

    ego_car_pub.publish(marker_array)

#发布imu资料函数
def publish_imu(imu_pub,imu_data):
    imu=Imu()#ros,imu 进行google可以查看文档说明
    imu.header.frame_id=FRAME_ID
    imu.header.stamp=rospy.Time.now()

    #旋转角度、加速度,角速度
    q=tf.transformations.quaternion_from_euler(float(imu_data.roll),float(imu_data.pitch),float(imu_data.yaw))#(np.pi/2,0,np.pi)这里根据下载的车子模型进行调整
    imu.orientation.x=q[0]#以下四个表示旋转角,将读取的数据转为四元数表示
    imu.orientation.y=q[1]
    imu.orientation.z=q[2]
    imu.orientation.w=q[3]
    imu.linear_acceleration.x=imu_data.af#根据雷达坐标系,确定x方向线性加速度
    imu.linear_acceleration.y=imu_data.al#根据雷达坐标系,确定y方向线性加速度
    imu.linear_acceleration.z=imu_data.au#根据雷达坐标系,确定z方向线性加速度
    imu.angular_velocity.x=imu_data.wf#这三个表示不同方向的角速度
    imu.angular_velocity.y=imu_data.wl
    imu.angular_velocity.z=imu_data.wu

    imu_pub.publish(imu)

#发布gps资料函数
def publish_gps(gps_pub,imu_data):
    gps=NavSatFix()#ros里面对于gps资料识别包
    gps.header.frame_id=FRAME_ID
    gps.header.stamp=rospy.Time.now()

    gps.latitude=imu_data.lat#纬度
    gps.longitude=imu_data.lon#经度
    gps.altitude=imu_data.alt#海拔

    gps_pub.publish(gps)

#发布侦测盒函数
#def publish_3dbox(box3d_pub,corners_3d_velos):#侦测盒颜色一致写法
#def publish_3dbox(box3d_pub,corners_3d_velos,types):#types指定物体种类以表示不同颜色
def publish_3dbox(box3d_pub,corners_3d_velos,types,track_ids):#再增加track_id参数    
    marker_array=MarkerArray()#把所有marker放在一起发布
    for i,corners_3d_velo in enumerate(corners_3d_velos):#对每个顶点建立marker
        marker = Marker()
        marker.header.frame_id = FRAME_ID
        marker.header.stamp =rospy.Time.now()

        marker.id =i 
        marker.action = Marker.ADD
        #由于车子一直在运动,0.1秒会更新一次,所以侦测盒更新时间为LIFETIME=0.1秒,防止侦测盒一直存在
        marker.lifetime =rospy.Duration(LIFETIME)
        marker.type = Marker.LINE_LIST

        # marker.color.r = 0.0#这几行表示发布的侦查盒颜色都一样的
        # marker.color.g = 1.0
        # marker.color.b = 1.0
        b, g, r = DETECTION_COLOR_DICT[types[i]]#根据不同类型,侦测盒颜色给不一样
        marker.color.r = r/255.0   #由于是python2,所以需要加.0才会做小数点除法
        marker.color.g = g/255.0
        marker.color.b = b/255.0
        
        marker.color.a = 1.0

        marker.scale.x = 0.1

        marker.points = []
        for l in LINES:#给8个顶点指定连线顺序,上面有定义
            p1 = corners_3d_velo[l[0]]
            marker.points.append(Point(p1[0],p1[1],p1[2]))
            p2 = corners_3d_velo[l[1]]
            marker.points.append(Point(p2[0],p2[1],p2[2]))
        marker_array.markers.append(marker)

        #track_id的marker
        text_marker = Marker()
        text_marker.header.frame_id = FRAME_ID
        text_marker.header.stamp = rospy.Time.now()

        text_marker.id = i +1000 #i和上面定义一致,保证发布正常显示
        text_marker.action = Marker.ADD
        text_marker.lifetime = rospy.Duration(LIFETIME)
        text_marker.type = Marker.TEXT_VIEW_FACING #TEXT表示文字,VIEW_FACING表示一直朝向你观看方向

        #p4 = corners_3d_velo[4]#upper front left corner定义设置的marker位置,这里表示上左角
        p4 = np.mean(corners_3d_velo,axis=0)#axis=0表示取的是垂直方向的轴的平均,是的显示在侦测盒中心上方

        text_marker.pose.position.x = p4[0]
        text_marker.pose.position.y = p4[1]
        text_marker.pose.position.z = p4[2] + 1 #让track_id显示在侦测盒上方

        text_marker.text = str(track_ids[i])  #指定marker显示文字内容,str将track_id内容转换为string类型才行显示

        #指定marker大小
        text_marker.scale.x = 1
        text_marker.scale.y = 1
        text_marker.scale.z = 1

        b, g, r = DETECTION_COLOR_DICT[types[i]] #track_id文字显示颜色根据物体种类显示
        text_marker.color.r = r/255.0
        text_marker.color.g = g/255.0
        text_marker.color.b = b/255.0
        text_marker.color.a = 1.0
        marker_array.markers.append(text_marker)

    box3d_pub.publish(marker_array)#发布

#发布车子本身轨迹
def publish_loc(loc_pub,locations):
    marker_array = MarkerArray()

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

    marker.action = Marker.ADD
    marker.type = Marker.LINE_STRIP

    #定义车子轨迹颜色以及大小
    marker.color.r = 1.0
    marker.color.g = 0.0
    marker.color.b = 0.0
    marker.color.a = 1.0
    marker.scale.x = 0.2

    marker.points = []
    for p in locations:#把所有的点连接起来
        marker.points.append(Point(p[0],p[1],0))
    
    marker_array.markers.append(marker)
    loc_pub.publish(marker_array)#把车子自身轨迹发布出去

kitti_utils.py:
(这个文件和链接里面一样的,copy一份就行了)

https://blog.csdn.net/qq_45701501/article/details/117547974

p20_kitti.py:

#!/usr/bin/env python
# -*- coding:utf8 -*-

import os
from collections import deque

from data_utils import *
from publish_utils import *
from kitti_utils import * #kitti_utils.py文件有报错,但是不影响运行

DATA_PATH='/home/ylh/data/kitti/RawData/2011_09_26/2011_09_26_drive_0005_sync'

#建立一个类别,用于实现不同物体的坐标转换计算
class Object():
    def __init__(self):
        #self.locations = [] #保存的是物体过去所有的轨迹
        self.locations = deque(maxlen=20)#方法2:指定过去轨迹点,最大保存20个
    
    def update(self,displacement,yaw): #更新自己本身过去的位置,过去轨迹,移动量,旋转角;
        #其中,移动量和旋转角跟具体物体无关,都只是关系到自身移动量或者旋转角,所以跟其他物体无关,因此都是同一个值(个人有点迷)
        for i in range(len(self.locations)):#对每一帧坐标进行转换
            x0,y0 = self.locations[i]#初始位置坐标
            #坐标转换后的新坐标
            x1 = x0*np.cos(yaw_change)+y0*np.sin(yaw_change)-displacement
            y1 = -x0*np.sin(yaw_change)+y0*np.cos(yaw_change)
            self.locations[i] = np.array(kitti之ros可视化_学习笔记--第1课:资料准备及环境介绍

kitti之ros可视化_学习笔记--第4课:车子模型的添加

kitti之ros可视化_学习笔记--第6课:发布imu资料

kitti之ros可视化_学习笔记--第7课:发布gps资料

kitti之ros可视化_学习笔记--第5课:相机视野指示线添加

kitti之ros可视化_学习笔记--第3课:点云资料发布