def convert_position_and_heading_to_ego_coordinates(ref_position_and_heading_n13,
world_position_and_heading_nk3):
""" Converts a sequence of position and headings to the ego frame."""
position_nk2 = world_position_and_heading_nk3[:, :, :2] - ref_position_and_heading_n13[:, :, :2]
position_nk2 = rotate_pos_nk2(position_nk2, -ref_position_and_heading_n13[:, :, 2:3])
heading_nk1 = angle_normalize(world_position_and_heading_nk3[:, :, 2:3] -
ref_position_and_heading_n13[:, :, 2:3])
return tf.concat([position_nk2, heading_nk1], axis=2)
def convert_position_and_heading_to_world_coordinates(
ref_position_and_heading_n13, ego_position_and_heading_nk3):
""" Converts a sequence of position and headings to the world frame."""
position_nk2 = rotate_pos_nk2(ego_position_and_heading_nk3[:, :, :2],
ref_position_and_heading_n13[:, :, 2:3])
position_nk2 = position_nk2 + ref_position_and_heading_n13[:, :, :2]
heading_nk1 = angle_normalize(ego_position_and_heading_nk3[:, :, 2:3] +
ref_position_and_heading_n13[:, :, 2:3])
return tf.concat([position_nk2, heading_nk1], axis=2)
def test_rotate():
pos_2 = np.array([1.0, 0], dtype=np.float32)
theta_1 = np.array([np.pi / 2.], dtype=np.float32)
pos_112 = tf.constant(pos_2[None, None])
theta_111 = tf.constant(theta_1[None, None])
new_pos_112 = rotate_pos_nk2(pos_112, theta_111)
new_pos_2 = new_pos_112[0, 0]
assert(np.abs(new_pos_2[0]) < 1e-5)
assert(np.abs(1.0 - new_pos_2[1]) < 1e-5)
def convert_position_and_heading_to_world_coordinates(ref_position_and_heading_n13,
ego_position_and_heading_nk3):
""" Converts a sequence of position and headings to the world frame.
the ref_position_and_heading_n13 is the base parameters for the world frame [0,0,0]
"""
position_nk2 = rotate_pos_nk2(ego_position_and_heading_nk3[:, :, :2], ref_position_and_heading_n13[:, :, 2:3])
ref_position_n12 = ref_position_and_heading_n13[:, :, :2] # x and y coordinates
ref_position_0 = ref_position_n12[0]
ref_position_nk2 = np.broadcast_to(ref_position_0, ego_position_and_heading_nk3[:, :, :2].shape)
ref_heading_n11 = ref_position_and_heading_n13[:, :, 2:3]
ref_heading_0 = ref_heading_n11[0]
ref_heading_nk1 = np.broadcast_to(ref_heading_0, ego_position_and_heading_nk3[:, :, 2:3].shape)
position_nk2 = position_nk2 + ref_position_nk2
heading_nk1 = angle_normalize(ego_position_and_heading_nk3[:, :, 2:3] + ref_heading_nk1)
return tf.concat([position_nk2, heading_nk1], axis=2)