Commit c2cdb320 authored by Jesse Mapel's avatar Jesse Mapel Committed by jlaura
Browse files

Fixed lagrange interpolation reading bad data with low number of points (#209) 

* moved lagrange interpolation to utilities for testability

* Added lagrange interpolation tests
parent 52649bc0

include/usgscsm/UsgsAstroLsSensorModel.h

+0 −11
Original line number Diff line number Diff line
@@ -953,17 +953,6 @@ private: 
      double&       dyl,

      double&       dzl) const;



   // Lagrange interpolation of variable order.

   void lagrangeInterp (

      const int&     numTime,

      const double*  valueArray,

      const double&  startTime,

      const double&  delTime,

      const double&  time,

      const int&     vectorLength,

      const int&     i_order,

      double*        valueVector ) const;



   // Intersects a LOS at a specified height above the ellipsoid.

   void losEllipsoidIntersect (

      const double& height,

include/usgscsm/Utilities.h

+9 −5
Original line number Diff line number Diff line
@@ -43,11 +43,15 @@ void createCameraLookVector( 
  const double& focalLength,

  double cameraLook[]);



//void calculateAttitudeCorrection(

//  const double& time,

//

//  double attCorr[9]);

//

void lagrangeInterp (

  const int&     numTime,

  const double*  valueArray,

  const double&  startTime,

  const double&  delTime,

  const double&  time,

  const int&     vectorLength,

  const int&     i_order,

  double*        valueVector);



// Methods for checking/accessing the ISD

src/UsgsAstroLsSensorModel.cpp

+0 −130
Original line number Diff line number Diff line
@@ -1726,136 +1726,6 @@ void UsgsAstroLsSensorModel::lightAberrationCorr( 
   dzl = cfac * vz;

}



//***************************************************************************

// UsgsAstroLsSensorModel::lagrangeInterp

//***************************************************************************

void UsgsAstroLsSensorModel::lagrangeInterp(

   const int&     numTime,

   const double*  valueArray,

   const double&  startTime,

   const double&  delTime,

   const double&  time,

   const int&     vectorLength,

   const int&     i_order,

   double*        valueVector) const

{

   // Lagrange interpolation for uniform post interval.

   // Largest order possible is 8th. Points far away from

   // data center are handled gracefully to avoid failure.



   // Compute index



   double fndex = (time - startTime) / delTime;

   int    index = int(fndex);



   //Time outside range

   //printf("%f | %i\n", fndex, index);

   //if (index < 0 || index >= numTime - 1) {

   //    printf("TIME ISSUE\n");

   // double d1 = fndex / (numTime - 1);

   // double d0 = 1.0 - d1;

   // int indx0 = vectorLength * (numTime - 1);

   // for (int i = 0; i < vectorLength; i++)

   // {

   // valueVector[i] = d0 * valueArray[i] + d1 * valueArray[indx0 + i];

   // }

   // return;

   //}



   if (index < 0)

   {

      index = 0;

   }

   if (index > numTime - 2)

   {

      index = numTime - 2;

   }



   // Define order, max is 8



   int order;

   if (index >= 3 && index < numTime - 4) {

      order = 8;

   }

   else if (index == 2 || index == numTime - 4) {

      order = 6;

   }

   else if (index == 1 || index == numTime - 3) {

      order = 4;

   }

   else if (index == 0 || index == numTime - 2) {

      order = 2;

   }

   if (order > i_order) {

      order = i_order;

   }



   // Compute interpolation coefficients

   double tp3, tp2, tp1, tm1, tm2, tm3, tm4, d[8];

   double tau = fndex - index;

   if (order == 2) {

      tm1 = tau - 1;

      d[0] = -tm1;

      d[1] = tau;

   }

   else if (order == 4) {

      tp1 = tau + 1;

      tm1 = tau - 1;

      tm2 = tau - 2;

      d[0] = -tau * tm1 * tm2 / 6.0;

      d[1] = tp1 *       tm1 * tm2 / 2.0;

      d[2] = -tp1 * tau *       tm2 / 2.0;

      d[3] = tp1 * tau * tm1 / 6.0;

   }

   else if (order == 6) {

      tp2 = tau + 2;

      tp1 = tau + 1;

      tm1 = tau - 1;

      tm2 = tau - 2;

      tm3 = tau - 3;

      d[0] = -tp1 * tau * tm1 * tm2 * tm3 / 120.0;

      d[1] = tp2 *       tau * tm1 * tm2 * tm3 / 24.0;

      d[2] = -tp2 * tp1 *       tm1 * tm2 * tm3 / 12.0;

      d[3] = tp2 * tp1 * tau *       tm2 * tm3 / 12.0;

      d[4] = -tp2 * tp1 * tau * tm1 *       tm3 / 24.0;

      d[5] = tp2 * tp1 * tau * tm1 * tm2 / 120.0;

   }

   else if (order == 8) {

      tp3 = tau + 3;

      tp2 = tau + 2;

      tp1 = tau + 1;

      tm1 = tau - 1;

      tm2 = tau - 2;

      tm3 = tau - 3;

      tm4 = tau - 4;

      // Why are the denominators hard coded, as it should be x[0] - x[i]

      d[0] = -tp2 * tp1 * tau * tm1 * tm2 * tm3 * tm4 / 5040.0;

      d[1] = tp3 *       tp1 * tau * tm1 * tm2 * tm3 * tm4 / 720.0;

      d[2] = -tp3 * tp2 *       tau * tm1 * tm2 * tm3 * tm4 / 240.0;

      d[3] = tp3 * tp2 * tp1 *       tm1 * tm2 * tm3 * tm4 / 144.0;

      d[4] = -tp3 * tp2 * tp1 * tau *       tm2 * tm3 * tm4 / 144.0;

      d[5] = tp3 * tp2 * tp1 * tau * tm1 *       tm3 * tm4 / 240.0;

      d[6] = -tp3 * tp2 * tp1 * tau * tm1 * tm2 *       tm4 / 720.0;

      d[7] = tp3 * tp2 * tp1 * tau * tm1 * tm2 * tm3 / 5040.0;

   }



   // Compute interpolated point

   int    indx0 = index - order / 2 + 1;

   for (int i = 0; i < vectorLength; i++)

   {

      valueVector[i] = 0.0;

   }



   for (int i = 0; i < order; i++)

   {

      int jndex = vectorLength * (indx0 + i);

      for (int j = 0; j < vectorLength; j++)

      {

         valueVector[j] += d[i] * valueArray[jndex + j];

      }

   }

}



//***************************************************************************

// UsgsAstroLsSensorModel::computeElevation

//***************************************************************************

src/Utilities.cpp

+121 −0
Original line number Diff line number Diff line 
#include "Utilities.h"

#include <Error.h>



using json = nlohmann::json;
@@ -114,6 +115,126 @@ void createCameraLookVector( 
  cameraLook[2] /= magnitude;

}



// Lagrange Interpolation for equally spaced data

void lagrangeInterp(

   const int&     numTime,

   const double*  valueArray,

   const double&  startTime,

   const double&  delTime,

   const double&  time,

   const int&     vectorLength,

   const int&     i_order,

   double*        valueVector) {

  // Lagrange interpolation for uniform post interval.

  // Largest order possible is 8th. Points far away from

  // data center are handled gracefully to avoid failure.



  if (numTime < 2) {

    throw csm::Error(

      csm::Error::INDEX_OUT_OF_RANGE,

      "At least 2 points are required to perform Lagrange interpolation.",

      "lagrangeInterp");

  }



  // Compute index



  double fndex = (time - startTime) / delTime;

  int    index = int(fndex);



  if (index < 0)

  {

    index = 0;

  }

  if (index > numTime - 2)

  {

    index = numTime - 2;

  }



  // Define order, max is 8



  int order;

  if (index >= 3 && index < numTime - 4) {

    order = 8;

  }

  else if (index >= 2 && index < numTime - 3) {

    order = 6;

  }

  else if (index >= 1 && index < numTime - 2) {

    order = 4;

  }

  else {

    order = 2;

  }

  if (order > i_order) {

    order = i_order;

  }



  // Compute interpolation coefficients

  double tp3, tp2, tp1, tm1, tm2, tm3, tm4, d[8];

  double tau = fndex - index;

  if (order == 2) {

    tm1 = tau - 1;

    d[0] = -tm1;

    d[1] = tau;

  }

  else if (order == 4) {

    tp1 = tau + 1;

    tm1 = tau - 1;

    tm2 = tau - 2;

    d[0] = -tau * tm1 * tm2 / 6.0;

    d[1] = tp1 *       tm1 * tm2 / 2.0;

    d[2] = -tp1 * tau *       tm2 / 2.0;

    d[3] = tp1 * tau * tm1 / 6.0;

  }

  else if (order == 6) {

    tp2 = tau + 2;

    tp1 = tau + 1;

    tm1 = tau - 1;

    tm2 = tau - 2;

    tm3 = tau - 3;

    d[0] = -tp1 * tau * tm1 * tm2 * tm3 / 120.0;

    d[1] = tp2 *       tau * tm1 * tm2 * tm3 / 24.0;

    d[2] = -tp2 * tp1 *       tm1 * tm2 * tm3 / 12.0;

    d[3] = tp2 * tp1 * tau *       tm2 * tm3 / 12.0;

    d[4] = -tp2 * tp1 * tau * tm1 *       tm3 / 24.0;

    d[5] = tp2 * tp1 * tau * tm1 * tm2 / 120.0;

  }

  else if (order == 8) {

    tp3 = tau + 3;

    tp2 = tau + 2;

    tp1 = tau + 1;

    tm1 = tau - 1;

    tm2 = tau - 2;

    tm3 = tau - 3;

    tm4 = tau - 4;

    // Why are the denominators hard coded, as it should be x[0] - x[i]

    d[0] = -tp2 * tp1 * tau * tm1 * tm2 * tm3 * tm4 / 5040.0;

    d[1] = tp3 *       tp1 * tau * tm1 * tm2 * tm3 * tm4 / 720.0;

    d[2] = -tp3 * tp2 *       tau * tm1 * tm2 * tm3 * tm4 / 240.0;

    d[3] = tp3 * tp2 * tp1 *       tm1 * tm2 * tm3 * tm4 / 144.0;

    d[4] = -tp3 * tp2 * tp1 * tau *       tm2 * tm3 * tm4 / 144.0;

    d[5] = tp3 * tp2 * tp1 * tau * tm1 *       tm3 * tm4 / 240.0;

    d[6] = -tp3 * tp2 * tp1 * tau * tm1 * tm2 *       tm4 / 720.0;

    d[7] = tp3 * tp2 * tp1 * tau * tm1 * tm2 * tm3 / 5040.0;

  }



  // Compute interpolated point

  int    indx0 = index - order / 2 + 1;

  for (int i = 0; i < vectorLength; i++)

  {

    valueVector[i] = 0.0;

  }



  for (int i = 0; i < order; i++)

  {

    int jndex = vectorLength * (indx0 + i);

    for (int j = 0; j < vectorLength; j++)

    {

       valueVector[j] += d[i] * valueArray[jndex + j];

    }

  }

}



// convert a measurement

double metric_conversion(double val, std::string from, std::string to) {

    json typemap = {

tests/UtilitiesTests.cpp

+159 −0
Original line number Diff line number Diff line
@@ -67,3 +67,162 @@ TEST(UtilitiesTests, createCameraLookVector) { 
  EXPECT_NEAR(cameraLook[1], 0.007999744, 1e-8);

  EXPECT_NEAR(cameraLook[2], -0.999968001, 1e-8);

}



TEST(UtilitiesTests, lagrangeInterp1Point) {

  int numTime = 1;

  std::vector<double> singlePoint = {1};

  std::vector<double> interpPoint = {0};

  double startTime = 0;

  double delTime = 1;

  double time = 0;

  int vectorLength = 1;

  int order = 8;



  try {

     lagrangeInterp(numTime, &singlePoint[0], startTime, delTime,

                    time, vectorLength, order, &interpPoint[0]);

     FAIL() << "Expected an error";

  }

  catch(csm::Error &e) {

     EXPECT_EQ(e.getError(), csm::Error::INDEX_OUT_OF_RANGE);

  }

  catch(...) {

     FAIL() << "Expected csm INDEX_OUT_OF_RANGE error";

  }

}



TEST(UtilitiesTests, lagrangeInterp2ndOrder) {

  int numTime = 8;

  std::vector<double> interpValues = {1, 2, 4, 8, 16, 32, 64, 128};

  std::vector<double> outputValue = {0};

  double startTime = 0;

  double delTime = 1;

  double time = 3.5;

  int vectorLength = 1;

  int order = 2;



  lagrangeInterp(numTime, &interpValues[0], startTime, delTime,

                    time, vectorLength, order, &outputValue[0]);

  EXPECT_DOUBLE_EQ(outputValue[0], 24.0 / 2.0);

}



TEST(UtilitiesTests, lagrangeInterp4thOrder) {

  int numTime = 8;

  std::vector<double> interpValues = {1, 2, 4, 8, 16, 32, 64, 128};

  std::vector<double> outputValue = {0};

  double startTime = 0;

  double delTime = 1;

  double time = 3.5;

  int vectorLength = 1;

  int order = 4;



  lagrangeInterp(numTime, &interpValues[0], startTime, delTime,

                    time, vectorLength, order, &outputValue[0]);

  EXPECT_DOUBLE_EQ(outputValue[0], 180.0 / 16.0);

}



TEST(UtilitiesTests, lagrangeInterp6thOrder) {

  int numTime = 8;

  std::vector<double> interpValues = {1, 2, 4, 8, 16, 32, 64, 128};

  std::vector<double> outputValue = {0};

  double startTime = 0;

  double delTime = 1;

  double time = 3.5;

  int vectorLength = 1;

  int order = 6;



  lagrangeInterp(numTime, &interpValues[0], startTime, delTime,

                    time, vectorLength, order, &outputValue[0]);

  EXPECT_DOUBLE_EQ(outputValue[0], 2898.0 / 256.0);

}



TEST(UtilitiesTests, lagrangeInterp8thOrder) {

  int numTime = 8;

  std::vector<double> interpValues = {1, 2, 4, 8, 16, 32, 64, 128};

  std::vector<double> outputValue = {0};

  double startTime = 0;

  double delTime = 1;

  double time = 3.5;

  int vectorLength = 1;

  int order = 8;



  lagrangeInterp(numTime, &interpValues[0], startTime, delTime,

                    time, vectorLength, order, &outputValue[0]);

  EXPECT_DOUBLE_EQ(outputValue[0], 23169.0 / 2048.0);

}



TEST(UtilitiesTests, lagrangeInterpReduced2ndOrder) {

  int numTime = 8;

  std::vector<double> interpValues = {1, 2, 4, 8, 16, 32, 64, 128};

  std::vector<double> outputValue = {0};

  double startTime = 0;

  double delTime = 1;

  double time = 0.5;

  int vectorLength = 1;

  int order = 8;



  lagrangeInterp(numTime, &interpValues[0], startTime, delTime,

                    time, vectorLength, order, &outputValue[0]);

  EXPECT_DOUBLE_EQ(outputValue[0], 3.0 / 2.0);



  time = 6.5;

  lagrangeInterp(numTime, &interpValues[0], startTime, delTime,

                    time, vectorLength, order, &outputValue[0]);

  EXPECT_DOUBLE_EQ(outputValue[0], 192.0 / 2.0);

}



TEST(UtilitiesTests, lagrangeInterpReduced4thOrder) {

  int numTime = 8;

  std::vector<double> interpValues = {1, 2, 4, 8, 16, 32, 64, 128};

  std::vector<double> outputValue = {0};

  double startTime = 0;

  double delTime = 1;

  double time = 1.5;

  int vectorLength = 1;

  int order = 8;



  lagrangeInterp(numTime, &interpValues[0], startTime, delTime,

                    time, vectorLength, order, &outputValue[0]);

  EXPECT_DOUBLE_EQ(outputValue[0], 45.0 / 16.0);



  time = 5.5;

  lagrangeInterp(numTime, &interpValues[0], startTime, delTime,

                    time, vectorLength, order, &outputValue[0]);

  EXPECT_DOUBLE_EQ(outputValue[0], 720.0 / 16.0);

}



TEST(UtilitiesTests, lagrangeInterpReduced6thOrder) {

  int numTime = 8;

  std::vector<double> interpValues = {1, 2, 4, 8, 16, 32, 64, 128};

  std::vector<double> outputValue = {0};

  double startTime = 0;

  double delTime = 1;

  double time = 2.5;

  int vectorLength = 1;

  int order = 8;



  lagrangeInterp(numTime, &interpValues[0], startTime, delTime,

                    time, vectorLength, order, &outputValue[0]);

  EXPECT_DOUBLE_EQ(outputValue[0], 1449.0 / 256.0);



  time = 4.5;

  lagrangeInterp(numTime, &interpValues[0], startTime, delTime,

                    time, vectorLength, order, &outputValue[0]);

  EXPECT_DOUBLE_EQ(outputValue[0], 5796.0 / 256.0);

}



TEST(UtilitiesTests, lagrangeInterp2D) {

  int numTime = 2;

  std::vector<double> interpValues = {0, 1, 1, 2};

  std::vector<double> outputValue = {0, 0};

  double startTime = 0;

  double delTime = 1;

  double time = 0.5;

  int vectorLength = 2;

  int order = 2;



  lagrangeInterp(numTime, &interpValues[0], startTime, delTime,

                    time, vectorLength, order, &outputValue[0]);

  EXPECT_DOUBLE_EQ(outputValue[0], 0.5);

  EXPECT_DOUBLE_EQ(outputValue[1], 1.5);

}