def test_copy_so3_inplace(self): R1, R2 = sp.SO3(), sp.SO3(self.Rnp) id_old = id(R1) sp.copyto(R1, R2) id_new = id(R1) self.assertTrue(np.allclose(R1.matrix(), R2.matrix())) self.assertEqual(id_old, id_new)
def test_size_fault(self): with pytest.raises(TypeError) as e: sp.SO3(np.eye(4)) self.assertTrue('incompatible constructor arguments' in str(e.value)) with pytest.raises(TypeError) as e: sp.SO3(np.eye(3).flatten()) self.assertTrue('incompatible constructor arguments' in str(e.value)) with pytest.raises(TypeError) as e: sp.SO3.exp(np.ones(((1, 3)))) self.assertTrue('incompatible function arguments' in str(e.value))
def test_copy_inplace_failure(self): R, T = sp.SO3(), sp.SE3() with pytest.raises(TypeError) as e: sp.copyto(R, T) self.assertTrue('incompatible function arguments' in str(e.value)) with pytest.raises(TypeError) as e: sp.copyto(T, R) self.assertTrue('incompatible function arguments' in str(e.value))
def test_so3_log_map(): _so3_ori = np.random.random(3) / 10 _SO3 = sophus.SO3.exp(_so3_ori).matrix() print "the origin SO3 data is " print _SO3 tic0 = time.clock() _so3 = SO3.log_so3(_SO3) toc0 = time.clock() print "time cost of package ", toc0 - tic0 tic1 = time.clock() _so3_ = sophus.SO3.log(sophus.SO3(_SO3)) toc1 = time.clock() print "time cost of sophus ", toc1 - tic1 print _so3 print _so3_.flatten()
def test_data_type_compatibility(self): R1 = sp.SO3(np.eye(3, dtype=np.float32)) R2 = sp.SO3(np.eye(3, dtype=int)) self.assertTrue(R1, np.eye(3)) self.assertTrue(R2, np.eye(3))
def test_static_exp(self): R = sp.SO3(self.Rnp) R_prime = sp.SO3.exp(R.log()) self.assertTrue(np.allclose(R.matrix(), R_prime.matrix()))
def test_copy_lib(self): R = sp.SO3(self.Rnp) R1 = copy.copy(R) R2 = copy.deepcopy(R) self.assertTrue(np.allclose(R.matrix(), R1.matrix())) self.assertTrue(np.allclose(R.matrix(), R2.matrix()))
def test_copy_method(self): R = sp.SO3(self.Rnp) R_copied = R.copy() self.assertTrue(np.allclose(R.matrix(), R_copied.matrix()))
def test_imul_SO3(self): R1 = sp.SO3() R2 = sp.SO3(self.Rnp) R1 *= R2 self.assertTrue(np.allclose(R1.matrix(), self.Rnp))
def test_log(self): R1 = sp.SO3() R2 = sp.SO3(self.Rnp) self.assertTrue(np.allclose(R1.log().shape, (3,))) self.assertTrue(np.allclose(R1.log(), np.zeros(3))) self.assertTrue(np.allclose(R2.log(), np.array([0.06646925, 1.59563459, 0.04069513])))
def test_inverse(self): R = sp.SO3(self.Rnp) R_inv = R.inverse() self.assertTrue(np.allclose(R_inv.matrix(), self.Rnp.T))
def test_str_representation(self): R = sp.SO3() answer = ('1 0 0\n' '0 1 0\n' '0 0 1') self.assertEqual(str(R), answer)
def test_numpy_constructor(self): R = sp.SO3(self.Rnp) self.assertTrue(np.allclose(R.matrix(), self.Rnp))
def test_copy_constructor(self): R1 = sp.SO3(self.Rnp) R2 = sp.SO3(R1) self.assertTrue(np.allclose(R1.matrix(), R2.matrix()))
def test_default_constructor(self): R = sp.SO3() self.assertTrue(np.allclose(R.matrix(), np.eye(3)))
def test_mul_point(self): R = sp.SO3() pt = R * np.ones(3) self.assertTrue(np.allclose(pt, np.ones(3)))
def test_mul_points(self): R = sp.SO3() pts = R * np.ones((4, 3)) self.assertTrue(np.allclose(pts, np.ones((4, 3))))
import numpy as np import sophus as sp from scipy.spatial.transform import Rotation as R r = R.from_matrix([[0, -1, 0], [1, 0, 0], [0, 0, 1]]) print(type(r.as_quat())) print(r.as_quat()) sp.SO3() ''' SO3([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) ''' T = sp.SE3(np.eye(3), np.arange(3)) print(sp.SE3.log(T)) print(type(T.translation())) R = sp.SO3([[0, 1, 0], [0, 0, 1], [1, 0, 0]]) R1 = sp.SO3([[0, 1, 0], [0, 0, 1], [1, 0, 0]]) print((R * R1).matrix()) print(type(R * R1)) print(type((T * T).matrix())) #print(np.array(R*R1.matrix()).shape)
stream = open(args.yaml, 'r') # stream = open("/media/nvidia/SD/catkin_ws/src/basalt-mirror/data/tis_23/camchain-imucam-2020-08-08-16-00-21.yaml", 'r') f = yaml.load(stream) stream.close() T_c1_c0 = sp.SE3(f['cam1']['T_cn_cnm1']) print('camera 0 in camera 1 transformation:') print(T_c1_c0) print('camera 0 in imu transformation') # assume IMU is in NWU frame and is mounting facing forward # assume the two cameras are mounted forward too. frame right-down-forward R_imu_c0 = sp.SO3([[0, 0, 1], [-1, 0, 0], [0, -1, 0]]) t_imu_c0 = [0.1, 0.1, 0] T_imu_c0 = sp.SE3(R_imu_c0.matrix(), t_imu_c0) print(T_imu_c0) q_imu_c0 = R.from_matrix(R_imu_c0.matrix()).as_quat() T_imu_c1 = T_imu_c0 * T_c1_c0.inverse() print('camera 1 in imu transformation') print(T_imu_c1) t_imu_c1 = T_imu_c1.translation() q_imu_c1 = R.from_matrix(T_imu_c1.rotationMatrix()).as_quat() # Extract cam1 to imu from cam0 to cam1