def test_relation1():
t = np.array([10,0,0])
n = np.array([-1,0,0])
t_est = t
n_est = n
xy, theta = get_estimate(t, n, t_est, n_est)
assert_almost_equal(xy, [0,0])
assert_almost_equal(theta, 0)
def test_relation2():
t = np.array([10,0,0])
n = np.array([-1,0,0])
# I see it at 3
t_est = np.array([3,0,0])
n_est = n
xy, theta = get_estimate(t, n, t_est, n_est)
assert_almost_equal(xy, [7,0])
assert_almost_equal(theta, 0)
def test_faster_math():
for _ in range(10):
alpha1 = np.random.randn()
alpha2 = np.random.randn()
t = np.random.rand(3)
t_est = np.random.rand(3)
n = np.array([np.cos(alpha1), np.sin(alpha1)])
n_est = np.array([np.cos(alpha2), np.sin(alpha2)])
xy1, theta1 = get_estimate(t, n, t_est, n_est)
xy2, theta2 = get_estimate_2(t, n, t_est, n_est)
assert_almost_equal(xy1, xy2)
assert_almost_equal(theta1, theta2)
def test_relation3():
# observer
# v
#
# * <-|
t = np.array([10,0,0])
n = np.array([-1,0,0])
# I see it at 3, pointing to my right (negative y)
t_est = np.array([3,0,0])
n_est = np.array([0,-1,0])
xy, theta = get_estimate(t, n, t_est, n_est)
assert_almost_equal(xy, [10,3])
assert_almost_equal(theta, -np.pi/2)
