Skip to content
Snippets Groups Projects
Commit f6e1f239 authored by Jesse Mapel's avatar Jesse Mapel
Browse files

Minor updates

parent 86f9fdc4
Branches reduced_normal
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
examples/Reduced_normal_equations.ipynb
+ 184
− 19
View file @ f6e1f239
%% 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_sigma(V, dX, W_params, W_observations)
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_sigma(V, dX, W_params, W_observations)
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_sigma_sparse(V, np.zeros(total_corrections.shape), W_CC_sparse, W_PP, W_observations, column_dict)
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_sigma_sparse(V, updates, W_CC_sparse, W_PP, W_observations, column_dict)
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
```
......
knoten/bundle.py
+ 3
− 3
View file @ f6e1f239
......@@ -435,7 +435,7 @@ def update_parameters(sensors, parameters, network, updates, coefficient_columns
for sn, sensor in sensors.items():
for i, param in enumerate(parameters[sn]):
if i > coefficient_columns[sn][1]:
print('THIS SHOULD BE AN ACTUAL ERROR')
raise IndexError(f'parameter [{param.name}] at index [{i}] is beyond the max index [{coefficient_columns[sn][1]}].')
current_value = sensor.getParameterValue(param.index)
sensor.setParameterValue(param.index, current_value+updates[coefficient_columns[sn][0]+i])
......@@ -446,7 +446,7 @@ def update_parameters(sensors, parameters, network, updates, coefficient_columns
adj = updates[coefficient_columns[point_id][0]:coefficient_columns[point_id][1]]
network.loc[network.id == point_id, ["adjustedX", "adjustedY", "adjustedZ"]] = ground_pt + adj
def compute_sigma(V, dX, W_parameters, W_observations):
def compute_sigma0(V, dX, W_parameters, W_observations):
"""
Computes the resulting standard deviation of the residuals for the current state of the bundle network.
......@@ -475,7 +475,7 @@ def compute_sigma(V, dX, W_parameters, W_observations):
sigma0 = np.sqrt(VTPV/dof)
return sigma0
def compute_sigma_sparse(V, dX, W_sensors, W_points, W_observations, column_dict):
def compute_sigma0_sparse(V, dX, W_sensors, W_points, W_observations, column_dict):
"""
Computes the resulting standard deviation of the residuals for the current state of the bundle network.
......
tests/test_bundle.py
+ 2
− 2
View file @ f6e1f239
......@@ -184,7 +184,7 @@ def test_compute_sigma0():
[0, 0, 0, -7, -8, -9]]
)
dX = np.arange(-6, 0)
assert bundle.compute_sigma(V, dX, W_params, W_obs) == np.sqrt(7809 / 10)
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
......@@ -198,7 +198,7 @@ def test_compute_sigma0_sparse():
"image_1" : (0, 3),
"point_1" : (3, 6)
}
assert bundle.compute_sigma_sparse(V, dX, W_sensors, W_points, W_obs, column_dict) == np.sqrt(7809 / 10)
assert bundle.compute_sigma0_sparse(V, dX, W_sensors, W_points, W_obs, column_dict) == np.sqrt(7809 / 10)
def test_compute_image_weight():
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment