From 43902583a87614c315ff0777e99f770a5571ef08 Mon Sep 17 00:00:00 2001
From: Jesse Mapel <jmapel@usgs.gov>
Date: Mon, 27 Jan 2020 15:35:03 -0700
Subject: [PATCH] Rought bundle adjust utilities
---
knoten/bundle.py | 201 +++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 201 insertions(+)
create mode 100644 knoten/bundle.py
diff --git a/knoten/bundle.py b/knoten/bundle.py
new file mode 100644
index 0000000..c897505
--- /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
--
GitLab