Draw trajectory about geonet HOT 1 CLOSED

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')