前言
上一篇博客写了 《KITTI Odometry数据集处理:将全局pose转换为帧间pose转换》.
为了验证运算过程是否正确,也为了后续VO计算出来的6D数据进行可视化,于是便有了下面的代码
数学原理
这里的数学原理上一个博客简单介绍了
Show you the Code
'''
@Author: Astrophil (luo19902567292@163.com)
@Date: 2022-03-17
@LastEditTime: 2022-03-17
@LastEditors: Astrophil
@Description: This program used to transfrom the relative pose to global pose and test the right of the program named 'Handle_pose_for_frames.py'.
'''
import numpy as np
import matplotlib.pyplot as plt
'''------- init setting ---------'''
test_objection = '00'
split_seq = ','
'''------- initilization ---------'''
file_name = r'{}.txt'.format(test_objection)
Rn_ = np.array(
[
[1.0, 0.0, 0.0],
[0.0, 1.0, 0.0],
[0.0, 0.0, 1.0]
]
)
tn_ = np.array(
[
[0.0],
[0.0],
[0.0]
]
)
scale = 1.0
poses_R = [Rn_]
poses_t = [tn_]
with open(file_name, 'r') as f:
readlines = f.readlines()
for i in range(len(readlines)):
readline = readlines[i].split(split_seq)
readline = list(map(float, readline))
readline = np.array(readline)
theta = readline[:3]
t = readline[3:6]
R = readline[6:]
t.resize((3,1))
R.resize((3,3))
tn = Rn_ @ t * scale + tn_
# tn = tn_ + t
Rn = Rn_ @ R
poses_R.append(Rn)
poses_t.append(tn)
Rn_ = Rn
tn_ = tn
x_data = []
y_data = []
for i in poses_t:
x_data.append(i[0])
y_data.append(i[2])
print(x_data)
plt.plot(x_data,y_data)
plt.show()
运行结果
这里还是用的00.txt文件作为可视化的,根之前写的博客效果差不多,说明换算公式是对的。
Note
运行之前需要注意文件路径哦
文件夹内的txt文件是生成的,不是原生的,每行数据都是以下的格式:
[theta1, theta2, theta3, t1, t2, t3, R11, R12, R13, R21, R22, R23, R31, R32, R33]
然后两个PY文件:
- Handle_pose_for_frames.py 上一篇博客写的
- update_global_poses.py 就是这篇博客的代码