Measurement func and jacobian func made more concise and added functi…

…on to check derivatives

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]`)

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)

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]

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

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))

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):

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):

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.