From ec482a89c6a12faba68e812e1c79729063356cfe Mon Sep 17 00:00:00 2001 From: Lauren Adoram-Kershner <42873279+ladoramkershner@users.noreply.github.com> Date: Tue, 28 Jan 2020 13:58:17 -0700 Subject: [PATCH] Jessemapel bundle adjust (#71) * added to bundle adjust nb, up to setting up bundle iteration, known issues * Rought bundle adjust utilities * bug fixes and adding tests Co-authored-by: Jesse Mapel <jam826@nau.edu> --- knoten/bundle.py | 206 +++++++++++++++++++++++++++++++++++++++++++ tests/test_bundle.py | 143 ++++++++++++++++++++++++++++++ 2 files changed, 349 insertions(+) create mode 100644 knoten/bundle.py create mode 100644 tests/test_bundle.py diff --git a/knoten/bundle.py b/knoten/bundle.py new file mode 100644 index 0000000..789b4e5 --- /dev/null +++ b/knoten/bundle.py @@ -0,0 +1,206 @@ +import numpy as np +import pandas as pd +import pvl +import os +import csmapi +from collections import OrderedDict + +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, rcond=None)[0] + 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 network.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, ["aprioriX", "aprioriY", "aprioriZ"]] = ground_pt + 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.getParameterValue(index) + + def __repr__(self): + return f'{self.index} {self.name.strip()} ({self.type}): {self.value} {self.units}' + +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 \"{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 = OrderedDict() + for serial in network["serialnumber"].unique(): + coefficient_columns[serial] = num_columns + num_columns += len(parameters[serial]) + 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 + + 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) + + return jacobian diff --git a/tests/test_bundle.py b/tests/test_bundle.py new file mode 100644 index 0000000..657015c --- /dev/null +++ b/tests/test_bundle.py @@ -0,0 +1,143 @@ +from unittest import mock +import pytest + +import numpy as np +import pandas as pd +from knoten import bundle +from csmapi import csmapi + +@pytest.fixture +def control_network(): + df_dict = { + 'id': ['bob', 'bob', 'bob', 'tim', 'tim', 'sally', 'sally', 'sally', 'sally'], + 'serialnumber': ['a', 'b', 'c', 'd', 'b', 'a', 'b', 'c', 'd'], + 'line': np.arange(9), + 'sample': np.arange(9)[::-1], + 'aprioriX': np.zeros(9), + 'aprioriX': np.zeros(9), + 'aprioriY': np.zeros(9), + 'aprioriZ': np.zeros(9), + 'adjustedX': np.zeros(9), + 'adjustedY': np.zeros(9), + 'adjustedZ': np.zeros(9), + 'pointType': [2, 2, 2, 3, 3, 2, 2, 2, 2] + } + return pd.DataFrame(df_dict) + +@pytest.fixture +def sensors(): + sensors = { + 'a': mock.MagicMock(spec=csmapi.RasterGM), + 'b': mock.MagicMock(spec=csmapi.RasterGM), + 'c': mock.MagicMock(spec=csmapi.RasterGM), + 'd': mock.MagicMock(spec=csmapi.RasterGM) + } + return sensors + +def test_closest_approach_intersect(): + points = np.array([[-1, 1, 2], [0, 2, 2], [0, 1, 3]]) + directions = np.array([[1, 0, 0], [0, 2, 0], [0, 0, -1]]) + res = bundle.closest_approach(points, directions) + + np.testing.assert_allclose(res, [0, 1, 2]) + +def test_closest_approach_no_intersect(): + points = np.array([[-1, 1, 2], [0.5, 1-np.sqrt(3)/2.0, 2], [0.5, 1+np.sqrt(3)/2.0, 4]]) + directions = np.array([[0, 1, 0], [np.sqrt(3)/2.0, 0.5, 0], [0, 0, 1]]) + res = bundle.closest_approach(points, directions) + + np.testing.assert_allclose(res, [0, 1, 2], atol=1e-12) + +def test_compute_ground_points(control_network, sensors): + expected_bob = np.array([1.0, 7.0, 0.0]) + expected_sally = np.array([6.5, 1.5, 0.0]) + + with mock.patch('knoten.bundle.closest_approach', side_effect=[expected_bob, expected_sally]) as mock_closest: + out_df = bundle.compute_apriori_ground_points(control_network, sensors) + mock_closest.assert_called() + + np.testing.assert_array_equal( + out_df[out_df.id == "bob"][["aprioriX", "aprioriY", "aprioriZ"]].values, + np.repeat(expected_bob[:, None], 3, axis=1).T) + np.testing.assert_array_equal( + out_df[out_df.id == "bob"][["adjustedX", "adjustedY", "adjustedZ"]].values, + np.repeat(expected_bob[:, None], 3, axis=1).T) + np.testing.assert_array_equal( + out_df[out_df.id == "tim"][["aprioriX", "aprioriY", "aprioriZ"]].values, + np.zeros((2, 3))) + np.testing.assert_array_equal( + out_df[out_df.id == "tim"][["adjustedX", "adjustedY", "adjustedZ"]].values, + np.zeros((2, 3))) + np.testing.assert_array_equal( + out_df[out_df.id == "sally"][["aprioriX", "aprioriY", "aprioriZ"]].values, + np.repeat(expected_sally[:, None], 4, axis=1).T) + np.testing.assert_array_equal( + out_df[out_df.id == "sally"][["adjustedX", "adjustedY", "adjustedZ"]].values, + np.repeat(expected_sally[:, None], 4, axis=1).T) + +def test_get_sensor_parameter(): + mock_sensor = mock.MagicMock(spec=csmapi.RasterGM) + mock_sensor.getParameterSetIndices.return_value = [0, 1, 2] + parameters = bundle.get_sensor_parameters(mock_sensor) + + mock_sensor.getParameterSetIndices.assert_called() + assert len(parameters) == 3 + +def test_csm_parameters(): + mock_sensor = mock.MagicMock(spec=csmapi.RasterGM) + test_parameter = bundle.CsmParameter(mock_sensor, 5) + + mock_sensor.getParameterName.assert_called_with(5) + mock_sensor.getParameterType.assert_called_with(5) + mock_sensor.getParameterUnits.assert_called_with(5) + mock_sensor.getParameterValue.assert_called_with(5) + assert test_parameter.index == 5 + assert test_parameter.name == mock_sensor.getParameterName.return_value + assert test_parameter.type == mock_sensor.getParameterType.return_value + assert test_parameter.units == mock_sensor.getParameterUnits.return_value + assert test_parameter.value == mock_sensor.getParameterValue.return_value + +def test_compute_sensor_partials(): + ground_pt = [9, 8, 10] + sensor = mock.MagicMock(spec=csmapi.RasterGM) + sensor.computeSensorPartials.side_effect = [(5, 3), (4, 2), (6, 1)] + parameters = [mock.MagicMock(), mock.MagicMock(), mock.MagicMock()] + partials = bundle.compute_sensor_partials(sensor, parameters, ground_pt) + + np.testing.assert_array_equal(partials, [(5, 4, 6), (3, 2, 1)]) + +def test_compute_ground_partials(): + ground_pt = [9, 8, 10] + sensor = mock.MagicMock(spec=csmapi.RasterGM) + sensor.computeGroundPartials.return_value = (1, 2, 3, 4, 5, 6) + partials = bundle.compute_ground_partials(sensor, ground_pt) + np.testing.assert_array_equal(partials, [[1, 2, 3], [4, 5, 6]]) + +def test_compute_jacobian(control_network, sensors): + parameters = {sn: [mock.MagicMock()]*2 for sn in sensors} + sensor_partials = [(i+1) * np.ones((2, 2)) for i in range(9)] + ground_partials = [-(i+1) * np.ones((2, 3)) for i in range(9)] + with mock.patch('knoten.bundle.compute_sensor_partials', side_effect=sensor_partials) as sensor_par_mock, \ + mock.patch('knoten.bundle.compute_ground_partials', side_effect=ground_partials) as ground_par_mock: + J = bundle.compute_jacobian(control_network, sensors, parameters) + + expected_J = [ + [1, 1, 0, 0, 0, 0, 0, 0, -1, -1, -1, 0, 0, 0, 0, 0, 0], + [1, 1, 0, 0, 0, 0, 0, 0, -1, -1, -1, 0, 0, 0, 0, 0, 0], + [0, 0, 2, 2, 0, 0, 0, 0, -2, -2, -2, 0, 0, 0, 0, 0, 0], + [0, 0, 2, 2, 0, 0, 0, 0, -2, -2, -2, 0, 0, 0, 0, 0, 0], + [0, 0, 0, 0, 3, 3, 0, 0, -3, -3, -3, 0, 0, 0, 0, 0, 0], + [0, 0, 0, 0, 3, 3, 0, 0, -3, -3, -3, 0, 0, 0, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 4, 4, 0, 0, 0, -4, -4, -4, 0, 0, 0], + [0, 0, 0, 0, 0, 0, 4, 4, 0, 0, 0, -4, -4, -4, 0, 0, 0], + [0, 0, 5, 5, 0, 0, 0, 0, 0, 0, 0, -5, -5, -5, 0, 0, 0], + [0, 0, 5, 5, 0, 0, 0, 0, 0, 0, 0, -5, -5, -5, 0, 0, 0], + [6, 6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -6, -6, -6], + [6, 6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -6, -6, -6], + [0, 0, 7, 7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -7, -7, -7], + [0, 0, 7, 7, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -7, -7, -7], + [0, 0, 0, 0, 8, 8, 0, 0, 0, 0, 0, 0, 0, 0, -8, -8, -8], + [0, 0, 0, 0, 8, 8, 0, 0, 0, 0, 0, 0, 0, 0, -8, -8, -8], + [0, 0, 0, 0, 0, 0, 9, 9, 0, 0, 0, 0, 0, 0, -9, -9, -9], + [0, 0, 0, 0, 0, 0, 9, 9, 0, 0, 0, 0, 0, 0, -9, -9, -9]] + np.testing.assert_array_equal(J, expected_J) -- GitLab