def test2(self):
# rotate around the z axis by pi/2
P = np.array([0., 0., 1.]) * np.pi/2
v1 = np.array([1.,0,0])
rm = _rot_mat_derivative(P, False)[0]
# rm = rmdrvt(P, False)[0]
v2 = rm.dot(v1)
v2_true = np.array([0.,1,0])
self.assert_array_almost_equal(v2, v2_true)
def test_rot_mat_small_theta(self):
p = np.array([1., 2, 3])
p /= np.linalg.norm(p) * 1e7
print "test_rot_mat1 small_theta"
print p
mx, r1, r2, r3 = _rot_mat_derivative(p, with_grad=True)
print mx
print r1
print r2
print r3
def test_rot_mat1(self):
p = np.array([1., 2, 3]);
p /= np.linalg.norm(p);
print "test_rot_mat1"
print p;
mx, r1, r2, r3 = _rot_mat_derivative(p, with_grad=True)
print mx
print r1
print r2
print r3
def test_rot_mat_small_theta(self):
p = np.array([1., 2, 3])
p /= np.linalg.norm(p) * 1e7
print("test_rot_mat1 small_theta")
print(p)
mx, r1, r2, r3 = _rot_mat_derivative(p, with_grad=True)
print(mx)
print(r1)
print(r2)
print(r3)
def test_rot_mat_small_theta(self):
p = np.array([1., 2, 3])
p /= np.linalg.norm(p) * 1e7
print("test_rot_mat1 small_theta")
print(p)
mx, r1, r2, r3 = _rot_mat_derivative(p, with_grad=True)
print(mx)
print(r1)
print(r2)
print(r3)
def check(self, P, with_grad):
rm, drm1, drm2, drm3 = rmdrvt(P, with_grad)
rmp, drm1p, drm2p, drm3p = _rot_mat_derivative(P, with_grad)
print rm, rmp
print "rm ", rm
print "rmp", rmp
print np.abs(rm - rmp)/np.abs(rm)
self.assert_array_almost_equal(rm, rmp)
self.assert_array_almost_equal(drm1, drm1p, places=4)
self.assert_array_almost_equal(drm2, drm2p, places=4)
self.assert_array_almost_equal(drm3, drm3p, places=4)