Unverified Commit b5bb0f85 authored by Jesse Mapel's avatar Jesse Mapel Committed by GitHub
Browse files

Added ground partial computations to the SAR sensor model (#287) 

* Partial tests

* Updated test ISD

* Removed debug

* Better test tolerances
parent 424ec8e2

jupyter_notebooks/OrbitalSarGeneration.ipynb

+61 −20
Original line number Diff line number Diff line 
%% Cell type:code id: tags:



``` python

import numpy as np

from itertools import product

```



%% Cell type:code id: tags:



``` python

radius   = 1737400

alt      = 2000000

ground   = 7.5

exposure = 0.005

samples  = 1000

lines    = 1000

```



%% Cell type:code id: tags:



``` python

sensor_rad = radius + alt

angle_per_line = ground / radius

angle_per_samp = angle_per_line

angle_per_Second = angle_per_line / exposure

```



%% Cell type:code id: tags:



``` python

line_vec = np.arange(0, lines+0.00000001)

sample_vec = np.arange(0, samples+0.00000001)

# From here on, matrix indexing is [line, sample, (xyz)]

sample_mat, line_mat = np.meshgrid(line_vec, sample_vec)

```



%% Cell type:code id: tags:



``` python

positions = sensor_rad * np.vstack([np.cos(-angle_per_line * line_vec), np.zeros(line_vec.shape), np.sin(-angle_per_line * line_vec)]).T

# Note: The chain rule results in an extra negative on the velocity calculations

velocities = sensor_rad * np.vstack([np.sin(-angle_per_line * line_vec), np.zeros(line_vec.shape), -np.cos(-angle_per_line * line_vec)]).T

print('Positions')

for pos in positions[::int(0.25/exposure)]:

    print(list(pos))

print('Velocities')

for vel in velocities[::int(0.25/exposure)]:

    print(list(vel))

```



%% Output



    Positions

    [3737400.0, 0.0, -0.0]

    [3737399.912943243, 0.0, -806.6795148600813]

    [3737399.651772976, 0.0, -1613.3589921395437]

    [3737399.216489211, 0.0, -2420.03839425777]

    [3737398.607091969, 0.0, -3226.7176836341473]

    [3737397.823581278, 0.0, -4033.396822688066]

    [3737396.8659571735, 0.0, -4840.075773838925]

    [3737395.734219702, 0.0, -5646.754499506133]

    [3737394.4283689144, 0.0, -6453.432962109106]

    [3737392.9484048723, 0.0, -7260.111124067275]

    [3737391.2943276456, 0.0, -8066.788947800085]

    [3737389.4661373096, 0.0, -8873.466395726995]

    [3737387.4638339505, 0.0, -9680.14343026748]

    [3737385.287417662, 0.0, -10486.820013841041]

    [3737382.936888544, 0.0, -11293.496108867193]

    [3737380.4122467074, 0.0, -12100.17167776548]

    [3737377.713492269, 0.0, -12906.846682955462]

    [3737374.840625355, 0.0, -13713.521086856732]

    [3737371.7936460995, 0.0, -14520.194851888911]

    [3737368.5725546437, 0.0, -15326.867940471646]

    [3737365.177351138, 0.0, -16133.540315024618]

    Velocities

    [-0.0, 0.0, -3737400.0]

    [-806.6795148600813, 0.0, -3737399.912943243]

    [-1613.3589921395437, 0.0, -3737399.651772976]

    [-2420.03839425777, 0.0, -3737399.216489211]

    [-3226.7176836341473, 0.0, -3737398.607091969]

    [-4033.396822688066, 0.0, -3737397.823581278]

    [-4840.075773838925, 0.0, -3737396.8659571735]

    [-5646.754499506133, 0.0, -3737395.734219702]

    [-6453.432962109106, 0.0, -3737394.4283689144]

    [-7260.111124067275, 0.0, -3737392.9484048723]

    [-8066.788947800085, 0.0, -3737391.2943276456]

    [-8873.466395726995, 0.0, -3737389.4661373096]

    [-9680.14343026748, 0.0, -3737387.4638339505]

    [-10486.820013841041, 0.0, -3737385.287417662]

    [-11293.496108867193, 0.0, -3737382.936888544]

    [-12100.17167776548, 0.0, -3737380.4122467074]

    [-12906.846682955462, 0.0, -3737377.713492269]

    [-13713.521086856732, 0.0, -3737374.840625355]

    [-14520.194851888911, 0.0, -3737371.7936460995]

    [-15326.867940471646, 0.0, -3737368.5725546437]

    [-16133.540315024618, 0.0, -3737365.177351138]



%% Cell type:code id: tags:



``` python

lat = -angle_per_line * line_mat

# Image is a right look, so the longitude goes negative

lon = -angle_per_samp * sample_mat

ground_points = radius * np.stack([np.multiply(np.cos(lat), np.cos(lon)), np.multiply(np.cos(lat), np.sin(lon)), np.sin(lat)], axis=-1)

print("Ground point at line: 500, sample: 500")

print(ground_points[500, 500])

# print("Ground point at line: 500, sample: 500")

# print(ground_points[500, 500])

```



%% Output



    Ground point at line: 500, sample: 500

    [1737391.90602155   -3749.98835331   -3749.99708833]



%% Cell type:code id: tags:



``` python

slant_range = np.array([[np.linalg.norm(point) for point in row] for row in ground_points - positions[:, None, :]])

ground_range = radius * np.abs(lon)

```



%% Cell type:code id: tags:



``` python

range_poly = np.polynomial.polynomial.polyfit(ground_range.flatten(), slant_range.flatten(), 3)

# Start with a crude linear approximations

starting_ground = ground_range[0,0]

starting_slant = slant_range[0,0]

ending_ground = ground_range[0, -1]

ending_slant = slant_range[0, -1]

guess_slope = (ending_slant - starting_slant) / (ending_ground - starting_ground)

guess_intercept = starting_slant - guess_slope * starting_ground

guess_coeffs = np.array([guess_intercept, guess_slope, 0.0, 0.0])

print("Ground range to slant range polynomial coefficients")

print(list(range_poly))

print(list(guess_coeffs))

```



%% Output



    Ground range to slant range polynomial coefficients

    [2000000.000003915, -1.0488420462327845e-08, 5.377893056639776e-07, -1.3072058387193456e-15]

    [2000000.0, 0.0040333608396661775, 0.0, 0.0]



%% Cell type:code id: tags:



``` python

test_line, test_sample = (500, 500)

test_ground_range = test_sample * ground

test_slant_range = np.polynomial.polynomial.polyval(test_ground_range, guess_coeffs)

v_hat = velocities[test_line] / np.linalg.norm(velocities[test_line])

t_hat = positions[test_line] - np.dot(positions[test_line], v_hat) * v_hat

t_hat = t_hat / np.linalg.norm(t_hat)

u_hat = np.cross(v_hat, t_hat)

ct = np.dot(positions[test_line], t_hat)

cv = np.dot(positions[test_line], v_hat)

c = np.linalg.norm(positions[test_line])

alpha = (radius * radius - test_slant_range * test_slant_range - c * c) / (2 * ct)

beta = np.sqrt(test_slant_range * test_slant_range - alpha * alpha)

test_ground_pt = alpha * t_hat + beta * u_hat + positions[test_line]

print("Test image point:", test_line, test_sample)

print("Test ground point:", list(test_ground_pt))

```



%% Output



    Test image point: 500 500

    Test ground point: [1737387.8590770673, -5303.280537826621, -3749.9796183814506]



%% Cell type:code id: tags:



``` python

print(test_slant_range)

print(test_slant_range - guess_coeffs[0])

```



%% Output



    2000015.1251031486

    15.125103148631752



%% Cell type:code id: tags:



``` python

sc_pos = positions[500]

sc_vel = velocities[500]

off_ground_pt = ground_points[500, 500] - np.array([100, 100, 100])

look_vec = off_ground_pt - positions[500]

sc_pos = positions[test_line]

sc_vel = velocities[test_line]

off_ground_pt = test_ground_pt - np.array([100, 100, 100])

look_vec = off_ground_pt - sc_pos

zero_doppler_look_vec = look_vec - np.dot(look_vec, sc_vel) / np.dot(sc_vel, sc_vel) * sc_vel

locus_point = sc_pos + slant_range[500, 500] / np.linalg.norm(zero_doppler_look_vec) * zero_doppler_look_vec

locus_point = sc_pos + np.linalg.norm(test_ground_pt - sc_pos) / np.linalg.norm(zero_doppler_look_vec) * zero_doppler_look_vec

# Image is a right look, so do look X velocity

locus_direction = np.cross(zero_doppler_look_vec, sc_vel)

locus_direction = 1.0 / np.linalg.norm(locus_direction) * locus_direction

print("Input point:", list(off_ground_pt))

print("Locus point:", list(locus_point))

print("Locus direction:", list(locus_direction))

```



%% Output



    Locus point: [1737392.0956685692, -3849.7959147875467, -3749.9887626446034]

    Locus direction: [0.0019248861951120758, -0.9999981473962212, -4.154676206387554e-06]

    Input point: [1737287.8590770673, -5403.280537826621, -3849.9796183814506]

    Locus point: [1737388.1260092105, -5403.0102509726485, -3749.9801945280433]

    Locus direction: [0.002701478402694769, -0.9999963509835628, -5.830873570731962e-06]



%% Cell type:code id: tags:



``` python

remote_look_vec = -slant_range[500, 500] / sensor_rad * sc_pos

remote_look_vec = -np.linalg.norm(test_ground_pt - sc_pos) / sensor_rad * sc_pos

remote_zero_doppler_look_vec = remote_look_vec - np.dot(remote_look_vec, sc_vel) / np.dot(sc_vel, sc_vel) * sc_vel

remote_locus_point = sc_pos + remote_zero_doppler_look_vec

remote_locus_direction = np.cross(remote_zero_doppler_look_vec, sc_vel)

remote_locus_direction = 1.0 / np.linalg.norm(remote_locus_direction) * remote_locus_direction

print("Remote locus point:", list(remote_locus_point))

print("Remote locus direction:", list(remote_locus_direction))

```



%% Output



    Remote locus point: [1737388.3904556318, 0.0, -3749.980765309453]

    Remote locus direction: [-0.0, -1.0, 0.0]

    Remote locus point: [1737380.8279381434, 0.0, -3749.964442364465]

    Remote locus direction: [0.0, -1.0, -0.0]



%% Cell type:code id: tags:



``` python

```

src/UsgsAstroSarSensorModel.cpp

+23 −3
Original line number Diff line number Diff line
@@ -384,7 +384,8 @@ double UsgsAstroSarSensorModel::dopplerShift( 
   };



  // Do root-finding for "dopplerShift"

  return brentRoot(m_startingEphemerisTime, m_endingEphemerisTime, dopplerShiftFunction, tolerance);

  double timePadding = m_exposureDuration * m_nLines * 0.25;

  return brentRoot(m_startingEphemerisTime - timePadding, m_endingEphemerisTime + timePadding, dopplerShiftFunction, tolerance);

}
@@ -425,7 +426,7 @@ double UsgsAstroSarSensorModel::slantRangeToGroundRange( 
  double maxGroundRangeGuess = (1.25 * m_nSamples - 1.0) * m_scaledPixelWidth;



  // Tolerance to 1/20th of a pixel for now.

  return brentRoot(minGroundRangeGuess, maxGroundRangeGuess, slantRangeToGroundRangeFunction, tolerance);

  return secantRoot(minGroundRangeGuess, maxGroundRangeGuess, slantRangeToGroundRangeFunction, tolerance);

}



