Visualizing waypoints

[macandro96]

While running the evaluation, I noticed that the tcp_agent.py dumps the network predictions in a json file. Is is possible to project these waypoint coordinates on the image?

Thanks!

[penghao-wu]

Yes, the saved waypoints coordinate is [right, forward]. You can project them to the image using intrinsic and extrinsic matrices. For the intrinsic matrix, the fov is 100 degrees, Cu = 900/2=450, Cv = 256/2 = 128. For the extrinsic matrix, the coordinate of the camera is 'x': -1.5, 'y': 0.0, 'z':2.0, with respect to the vehicle.

[macandro96]

Thank you so much. Just to confirm, does this logic look okay?

x_c, y_c, z_c = -1.5, 0.0, 2.0  # camera centers
C_u, C_v = 900 / 2, 256 / 2
F_x = 900 /(2 * np.tan(100 * np.pi / 360))
F_y = 256 /(2 * np.tan(100 * np.pi / 360))

# calculating extrinsic matrix
E = np.zeros((3, 4))

# Using E = [R | t]
# R is Yaw * Pitch * Roll (all are 0)
E[:3, :3] = np.eye(3)  # R = Identity
E[:, 3] = -np.array([x_c, y_c, z_c])  # t = - R C

# Intrinsic camera matrix
K = np.array([
    [F_x, 0, C_u],
    [0, F_y, C_v],
    [0, 0, 1]
])

# waypoint predictions
out = {
    'wp1': (-0.26721786, -2.74913808),
    'wp2': (-1.09084616, -5.1806664),
    'wp3': (-2.53841191, -7.36515022),
    'wp4': (-4.54262737, -8.98083773)
}
# stacking waypoint predictions
wp = np.zeros((4, 4))
wp[0:2, 0] = np.array(out['wp1'])
wp[0:2, 1] = np.array(out['wp2'])
wp[0:2, 2] = np.array(out['wp3'])
wp[0:2, 3] = np.array(out['wp4'])
# z coordinate is set to zero

# setting homogenous coordinate to 1
wp[-1, :] = np.ones(4)

img_coords = K @ E @ wp

[penghao-wu]

First, make sure the waypoint coordinate is [forward, left]. Then the extrinsic matrix should be [[0,-1,0,0],[0,0,-1,2],[1,0,0,1.5],[0,0,0,1]]

[macandro96]

Got it. Thank you so much.