diff --git a/include/usgscsm/UsgsAstroLsSensorModel.h b/include/usgscsm/UsgsAstroLsSensorModel.h
index eb8bf85dc0caaaefba054c37409b8fdd815a09da..007d2633506d93dfdd3835718a528880a5686511 100644
--- a/include/usgscsm/UsgsAstroLsSensorModel.h
+++ b/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(
diff --git a/include/usgscsm/Utilities.h b/include/usgscsm/Utilities.h
index 4776edf480d3b74a76cfa80a117ed595a219afb5..e781c0afe0ad79e515e9fa3c96e620598b9289e2 100644
--- a/include/usgscsm/Utilities.h
+++ b/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(
diff --git a/src/UsgsAstroLsSensorModel.cpp b/src/UsgsAstroLsSensorModel.cpp
index e65e58d7e0220804bd3150de8267aca05446641a..8f9dd96bda1f886af521e5c179de7f707caf904b 100644
--- a/src/UsgsAstroLsSensorModel.cpp
+++ b/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];
diff --git a/src/Utilities.cpp b/src/Utilities.cpp
index 840a3d61922125a09a0b8263fd51ba8222c44c3c..4998c564404312388dcc88366d7d33f526c2aeb9 100644
--- a/src/Utilities.cpp
+++ b/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,16 +48,18 @@ 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(
- const double& line,
- const double& sample,
- const double& sampleOrigin,
- const double& lineOrigin,
- const double& sampleSumming,
- const double& startingSample,
- const double& lineOffset,
- const double iTransS[],
- const double iTransL[]) {
+void computeDistortedFocalPlaneCoordinates(
+ const double& line,
+ const double& sample,
+ const double& sampleOrigin,
+ const double& lineOrigin,
+ const double& sampleSumming,
+ const double& startingSample,
+ const double& lineOffset,
+ const double iTransS[],
+ const double iTransL[],
+ std::tuple<double, double>& natFocalPlane)
+{
double detSample = (sample - 1.0) * sampleSumming + startingSample;
double m11 = iTransL[1];
double m12 = iTransL[2];
@@ -66,29 +73,27 @@ 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)
void createCameraLookVector(
- const double& undistortedFocalPlaneX,
- const double& undistortedFocalPlaneY,
- const double& zDirection,
- const double& focalLength,
- const double& focalLengthBias,
- const double& halfSwath,
- double cameraLook[]) {
- cameraLook[0] = -undistortedFocalPlaneX * zDirection;
- cameraLook[1] = -undistortedFocalPlaneY * zDirection;
- cameraLook[2] = -focalLength * (1.0 - focalLengthBias / halfSwath);
- double magnitude = sqrt(cameraLook[0] * cameraLook[0]
- + cameraLook[1] * cameraLook[1]
- + cameraLook[2] * cameraLook[2]);
- cameraLook[0] /= magnitude;
- cameraLook[1] /= magnitude;
- cameraLook[2] /= magnitude;
+ const double& undistortedFocalPlaneX,
+ const double& undistortedFocalPlaneY,
+ const double& zDirection,
+ const double& focalLength,
+ double cameraLook[])
+{
+ cameraLook[0] = -undistortedFocalPlaneX * zDirection;
+ cameraLook[1] = -undistortedFocalPlaneY * zDirection;
+ cameraLook[2] = -focalLength;
+ double magnitude = sqrt(cameraLook[0] * cameraLook[0]
+ + cameraLook[1] * cameraLook[1]
+ + cameraLook[2] * cameraLook[2]);
+ cameraLook[0] /= magnitude;
+ cameraLook[1] /= magnitude;
+ cameraLook[2] /= magnitude;
}
double metric_conversion(double val, std::string from, std::string to) {
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 3306376a11e7477ba008192ebfda67ebc3b71742..3948c05f5bbe3b5a1d4302e55e39484bfb61fa8a 100644
--- a/tests/CMakeLists.txt
+++ b/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)
diff --git a/tests/LineScanCameraTests.cpp b/tests/LineScanCameraTests.cpp
index 84a813b94231636cbac301f3d3794a8ebfea81b8..2f63651a4af685d5843125724efdd4d217bb355d 100644
--- a/tests/LineScanCameraTests.cpp
+++ b/tests/LineScanCameraTests.cpp
@@ -1,10 +1,14 @@
+#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);
+}
diff --git a/tests/UtilitiesTests.cpp b/tests/UtilitiesTests.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a3519802c2f7d25011c14dd0d21a92e058a7a37b
--- /dev/null
+++ b/tests/UtilitiesTests.cpp
@@ -0,0 +1,69 @@
+#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);
+}