Updated Bundle Adjust notebook

knoten/bundle.py

@@ -3,6 +3,8 @@ import pandas as pd
 import pvl
 import os
 import csmapi
+import itertools
+from math import floor
 
 from pysis import isis
 from ale.drivers import loads
@@ -199,55 +201,135 @@ def compute_ground_partials(sensor, ground_pt):
     partials = np.array(sensor.computeGroundPartials(csm_ground))
     return np.reshape(partials, (2, 3))
 
-def compute_jacobian(network, sensors, parameters):
+def compute_coefficient_columns(network, sensors, parameters):
     """
-    Compute the Jacobian matrix.
+    Compute the columns for different coefficients
 
     Parameters
     ----------
     network : DataFrame
-              The control network as a dataframe generated by plio. The ground
-              point columns will be in the same order as the control points are
-              in this.
+              The control network as a dataframe generated by plio.
     sensors : dict
               Dictionary that maps ISIS serial numbers to CSM sensor models
     parameters : dict
                  Dictionary that maps serial numbers to lists of parameters to
-                 solve for. The image parameter columns of the Jacobian will be
-                 in the same order as this.
+                 solve for.
 
     Returns
     -------
-     : ndarray
-       The Jacobian matrix
+     : OrderedDict
+       Dictionary that maps serial numbers and point IDs to the column range
+       their parameters are in the Jacobian matrix.
     """
     num_columns = 0
     coefficient_columns = OrderedDict()
     for serial in network["serialnumber"].unique():
         coefficient_columns[serial] = num_columns
         num_columns += len(parameters[serial])
+        coefficient_columns[serial] = (coefficient_columns[serial], num_columns)
     for point_id in network["id"].unique():
         # Skip fixed points
         if network.loc[network.id == point_id].iloc[0]["pointType"] == 4:
             continue
         coefficient_columns[point_id] = num_columns
         num_columns += 3
+        coefficient_columns[point_id] = (coefficient_columns[point_id], num_columns)
+    return coefficient_columns
 
-    num_rows = len(network) * 2
+def compute_jacobian(network, sensors, parameters, coefficient_columns):
+    """
+    Compute the Jacobian matrix.
 
+    Parameters
+    ----------
+    network : DataFrame
+              The control network as a dataframe generated by plio.
+    sensors : dict
+              Dictionary that maps ISIS serial numbers to CSM sensor models
+    parameters : dict
+                 Dictionary that maps serial numbers to lists of parameters to
+                 solve for.
+    coefficient_columns : OrderedDict
+                          Dictionary that maps serial numbers and point IDs to
+                          the column range their parameters are in the Jacobian
+                          matrix.
+
+    Returns
+    -------
+     : ndarray
+       The Jacobian matrix
+    """
+    num_columns = max([col_range[1] for col_range in coefficient_columns.values()])
+    num_rows = len(network) * 2
     jacobian = np.zeros((num_rows, num_columns))
+
     for i in range(len(network)):
         row = network.iloc[i]
         serial = row["serialnumber"]
         ground_pt = row[["adjustedX", "adjustedY", "adjustedZ"]]
         sensor = sensors[serial]
         params = parameters[serial]
-        image_column = coefficient_columns[serial]
-        point_column = coefficient_columns[row["id"]]
-        jacobian[2*i : 2*i+2, image_column : image_column+len(params)] = compute_sensor_partials(sensor, params, ground_pt)
-        jacobian[2*i : 2*i+2, point_column : point_column+3] = compute_ground_partials(sensor, ground_pt)
+        image_range = coefficient_columns[serial]
+        point_range = coefficient_columns[row["id"]]
+        jacobian[2*i : 2*i+2, image_range[0] : image_range[1]] = compute_sensor_partials(sensor, params, ground_pt)
+        jacobian[2*i : 2*i+2, point_range[0] : point_range[1]] = compute_ground_partials(sensor, ground_pt)
+
+    return jacobian
+
+def compute_parameter_weights(network, sensors, parameters, coefficient_columns):
+    """
+    Compute the parameter weight matrix
 
-    return jacobian, coefficient_columns
+    Parameters
+    ----------
+    network : DataFrame
+              The control network as a dataframe generated by plio.
+    sensors : dict
+              Dictionary that maps ISIS serial numbers to CSM sensor models
+    parameters : dict
+                 Dictionary that maps serial numbers to lists of parameters to
+                 solve for.
+    coefficient_columns : OrderedDict
+                          Dictionary that maps serial numbers and point IDs to
+                          the column range their parameters are in the Jacobian
+                          matrix. Their parameters weights will have the same
+                          ordering in the weight matrix.
+
+    Returns
+    -------
+     : ndarray
+       The parameter weight matrix
+    """
+    num_params = max([col_range[1] for col_range in coefficient_columns.values()])
+    weight_mat = np.zeros((num_params, num_params))
+
+    # Image parameters
+    for sn, params in parameters.items():
+        param_count = len(params)
+        covar_mat = np.zeros((param_count, param_count))
+        for a, b in itertools.product(range(param_count), range(param_count)):
+            covar_mat[a, b] = sensors[sn].getParameterCovariance(params[a].index, params[b].index)
+        col_range = coefficient_columns[sn]
+        weight_mat[col_range[0]:col_range[1], col_range[0]:col_range[1]] = np.linalg.inv(covar_mat)
+
+    # Point parameters
+    for point_id, group in network.groupby('id'):
+        ## If there is no covariance matrix, then just continue on
+        point_covar = list(group.iloc[0]["aprioriCovar"])
+        if len(point_covar) != 6:
+            continue
+        # The covariance matrix is stored as just one triangle, so we have
+        # to unpack it.
+        if len(point_covar) == 6:
+            covar_mat = np.array(
+                [[point_covar[0], point_covar[1], point_covar[2]],
+                 [point_covar[1], point_covar[3], point_covar[4]],
+                 [point_covar[2], point_covar[4], point_covar[5]]]
+            )
+            col_range = coefficient_columns[point_id]
+            weight_mat[col_range[0]:col_range[1], col_range[0]:col_range[1]] = np.linalg.inv(covar_mat)
+
+    return weight_mat
 
 def compute_residuals(network, sensors):
     """