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

Replaced secant root with Newton's method in SAR (#302) 

* Added Newton's method root finder

* Replaced secant root with Newton's method

* cleaned up slantRangeToGroundRange

* Review comments

* Better error message
parent 342b2168

include/usgscsm/Utilities.h

+19 −6
Original line number Diff line number Diff line
@@ -78,11 +78,24 @@ 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,

// 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(

src/UsgsAstroSarSensorModel.cpp

+14 −26
Original line number Diff line number Diff line
@@ -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);

}

src/Utilities.cpp

+108 −113
Original line number Diff line number Diff line
@@ -279,8 +279,7 @@ void lagrangeInterp( 
  }

}



double brentRoot(

  double lowerBound,

double brentRoot(double lowerBound,

                 double upperBound,

                 std::function<double(double)> func,

                 double epsilon) {
@@ -347,61 +346,57 @@ double brentRoot( 
  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);

double evaluatePolynomial(const std::vector<double> &coeffs,

                          double x) {

  if (coeffs.empty()) {

    throw std::invalid_argument("Polynomial coeffs must be non-empty.");

  }



  for (int iteration=0; iteration < maxIters; iteration++) {

    x2 = x1 - f1 * (x1 - x0)/(f1 - f0);

    f2 = func(x2);



    // Update the bounds for the next iteration

    if (f2 < 0.0) {

      diff = x1 - x2;

      x1 = x2;

      f1 = f2;

  auto revIt = coeffs.crbegin();

  double value = *revIt;

  ++revIt;

  for (; revIt != coeffs.crend(); ++revIt) {

    value *= x;

    value += *revIt;

  }

    else {

      diff = x0 - x2;

      x0 = x2;

      f0 = f2;

  return value;

}



    // Check to see if we're done

    if ((fabs(diff) <= epsilon) || (f2 == 0.0) ) {

      found = true;

      break;

double evaluatePolynomialDerivative(const std::vector<double> &coeffs,

                                    double x) {

  if (coeffs.empty()) {

    throw std::invalid_argument("Polynomial coeffs must be non-empty.");

  }

  int i = coeffs.size() - 1;

  double value = i * coeffs[i];

  --i;

  for (; i > 0; --i) {

    value *= x;

    value += i * coeffs[i];

  }

  return value;

}



  if (found) {

   return x2;

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;

    }

  else {

    throw csm::Error(

    csm::Error::UNKNOWN_ERROR,

    "Could not find a root of the function using the secant method",

    "secantRoot");

    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(

tests/UtilitiesTests.cpp

+19 −7
Original line number Diff line number Diff line
@@ -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) {