Unverified Commit 5d5b7103 authored by Jesse Mapel's avatar Jesse Mapel Committed by GitHub
Browse files

Added methods for computing linearized detector coordinates (#257) 

* Added line and point comp and distance

* Finished linearization

* Fixed removing wrong line

* More fixes

* First pass at detector coords

* Added line coefficient computation
parent f5796751

include/usgscsm/UsgsAstroLsSensorModel.h

+5 −0
Original line number Diff line number Diff line
@@ -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

+18 −0
Original line number Diff line number Diff line
@@ -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

+36 −0
Original line number Diff line number Diff line
@@ -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

+100 −0
Original line number Diff line number Diff line 
#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

+75 −0
Original line number Diff line number Diff line
@@ -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];