赞
踩
环境: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
包存储位置、创建、编译、运行这些参考本人这系列前面的博客。
主要思路:添加id给3d侦测盒,也就是从tracking数据集中,读取track_id,并转为np数组格式;给发布3d侦测盒函数添加一个用于传入id的形参,在函数体中添加用于显示id的marker。
包含四个文件:读取资料文件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
物体3d侦测盒上方出现数字,则表示显示id成功。
至此,kitti数据集的3d侦测盒的id显示操作完成~
#####################
学习课程来源up主,AI葵:
https://www.youtube.com/watch?v=TBdcwwr5Wyk
致谢AI葵老师
不积硅步,无以至千里
好记性不如烂笔头
感觉有点收获的话,麻烦大大们点赞收藏哈
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。