diff --git a/README.md b/README.md index eca1139..12105f0 100644 --- a/README.md +++ b/README.md @@ -2,10 +2,6 @@ ## Setup -Install dependencies for gbp library: - -- numpy, scipy - Install dependencies for bundle adjustment visualization: - rtree (`conda install rtree`), shapely (`conda install shapely`), trimesh (`pip install trimesh[easy]`) diff --git a/gbp/factors/reprojection.py b/gbp/factors/reprojection.py index 26e8fbe..ebc519a 100644 --- a/gbp/factors/reprojection.py +++ b/gbp/factors/reprojection.py @@ -1,139 +1,54 @@ import numpy as np -from utils import transformations, lie_algebra -_EPS = np.finfo(float).eps +from utils import transformations, lie_algebra, derivatives """ - Axis angle parametrisation is used for angle parameters of pose. - We store the pose Tcw to transform from world to camera frame. - Derivative of rotation matrix wrt lie algebra is in equation 8 (https://arxiv.org/pdf/1312.0788.pdf) +Axis angle parametrisation is used for angle parameters of pose. +We store the pose Tcw to transform from world to camera frame. +Derivative of rotation matrix wrt lie algebra is in equation 8 (https://arxiv.org/pdf/1312.0788.pdf) """ -def meas_fn(x, K): +def meas_fn(inp, K): """ Measurement function which projects landmark into image plane of camera. :param x: first 6 params are keyframe pose, latter 3 are landmark location in world frame. + First 3 params of pose are the translation and latter 3 are SO(3) minimal rep. :param K: camera matrix. """ - Tcw = transformations.getT_axisangle(x[:6]) - ycf = np.dot(Tcw, np.concatenate((x[6:], [1])))[:3] - fx, fy = K[0, 0], K[1, 1] - px, py = K[0, 2], K[1, 2] - z = 1 / ycf[2] * np.array([fx * ycf[0], fy * ycf[1]]) + np.array([px, py]) - return z + assert len(inp) == 9 + t = inp[:3] + R_cw = lie_algebra.so3exp(inp[3:6]) + y_wf = inp[6:9] + return transformations.proj(K @ (R_cw @ y_wf + t)) -def jac_fn(x, K): - """ - Computes the Jacobian of the function that projects a landmark into the image plane of a camera. - - :param x: first 6 params are keyframe pose, latter 3 are landmark location in world frame. - :param K: camera matrix. - """ - cam_params = x[:6] - ywf = x[6:] - fx, fy = K[0, 0], K[1, 1] - - J = np.zeros([2, 9]) - Tcw = transformations.getT_axisangle(cam_params) - ycf = np.dot(Tcw, np.concatenate((ywf, [1])))[:3] - - J_proj = np.array([[fx / ycf[2], 0., -fx * ycf[0] / ycf[2]**2], [0., fy / ycf[2], -fy * ycf[1] / ycf[2]**2]]) - - v = cam_params[3:] - R = Tcw[:3, :3] - - dRydv = -np.dot(np.dot(R, lie_algebra.S03_hat_operator(ywf)), - (np.outer(v, v) + np.dot(R.T - np.eye(3), lie_algebra.S03_hat_operator(v))) / np.dot(v, v)) - - J[:, 0:3] = J_proj - J[:, 3:6] = np.dot(J_proj, dRydv) - J[:, 6:] = np.dot(J_proj, Tcw[:3, :3]) - return J - - -#################################### -# Finite difference Jacobian checks -#################################### - - -def jac_fd(x, K, delta): - J = np.zeros([2, 9]) - for i in range(9): - x_dx = np.copy(x) - x_dx[i] += delta - J[:, i] = (meas_fn(x_dx, K) - meas_fn(x, K)) / delta - return J - - -def jac_proj_fd(ycf, K, delta=1e-5): - """ - Computes Jacobian of projection function using finite difference method. - """ - fx, fy = K[0, 0], K[1, 1] - Jproj_fd = np.zeros([2, 3]) - for i in range(3): - ycfcp = np.copy(ycf) - ycfcp[i] += delta - Jproj_fd[:, i] = (1 / ycfcp[2] * np.array([fx * ycfcp[0], fy * ycfcp[1]]) - 1 / ycf[2] * - np.array([fx * ycf[0], fy * ycf[1]])) / delta - return Jproj_fd - - -def dRydv_fd(x, K, delta=1e-5): - """ - Computes derivative of Rotation matrix multiplied by point in world frame wrt lie algebra rep of rotation matrix - using finite difference method. +def jac_fn(inp, K): """ - v = x[3:6] - ywf = x[6:] - dRydv_fd = np.zeros([3, 3]) - - for i in range(3): - vcp = np.copy(v) - vcp[i] += delta - dRydv_fd[:, i] = (np.dot(lie_algebra.so3exp(vcp), ywf) - np.dot(lie_algebra.so3exp(v), ywf)) / delta - return dRydv_fd - - -""" - Quaternion angle parametrisation is used for angle parameters of pose. - We store the quaternion params for Tcw to transform from world to camera frame. -""" - - -def meas_fn_qt(x, K): - """ - Measurement function which projects landmark into image plane of camera. - :param x: first 7 params are keyframe pose, latter 3 are landmark location in world frame. - :param K: camera matrix. + Computes the Jacobian of the function that projects a landmark into the image plane of a camera. """ - Tcw = transformations.getT_qt(x[:7]) - ycf = np.dot(Tcw, np.concatenate((x[7:], [1])))[:3] - fx, fy = K[0, 0], K[1, 1] - px, py = K[0, 2], K[1, 2] - z = 1 / ycf[2] * np.array([fx * ycf[0], fy * ycf[1]]) + np.array([px, py]) - return z + assert len(inp) == 9 + t = inp[:3] + w = inp[3:6] # minimal SO(3) rep + R_cw = lie_algebra.so3exp(inp[3:6]) + y_wf = inp[6:9] + jac = np.zeros([2, 9]) -def jac_fn_qt(x, K): - """ - Computes the Jacobian of the function that projects a landmark into the image plane of a camera. - - :param x: first 7 params are keyframe pose, latter 3 are landmark location in world frame. - :param K: camera matrix. - """ - cam_params = x[:7] - ywf = x[7:] - fx, fy = K[0, 0], K[1, 1] + J_proj = derivatives.proj_derivative(K @ (R_cw @ y_wf + t)) - J = np.zeros([2, 10]) - Tcw = transformations.getT_axisangle(cam_params) - ycf = np.dot(Tcw, np.concatenate((ywf, [1])))[:3] + jac[:, 0:3] = J_proj @ K + jac[:, 3:6] = J_proj @ K @ derivatives.dR_wx_dw(w, y_wf) + jac[:, 6:] = J_proj @ K @ R_cw + return jac - # TO DO +if __name__ == '__main__': + # Check Jacobian function + x = np.random.rand(9) + K = np.array([[517.306408, 0., 318.64304], + [0., 516.469215, 255.313989], + [0., 0., 1.]]) - return J + derivatives.check_jac(jac_fn, x, meas_fn, K) diff --git a/gbp/gbp.py b/gbp/gbp.py index aed75b3..0ece2c9 100644 --- a/gbp/gbp.py +++ b/gbp/gbp.py @@ -350,7 +350,6 @@ def compute_messages(self, eta_damping): lam_factor[mess_start_dim:mess_start_dim + var_dofs, mess_start_dim:mess_start_dim + var_dofs] += self.adj_beliefs[var].lam - self.messages[var].lam mess_start_dim += self.adj_var_nodes[var].dofs - # Divide up parameters of distribution mess_dofs = self.adj_var_nodes[v].dofs eo = eta_factor[start_dim:start_dim + mess_dofs] diff --git a/gbp/gbp_ba.py b/gbp/gbp_ba.py index 5430278..499ae4d 100644 --- a/gbp/gbp_ba.py +++ b/gbp/gbp_ba.py @@ -1,5 +1,5 @@ import numpy as np -from gbp import gbp +from gbp import gbp, gbp_ba from gbp.factors import reprojection from utils import read_balfile @@ -19,15 +19,16 @@ def __init__(self, **kwargs): def generate_priors_var(self, weaker_factor=100): """ - Sets automatically the std of the priors such that standard deviations of prior factors are a factor of weaker_factor + Sets automatically the std of the priors such that standard deviations + of prior factors are a factor of weaker_factor weaker than the standard deviations of the adjacent factors. NB. Jacobian of measurement function effectively sets the scale of the factors. """ for var_node in self.cam_nodes + self.lmk_nodes: max_factor_lam = 0. for factor in var_node.adj_factors: - max_factor_lam = max(max_factor_lam, np.max(factor.factor.lam)) - + if isinstance(factor, gbp_ba.ReprojectionFactor): + max_factor_lam = max(max_factor_lam, np.max(factor.factor.lam)) lam_prior = np.eye(var_node.dofs) * max_factor_lam / (weaker_factor ** 2) var_node.prior.lam = lam_prior var_node.prior.eta = lam_prior @ var_node.mu @@ -101,9 +102,9 @@ def create_ba_graph(bal_file, configs): measurements_lIDs, K = read_balfile.read_balfile(bal_file) graph = BAFactorGraph(eta_damping=configs['eta_damping'], - beta=configs['beta'], - num_undamped_iters=configs['num_undamped_iters'], - min_linear_iters=configs['min_linear_iters']) + beta=configs['beta'], + num_undamped_iters=configs['num_undamped_iters'], + min_linear_iters=configs['min_linear_iters']) variable_id = 0 factor_id = 0 diff --git a/utils/derivatives.py b/utils/derivatives.py new file mode 100644 index 0000000..c0c27b5 --- /dev/null +++ b/utils/derivatives.py @@ -0,0 +1,50 @@ +import numpy as np +from utils import lie_algebra + +""" +Standard useful derivatives. +""" + + +def jac_fd(inp, meas_fn, *args, delta=1e-8): + """ + Compute Jacobian of meas_fn at inp using finite difference method. + """ + z = meas_fn(inp, *args) + if isinstance(z, float): + jac = np.zeros([1, len(inp)]) + else: + jac = np.zeros([len(z), len(inp)]) + for i in range(len(inp)): + d_inp = np.copy(inp) + d_inp[i] += delta + jac[:, i] = (meas_fn(d_inp, *args) - meas_fn(inp, *args)) / delta + return jac + + +def check_jac(jac_fn, inp, meas_fn, *args, threshold=1e-3): + jac = jac_fn(inp, *args) + jacfd = jac_fd(inp, meas_fn, *args) + + if np.max(jac - jacfd) < threshold: + print(f"Passed! Jacobian correct to within {threshold}") + else: + print(f"Failed: Jacobian difference to finite difference Jacobian not within threshold ({threshold})" + f"\nMaximum discrepancy between Jacobian and finite diff Jacobian: {np.max(jac - jacfd)}") + + +def dR_wx_dw(w, x): + """ + :param w: Minimal SO(3) rep + :param x: 3D point / vector + :return: derivative of R(w)x wrt w + """ + R = lie_algebra.so3exp(w) + dR_wx_dw = -np.dot(np.dot(R, lie_algebra.S03_hat_operator(x)), + (np.outer(w, w) + np.dot(R.T - np.eye(3), lie_algebra.S03_hat_operator(w))) / np.dot(w, w)) + return dR_wx_dw + + +def proj_derivative(x): + if x.ndim == 1: + return np.hstack((np.eye(len(x) - 1) / x[-1], np.array([- x[:-1] / x[-1]**2]).T)) diff --git a/utils/lie_algebra.py b/utils/lie_algebra.py index fa747ed..c8a7f9d 100644 --- a/utils/lie_algebra.py +++ b/utils/lie_algebra.py @@ -12,7 +12,9 @@ def S03_hat_operator(x): """ Hat operator for SO(3) Lie Group """ - return np.array([[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]]) + return np.array([[0., -x[2], x[1]], + [x[2], 0., -x[0]], + [-x[1], x[0], 0.]]) def SE3_hat_operator(x): @@ -21,7 +23,10 @@ def SE3_hat_operator(x): First 3 elements of the minimal representation x are to do with the translation part while the latter 3 elements are to do with the rotation part. """ - return np.array([[0, -x[5], x[4], x[0]], [x[5], 0, -x[3], x[1]], [-x[4], x[3], 0, x[2]], [0, 0, 0, 0]]) + return np.array([[0., -x[5], x[4], x[0]], + [x[5], 0., -x[3], x[1]], + [-x[4], x[3], 0., x[2]], + [0., 0., 0., 0.]]) def so3exp(w): diff --git a/utils/read_balfile.py b/utils/read_balfile.py index 1654a07..1ab5657 100644 --- a/utils/read_balfile.py +++ b/utils/read_balfile.py @@ -13,6 +13,7 @@ def read_balfile(balfile): n_keyframes, n_points, n_edges = [int(x) for x in line.split()] K = np.zeros([3, 3]) K[0, 0], K[1, 1], K[0, 2], K[1, 2] = [float(x) for x in f.readline().split()] + K[2, 2] = 1. measurements_camIDs, measurements_lIDs, measurements = [], [], [] for i in range(n_edges): diff --git a/utils/transformations.py b/utils/transformations.py index 30d325c..4499599 100644 --- a/utils/transformations.py +++ b/utils/transformations.py @@ -2,6 +2,13 @@ from utils import lie_algebra +def proj(x): + if x.ndim == 1: + return x[:-1] / x[-1] + elif x.ndim == 2: + return np.divide(x[:, :-1].T, x[:, -1]).T + + # ----------------------------- get transformation functions ----------------------------- def getT_axisangle(x): @@ -28,6 +35,7 @@ def getT_qt(x): # ---------------------------------- Quaternions ----------------------------------------------- + def normalize(v, tolerance=1e-4): mag2 = sum(n * n for n in v) if abs(mag2 - 1.0) > tolerance: @@ -84,6 +92,7 @@ def q_multiply(q1, q2): # ---------------------------------------- Euler ----------------------------------------------- + def eulerAnglesToRotationMatrix(theta): """ Calculates Rotation Matrix given euler angles.