def _compute_frame_to_frame_motion(self, ref_frame, track_frame):
# Get feature matches
self.matcher.pushBack(ref_frame.im_left, ref_frame.im_right)
self.matcher.pushBack(track_frame.im_left, track_frame.im_right)
self.matcher.matchFeatures(self.matcher_mode)
matches = self.matcher.getMatches()
# print('libviso2 matched {} features.'.format(matches.size()))
# Stereo observations (uvd)
self.obs_0 = [np.array([m.u1p, m.v1p, m.u1p - m.u2p]) for m in matches]
self.obs_1 = [np.array([m.u1c, m.v1c, m.u1c - m.u2c]) for m in matches]
# Prune all observations with disparity <= 0
keep_mask = (self.obs_0[:, 2] > 0) & (self.obs_1[:, 2] > 0)
self.obs_0 = self.obs_0[keep_mask, :]
self.obs_1 = self.obs_1[keep_mask, :]
# print('Matches after pruning: {} '.format(len(self.obs_0)))
# RANSAC
self.ransac.set_obs(self.obs_0, self.obs_1)
T_1_0_guess, obs_0_inliers, obs_1_inliers, _ = self.ransac.perform_ransac()
# Optimize with inliers
residual = ReprojectionMotionOnlyBatchResidual(
self.camera, obs_0_inliers, obs_1_inliers, self.reprojection_stiffness)
problem = Problem(self.motion_options)
problem.add_residual_block(residual, ['T_1_0'], loss=self.loss)
params = {'T_1_0': T_1_0_guess}
problem.initialize_params(params)
params = problem.solve()
return params['T_1_0']
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_residual_blocks(self):
from pyslam.residuals import QuadraticResidual
problem = Problem()
param_keys = ['a', 'b', 'c']
problem.add_residual_block(QuadraticResidual(2., 4., 1.),
param_keys)
assert param_keys == problem.block_param_keys[0]
def test_loop_closure(self, options, residuals, residual_params,
poses_init, poses_true):
from liegroups import SE2
problem = Problem(options)
for i in range(0, 7):
problem.add_residual_block(residuals[i], residual_params[i])
problem.initialize_params(poses_init)
poses_final = problem.solve()
for key in poses_true.keys():
assert np.linalg.norm(
SE2.log(poses_final[key].inv().dot(poses_true[key]))) < 1e-4
def test_first_pose_constant(self, options, residuals, residual_params,
poses_init, poses_true):
from liegroups import SE2
problem = Problem(options)
for i in range(1, 6):
problem.add_residual_block(residuals[i], residual_params[i])
problem.set_parameters_constant('T_0_w')
problem.initialize_params(poses_init)
poses_final = problem.solve()
for key in poses_true.keys():
assert np.linalg.norm(
SE2.log(poses_final[key].inv().dot(poses_true[key]))) < 1e-4
def _compute_frame_to_frame_motion(self,
ref_frame,
track_frame,
guess=SE3.identity()):
# params = {'T_1_0': guess}
params = {'R_1_0': guess.rot, 't_1_0_1': guess.trans}
for (pyrlevel, pyr_camera) in zip(self.pyrlevel_sequence,
self.pyr_cameras):
pyrfactor = 2**-pyrlevel
im_jacobian = ref_frame.jacobian[pyrlevel]
# ESM
# im_jacobian = 0.5 * (ref_frame.jacobian[pyrlevel] +
# track_frame.jacobian[pyrlevel])
if self.depth_map_type is 'disparity':
depth_ref = ref_frame.disparity[pyrlevel]
# Disparity is in pixels, so we need to scale it according to the pyramid level
depth_stiffness = self.depth_stiffness / pyrfactor
else:
depth_ref = ref_frame.depth[pyrlevel]
depth_stiffness = self.depth_stiffness
residual = PhotometricResidualSE3(
pyr_camera, ref_frame.im_pyr[pyrlevel], depth_ref,
track_frame.im_pyr[pyrlevel], im_jacobian,
self.intensity_stiffness, depth_stiffness, self.min_grad)
problem = Problem(self.motion_options)
# problem.add_residual_block(residual, ['T_1_0'], loss=self.loss)
problem.add_residual_block(residual, ['R_1_0', 't_1_0_1'],
loss=self.loss)
problem.initialize_params(params)
if pyrlevel > 2:
problem.set_parameters_constant('t_1_0_1')
# else:
# problem.set_parameters_constant('R_1_0')
params = problem.solve()
# print(problem.summary(format='brief'))
# # DEBUG: Store residuals for later
# try:
# self.residuals = np.hstack(
# [self.residuals, residual.evaluate([guess])])
# except AttributeError:
# self.residuals = residual.evaluate([guess])
# return params['T_1_0']
return SE3(params['R_1_0'], params['t_1_0_1'])
def test_eval_cost(self):
from pyslam.residuals import QuadraticResidual
problem = Problem()
good_params = {'a': 1., 'b': 2., 'c': 1.}
bad_params = {'a': 1., 'b': 0., 'c': 0.}
residual1 = QuadraticResidual(1., 4., 0.5)
residual2 = QuadraticResidual(0., 1., 2.)
problem.add_residual_block(residual1, ['a', 'b', 'c'])
problem.add_residual_block(residual2, ['a', 'b', 'c'])
problem.initialize_params(good_params)
assert problem.eval_cost() == 0.
assert problem.eval_cost(bad_params) == 0.5 * ((0.5 * 0.5 * 3. * 3.)
+ (2. * 2. * 1. * 1.))
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 test_fit_quadratic(self):
from pyslam.residuals import QuadraticResidual
params_true = {'a': 1., 'b': -2., 'c': 3.}
params_init = {'a': -20., 'b': 10., 'c': -30.}
x_data = np.linspace(-5, 5, 10)
y_data = params_true['a'] * x_data * x_data \
+ params_true['b'] * x_data + params_true['c']
problem = Problem()
problem.options.num_threads = 1
for x, y in zip(x_data, y_data):
problem.add_residual_block(QuadraticResidual(
x, y, 1.), ['a', 'b', 'c'])
problem.initialize_params(params_init)
params_final = problem.solve()
for key in params_true.keys():
assert(np.allclose(params_final[key], params_true[key]))
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()))))
residual4 = PoseToPoseResidual(T_5_4_obs, odom_stiffness)
residual4_params = ['T_4_0', 'T_5_0']
residual5 = PoseToPoseResidual(T_6_5_obs, odom_stiffness)
residual5_params = ['T_5_0', 'T_6_0']
residual6 = PoseToPoseResidual(T_6_2_obs, loop_stiffness)
residual6_params = ['T_2_0', 'T_6_0']
options = Options()
options.allow_nondecreasing_steps = True
options.max_nondecreasing_steps = 3
problem = Problem(options)
problem.add_residual_block(residual0, residual0_params)
problem.add_residual_block(residual1, residual1_params)
problem.add_residual_block(residual2, residual2_params)
problem.add_residual_block(residual3, residual3_params)
problem.add_residual_block(residual4, residual4_params)
problem.add_residual_block(residual5, residual5_params)
problem.add_residual_block(residual6, residual6_params)
# problem.set_parameters_constant(residual0_params)
# problem.set_parameters_constant(residual1_params)
params_init = {
'T_1_0': T_1_0_init,
'T_2_0': T_2_0_init,
'T_3_0': T_3_0_init,
'T_4_0': T_4_0_init,
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
options = Options()
options.allow_nondecreasing_steps = True
options.max_nondecreasing_steps = 3
problem = Problem(options)
for i, this_pose_obs in enumerate(obs):
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(pts_w_GT):
pid = 'pt{}_w'.format(i)
params_true.update({pid: pt})
params_init.update({pid: camera.triangulate(
obs[0][i] + 10. * np.random.rand(3))})
for i, pose in enumerate(T_cam_w_GT):
pid = 'T_cam{}_w'.format(i)
params_true.update({pid: pose})
params_init.update({pid: SE3.identity()})
options = Options()
options.allow_nondecreasing_steps = True
options.max_nondecreasing_steps = 3
problem = Problem(options)
# Collect observations in pairs of poses (i.e. 0-1, 1-2, 2-3) and add
# residuals to problem
for i in range(len(obs) - 1):
pose_1_obs = obs[i]
pose_2_obs = obs[i + 1]
for j, o_1 in enumerate(pose_1_obs):
o_2 = pose_2_obs[j]
residual = ReprojectionResidualFrameToFrame(camera, o_1, o_2,
obs_stiffness)
problem.add_residual_block(residual,
['T_cam{}_cam{}'.format(i + 1, i)])
params_true = OrderedDict({})
params_init = OrderedDict({})
# Initialize the relative SE(3) transforms
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()