diff --git a/knoten/bundle.py b/knoten/bundle.py index ed64bdc82a3c1a1df7aa880ccffd956fb3c12d42..967f3a6f5685d465563b3ad6ddaef3b99238eeef 100644 --- a/knoten/bundle.py +++ b/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): """