假设我们已经计算出了帧间pose变换：将帧间pose转换为全局pose转换

前言

上一篇博客写了 《KITTI Odometry数据集处理：将全局pose转换为帧间pose转换》.

为了验证运算过程是否正确，也为了后续VO计算出来的6D数据进行可视化，于是便有了下面的代码

数学原理

这里的数学原理上一个博客简单介绍了

Show you the Code

'''
@Author: Astrophil ([email protected])
@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 就是这篇博客的代码