diff --git a/knoten/bundle.py b/knoten/bundle.py new file mode 100644 index 0000000000000000000000000000000000000000..c897505df573140b4294e21666e42d2ad6ee84fc --- /dev/null +++ b/knoten/bundle.py @@ -0,0 +1,201 @@ +import numpy as np +import pandas as pd +import pvl +import os +import csmapi + +def closest_approach(points, direction): + """ + Compute the point of closest approach between lines. + + Parameters + ---------- + points : ndarray + An n x 3 array of points on each line + direction : ndarray + An n x 3 array of vectors that defines the direction of each line + + Returns + ------- + : array + The (x, y, z) point that is closest to all of the lines + """ + num_lines = points.shape[0] + design_mat = np.zeros((num_lines * 3, 3)) + rhs = np.zeros(num_lines * 3) + for i in range(num_lines): + point = points[i] + line = direction[i] / np.linalg.norm(direction[i]) + design_mat[3*i:3*i+3] = np.identity(3) - np.outer(line, line) + rhs[3*i:3*i+3] = np.dot(point,line) * line + point + closest_point, _ = np.linalg.lstsq(design_mat, rhs) + return closest_point + +def compute_apriori_ground_points(network, sensors): + """ + Compute a priori ground points for all of the free points in a control network. + + Parameters + ---------- + network : DataFrame + The control network as a dataframe generated by plio + sensors : dict + A dictionary that maps ISIS serial numbers to CSM sensors + + Returns + ------- + : DataFrame + The control network dataframe with updated ground points + """ + for point_id, group in df.groupby('id'): + # Free points are type 2 for V2 and V5 control networks + if group.iloc[0]["pointType"] != 2: + continue + positions = [] + look_vecs = [] + for measure_id, row in group.iterrows(): + measure = csmapi.ImageCoord(row["line"], row["sample"]) + locus = sensors[row["serialnumber"]].imageToRemoteImagingLocus(measure) + positions.append([locus.point.x, locus.point.y, locus.point.z]) + look_vecs.append([locus.direction.x, locus.direction.y, locus.direction.z]) + ground_pt = closest_approach(np.array(positions), np.array(look_vecs)) + network.loc[network.id == point_id, ["adjustedX", "adjustedY", "adjustedZ"]] = ground_pt + return network + +class CsmParameter: + """ + Container class that describes a parameter for a CSM sensor model + """ + + def __init__(self, sensor, index): + self.index = index + self.name = sensor.getParameterName(index) + self.type = sensor.getParameterType(index) + self.units = sensor.getParameterUnits(index) + self.value = sensor.setParameterValue(index) + +def get_sensor_parameters(sensor, set="adjustable"): + """ + Get a set of the CSM parameters for a CSM sensor + + Parameters + ---------- + sensor : CSM sensor + The CSM sensor model + set : str + The CSM parameter set to get. Either valid, adjustable, or non_adjustable + + Returns + ------- + : List + A list of CsmParameters + """ + if (set.upper() == "VALID"): + param_set = csmapi.VALID + elif (set.upper() == "ADJUSTABLE"): + param_set = csmapi.ADJUSTABLE + elif (set.upper() == "NON_ADJUSTABLE"): + param_set = csmapi.NON_ADJUSTABLE + else: + raise ValueError(f"Invalid parameter set \"{}\".") + return [CsmParameter(sensor, index) for index in sensor.getParameterSetIndices(param_set)] + +def compute_sensor_partials(sensor, parameters, ground_pt): + """ + Compute the partial derivatives of the line and sample with respect to a set + of parameters. + + Parameters + ---------- + sensor : CSM sensor + The CSM sensor model + parameters : list + The list of CsmParameter to compute the partials W.R.T. + ground_pt : array + The (x, y, z) ground point to compute the partial derivatives at + + Returns + ------- + : ndarray + The 2 x n array of partial derivatives. The first array is the line + partials and the second array is the sample partials. The partial + derivatives are in the same order as the parameter list passed in. + """ + partials = np.zeros((2, len(parameters))) + csm_ground = csmapi.EcefCoord(ground_pt[0], ground_pt[1], ground_pt[2]) + for i in range(len(parameters)): + partials[:, i] = sensor.computeSensorPartials(parameters[i].index, csm_ground) + return partials + +def compute_ground_partials(sensor, ground_pt): + """ + Compute the partial derivatives of the line and sample with respect to a + ground point. + + Parameters + ---------- + sensor : CSM sensor + The CSM sensor model + ground_pt : array + The (x, y, z) ground point to compute the partial derivatives W.R.T. + + Returns + ------- + : ndarray + The 2 x 3 array of partial derivatives. The first array is the line + partials and the second array is the sample partials. The partial + derivatives are in (x, y, z) order. + """ + csm_ground = csmapi.EcefCoord(ground_pt[0], ground_pt[1], ground_pt[2]) + partials = np.array(sensor.computeGroundPartials(csm_ground)) + return np.reshape(partials, (2, 3)) + +def compute_jacobian(network, sensors, parameters): + """ + Compute the Jacobian matrix. + + 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. + 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. + + Returns + ------- + : ndarray + The Jacobian matrix + """ + num_columns = 0 + coefficient_columns = {} + for serial, params in parameters.items(): + coefficient_columns[serial] = num_columns + num_columns += len(params) + for point_id in network["id"].unique(): + # Skip fixed points + if network.loc[network.id == point_id, 0]["pointType"] == 4: + continue + coefficient_columns[serial] = num_columns + num_columns += len(params) + + 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 = sensor[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) + + return jacobian