Jessemapel bundle adjust (#71)

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

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)