batch_predict_pose = batch_predict_pose.data.cpu().numpy()
if i == 0:
for pose in batch_predict_pose[0]:
# use all predicted pose in the first prediction
for i in range(len(pose)):
# Convert predicted relative pose to absolute pose by adding last pose
pose[i] += answer[-1][i]
answer.append(pose.tolist())
batch_predict_pose = batch_predict_pose[1:]
# transform from relative to absolute
for predict_pose_seq in batch_predict_pose:
# predict_pose_seq[1:] = predict_pose_seq[1:] + predict_pose_seq[0:-1]
ang = eulerAnglesToRotationMatrix(
[0, answer[-1][0], 0]) # eulerAnglesToRotationMatrix([answer[-1][1], answer[-1][0], answer[-1][2]])
location = ang.dot(predict_pose_seq[-1][3:])
predict_pose_seq[-1][3:] = location[:]
# use only last predicted pose in the following prediction
last_pose = predict_pose_seq[-1]
for i in range(len(last_pose)):
last_pose[i] += answer[-1][i]
# normalize angle to -Pi...Pi over y axis
last_pose[0] = (last_pose[0] + np.pi) % (2 * np.pi) - np.pi
answer.append(last_pose.tolist())
print('len(answer): ', len(answer))
print('expect len: ', len(glob.glob('{}{}/*.JPEG'.format(par.image_dir, test_video))))
print('Predict use {} sec'.format(time.time() - st_t))
import numpy as np
from helper import eulerAnglesToRotationMatrix
import glob
for file in glob.glob('./output/*.txt'):
data = np.loadtxt(file, delimiter=',')
full_list = []
for row in data:
R = eulerAnglesToRotationMatrix(row[:3])
t = row[3:].reshape(3, 1)
R = np.append(R, t, axis=1)
R = R.reshape(12, )
full_list.append(R)
np.savetxt(file, full_list, fmt='%.8f')