本次内容主要是使用kitti数据集来可视化kitti车上一些传感器（相机、激光雷达、IMU）采集的资料以及对行人和车辆进行检测并在图像中画出行人和车辆的2D框、在点云中画出行人和车辆的3D框。
1、准备工作
1.1数据集下载
在开始之前，先做一些准备工作，即从kitti上下载相关数据：kitty官网
如图所示：下载途中箭头所指的两个文件【注：需要先进行注册】
除了下载这两个文件，后面还需要下载汽车模型文件和标注文件，这里直接贴出下载地址：数据下载
1.2 创建工作空间并建立一些文件
- 创建功能包
cd ~/catkin_ws/src catkin_create_pkg kitti_turtorial rospy
- 在刚创建的功能包下的src文件夹中创建以下python文件
2、详细步骤
说明：该部分只是自己的学习笔记，故只会贴出每一步比较核心的代码，要想看懂整个流程，建议完整的观看相关视频：视频
当然最后我也会贴出所有文件的源码供大家学习
2.1 发布照片
#创建一个摄像头的发布者 cam_pub = rospy.Publisher('kitti_cam',Image,queue_size=10) #读取摄像机数据 image = read_camera(os.path.join(DAtA_PATH, 'image_02/data/%010d.png'%frame)) #发布数据 publish_camera(cam_pub,bridge,image,boxes_2d,types)
2.2 发布点云
#创建一个点云的发布者 pcl_pub = rospy.Publisher('kitti_point_cloud',PointCloud2,queue_size=10) #读取点云数据 point_cloud = read_point_cloud(os.path.join(DAtA_PATH,'velodyne_points/data/%010d.bin'%frame)) #发布数据 publish_point_cloud(pcl_pub,point_cloud)
2.3 画出自己车子以及照相机视野
#创建一个汽车的发布者 ego_pub = rospy.Publisher('kitti_ego_car',MarkerArray,queue_size=10) #发布ego_car数据 publish_ego_car(ego_pub) ##绘制车子的照相机视野 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)) marker_array.markers.append(marker)
2.4 发布IMU
#创建一个IMU发布者 imu_pub = rospy.Publisher('kitti_imu',Imu,queue_size=10) #发布imu数据 publish_imu(imu_pub,imu_data) ##IMU发布函数相关设置 def publish_imu(imu_pub,imu_data): imu = Imu() 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)); 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 imu.linear_acceleration.y = imu_data.al imu.linear_acceleration.z = imu_data.au #设置角加速度 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)
2.5 发布GPS
#创建一个GPS发布者 gps_pub = rospy.Publisher('kitti_gps',NavSatFix,queue_size=10) #发布GPS数据 publish_gps(gps_pub,imu_data) ##GPS发布函数相关设置 def publish_gps(gps_pub,imu_data): gps = NavSatFix() gps.header.frame_id = FRAME_ID gps.header.stamp = rospy.Time.now() #gps经度、纬度、海拔高度 gps.latitude = imu_data.lat gps.longitude = imu_data.lon gps.altitude = imu_data.alt gps_pub.publish(gps)
2.6 在rviz上显示2D侦测框
#读取2D检测框数据 boxes_2d = np.array(df_tracking_frame[['bbox_left', 'bbox_top', 'bbox_right', 'bbox_bottom']]) types = np.array(df_tracking_frame['type']) ## 2D框相关设置 for typ,box in zip(types,boxes): top_left = int(box[0]),int(box[1]) bottom_right = int(box[2]),int(box[3]) cv2.rectangle(image,top_left,bottom_right,DETECTION_COLOR_DICT[typ],2) cam_pub.publish(bridge.cv2_to_imgmsg(image,"bgr8"))
2.7 在rviz上显示3D 侦测框
#读取3D检测框数据 boxes_3d = np.array(df_tracking_frame[['height', 'width', 'length', 'pos_x', 'pos_y', 'pos_z', 'rot_y']] corners_3d_velos = [] for box_3d in boxes_3d: corners_3d_cam2 = compute_3d_box_cam2(*box_3d) corners_3d_velo = calib.project_rect_to_velo(corners_3d_cam2.T) corners_3d_velos += [corners_3d_velo] ##3D框发布函数相关设置 def publish_3dbox(box3d_pub,corners_3d_velos,types): marker_array = MarkerArray() for i, corners_3d_velo in enumerate(corners_3d_velos): # 3d box marker = Marker() marker.header.frame_id = FRAME_ID marker.header.stamp = rospy.Time.now() marker.id = i marker.action = Marker.ADD marker.lifetime = rospy.Duration(LIFETIME) marker.type = Marker.LINE_LIST b, g, r = DETECTION_COLOR_DICT[types[i]] marker.color.r = r/255.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: 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) box3d_pub.publish(marker_array)
3、代码合集
代码托管在Gitee上，自行下载：代码
