Rot2

from gtsam import Rot2, Point2
import numpy as np

# The identity rotation has theta = 0.
identity = Rot2()
print("Identities:")
print(identity.theta())
print(Rot2.Identity().theta())

# The constructor uses radians, so it is identical to Rot2.fromAngle(r).
rads = Rot2(np.pi / 2)
also_rads = Rot2.fromAngle(np.pi / 2)
print("Radians:")
print(rads.theta())
print(also_rads.theta())

# Rot2.fromDegrees(d) constructs from degrees.
degs = Rot2.fromDegrees(90)
print("Degrees:")
print(degs.theta())

# Rot2 can also be constructed using cosine and sine values, if you have them lying around.
c = np.cos(np.pi / 6)
s = np.sin(np.pi / 6)
cs = Rot2.fromCosSin(c, s)
print("Cos-Sin:")
print(cs.theta())

# Construct with bearing to point from theta = 0.
p = Point2(2, 2)
bear = Rot2.relativeBearing(p)
print("Bearing:")
print(bear.theta())
# Or with atan2(y, x), which accomplishes the same thing.
atan = Rot2.atan2(p[1], p[0])
print(atan.theta())

example_rot = Rot2(3 * np.pi / 4)

# The default print statement includes 'theta: ' and a newline at the end.
print(example_rot)

print(f"Radians: {example_rot.theta()}")
print(f"Degrees: {example_rot.degrees()}")
print(f"Cosine: {example_rot.c()}")
print(f"Sine: {example_rot.s()}")
print(f"Matrix:\n{example_rot.matrix()}")

rot = Rot2.fromDegrees(45)
p = Point2(-2, 2)

# Rotate the point at (-2, 2) 45 degrees to the -x axis.
rotated = rot.rotate(p)
print(f"Rotated: {rotated}")
# Perform the inverse rotation with unrotate()
print(f"Unrotated: {rot.unrotate(rotated)}")
# Of course, unrotating a point you didn't rotate just rotates it backwards.
print(f"Unrotated again: {rot.unrotate(p)}")

eq_rads = Rot2(np.pi / 4)
eq_degs = Rot2.fromDegrees(45)

print(eq_rads.equals(eq_degs, 1e-8))

# Direct comparison does not work for Rot2.
print(eq_rads == eq_degs)

a = Rot2(np.pi / 6)
b = Rot2(np.pi / 3)

# The inverse of a Rot2 is just the negative of its angle.
print("Inverse:")
print(a.inverse())

# The composition of two Rot2 objects is their angles added together.
# The operator for compose is *, but make no mistake, this does not multiply the angles.
print("Compose:")
print(a * b)
print(a.compose(b))

# Between gives the difference between the two angles.
print("Between:")
print(a.between(b))

# The identity is theta = 0, as above.
print("Identity:")
print(Rot2.Identity())

r = Rot2(np.pi / 2)
w = Rot2(np.pi / 4)
v = [np.pi / 2]

# The exponential map transforms a 1-dimensional vector representing an angle
# into its Rot2 equivalent.
print(Rot2.Expmap(v))
# The retract function takes the exponential map of the supplied 1D vector and
# composes it with the calling Rot2.
print(r.retract(v))

# The static log map transforms a Rot2 into its 1D vector equivalent.
print(Rot2.Logmap(r))
# The member log map transforms a Rot2 into its 1D vector equivalent relative to
# the Rot2 calling the function.
print(r.logmap(w))
# logmap is the same as calculating the coordinate of the second Rot2 in the
# local frame of the first, which localCoordinates (inherited from LieGroup) does.
print(r.localCoordinates(w))