diff --git a/include/usgscsm/UsgsAstroLsSensorModel.h b/include/usgscsm/UsgsAstroLsSensorModel.h
index fcc304834da648562aa1df1750ae2068a654edc7..a4a82876328947a662ae28d96cd3d2c93e04f067 100644
--- a/include/usgscsm/UsgsAstroLsSensorModel.h
+++ b/include/usgscsm/UsgsAstroLsSensorModel.h
@@ -1018,15 +1018,6 @@ private:
double& z,
int& mode ) const;
- // Computes the height above ellipsoid for an input ECF coordinate
- void computeElevation (
- const double& x,
- const double& y,
- const double& z,
- double& height,
- double& achieved_precision,
- const double& desired_precision) const;
-
// determines the sensor velocity accounting for parameter adjustments.
void getAdjSensorPosVel(
const double& time,
diff --git a/include/usgscsm/UsgsAstroSarSensorModel.h b/include/usgscsm/UsgsAstroSarSensorModel.h
index af79e94e9fdbec6924df844af61662858e7d19e6..98f436ac892671ab75c2641a16d8c52ded7f6cf4 100644
--- a/include/usgscsm/UsgsAstroSarSensorModel.h
+++ b/include/usgscsm/UsgsAstroSarSensorModel.h
@@ -210,7 +210,9 @@ class UsgsAstroSarSensorModel : public csm::RasterGM, virtual public csm::Settab
double groundRangeToSlantRange(double groundRange, const std::vector<double> &coeffs) const;
csm::EcefVector getSpacecraftPosition(double time) const;
+
csm::EcefVector getSunPosition(const double imageTime) const;
+
std::vector<double> getRangeCoefficients(double time) const;
////////////////////////////
diff --git a/include/usgscsm/Utilities.h b/include/usgscsm/Utilities.h
index b1d2abe9a4ccd0b3c253aca2d94835d2a2c0b521..4990a10d27b8860f5a4e600043d18d4d224fe356 100644
--- a/include/usgscsm/Utilities.h
+++ b/include/usgscsm/Utilities.h
@@ -85,6 +85,15 @@ double secantRoot(
double epsilon = 1e-10,
int maxIterations = 30);
+double computeEllipsoidElevation(
+ double x,
+ double y,
+ double z,
+ double semiMajor,
+ double semiMinor,
+ double desired_precision = 0.001,
+ double* achieved_precision = nullptr);
+
// Vector operations
csm::EcefVector operator*(double scalar, const csm::EcefVector &vec);
csm::EcefVector operator*(const csm::EcefVector &vec, double scalar);
diff --git a/src/UsgsAstroLsSensorModel.cpp b/src/UsgsAstroLsSensorModel.cpp
index 93daa56117dac0262f846c34227c2b1bba47eea4..2c5618399cc6c1c31e4729b3cf313a223570dd88 100644
--- a/src/UsgsAstroLsSensorModel.cpp
+++ b/src/UsgsAstroLsSensorModel.cpp
@@ -1018,8 +1018,9 @@ csm::EcefLocus UsgsAstroLsSensorModel::imageToProximateImagingLocus(
double z = ground_pt.z;
// Elevation at input ground point
- double height, aPrec;
- computeElevation(x, y, z, height, aPrec, desired_precision);
+ double height = computeEllipsoidElevation(
+ x, y, z,
+ m_majorAxis, m_minorAxis, desired_precision);
// Ground point on object ray with the same elevation
csm::EcefCoord gp1 = imageToGround(
@@ -1050,10 +1051,11 @@ csm::EcefLocus UsgsAstroLsSensorModel::imageToProximateImagingLocus(
gp2.y = gp1.y + scale * dy2;
gp2.z = gp1.z + scale * dz2;
- double hLocus;
- computeElevation(gp2.x, gp2.y, gp2.z, hLocus, aPrec, desired_precision);
+ double hLocus = computeEllipsoidElevation(
+ gp2.x, gp2.y, gp2.z,
+ m_majorAxis, m_minorAxis, desired_precision);
locus.point = imageToGround(
- image_pt, hLocus, desired_precision, achieved_precision, warnings);
+ image_pt, hLocus, desired_precision, achieved_precision, warnings);
locus.direction.x = dx2;
locus.direction.y = dy2;
@@ -2051,76 +2053,6 @@ void UsgsAstroLsSensorModel::lightAberrationCorr(
"{} {} {}", dxl, dyl, dzl)
}
-//***************************************************************************
-// UsgsAstroLsSensorModel::computeElevation
-//***************************************************************************
-void UsgsAstroLsSensorModel::computeElevation(
- const double& x,
- const double& y,
- const double& z,
- double& height,
- double& achieved_precision,
- const double& desired_precision) const
-{
- MESSAGE_LOG(m_logger, "Calculating computeElevation for {} {} {}"
- "with desired precision {}",
- x, y, z, desired_precision)
- // Compute elevation given xyz
- // Requires semi-major-axis and eccentricity-square
- const int MKTR = 10;
- double ecc_sqr = 1.0 - m_minorAxis * m_minorAxis / m_majorAxis / m_majorAxis;
- double ep2 = 1.0 - ecc_sqr;
- double d2 = x * x + y * y;
- double d = sqrt(d2);
- double h = 0.0;
- int ktr = 0;
- double hPrev, r;
-
- // Suited for points near equator
- if (d >= z)
- {
- double tt, zz, n;
- double tanPhi = z / d;
- do
- {
- hPrev = h;
- tt = tanPhi * tanPhi;
- r = m_majorAxis / sqrt(1.0 + ep2 * tt);
- zz = z + r * ecc_sqr * tanPhi;
- n = r * sqrt(1.0 + tt);
- h = sqrt(d2 + zz * zz) - n;
- tanPhi = zz / d;
- ktr++;
- } while (MKTR > ktr && fabs(h - hPrev) > desired_precision);
- MESSAGE_LOG(m_logger, "computeElevation: point is near equator")
- }
-
- // Suited for points near the poles
- else
- {
- double cc, dd, nn;
- double cotPhi = d / z;
- do
- {
- hPrev = h;
- cc = cotPhi * cotPhi;
- r = m_majorAxis / sqrt(ep2 + cc);
- dd = d - r * ecc_sqr * cotPhi;
- nn = r * sqrt(1.0 + cc) * ep2;
- h = sqrt(dd * dd + z * z) - nn;
- cotPhi = dd / z;
- ktr++;
- } while (MKTR > ktr && fabs(h - hPrev) > desired_precision);
- MESSAGE_LOG(m_logger, "computeElevation: point is near poles")
- }
-
- height = h;
- achieved_precision = fabs(h - hPrev);
- MESSAGE_LOG(m_logger, "computeElevation: height {} with achieved"
- "precision of {}",
- height, achieved_precision)
-}
-
//***************************************************************************
// UsgsAstroLsSensorModel::losEllipsoidIntersect
//**************************************************************************
@@ -2173,7 +2105,7 @@ void UsgsAstroLsSensorModel::losEllipsoidIntersect(
}
double scale, scale1, h, slope;
double sprev, hprev;
- double aPrec, sTerm;
+ double sTerm;
int ktr = 0;
// Compute ground point vector
@@ -2187,7 +2119,7 @@ void UsgsAstroLsSensorModel::losEllipsoidIntersect(
x = xc + scale * xl;
y = yc + scale * yl;
z = zc + scale * zl;
- computeElevation(x, y, z, h, aPrec, desired_precision);
+ h = computeEllipsoidElevation(x, y, z, m_majorAxis, m_minorAxis, desired_precision);
slope = -1;
achieved_precision = fabs(height - h);
@@ -2515,9 +2447,10 @@ void UsgsAstroLsSensorModel::setLinearApproximation()
csm::EcefCoord refPt = getReferencePoint();
- double height, aPrec;
double desired_precision = 0.01;
- computeElevation(refPt.x, refPt.y, refPt.z, height, aPrec, desired_precision);
+ double height = computeEllipsoidElevation(
+ refPt.x, refPt.y, refPt.z,
+ m_majorAxis, m_minorAxis, desired_precision);
if (std::isnan(height))
{
MESSAGE_LOG(m_logger, "setLinearApproximation: computeElevation of"
diff --git a/src/UsgsAstroSarSensorModel.cpp b/src/UsgsAstroSarSensorModel.cpp
index 7209180a4884883464679db7e25178f4a42fc634..fac1920fb32f307c813e9098bab258b643366f12 100644
--- a/src/UsgsAstroSarSensorModel.cpp
+++ b/src/UsgsAstroSarSensorModel.cpp
@@ -513,14 +513,29 @@ csm::EcefCoord UsgsAstroSarSensorModel::imageToGround(
double inTrackComp = dot(spacecraftPosition, vHat);
// Compute the substituted values
- // TODO properly handle ellipsoid radii
- double radiusSqr = m_majorAxis * m_majorAxis;
- double alpha = (radiusSqr - slantRange * slantRange - positionMag * positionMag) / (2 * nadirComp);
- // TODO use right/left look to determine +/-
- double beta = -sqrt(slantRange * slantRange - alpha * alpha);
- csm::EcefVector groundVec = alpha * tHat + beta * uHat + spacecraftPosition;
+ // Iterate to find proper radius value
+ double pointRadius = m_majorAxis + height;
+ double radiusSqr;
+ double pointHeight;
+ csm::EcefVector groundVec;
+ do {
+ radiusSqr = pointRadius * pointRadius;
+ double alpha = (radiusSqr - slantRange * slantRange - positionMag * positionMag) / (2 * nadirComp);
+ double beta = sqrt(slantRange * slantRange - alpha * alpha);
+ if (m_lookDirection == LEFT) {
+ beta *= -1;
+ }
+ groundVec = alpha * tHat + beta * uHat + spacecraftPosition;
+ pointHeight = computeEllipsoidElevation(
+ groundVec.x, groundVec.y, groundVec.z,
+ m_majorAxis, m_minorAxis);
+ pointRadius -= (pointHeight - height);
+ } while(fabs(pointHeight - height) > desiredPrecision);
+
- return csm::EcefCoord(groundVec.x, groundVec.y, groundVec.z);
+ csm::EcefCoord groundPt(groundVec.x, groundVec.y, groundVec.z);
+
+ return groundPt;
}
csm::EcefCoordCovar UsgsAstroSarSensorModel::imageToGround(
@@ -611,8 +626,33 @@ csm::EcefLocus UsgsAstroSarSensorModel::imageToProximateImagingLocus(
double* achievedPrecision,
csm::WarningList* warnings) const
{
- return csm::EcefLocus(0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0);
+ // 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 sensor to ground point 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 groundVec(groundPt.x, groundPt.y, groundPt.z);
+ csm::EcefVector lookVec = normalized(rejection(groundVec - 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);
+
+ return csm::EcefLocus(closestVec.x, closestVec.y, closestVec.z,
+ tangent.x, tangent.y, tangent.z);
}
csm::EcefLocus UsgsAstroSarSensorModel::imageToRemoteImagingLocus(
@@ -621,8 +661,32 @@ csm::EcefLocus UsgsAstroSarSensorModel::imageToRemoteImagingLocus(
double* achievedPrecision,
csm::WarningList* warnings) const
{
- return csm::EcefLocus(0.0, 0.0, 0.0,
- 0.0, 0.0, 0.0);
+ // 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);
+
+ return csm::EcefLocus(closestVec.x, closestVec.y, closestVec.z,
+ tangent.x, tangent.y, tangent.z);
}
csm::ImageCoord UsgsAstroSarSensorModel::getImageStart() const
diff --git a/src/Utilities.cpp b/src/Utilities.cpp
index 38cbfa7cdbcbef78a8e48d3bb9c6a25990fe1102..0a18184cb79207a8106452495425da3e6dc4d7eb 100644
--- a/src/Utilities.cpp
+++ b/src/Utilities.cpp
@@ -404,6 +404,69 @@ double secantRoot(double lowerBound, double upperBound, std::function<double(dou
}
}
+double computeEllipsoidElevation(
+ double x,
+ double y,
+ double z,
+ double semiMajor,
+ double semiMinor,
+ double desired_precision,
+ double* achieved_precision)
+{
+ // Compute elevation given xyz
+ // Requires semi-major-axis and eccentricity-square
+ const int MKTR = 10;
+ double ecc_sqr = 1.0 - semiMinor * semiMinor / semiMajor / semiMajor;
+ double ep2 = 1.0 - ecc_sqr;
+ double d2 = x * x + y * y;
+ double d = sqrt(d2);
+ double h = 0.0;
+ int ktr = 0;
+ double hPrev, r;
+
+ // Suited for points near equator
+ if (d >= z)
+ {
+ double tt, zz, n;
+ double tanPhi = z / d;
+ do
+ {
+ hPrev = h;
+ tt = tanPhi * tanPhi;
+ r = semiMajor / sqrt(1.0 + ep2 * tt);
+ zz = z + r * ecc_sqr * tanPhi;
+ n = r * sqrt(1.0 + tt);
+ h = sqrt(d2 + zz * zz) - n;
+ tanPhi = zz / d;
+ ktr++;
+ } while (MKTR > ktr && fabs(h - hPrev) > desired_precision);
+ }
+
+ // Suited for points near the poles
+ else
+ {
+ double cc, dd, nn;
+ double cotPhi = d / z;
+ do
+ {
+ hPrev = h;
+ cc = cotPhi * cotPhi;
+ r = semiMajor / sqrt(ep2 + cc);
+ dd = d - r * ecc_sqr * cotPhi;
+ nn = r * sqrt(1.0 + cc) * ep2;
+ h = sqrt(dd * dd + z * z) - nn;
+ cotPhi = dd / z;
+ ktr++;
+ } while (MKTR > ktr && fabs(h - hPrev) > desired_precision);
+ }
+
+ if (achieved_precision) {
+ *achieved_precision = fabs(h - hPrev);
+ }
+
+ return h;
+}
+
csm::EcefVector operator*(double scalar, const csm::EcefVector &vec)
{
diff --git a/tests/SarTests.cpp b/tests/SarTests.cpp
index a8279c736a346661db834ba3914ec6702e56d7eb..2bd7eaab900b70b4737e737d0fbef36e095a84bc 100644
--- a/tests/SarTests.cpp
+++ b/tests/SarTests.cpp
@@ -48,12 +48,12 @@ TEST_F(SarSensorModel, Center) {
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.y, -3749.98835331, 1e-2);
EXPECT_NEAR(groundPt.z, -3749.99708833, 1e-2);
}
TEST_F(SarSensorModel, GroundToImage) {
- csm::EcefCoord groundPt(1737391.90602155, 3749.98835331, -3749.99708833);
+ csm::EcefCoord groundPt(1737391.90602155, -3749.98835331, -3749.99708833);
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
@@ -81,3 +81,26 @@ TEST_F(SarSensorModel, getRangeCoefficients) {
EXPECT_NEAR(coeffs[2], 5.377926500384916e-07, 1e-8);
EXPECT_NEAR(coeffs[3], -1.3072206620088014e-15, 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);
+}
+
+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.direction.x, 0.0, 1e-8);
+ EXPECT_NEAR(locus.direction.y, -1.0, 1e-8);
+ EXPECT_NEAR(locus.direction.z, 0.0, 1e-8);
+}