double UsgsAstroSarSensorModel::groundRangeToSlantRange(double groundRange, const std::vector<double> &coeffs) const {
@@ -811,7 +812,26 @@ vector<csm::RasterGM::SensorPartials> UsgsAstroSarSensorModel::computeAllSensorP 


vector<double> UsgsAstroSarSensorModel::computeGroundPartials(const csm::EcefCoord& groundPt) const

{

  return vector<double>(6, 0.0);

  double GND_DELTA = m_scaledPixelWidth;

  // Partial of line, sample wrt X, Y, Z

  double x = groundPt.x;

  double y = groundPt.y;

  double z = groundPt.z;



  csm::ImageCoord ipB = groundToImage(groundPt);

  csm::ImageCoord ipX = groundToImage(csm::EcefCoord(x + GND_DELTA, y, z));

  csm::ImageCoord ipY = groundToImage(csm::EcefCoord(x, y + GND_DELTA, z));

  csm::ImageCoord ipZ = groundToImage(csm::EcefCoord(x, y, z + GND_DELTA));



  std::vector<double> partials(6, 0.0);

  partials[0] = (ipX.line - ipB.line) / GND_DELTA;

  partials[3] = (ipX.samp - ipB.samp) / GND_DELTA;

  partials[1] = (ipY.line - ipB.line) / GND_DELTA;

  partials[4] = (ipY.samp - ipB.samp) / GND_DELTA;

  partials[2] = (ipZ.line - ipB.line) / GND_DELTA;

  partials[5] = (ipZ.samp - ipB.samp) / GND_DELTA;



  return partials;

}



const csm::CorrelationModel& UsgsAstroSarSensorModel::getCorrelationModel() const

src/Utilities.cpp

+0 −2
Original line number Diff line number Diff line
@@ -359,8 +359,6 @@ double secantRoot(double lowerBound, double upperBound, std::function<double(dou 
  double x2 = 0;

  double f2 = 0;



  std::cout << "f0, f1: " << f0 << ", " << f1 << std::endl;



  // Make sure we bound the root (f = 0.0)

  if (f0 * f1 > 0.0) {

    throw std::invalid_argument("Function values at the boundaries have the same sign [secantRoot].");

tests/SarTests.cpp

+32 −22
Original line number Diff line number Diff line
@@ -46,18 +46,16 @@ TEST_F(SarSensorModel, State) { 
TEST_F(SarSensorModel, Center) {

  csm::ImageCoord imagePt(500.0, 500.0);

  csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0);

  // TODO these tolerances are bad

  EXPECT_NEAR(groundPt.x, 1737391.90602155, 1e-2);

  EXPECT_NEAR(groundPt.y, -3749.98835331, 1e-2);

  EXPECT_NEAR(groundPt.z, -3749.99708833, 1e-2);

  EXPECT_NEAR(groundPt.x, 1737387.8590770673, 1e-3);

  EXPECT_NEAR(groundPt.y, -5303.280537826621, 1e-3);

  EXPECT_NEAR(groundPt.z, -3749.9796183814506, 1e-3);

}



TEST_F(SarSensorModel, GroundToImage) {

  csm::EcefCoord groundPt(1737391.90602155, -3749.98835331, -3749.99708833);

  csm::EcefCoord groundPt(1737387.8590770673, -5303.280537826621, -3749.9796183814506);

  csm::ImageCoord imagePt = sensorModel->groundToImage(groundPt, 0.001);

  EXPECT_NEAR(imagePt.line, 500.0, 0.001);

  // Due to position interpolation, the sample is slightly less accurate than the line

  EXPECT_NEAR(imagePt.samp, 500.0, 0.002);

  EXPECT_NEAR(imagePt.line, 500.0, 1e-3);

  EXPECT_NEAR(imagePt.samp, 500.0, 1e-3);

}



TEST_F(SarSensorModel, spacecraftPosition) {
@@ -76,30 +74,42 @@ TEST_F(SarSensorModel, spacecraftVelocity) { 


TEST_F(SarSensorModel, getRangeCoefficients) {

  std::vector<double> coeffs = sensorModel->getRangeCoefficients(-0.0025);

  EXPECT_NEAR(coeffs[0], 2000000.0000039602, 1e-8);

  EXPECT_NEAR(coeffs[1], -1.0504347070801814e-08, 1e-8);

  EXPECT_NEAR(coeffs[2], 5.377926500384916e-07, 1e-8);

  EXPECT_NEAR(coeffs[3], -1.3072206620088014e-15, 1e-8);

  EXPECT_NEAR(coeffs[0], 2000000.0, 1e-8);

  EXPECT_NEAR(coeffs[1], 0.0040333608396661775, 1e-8);

  EXPECT_NEAR(coeffs[2], 0.0, 1e-8);

  EXPECT_NEAR(coeffs[3], 0.0, 1e-8);

}



TEST_F(SarSensorModel, computeGroundPartials) {

  csm::EcefCoord groundPt(1737400.0, 0.0, 0.0);

  std::vector<double> partials = sensorModel->computeGroundPartials(groundPt);

  ASSERT_EQ(partials.size(), 6);

  EXPECT_NEAR(partials[0], -0.00023385137104424322, 1e-8);

  EXPECT_NEAR(partials[1], -3.3408269124235446e-05, 1e-8);

  EXPECT_NEAR(partials[2], -1.0 / 7.5, 1e-8);

  EXPECT_NEAR(partials[3], -33.057612335589731, 1e-8);

  EXPECT_NEAR(partials[4], 6.3906252180458977e-05, 1e-8);

  EXPECT_NEAR(partials[5], 0.0077565105242805047, 1e-8);

}



TEST_F(SarSensorModel, imageToProximateImagingLocus) {

  csm::EcefLocus locus = sensorModel->imageToProximateImagingLocus(

      csm::ImageCoord(500.0, 500.0),

      csm::EcefCoord(1737291.90643026, -3750.17585202, -3749.78124955));

  EXPECT_NEAR(locus.point.x, 1737391.90602155, 1e-2);

  EXPECT_NEAR(locus.point.y, -3749.98835331, 1e-2);

  EXPECT_NEAR(locus.point.z, -3749.99708833, 1e-2);

  EXPECT_NEAR(locus.direction.x, 0.0018750001892442036, 1e-5);

  EXPECT_NEAR(locus.direction.y, -0.9999982421774111, 1e-5);

  EXPECT_NEAR(locus.direction.z, -4.047002203562211e-06, 1e-5);

      csm::EcefCoord(1737287.8590770673, -5403.280537826621, -3849.9796183814506));

  EXPECT_NEAR(locus.point.x, 1737388.1260092105, 1e-2);

  EXPECT_NEAR(locus.point.y, -5403.0102509726485, 1e-2);

  EXPECT_NEAR(locus.point.z, -3749.9801945280433, 1e-2);

  EXPECT_NEAR(locus.direction.x, 0.002701478402694769, 1e-5);

  EXPECT_NEAR(locus.direction.y, -0.9999963509835628, 1e-5);

  EXPECT_NEAR(locus.direction.z, -5.830873570731962e-06, 1e-5);

}



TEST_F(SarSensorModel, imageToRemoteImagingLocus) {

  csm::EcefLocus locus = sensorModel->imageToRemoteImagingLocus(

      csm::ImageCoord(500.0, 500.0));

      EXPECT_NEAR(locus.point.x, 1737388.3904556315, 1e-2);

      EXPECT_NEAR(locus.point.y, 0.0, 1e-2);

      EXPECT_NEAR(locus.point.z, -3749.9807653094517, 1e-2);

      EXPECT_NEAR(locus.point.x, 1737380.8279381434, 1e-3);

      EXPECT_NEAR(locus.point.y, 0.0, 1e-3);

      EXPECT_NEAR(locus.point.z, -3749.964442364465, 1e-3);

      EXPECT_NEAR(locus.direction.x, 0.0, 1e-8);

      EXPECT_NEAR(locus.direction.y, -1.0, 1e-8);

      EXPECT_NEAR(locus.direction.z, 0.0, 1e-8);

tests/data/orbitalSar.json

+20 −20
Original line number Diff line number Diff line
@@ -69,26 +69,26 @@ 
  "range_conversion_times": [0.0, 0.25, 0.5, 0.75, 1.0, 1.25, 1.5, 1.75, 2.0, 2.25, 2.5, 2.75, 3.0,

                             3.25, 3.5, 3.75, 4.0, 4.25, 4.5, 4.75],

  "range_conversion_coefficients" : [

    [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15],

    [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15],

    [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15],

    [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15],

    [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15],

    [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15],

    [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15],

    [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15],

    [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15],

    [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15],

    [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15],

    [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15],

    [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15],

    [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15],

    [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15],

    [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15],

    [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15],

    [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15],

    [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15],

    [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15]

    [2000000.0, 0.0040333608396661775, 0.0, 0.0],

    [2000000.0, 0.0040333608396661775, 0.0, 0.0],

    [2000000.0, 0.0040333608396661775, 0.0, 0.0],

    [2000000.0, 0.0040333608396661775, 0.0, 0.0],

    [2000000.0, 0.0040333608396661775, 0.0, 0.0],

    [2000000.0, 0.0040333608396661775, 0.0, 0.0],

    [2000000.0, 0.0040333608396661775, 0.0, 0.0],

    [2000000.0, 0.0040333608396661775, 0.0, 0.0],

    [2000000.0, 0.0040333608396661775, 0.0, 0.0],

    [2000000.0, 0.0040333608396661775, 0.0, 0.0],

    [2000000.0, 0.0040333608396661775, 0.0, 0.0],

    [2000000.0, 0.0040333608396661775, 0.0, 0.0],

    [2000000.0, 0.0040333608396661775, 0.0, 0.0],

    [2000000.0, 0.0040333608396661775, 0.0, 0.0],

    [2000000.0, 0.0040333608396661775, 0.0, 0.0],

    [2000000.0, 0.0040333608396661775, 0.0, 0.0],

    [2000000.0, 0.0040333608396661775, 0.0, 0.0],

    [2000000.0, 0.0040333608396661775, 0.0, 0.0],

    [2000000.0, 0.0040333608396661775, 0.0, 0.0],

    [2000000.0, 0.0040333608396661775, 0.0, 0.0]

  ],

  "sun_position": {

    "positions": [