def get_fiducial_origin(self):
"""
Get an SE3Pose proto defining the origin of the fiducial in the world frame.
The world frame starts at the bottom left of the image, with positive y up, positive x
to the right, and positive z out of the page.
:return: the SE3Pose proto defining the fiducial in this origin.
"""
theta = np.deg2rad(self.fiducial_rotation)
# Fiducial frame:
# Assume x is up, and z points out. The rotation matrix
# therefore has x pointed directly out of the page, and
# the zy vectors pointing to the left and up respectively.
# Note that the image origin has z pointing out of the page,
# y up and x to the right.
# Therefore the z axis is equal to (cos(t), sin(t)) and the y axis is
# (sin(t), -cos(t)).
rot_matrix = np.array([[0, np.sin(theta),
np.cos(theta)],
[0, -np.cos(theta),
np.sin(theta)], [1, 0, 0]])
world_tform_fiducial = SE3Pose(
rot=Quat.from_matrix(rot_matrix),
x=self.fiducial_position[0] / self.pixels_per_meter,
y=self.fiducial_position[1] / self.pixels_per_meter,
z=0)
return world_tform_fiducial.to_proto()
def get_desired_angle(self, xhat):
"""Compute heading based on the vector from robot to object."""
zhat = [0.0, 0.0, 1.0]
yhat = np.cross(zhat, xhat)
mat = np.array([xhat, yhat, zhat]).transpose()
return Quat.from_matrix(mat).to_yaw()
def _get_heading(xhat):
zhat = [0.0, 0.0, 1.0]
yhat = np.cross(zhat, xhat)
mat = np.array([xhat, yhat, zhat]).transpose()
return Quat.from_matrix(mat).to_yaw()
