diff --git a/include/usgscsm/UsgsAstroLsSensorModel.h b/include/usgscsm/UsgsAstroLsSensorModel.h
index 78bb3633fe9ac28c8e037fc9a3dba63486b05341..f935cd2014612265ecb8c2b1aff858bac3303d8c 100644
--- a/include/usgscsm/UsgsAstroLsSensorModel.h
+++ b/include/usgscsm/UsgsAstroLsSensorModel.h
@@ -108,6 +108,11 @@ public:
std::vector<double> m_positions;
std::vector<double> m_velocities;
std::vector<double> m_quaternions;
+ std::vector<int> m_detectorNodes;
+ std::vector<double> m_detectorXCoords;
+ std::vector<double> m_detectorYCoords;
+ std::vector<double> m_detectorLineCoeffs;
+ double m_averageDetectorSize;
std::vector<double> m_currentParameterValue;
std::vector<csm::param::Type> m_parameterType;
csm::EcefCoord m_referencePointXyz;
diff --git a/include/usgscsm/Utilities.h b/include/usgscsm/Utilities.h
index 44375d9b2f75994348c4541f9045b6d6a14fa927..94459d9d4ed1bda9d0f1b2fadfd4a18b79c2f603 100644
--- a/include/usgscsm/Utilities.h
+++ b/include/usgscsm/Utilities.h
@@ -12,6 +12,24 @@
#include <Warning.h>
+double distanceToLine(double x, double y,
+ double a, double b, double c);
+
+double distanceToPlane(double x, double y, double z,
+ double a, double b, double c, double d);
+
+void line(double x1, double y1, double x2, double y2,
+ double &a, double &b, double &c);
+
+void plane(double x0, double y0, double z0,
+ double v1x, double v1y, double v1z,
+ double v2x, double v2y, double v2z,
+ double &a, double &b, double &c, double &d);
+
+std::vector<int> fitLinearApproximation(const std::vector<double> &x,
+ const std::vector<double> &y,
+ double tolerance);
+
// methods pulled out of los2ecf and computeViewingPixel
// for now, put everything in here.
diff --git a/src/UsgsAstroLsSensorModel.cpp b/src/UsgsAstroLsSensorModel.cpp
index c1da7469836d19e315a7a68b7efcd3cdb20d7652..ec9350b2c294db8434ceaae506d61ef217682f34 100644
--- a/src/UsgsAstroLsSensorModel.cpp
+++ b/src/UsgsAstroLsSensorModel.cpp
@@ -506,6 +506,11 @@ void UsgsAstroLsSensorModel::reset()
m_positions.clear(); // 42
m_velocities.clear(); // 43
m_quaternions.clear(); // 44
+ m_detectorNodes.clear();
+ m_detectorXCoords.clear();
+ m_detectorYCoords.clear();
+ m_detectorLineCoeffs.clear();
+ m_averageDetectorSize = 0.0;
m_currentParameterValue.assign(NUM_PARAMETERS,0.0);
m_parameterType.assign(NUM_PARAMETERS,csm::param::REAL);
@@ -595,6 +600,37 @@ void UsgsAstroLsSensorModel::updateState()
MESSAGE_LOG(m_logger, "updateState: half time duration set to {}",
m_halfTime)
+ // compute linearized detector coordinates
+ m_detectorXCoords.resize(m_nSamples);
+ m_detectorYCoords.resize(m_nSamples);
+
+ for (int i = 0; i < m_nSamples; i++) {
+ computeDistortedFocalPlaneCoordinates(
+ 0.5, i,
+ m_detectorSampleOrigin, m_detectorLineOrigin,
+ m_detectorSampleSumming, 1.0,
+ m_startingSample, 0.0,
+ m_iTransS, m_iTransL,
+ m_detectorXCoords[i], m_detectorYCoords[i]
+ );
+ }
+ m_averageDetectorSize = 0;
+ for (int i = 1; i < m_nSamples; i++) {
+ double xDist = m_detectorXCoords[i] - m_detectorXCoords[i-1];
+ double yDist = m_detectorYCoords[i] - m_detectorYCoords[i-1];
+ m_averageDetectorSize += sqrt(xDist*xDist + yDist*yDist);
+ }
+ m_averageDetectorSize /= (m_nSamples-1);
+ m_detectorNodes = fitLinearApproximation(m_detectorXCoords, m_detectorYCoords,
+ m_averageDetectorSize);
+
+ m_detectorLineCoeffs.resize(3 * (m_detectorNodes.size() - 1));
+ for (size_t i = 0; i + 1 < m_detectorNodes.size(); i++) {
+ line(m_detectorXCoords[m_detectorNodes[i]], m_detectorYCoords[m_detectorNodes[i]],
+ m_detectorXCoords[m_detectorNodes[i+1]], m_detectorYCoords[m_detectorNodes[i+1]],
+ m_detectorLineCoeffs[3*i], m_detectorLineCoeffs[3*i+1], m_detectorLineCoeffs[3*i+2]);
+ }
+
// Parameter covariance, hardcoded accuracy values
int num_params = NUM_PARAMETERS;
int num_paramsSquare = num_params * num_params;
diff --git a/src/Utilities.cpp b/src/Utilities.cpp
index dbcc87d0e6c73d66ca74bab02658a4f67287ff01..5c17d131b6b27bc124fcbc1fdddb968b701b786f 100644
--- a/src/Utilities.cpp
+++ b/src/Utilities.cpp
@@ -1,8 +1,108 @@
#include "Utilities.h"
+
+#include <cmath>
#include <Error.h>
+#include <stack>
+#include <utility>
using json = nlohmann::json;
+// Calculate the signed distance from a 2D point to a line given in standard form
+//
+// --NOTE-- This function assumes that the coefficients of the line have been reduced
+// so that sqrt(a^2 + b^2) = 1
+double distanceToLine(double x, double y,
+ double a, double b, double c) {
+ return a*x + b*y + c;
+}
+
+// Calculate the distance from a 3D point to a plane given in standard form
+//
+// --NOTE-- This function assumes that the coefficients of the plane have been reduced
+// so that sqrt(a^2 + b^2 + c^2) = 1
+double distanceToPlane(double x, double y, double z,
+ double a, double b, double c, double d) {
+ return std::abs(a*x + b*y + c*z + d);
+}
+
+// Calculate the standard form coefficients of a line that passes through two points
+//
+// --NOTE-- The coefficients have been reduced such that sqrt(a^2 + b^2) = 1
+void line(double x1, double y1, double x2, double y2,
+ double &a, double &b, double &c) {
+ a = y1 - y2;
+ b = x2 - x1;
+ c = x1*y2 - x2*y1;
+ double scale = sqrt(a*a + b*b);
+ a /= scale;
+ b /= scale;
+ c /= scale;
+}
+
+// Calculate the standard form coefficients of a plane that passes through a point
+// and contains two vectors.
+//
+// --NOTE-- The coefficients have been reduced such that sqrt(a^2 + b^2 + c^2) = 1
+void plane(double x0, double y0, double z0,
+ double v1x, double v1y, double v1z,
+ double v2x, double v2y, double v2z,
+ double &a, double &b, double &c, double &d) {
+ // Compute normal vector and scale
+ a = v1y*v2z - v1z*v2y;
+ b = v1z*v2x - v1x*v2z;
+ c = v1x*v2y - v1y*v2x;
+ double scale = sqrt(a*a + b*b + c*c);
+ a /= scale;
+ b /= scale;
+ c /= scale;
+ // Shift to point
+ d = - (a*x0 + b*y0 + c*z0);
+}
+
+
+
+// Fit a piecewise-linear approximations to 2D data within a tolerance
+//
+// Returns a vector of node indices
+std::vector<int> fitLinearApproximation(const std::vector<double> &x,
+ const std::vector<double> &y,
+ double tolerance) {
+ std::vector<int> nodes;
+ nodes.push_back(0);
+
+ std::stack<std::pair<int, int>> workStack;
+ workStack.push(std::make_pair(0, x.size() - 1));
+ while (!workStack.empty()) {
+ std::pair<int, int> range = workStack.top();
+ workStack.pop();
+ double a, b, c;
+ line(x[range.first], y[range.first], x[range.second], y[range.second], a, b, c);
+ double maxError = 0;
+ int maxIndex = (range.second + range.first) / 2;
+ for (int i = range.first + 1; i < range.second; i++) {
+ double error = std::abs(distanceToLine(x[i], y[i], a, b, c));
+ if (error > maxError) {
+ maxError = error;
+ maxIndex = i;
+ }
+ }
+ // If the max error is greater than the tolerance, split at the largest error
+ // Do not split if the range only contains two nodes
+ if (maxError > tolerance && range.second - range.first > 1) {
+ // Append the second range and then the first range
+ // so that nodes are added in the same order they came in
+ workStack.push(std::make_pair(maxIndex, range.second));
+ workStack.push(std::make_pair(range.first, maxIndex));
+ }
+ else {
+ // segment is good so append last point to nodes
+ nodes.push_back(range.second);
+ }
+ }
+
+ return nodes;
+}
+
// Calculates a rotation matrix from Euler angles
// in - euler[3]
// out - rotationMatrix[9]
diff --git a/tests/UtilitiesTests.cpp b/tests/UtilitiesTests.cpp
index 19ca11b5db7e7ff3819fd0637e43f491073c7255..d6fdca569ba8617f5b011db8ff6c0c5f2a28d455 100644
--- a/tests/UtilitiesTests.cpp
+++ b/tests/UtilitiesTests.cpp
@@ -11,6 +11,81 @@
using json = nlohmann::json;
+TEST(UtilitiesTests, distanceToLine) {
+ // Line passing through (0, 1) and (2,2)
+ double a = -1.0 / sqrt(5);
+ double b = 2.0 / sqrt(5);
+ double c = -2.0 / sqrt(5);
+ // Point on line
+ EXPECT_NEAR(distanceToLine(4.0, 3.0, a, b, c), 0, 1e-12);
+ // Point above line
+ EXPECT_DOUBLE_EQ(distanceToLine(1.0, 4.0, a, b, c), sqrt(5));
+ // Point below line
+ EXPECT_DOUBLE_EQ(distanceToLine(4.5, 2.0, a, b, c), -sqrt(5) / 2.0);
+}
+
+TEST(UtilitiesTests, line) {
+ double x1 = 0.0;
+ double y1 = 1.0;
+ double x2 = 2.0;
+ double y2 = 2.0;
+ double a, b, c;
+ line(x1, y1, x2, y2, a, b, c);
+ EXPECT_DOUBLE_EQ(a, -1.0 / sqrt(5));
+ EXPECT_DOUBLE_EQ(b, 2.0 / sqrt(5));
+ EXPECT_DOUBLE_EQ(c, -2.0 / sqrt(5));
+}
+
+TEST(UtilitiesTests, distanceToPlane) {
+ // Plane passing through (0, 0, 1), (0, 2, 0) and (3, 0, 0)
+ double a = 2.0 / 7.0;
+ double b = 3.0 / 7.0;
+ double c = 6.0 / 7.0;
+ double d = -12.0 / 7.0;
+ // Points on plane
+ EXPECT_NEAR(distanceToPlane(0.0, 0.0, 2.0, a, b, c, d), 0.0, 1e-12);
+ EXPECT_NEAR(distanceToPlane(0.0, 4.0, 0.0, a, b, c, d), 0.0, 1e-12);
+ EXPECT_NEAR(distanceToPlane(6.0, 0.0, 0.0, a, b, c, d), 0.0, 1e-12);
+ // Points off plane
+ EXPECT_DOUBLE_EQ(distanceToPlane(0.0, 0.0, 0.0, a, b, c, d), 12.0 / 7.0);
+ EXPECT_DOUBLE_EQ(distanceToPlane(0.0, 0.0, 1.0, a, b, c, d), 6.0 / 7.0);
+ EXPECT_DOUBLE_EQ(distanceToPlane(0.0, 2.0, 0.0, a, b, c, d), 6.0 / 7.0);
+ EXPECT_DOUBLE_EQ(distanceToPlane(3.0, 0.0, 0.0, a, b, c, d), 6.0 / 7.0);
+}
+
+TEST(UtilitiesTests, plane) {
+ double x0 = 0.0;
+ double y0 = 0.0;
+ double z0 = 2.0;
+ double v1x = 0.0;
+ double v1y = 4.0;
+ double v1z = -2.0;
+ double v2x = 6.0;
+ double v2y = 0.0;
+ double v2z = -2.0;
+ double a, b, c, d;
+ plane(x0, y0, z0, v1x, v1y, v1z, v2x, v2y, v2z, a, b, c, d);
+ EXPECT_DOUBLE_EQ(a, -2.0 / 7.0);
+ EXPECT_DOUBLE_EQ(b, -3.0 / 7.0);
+ EXPECT_DOUBLE_EQ(c, -6.0 / 7.0);
+ EXPECT_DOUBLE_EQ(d, 12.0 / 7.0);
+}
+
+TEST(UtilitiesTests, fitLinearApproximation) {
+ std::vector<double> x = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10};
+ std::vector<double> y = {1, 0, 5, 7, -2, 1, 2, 2, 1, 0, 1};
+
+ std::vector<int> nodes = fitLinearApproximation(x, y, 1.0);
+ ASSERT_EQ(nodes.size(), 7);
+ EXPECT_EQ(nodes[0], 0);
+ EXPECT_EQ(nodes[1], 1);
+ EXPECT_EQ(nodes[2], 3);
+ EXPECT_EQ(nodes[3], 4);
+ EXPECT_EQ(nodes[4], 6);
+ EXPECT_EQ(nodes[5], 9);
+ EXPECT_EQ(nodes[6], 10);
+}
+
TEST(UtilitiesTests, calculateRotationMatrixFromEuler) {
double euler[3], rotationMatrix[9];