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