def poses(self):
from liegroups import SE3
T_cam_w_true = [
SE3.identity(),
SE3.exp(0.1 * np.ones(6)),
SE3.exp(0.2 * np.ones(6)),
SE3.exp(0.3 * np.ones(6))
]
return T_cam_w_true
def test_jacobians_se3(self, se3_residual):
T1 = SE3.exp([1, 2, 3, 4, 5, 6])
T2 = SE3.exp([7, 8, 9, 10, 11, 12])
_, jac1 = se3_residual.evaluate([T1, T2],
compute_jacobians=[True, True])
_, jac2 = se3_residual.evaluate([T1, T2],
compute_jacobians=[True, False])
_, jac3 = se3_residual.evaluate([T1, T2],
compute_jacobians=[False, True])
assert len(jac1) == len(jac2) == len(jac3) == 2
assert jac1[0].shape == (6, 6) and jac1[1].shape == (6, 6)
assert jac2[0].shape == (6, 6) and jac2[1] is None
assert jac3[0] is None and jac3[1].shape == (6, 6)
def compute_target(self, idx, target_idx, source_idx):
## Compute ts
# dt = np.abs(self.ts_samples[idx,target_idx] - self.ts_samples[idx, source_idx])
dt = self.ts_samples[idx, target_idx] - self.ts_samples[idx,
source_idx]
#compute Tau_gt
T2 = SE3.from_matrix(self.gt_samples[idx, target_idx, :, :],
normalize=True).inv()
T1 = SE3.from_matrix(self.gt_samples[idx, source_idx, :, :],
normalize=True)
dT_gt = T2.dot(
T1
) #pose change from source to a target (for reconstructing source from target)
gt_lie_alg = dT_gt.log()
#compute Tau_vo
T2 = SE3.from_matrix(self.vo_samples[idx, target_idx, :, :],
normalize=True).inv()
T1 = SE3.from_matrix(self.vo_samples[idx, source_idx, :, :],
normalize=True)
dT_vo = T2.dot(T1)
vo_lie_alg = dT_vo.log()
if self.config['estimator_type'] == 'mono':
if np.linalg.norm(vo_lie_alg[0:3]) >= 1e-8:
scale = np.linalg.norm(gt_lie_alg[0:3]) / np.linalg.norm(
vo_lie_alg[0:3])
scale = 1
vo_lie_alg[0:3] = scale * vo_lie_alg[0:3]
dT_vo = SE3.exp(vo_lie_alg)
gt_correction = (dT_gt.dot(dT_vo.inv())).log()
return gt_lie_alg, vo_lie_alg, gt_correction, dt
def test_se3(self, options):
from liegroups import SE3
from pyslam.residuals import PoseResidual, PoseToPoseResidual
problem = Problem(options)
odom = SE3.exp(0.1 * np.ones(6))
odom_stiffness = invsqrt(1e-3 * np.eye(SE3.dof))
T0_stiffness = invsqrt(1e-6 * np.eye(SE3.dof))
odom_covar = np.linalg.inv(np.dot(odom_stiffness, odom_stiffness))
T0_covar = np.linalg.inv(np.dot(T0_stiffness, T0_stiffness))
residual0 = PoseResidual(SE3.identity(), T0_stiffness)
residual1 = PoseToPoseResidual(odom, odom_stiffness)
params_init = {'T0': SE3.identity(), 'T1': SE3.identity()}
problem.add_residual_block(residual0, 'T0')
problem.add_residual_block(residual1, ['T0', 'T1'])
problem.initialize_params(params_init)
problem.solve()
problem.compute_covariance()
estimated_covar = problem.get_covariance_block('T1', 'T1')
expected_covar = odom_covar + odom.adjoint().dot(T0_covar.dot(odom.adjoint().T))
assert np.allclose(estimated_covar, expected_covar)
def test_evaluate_stereo(self, stereo_residual):
T_cam_w = SE3.exp([1, 2, 3, 4, 5, 6])
pt_good_w = T_cam_w.inv().dot(
stereo_residual.camera.triangulate(stereo_residual.obs))
pt_bad_w = pt_good_w + [1, 1, 1]
assert np.allclose(stereo_residual.evaluate([T_cam_w, pt_good_w]),
np.zeros(3))
assert not np.allclose(stereo_residual.evaluate([T_cam_w, pt_bad_w]),
np.zeros(3))
def evolve_world_pose_tractor(
self,
world_pose_tractor: SE3,
v_left: float,
v_right: float,
delta_t: float,
) -> SE3:
v, w = self.wheel_velocity_to_unicycle(v_left, v_right)
tractor_pose_t = SE3.exp([v * delta_t, 0, 0, 0, 0, w * delta_t])
return world_pose_tractor.dot(tractor_pose_t)
def __init__(self, event_loop):
self.event_loop = event_loop
self.command_rate_hz = 50
self.command_period_seconds = 1.0 / self.command_rate_hz
self.event_bus = get_event_bus('farm_ng.pose_vis_toy')
self.odom_pose_tractor = SE3.identity()
self.tractor_pose_wheel_left = SE3.exp(
(0.0, 1.0, 0, 0, 0,
0)).dot(SE3.exp((0.0, 0.0, 0, -np.pi / 2, 0, 0)))
self.tractor_pose_wheel_right = SE3.exp(
(0.0, -1.0, 0, 0, 0,
0)).dot(SE3.exp((0.0, 0.0, 0, -np.pi / 2, 0, 0)))
self.kinematic_model = TractorKinematics(default_config())
self.control_timer = Periodic(
self.command_period_seconds,
self.event_loop,
self._command_loop,
name='control_loop',
)
def _command_loop(self, n_periods):
pose_msg = NamedSE3Pose()
left_speed = 1.0
right_speed = 0.5
self.odom_pose_tractor = self.kinematic_model.evolve_world_pose_tractor(
self.odom_pose_tractor,
left_speed,
right_speed,
n_periods * self.command_period_seconds,
)
pose_msg.a_pose_b.CopyFrom(se3_to_proto(self.odom_pose_tractor))
pose_msg.frame_a = 'odometry/wheel'
pose_msg.frame_b = 'tractor/base'
self.event_bus.send(make_event('pose/tractor/base', pose_msg))
pose_msg = NamedSE3Pose()
radius = self.kinematic_model.wheel_radius
self.tractor_pose_wheel_left = self.tractor_pose_wheel_left.dot(
SE3.exp((0, 0, 0, 0, 0, left_speed / radius *
self.command_period_seconds * n_periods)))
pose_msg.a_pose_b.CopyFrom(se3_to_proto(self.tractor_pose_wheel_left))
pose_msg.frame_a = 'tractor/base'
pose_msg.frame_b = 'tractor/wheel/left'
self.event_bus.send(make_event('pose/tractor/wheel/left', pose_msg))
pose_msg = NamedSE3Pose()
self.tractor_pose_wheel_right = self.tractor_pose_wheel_right.dot(
SE3.exp((0, 0, 0, 0, 0, right_speed / radius *
self.command_period_seconds * n_periods)))
pose_msg.a_pose_b.CopyFrom(se3_to_proto(self.tractor_pose_wheel_right))
pose_msg.frame_a = 'tractor/base'
pose_msg.frame_b = 'tractor/wheel/right'
self.event_bus.send(make_event('pose/tractor/wheel/right', pose_msg))
pose_msg = NamedSE3Pose()
pose_msg.a_pose_b.position.z = 1.0
pose_msg.a_pose_b.position.y = 0.5
pose_msg.frame_a = 'tractor/base'
pose_msg.frame_b = 'tractor/camera'
self.event_bus.send(make_event('pose/tractor/camera', pose_msg))
def MakeApriltagRigNode(rig_name, tag_id, size, x, y):
node = ApriltagRig.Node()
node.id = tag_id
node.frame_name = '%s/tag/%05d' % (rig_name, tag_id)
node.tag_size = size
half_size = size / 2.0
node.points_tag.append(Vec3(x=-half_size, y=-half_size, z=0))
node.points_tag.append(Vec3(x=half_size, y=-half_size, z=0))
node.points_tag.append(Vec3(x=half_size, y=half_size, z=0))
node.points_tag.append(Vec3(x=-half_size, y=half_size, z=0))
node.pose.frame_a = rig_name
node.pose.frame_b = node.frame_name
node.pose.a_pose_b.CopyFrom(se3_to_proto(SE3.exp([x, y, 0, 0, 0, 0])))
return node
def test_jacobians_stereo(self, stereo_residual):
T_cam_w = SE3.exp([1, 2, 3, 4, 5, 6])
pt_w = T_cam_w.inv().dot(
stereo_residual.camera.triangulate(stereo_residual.obs))
_, jac1 = stereo_residual.evaluate([T_cam_w, pt_w],
compute_jacobians=[True, True])
_, jac2 = stereo_residual.evaluate([T_cam_w, pt_w],
compute_jacobians=[True, False])
_, jac3 = stereo_residual.evaluate([T_cam_w, pt_w],
compute_jacobians=[False, True])
assert len(jac1) == len(jac2) == len(jac3) == 2
assert jac1[0].shape == (3, 6) and jac1[1].shape == (3, 3)
assert jac2[0].shape == (3, 6) and jac2[1] is None
assert jac3[0] is None and jac3[1].shape == (3, 3)
def compute_trajectory(pose_vec, gt_traj, method='odom'):
est_traj = [gt_traj[0]]
cum_dist = [0]
for i in range(0, pose_vec.shape[0]):
#classically estimated traj
dT = SE3.exp(pose_vec[i])
new_est = SE3.as_matrix(
(dT.dot(SE3.from_matrix(est_traj[i], normalize=True).inv())).inv())
est_traj.append(new_est)
cum_dist.append(cum_dist[i] + np.linalg.norm(dT.trans))
gt_traj_se3 = [SE3.from_matrix(T, normalize=True) for T in gt_traj]
est_traj_se3 = [SE3.from_matrix(T, normalize=True) for T in est_traj]
tm_est = TrajectoryMetrics(gt_traj_se3, est_traj_se3, convention='Twv')
est_mean_trans, est_mean_rot = tm_est.mean_err()
est_mean_rot = (est_mean_rot * 180 / np.pi).round(3)
est_mean_trans = est_mean_trans.round(3)
seg_lengths = list(range(100, 801, 100))
_, seg_errs_est = tm_est.segment_errors(seg_lengths, rot_unit='rad')
print('trans. rel. err: {}, rot. rel. err: {}'.format(
np.mean(tm_est.rel_errors()[0]), np.mean(tm_est.rel_errors()[1])))
rot_seg_err = (100 * np.mean(seg_errs_est[:, 2]) * 180 / np.pi).round(3)
trans_seg_err = (np.mean(seg_errs_est[:, 1]) * 100).round(3)
if np.isnan(trans_seg_err):
max_dist = cum_dist[-1] - cum_dist[-1] % 100 + 1 - 100
print('max dist', max_dist)
seg_lengths = list(range(100, int(max_dist), 100))
_, seg_errs_est = tm_est.segment_errors(seg_lengths, rot_unit='rad')
rot_seg_err = (100 * np.mean(seg_errs_est[:, 2]) * 180 /
np.pi).round(3)
trans_seg_err = (np.mean(seg_errs_est[:, 1]) * 100).round(3)
print("{} mean trans. error: {} | mean rot. error: {}".format(
method, est_mean_trans, est_mean_rot))
print("{} mean Segment Errors: {} (trans, %) | {} (rot, deg/100m)".format(
method, trans_seg_err, rot_seg_err))
errors = (est_mean_trans, est_mean_rot, trans_seg_err, rot_seg_err)
return np.array(est_traj), np.array(gt_traj), errors, np.array(cum_dist)
def compute_tractor_pose_delta(self, v_left: float, v_right: float,
delta_t: float) -> SE3:
v, w = self.wheel_velocity_to_unicycle(v_left, v_right)
tractor_pose_delta = SE3.exp([v * delta_t, 0, 0, 0, 0, w * delta_t])
return tractor_pose_delta
def optimize_transformation(
previous_ids: np.ndarray, previous_points: np.ndarray,
current_ids: np.ndarray,
current_points: np.ndarray) -> Tuple[Quaternion, np.ndarray]:
common_points = set(previous_ids).intersection(set(current_ids))
if len(common_points) < 3:
print(
'WARNING: Fewer than 3 common points. Unable to find transfomration'
)
return Quaternion(), np.zeros(3)
previous_points_filter = np.array([
i for i in range(len(previous_ids)) if previous_ids[i] in common_points
])
current_points_filter = np.array([
i for i in range(len(current_ids)) if current_ids[i] in common_points
])
previous_points = previous_points[:, previous_points_filter]
current_points = current_points[:, current_points_filter]
_, n_points = previous_points.shape
xi = np.zeros(6)
plot_points(previous_points, current_points)
weights = np.ones(len(common_points))
for attempt in range(5):
for iteration in range(10):
A = np.zeros((3 * n_points, 6))
b = np.zeros(3 * n_points)
T = SE3.exp(xi)
R = T.rot.as_matrix()
t = T.trans
iteration_points = R @ previous_points + t[:, np.newaxis]
errors = np.sqrt(
np.sum(np.square(current_points - iteration_points), axis=0))
k = 1.345 * np.std(errors)
huber_weights = np.array(
list(map(lambda x: 1.0
if x <= k else k / abs(x), errors))) * weights
huber_weights = np.repeat(huber_weights, 3)
W = np.diag(huber_weights)
for i in range(n_points):
x1, x2, x3 = iteration_points[:, i]
A[i * 3:i * 3 + 3, :] = np.array([[1, 0, 0, 0, x3, -x2],
[0, 1, 0, -x3, 0, x1],
[0, 0, 1, x2, -x1, 0]])
b[i * 3:i * 3 +
3] = current_points[:, i] - iteration_points[:, i]
AA = A.T @ W @ A
AA_inv = np.linalg.inv(AA)
xi_delta = AA_inv @ A.T @ W @ b
xi = SE3.log(SE3.exp(xi) * SE3.exp(xi_delta))
if np.linalg.norm(xi_delta) < 1e-8:
break
for i in range(len(common_points)):
weights[i] = 0 if errors[i] > np.average(
errors) + 1 * np.std(errors) else 1
print(weights)
T = SE3.exp(xi)
R = T.rot.as_matrix()
t = T.trans
print(
f'error: {np.max(errors)}, {np.min(errors)}, {np.average(errors)}, {np.std(errors)}'
)
plot_points(iteration_points, current_points)
return Quaternion(matrix=R), t
# Loop closure T_6_2_obs = T_6_0_true.dot(T_2_0_true.inv()) # Random start # T_1_0_init = SE3.exp(0.1 * 2 * (np.random.rand(6) - 0.5)) * T_1_0_true # T_2_0_init = SE3.exp(0.1 * 2 * (np.random.rand(6) - 0.5)) * T_2_0_true # T_3_0_init = SE3.exp(0.1 * 2 * (np.random.rand(6) - 0.5)) * T_3_0_true # T_4_0_init = SE3.exp(0.1 * 2 * (np.random.rand(6) - 0.5)) * T_4_0_true # T_5_0_init = SE3.exp(0.1 * 2 * (np.random.rand(6) - 0.5)) * T_5_0_true # T_6_0_init = SE3.exp(0.1 * 2 * (np.random.rand(6) - 0.5)) * T_6_0_true # Constant wrong start offset1 = np.array([-0.1, 0.1, -0.1, 0.1, -0.1, 0.1]) offset2 = np.array([0.1, -0.1, 0.1, -0.1, 0.1, -0.1]) T_1_0_init = SE3.exp(offset2).dot(T_1_0_true) T_2_0_init = SE3.exp(offset1).dot(T_2_0_true) T_3_0_init = SE3.exp(offset2).dot(T_3_0_true) T_4_0_init = SE3.exp(offset1).dot(T_4_0_true) T_5_0_init = SE3.exp(offset2).dot(T_5_0_true) T_6_0_init = SE3.exp(offset1).dot(T_6_0_true) # Either we need a prior on the first pose, or it needs to be held constant # so that the resulting system of linear equations is solveable prior_stiffness = invsqrt(1e-12 * np.identity(6)) odom_stiffness = invsqrt(1e-3 * np.identity(6)) loop_stiffness = invsqrt(1. * np.identity(6)) residual0 = PoseResidual(T_1_0_obs, prior_stiffness) residual0_params = ['T_1_0']
gt_traj = matfile['gt_traj'].item()
odom_pose_vec = matfile['odom_pose_vecs'].item(
) #original odometry 6x1 vecs (seq_size-1 x 6)
corr_pose_vec = matfile['corr_pose_vecs'].item(
) # corrected 6x1 vecs (seq_size-1 x 6)
if estimator == 'stereo': #use rotation corrections only.
corr_pose_vec[:, :, 0:3] = odom_pose_vec[:, 0:3]
best_loop_closure_pose_vec = corr_pose_vec[best_loop_closure_epoch]
est_traj, avg_corr_traj, dense_traj, dense_gt = [], [], [], []
est_traj.append(gt_traj[0])
avg_corr_traj.append(gt_traj[0])
for i in range(0, odom_pose_vec.shape[0]):
#classically estimated traj
dT = SE3.exp(odom_pose_vec[i])
new_est = SE3.as_matrix(
(dT.dot(SE3.from_matrix(est_traj[i], normalize=True).inv())).inv())
est_traj.append(new_est)
#averaging corrections before fusing with S-VO
if mode == 'offline':
dT_corr = SE3.exp(avg_corrections[i]).dot(SE3.exp(
odom_pose_vec[i]))
if mode == 'online':
dT_corr = SE3.exp(best_loop_closure_pose_vec[i])
new_est = SE3.as_matrix((dT_corr.dot(
SE3.from_matrix(avg_corr_traj[i], normalize=True).inv())).inv())
avg_corr_traj.append(new_est)
est_tm, est_metrics = generate_trajectory_metrics(gt_traj,
def test_evaluate_se3(self, se3_residual):
T1 = SE3.exp([1, 2, 3, 4, 5, 6])
T2 = SE3.exp([7, 8, 9, 10, 11, 12])
assert np.allclose(se3_residual.evaluate([T1, T1]), np.zeros(6))
assert not np.allclose(se3_residual.evaluate([T1, T2]), np.zeros(6))
def test_jacobians_se3(self, se3_residual):
T_test = SE3.exp([7, 8, 9, 10, 11, 12])
_, jacobians = se3_residual.evaluate([T_test],
compute_jacobians=[True])
assert len(jacobians) == 1 and jacobians[0].shape == (6, 6)
def test_evaluate_se3(self, se3_residual):
T_same = se3_residual.T_obs
T_diff = SE3.exp([7, 8, 9, 10, 11, 12])
assert np.allclose(se3_residual.evaluate([T_same]), np.zeros(6))
assert not np.allclose(se3_residual.evaluate([T_diff]), np.zeros(6))
def test_trajectory(device, pose_model, spatial_trans, dset, epoch):
pose_model.train(False) # Set model to evaluate mode
pose_model.eval() #used for batch normalization # Set model to training mode
spatial_trans.train(False)
spatial_trans.eval()
#initialize the relevant outputs
full_corr_lie_alg_stacked, rot_corr_lie_alg_stacked, gt_lie_alg_stacked, vo_lie_alg_stacked, corrections_stacked, gt_corrections_stacked= \
np.empty((0,6)), np.empty((0,6)), np.empty((0,6)), np.empty((0,6)), np.empty((0,6)), np.empty((0,6))
for data in dset:
imgs, gt_lie_alg, intrinsics, vo_lie_alg, gt_correction = data
gt_lie_alg = gt_lie_alg.type(torch.FloatTensor).to(device)
vo_lie_alg = vo_lie_alg.type(torch.FloatTensor).to(device)
img_list = []
for im in imgs:
img_list.append(im.to(device))
corr, exp_mask, disp = pose_model(img_list[0:3], vo_lie_alg)
exp_mask, disp = exp_mask[0], disp[0][:,0]
corr_rot = torch.clone(corr)
corr_rot[:,0:3]=0
corrected_pose = se3_log_exp(corr, vo_lie_alg)
corrected_pose_rot_only = se3_log_exp(corr_rot, vo_lie_alg)
corrections_stacked = np.vstack((corrections_stacked, corr.cpu().detach().numpy()))
gt_corrections_stacked = np.vstack((gt_corrections_stacked, gt_correction.cpu().detach().numpy()))
full_corr_lie_alg_stacked = np.vstack((full_corr_lie_alg_stacked, corrected_pose.cpu().detach().numpy()))
rot_corr_lie_alg_stacked = np.vstack((rot_corr_lie_alg_stacked, corrected_pose_rot_only.cpu().detach().numpy()))
gt_lie_alg_stacked = np.vstack((gt_lie_alg_stacked, gt_lie_alg.cpu().detach().numpy()))
vo_lie_alg_stacked = np.vstack((vo_lie_alg_stacked, vo_lie_alg.cpu().detach().numpy()))
est_traj, corr_traj, corr_traj_rot, gt_traj = [],[],[],[]
gt_traj = dset.dataset.raw_gt_trials[0]
est_traj.append(gt_traj[0])
corr_traj.append(gt_traj[0])
corr_traj_rot.append(gt_traj[0])
cum_dist = [0]
for i in range(0,full_corr_lie_alg_stacked.shape[0]):
#classically estimated traj
dT = SE3.exp(vo_lie_alg_stacked[i])
new_est = SE3.as_matrix((dT.dot(SE3.from_matrix(est_traj[i],normalize=True).inv())).inv())
est_traj.append(new_est)
cum_dist.append(cum_dist[i]+np.linalg.norm(dT.trans))
#corrected traj (rotation only)
dT = SE3.exp(rot_corr_lie_alg_stacked[i])
new_est = SE3.as_matrix((dT.dot(SE3.from_matrix(corr_traj_rot[i],normalize=True).inv())).inv())
corr_traj_rot.append(new_est)
#
#
# #corrected traj (full pose)
dT = SE3.exp(full_corr_lie_alg_stacked[i])
new_est = SE3.as_matrix((dT.dot(SE3.from_matrix(corr_traj[i],normalize=True).inv())).inv())
corr_traj.append(new_est)
gt_traj_se3 = [SE3.from_matrix(T,normalize=True) for T in gt_traj]
est_traj_se3 = [SE3.from_matrix(T,normalize=True) for T in est_traj]
corr_traj_se3 = [SE3.from_matrix(T,normalize=True) for T in corr_traj]
corr_traj_rot_se3 = [SE3.from_matrix(T,normalize=True) for T in corr_traj_rot]
tm_est = TrajectoryMetrics(gt_traj_se3, est_traj_se3, convention = 'Twv')
tm_corr = TrajectoryMetrics(gt_traj_se3, corr_traj_se3, convention = 'Twv')
tm_corr_rot = TrajectoryMetrics(gt_traj_se3, corr_traj_rot_se3, convention = 'Twv')
if epoch >= 0:
est_mean_trans, est_mean_rot = tm_est.mean_err()
corr_mean_trans, corr_mean_rot = tm_corr.mean_err()
corr_rot_mean_trans, corr_rot_mean_rot = tm_corr_rot.mean_err()
print("Odom. mean trans. error: {} | mean rot. error: {}".format(est_mean_trans, est_mean_rot*180/np.pi))
print("Corr. mean trans. error: {} | mean rot. error: {}".format(corr_mean_trans, corr_mean_rot*180/np.pi))
print("Corr. (rot. only) mean trans. error: {} | mean rot. error: {}".format(corr_rot_mean_trans, corr_rot_mean_rot*180/np.pi))
seg_lengths = list(range(100,801,100))
_, seg_errs_est = tm_est.segment_errors(seg_lengths, rot_unit='rad')
_, seg_errs_corr = tm_corr.segment_errors(seg_lengths, rot_unit='rad')
_, seg_errs_corr_rot = tm_corr_rot.segment_errors(seg_lengths, rot_unit='rad')
print("Odom. mean Segment Errors: {} (trans, %) | {} (rot, deg/100m)".format(np.mean(seg_errs_est[:,1])*100, 100*np.mean(seg_errs_est[:,2])*180/np.pi))
print("Corr. mean Segment Errors: {} (trans, %) | {} (rot, deg/100m)".format(np.mean(seg_errs_corr[:,1])*100, 100*np.mean(seg_errs_corr[:,2])*180/np.pi))
print("Corr. (rot. only) mean Segment Errors: {} (trans, %) | {} (rot, deg/100m)".format(np.mean(seg_errs_corr_rot[:,1])*100, 100*np.mean(seg_errs_corr_rot[:,2])*180/np.pi))
rot_seg_err = 100*np.mean(seg_errs_corr_rot[:,2])*180/np.pi
return corrections_stacked, gt_corrections_stacked, full_corr_lie_alg_stacked, vo_lie_alg_stacked, gt_lie_alg_stacked, \
np.array(corr_traj), np.array(corr_traj_rot), np.array(est_traj), np.array(gt_traj), rot_seg_err, corr_rot_mean_trans, np.array(cum_dist)
gt_traj = matfile['gt_traj'].item()
odom_pose_vec = matfile['odom_pose_vecs'].item(
) #original odometry 6x1 vecs (seq_size-1 x 6)
corr_pose_vec = matfile['corr_pose_vecs'].item(
) # corrected 6x1 vecs (seq_size-1 x 6)
if estimator == 'stereo': #use rotation corrections only.
corr_pose_vec[:, :, 0:3] = odom_pose_vec[:, 0:3]
best_loop_closure_pose_vec = corr_pose_vec[best_loop_closure_epoch]
est_traj, avg_corr_traj, dense_traj, dense_gt = [], [], [], []
est_traj.append(gt_traj[0])
avg_corr_traj.append(gt_traj[0])
for i in range(0, odom_pose_vec.shape[0]):
#classically estimated traj
dT = SE3.exp(odom_pose_vec[i])
new_est = SE3.as_matrix(
(dT.dot(SE3.from_matrix(est_traj[i], normalize=True).inv())).inv())
est_traj.append(new_est)
dT_corr = SE3.exp(best_loop_closure_pose_vec[i])
new_est = SE3.as_matrix((dT_corr.dot(
SE3.from_matrix(avg_corr_traj[i], normalize=True).inv())).inv())
avg_corr_traj.append(new_est)
est_tm, est_metrics = generate_trajectory_metrics(gt_traj,
est_traj,
name='libviso2',
seq=seq)
corr_tm, avg_corr_metrics = generate_trajectory_metrics(gt_traj,
avg_corr_traj,
def se3_residual(self):
from pyslam.residuals import PoseResidual
return PoseResidual(SE3.exp([1, 2, 3, 4, 5, 6]), np.eye(6))
from pyslam.utils import invsqrt
# Reproducibility
np.random.seed(42)
# Landmarks (world frame is first camera frame)
pts_w_GT = [
np.array([0., -1., 10.]),
np.array([1., 1., 5.]),
np.array([-1., 1., 15.])
]
# Trajectory
T_cam_w_GT = [
SE3.identity(),
SE3.exp(0.1 * np.ones(6)),
SE3.exp(0.2 * np.ones(6)),
SE3.exp(0.3 * np.ones(6))
]
# Camera
camera = StereoCamera(640, 480, 1000, 1000, 0.25, 1280, 960)
# Observations
obs_var = [1, 1, 2] # [u,v,d]
obs_stiffness = invsqrt(np.diagflat(obs_var))
obs = [[camera.project(T.dot(p))
for p in pts_w_GT] for T in T_cam_w_GT]
# Optimize
