diff --git a/src/UsgsAstroSarSensorModel.cpp b/src/UsgsAstroSarSensorModel.cpp index 0d37d2750bb53e36385163f55015102d4ed90952..dfbd5e8989f3532c1b2669c2b1fc14f1e905e711 100644 --- a/src/UsgsAstroSarSensorModel.cpp +++ b/src/UsgsAstroSarSensorModel.cpp @@ -543,6 +543,10 @@ csm::EcefCoord UsgsAstroSarSensorModel::imageToGround( pointRadius -= (pointHeight - height); } while (fabs(pointHeight - height) > desiredPrecision); + if (achievedPrecision) { + *achievedPrecision = fabs(pointHeight - height); + } + csm::EcefCoord groundPt(groundVec.x, groundVec.y, groundVec.z); return groundPt; @@ -669,36 +673,24 @@ csm::EcefLocus UsgsAstroSarSensorModel::imageToProximateImagingLocus( csm::EcefLocus UsgsAstroSarSensorModel::imageToRemoteImagingLocus( const csm::ImageCoord& imagePt, double desiredPrecision, double* achievedPrecision, csm::WarningList* warnings) const { - // Compute the slant range - double time = - m_startingEphemerisTime + (imagePt.line - 0.5) * m_exposureDuration; - double groundRange = imagePt.samp * m_scaledPixelWidth; - std::vector<double> coeffs = getRangeCoefficients(time); - double slantRange = groundRangeToSlantRange(groundRange, coeffs); - - // Project the negative sensor position vector onto the 0 doppler plane - // then compute the closest point at the slant range to that - csm::EcefVector spacecraftPosition = getSpacecraftPosition(time); - csm::EcefVector spacecraftVelocity = getSensorVelocity(time); - csm::EcefVector lookVec = - normalized(rejection(-1 * spacecraftPosition, spacecraftVelocity)); - csm::EcefVector closestVec = spacecraftPosition + slantRange * lookVec; - // Compute the tangent at the closest point - csm::EcefVector tangent; - if (m_lookDirection == LEFT) { - tangent = cross(spacecraftVelocity, lookVec); - } else { - tangent = cross(lookVec, spacecraftVelocity); - } - tangent = normalized(tangent); - - if (achievedPrecision) { - *achievedPrecision = 0.0; - } - - return csm::EcefLocus(closestVec.x, closestVec.y, closestVec.z, tangent.x, - tangent.y, tangent.z); + // Find the sensor coordinates + csm::EcefCoord sensorPt = getSensorPosition(imagePt); + + // Compute the intersection of the ray traced through the + // current pixel with the datum. + double height = 0.0; + csm::EcefCoord groundPt = imageToGround(imagePt, height, desiredPrecision, + achievedPrecision, warnings); + + // Normalized direction from camera to ground + csm::EcefVector dir + = normalized(csm::EcefVector(groundPt.x - sensorPt.x, + groundPt.y - sensorPt.y, + groundPt.z - sensorPt.z)); + + return csm::EcefLocus(groundPt.x, groundPt.y, groundPt.z, + dir.x, dir.y, dir.z); } csm::ImageCoord UsgsAstroSarSensorModel::getImageStart() const { diff --git a/tests/SarTests.cpp b/tests/SarTests.cpp index 574b5fc874f3b253e221aed7d63c7e48ee4f1925..00285c20bea63617742df7c88dc31f654483e5dc 100644 --- a/tests/SarTests.cpp +++ b/tests/SarTests.cpp @@ -113,17 +113,28 @@ TEST_F(SarSensorModel, imageToProximateImagingLocus) { TEST_F(SarSensorModel, imageToRemoteImagingLocus) { double precision; csm::WarningList warnings; + csm::ImageCoord imagePt(500.0, 500.0); csm::EcefLocus locus = sensorModel->imageToRemoteImagingLocus( - csm::ImageCoord(500.0, 500.0), + imagePt, 0.001, &precision, &warnings); - 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); + + csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); + csm::EcefCoord sensorPt = sensorModel->getSensorPosition(imagePt); + + double lookX = groundPt.x - sensorPt.x; + double lookY = groundPt.y - sensorPt.y; + double lookZ = groundPt.z - sensorPt.z; + double lookMag = sqrt(lookX * lookX + lookY * lookY + lookZ * lookZ); + lookX /= lookMag; + lookY /= lookMag; + lookZ /= lookMag; + + EXPECT_NEAR(locus.direction.x, lookX, 1e-10); + EXPECT_NEAR(locus.direction.y, lookY, 1e-10); + EXPECT_NEAR(locus.direction.z, lookZ, 1e-10); + EXPECT_LT(precision, 0.001); EXPECT_TRUE(warnings.empty()); }