From c2cdb320c72984901d5f77cace08d080e881643b Mon Sep 17 00:00:00 2001
From: Jesse Mapel <jam826@nau.edu>
Date: Wed, 3 Apr 2019 15:08:10 -0700
Subject: [PATCH] Fixed lagrange interpolation reading bad data with low number
of points (#209)
* moved lagrange interpolation to utilities for testability
* Added lagrange interpolation tests
---
include/usgscsm/UsgsAstroLsSensorModel.h | 11 --
include/usgscsm/Utilities.h | 14 +-
src/UsgsAstroLsSensorModel.cpp | 130 ------------------
src/Utilities.cpp | 121 +++++++++++++++++
tests/UtilitiesTests.cpp | 159 +++++++++++++++++++++++
5 files changed, 289 insertions(+), 146 deletions(-)
diff --git a/include/usgscsm/UsgsAstroLsSensorModel.h b/include/usgscsm/UsgsAstroLsSensorModel.h
index 5329ede..1bb1823 100644
--- a/include/usgscsm/UsgsAstroLsSensorModel.h
+++ b/include/usgscsm/UsgsAstroLsSensorModel.h
@@ -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,
diff --git a/include/usgscsm/Utilities.h b/include/usgscsm/Utilities.h
index 719a724..70ffd3e 100644
--- a/include/usgscsm/Utilities.h
+++ b/include/usgscsm/Utilities.h
@@ -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
diff --git a/src/UsgsAstroLsSensorModel.cpp b/src/UsgsAstroLsSensorModel.cpp
index 2aa6336..c43da11 100644
--- a/src/UsgsAstroLsSensorModel.cpp
+++ b/src/UsgsAstroLsSensorModel.cpp
@@ -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
//***************************************************************************
diff --git a/src/Utilities.cpp b/src/Utilities.cpp
index b601de8..6a33c34 100644
--- a/src/Utilities.cpp
+++ b/src/Utilities.cpp
@@ -1,4 +1,5 @@
#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 = {
diff --git a/tests/UtilitiesTests.cpp b/tests/UtilitiesTests.cpp
index 5473dc5..4e7010c 100644
--- a/tests/UtilitiesTests.cpp
+++ b/tests/UtilitiesTests.cpp
@@ -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);
+}
--
GitLab