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)