def track(self, trackframe, guess=None):
""" Track an image.
Args:
trackframe : frame to track
guess : optional initial guess of the motion
"""
if len(self.keyframes) == 0:
# First frame, so don't track anything yet
trackframe.compute_pyramids()
self.keyframes.append(trackframe)
self.active_keyframe_idx = 0
active_keyframe = self.keyframes[0]
else:
# Default behaviour for second frame and beyond
active_keyframe = self.keyframes[self.active_keyframe_idx]
if guess is None:
# Default initial guess is previous pose relative to keyframe
if len(self.T_c_w) == 0:
# We just started relocalizing
guess = SE3.identity()
else:
guess = self.T_c_w[-1].dot(active_keyframe.T_c_w.inv())
# Better initial guess is previous pose + previous motion
if self.use_motion_model_guess and len(self.T_c_w) > 1:
guess = self.T_c_w[-1].dot(self.T_c_w[-2].inv().dot(guess))
else:
guess = guess.dot(active_keyframe.T_c_w.inv())
# Estimate pose change from keyframe to tracking frame
T_track_ref = self._compute_frame_to_frame_motion(
active_keyframe, trackframe, guess)
T_track_ref.normalize() # Numerical instability problems otherwise
self.T_c_w.append(T_track_ref.dot(active_keyframe.T_c_w))
# Threshold the distance from the active keyframe to drop a new one
se3_vec = SE3.log(T_track_ref)
trans_dist = np.linalg.norm(se3_vec[0:3])
rot_dist = np.linalg.norm(se3_vec[3:6])
if trans_dist > self.keyframe_trans_thresh or \
rot_dist > self.keyframe_rot_thresh:
if self.mode is 'map':
trackframe.T_c_w = self.T_c_w[-1]
trackframe.compute_pyramids()
self.keyframes.append(trackframe)
print('Dropped new keyframe. '
'Trans dist was {:.3f}. Rot dist was {:.3f}.'.format(
trans_dist, rot_dist))
self.active_keyframe_idx += 1
print('Active keyframe idx: {}'.format(
self.active_keyframe_idx))
def test_bundle_adjust(self, options, camera, points, poses, observations):
from liegroups import SE3
from pyslam.residuals import ReprojectionResidual
problem = Problem(options)
obs_var = [1, 1, 2] # [u,v,d]
obs_stiffness = invsqrt(np.diagflat(obs_var))
for i, this_pose_obs in enumerate(observations):
for j, o in enumerate(this_pose_obs):
residual = ReprojectionResidual(camera, o, obs_stiffness)
problem.add_residual_block(
residual, ['T_cam{}_w'.format(i), 'pt{}_w'.format(j)])
params_true = {}
params_init = {}
for i, pt in enumerate(points):
pid = 'pt{}_w'.format(i)
params_true.update({pid: pt})
params_init.update({pid: camera.triangulate(
observations[0][i] + 10. * np.random.rand(3))})
for i, pose in enumerate(poses):
pid = 'T_cam{}_w'.format(i)
params_true.update({pid: pose})
params_init.update({pid: SE3.identity()})
problem.initialize_params(params_init)
problem.set_parameters_constant('T_cam0_w')
params_final = problem.solve()
for key in params_true.keys():
p_est = params_final[key]
p_true = params_true[key]
if isinstance(p_est, SE3):
err = SE3.log(p_est.inv().dot(p_true))
else:
err = p_est - p_true
assert np.linalg.norm(err) < 1e-4
def track(self, trackframe):
""" Track an image.
Args:
trackframe : frame to track
"""
if len(self.keyframes) == 0:
# First frame, so don't track anything yet
self.keyframes.append(trackframe)
self.active_keyframe_idx = 0
active_keyframe = self.keyframes[0]
else:
# Default behaviour for second frame and beyond
active_keyframe = self.keyframes[self.active_keyframe_idx]
# Estimate pose change from keyframe to tracking frame
T_track_ref = self._compute_frame_to_frame_motion(
active_keyframe, trackframe)
T_track_ref.normalize() # Numerical instability problems otherwise
self.T_c_w.append(T_track_ref.dot(active_keyframe.T_c_w))
# Threshold the distance from the active keyframe to drop a new one
se3_vec = SE3.log(T_track_ref)
trans_dist = np.linalg.norm(se3_vec[0:3])
rot_dist = np.linalg.norm(se3_vec[3:6])
if trans_dist > self.keyframe_trans_thresh or \
rot_dist > self.keyframe_rot_thresh:
if self.mode is 'map':
trackframe.T_c_w = self.T_c_w[-1]
self.keyframes.append(trackframe)
print('Dropped new keyframe. '
'Trans dist was {:.3f}. Rot dist was {:.3f}.'.format(
trans_dist, rot_dist))
self.active_keyframe_idx += 1
print('Active keyframe idx: {}'.format(
self.active_keyframe_idx))
residual = PhotometricResidualSE3(camera, im_ref, disp_ref, im_track,
jac_ref, 1., 1.)
# Timing debug
# niters = 100
# start = time.perf_counter()
# for _ in range(niters):
# cost.evaluate([params_init['T_1_0']])
# end = time.perf_counter()
# print('cost.evaluate avg {} s', (end - start) / niters)
# start = time.perf_counter()
# for _ in range(niters):
# cost.evaluate([params_init['T_1_0']], [True])
# end = time.perf_counter()
# print('cost.evaluate w jac avg {} s', (end - start) / niters)
# Optimize
# start = time.perf_counter()
problem = Problem(options)
problem.add_residual_block(residual, ['T_1_0'])
problem.initialize_params(params)
params = problem.solve()
# end = time.perf_counter()
# print('Elapsed time: {} s'.format(end - start))
print('Error in T_1_w: {}'.format(
SE3.log(T_1_w.dot((params['T_1_0'].dot(T_0_w)).inv()))))
residual = PhotometricResidual(camera, im_ref, disp_ref, jac_ref, im_track,
1.)
# Timing debug
# niters = 100
# start = time.perf_counter()
# for _ in range(niters):
# cost.evaluate([params_init['T_1_0']])
# end = time.perf_counter()
# print('cost.evaluate avg {} s', (end - start) / niters)
# start = time.perf_counter()
# for _ in range(niters):
# cost.evaluate([params_init['T_1_0']], [True])
# end = time.perf_counter()
# print('cost.evaluate w jac avg {} s', (end - start) / niters)
# Optimize
# start = time.perf_counter()
problem = Problem(options)
problem.add_residual_block(residual, ['T_1_0'])
problem.initialize_params(params)
params = problem.solve()
# end = time.perf_counter()
# print('Elapsed time: {} s'.format(end - start))
print('Error in T_1_w: {}'.format(
SE3.log(T_1_w * (params['T_1_0'] * T_0_w).inv())))
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
'T_3_0': T_3_0_init,
'T_4_0': T_4_0_init,
'T_5_0': T_5_0_init,
'T_6_0': T_6_0_init
}
params_true = {
'T_1_0': T_1_0_true,
'T_2_0': T_2_0_true,
'T_3_0': T_3_0_true,
'T_4_0': T_4_0_true,
'T_5_0': T_5_0_true,
'T_6_0': T_6_0_true
}
problem.initialize_params(params_init)
params_final = problem.solve()
print(problem.summary(format='full'))
print("Initial Error:")
for key in params_true.keys():
print('{}: {}'.format(
key, SE3.log(params_init[key].inv().dot(params_true[key]))))
print()
print("Final Error:")
for key in params_true.keys():
print('{}: {}'.format(
key, SE3.log(params_final[key].inv().dot(params_true[key]))))
problem.initialize_params(params_init)
problem.set_parameters_constant('T_cam0_w')
# import ipdb
# ipdb.set_trace()
params_final = problem.solve()
print(problem.summary(format='full'))
# Compute errors
print("Initial Error:")
for key in params_true.keys():
p_est = params_init[key]
p_true = params_true[key]
if isinstance(p_est, SE3):
err = SE3.log(p_est.inv().dot(p_true))
else:
err = p_est - p_true
print('{}: {}'.format(key, err))
print()
print("Final Error:")
for key in params_true.keys():
p_est = params_final[key]
p_true = params_true[key]
if isinstance(p_est, SE3):
err = SE3.log(p_est.inv().dot(p_true))
else:
for i in range(len(obs) - 1):
T_c1_w = T_cam_w_GT[i]
T_c2_w = T_cam_w_GT[i + 1]
pid = 'T_cam{}_cam{}'.format(i + 1, i)
params_true.update({pid: T_c2_w * T_c1_w.inv()})
params_init.update({pid: SE3.identity()})
problem.initialize_params(params_init)
params_final = problem.solve()
print()
# Compute errors
print("Initial Error:")
for key in params_true.keys():
p_est = params_init[key]
p_true = params_true[key]
err = SE3.log(p_est.inv() * p_true)
print('{}: {}'.format(key, err))
print()
print("Final Error:")
for key in params_true.keys():
p_est = params_final[key]
p_true = params_true[key]
err = SE3.log(p_est.inv() * p_true)
print('{}: {}'.format(key, err))
