def test_exp_jacobian(self): np.random.seed(0) x0 = np.random.randn(3) j_numerical = numeric_jacobian(lambda x: rotation.exp(x), x0, output_atlas=rotation.RotationAtlas) j_analytic = rotation.exp_jacobian(x0) np.testing.assert_array_almost_equal(j_numerical, j_analytic)
def test_displacement_left_jacobian_wrt_lhs(self): np.random.seed(0) r1 = rotation.exp(np.random.randn(3)) r2 = rotation.exp(np.random.randn(3)) j_numerical = numeric_jacobian(lambda r: rotation.displacement_left(r, r2), r1, atlas=rotation.RotationAtlas) j_analytic = rotation.displacement_left_jacobian_wrt_lhs(r1, r2) assert_arrays_almost_equal(j_numerical, j_analytic, xlabel='analytic', ylabel='numerical')
def test_jacobian(self): np.random.seed(0) p = np.random.randn(3) print(homography.exp(np.zeros(8))) j_numerical = numeric_jacobian(lambda x: np.dot(homography.exp(x), p), np.zeros(8)) j_analytic = homography.multiply_generators(p).T np.testing.assert_array_almost_equal(j_numerical, j_analytic)
def test_displacement_left_jacobian_wrt_lhs(self): np.random.seed(0) r1 = rotation.exp(np.random.randn(3)) r2 = rotation.exp(np.random.randn(3)) j_numerical = numeric_jacobian( lambda r: rotation.displacement_left(r, r2), r1, atlas=rotation.RotationAtlas) j_analytic = rotation.displacement_left_jacobian_wrt_lhs(r1, r2) assert_arrays_almost_equal(j_numerical, j_analytic, xlabel='analytic', ylabel='numerical')
def displacement_left_jacobian_wrt_rhs(r1, r2): # TODO: implement analytically from manifolds import numeric_jacobian return numeric_jacobian(lambda q: displacement_left(r1, q), r2, atlas=RotationAtlas)