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

Updated Bundle Adjust notebook

parent cfc0972b
No related branches found
No related tags found
No related merge requests found
...@@ -3,6 +3,8 @@ import pandas as pd ...@@ -3,6 +3,8 @@ import pandas as pd
import pvl import pvl
import os import os
import csmapi import csmapi
import itertools
from math import floor
from pysis import isis from pysis import isis
from ale.drivers import loads from ale.drivers import loads
...@@ -199,55 +201,135 @@ def compute_ground_partials(sensor, ground_pt): ...@@ -199,55 +201,135 @@ def compute_ground_partials(sensor, ground_pt):
partials = np.array(sensor.computeGroundPartials(csm_ground)) partials = np.array(sensor.computeGroundPartials(csm_ground))
return np.reshape(partials, (2, 3)) 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 Parameters
---------- ----------
network : DataFrame network : DataFrame
The control network as a dataframe generated by plio. The ground The control network as a dataframe generated by plio.
point columns will be in the same order as the control points are
in this.
sensors : dict sensors : dict
Dictionary that maps ISIS serial numbers to CSM sensor models Dictionary that maps ISIS serial numbers to CSM sensor models
parameters : dict parameters : dict
Dictionary that maps serial numbers to lists of parameters to Dictionary that maps serial numbers to lists of parameters to
solve for. The image parameter columns of the Jacobian will be solve for.
in the same order as this.
Returns Returns
------- -------
: ndarray : OrderedDict
The Jacobian matrix Dictionary that maps serial numbers and point IDs to the column range
their parameters are in the Jacobian matrix.
""" """
num_columns = 0 num_columns = 0
coefficient_columns = OrderedDict() coefficient_columns = OrderedDict()
for serial in network["serialnumber"].unique(): for serial in network["serialnumber"].unique():
coefficient_columns[serial] = num_columns coefficient_columns[serial] = num_columns
num_columns += len(parameters[serial]) num_columns += len(parameters[serial])
coefficient_columns[serial] = (coefficient_columns[serial], num_columns)
for point_id in network["id"].unique(): for point_id in network["id"].unique():
# Skip fixed points # Skip fixed points
if network.loc[network.id == point_id].iloc[0]["pointType"] == 4: if network.loc[network.id == point_id].iloc[0]["pointType"] == 4:
continue continue
coefficient_columns[point_id] = num_columns coefficient_columns[point_id] = num_columns
num_columns += 3 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)) jacobian = np.zeros((num_rows, num_columns))
for i in range(len(network)): for i in range(len(network)):
row = network.iloc[i] row = network.iloc[i]
serial = row["serialnumber"] serial = row["serialnumber"]
ground_pt = row[["adjustedX", "adjustedY", "adjustedZ"]] ground_pt = row[["adjustedX", "adjustedY", "adjustedZ"]]
sensor = sensors[serial] sensor = sensors[serial]
params = parameters[serial] params = parameters[serial]
image_column = coefficient_columns[serial] image_range = coefficient_columns[serial]
point_column = coefficient_columns[row["id"]] point_range = 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, image_range[0] : image_range[1]] = compute_sensor_partials(sensor, params, ground_pt)
jacobian[2*i : 2*i+2, point_column : point_column+3] = compute_ground_partials(sensor, ground_pt) jacobian[2*i : 2*i+2, point_range[0] : point_range[1]] = compute_ground_partials(sensor, ground_pt)
return jacobian
def compute_parameter_weights(network, sensors, parameters, coefficient_columns):
"""
Compute the parameter weight matrix
return jacobian, coefficient_columns 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): def compute_residuals(network, sensors):
""" """
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment