使用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']])]