Python下的KITTI数据集odometry中相机位姿预处理，旋转矩阵转旋转向量

前言

最近在鼓捣KITTI数据集，上一篇绘制了KITTI数据集中odometry的xy数据。之后也进行了一些详细的了解，odometry数据集中每一帧图像的相机坐标都是有12个元素构成，即[R|t]，R是旋转矩阵，t是平移向量，今天突发奇想，想将旋转矩阵转为旋转向量。本来是想着自己写的，写了一半出去玩了一天，回来之后找到了相关的转换算法，就是deepvo项目中有一个转换算法。

Show you the Code

下面这段是主要的转换代码，项目里面把这段代码封装在helper.py里面

import numpy as np
import math

# epsilon for testing whether a number is close to zero
_EPS = np.finfo(float).eps * 4.0

def isRotationMatrix(R) :
	Rt = np.transpose(R)
	shouldBeIdentity = np.dot(Rt, R)
	I = np.identity(3, dtype = R.dtype)
	n = np.linalg.norm(I - shouldBeIdentity)
	return n < 1e-6

def R_to_angle(Rt):
# Ground truth pose is present as [R | t] 
# R: Rotation Matrix, t: translation vector
# transform matrix to angles
	Rt = np.reshape(np.array(Rt), (3,4))
	t = Rt[:,-1]
	R = Rt[:,:3]

	assert(isRotationMatrix(R))
	
	x, y, z = euler_from_matrix(R)
	
	theta = [x, y, z]
	pose_15 = np.concatenate((theta, t, R.flatten()))
	assert(pose_15.shape == (15,))
	return pose_15

def eulerAnglesToRotationMatrix(theta) :
    R_x = np.array([[1,         0,                  0                   ],
                    [0,         np.cos(theta[0]), -np.sin(theta[0]) ],
                    [0,         np.sin(theta[0]), np.cos(theta[0])  ]
                    ])
    R_y = np.array([[np.cos(theta[1]),    0,      np.sin(theta[1])  ],
                    [0,                     1,      0                   ],
                    [-np.sin(theta[1]),   0,      np.cos(theta[1])  ]
                    ])
    R_z = np.array([[np.cos(theta[2]),    -np.sin(theta[2]),    0],
                    [np.sin(theta[2]),    np.cos(theta[2]),     0],
                    [0,                     0,                      1]
                    ])
    R = np.dot(R_z, np.dot( R_y, R_x ))
    return R
	
def euler_from_matrix(matrix):
    
	# y-x-z Tait–Bryan angles intrincic
	# the method code is taken from https://github.com/awesomebytes/delta_robot/blob/master/src/transformations.py
    
    i = 2
    j = 0
    k = 1
    repetition = 0
    frame = 1
    parity = 0
	

    M = np.array(matrix, dtype=np.float64, copy=False)[:3, :3]
    if repetition:
        sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k])
        if sy > _EPS:
            ax = math.atan2( M[i, j],  M[i, k])
            ay = math.atan2( sy,       M[i, i])
            az = math.atan2( M[j, i], -M[k, i])
        else:
            ax = math.atan2(-M[j, k],  M[j, j])
            ay = math.atan2( sy,       M[i, i])
            az = 0.0
    else:
        cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i])
        if cy > _EPS:
            ax = math.atan2( M[k, j],  M[k, k])
            ay = math.atan2(-M[k, i],  cy)
            az = math.atan2( M[j, i],  M[i, i])
        else:
            ax = math.atan2(-M[j, k],  M[j, j])
            ay = math.atan2(-M[k, i],  cy)
            az = 0.0

    if parity:
        ax, ay, az = -ax, -ay, -az
    if frame:
        ax, az = az, ax
    return ax, ay, az
	
def normalize_angle_delta(angle):
    if(angle > np.pi):
        angle = angle - 2 * np.pi
    elif(angle < -np.pi):
        angle = 2 * np.pi + angle
    return angle

项目的另外一个数据处理函数中调用了这个功能，在项目里面的process.py里面，这边稍加了修改

import os
import numpy as np
import time
from helper import R_to_angle
# transform poseGT [R|t] to [theta_x, theta_y, theta_z, x, y, z]
# save as .npy file
def create_pose_data():
	info = {'00': [0, 4540], '01': [0, 1100], '02': [0, 4660], '03': [0, 800], '04': [0, 270], '05': [0, 2760], '06': [0, 1100], '07': [0, 1100], '08': [1100, 5170], '09': [0, 1590], '10': [0, 1200]}
	start_t = time.time()
	for video in info.keys():
		file_name = 'KITTI/poses/{}.txt'.format(video) # 这里的地址如果不一样的话，需要更改哦
		print('Transforming {} ...'.format(file_name))
		with open(file_name) as f:
			lines = [line.split('\n')[0] for line in f.readlines()]
			#print(lines[0])
			poses = [ R_to_angle([float(value) for value in l.split(' ')]) for l in lines]  # list of pose (pose=list of 12 floats)
			print(poses[0])
			poses = np.array(poses)
			base_file_name = os.path.splitext(file_name)[0]
			#print(poses[0])
			np.save(base_file_name+'.npy', poses)
			print('Video {}: shape={}'.format(video, poses.shape))
	print('elapsed time = {}'.format(time.time()-start_t))

if __name__ == '__main__':
	#clean_unused_images()
	create_pose_data()

作者为了方便后续的训练，将其保存为npy文件。里面的格式是[r, t, R]。其中r是旋转向量，t是平移向量，R是原来的旋转矩阵，也是保存在其中。

注意事项

运行代码之前需要注意的是数据集保存的格式，像我保存的格式如下，pose是从官网下载的数据，主要是保存的相机的位置，也就是这次需要操作的所有文件都在里面了。而data_odometry_color文件夹里面保存的是彩色的相机数据，官网很难下载到，我是从下面的链接下载的，在此感谢该作者的，嘻嘻。

参考

DeepVO项目的地址：https://github.com/Kallaf/Visual-Odometry
KITTI官网：http://www.cvlibs.net/datasets/kitti/raw_data.php
百度云下载链接：https://blog.csdn.net/wyy13273181006/article/details/107686370?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522163689359816780261931883%2522%252C%2522scm%2522%253A%252220140713.130102334.pc%255Fall.%2522%257D&request_id=163689359816780261931883&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2_allfirst_rank_ecpm_v1~rank_v31_ecpm-6-107686370.first_rank_v2_pc_rank_v29&utm_term=dkitti%E6%95%B0%E6%8D%AE%E9%9B%86odometry&spm=1018.2226.3001.4187