diff --git a/include/usgscsm/Utilities.h b/include/usgscsm/Utilities.h
index 5034abea94b429ecda3d6fc1fdb2fb795d05d86b..cb7f55160737b650fdbc236c404ed6bb09c00055 100644
--- a/include/usgscsm/Utilities.h
+++ b/include/usgscsm/Utilities.h
@@ -78,12 +78,25 @@ double brentRoot(
std::function<double(double)> func,
double epsilon = 1e-10);
-double secantRoot(
- double lowerBound,
- double upperBound,
- std::function<double(double)> func,
- double epsilon = 1e-10,
- int maxIterations = 30);
+// Evaluate a polynomial function.
+// Coefficients should be ordered least order to greatest I.E. {1, 2, 3} is 1 + 2x + 3x^2
+double evaluatePolynomial(
+ const std::vector<double> &coeffs,
+ double x);
+
+// Evaluate the derivative of a polynomial function.
+// Coefficients should be ordered least order to greatest I.E. {1, 2, 3} is 1 + 2x + 3x^2
+double evaluatePolynomialDerivative(
+ const std::vector<double> &coeffs,
+ double x);
+
+// Find a root of a polynomial using Newton's method.
+// Coefficients should be ordered least order to greatest I.E. {1, 2, 3} is 1 + 2x + 3x^2
+double polynomialRoot(
+ const std::vector<double> &coeffs,
+ double guess,
+ double threshold = 1e-10,
+ int maxIterations = 30);
double computeEllipsoidElevation(
double x,
diff --git a/src/UsgsAstroSarSensorModel.cpp b/src/UsgsAstroSarSensorModel.cpp
index b75a9c1ad6446159e6712d4ef7ca8042ca11c586..48ae73335afdf51151699fe81844208d57dff41a 100644
--- a/src/UsgsAstroSarSensorModel.cpp
+++ b/src/UsgsAstroSarSensorModel.cpp
@@ -45,7 +45,7 @@ const csm::param::Type
string UsgsAstroSarSensorModel::constructStateFromIsd(
const string imageSupportData,
csm::WarningList *warnings){
-
+
MESSAGE_LOG("UsgsAstroSarSensorModel constructing state from ISD, with {}", imageSupportData);
json isd = json::parse(imageSupportData);
json state = {};
@@ -227,7 +227,7 @@ void UsgsAstroSarSensorModel::reset() {
void UsgsAstroSarSensorModel::replaceModelState(const string& argState){
reset();
-
+
MESSAGE_LOG("Replacing model state with: {}", argState);
auto stateJson = json::parse(argState);
@@ -286,7 +286,7 @@ void UsgsAstroSarSensorModel::replaceModelState(const string& argState){
csm::ImageCoord ip(lineCtr, sampCtr);
MESSAGE_LOG("updateState: center image coordinate set to {} {}",
lineCtr, sampCtr)
-
+
double refHeight = 0;
m_referencePointXyz = imageToGround(ip, refHeight);
MESSAGE_LOG("updateState: reference point (x, y, z) {} {} {}",
@@ -386,18 +386,18 @@ csm::ImageCoord UsgsAstroSarSensorModel::groundToImage(
double UsgsAstroSarSensorModel::dopplerShift(
csm::EcefCoord groundPt,
double tolerance) const {
- MESSAGE_LOG("Calculating doppler shift with: {}, {}, {}, and tolerance {}.",
+ MESSAGE_LOG("Calculating doppler shift with: {}, {}, {}, and tolerance {}.",
groundPt.x, groundPt.y, groundPt.z, tolerance);
csm::EcefVector groundVec(groundPt.x ,groundPt.y, groundPt.z);
std::function<double(double)> dopplerShiftFunction = [this, groundVec](double time) {
csm::EcefVector spacecraftPosition = getSpacecraftPosition(time);
csm::EcefVector spacecraftVelocity = getSensorVelocity(time);
csm::EcefVector lookVector = spacecraftPosition - groundVec;
-
+
double slantRange = norm(lookVector);
-
+
double dopplerShift = -2.0 * dot(lookVector, spacecraftVelocity) / (slantRange * m_wavelength);
-
+
return dopplerShift;
};
@@ -409,7 +409,7 @@ double UsgsAstroSarSensorModel::dopplerShift(
double UsgsAstroSarSensorModel::slantRange(csm::EcefCoord surfPt,
double time) const {
- MESSAGE_LOG("Calculating slant range with: {}, {}, {}, and time {}.",
+ MESSAGE_LOG("Calculating slant range with: {}, {}, {}, and time {}.",
surfPt.x, surfPt.y, surfPt.z, time);
csm::EcefVector surfVec(surfPt.x ,surfPt.y, surfPt.z);
csm::EcefVector spacecraftPosition = getSpacecraftPosition(time);
@@ -426,29 +426,17 @@ double UsgsAstroSarSensorModel::slantRangeToGroundRange(
std::vector<double> coeffs = getRangeCoefficients(time);
- // Calculates the ground range from the slant range.
- std::function<double(double)> slantRangeToGroundRangeFunction =
- [this, coeffs, slantRange](double groundRange){
- return slantRange - groundRangeToSlantRange(groundRange, coeffs);
- };
-
// Need to come up with an initial guess when solving for ground
- // range given slant range. Compute the ground range at the
- // near and far edges of the image by evaluating the sample-to-
- // ground-range equation: groundRange=(sample-1)*scaled_pixel_width
- // at the edges of the image. We also need to add some padding to
- // allow for solving for coordinates that are slightly outside of
- // the actual image area. Use sample=-0.25*image_samples and
- // sample=1.25*image_samples.
- double minGroundRangeGuess = (-0.25 * m_nSamples - 1.0) * m_scaledPixelWidth;
- double maxGroundRangeGuess = (1.25 * m_nSamples - 1.0) * m_scaledPixelWidth;
+ // range given slant range. Naively use the middle of the image.
+ double guess = 0.5 * m_nSamples * m_scaledPixelWidth;
// Tolerance to 1/20th of a pixel for now.
- return secantRoot(minGroundRangeGuess, maxGroundRangeGuess, slantRangeToGroundRangeFunction, tolerance);
+ coeffs[0] -= slantRange;
+ return polynomialRoot(coeffs, guess, tolerance);
}
double UsgsAstroSarSensorModel::groundRangeToSlantRange(double groundRange, const std::vector<double> &coeffs) const {
- return coeffs[0] + groundRange * (coeffs[1] + groundRange * (coeffs[2] + groundRange * coeffs[3]));
+ return evaluatePolynomial(coeffs, groundRange);
}
@@ -578,7 +566,7 @@ csm::EcefCoordCovar UsgsAstroSarSensorModel::imageToGround(
double desiredPrecision,
double* achievedPrecision,
csm::WarningList* warnings) const {
- MESSAGE_LOG("Calculating imageToGroundWith: {}, {}, {}, {}, {}", imagePt.samp, imagePt.line,
+ MESSAGE_LOG("Calculating imageToGroundWith: {}, {}, {}, {}, {}", imagePt.samp, imagePt.line,
height, heightVariance, desiredPrecision);
// Image to ground with error propagation
// Use numerical partials
diff --git a/src/Utilities.cpp b/src/Utilities.cpp
index 07eb6759a201c43b179555c5f46560322d322f0b..e452881316c59dc18cde3fa5bdf0939ba20660fe 100644
--- a/src/Utilities.cpp
+++ b/src/Utilities.cpp
@@ -279,129 +279,124 @@ void lagrangeInterp(
}
}
-double brentRoot(
- double lowerBound,
- double upperBound,
- std::function<double(double)> func,
- double epsilon) {
- double counterPoint = lowerBound;
- double currentPoint = upperBound;
- double counterFunc = func(counterPoint);
- double currentFunc = func(currentPoint);
- if (counterFunc * currentFunc > 0.0) {
- throw std::invalid_argument("Function values at the boundaries have the same sign [brentRoot].");
- }
- if (fabs(counterFunc) < fabs(currentFunc)) {
- std::swap(counterPoint, currentPoint);
- std::swap(counterFunc, currentFunc);
- }
-
- double previousPoint = counterPoint;
- double previousFunc = counterFunc;
- double evenOlderPoint = previousPoint;
- double nextPoint;
- double nextFunc;
- int iteration = 0;
- bool bisected = true;
-
- do {
- // Inverse quadratic interpolation
- if (counterFunc != previousFunc && counterFunc != currentFunc && currentFunc != previousFunc) {
- nextPoint = (counterPoint * currentFunc * previousFunc) / ((counterFunc - currentFunc) * (counterFunc - previousFunc));
- nextPoint += (currentPoint * counterFunc * previousFunc) / ((currentFunc - counterFunc) * (currentFunc - previousFunc));
- nextPoint += (previousPoint * currentFunc * counterFunc) / ((previousFunc - counterFunc) * (previousFunc - currentFunc));
- }
- // Secant method
- else {
- nextPoint = currentPoint - currentFunc * (currentPoint - counterPoint) / (currentFunc - counterFunc);
- }
-
- // Bisection method
- if (((currentPoint - nextPoint) * (nextPoint - (3 * counterPoint + currentPoint) / 4) < 0) ||
- (bisected && fabs(nextPoint - currentPoint) >= fabs(currentPoint - previousPoint) / 2) ||
- (!bisected && fabs(nextPoint - currentPoint) >= fabs(previousPoint - evenOlderPoint) / 2) ||
- (bisected && fabs(currentPoint - previousPoint) < epsilon) ||
- (!bisected && fabs(previousPoint - evenOlderPoint) < epsilon)) {
- nextPoint = (currentPoint + counterPoint) / 2;
- bisected = true;
- }
- else {
- bisected = false;
- }
-
- // Setup for next iteration
- evenOlderPoint = previousPoint;
- previousPoint = currentPoint;
- previousFunc = currentFunc;
- nextFunc = func(nextPoint);
- if (counterFunc * nextFunc < 0) {
- currentPoint = nextPoint;
- currentFunc = nextFunc;
- }
- else {
- counterPoint = nextPoint;
- counterFunc = nextFunc;
- }
- } while (++iteration < 30 && fabs(counterPoint - currentPoint) > epsilon);
-
- return nextPoint;
- }
-
-double secantRoot(double lowerBound, double upperBound, std::function<double(double)> func,
- double epsilon, int maxIters) {
- bool found = false;
-
- double x0 = lowerBound;
- double x1 = upperBound;
- double f0 = func(x0);
- double f1 = func(x1);
- double diff = 0;
- double x2 = 0;
- double f2 = 0;
-
- // 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].");
- }
-
- // Order the bounds
- if (f1 < f0) {
- std::swap(x0, x1);
- std::swap(f0, f1);
- }
-
- for (int iteration=0; iteration < maxIters; iteration++) {
- x2 = x1 - f1 * (x1 - x0)/(f1 - f0);
- f2 = func(x2);
+double brentRoot(double lowerBound,
+ double upperBound,
+ std::function<double(double)> func,
+ double epsilon) {
+ double counterPoint = lowerBound;
+ double currentPoint = upperBound;
+ double counterFunc = func(counterPoint);
+ double currentFunc = func(currentPoint);
+ if (counterFunc * currentFunc > 0.0) {
+ throw std::invalid_argument("Function values at the boundaries have the same sign [brentRoot].");
+ }
+ if (fabs(counterFunc) < fabs(currentFunc)) {
+ std::swap(counterPoint, currentPoint);
+ std::swap(counterFunc, currentFunc);
+ }
+
+ double previousPoint = counterPoint;
+ double previousFunc = counterFunc;
+ double evenOlderPoint = previousPoint;
+ double nextPoint;
+ double nextFunc;
+ int iteration = 0;
+ bool bisected = true;
+
+ do {
+ // Inverse quadratic interpolation
+ if (counterFunc != previousFunc && counterFunc != currentFunc && currentFunc != previousFunc) {
+ nextPoint = (counterPoint * currentFunc * previousFunc) / ((counterFunc - currentFunc) * (counterFunc - previousFunc));
+ nextPoint += (currentPoint * counterFunc * previousFunc) / ((currentFunc - counterFunc) * (currentFunc - previousFunc));
+ nextPoint += (previousPoint * currentFunc * counterFunc) / ((previousFunc - counterFunc) * (previousFunc - currentFunc));
+ }
+ // Secant method
+ else {
+ nextPoint = currentPoint - currentFunc * (currentPoint - counterPoint) / (currentFunc - counterFunc);
+ }
- // Update the bounds for the next iteration
- if (f2 < 0.0) {
- diff = x1 - x2;
- x1 = x2;
- f1 = f2;
+ // Bisection method
+ if (((currentPoint - nextPoint) * (nextPoint - (3 * counterPoint + currentPoint) / 4) < 0) ||
+ (bisected && fabs(nextPoint - currentPoint) >= fabs(currentPoint - previousPoint) / 2) ||
+ (!bisected && fabs(nextPoint - currentPoint) >= fabs(previousPoint - evenOlderPoint) / 2) ||
+ (bisected && fabs(currentPoint - previousPoint) < epsilon) ||
+ (!bisected && fabs(previousPoint - evenOlderPoint) < epsilon)) {
+ nextPoint = (currentPoint + counterPoint) / 2;
+ bisected = true;
}
else {
- diff = x0 - x2;
- x0 = x2;
- f0 = f2;
+ bisected = false;
}
- // Check to see if we're done
- if ((fabs(diff) <= epsilon) || (f2 == 0.0) ) {
- found = true;
- break;
+ // Setup for next iteration
+ evenOlderPoint = previousPoint;
+ previousPoint = currentPoint;
+ previousFunc = currentFunc;
+ nextFunc = func(nextPoint);
+ if (counterFunc * nextFunc < 0) {
+ currentPoint = nextPoint;
+ currentFunc = nextFunc;
}
+ else {
+ counterPoint = nextPoint;
+ counterFunc = nextFunc;
+ }
+ } while (++iteration < 30 && fabs(counterPoint - currentPoint) > epsilon);
+
+ return nextPoint;
+}
+
+double evaluatePolynomial(const std::vector<double> &coeffs,
+ double x) {
+ if (coeffs.empty()) {
+ throw std::invalid_argument("Polynomial coeffs must be non-empty.");
+ }
+ auto revIt = coeffs.crbegin();
+ double value = *revIt;
+ ++revIt;
+ for (; revIt != coeffs.crend(); ++revIt) {
+ value *= x;
+ value += *revIt;
}
+ return value;
+}
- if (found) {
- return x2;
+double evaluatePolynomialDerivative(const std::vector<double> &coeffs,
+ double x) {
+ if (coeffs.empty()) {
+ throw std::invalid_argument("Polynomial coeffs must be non-empty.");
}
- else {
- throw csm::Error(
- csm::Error::UNKNOWN_ERROR,
- "Could not find a root of the function using the secant method",
- "secantRoot");
+ int i = coeffs.size() - 1;
+ double value = i * coeffs[i];
+ --i;
+ for (; i > 0; --i) {
+ value *= x;
+ value += i * coeffs[i];
}
+ return value;
+}
+
+double polynomialRoot(const std::vector<double> &coeffs,
+ double guess,
+ double threshold,
+ int maxIterations) {
+ double root = guess;
+ double polyValue = evaluatePolynomial(coeffs, root);
+ double polyDeriv = 0.0;
+ for (int iteration = 0; iteration < maxIterations+1; iteration++) {
+ if (fabs(polyValue) < threshold) {
+ return root;
+ }
+ polyDeriv = evaluatePolynomialDerivative(coeffs, root);
+ if (fabs(polyDeriv) < 1e-15) {
+ throw std::invalid_argument("Derivative at guess (" +
+ std::to_string(guess) + ") is too close to 0.");
+ }
+ root -= polyValue / polyDeriv;
+ polyValue = evaluatePolynomial(coeffs, root);
+ }
+ throw std::invalid_argument("Root finder did not converge after " +
+ std::to_string(maxIterations) + " iterations");
}
double computeEllipsoidElevation(
diff --git a/tests/UtilitiesTests.cpp b/tests/UtilitiesTests.cpp
index 4dde5acc18b8585526d25ee70d7b5906d54645a7..f228c63c43a29eed7c306fb4346a4649af427b07 100644
--- a/tests/UtilitiesTests.cpp
+++ b/tests/UtilitiesTests.cpp
@@ -350,13 +350,25 @@ TEST(UtilitiesTests, brentRoot) {
EXPECT_THROW(brentRoot(-3.0, 3.0, testPoly), std::invalid_argument);
}
-TEST(UtilitiesTests, secantRoot) {
- std::function<double(double)> testPoly = [](double x) {
- return (x - 2) * (x + 1) * (x + 7);
- };
- EXPECT_NEAR(secantRoot(1.0, 3.0, testPoly, 1e-10), 2.0, 1e-10);
- EXPECT_NEAR(secantRoot(0.0, -3.0, testPoly, 1e-10), -1.0, 1e-10);
- EXPECT_THROW(secantRoot(-1000.0, 1000.0, testPoly), csm::Error);
+TEST(UtilitiesTests, polynomialEval) {
+ std::vector<double> coeffs = {-12.0, 4.0, -3.0, 1.0};
+ EXPECT_DOUBLE_EQ(evaluatePolynomial(coeffs, -1.0), -20.0);
+ EXPECT_DOUBLE_EQ(evaluatePolynomialDerivative(coeffs, -1.0), 13.0);
+ EXPECT_DOUBLE_EQ(evaluatePolynomial(coeffs, 0.0), -12.0);
+ EXPECT_DOUBLE_EQ(evaluatePolynomialDerivative(coeffs, 0.0), 4.0);
+ EXPECT_DOUBLE_EQ(evaluatePolynomial(coeffs, 2.0), -8.0);
+ EXPECT_DOUBLE_EQ(evaluatePolynomialDerivative(coeffs, 2.0), 4.0);
+ EXPECT_DOUBLE_EQ(evaluatePolynomial(coeffs, 3.5), 8.125);
+ EXPECT_DOUBLE_EQ(evaluatePolynomialDerivative(coeffs, 3.5), 19.75);
+ EXPECT_THROW(evaluatePolynomial(std::vector<double>(), 0.0), std::invalid_argument);
+ EXPECT_THROW(evaluatePolynomialDerivative(std::vector<double>(), 0.0), std::invalid_argument);
+}
+
+TEST(UtilitiesTests, polynomialRoot) {
+ std::vector<double> oneRootCoeffs = {-12.0, 4.0, -3.0, 1.0}; // roots are 3, +-2i
+ std::vector<double> noRootCoeffs = {4.0, 0.0, 1.0}; // roots are +-2i
+ EXPECT_NEAR(polynomialRoot(oneRootCoeffs, 0.0), 3.0, 1e-10);
+ EXPECT_THROW(polynomialRoot(noRootCoeffs, 0.0), std::invalid_argument);
}
TEST(UtilitiesTests, vectorProduct) {