“成败也当笑看”
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()