DaddyWesker
commented
3 years ago Wrote drawer myself. If anyone needs it, here. THough it should be mentioned, that in my case i've inferenced model with seq_length 5 and it saved tons of files in the eval folder. If you somehow test this model other way, you'll probably need to change the code.
import os
import numpy as np
import matplotlib.pyplot as plt
path_to_poses = "/home/daddywesker/TestingNeuralSlam/GeoNet/eval/Pose/"
plt.switch_backend('agg')
def get_matrix_from_quat(quat):
"""Create a 4x4 homography matrix that represents the rotation
of the quaternion.
"""
#quat - x, y, z, qx, qy, qz, qw
# Init matrix (remember, a matrix, not an array)
a = np.zeros((4, 4), dtype=np.float32)
quat = [float(x) for x in quat]
x, y, z, qx, qy, qz, qw = quat
# First row
a[0, 0] = - 2.0 * (qy * qy + qz * qz) + 1.0
a[1, 0] = + 2.0 * (qx * qy + qz * qw)
a[2, 0] = + 2.0 * (qx * qz - qy * qw)
a[3, 0] = 0.0
# Second row
a[0, 1] = + 2.0 * (qx * qy - qz * qw)
a[1, 1] = - 2.0 * (qx * qx + qz * qz) + 1.0
a[2, 1] = + 2.0 * (qz * qy + qx * qw)
a[3, 1] = 0.0
# Third row
a[0, 2] = + 2.0 * (qx * qz + qy * qw)
a[1, 2] = + 2.0 * (qy * qz - qx * qw)
a[2, 2] = - 2.0 * (qx * qx + qy * qy) + 1.0
a[3, 2] = 0.0
# Fourth row
a[0, 3] = x
a[1, 3] = y
a[2, 3] = z
a[3, 3] = 1.0
return a
res = sorted(os.listdir(path_to_poses))
opened = open(path_to_poses + res[0])
pose0 = opened.readline()
H0 = get_matrix_from_quat(pose0.split("\n")[0].split(" ")[1:])
traj = []
traj.append(np.asarray([H0[0, 3], H0[1, 3]]))
for i in range (4, len(res), 4):
opened = open(path_to_poses + res[i])
pose = opened.readlines()[-1]
HLast = get_matrix_from_quat(pose.split("\n")[0].split(" ")[1:])
H0 = np.matmul(H0, HLast)
traj.append(np.asarray([H0[0, 3], H0[1, 3]]))
traj = np.asarray(traj)
plt.scatter(traj[:, 0], traj[:, 1])
plt.savefig('traj.jpg')
Hello. Thanks for your code.
How ca i possibly draw trajectory after? Do you have any code for that? It will be much of the help for me.
python geonet_main.py --mode=test_pose --dataset_dir=/path/to/kitti/odom/dataset/ --init_ckpt_file=/path/to/trained/model/ --batch_size=1 --seq_length=5 --pose_test_seq=9 --output_dir=/path/to/save/predictions/THanks in advance.