Rought bundle adjust utilities

knoten/bundle.py

+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