def test_copy_se3_inplace(self):
T1, T2 = sp.SE3(), sp.SE3(self.Tnp)
id_old = id(T1)
sp.copyto(T1, T2)
id_new = id(T1)
self.assertTrue(np.allclose(T1.matrix(), T2.matrix()))
self.assertEqual(id_old, id_new)
class SE3Object:
# internal storage for left and right camera transformation
T = sophus.SE3() # active rotation from world to the camera
# change of coordinates from NWU to Camera
T_nwu_cam = sophus.SE3([[0, -1, 0, 0], [0, 0, -1, 0], [1, 0, 0, 0],
[0, 0, 0, 1]])
# roll, pitch, yaw (yaw = - heading) in degree
def fromRPYt_deg(self, roll, pitch, yaw, t_vec):
self.fromRPYt_rad(roll * d2r, pitch * d2r, yaw * d2r, t_vec)
def fromRPYt_rad(self, roll, pitch, yaw, t_vec):
R = Rot.from_rotvec([roll, pitch, yaw])
self.T = sophus.SE3(R.as_dcm(), t_vec)
# convert from body (NWU) coordinates to world coordinates
def T_body2world(self):
return self.T
def T_cam2world(self):
return self.T * self.T_nwu_cam.inverse() # T is in NWU frame
# convert from body (NWU) coordinates to world coordinates
def T_world2body(self):
return self.T_body2world().inverse()
def T_world2cam(self):
return self.T_cam2world().inverse()
def test_size_fault(self):
with pytest.raises(TypeError) as e:
sp.SE3(np.eye(3))
self.assertTrue('incompatible constructor arguments' in str(e.value))
with pytest.raises(TypeError) as e:
sp.SE3(np.eye(4).flatten())
self.assertTrue('incompatible constructor arguments' in str(e.value))
with pytest.raises(TypeError) as e:
sp.SE3.hat(np.ones(((1, 6))))
self.assertTrue('incompatible function arguments' in str(e.value))
def __init__(self,
timeStamp=None,
left_image=None,
right_image=None,
depth_image=None,
camera=None):
"""
帧结构，构造时间戳，frameid，左右图像，深度图像，相机
:param timeStamp: 时间戳
:param left_image: 左图像
:param right_image: 右图像
:param depth_image: 深度图 像
:param camera: 金字塔 相机
"""
self.mTimeStamp = timeStamp
self.mLeftImage = left_image
self.mRightImage = right_image
self.mDepthImage = depth_image
self.height = list()
self.width = list()
# 构造图像金字塔，左眼图像，梯度x，y，mag，深度图像
self.numPyrmaid = 4
self.border = 30
self.mLeftPyr = list()
self.mGradXPyr = list()
self.mGradYPyr = list()
self.mGradMagPyr = list()
self.mDepthPyr = list()
# 构造结构数据，2dpoint，3d点，gftt点，梯度点。
self.points = None
self.mPyrPoint = dict()
self.mMapPoints = dict()
self.mFeatures = dict()
# 构造运动数据，相机的位姿，位姿真值
self.mPose2World = sophus.SE3()
self.mGTPose2World = sophus.SE3()
# 标定相关数据
self.GradSize = (40, 20, 10, 5)
self.cam = camera
# 开始进行一些每帧 要做的工作
if left_image is None or depth_image is None or camera is None:
return
# assert isinstance(camera.camera, dict)
self.left_pyr()
def main():
rd = renderer()
Tce = sp.SE3([[0, 1, 0, 0], [-1, 0, 0, 0.3], [0, 0, 1, 0], [0, 0, 0, 1]])
rgb = rd.render_img(0.2, 0.2, Tce, plot_img=True)
p.disconnect(rd.physicsClient)
def test_se3():
_SE3 = SE3.SE3()
_SE3.set_translation([10, 2, 30])
# _SE3.set_translation(np.array([1e-10, 2, 3e-4]))
# _SE3.set_translation([1e-10, 2, 3e-4])
_SE3.set_euler([0.1, 0.2, 0.3])
print _SE3.log()
_SE3.set_axis_angle(np.pi / 3, [1, 0, 0])
print _SE3.translation
print _SE3.rotation_matrix()
print _SE3.log()
print _SE3.exp()
_SE3_ = sophus.SE3()
x, y, z = _SE3.translation
_SE3_ *= _SE3_.trans(x, y, z)
_SE3_ *= _SE3_.rotX(np.pi / 3)
print _SE3_.translation()
print _SE3_.rotationMatrix()
print _SE3_.log().flatten()
print _SE3.log()
print SE3.exp_se3(_SE3_.log().flatten())
print _SE3_.matrix()
def run(self, frm_ref, frm_cur):
"""Computes T for current frame"""
self.reset()
N = len(frm_ref.features)
if not N:
LOG_ERROR('SparseImgAlign: no features to track!')
self._frm_ref = frm_ref
self._frm_cur = frm_cur
self._ref_patch_cache.resize(N, self._patch_area)
self._jacobian_cache.resize(N, self._patch_area, 6)
self._visible_fts = np.full(N, False)
# identity matrix at initial
T_cur_from_ref = sp.SE3(frm_cur.T_from_w * frm_ref.T_from_w.inverse())
for self._level in range(self._max_level, self._min_level - 1, -1):
LOG_INFO("Pyramid level:", self._level)
self._mu = 0.1
self._have_ref_patch_cache = False
self._jacobian_cache.fill(0)
T_cur_from_ref = self.optimize(T_cur_from_ref)
frm_cur.T_from_w = T_cur_from_ref * frm_ref.T_from_w
return self._n_meas / self._patch_area
def _add_world_prior(self, graph, lock_frames, cost_multiplier=1.0):
self._init_frame(graph,
"world",
prefix="",
lock_frames=lock_frames,
cost_multiplier=cost_multiplier)
graph.add_prior("f__world", sp.SE3())
def __init__(self):
self.state = VO.INITIALIZING
self.ref = None
self.cur = None
self.map = Map()
self.num_lost = 0
self.num_inliers = 0
self.Tcr_estimated = sp.SE3()
self.num_of_features = Config.get('number_of_features')
self.scale_factor = Config.get('scale_factor')
self.level_pyramid = Config.get('level_pyramid')
self.match_ratio = Config.get('match_ratio')
self.max_num_lost = Config.get('max_num_lost')
self.min_inliers = Config.get('min_inliers')
self.key_frame_min_rot = Config.get('key_frame_min_rot')
self.key_frame_min_trans = Config.get('key_frame_min_trans')
self.keypoints_cur = []
self.pts_3d_ref = np.zeros((0, 3))
self.descriptors_cur = np.zeros((0, 32))
self.descriptors_ref = np.zeros((0, 32))
self.orb = cv2.ORB_create(nfeatures=self.num_of_features,
scaleFactor=self.scale_factor,
nlevels=self.level_pyramid)
def __init__(self, odometryType):
self.odometryType = odometryType
self.initial_pose = sp.SE3()
self.max_it = 100
self.sobelSize = 3
self.sobelScale = 1.0 / 8.0
self.maxPointsPart = 0.07 #in [0, 1]
self.maxDepthDiff = 0.07 #in meters
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 _optimize_gauss_newton(self, model):
"""optimize by gauss newton method"""
# calculate weight scale
if self._use_weight:
LOG_INFO('Using weight scale')
self._compute_residuals(model, False, True)
old_model = sp.SE3(model)
for i in range(self._n_iter):
self._rho = 0
self._start_iteration()
self._H.fill(0)
self._Jres.fill(0)
# calculate initial residuals
self._n_meas = 0
new_chi2 = self._compute_residuals(model, True, False)
# add prior estimate
if self._have_prior:
self._apply_prior(model)
# calculate linear problem
if not self._solve():
LOG_WARN('Matrix is close to singular! Stop Optimizing.')
LOG_INFO('H =', self._H)
LOG_INFO('Jres =', self._Jres)
self._stop = True
# check if error has increased, roll model back when yes
if (i > 0 and new_chi2 > self._chi2) or self._stop:
LOG_ERROR('Iteration.', i, 'Failure. new_chi2 =', new_chi2,
'Error increased. Stop optimizing.')
model = old_model
break
new_model = self._update(model)
old_model = model
model = new_model
self._chi2 = new_chi2
LOG_INFO('Iteration.', i, 'Success. new_chi2 =', new_chi2,
'n_meas=', self._n_meas, 'x norm=', max(abs(self._x)))
self._finish_iteration()
# stop when converge, step is too small
if max(abs(self._x)) < self._eps:
break
return model
def __init__(self, cam, img, timestamp):
self.cam = cam
self.img = img
self.timestamp = timestamp
self.iskey = False # is key frame or not
self.img_pyr = []
self.id = next(self.id_generator)
self.features = [] # to store all features
self.keypoints = [None] * 5 # to store 5 key Features
self.T_from_w = sp.SE3()
self._init_frame(img)
def test_scene_calibration1(setup_dict):
"""
Assume both cameras can see marker A.
Tests calibration of two cameras in the same workspace.
"""
scene = frt.Scene()
# Setup scene (Model base as a camera and ee as a marker)
scene.add_camera("0", pose_in_frame=sp.SE3())
scene.add_camera("1")
scene.add_frame("ee")
scene.add_marker(2, frame="ee", pose_in_frame=sp.SE3())
# Parse observation data
for t02, t12 in zip(setup_dict["t02_samples"][::-1], setup_dict["t12_samples"][::-1]):
detected_markers = {
"0": [frt.MarkerInfo(id=2, pose=t02, corner=None, length=None)],
"1": [frt.MarkerInfo(id=2, pose=t12, corner=None, length=None)],
}
scene.add_snapshot(detected_markers)
# Calibrate extrinsics
scene.calibrate_extrinsics()
# Check results
t01_inferred = scene.get_camera_info("1")["pose_in_frame"]
t01_gt = setup_dict["t01"]
print(t01_inferred.log() - t01_gt.log())
assert np.allclose(t01_inferred.log(), t01_gt.log(), atol=1e-2)
# Try state estimation
t12_sample = setup_dict["t12_samples"][-1]
detected_markers = {
"1": [frt.MarkerInfo(id=2, pose=t12_sample, corner=None, length=None)],
}
scene.update_pose_estimations(detected_markers)
t02_inferred = scene.get_marker_info(2)["pose"]
t02_gt = setup_dict["t02_samples"][-1]
assert np.allclose(t02_inferred.log(), t02_gt.log(), atol=1e-2)
def test_inverse(self):
T = sp.SE3(self.Tnp)
T_inv = T.inverse()
Tnp_inv = np.eye(4)
Rnp_inv = self.Tnp[:3, :3].T
tnp = self.Tnp[:3, 3]
Tnp_inv[:3, :3] = Rnp_inv
Tnp_inv[:3, 3] = -Rnp_inv.dot(tnp)
self.assertTrue(np.allclose(T_inv.matrix(), Tnp_inv))
def _test_scene_calibration2(setup_dict):
"""
Assume only cameras A can see marker A, and only camera B can see marker B.
Tests a problem similar to hand-eye calibration of manipulators, where
camera A is equivalent to the robot base, and marker A is equivalent to the
end-effector.
TODO: Disabled for now. Bring back when hand-eye calibration is solved.
"""
scene = frt.Scene()
# Setup scene (Model base as a camera and ee as a marker)
scene.add_camera("0", pose_in_frame=sp.SE3())
scene.add_camera("1")
scene.add_frame("ee")
scene.add_marker(2, frame="ee", pose_in_frame=sp.SE3())
scene.add_marker(3, frame="ee")
# Parse observation data
for t02, t13 in zip(setup_dict["t02_samples"], setup_dict["t13_samples"]):
detected_markers = {
"0": [frt.MarkerInfo(id=2, pose=t02, corner=None, length=None)],
"1": [frt.MarkerInfo(id=3, pose=t13, corner=None, length=None)],
}
scene.add_snapshot(detected_markers)
# Calibrate extrinsics
scene.calibrate_extrinsics()
# Check results
t01_inferred = scene.get_camera_info("1")["pose_in_frame"]
t23_inferred = scene.get_marker_info(3)["pose_in_frame"]
t01_gt = setup_dict["t01"]
t23_gt = setup_dict["t23"]
print(t01_inferred.log() - t01_gt.log())
print(t23_inferred.log() - t23_gt.log())
assert np.allclose(t01_inferred.log(), t01_gt.log(), atol=1e-2)
assert np.allclose(t23_inferred.log(), t23_gt.log(), atol=1e-2)
def __init__(self,
nid,
time_stamp=0,
Tcw=None,
camera=None,
color=None,
depth=None):
self.id = nid
self.time_stamp = time_stamp
self.Tcw = Tcw if Tcw is not None else sp.SE3()
self.camera = camera
self.color = color
self.depth = depth
def compute_reproj_g2o(self, srcImage, srcDepth, srcMask, dstImage,
dstDepth, dstMask):
pose_pre = g2o.SE3Quat(sp.SE3().matrix()[:3, :3],
sp.SE3().matrix()[:3, 3])
pose_cur = g2o.SE3Quat(self.initial_pose.matrix()[:3, :3],
self.initial_pose.matrix()[:3, 3])
cam = CameraParameter(self.cameraMatrix[0][0], self.cameraMatrix[1][1],
self.cameraMatrix[0][2], self.cameraMatrix[1][2])
BA = BundleAdjustment()
BA.add_pose(0, pose_pre, cam, fixed=True)
BA.add_pose(1, pose_cur, cam, fixed=False)
points2D_pre, points2D_cur, kp_pre, kp_cur = self.process_feature(
srcImage, dstImage)
points3D = []
for i, j in enumerate(points2D_pre):
row = int(kp_pre[i].pt[1])
col = int(kp_pre[i].pt[0])
z = srcDepth[row][col]
p3d_x = (j[0] - cx) * z / fx
p3d_y = (j[1] - cy) * z / fy
p3d = np.array([p3d_x, p3d_y, z])
points3D.append(p3d)
points3D = np.array(points3D)
for i in range(np.shape(points3D)[0]):
p3d = points3D[i]
if (np.isnan(p3d[2]) == False):
print(p3d)
BA.add_point(i, points3D[i], fixed=True)
BA.add_edge(i, 0, i, points2D_pre[i])
BA.add_edge(i, 1, i + 1, points2D_cur[i])
BA.optimize(max_iterations=self.max_it)
BA_pose = sp.SE3(BA.get_pose(1).to_homogeneous_matrix())
self.initial_pose = BA_pose
return BA_pose.matrix()
def detect_markers(self, img):
# Detect markers in img
corners, ids, rejected_candidates = cv2.aruco.detectMarkers(
img, dictionary=self.dictionary, parameters=self.parameters)
# Return empty list if no marker found
if ids is None:
return []
# Estimate pose of detected markers
num_markers = len(corners)
ids = ids.squeeze(-1)
if self.intrinsics is None:
print(
"Warning: Intrinsics not set in CameraModule. Pose estimation of markers unavailble."
)
else:
poses = []
lengths = []
for id, corner in zip(ids, corners):
# No pose estimation if marker id not registered
if id not in self.registered_markers:
poses.append(None)
lengths.append(None)
continue
# Pose estimation using registered length
length = self.registered_markers[id]
rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers(
corner,
length,
self._intrinsics2matrix(self.intrinsics),
self.intrinsics.coeffs,
)
r = rvecs[0].squeeze()
t = tvecs[0].squeeze()
pose = sp.SE3(sp.SO3.exp(r).matrix(), t)
lengths.append(length)
poses.append(pose)
# Output
markers = []
for id, corner, length, pose in zip(ids, corners, lengths, poses):
marker = MarkerInfo(id, corner.squeeze(), length, pose)
markers.append(marker)
return markers
def test_se3_log_map():
print "testing SE3 log map"
_se3_ori = np.random.random(6) / 10
_SE3 = sophus.SE3.exp(_se3_ori).matrix()
print "the origin SE3 data is "
print _SE3
tic0 = time.clock()
_se3 = SE3.log_se3(_SE3)
toc0 = time.clock()
print "time cost of package ", toc0 - tic0
tic1 = time.clock()
_SE3_ = sophus.SE3(_SE3)
_se3_ = sophus.SE3.log(_SE3_)
toc1 = time.clock()
print "time cost of sophus ", toc1 - tic1
print _se3, type(_se3)
print _se3_.flatten(), type(_se3_)
def init_variable(self, name, pose=sp.SE3()):
pose_gt = sophus2gtsam(pose)
# Update pose only if already exists
if name in self.vars:
var = self.vars[name]
self.values.update(var, pose_gt)
return
# Create symbol
var = gtsam.symbol_shorthand.X(self.n_variables)
self.vars[name] = var
self.n_variables += 1
# Add to values
self.values.insert(var, pose_gt)
# Add to edges
self.factor_edges[var] = []
def __init__(self, camera_noise=None, calib_noise=None):
# Initialize data containers
self._frames = {}
self._objects = {}
self._snapshots = []
# Noise
if camera_noise is None:
self._camera_noise = np.array(DEFAULT_CAMERA_NOISE)
else:
assert len(camera_noise) == 6, "Invalid noise vector dimensions."
self._camera_noise = np.array(camera_noise)
if calib_noise is None:
self._calib_noise = np.array(DEFAULT_CALIB_NOISE)
else:
assert len(calib_noise) == 6, "Invalid noise vector dimensions."
self._calib_noise = np.array(calib_noise)
# Default world frame
f0 = Frame("world", sp.SE3())
self._frames["world"] = f0
def _add_object(self, name, obj_type, frame, pose_in_frame, size):
# Parse input
assert name not in self._objects.keys(
), f"Object name already exists: {name}"
assert frame in self._frames.keys(), f"Unknown frame: {frame}"
is_anchor = pose_in_frame is not None
pose_in_frame = sp.SE3() if pose_in_frame is None else pose_in_frame
pose = self._frames[frame].pose * pose_in_frame
# Add to data
obj = Object(
name,
obj_type,
frame,
pose,
pose_in_frame=pose_in_frame,
size=size,
is_anchor=is_anchor,
)
self._objects[name] = obj
self._frames[frame].objects.append(name)
def add_frame(self, name, pose=None):
pose = sp.SE3() if pose is None else pose
f = Frame(name, pose)
self._frames[name] = f
def pbvs(self,
arm,
goal_pos,
goal_rot,
kv=1.3,
kw=0.8,
eps_pos=0.005,
eps_rot=0.05,
vs_rate=20,
plot_pose=False,
print_err=False,
perturb_jacobian=False,
perturb_Jac_joint_mu=0.1,
perturb_Jac_joint_sigma=0.1,
perturb_orientation=False,
mu_R=0.4,
sigma_R=0.4,
perturb_motion_R_mu=0.0,
perturb_motion_R_sigma=0.0,
perturb_motion_t_mu=0.0,
perturb_motion_t_sigma=0.0,
plot_result=False,
pf_mode=False):
# Initialize variables
self.perturb_Jac_joint_mu = perturb_Jac_joint_mu
self.perturb_Jac_joint_sigma = perturb_Jac_joint_sigma
self.perturb_R_mu = mu_R
self.perturb_R_sigma = sigma_R
if arm == "right":
tool = self.right_tool
elif arm == "left":
tool = self.left_tool
else:
print('''arm incorrect, input "left" or "right" ''')
return
# Initialize pos_plot_id for later use
if pf_mode:
cur_pos, cur_rot = SE32_trans_rot(self.cur_pose_est)
else:
cur_pos, cur_rot = SE32_trans_rot(
self.Trc.inverse() * get_pose(self.robot, tool, SE3=True))
if plot_pose:
pos_plot_id = self.draw_pose_cam(trans_rot2SE3(cur_pos, cur_rot))
# record end effector pose and time stamp at each iteration
if plot_result:
start = time.time()
times = []
eef_pos = []
eef_rot = []
if pf_mode:
motion = sp.SE3()
##### Control loop starts #####
while True:
# If using particle filter and the hog computation has finished, exit pbvs control
# and assign total estimated motion in eef frame
if pf_mode and self.shared_pf_fin.value == 1:
# eef Motion estimated from pose estimate of last iteration and current fk
motion_truth = (self.Trc *
self.cur_pose_est).inverse() * get_pose(
self.robot, tool, SE3=True)
# print("motion truth:", motion_truth)
dR = sp.SO3.exp(
np.random.normal(perturb_motion_R_mu,
perturb_motion_R_sigma, 3)).matrix()
dt = np.random.normal(perturb_motion_t_mu,
perturb_motion_t_sigma, 3)
# self.draw_pose_cam(motion)
self.motion = motion_truth * sp.SE3(dR, dt)
return
# get current eef pose in camera frame
if pf_mode:
cur_pos, cur_rot = SE32_trans_rot(self.cur_pose_est)
else:
cur_pos, cur_rot = SE32_trans_rot(
self.Trc.inverse() * get_pose(self.robot, tool, SE3=True))
if plot_pose:
self.draw_pose_cam(trans_rot2SE3(cur_pos, cur_rot),
uids=pos_plot_id)
if plot_result:
eef_pos.append(cur_pos)
eef_rot.append(cur_rot)
times.append(time.time() - start)
cur_pos_inv, cur_rot_inv = p.invertTransform(cur_pos, cur_rot)
# Pose of goal in camera frame
pos_cg, rot_cg = p.multiplyTransforms(cur_pos_inv, cur_rot_inv,
goal_pos, goal_rot)
# Evaluate current translational and rotational error
err_pos = np.linalg.norm(pos_cg)
err_rot = np.linalg.norm(p.getAxisAngleFromQuaternion(rot_cg)[1])
if err_pos < eps_pos and err_rot < eps_rot:
p.removeAllUserDebugItems()
break
else:
if print_err:
print("Error to goal, position: {:2f}, orientation: {:2f}".
format(err_pos, err_rot))
Rsc = np.array(p.getMatrixFromQuaternion(cur_rot)).reshape(3, 3)
# Perturb Rsc in SO(3) by a random variable in tangent space so(3)
if perturb_orientation:
dR = sp.SO3.exp(
np.random.normal(self.perturb_R_mu, self.perturb_R_sigma,
3))
Rsc = Rsc.dot(dR.matrix())
twist_local = np.hstack(
(np.array(pos_cg) * kv,
np.array(quat2se3(rot_cg)) * kw)).reshape(6, 1)
local2global = np.block([[Rsc, np.zeros((3, 3))],
[np.zeros((3, 3)), Rsc]])
twist_global = local2global.dot(twist_local)
start_loop = time.time()
self.cartesian_vel_control(arm,
np.asarray(twist_global),
1 / vs_rate,
perturb_jacobian=perturb_jacobian)
if pf_mode:
delta_t = time.time() - start_loop
motion = motion * sp.SE3.exp(twist_local * delta_t)
# self.draw_pose_cam(motion)
self.goal_reached = True
print("PBVS goal achieved!")
if plot_result:
eef_pos = np.array(eef_pos)
eef_rot_rpy = np.array(
[p.getEulerFromQuaternion(quat) for quat in eef_rot])
fig, axs = plt.subplots(3, 2)
sub_titles = [['x', 'roll'], ['y', 'pitch'], ['z', 'yaw']]
fig.suptitle(
"Position Based Visual Servo End Effector Pose - time plot")
for i in range(3):
axs[i, 0].plot(times, eef_pos[:, i] * 100)
axs[i, 0].plot(times, goal_pos[i] * np.ones(len(times)) * 100)
axs[i, 0].legend([sub_titles[i][0], 'goal'])
axs[i, 0].set_xlabel('Time(s)')
axs[i, 0].set_ylabel('cm')
goal_rpy = p.getEulerFromQuaternion(goal_rot)
for i in range(3):
axs[i, 1].plot(times, eef_rot_rpy[:, i] * 180 / np.pi)
axs[i, 1].plot(times,
goal_rpy[i] * np.ones(len(times)) * 180 / np.pi)
axs[i, 1].legend([sub_titles[i][1], 'goal'])
axs[i, 1].set_xlabel('Time(s)')
axs[i, 1].set_ylabel('deg')
for ax in axs.flat:
ax.set(xlabel='time')
plt.show()
def fromRPYt_rad(self, roll, pitch, yaw, t_vec):
R = Rot.from_rotvec([roll, pitch, yaw])
self.T = sophus.SE3(R.as_dcm(), t_vec)
def trans_rot2SE3(trans, rot):
rotm = np.array(p.getMatrixFromQuaternion(rot)).reshape(3, 3)
return sp.SE3(rotm, np.array(trans))
def gtsam2sophus(pose):
return sp.SE3(pose.matrix())
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)
def set_gt_pose(self, pose2world):
assert pose2world.shape == (4, 4)
self.mGTPose2World = sophus.SE3(pose2world.T.flatten())
