Commit 6c7a4459 authored by Jesse Mapel's avatar Jesse Mapel Committed by GitHub
Browse files

Added reduced normal equations nb (#82) 

* Added reduced normal equations nb

* Made cube paths relative to examples dir

* Moved reduced code into bundle.py

* Added Sigma0 tests

* Added tests that needed mocks

* Minor updates
parent e7fcf5cd

examples/Reduced_normal_equations.ipynb

0 → 100644
+394 −0
Original line number Diff line number Diff line 
%% Cell type:code id: tags:



``` python

import os

from plio.io import io_controlnetwork

from knoten.csm import create_csm

from scipy import sparse

import ale

import csmapi

import numpy as np

from collections import OrderedDict



import matplotlib.pyplot as plt



from knoten.bundle import *

```



%% Output



    /Users/jmapel/miniconda3/envs/knoten/lib/python3.7/site-packages/ale/__init__.py:22: UserWarning: ALESPICEROOT environment variable not set, Spice Drivers will not function correctly

      warnings.warn('ALESPICEROOT environment variable not set, Spice Drivers will not function correctly')



%% Cell type:code id: tags:



``` python

cubes = 'data/cubes.lis'

sensors = generate_sensors(cubes)



network = 'data/hand_dense.net'

cnet = io_controlnetwork.from_isis(network)

sensors = {sn: sensors[sn] for sn in cnet["serialnumber"].unique()}

cnet = compute_apriori_ground_points(cnet, sensors) # autoseed did not generate ground points, calculate and repopulate the data frame

```



%% Cell type:code id: tags:



``` python

all_parameters = {sn: get_sensor_parameters(sensor) for sn, sensor in sensors.items()}

for sn, parameters in all_parameters.items():

    print(f"Image: {sn}")

    for param in parameters:

        print(f"  {param.name} | {param.index} | {param.value}")

```



%% Output



    Image: MRO/CTX/1085197697:073

      IT Pos. Bias    | 0 | 0.0

      CT Pos. Bias    | 1 | 0.0

      Rad Pos. Bias   | 2 | 0.0

      IT Vel. Bias    | 3 | 0.0

      CT Vel. Bias    | 4 | 0.0

      Rad Vel. Bias   | 5 | 0.0

      Omega Bias      | 6 | 0.0

      Phi Bias        | 7 | 0.0

      Kappa Bias      | 8 | 0.0

      Omega Rate      | 9 | 0.0

      Phi Rate        | 10 | 0.0

      Kappa Rate      | 11 | 0.0

      Omega Accl      | 12 | 0.0

      Phi Accl        | 13 | 0.0

      Kappa Accl      | 14 | 0.0

      Focal Bias      | 15 | 0.0

    Image: MRO/CTX/1157902986:250

      IT Pos. Bias    | 0 | 0.0

      CT Pos. Bias    | 1 | 0.0

      Rad Pos. Bias   | 2 | 0.0

      IT Vel. Bias    | 3 | 0.0

      CT Vel. Bias    | 4 | 0.0

      Rad Vel. Bias   | 5 | 0.0

      Omega Bias      | 6 | 0.0

      Phi Bias        | 7 | 0.0

      Kappa Bias      | 8 | 0.0

      Omega Rate      | 9 | 0.0

      Phi Rate        | 10 | 0.0

      Kappa Rate      | 11 | 0.0

      Omega Accl      | 12 | 0.0

      Phi Accl        | 13 | 0.0

      Kappa Accl      | 14 | 0.0

      Focal Bias      | 15 | 0.0

    Image: MRO/CTX/1096561308:045

      IT Pos. Bias    | 0 | 0.0

      CT Pos. Bias    | 1 | 0.0

      Rad Pos. Bias   | 2 | 0.0

      IT Vel. Bias    | 3 | 0.0

      CT Vel. Bias    | 4 | 0.0

      Rad Vel. Bias   | 5 | 0.0

      Omega Bias      | 6 | 0.0

      Phi Bias        | 7 | 0.0

      Kappa Bias      | 8 | 0.0

      Omega Rate      | 9 | 0.0

      Phi Rate        | 10 | 0.0

      Kappa Rate      | 11 | 0.0

      Omega Accl      | 12 | 0.0

      Phi Accl        | 13 | 0.0

      Kappa Accl      | 14 | 0.0

      Focal Bias      | 15 | 0.0

    Image: MRO/CTX/1136952576:186

      IT Pos. Bias    | 0 | 0.0

      CT Pos. Bias    | 1 | 0.0

      Rad Pos. Bias   | 2 | 0.0

      IT Vel. Bias    | 3 | 0.0

      CT Vel. Bias    | 4 | 0.0

      Rad Vel. Bias   | 5 | 0.0

      Omega Bias      | 6 | 0.0

      Phi Bias        | 7 | 0.0

      Kappa Bias      | 8 | 0.0

      Omega Rate      | 9 | 0.0

      Phi Rate        | 10 | 0.0

      Kappa Rate      | 11 | 0.0

      Omega Accl      | 12 | 0.0

      Phi Accl        | 13 | 0.0

      Kappa Accl      | 14 | 0.0

      Focal Bias      | 15 | 0.0



%% Cell type:code id: tags:



``` python

# Solve for angles and angular rates

solve_parameters = {sn: params[6:12] for sn, params in all_parameters.items()}

for sn, parameters in solve_parameters.items():

    print(f"Image: {sn}")

    for param in parameters:

        print(f"  {param.name} | {param.index} | {param.value}")

```



%% Output



    Image: MRO/CTX/1085197697:073

      Omega Bias      | 6 | 0.0

      Phi Bias        | 7 | 0.0

      Kappa Bias      | 8 | 0.0

      Omega Rate      | 9 | 0.0

      Phi Rate        | 10 | 0.0

      Kappa Rate      | 11 | 0.0

    Image: MRO/CTX/1157902986:250

      Omega Bias      | 6 | 0.0

      Phi Bias        | 7 | 0.0

      Kappa Bias      | 8 | 0.0

      Omega Rate      | 9 | 0.0

      Phi Rate        | 10 | 0.0

      Kappa Rate      | 11 | 0.0

    Image: MRO/CTX/1096561308:045

      Omega Bias      | 6 | 0.0

      Phi Bias        | 7 | 0.0

      Kappa Bias      | 8 | 0.0

      Omega Rate      | 9 | 0.0

      Phi Rate        | 10 | 0.0

      Kappa Rate      | 11 | 0.0

    Image: MRO/CTX/1136952576:186

      Omega Bias      | 6 | 0.0

      Phi Bias        | 7 | 0.0

      Kappa Bias      | 8 | 0.0

      Omega Rate      | 9 | 0.0

      Phi Rate        | 10 | 0.0

      Kappa Rate      | 11 | 0.0



%% Cell type:code id: tags:



``` python

column_dict = compute_coefficient_columns(cnet, sensors, solve_parameters)

```



%% Cell type:code id: tags:



``` python

num_observations = 2 * len(cnet)

W_observations = np.eye(num_observations) # this is a place holder until Jesse adds his calculations

W_params = compute_parameter_weights(cnet, sensors, solve_parameters, column_dict)

dof = W_observations.shape[0] - W_params.shape[0]

```



%% Cell type:code id: tags:



``` python

max_iterations = 10

tol = 1e-10

```



%% Cell type:code id: tags:



``` python

num_sensor_params = sum([len(parameters) for parameters in solve_parameters.values()])

num_point_params = 3 * len(cnet["id"].unique())

```



%% Cell type:code id: tags:



``` python

sensors = generate_sensors(cubes) # generate sensors

cnet = io_controlnetwork.from_isis(network) # load in network

cnet = compute_apriori_ground_points(cnet, sensors) # calculate ground points



column_dict = compute_coefficient_columns(cnet, sensors, solve_parameters)

num_parameters = max(col_range[1] for col_range in column_dict.values())

num_observations = 2 * len(cnet)

W_observations = np.eye(num_observations)

W_params = compute_parameter_weights(cnet, sensors, solve_parameters, column_dict)



iteration = 0

V = compute_residuals(cnet, sensors)

dX = np.zeros(W_params.shape[0]) #initialize for sigma calculatioN

sigma0 = compute_sigma0(V, dX, W_params, W_observations)

print(f'iteration {iteration}: sigma0 = {sigma0}\n')



total_correction_dense = np.zeros(num_parameters)

for i in range(max_iterations):

    iteration += 1

    old_sigma0 = sigma0



    J = compute_jacobian(cnet, sensors, solve_parameters, column_dict)

    N = J.T.dot(W_observations).dot(J) + W_params # calculate the normal equation

    C = J.T.dot(W_observations).dot(V) - W_params.dot(total_correction_dense)

    dX = np.linalg.inv(N).dot(C) #calculate change in camera parameters and ground points

    total_correction_dense += dX

    print(f'corrections: mean = {dX.mean()} min = {dX.min()} max = {dX.max()}')



    update_parameters(sensors, solve_parameters, cnet, dX, column_dict)



    V = compute_residuals(cnet, sensors)

    sigma0 = compute_sigma0(V, dX, W_params, W_observations)

    sigma0 = np.sqrt((V.dot(W_observations).dot(V) + dX.dot(W_params).dot(dX))/dof)

    print(f'iteration {iteration}: sigma0 = {sigma0}\n')



    if (abs(sigma0 - old_sigma0) < tol):

        print(f'change in sigma0 of {abs(sigma0 - old_sigma0)} converged!')

        break



```



%% Output



    iteration 0: sigma0 = 4.204919720360203

    

    corrections: mean = 0.05349160077573805 min = -14.251221291713048 max = 40.834990964537575

    iteration 1: sigma0 = 1.3539063538494558

    

    corrections: mean = 2.7124445740181957e-06 min = -0.011426521506164248 max = 0.011842165594603658

    iteration 2: sigma0 = 1.127336617096085

    

    corrections: mean = 4.600122255126198e-08 min = -1.8475863314383526e-06 max = 4.573929399245028e-06

    iteration 3: sigma0 = 1.1273365324778626

    

    corrections: mean = 1.879394505450981e-11 min = -1.283151025419733e-09 max = 1.8514518745707175e-09

    iteration 4: sigma0 = 1.127336532470605

    

    change in sigma0 of 7.257527911974648e-12 converged!



%% Cell type:code id: tags:



``` python

sensors = generate_sensors(cubes) # generate sensors

cnet = io_controlnetwork.from_isis(network) # load in network

cnet = compute_apriori_ground_points(cnet, sensors) # calculate ground points



# This is setup once per bundle and then accumulates

total_corrections = np.zeros(num_sensor_params + num_point_params)



# These are computed once per bundle

W_CC_sparse = sparse.csc_matrix((num_sensor_params, num_sensor_params))

W_PP = {}



# Compute image param weight matrices

for sn, params in solve_parameters.items():

    coeff_indices = column_dict[sn]

    coeff_range = np.arange(coeff_indices[0], coeff_indices[1])

    num_image_coeffs = coeff_indices[1] - coeff_indices[0]

    W_CC_data = compute_image_weight(sensors[sn], params).ravel()

    W_CC_row = np.repeat(coeff_range, num_image_coeffs)

    W_CC_column = np.tile(coeff_range, num_image_coeffs)

    W_CC_sparse += sparse.coo_matrix((W_CC_data, (W_CC_row, W_CC_column)), (num_sensor_params, num_sensor_params)).tocsc()



# Compute point param weight matrices

for point_id in cnet['id'].unique():

    W_PP[point_id] = compute_point_weight(cnet, point_id)



V = compute_residuals(cnet, sensors)

sigma0 = compute_sigma0_sparse(V, np.zeros(total_corrections.shape), W_CC_sparse, W_PP, W_observations, column_dict)



# Start iteration logic

for i in range(max_iterations):

    old_sigma0 = sigma0



    H_CC_sparse = sparse.csc_matrix((num_sensor_params, num_sensor_params))

    H_CC_sparse += W_CC_sparse



    g_C_sparse = np.zeros(num_sensor_params)

    g_C_sparse -= W_CC_sparse.dot(total_corrections[:num_sensor_params])

    # g_C_sparse += W_CC_sparse.dot(sensor_corrections)



    # Q = H_PP^-1 * H_PC

    Q_mats = {}

    # NIC = H_PP^-1 * g_P

    NIC_vecs = {}



    updates = np.zeros(num_sensor_params + num_point_params)



    for point_id, group in cnet.groupby('id'):

        ground_pt = group.iloc[0][["adjustedX", "adjustedY", "adjustedZ"]]

        H_CP = sparse.csc_matrix((num_sensor_params, 3))

        H_PP = np.zeros((3, 3))

        g_P = np.zeros(3)

        for measure_idx, row in group.iterrows():

            serial = row["serialnumber"]

            sensor = sensors[serial]

            point_partials = compute_ground_partials(sensor, ground_pt)

            sensor_partials = compute_sensor_partials(sensor, solve_parameters[serial], ground_pt)

            coeff_indices = column_dict[serial]

            coeff_range = np.arange(coeff_indices[0], coeff_indices[1])

            num_image_coeffs = coeff_indices[1] - coeff_indices[0]



            H_CC_point_data = np.dot(sensor_partials.T, sensor_partials).ravel()

            H_CC_point_row = np.repeat(coeff_range, num_image_coeffs)

            H_CC_point_column = np.tile(coeff_range, num_image_coeffs)

            H_CC_sparse += sparse.coo_matrix((H_CC_point_data, (H_CC_point_row, H_CC_point_column)), (num_sensor_params, num_sensor_params)).tocsc()



            H_CP_point_data = np.dot(sensor_partials.T, point_partials).ravel()

            H_CP_point_row = np.repeat(coeff_range, 3)

            H_CP_point_column = np.tile(np.arange(0, 3), num_image_coeffs)

            H_CP += sparse.coo_matrix((H_CP_point_data, (H_CP_point_row, H_CP_point_column)), (num_sensor_params, 3)).tocsc()



            H_PP += np.dot(point_partials.T, point_partials)



            g_C_sparse[coeff_indices[0]:coeff_indices[1]] += np.dot(sensor_partials.T, V[2*measure_idx:2*measure_idx+2])

            g_P += np.dot(point_partials.T, V[2*measure_idx:2*measure_idx+2])

        point_param_range = column_dict[point_id]

        g_P -= W_PP[point_id].dot(total_corrections[point_param_range[0]:point_param_range[1]])

        H_PP += W_PP[point_id]

        H_PP_inv = sparse.csc_matrix(np.linalg.inv(H_PP))

        Q_mats[point_id] = H_PP_inv.dot(H_CP.transpose())

        NIC_vecs[point_id] = H_PP_inv.dot(g_P)

        H_CC_sparse -= H_CP.dot(Q_mats[point_id])

        g_C_sparse -= H_CP.dot(NIC_vecs[point_id])



    updates[:num_sensor_params] = sparse.linalg.spsolve(H_CC_sparse, g_C_sparse)



    for point_id in Q_mats:

        point_param_indices = column_dict[point_id]

        updates[point_param_indices[0]:point_param_indices[1]] = NIC_vecs[point_id] - Q_mats[point_id].dot(updates[:num_sensor_params])



    print(f'corrections: mean = {updates.mean()} min = {updates.min()} max = {updates.max()}')



    total_corrections += updates



    update_parameters(sensors, solve_parameters, cnet, updates, column_dict)

    V = compute_residuals(cnet, sensors)

    sigma0 = compute_sigma0_sparse(V, updates, W_CC_sparse, W_PP, W_observations, column_dict)

    print(f'iteration {i+1}: sigma0 = {sigma0}\n')



    if (abs(sigma0 - old_sigma0) < tol):

        print(f'change in sigma0 of {abs(sigma0 - old_sigma0)} converged!')

        break

```



%% Output



    corrections: mean = 0.05349160077573873 min = -14.251221291713076 max = 40.834990964537546

    iteration 1: sigma0 = 1.353906353849451

    

    corrections: mean = 2.712443886869334e-06 min = -0.011426521492657233 max = 0.011842165579696888

    iteration 2: sigma0 = 1.1273366170980164

    

    corrections: mean = 4.6016402655676494e-08 min = -1.8475512374681445e-06 max = 4.5737885812869825e-06

    iteration 3: sigma0 = 1.1273365324789022

    

    corrections: mean = 3.7306738076770124e-12 min = -1.429027500961002e-09 max = 1.6954805056745274e-09

    iteration 4: sigma0 = 1.1273365324578994

    

    change in sigma0 of 2.100275509064886e-11 converged!



%% Cell type:code id: tags:



``` python

print("Sensor diff")

print(np.min(total_correction_dense[:num_sensor_params].flatten() - total_corrections[:num_sensor_params]))

print(np.max(total_correction_dense[:num_sensor_params].flatten() - total_corrections[:num_sensor_params]))

print("Point diff")

print(np.min(total_correction_dense[num_sensor_params:].flatten() - total_corrections[num_sensor_params:]))

print(np.max(total_correction_dense[num_sensor_params:].flatten() - total_corrections[num_sensor_params:]))

```



%% Output



    Sensor diff

    -1.450808806424675e-09

    1.2130136894938914e-09

    Point diff

    -2.297802850770303e-10

    4.665956510052638e-10



%% Cell type:code id: tags:



``` python

```

examples/data/cubes.lis

+4 −4
Original line number Diff line number Diff line 
/work/projects/control_network_metrics/distribution/lvl1/F02_036648_2021_XN_22N022W.cal.cub

/work/projects/control_network_metrics/distribution/lvl1/F06_038336_2024_XN_22N022W.cal.cub

/work/projects/control_network_metrics/distribution/lvl1/F22_044336_2020_XN_22N022W.cal.cub

/work/projects/control_network_metrics/distribution/lvl1/J07_047448_2024_XN_22N022W.cal.cub
 
data/F02_036648_2021_XN_22N022W.cal.cub

data/F06_038336_2024_XN_22N022W.cal.cub

data/F22_044336_2020_XN_22N022W.cal.cub

data/J07_047448_2024_XN_22N022W.cal.cub

knoten/bundle.py

+142 −5

File changed.

Preview size limit exceeded, changes collapsed.

tests/test_bundle.py

+100 −0
Original line number Diff line number Diff line
@@ -171,3 +171,103 @@ def test_compute_residuals(control_network, sensors): 
    V = bundle.compute_residuals(control_network, sensors)

    assert V.shape == (18,)

    np.testing.assert_allclose(V, [0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, -0.9, -0.8, -0.7, -0.6, -0.5, -0.4, -0.3, -0.2, -0.1])



def test_compute_sigma0():

    V = np.arange(0, 16) + 1

    W_obs = np.diag(np.arange(16, 0, -1))

    W_params = np.array(

        [[1,  2,  3,  0,  0,  0],

         [4,  5,  6,  0,  0,  0],

         [7,  8,  9,  0,  0,  0],

         [0,  0,  0, -1, -2, -3],

         [0,  0,  0, -4, -5, -6],

         [0,  0,  0, -7, -8, -9]]

     )

    dX = np.arange(-6, 0)

    assert bundle.compute_sigma0(V, dX, W_params, W_obs) == np.sqrt(7809 / 10)



def test_compute_sigma0_sparse():

    V = np.arange(0, 16) + 1

    W_obs = np.diag(np.arange(16, 0, -1))

    W_sensors  = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]])

    W_points = {

        "point_1" : np.array([[-1, -2, -3], [-4, -5, -6], [-7, -8, -9]])

    }

    dX = np.arange(-6, 0)

    column_dict = {

        "image_1" : (0, 3),

        "point_1" : (3, 6)

    }

    assert bundle.compute_sigma0_sparse(V, dX, W_sensors, W_points, W_obs, column_dict) == np.sqrt(7809 / 10)





