Commit 903edd23 authored by Jesse Mapel's avatar Jesse Mapel
Browse files

Updated weights and sigma0 calculations

parent 618e5f30

knoten/bundle.py

+14 −3
Original line number Diff line number Diff line
@@ -66,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))
@@ -75,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):

    """
@@ -105,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:

tests/test_bundle.py

+38 −22
Original line number Diff line number Diff line
@@ -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],