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
