Updated Bundle Adjust notebook (618e5f30) · Commits · aflab / astrogeology / Knoten

knoten/bundle.py

+97 −15

Original line number	Diff line number	Diff line
		@@ -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
		@@ -199,55 +201,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[2i : 2i+2, image_column : image_column+len(params)] = compute_sensor_partials(sensor, params, ground_pt)
		jacobian[2i : 2i+2, point_column : point_column+3] = compute_ground_partials(sensor, ground_pt)
		image_range = coefficient_columns[serial]
		point_range = coefficient_columns[row["id"]]
		jacobian[2i : 2i+2, image_range[0] : image_range[1]] = compute_sensor_partials(sensor, params, ground_pt)
		jacobian[2i : 2i+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):
		"""