【从kitti开始自动驾驶】--9.2 利用IMU得到的速度画出自己的轨迹

9.1讲了通过kitti的数据集得到每两帧之间的变化量,这一节将上一节得到的数据,轨迹化,
意为将某一帧作为参考点(坐标系),得到之前的坐标(轨迹)

1. jupyter代码增添

• 因为IMU在当前条件下,效果更好,所以以它的数据展开计算
• 这个问题其实转化成了坐标系间的坐标转换问题,解决步骤如下:

1.1 得到帧间距离

displacement = 0.1 * np.linalg.norm(imu_dat[['vf', 'vl']])   #得到帧间距离改变量

1.2 得到角度偏移

yaw_change = float(imu_dat.yaw - prev_imu_data.yaw)            #得到帧间角度偏移量

1.3 得到上一位置在当前坐标系的坐标

x0, y0 = locations[i]

1.4 求转换后的坐标

• 利用旋转矩阵的方法
• 先旋转,后平移,先乘以旋转矩阵,再添加位移量

#先旋转,后平移,先乘以旋转矩阵,再添加位移量
            x1 = x0*np.cos(yaw_change) + y0*np.sin(yaw_change) - displacement
            y1 = -x0*np.sin(yaw_change) + y0*np.cos(yaw_change) 

1.5 加上在原坐标系的原有坐标

• 在本例中,本车为0,0原点:

locations += [np.array([0, 0])]

1.6 绘图

• 将location转为数组, 并且提出第0列作为x坐标, 提出第1列作为y坐标

plt.figure(figsize=(20, 10))
plt.plot(np.array(locations)[:, 0], np.array(locations)[:, 1], label = 'position')
plt.legend()
plt.show()

2 效果展示

• 对比小车的运行视频,可以看到小车先往左,再往右,比较符合真实情况

3. Jupyter源码

在9.1节的源码后边加如下代码即可:

# 保留前一帧IMU的数据
prev_imu_data = None
locations = []

for frame in range(150):
    imu_dat = read_imu('/home/qinsir/kitti_folder/2011_09_26/2011_09_26_drive_0005_sync/oxts/data/%010d.txt'%frame)
    
    if prev_imu_data is not None:
        displacement = 0.1 * np.linalg.norm(imu_dat[['vf', 'vl']])   #得到帧间距离改变量
        yaw_change = float(imu_dat.yaw - prev_imu_data.yaw)            #得到帧间角度偏移量
        
        #得到当前帧时,所有过去位置的坐标
        for i in range(len(locations)):
            x0, y0 = locations[i]
            #先旋转,后平移,先乘以旋转矩阵,再添加位移量
            x1 = x0*np.cos(yaw_change) + y0*np.sin(yaw_change) - displacement
            y1 = -x0*np.sin(yaw_change) + y0*np.cos(yaw_change)    
            locations[i] = np.array([x1, y1])
    # 加上原有坐标    
    locations += [np.array([0, 0])]
    #迭代
    prev_imu_data = imu_dat

plt.figure(figsize=(20, 10))
#   将location转为数组, 并且提出第0列作为x坐标,   提出第1列作为y坐标
plt.plot(np.array(locations)[:, 0], np.array(locations)[:, 1], label = 'position')
plt.legend()
plt.show()