Skip to content
Snippets Groups Projects
Commit a2823b27 authored by Lauren Adoram-Kershner's avatar Lauren Adoram-Kershner Committed by GitHub
Browse files

Updating bundle_adjust NB and creating data_snooping logic (#76)


* added to bundle adjust nb, up to setting up bundle iteration, known issues

* Updated Bundle Adjust notebook

* Updated weights and sigma0 calculations

* Now converging!

* updating ba nb to use function calls for processes and creating data_snooping nb logic

Co-authored-by: default avatarJesse Mapel <jam826@nau.edu>
parent cfc0972b
No related branches found
No related tags found
No related merge requests found
......@@ -3,6 +3,8 @@ import pandas as pd
import pvl
import os
import csmapi
import itertools
from math import floor
from pysis import isis
from ale.drivers import loads
......@@ -64,6 +66,9 @@ def closest_approach(points, direction):
-------
: array
The (x, y, z) point that is closest to all of the lines
: ndarray
The (x, y, z) covariance matrix that describes the uncertaintly of the
point
"""
num_lines = points.shape[0]
design_mat = np.zeros((num_lines * 3, 3))
......@@ -73,8 +78,10 @@ def closest_approach(points, direction):
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
N_inv = np.linalg.inv(design_mat.T.dot(design_mat))
closest_point = N_inv.dot(design_mat.T).dot(rhs)
return closest_point, N_inv
def compute_apriori_ground_points(network, sensors):
"""
......@@ -103,9 +110,15 @@ def compute_apriori_ground_points(network, sensors):
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))
ground_pt, covar_mat = closest_approach(np.array(positions), np.array(look_vecs))
covar_vec = [covar_mat[0,0], covar_mat[0,1], covar_mat[0,2],
covar_mat[1,1], covar_mat[1,2], covar_mat[2,2]]
network.loc[network.id == point_id, ["aprioriX", "aprioriY", "aprioriZ"]] = ground_pt
network.loc[network.id == point_id, ["adjustedX", "adjustedY", "adjustedZ"]] = ground_pt
network.loc[network.id == point_id, ["adjustedX", "adjustedY", "adjustedZ"]] = ground_pt
# We have to do a separate loop to assign a list to a single cell
for measure_id, row in group.iterrows():
network.at[measure_id, 'aprioriCovar'] = covar_vec
return network
class CsmParameter:
......@@ -199,55 +212,135 @@ def compute_ground_partials(sensor, ground_pt):
partials = np.array(sensor.computeGroundPartials(csm_ground))
return np.reshape(partials, (2, 3))
def compute_jacobian(network, sensors, parameters):
def compute_coefficient_columns(network, sensors, parameters):
"""
Compute the Jacobian matrix.
Compute the columns for different coefficients
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.
The control network as a dataframe generated by plio.
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.
solve for.
Returns
-------
: ndarray
The Jacobian matrix
: OrderedDict
Dictionary that maps serial numbers and point IDs to the column range
their parameters are in 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])
coefficient_columns[serial] = (coefficient_columns[serial], num_columns)
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
coefficient_columns[point_id] = (coefficient_columns[point_id], num_columns)
return coefficient_columns
num_rows = len(network) * 2
def compute_jacobian(network, sensors, parameters, coefficient_columns):
"""
Compute the Jacobian matrix.
Parameters
----------
network : DataFrame
The control network as a dataframe generated by plio.
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.
coefficient_columns : OrderedDict
Dictionary that maps serial numbers and point IDs to
the column range their parameters are in the Jacobian
matrix.
Returns
-------
: ndarray
The Jacobian matrix
"""
num_columns = max([col_range[1] for col_range in coefficient_columns.values()])
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)
image_range = coefficient_columns[serial]
point_range = coefficient_columns[row["id"]]
jacobian[2*i : 2*i+2, image_range[0] : image_range[1]] = compute_sensor_partials(sensor, params, ground_pt)
jacobian[2*i : 2*i+2, point_range[0] : point_range[1]] = compute_ground_partials(sensor, ground_pt)
return jacobian
return jacobian, coefficient_columns
def compute_parameter_weights(network, sensors, parameters, coefficient_columns):
"""
Compute the parameter weight matrix
Parameters
----------
network : DataFrame
The control network as a dataframe generated by plio.
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.
coefficient_columns : OrderedDict
Dictionary that maps serial numbers and point IDs to
the column range their parameters are in the Jacobian
matrix. Their parameters weights will have the same
ordering in the weight matrix.
Returns
-------
: ndarray
The parameter weight matrix
"""
num_params = max([col_range[1] for col_range in coefficient_columns.values()])
weight_mat = np.zeros((num_params, num_params))
# Image parameters
for sn, params in parameters.items():
param_count = len(params)
covar_mat = np.zeros((param_count, param_count))
for a, b in itertools.product(range(param_count), range(param_count)):
covar_mat[a, b] = sensors[sn].getParameterCovariance(params[a].index, params[b].index)
col_range = coefficient_columns[sn]
weight_mat[col_range[0]:col_range[1], col_range[0]:col_range[1]] = np.linalg.inv(covar_mat)
# Point parameters
for point_id, group in network.groupby('id'):
## If there is no covariance matrix, then just continue on
point_covar = list(group.iloc[0]["aprioriCovar"])
if len(point_covar) != 6:
continue
# The covariance matrix is stored as just one triangle, so we have
# to unpack it.
if len(point_covar) == 6:
covar_mat = np.array(
[[point_covar[0], point_covar[1], point_covar[2]],
[point_covar[1], point_covar[3], point_covar[4]],
[point_covar[2], point_covar[4], point_covar[5]]]
)
col_range = coefficient_columns[point_id]
weight_mat[col_range[0]:col_range[1], col_range[0]:col_range[1]] = np.linalg.inv(covar_mat)
return weight_mat
def compute_residuals(network, sensors):
"""
......
......@@ -4,6 +4,7 @@ import pytest
import numpy as np
import pandas as pd
from knoten import bundle
from collections import OrderedDict
from csmapi import csmapi
@pytest.fixture
......@@ -13,6 +14,7 @@ def control_network():
'serialnumber': ['a', 'b', 'c', 'd', 'b', 'a', 'b', 'c', 'd'],
'line': np.arange(9),
'sample': np.arange(9)[::-1],
'aprioriCovar': [[], [], [], [], [], [], [], [], []],
'aprioriX': np.zeros(9),
'aprioriX': np.zeros(9),
'aprioriY': np.zeros(9),
......@@ -37,43 +39,49 @@ def 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)
res, covar = 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)
res, covar = 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])
bob_covar = np.array([[1.0, 0.1, 0.2], [0.1, 1.5, 0.15], [0.2, 0.15, 3.0]])
expected_sally = np.array([6.5, 1.5, 0.0])
sally_covar = np.array([[2.0, 1.1, 0.6], [1.1, 1.0, 0.45], [0.6, 0.45, 3.2]])
with mock.patch('knoten.bundle.closest_approach', side_effect=[expected_bob, expected_sally]) as mock_closest:
with mock.patch('knoten.bundle.closest_approach', side_effect=[(expected_bob, bob_covar), (expected_sally, sally_covar)]) 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)
for _, row in out_df[out_df.id == "bob"].iterrows():
np.testing.assert_array_equal(row[["aprioriX", "aprioriY", "aprioriZ"]].values,
expected_bob)
np.testing.assert_array_equal(row[["adjustedX", "adjustedY", "adjustedZ"]].values,
expected_bob)
np.testing.assert_array_equal(list(row["aprioriCovar"]),
[bob_covar[0,0], bob_covar[0,1], bob_covar[0,2],
bob_covar[1,1], bob_covar[1,2], bob_covar[2,2]])
for _, row in out_df[out_df.id == "tim"].iterrows():
np.testing.assert_array_equal(row[["aprioriX", "aprioriY", "aprioriZ"]].values,
np.zeros(3))
np.testing.assert_array_equal(row[["adjustedX", "adjustedY", "adjustedZ"]].values,
np.zeros(3))
assert not list(row["aprioriCovar"])
for _, row in out_df[out_df.id == "sally"].iterrows():
np.testing.assert_array_equal(row[["aprioriX", "aprioriY", "aprioriZ"]].values,
expected_sally)
np.testing.assert_array_equal(row[["adjustedX", "adjustedY", "adjustedZ"]].values,
expected_sally)
np.testing.assert_array_equal(list(row["aprioriCovar"]),
[sally_covar[0,0], sally_covar[0,1], sally_covar[0,2],
sally_covar[1,1], sally_covar[1,2], sally_covar[2,2]])
def test_get_sensor_parameter():
mock_sensor = mock.MagicMock(spec=csmapi.RasterGM)
......@@ -117,9 +125,17 @@ 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)]
coefficient_columns = OrderedDict()
coefficient_columns['a'] = (0, 2)
coefficient_columns['b'] = (2, 4)
coefficient_columns['c'] = (4, 6)
coefficient_columns['d'] = (6, 8)
coefficient_columns['bob'] = (8, 11)
coefficient_columns['tim'] = (11, 14)
coefficient_columns['sally'] = (14, 17)
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, coefficient_columns = bundle.compute_jacobian(control_network, sensors, parameters)
J = bundle.compute_jacobian(control_network, sensors, parameters, coefficient_columns)
expected_J = [
[1, 1, 0, 0, 0, 0, 0, 0, -1, -1, -1, 0, 0, 0, 0, 0, 0],
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment