Merge pull request #261 from USGS-Astrogeology/gendistance

Merge distance and linearization into dev

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;

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.

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;

src/Utilities.cpp

 #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]

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];