def test():
from math import pi
import numpy
# pose in frame A
a = EKF.column_vector(1, 2, pi / 2)
# pose in frame B
b = EKF.column_vector(1, 1, 0)
# actual origin of B in A
B = EKF.column_vector(2, 1, pi / 2)
# calculated origin of B in A
BB = EKF.get_offset(a, b)
# use it to convert b into a
aa = EKF.coord_transform(b, BB)
# assert that things match
assert numpy.equal(B, BB).all()
assert numpy.equal(a, aa).all()
def test():
from math import pi
import numpy
# pose in frame A
a = EKF.column_vector(1, 2, pi / 2)
# pose in frame B
b = EKF.column_vector(1, 1, 0)
# actual origin of B in A
B = EKF.column_vector(2, 1, pi / 2)
# calculated origin of B in A
BB = EKF.get_offset(a, b)
# use it to convert b into a
aa = EKF.coord_transform(b, BB)
# assert that things match
assert numpy.equal(B, BB).all()
assert numpy.equal(a, aa).all()
