如何根据相机位姿画出相机运动轨迹

相机位姿的基本概念

相机位姿主要就是反映了若干张图片之间相机的运动状态。通常会用三维度的位移量加上一个反映旋转量的四元数进行存储，$(t_x,t_y,t_z,q_x,q_y,q_z,q_w)$，

绘制相机运动曲线

当相机的位姿始终是以第一张图像为参考时

此时，很显然以第一张图像中相机坐标为世界系坐标，则其它位姿中的位移向量就是当前相机所在世界系的坐标。以KITTI odometry中的pose.txt中数据为例，具体格式可以在其中的development kit中查看。三维坐标系下：

from mpl_toolkits.mplot3d import axes3d
import matplotlib.pyplot as plt
import numpy as np

with open('./poses/09.txt','r') as f:
    poses = f.readlines()
posexyz = []
for pose in poses:
    pose = np.array([float(s) for s in pose[:-1].split(' ')]).reshape((3,4))
    # 位移向量
    dataxyz = pose[:,3]
    # 旋转矩阵
    rot = np.linalg.inv(pose[:,:3])
    posexyz.append(dataxyz.tolist())
posedata_gt = np.array(posexyz)

fig = plt.figure()
ax = fig.gca(projection='3d')
# 设置图像信息
ax.set_title("3D_Curve")
ax.set_xlabel("x")
ax.set_ylabel("y")
ax.set_zlabel("z")
point_num = 480

# 在KITTI采集的数据集中，z轴向前，x向右，y轴向下
figure = ax.plot(posedata_gt[:point_num,2],posedata_gt[:point_num,0],-posedata_gt[:point_num,1],c='r',lw =0.5)
#figure = ax.scatter(posedata_gt[:point_num,2],posedata_gt[:point_num,0],-posedata_gt[:point_num,1],c='r',s =0.5)
plt.show()

二维坐标系下：

# 同时画出groundtruth与prediction对应的平面轨迹图
fig = plt.figure(num=1)

point_num = 480
pre_m = np.median(data_total[2,:point_num])
# print('predict median:',pre_m)
gt_m = np.median(posedata_gt[:point_num,2])
# print('groundtruth median:',gt_m)
scale = gt_m/pre_m
# print(scale)
data_total = data_total*scale

plt.plot(data_total[0,0:point_num],data_total[2,0:point_num],c='b',lw =0.5)
plt.plot(posedata_gt[:point_num,0],posedata_gt[:point_num,2],c='r',lw =0.5)
plt.show()

当生成的位姿并不是一直以第一张图像为参考时

基本概念

举个具体地栗子，以商谈科技在2018CVPR提出的GeoNet为例，该网络输出的相机位姿是五个五个为一组，即:

组	图像帧	参考系
第0组	0,1,2,3,4	0（即世界坐标系）
第1组	1,2,3,4,5	1
…	…	…
第N组	N,N+1,N+2,N+3,N+4	N

这就需要将除第一组外，其他组内的相机坐标从第N个相机坐标系，转移至世界坐标系

基本原理

为了简化起见，此处仅对每一个坐标计算一次（理论上，从第五帧图像开始可以算五次求平均值）
以计算第$i$帧图像对应的相机所在世界系坐标为例，先读取第$(i-1)$个pose.txt，即第$(i-1)$组，记录的是以第$(i-1)$个相机坐标系为参考，记录的是第$i-1,i,i+1,i+2,i+3$帧图像的相对位姿，其中第$i$个相机坐标为$(t_{xi}^{i-1},t_{yi}^{i-1},t_{zi}^{}$