Gtests for Utilities.cpp (#181)

* Finishing modularization of UsgsAstroLsSensorModel::computeViewingPixel

* Fixes per request

* Removing ISIS references in variable names while I'm here

* Beginning to implement tests for modularization changes in LS sensor model class

* Adding tests for Utilities as well as a few minor modifications

* Making suggested changes and adding test for caculateAttitudeCorrection

* Fixing vector size

include/usgscsm/UsgsAstroLsSensorModel.h

@@ -891,6 +891,11 @@ public:
    //> This method sets the planetary ellipsoid.
    //<
 
+   void calculateAttitudeCorrection(
+       const double& time,
+       const std::vector<double>& adj,
+       double attCorr[9]) const;
+
 private:
 
    void determineSensorCovarianceInImageSpace(
@@ -923,11 +928,6 @@ private:
    void getQuaternions(const double& time,
                        double quaternion[4]) const;
 
-   void calculateAttitudeCorrection(
-       const double& time,
-       const std::vector<double>& adj,
-       double attCorr[9]) const;
-
 // This method computes the imaging locus.
 // imaging locus : set of ground points associated with an image pixel.
    void losToEcf(

include/usgscsm/Utilities.h

@@ -15,7 +15,7 @@
 // for now, put everything in here.
 // TODO: later, consider if it makes sense to pull sample/line offsets out
 // Compute distorted focalPlane coordinates in mm
-std::tuple<double, double> computeDistortedFocalPlaneCoordinates(
+void computeDistortedFocalPlaneCoordinates(
   const double& line,
   const double& sample,
   const double& sampleOrigin,
@@ -24,7 +24,8 @@ std::tuple<double, double> computeDistortedFocalPlaneCoordinates(
   const double& startingSample,
   const double& lineOffset,
   const double iTransS[],
-  const double iTransL[]);
+  const double iTransL[],
+  std::tuple<double, double>& natFocalPlane);
 
 void calculateRotationMatrixFromQuaternions(
   double quaternions[4],
@@ -39,8 +40,6 @@ void createCameraLookVector(
   const double& undistortedFocalPlaneY,
   const double& zDirection,
   const double& focalLength,
-  const double& focalLengthBias,
-  const double& halfSwath,
   double cameraLook[]);
 
 //void calculateAttitudeCorrection(

src/UsgsAstroLsSensorModel.cpp

@@ -1657,7 +1657,11 @@ void UsgsAstroLsSensorModel::getQuaternions(const double& time, double q[4]) con
 }
 
 
-void UsgsAstroLsSensorModel::calculateAttitudeCorrection(const double& time, const std::vector<double>& adj, double attCorr[9]) const {
+void UsgsAstroLsSensorModel::calculateAttitudeCorrection(
+   const double& time,
+   const std::vector<double>& adj,
+   double attCorr[9]) const
+{
   double aTime = time - m_t0Quat;
   double euler[3];
   double nTime = aTime / m_halfTime;
@@ -1704,7 +1708,7 @@ void UsgsAstroLsSensorModel::losToEcf(
 
    // Compute distorted image coordinates in mm (sample, line on image (pixels) -> focal plane
    std::tuple<double, double> natFocalPlane;
-   natFocalPlane = computeDistortedFocalPlaneCoordinates(fractionalLine, sampleUSGSFull, m_detectorSampleOrigin, m_detectorLineOrigin, m_detectorSampleSumming, m_startingSample, m_detectorLineOffset, m_iTransS, m_iTransL);
+   computeDistortedFocalPlaneCoordinates(fractionalLine, sampleUSGSFull, m_detectorSampleOrigin, m_detectorLineOrigin, m_detectorSampleSumming, m_startingSample, m_detectorLineOffset, m_iTransS, m_iTransL, natFocalPlane);
 
    // Remove lens distortion
    std::tuple<double, double> undistortedPoint;
@@ -1712,7 +1716,7 @@ void UsgsAstroLsSensorModel::losToEcf(
 
   // Define imaging ray (look vector) in camera space
    double cameraLook[3];
-   createCameraLookVector(std::get<0>(undistortedPoint), std::get<1>(undistortedPoint), m_zDirection, m_focal, getValue(15, adj), m_halfSwath, cameraLook);
+   createCameraLookVector(std::get<0>(undistortedPoint), std::get<1>(undistortedPoint), m_zDirection, m_focal, cameraLook);
 
    // Apply attitude correction
    double attCorr[9];

src/Utilities.cpp

@@ -3,8 +3,10 @@
 using json = nlohmann::json;
 
 // Calculates a rotation matrix from Euler angles
-void calculateRotationMatrixFromEuler(double euler[],
-                                      double rotationMatrix[]) {
+void calculateRotationMatrixFromEuler(
+    double euler[],
+    double rotationMatrix[])
+{
   double cos_a = cos(euler[0]);
   double sin_a = sin(euler[0]);
   double cos_b = cos(euler[1]);
@@ -25,7 +27,10 @@ void calculateRotationMatrixFromEuler(double euler[],
 
 
 // uses a quaternion to calclate a rotation matrix.
-void calculateRotationMatrixFromQuaternions(double q[4], double rotationMatrix[9]) {
+void calculateRotationMatrixFromQuaternions(
+    double q[4],
+    double rotationMatrix[9])
+{
   double norm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
   q[0] /= norm;
   q[1] /= norm;
@@ -43,7 +48,7 @@ void calculateRotationMatrixFromQuaternions(double q[4], double rotationMatrix[9
   rotationMatrix[8] = -q[0] * q[0] - q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
 }
 
-std::tuple<double, double> computeDistortedFocalPlaneCoordinates(
+void computeDistortedFocalPlaneCoordinates(
     const double& line,
     const double& sample,
     const double& sampleOrigin,
@@ -52,7 +57,9 @@ std::tuple<double, double> computeDistortedFocalPlaneCoordinates(
     const double& startingSample,
     const double& lineOffset,
     const double iTransS[],
-  const double iTransL[]) {
+    const double iTransL[],
+    std::tuple<double, double>& natFocalPlane)
+{
   double detSample = (sample - 1.0) * sampleSumming + startingSample;
   double m11 = iTransL[1];
   double m12 = iTransL[2];
@@ -66,9 +73,8 @@ std::tuple<double, double> computeDistortedFocalPlaneCoordinates(
   double p21 = -m21 / determinant;
   double p22 = m22 / determinant;
 
-  double distortedLine = p11 * t1 + p12 * t2;
-  double distortedSample = p21 * t1 + p22 * t2;
-  return std::make_tuple(distortedLine, distortedSample);
+  std::get<0>(natFocalPlane) = p11 * t1 + p12 * t2;
+  std::get<1>(natFocalPlane) = p21 * t1 + p22 * t2;
 };
 
 // Define imaging ray in image space (In other words, create a look vector in camera space)
@@ -77,12 +83,11 @@ void createCameraLookVector(
     const double& undistortedFocalPlaneY,
     const double& zDirection,
     const double& focalLength,
-  const double& focalLengthBias,
-  const double& halfSwath,
-  double cameraLook[]) {
+    double cameraLook[])
+{
   cameraLook[0] = -undistortedFocalPlaneX * zDirection;
   cameraLook[1] = -undistortedFocalPlaneY * zDirection;
-   cameraLook[2] = -focalLength * (1.0 - focalLengthBias / halfSwath);
+  cameraLook[2] = -focalLength;
   double magnitude = sqrt(cameraLook[0] * cameraLook[0]
                 + cameraLook[1] * cameraLook[1]
                 + cameraLook[2] * cameraLook[2]);

tests/CMakeLists.txt

@@ -6,7 +6,8 @@ add_executable(runCSMCameraModelTests
                FrameCameraTests.cpp
                LineScanCameraTests.cpp
                DistortionTests.cpp
-               ISDParsingTests.cpp)
+               ISDParsingTests.cpp
+               UtilitiesTests.cpp)
 if(WIN32)
   option(CMAKE_USE_WIN32_THREADS_INIT "using WIN32 threads" ON)
   option(gtest_disable_pthreads "Disable uses of pthreads in gtest." ON)

tests/LineScanCameraTests.cpp

+#define _USE_MATH_DEFINES
+
+#include "Fixtures.h"
 #include "UsgsAstroPlugin.h"
 #include "UsgsAstroLsSensorModel.h"
 
 #include <json.hpp>
 #include <gtest/gtest.h>
 
-#include "Fixtures.h"
+#include <math.h>
+#include <iostream>
 
 using json = nlohmann::json;
 
@@ -124,3 +128,25 @@ TEST_F(ConstAngularVelocityLineScanSensorModel, OffBody4) {
    // EXPECT_NEAR(groundPt.y, -14.99325304, 1e-8);
    // EXPECT_NEAR(groundPt.z, -14.99325304, 1e-8);
 }
+
+TEST_F(ConstVelocityLineScanSensorModel, calculateAttitudeCorrection) {
+  std::vector<double> adj;
+  double attCorr[9];
+  adj.resize(15, 0);
+  // Pi/2 with simply compensating for member variable m_flyingHeight in UsgsAstroLsSensorModel
+  adj[7] = (M_PI / 2) * 990.0496255790623081338708;
+  sensorModel->calculateAttitudeCorrection(999.5, adj, attCorr);
+
+  // EXPECT_NEARs are used here instead of EXPECT_DOUBLE_EQs because index 0 and 8 of the matrix
+  // are evaluating to 6.12...e-17. This is too small to be worried about here, but
+  // EXPECT_DOUBLE_EQ is too sensitive.
+  EXPECT_NEAR(attCorr[0], 0, 1e-8);
+  EXPECT_NEAR(attCorr[1], 0, 1e-8);
+  EXPECT_NEAR(attCorr[2], 1, 1e-8);
+  EXPECT_NEAR(attCorr[3], 0, 1e-8);
+  EXPECT_NEAR(attCorr[4], 1, 1e-8);
+  EXPECT_NEAR(attCorr[5], 0, 1e-8);
+  EXPECT_NEAR(attCorr[6], -1, 1e-8);
+  EXPECT_NEAR(attCorr[7], 0, 1e-8);
+  EXPECT_NEAR(attCorr[8], 0, 1e-8);
+}

tests/UtilitiesTests.cpp

+#define _USE_MATH_DEFINES
+
+#include "Fixtures.h"
+#include "UsgsAstroPlugin.h"
+#include "Utilities.h"
+
+#include <json.hpp>
+#include <gtest/gtest.h>
+
+#include <math.h>
+
+using json = nlohmann::json;
+
+
+TEST(UtilitiesTests, calculateRotationMatrixFromEuler) {
+   double euler[3], rotationMatrix[9];
+   euler[0] = 0;
+   euler[1] = M_PI/2;
+   euler[2] = 0;
+   calculateRotationMatrixFromEuler(euler, rotationMatrix);
+
+   // EXPECT_NEARs are used here instead of EXPECT_DOUBLE_EQs because index 0 and 8 of the matrix
+   // are evaluating to 6.12...e-17. This is too small to be worried about here, but
+   // EXPECT_DOUBLE_EQ is too sensitive.
+   EXPECT_NEAR(rotationMatrix[0], 0, 1e-8);
+   EXPECT_NEAR(rotationMatrix[1], 0, 1e-8);
+   EXPECT_NEAR(rotationMatrix[2], 1, 1e-8);
+   EXPECT_NEAR(rotationMatrix[3], 0, 1e-8);
+   EXPECT_NEAR(rotationMatrix[4], 1, 1e-8);
+   EXPECT_NEAR(rotationMatrix[5], 0, 1e-8);
+   EXPECT_NEAR(rotationMatrix[6], -1, 1e-8);
+   EXPECT_NEAR(rotationMatrix[7], 0, 1e-8);
+   EXPECT_NEAR(rotationMatrix[8], 0, 1e-8);
+}
+
+TEST(UtilitiesTests, calculateRotationMatrixFromQuaternions) {
+   double q[4], rotationMatrix[9];
+   q[0] = 0;
+   q[1] = -1/sqrt(2);
+   q[2] = 0;
+   q[3] = 1/sqrt(2);
+   calculateRotationMatrixFromQuaternions(q, rotationMatrix);
+   EXPECT_DOUBLE_EQ(rotationMatrix[0], 0);
+   EXPECT_DOUBLE_EQ(rotationMatrix[1], 0);
+   EXPECT_DOUBLE_EQ(rotationMatrix[2], -1);
+   EXPECT_DOUBLE_EQ(rotationMatrix[3], 0);
+   EXPECT_DOUBLE_EQ(rotationMatrix[4], 1);
+   EXPECT_DOUBLE_EQ(rotationMatrix[5], 0);
+   EXPECT_DOUBLE_EQ(rotationMatrix[6], 1);
+   EXPECT_DOUBLE_EQ(rotationMatrix[7], 0);
+   EXPECT_DOUBLE_EQ(rotationMatrix[8], 0);
+}
+
+TEST(UtilitiesTests, computeDistortedFocalPlaneCoordinates) {
+   double iTransS[] = {0.0, 0.0, 10.0};
+   double iTransL[] = {0.0, 10.0, 0.0};
+   std::tuple<double, double> natFocalPlane;
+   computeDistortedFocalPlaneCoordinates(0.5, 4, 8, 0.5, 1, 1, 0, iTransS, iTransL, natFocalPlane);
+   EXPECT_DOUBLE_EQ(std::get<0> (natFocalPlane), 0);
+   EXPECT_DOUBLE_EQ(std::get<1> (natFocalPlane), -0.4);
+}
+
+TEST(UtilitiesTests, createCameraLookVector) {
+  double cameraLook[3];
+  createCameraLookVector(0, -0.4, 1, 50, cameraLook);
+  EXPECT_NEAR(cameraLook[0], 0, 1e-8);
+  EXPECT_NEAR(cameraLook[1], 0.007999744, 1e-8);
+  EXPECT_NEAR(cameraLook[2], -0.999968001, 1e-8);
+}