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_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 _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_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_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_param_dict(self):
problem = Problem()
params = {'a': 1, 'b': 2, 'c': 3}
problem.initialize_params(params)
assert(
problem.param_dict == params
)
extra_param = {'d': 4}
params.update(extra_param)
problem.initialize_params(extra_param)
assert problem.param_dict == params
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()))))
residual3_params = ['T_3_0', 'T_4_0']
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,
def test_constant_params(self):
problem = Problem()
problem.set_parameters_constant('a')
assert problem.constant_param_keys == ['a']
problem.set_parameters_constant(['a', 'b_param'])
assert problem.constant_param_keys == ['a', 'b_param']
problem.set_parameters_variable('a')
assert problem.constant_param_keys == ['b_param']
problem.set_parameters_variable('c')
assert problem.constant_param_keys == ['b_param']
problem.set_parameters_variable(['a', 'b_param', 'c'])
assert problem.constant_param_keys == []
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 error_ij():
# define later in case of measurement-based model
def Q_i(Q_C,t_i,t_im1):
delta_t_i = t_i - t_im1
return np.array([(1/3)*((delta_t_i)**3)*Q_C, (1/2)*((delta_t_i)**2)*Q_C;
(1/2)*((delta_t_i)**2)*Q_C, (delta_t_i)*Q_C])
# TODO
"""
DONE
setup xi and t data containers
defined the cost function structure
come up with matrix Q_C
#TODO
do some research on the gauss-newton algorithm
define dataset xi above
define w_bar dataset above (lin and rotational velocity)
"""
def Q_C(dim):
return np.eye(dim)
def J(xi, w_bar, N, Q_C, t):
# create error vector
# fill up error vector with all training data
e = np.empty([N,1])
for i in range(N):
e.append(error_i(xi[i], xi[i+1],w_bar[i],w_bar[i+1]))
# create Q matrix and invert it
# create a vector of all Q_i entries
Q_vect = np.empty([N,1])
for i in range(N):
Q_vect.append(Q_i(Q_C(6),t[i],t[i-1]))
Q = np.diag(Q_vect)
Q_inv = np.linalg.inv(Q)
return (1/2)*np.matmul(np.matmul(np.tranpose(e),Q_inv),e)
#system_to_solve
# implementation of E:
def E():
F_k = np.empty([4,2])
k = 0
dim = 3
T_kp1 = SE3.exp(xi[k+1])
T_k = SE3.exp(xi[k])
tau_bar = (SE3.exp([xi[k+1]]).dot(SE3.exp([xi[k]]))).adjoint()
SE3.exp
F_k[0,0] = SE3.inv_left_jacobian(T_kp1.dot(T_k)).dot(tau_bar_kp1_k)
F_k[1,0] = (1/2)*w_bar(v_kp1,omega_kp1).dot(SE3.inv_left_jacobian(T_kp1.dot(T_k))).dot(tau_bar_kp1_k):
F_k[0,1] = (t_kp1-tk)*np.eye(dim)
F_k[1,1] = np.eye(dim)
F_k[0,2] = -SE3.inv_left_jacobian(T_kp1.dot(T_k))
F_k[1,2] = (-1/2)*SE3.vee(w_bar(v_kp1,omega_kp1)).dot(SE3.inv_left_jacobian(T_kp1.dot(T_k)))
F_k[0,3] = np.zeros(dim)
F_k[1,3] = -SE3.inv_left_jacobian(T_kp1.dot(T_k))
P = np.empty([2,1])
"""
perturbation: epsilon*
initial pose: identity transform
per iteration we update: x_op <- x_op + delta_x_op
T_i =
"""
xi_init = np.array([0,0,0,0,0,0])
T_init = T_i = SE3.exp(xi_init)
v_init = np.array([0.5, 0.5, 0.5])
omega_init = np.array([0.0, 0.0, 0.0])
w_bar_init = w_bar(v_init, omega_init)
e_op_init = error_i(xi_init, xi[0], w_bar_init, w_bar[0])
error_i(xi_i, xi_ip1,w_bar_i,w_bar_ip1):
def delta_i(theta_i,psi_i):
return np.array([theta_i, psi_i])
def epsilon():
delta = np.empty([N,1])
for i in range(N):
delta.append(delta_i(theta[i], psi[i]))
return epsilon
from pyslam.problem import Problem, Options
options = Options()
options.print_summary = True
problem = Problem(options)
def residuals(self, b):
"""Evaluate the residuals f(x, b) - y with the given parameters.
Parameters
----------
b : tuple, list or ndarray
Values for the model parameters.
Return
------
out : ndarray
Residual vector for the given model parameters.
"""
x, y = self.xvals, self.yvals
return self._numexpr(x, *b) - y
def jacobian(self, b):
"""Evaluate the model's Jacobian matrix with the given parameters.
Parameters
----------
b : tuple, list or ndarray
Values for the model parameters.
Return
------
out : ndarray
Evaluation of the model's Jacobian matrix in column-major order wrt
the model parameters.
"""
# Substitute parameters in partial derivatives
subs = [pd.subs(zip(self._b, b)) for pd in self._pderivs]
# Evaluate substituted partial derivatives for all x-values
vals = [sp.lambdify(self._x, sub, "numpy")(self.xvals) for sub in subs]
# Arrange values in column-major order
return np.column_stack(vals)
def gauss_newton(sys, x0, tol = 1e-10, maxits = 256):
"""Gauss-Newton algorithm for solving nonlinear least squares problems.
Parameters
----------
sys : Dataset
Class providing residuals() and jacobian() functions. The former should
evaluate the residuals of a nonlinear system for a given set of
parameters. The latter should evaluate the Jacobian matrix of said
system for the same parameters.
x0 : tuple, list or ndarray
Initial guesses or starting estimates for the system.
tol : float
Tolerance threshold. The problem is considered solved when this value
becomes smaller than the magnitude of the correction vector.
Defaults to 1e-10.
maxits : int
Maximum number of iterations of the algorithm to perform.
Defaults to 256.
Return
------
sol : ndarray
Resultant values.
its : int
Number of iterations performed.
Note
----
Uses numpy.linalg.pinv() in place of similar functions from scipy, both
because it was found to be faster and to eliminate the extra dependency.
"""
dx = np.ones(len(x0)) # Correction vector
xn = np.array(x0) # Approximation of solution
i = 0
while (i < maxits) and (dx[dx > tol].size > 0):
# correction = pinv(jacobian) . residual vector
dx = np.dot(np.linalg.pinv(sys.jacobian(xn)), -sys.residuals(xn))
xn += dx # x_{n + 1} = x_n + dx_n
i += 1
return xn, i
# 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
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))})
# 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))
# Collect all observations of the landmarks
obs = [[camera.project(T * p) for p in pts_w_GT] for T in T_cam_w_GT]
# Options
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({})