使用kitti数据集实现自动驾驶——绘制出所有物体的行驶轨迹

继续创作，加速生长！这是我参与「日新计划 6 月更文挑战」的第14天，点击查看活动概况

本次内容主要是上星期内容的延续，主要画出kitti车的行进的轨道

相同的，咱们先来看看最终完成的作用：

视频

  接下来就进入一步步的编码环节。。。

1、运用IMU、GPS核算汽车移动间隔和旋转角度

• 核算移动间隔

  • 经过GPS核算

    #定义核算GPS间隔办法
    def computer_great_circle_distance(lat1,lon1,lat2,lon2):
        delta_sigma = float(np.sin(lat1*np.pi/180)*np.sin(lat2*np.pi/180)+
                            np.cos(lat1*np.pi/180)*np.cos(lat2*np.pi/180)*np.cos(lon1*np.pi/180-lon2*np.pi/180))
        return 6371000.0*np.arccos(np.clip(delta_sigma,-1,1))
    #运用GPS核算间隔
     gps_distance += [computer_great_circle_distance(imu_data.lat,imu_data.lon,prev_imu_data.lat,prev_imu_data.lon)]
    

  • 经过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']
    #获取IMU数据
    imu_data = read_imu('/home/wsj/data/kitty/RawData/2011_09_26/2011_09_26_drive_0005_sync/oxts/data/%010d.txt'%frame)
    #运用IMU核算间隔
    imu_distance += [0.1*np.linalg.norm(imu_data[['vf','vl']])]
    

  • 比较两种方法核算出的间隔（GPS/IMU）

显然，IMU核算的间隔较为平滑。

• 核算旋转角度 旋转角度的核算较为简单，咱们只需要根据IMU获取到的yaw值就可以核算（前后两帧图像的yaw值相减）

  

2、画出kitti车的行进轨道

3、画出一切车辆的轨道

咻咻咻咻~~duang~~点个赞呗

上一篇：运用kitti数据集完成自动驾驶——发布照片、点云、IMU、GPS、显示2D和3D侦测框