Loading knoten/bundle.py 0 → 100644 +201 −0 Original line number Diff line number Diff line 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 Loading
knoten/bundle.py 0 → 100644 +201 −0 Original line number Diff line number Diff line 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
