環境：ubuntu16.04，ros-kinetic，python2，vscode，opencv，rviz
概要：這節課筆記，新增展示的是，在物體3d偵測盒上方顯示id。
資料準備及預處理可參考部落格，
https://blog.csdn.net/qq_45701501/article/details/116447770
tracking資料準備：
https://blog.csdn.net/qq_45701501/article/details/116586427
1、思路
包存儲位置、建立、編譯、運作這些參考本人這系列前面的部落格。
主要思路：添加id給3d偵測盒，也就是從tracking資料集中，讀取track_id，并轉為np數組格式;給釋出3d偵測盒函數添加一個用于傳入id的形參，在函數體中添加用于顯示id的marker。
2、源碼
包含四個檔案：讀取資料檔案data_utils.py,釋出函數檔案publish_utils.py,将3d偵測框從相機坐标系轉為雷達坐标系顯示檔案kitti_utils.py,主函數檔案p15_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)#釋出
kitti_utils.py:
""" Helper methods for loading and parsing KITTI data.
Author: Charles R. Qi
Date: September 2017
"""
from __future__ import print_function
import numpy as np
import cv2
import os
class Object3d(object):
''' 3d object label '''
def __init__(self, label_file_line):
data = label_file_line.split(' ')
data[1:] = [float(x) for x in data[1:]]
# extract label, truncation, occlusion
self.type = data[0] # 'Car', 'Pedestrian', ...
self.truncation = data[1] # truncated pixel ratio [0..1]
self.occlusion = int(data[2]) # 0=visible, 1=partly occluded, 2=fully occluded, 3=unknown
self.alpha = data[3] # object observation angle [-pi..pi]
# extract 2d bounding box in 0-based coordinates
self.xmin = data[4] # left
self.ymin = data[5] # top
self.xmax = data[6] # right
self.ymax = data[7] # bottom
self.box2d = np.array([self.xmin,self.ymin,self.xmax,self.ymax])
# extract 3d bounding box information
self.h = data[8] # box height
self.w = data[9] # box width
self.l = data[10] # box length (in meters)
self.t = (data[11],data[12],data[13]) # location (x,y,z) in camera coord.
self.ry = data[14] # yaw angle (around Y-axis in camera coordinates) [-pi..pi]
def print_object(self):
print('Type, truncation, occlusion, alpha: %s, %d, %d, %f' % \
(self.type, self.truncation, self.occlusion, self.alpha))
print('2d bbox (x0,y0,x1,y1): %f, %f, %f, %f' % \
(self.xmin, self.ymin, self.xmax, self.ymax))
print('3d bbox h,w,l: %f, %f, %f' % \
(self.h, self.w, self.l))
print('3d bbox location, ry: (%f, %f, %f), %f' % \
(self.t[0],self.t[1],self.t[2],self.ry))
class Calibration(object):
''' Calibration matrices and utils
3d XYZ in <label>.txt are in rect camera coord.
2d box xy are in image2 coord
Points in <lidar>.bin are in Velodyne coord.
y_image2 = P^2_rect * x_rect
y_image2 = P^2_rect * R0_rect * Tr_velo_to_cam * x_velo
x_ref = Tr_velo_to_cam * x_velo
x_rect = R0_rect * x_ref
P^2_rect = [f^2_u, 0, c^2_u, -f^2_u b^2_x;
0, f^2_v, c^2_v, -f^2_v b^2_y;
0, 0, 1, 0]
= K * [1|t]
image2 coord:
----> x-axis (u)
|
|
v y-axis (v)
velodyne coord:
front x, left y, up z
rect/ref camera coord:
right x, down y, front z
Ref (KITTI paper): http://www.cvlibs.net/publications/Geiger2013IJRR.pdf
TODO(rqi): do matrix multiplication only once for each projection.
'''
def __init__(self, calib_filepath, from_video=False):
if from_video:
calibs = self.read_calib_from_video(calib_filepath)
else:
calibs = self.read_calib_file(calib_filepath)
# Projection matrix from rect camera coord to image2 coord
self.P = calibs['P2']
self.P = np.reshape(self.P, [3,4])
# Rigid transform from Velodyne coord to reference camera coord
self.V2C = calibs['Tr_velo_to_cam']
self.V2C = np.reshape(self.V2C, [3,4])
self.C2V = inverse_rigid_trans(self.V2C)
# Rotation from reference camera coord to rect camera coord
self.R0 = calibs['R0_rect']
self.R0 = np.reshape(self.R0,[3,3])
# Camera intrinsics and extrinsics
self.c_u = self.P[0,2]
self.c_v = self.P[1,2]
self.f_u = self.P[0,0]
self.f_v = self.P[1,1]
self.b_x = self.P[0,3]/(-self.f_u) # relative
self.b_y = self.P[1,3]/(-self.f_v)
def read_calib_file(self, filepath):
''' Read in a calibration file and parse into a dictionary.
Ref: https://github.com/utiasSTARS/pykitti/blob/master/pykitti/utils.py
'''
data = {}
with open(filepath, 'r') as f:
for line in f.readlines():
line = line.rstrip()
if len(line)==0: continue
key, value = line.split(':', 1)
# The only non-float values in these files are dates, which
# we don't care about anyway
try:
data[key] = np.array([float(x) for x in value.split()])
except ValueError:
pass
return data
def read_calib_from_video(self, calib_root_dir):
''' Read calibration for camera 2 from video calib files.
there are calib_cam_to_cam and calib_velo_to_cam under the calib_root_dir
'''
data = {}
cam2cam = self.read_calib_file(os.path.join(calib_root_dir, 'calib_cam_to_cam.txt'))
velo2cam = self.read_calib_file(os.path.join(calib_root_dir, 'calib_velo_to_cam.txt'))
Tr_velo_to_cam = np.zeros((3,4))
Tr_velo_to_cam[0:3,0:3] = np.reshape(velo2cam['R'], [3,3])
Tr_velo_to_cam[:,3] = velo2cam['T']
data['Tr_velo_to_cam'] = np.reshape(Tr_velo_to_cam, [12])
data['R0_rect'] = cam2cam['R_rect_00']
data['P2'] = cam2cam['P_rect_02']
return data
def cart2hom(self, pts_3d):
''' Input: nx3 points in Cartesian
Oupput: nx4 points in Homogeneous by pending 1
'''
n = pts_3d.shape[0]
pts_3d_hom = np.hstack((pts_3d, np.ones((n,1))))
return pts_3d_hom
# ===========================
# ------- 3d to 3d ----------
# ===========================
def project_velo_to_ref(self, pts_3d_velo):
pts_3d_velo = self.cart2hom(pts_3d_velo) # nx4
return np.dot(pts_3d_velo, np.transpose(self.V2C))
def project_ref_to_velo(self, pts_3d_ref):
pts_3d_ref = self.cart2hom(pts_3d_ref) # nx4
return np.dot(pts_3d_ref, np.transpose(self.C2V))
def project_rect_to_ref(self, pts_3d_rect):
''' Input and Output are nx3 points '''
return np.transpose(np.dot(np.linalg.inv(self.R0), np.transpose(pts_3d_rect)))
def project_ref_to_rect(self, pts_3d_ref):
''' Input and Output are nx3 points '''
return np.transpose(np.dot(self.R0, np.transpose(pts_3d_ref)))
def project_rect_to_velo(self, pts_3d_rect):
''' Input: nx3 points in rect camera coord.
Output: nx3 points in velodyne coord.
'''
pts_3d_ref = self.project_rect_to_ref(pts_3d_rect)
return self.project_ref_to_velo(pts_3d_ref)
def project_velo_to_rect(self, pts_3d_velo):
pts_3d_ref = self.project_velo_to_ref(pts_3d_velo)
return self.project_ref_to_rect(pts_3d_ref)
# ===========================
# ------- 3d to 2d ----------
# ===========================
def project_rect_to_image(self, pts_3d_rect):
''' Input: nx3 points in rect camera coord.
Output: nx2 points in image2 coord.
'''
pts_3d_rect = self.cart2hom(pts_3d_rect)
pts_2d = np.dot(pts_3d_rect, np.transpose(self.P)) # nx3
pts_2d[:,0] /= pts_2d[:,2]
pts_2d[:,1] /= pts_2d[:,2]
return pts_2d[:,0:2]
def project_velo_to_image(self, pts_3d_velo):
''' Input: nx3 points in velodyne coord.
Output: nx2 points in image2 coord.
'''
pts_3d_rect = self.project_velo_to_rect(pts_3d_velo)
return self.project_rect_to_image(pts_3d_rect)
# ===========================
# ------- 2d to 3d ----------
# ===========================
def project_image_to_rect(self, uv_depth):
''' Input: nx3 first two channels are uv, 3rd channel
is depth in rect camera coord.
Output: nx3 points in rect camera coord.
'''
n = uv_depth.shape[0]
x = ((uv_depth[:,0]-self.c_u)*uv_depth[:,2])/self.f_u + self.b_x
y = ((uv_depth[:,1]-self.c_v)*uv_depth[:,2])/self.f_v + self.b_y
pts_3d_rect = np.zeros((n,3))
pts_3d_rect[:,0] = x
pts_3d_rect[:,1] = y
pts_3d_rect[:,2] = uv_depth[:,2]
return pts_3d_rect
def project_image_to_velo(self, uv_depth):
pts_3d_rect = self.project_image_to_rect(uv_depth)
return self.project_rect_to_velo(pts_3d_rect)
def rotx(t):
''' 3D Rotation about the x-axis. '''
c = np.cos(t)
s = np.sin(t)
return np.array([[1, 0, 0],
[0, c, -s],
[0, s, c]])
def roty(t):
''' Rotation about the y-axis. '''
c = np.cos(t)
s = np.sin(t)
return np.array([[c, 0, s],
[0, 1, 0],
[-s, 0, c]])
def rotz(t):
''' Rotation about the z-axis. '''
c = np.cos(t)
s = np.sin(t)
return np.array([[c, -s, 0],
[s, c, 0],
[0, 0, 1]])
def transform_from_rot_trans(R, t):
''' Transforation matrix from rotation matrix and translation vector. '''
R = R.reshape(3, 3)
t = t.reshape(3, 1)
return np.vstack((np.hstack([R, t]), [0, 0, 0, 1]))
def inverse_rigid_trans(Tr):
''' Inverse a rigid body transform matrix (3x4 as [R|t])
[R'|-R't; 0|1]
'''
inv_Tr = np.zeros_like(Tr) # 3x4
inv_Tr[0:3,0:3] = np.transpose(Tr[0:3,0:3])
inv_Tr[0:3,3] = np.dot(-np.transpose(Tr[0:3,0:3]), Tr[0:3,3])
return inv_Tr
def read_label(label_filename):
lines = [line.rstrip() for line in open(label_filename)]
objects = [Object3d(line) for line in lines]
return objects
def load_image(img_filename):
return cv2.imread(img_filename)
def load_velo_scan(velo_filename):
scan = np.fromfile(velo_filename, dtype=np.float32)
scan = scan.reshape((-1, 4))
return scan
def project_to_image(pts_3d, P):
''' Project 3d points to image plane.
Usage: pts_2d = projectToImage(pts_3d, P)
input: pts_3d: nx3 matrix
P: 3x4 projection matrix
output: pts_2d: nx2 matrix
P(3x4) dot pts_3d_extended(4xn) = projected_pts_2d(3xn)
=> normalize projected_pts_2d(2xn)
<=> pts_3d_extended(nx4) dot P'(4x3) = projected_pts_2d(nx3)
=> normalize projected_pts_2d(nx2)
'''
n = pts_3d.shape[0]
pts_3d_extend = np.hstack((pts_3d, np.ones((n,1))))
print(('pts_3d_extend shape: ', pts_3d_extend.shape))
pts_2d = np.dot(pts_3d_extend, np.transpose(P)) # nx3
pts_2d[:,0] /= pts_2d[:,2]
pts_2d[:,1] /= pts_2d[:,2]
return pts_2d[:,0:2]
def compute_box_3d(obj, P):
''' Takes an object and a projection matrix (P) and projects the 3d
bounding box into the image plane.
Returns:
corners_2d: (8,2) array in left image coord.
corners_3d: (8,3) array in in rect camera coord.
'''
# compute rotational matrix around yaw axis
R = roty(obj.ry)
# 3d bounding box dimensions
l = obj.l;
w = obj.w;
h = obj.h;
# 3d bounding box corners
x_corners = [l/2,l/2,-l/2,-l/2,l/2,l/2,-l/2,-l/2];
y_corners = [0,0,0,0,-h,-h,-h,-h];
z_corners = [w/2,-w/2,-w/2,w/2,w/2,-w/2,-w/2,w/2];
# rotate and translate 3d bounding box
corners_3d = np.dot(R, np.vstack([x_corners,y_corners,z_corners]))
#print corners_3d.shape
corners_3d[0,:] = corners_3d[0,:] + obj.t[0];
corners_3d[1,:] = corners_3d[1,:] + obj.t[1];
corners_3d[2,:] = corners_3d[2,:] + obj.t[2];
#print 'cornsers_3d: ', corners_3d
# only draw 3d bounding box for objs in front of the camera
if np.any(corners_3d[2,:]<0.1):
corners_2d = None
return corners_2d, np.transpose(corners_3d)
# project the 3d bounding box into the image plane
corners_2d = project_to_image(np.transpose(corners_3d), P);
#print 'corners_2d: ', corners_2d
return corners_2d, np.transpose(corners_3d)
def compute_orientation_3d(obj, P):
''' Takes an object and a projection matrix (P) and projects the 3d
object orientation vector into the image plane.
Returns:
orientation_2d: (2,2) array in left image coord.
orientation_3d: (2,3) array in in rect camera coord.
'''
# compute rotational matrix around yaw axis
R = roty(obj.ry)
# orientation in object coordinate system
orientation_3d = np.array([[0.0, obj.l],[0,0],[0,0]])
# rotate and translate in camera coordinate system, project in image
orientation_3d = np.dot(R, orientation_3d)
orientation_3d[0,:] = orientation_3d[0,:] + obj.t[0]
orientation_3d[1,:] = orientation_3d[1,:] + obj.t[1]
orientation_3d[2,:] = orientation_3d[2,:] + obj.t[2]
# vector behind image plane?
if np.any(orientation_3d[2,:]<0.1):
orientation_2d = None
return orientation_2d, np.transpose(orientation_3d)
# project orientation into the image plane
orientation_2d = project_to_image(np.transpose(orientation_3d), P);
return orientation_2d, np.transpose(orientation_3d)
def draw_projected_box3d(image, qs, color=(255,255,255), thickness=2):
''' Draw 3d bounding box in image
qs: (8,3) array of vertices for the 3d box in following order:
1 -------- 0
/| /|
2 -------- 3 .
| | | |
. 5 -------- 4
|/ |/
6 -------- 7
'''
qs = qs.astype(np.int32)
for k in range(0,4):
# Ref: http://docs.enthought.com/mayavi/mayavi/auto/mlab_helper_functions.html
i,j=k,(k+1)%4
# use LINE_AA for opencv3
cv2.line(image, (qs[i,0],qs[i,1]), (qs[j,0],qs[j,1]), color, thickness, cv2.CV_AA)
i,j=k+4,(k+1)%4 + 4
cv2.line(image, (qs[i,0],qs[i,1]), (qs[j,0],qs[j,1]), color, thickness, cv2.CV_AA)
i,j=k,k+4
cv2.line(image, (qs[i,0],qs[i,1]), (qs[j,0],qs[j,1]), color, thickness, cv2.CV_AA)
return image
p15_kitti.py:
#!/usr/bin/env python
# -*- coding:utf8 -*-
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'
#3d偵測盒生成函數
#以特殊情況為例，當rot_y=0時，(pos_x,pos_y,pos_z)就是位于偵測盒的下方平面的中心點
#根據資料中的長寬，可以擷取下方平面的四角坐标，然後根據高資料，進而擷取偵測盒的八個點的坐标
#對于rot_y!=0情況，需要每個點乘以一個旋轉矩陣（對相機坐标系中的y軸進行旋轉），那麼就可以得到
#帶有rot_y!=0也就是yaw非0情況，8個頂點坐标（yaw=0情況時）乘以旋轉矩陣，可得到新的8個頂點坐标
def compute_3d_box_cam2(h,w,l,x,y,z,yaw):
#return:3xn in can2 coordinate
#rot_y!=0時的旋轉矩陣
R = np.array([[np.cos(yaw),0,np.sin(yaw)],[0,1,0],[-np.sin(yaw),0,np.cos(yaw)]])
#8個頂點所對應的xyz坐标（rot_y=0時）
x_corners = [l/2,l/2,-l/2,-l/2,l/2,l/2,-l/2,-l/2]
y_corners = [0,0,0,0,-h,-h,-h,-h]
z_corners = [w/2,-w/2,-w/2,w/2,w/2,-w/2,-w/2,w/2]
#做旋轉，rot_y=0可視為旋轉特例，隻不過角度為0而已，然後，讓8個頂點坐标與旋轉矩陣相乘
corners_3d_cam2 = np.dot(R,np.vstack([x_corners,y_corners,z_corners]))
#由于以下方中心點做旋轉的，是以，需要加上該旋轉中心點坐标（x,y,z）
corners_3d_cam2 += np.vstack([x,y,z])
return corners_3d_cam2#傳回偵測盒8個頂點在相機坐标系中的坐标
if __name__=='__main__':
frame = 0
rospy.init_node('kitti_node',anonymous=True)
cam_pub=rospy.Publisher('kitti_cam',Image,queue_size=10)#建立釋出圖檔topic
pcl_pub=rospy.Publisher('kitti_point_cloud',PointCloud2,queue_size=10)#建立釋出點雲topic
#ego_pub=rospy.Publisher('kitti_ego_car',Marker,queue_size=10)#建立釋出訓示線marker的topic
ego_pub=rospy.Publisher('kitti_ego_car',MarkerArray,queue_size=10)#MarkerArray方式釋出
#model_pub=rospy.Publisher('kitti_car_model',Marker,queue_size=10)#建立釋出車子模型的marker的topic
imu_pub=rospy.Publisher('kitti_imu',Imu,queue_size=10)#建立釋出imu資料的topic
gps_pub=rospy.Publisher('kitti_gps',NavSatFix,queue_size=10)#建立釋出gps資料的topic，NavSatFix,ros裡面固定衛星偵測資料包
box3d_pub=rospy.Publisher('kitti_3d',MarkerArray,queue_size=10)#建立釋出偵測盒的topic
bridge=CvBridge()
rate=rospy.Rate(10)
#讀取tracking資料
df_tracking=read_tracking('/home/ylh/data/kitti/training/label_02/0000.txt')
#讀取坐标轉換檔案，from_video=True表示會讀取路徑中三個.txt坐标轉換檔案
calib = Calibration('/home/ylh/data/kitti/RawData/2011_09_26/',from_video=True)
while not rospy.is_shutdown():
#将tracking資料的繪制框框所需資料篩選并處理
df_tracking_frame = df_tracking[df_tracking.frame==frame]
boxes_2d = np.array(df_tracking_frame[['bbox_left','bbox_top','bbox_right','bbox_bottom']])#擷取tracking資料第frame幀圖檔中的box們對應的四邊坐标
types=np.array(df_tracking_frame['type'])#讀取tracking資料第frame幀圖檔中的物體種類類型并儲存到tpyes數組中
#讀取tracking裡面偵測盒參數
boxes_3d = np.array(df_tracking_frame[['height','width','length','pos_x','pos_y','pos_z','rot_y']])
#擷取track_id
track_ids = np.array(df_tracking_frame['track_id'])#将讀取的track_id儲存成一個數組
corners_3d_velos = []#存放偵測盒8個頂點資料
for box_3d in boxes_3d:#根據資料生成所有偵測盒
corners_3d_cam2 = compute_3d_box_cam2(*box_3d)#由于該函數有7個參數，是以使用星号自動展開;計算擷取偵測盒8個頂點坐标
corners_3d_velo = calib.project_rect_to_velo(corners_3d_cam2.T)#把8個頂點，從相機坐标系裝換到雷達坐标系
corners_3d_velos += [corners_3d_velo]#存放所有偵測盒8頂點資料
#讀取圖檔
image=read_camera(os.path.join(DATA_PATH,'image_02/data/%010d.png'%frame))
#釋出圖檔
#publish_camera(cam_pub,bridge,image)
publish_camera(cam_pub,bridge,image,boxes_2d,types)#增加參數boxes,types，為了給圖檔指定類型繪制框框
#讀取點雲
point_clond=read_point_cloud(os.path.join(DATA_PATH,'velodyne_points/data/%010d.bin'%frame))
#釋出點雲
publish_point_cloud(pcl_pub,point_clond)
#釋出訓示線marker;由于不需要讀取資料，是以直接釋出即可
#當采用markerarray釋出方式，則車子和訓示線都放在這個topic
#進行釋出即可。故下面的釋出車子模型marker可以删除。這樣子，可以解決不同marker釋出不同步問題
publish_ego_car(ego_pub)
#釋出車子模型marker;由于不需要讀取資料，是以直接釋出即可
#publish_car_model(model_pub)
#讀取imu資料，這裡也包含了gps資料了
imu_data=read_imu(os.path.join(DATA_PATH,'oxts/data/%010d.txt'%frame))
#釋出imu資料
publish_imu(imu_pub,imu_data)
#釋出gps資料
publish_gps(gps_pub,imu_data)
#釋出偵測盒
#publish_3dbox(box3d_pub,corners_3d_velos)#偵測盒顔色一緻寫法
#publish_3dbox(box3d_pub,corners_3d_velos,types) #增加偵測盒類型不同而不一樣寫法
publish_3dbox(box3d_pub,corners_3d_velos,types,track_ids) #增加傳遞track_id
#釋出
rospy.loginfo("published")
rate.sleep()
frame+=1
frame%=154
3、效果
物體3d偵測盒上方出現數字，則表示顯示id成功。
至此，kitti資料集的3d偵測盒的id顯示操作完成～
#####################
學習課程來源up主，AI葵：
https://www.youtube.com/watch?v=TBdcwwr5Wyk
緻謝AI葵老師
不積矽步，無以至千裡
好記性不如爛筆頭
感覺有點收獲的話，麻煩大大們點贊收藏哈
![Stereo Matching文獻筆記之（九）：經典算法Semi-Global Matching（SGM）之神奇的HMI代價計算~[圖]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)