def test_compute_image_weight():

    def mock_covar(index_1, index_2):

        if index_1 != index_2:

            return 0

        return index_1 + 1



    sensor = mock.MagicMock(spec=csmapi.RasterGM)

    sensor.getParameterCovariance = mock_covar

    params = [mock.MagicMock(), mock.MagicMock(), mock.MagicMock()]

    params[0].index = 0

    params[1].index = 1

    params[2].index = 3



    np.testing.assert_allclose(

        bundle.compute_image_weight(sensor, params),

        [[1, 0,   0],

         [0, 1/2, 0],

         [0, 0,   1/4]])



def test_compute_point_weight(control_network):

    control_network.at[(control_network['id'] == 'bob').idxmax(), 'aprioriCovar'] = np.array([1, 0, 0, 2, 0, 3])

    np.testing.assert_allclose(

        bundle.compute_point_weight(control_network, 'bob'),

        [[1, 0,   0],

         [0, 1/2, 0],

         [0, 0,   1/3]])



def test_update_parameters(control_network, sensors):

    def mock_getParameterValue(index):

        return index + 1



    params = {}

    coefficient_columns = {}

    current_column = 0



    for sn, sensor in sensors.items():

        sensor.getParameterValue = mock_getParameterValue

        sensor_params = []

        for param_idx in range(6):

            param_mock = mock.MagicMock()

            param_mock.index = param_idx

            sensor_params.append(param_mock)

        params[sn] = sensor_params

        coefficient_columns[sn] = (current_column, current_column+6)

        current_column += 6



    for point_id in control_network['id'].unique():

        coefficient_columns[point_id] = (current_column, current_column+3)

        current_column += 3



    updates = np.arange(0, current_column)



    bundle.update_parameters(sensors, params, control_network, updates, coefficient_columns)



    for sn, sensor in sensors.items():

        coeff_start = coefficient_columns[sn][0]

        for param_idx in range(6):

            sensor.setParameterValue.assert_any_call(

                param_idx,

                param_idx + 1 + coeff_start + param_idx

            )



    for point_id, group in control_network.groupby('id'):

        coeff_start = coefficient_columns[point_id][0]

        coeff_end = coefficient_columns[point_id][1]

        num_measures = len(group)

        np.testing.assert_allclose(

            group[['adjustedX', 'adjustedY', 'adjustedZ']].values,

            np.tile(updates[coeff_start:coeff_end], num_measures).reshape((num_measures,3))

        )