From b5bb0f85adea01c36e7264e3463982924e475e22 Mon Sep 17 00:00:00 2001 From: Jesse Mapel Date: Fri, 17 Jul 2020 09:01:35 -0700 Subject: [PATCH] Added ground partial computations to the SAR sensor model (#287) * Partial tests * Updated test ISD * Removed debug * Better test tolerances --- jupyter_notebooks/OrbitalSarGeneration.ipynb | 107 ++++++++++++++----- src/UsgsAstroSarSensorModel.cpp | 26 ++++- src/Utilities.cpp | 2 - tests/SarTests.cpp | 54 ++++++---- tests/data/orbitalSar.json | 40 +++---- 5 files changed, 153 insertions(+), 76 deletions(-) diff --git a/jupyter_notebooks/OrbitalSarGeneration.ipynb b/jupyter_notebooks/OrbitalSarGeneration.ipynb index 77afe82..2066d6e 100644 --- a/jupyter_notebooks/OrbitalSarGeneration.ipynb +++ b/jupyter_notebooks/OrbitalSarGeneration.ipynb @@ -124,31 +124,20 @@ "metadata": { "scrolled": false }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - "Ground point at line: 500, sample: 500\n", - "[1737391.90602155 -3749.98835331 -3749.99708833]\n" - ] - } - ], + "outputs": [], "source": [ "lat = -angle_per_line * line_mat\n", "# Image is a right look, so the longitude goes negative\n", "lon = -angle_per_samp * sample_mat\n", "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)\n", - "print(\"Ground point at line: 500, sample: 500\")\n", - "print(ground_points[500, 500])" + "# print(\"Ground point at line: 500, sample: 500\")\n", + "# print(ground_points[500, 500])" ] }, { "cell_type": "code", "execution_count": 7, - "metadata": { - "scrolled": true - }, + "metadata": {}, "outputs": [], "source": [ "slant_range = np.array([[np.linalg.norm(point) for point in row] for row in ground_points - positions[:, None, :]])\n", @@ -165,14 +154,21 @@ "output_type": "stream", "text": [ "Ground range to slant range polynomial coefficients\n", - "[2000000.000003915, -1.0488420462327845e-08, 5.377893056639776e-07, -1.3072058387193456e-15]\n" + "[2000000.0, 0.0040333608396661775, 0.0, 0.0]\n" ] } ], "source": [ - "range_poly = np.polynomial.polynomial.polyfit(ground_range.flatten(), slant_range.flatten(), 3)\n", + "# Start with a crude linear approximations\n", + "starting_ground = ground_range[0,0]\n", + "starting_slant = slant_range[0,0]\n", + "ending_ground = ground_range[0, -1]\n", + "ending_slant = slant_range[0, -1]\n", + "guess_slope = (ending_slant - starting_slant) / (ending_ground - starting_ground)\n", + "guess_intercept = starting_slant - guess_slope * starting_ground\n", + "guess_coeffs = np.array([guess_intercept, guess_slope, 0.0, 0.0])\n", "print(\"Ground range to slant range polynomial coefficients\")\n", - "print(list(range_poly))" + "print(list(guess_coeffs))" ] }, { @@ -184,41 +180,94 @@ "name": "stdout", "output_type": "stream", "text": [ - "Locus point: [1737392.0956685692, -3849.7959147875467, -3749.9887626446034]\n", - "Locus direction: [0.0019248861951120758, -0.9999981473962212, -4.154676206387554e-06]\n" + "Test image point: 500 500\n", + "Test ground point: [1737387.8590770673, -5303.280537826621, -3749.9796183814506]\n" ] } ], "source": [ - "sc_pos = positions[500]\n", - "sc_vel = velocities[500]\n", - "off_ground_pt = ground_points[500, 500] - np.array([100, 100, 100])\n", - "look_vec = off_ground_pt - positions[500]\n", + "test_line, test_sample = (500, 500)\n", + "test_ground_range = test_sample * ground\n", + "test_slant_range = np.polynomial.polynomial.polyval(test_ground_range, guess_coeffs)\n", + "v_hat = velocities[test_line] / np.linalg.norm(velocities[test_line])\n", + "t_hat = positions[test_line] - np.dot(positions[test_line], v_hat) * v_hat\n", + "t_hat = t_hat / np.linalg.norm(t_hat)\n", + "u_hat = np.cross(v_hat, t_hat)\n", + "ct = np.dot(positions[test_line], t_hat)\n", + "cv = np.dot(positions[test_line], v_hat)\n", + "c = np.linalg.norm(positions[test_line])\n", + "alpha = (radius * radius - test_slant_range * test_slant_range - c * c) / (2 * ct)\n", + "beta = np.sqrt(test_slant_range * test_slant_range - alpha * alpha)\n", + "test_ground_pt = alpha * t_hat + beta * u_hat + positions[test_line]\n", + "print(\"Test image point:\", test_line, test_sample)\n", + "print(\"Test ground point:\", list(test_ground_pt))" + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "2000015.1251031486\n", + "15.125103148631752\n" + ] + } + ], + "source": [ + "print(test_slant_range)\n", + "print(test_slant_range - guess_coeffs[0])" + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Input point: [1737287.8590770673, -5403.280537826621, -3849.9796183814506]\n", + "Locus point: [1737388.1260092105, -5403.0102509726485, -3749.9801945280433]\n", + "Locus direction: [0.002701478402694769, -0.9999963509835628, -5.830873570731962e-06]\n" + ] + } + ], + "source": [ + "sc_pos = positions[test_line]\n", + "sc_vel = velocities[test_line]\n", + "off_ground_pt = test_ground_pt - np.array([100, 100, 100])\n", + "look_vec = off_ground_pt - sc_pos\n", "zero_doppler_look_vec = look_vec - np.dot(look_vec, sc_vel) / np.dot(sc_vel, sc_vel) * sc_vel\n", - "locus_point = sc_pos + slant_range[500, 500] / np.linalg.norm(zero_doppler_look_vec) * zero_doppler_look_vec\n", + "locus_point = sc_pos + np.linalg.norm(test_ground_pt - sc_pos) / np.linalg.norm(zero_doppler_look_vec) * zero_doppler_look_vec\n", "# Image is a right look, so do look X velocity\n", "locus_direction = np.cross(zero_doppler_look_vec, sc_vel)\n", "locus_direction = 1.0 / np.linalg.norm(locus_direction) * locus_direction\n", + "print(\"Input point:\", list(off_ground_pt))\n", "print(\"Locus point:\", list(locus_point))\n", "print(\"Locus direction:\", list(locus_direction))" ] }, { "cell_type": "code", - "execution_count": 10, + "execution_count": 12, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ - "Remote locus point: [1737388.3904556318, 0.0, -3749.980765309453]\n", - "Remote locus direction: [-0.0, -1.0, 0.0]\n" + "Remote locus point: [1737380.8279381434, 0.0, -3749.964442364465]\n", + "Remote locus direction: [0.0, -1.0, -0.0]\n" ] } ], "source": [ - "remote_look_vec = -slant_range[500, 500] / sensor_rad * sc_pos\n", + "remote_look_vec = -np.linalg.norm(test_ground_pt - sc_pos) / sensor_rad * sc_pos\n", "remote_zero_doppler_look_vec = remote_look_vec - np.dot(remote_look_vec, sc_vel) / np.dot(sc_vel, sc_vel) * sc_vel\n", "remote_locus_point = sc_pos + remote_zero_doppler_look_vec\n", "remote_locus_direction = np.cross(remote_zero_doppler_look_vec, sc_vel)\n", diff --git a/src/UsgsAstroSarSensorModel.cpp b/src/UsgsAstroSarSensorModel.cpp index b1fff63..f55bf38 100644 --- a/src/UsgsAstroSarSensorModel.cpp +++ b/src/UsgsAstroSarSensorModel.cpp @@ -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 &coeffs) const { @@ -811,7 +812,26 @@ vector UsgsAstroSarSensorModel::computeAllSensorP vector UsgsAstroSarSensorModel::computeGroundPartials(const csm::EcefCoord& groundPt) const { - return vector(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 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 diff --git a/src/Utilities.cpp b/src/Utilities.cpp index 1cfbbf7..07eb675 100644 --- a/src/Utilities.cpp +++ b/src/Utilities.cpp @@ -359,8 +359,6 @@ double secantRoot(double lowerBound, double upperBound, std::function 0.0) { throw std::invalid_argument("Function values at the boundaries have the same sign [secantRoot]."); diff --git a/tests/SarTests.cpp b/tests/SarTests.cpp index 9180650..2d11de9 100644 --- a/tests/SarTests.cpp +++ b/tests/SarTests.cpp @@ -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 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 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); diff --git a/tests/data/orbitalSar.json b/tests/data/orbitalSar.json index 57aa104..51bd08e 100644 --- a/tests/data/orbitalSar.json +++ b/tests/data/orbitalSar.json @@ -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": [ -- GitLab