diff --git a/CMakeLists.txt b/CMakeLists.txt
index 9e5226c71a72c614d729657a814b7e2a279486bc..afea28afb603659980b2aceca63295cb0469b0d6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -27,7 +27,8 @@ add_library(usgscsm SHARED
src/UsgsAstroPlugin.cpp
src/UsgsAstroFrameSensorModel.cpp
src/UsgsAstroLsSensorModel.cpp
- src/Distortion.cpp)
+ src/Distortion.cpp
+ src/Utilities.cpp)
set_target_properties(usgscsm PROPERTIES
VERSION ${PROJECT_VERSION}
diff --git a/include/usgscsm/UsgsAstroLsSensorModel.h b/include/usgscsm/UsgsAstroLsSensorModel.h
index 3cd5f317dc330509f0b405e92bb4b4a37f71ddd1..eb8bf85dc0caaaefba054c37409b8fdd815a09da 100644
--- a/include/usgscsm/UsgsAstroLsSensorModel.h
+++ b/include/usgscsm/UsgsAstroLsSensorModel.h
@@ -31,7 +31,6 @@
#include <SettableEllipsoid.h>
#include <CorrelationModel.h>
-
class UsgsAstroLsSensorModel : public csm::RasterGM, virtual public csm::SettableEllipsoid
{
public:
@@ -916,27 +915,13 @@ private:
double* achievedPrecision = NULL,
csm::WarningList* warnings = NULL) const;
- // methods pulled out of los2ecf and computeViewingPixel
-
- void computeDistortedFocalPlaneCoordinates(
- const double& line,
- const double& sample,
- double& distortedLine,
- double& distortedSample) const;
-
- void calculateRotationMatrixFromQuaternions(
- const double& time,
- double cameraToBody[9]) const;
-
- void calculateRotationMatrixFromEuler(
- double euler[],
- double rotationMatrix[]) const;
+ void reconstructSensorDistortion(
+ double& focalX,
+ double& focalY,
+ const double& desiredPrecision) const;
- void createCameraLookVector(
- const double& undistortedFocalPlaneX,
- const double& undistortedFocalPlaneY,
- const std::vector<double>& adj,
- double cameraLook[]) const;
+ void getQuaternions(const double& time,
+ double quaternion[4]) const;
void calculateAttitudeCorrection(
const double& time,
diff --git a/include/usgscsm/Utilities.h b/include/usgscsm/Utilities.h
new file mode 100644
index 0000000000000000000000000000000000000000..a7b1b7606506fec497df472358e9705ee187b6e8
--- /dev/null
+++ b/include/usgscsm/Utilities.h
@@ -0,0 +1,48 @@
+#ifndef Utilities_h
+#define Utilities_h
+
+#include <vector>
+#include <math.h>
+#include <tuple>
+
+// methods pulled out of los2ecf and computeViewingPixel
+
+// 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(
+ 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 calculateRotationMatrixFromQuaternions(
+ double quaternions[4],
+ double cameraToBody[9]);
+
+void calculateRotationMatrixFromEuler(
+ double euler[],
+ double rotationMatrix[]);
+
+void createCameraLookVector(
+ const double& undistortedFocalPlaneX,
+ const double& undistortedFocalPlaneY,
+ const double& zDirection,
+ const double& focalLength,
+ const double& focalLengthBias,
+ const double& halfSwath,
+ double cameraLook[]);
+
+//void calculateAttitudeCorrection(
+// const double& time,
+//
+// double attCorr[9]);
+//
+
+#endif
+
diff --git a/src/UsgsAstroFrameSensorModel.cpp b/src/UsgsAstroFrameSensorModel.cpp
index 03f45a7357a996ad636e0006290026a881579f04..ca233c8be1d386da66118c5ae98fcf00ce25f248 100644
--- a/src/UsgsAstroFrameSensorModel.cpp
+++ b/src/UsgsAstroFrameSensorModel.cpp
@@ -126,7 +126,7 @@ csm::ImageCoord UsgsAstroFrameSensorModel::groundToImage(
// Camera rotation matrix
double m[3][3];
- calcRotationMatrix(m,adjustments);
+ calcRotationMatrix(m, adjustments);
// Sensor position
double undistortedx, undistortedy, denom;
diff --git a/src/UsgsAstroLsSensorModel.cpp b/src/UsgsAstroLsSensorModel.cpp
index ad040c96fc7b32dc7efe4b07b58e96d48a0ef534..616069537749fadca3a09fe877ba4b0048536a70 100644
--- a/src/UsgsAstroLsSensorModel.cpp
+++ b/src/UsgsAstroLsSensorModel.cpp
@@ -19,6 +19,7 @@
#define USGS_SENSOR_LIBRARY
#include "UsgsAstroLsSensorModel.h"
+#include "Utilities.h"
#include "Distortion.h"
#include <algorithm>
@@ -1658,92 +1659,18 @@ double UsgsAstroLsSensorModel::getValue(
// Functions pulled out of losToEcf and computeViewingPixel
// **************************************************************************
-// Compute distorted focalPlane coordinates in mm
-void UsgsAstroLsSensorModel::computeDistortedFocalPlaneCoordinates(const double& line, const double& sample, double& distortedLine, double& distortedSample) const{
- double detSample = (sample - 1.0)
- * m_detectorSampleSumming + m_startingSample;
- double m11 = m_iTransL[1];
- double m12 = m_iTransL[2];
- double m21 = m_iTransS[1];
- double m22 = m_iTransS[2];
- double t1 = line + m_detectorLineOffset
- - m_detectorLineOrigin - m_iTransL[0];
- double t2 = detSample - m_detectorSampleOrigin - m_iTransS[0];
- double determinant = m11 * m22 - m12 * m21;
- double p11 = m11 / determinant;
- double p12 = -m12 / determinant;
- double p21 = -m21 / determinant;
- double p22 = m22 / determinant;
- distortedLine = p11 * t1 + p12 * t2;
- distortedSample = p21 * t1 + p22 * t2;
-}
-
-
-// Define imaging ray in image space (In other words, create a look vector in camera space)
-void UsgsAstroLsSensorModel::createCameraLookVector(const double& undistortedFocalPlaneX, const double& undistortedFocalPlaneY, const std::vector<double>& adj, double cameraLook[]) const{
- cameraLook[0] = -undistortedFocalPlaneX * m_zDirection;
- cameraLook[1] = -undistortedFocalPlaneY * m_zDirection;
- cameraLook[2] = -m_focal * (1.0 - getValue(15, adj) / m_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;
-};
-
-
-// Given a time and a flag to indicate whether the a->b or b->a rotation should be calculated
-// uses the quaternions in the m_quaternions member to calclate a rotation matrix.
-void UsgsAstroLsSensorModel::calculateRotationMatrixFromQuaternions(const double& time, double rotationMatrix[9]) const {
+void UsgsAstroLsSensorModel::getQuaternions(const double& time, double q[4]) const{
int nOrder = 8;
if (m_platformFlag == 0)
nOrder = 4;
int nOrderQuat = nOrder;
if (m_numQuaternions < 6 && nOrder == 8)
nOrderQuat = 4;
- double q[4];
lagrangeInterp(
- m_numQuaternions, &m_quaternions[0], m_t0Quat, m_dtQuat,
- time, 4, nOrderQuat, q);
- 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;
- q[2] /= norm;
- q[3] /= norm;
-
- rotationMatrix[0] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
- rotationMatrix[1] = 2 * (q[0] * q[1] - q[2] * q[3]);
- rotationMatrix[2] = 2 * (q[0] * q[2] + q[1] * q[3]);
- rotationMatrix[3] = 2 * (q[0] * q[1] + q[2] * q[3]);
- rotationMatrix[4] = -q[0] * q[0] + q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
- rotationMatrix[5] = 2 * (q[1] * q[2] - q[0] * q[3]);
- rotationMatrix[6] = 2 * (q[0] * q[2] - q[1] * q[3]);
- rotationMatrix[7] = 2 * (q[1] * q[2] + q[0] * q[3]);
- rotationMatrix[8] = -q[0] * q[0] - q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
-};
-
-// Calculates a rotation matrix from Euler angles
-void UsgsAstroLsSensorModel::calculateRotationMatrixFromEuler(double euler[],
- double rotationMatrix[]) const {
- double cos_a = cos(euler[0]);
- double sin_a = sin(euler[0]);
- double cos_b = cos(euler[1]);
- double sin_b = sin(euler[1]);
- double cos_c = cos(euler[2]);
- double sin_c = sin(euler[2]);
-
- rotationMatrix[0] = cos_b * cos_c;
- rotationMatrix[1] = -cos_a * sin_c + sin_a * sin_b * cos_c;
- rotationMatrix[2] = sin_a * sin_c + cos_a * sin_b * cos_c;
- rotationMatrix[3] = cos_b * sin_c;
- rotationMatrix[4] = cos_a * cos_c + sin_a * sin_b * sin_c;
- rotationMatrix[5] = -sin_a * cos_c + cos_a * sin_b * sin_c;
- rotationMatrix[6] = -sin_b;
- rotationMatrix[7] = sin_a * cos_b;
- rotationMatrix[8] = cos_a * cos_b;
+ m_numQuaternions, &m_quaternions[0], m_t0Quat, m_dtQuat, time, 4, nOrderQuat, q);
}
+
void UsgsAstroLsSensorModel::calculateAttitudeCorrection(const double& time, const std::vector<double>& adj, double attCorr[9]) const {
double aTime = time - m_t0Quat;
double euler[3];
@@ -1790,16 +1717,16 @@ void UsgsAstroLsSensorModel::losToEcf(
double fractionalLine = line - floor(line);
// Compute distorted image coordinates in mm (sample, line on image (pixels) -> focal plane
- double natFocalPlaneX, natFocalPlaneY;
- computeDistortedFocalPlaneCoordinates(fractionalLine, sampleUSGSFull, natFocalPlaneX, natFocalPlaneY);
+ std::tuple<double, double> natFocalPlane;
+ natFocalPlane = computeDistortedFocalPlaneCoordinates(fractionalLine, sampleUSGSFull, m_detectorSampleOrigin, m_detectorLineOrigin, m_detectorSampleSumming, m_startingSample, m_detectorLineOffset, m_iTransS, m_iTransL);
// Remove lens distortion
std::tuple<double, double> undistortedPoint;
- undistortedPoint = removeDistortion(natFocalPlaneX, natFocalPlaneY, m_opticalDistCoef);
+ undistortedPoint = removeDistortion(std::get<0>(natFocalPlane), std::get<1>(natFocalPlane), m_opticalDistCoef);
// Define imaging ray (look vector) in camera space
double cameraLook[3];
- createCameraLookVector(std::get<0>(undistortedPoint), std::get<1>(undistortedPoint), adj, cameraLook);
+ createCameraLookVector(std::get<0>(undistortedPoint), std::get<1>(undistortedPoint), m_zDirection, m_focal, getValue(15, adj), m_halfSwath, cameraLook);
// Apply attitude correction
double attCorr[9];
@@ -1817,8 +1744,10 @@ void UsgsAstroLsSensorModel::losToEcf(
+ attCorr[8] * cameraLook[2];
// Rotate the look vector into the body fixed frame from the camera reference frame by applying the rotation matrix from the sensor quaternions
+ double quaternions[4];
+ getQuaternions(time, quaternions);
double cameraToBody[9];
- calculateRotationMatrixFromQuaternions(time, cameraToBody);
+ calculateRotationMatrixFromQuaternions(quaternions, cameraToBody);
bodyLookX = cameraToBody[0] * correctedCameraLook[0]
+ cameraToBody[1] * correctedCameraLook[1]
@@ -2354,8 +2283,10 @@ csm::ImageCoord UsgsAstroLsSensorModel::computeViewingPixel(
double bodyLookZ = groundPoint.z - zc;
// Rotate the look vector into the camera reference frame
+ double quaternions[4];
+ getQuaternions(time, quaternions);
double bodyToCamera[9];
- calculateRotationMatrixFromQuaternions(time, bodyToCamera);
+ calculateRotationMatrixFromQuaternions(quaternions, bodyToCamera);
// Apply transpose of matrix to rotate body->camera
double cameraLookX = bodyToCamera[0] * bodyLookX
diff --git a/src/Utilities.cpp b/src/Utilities.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e9c82f6cdf766fc760136fea19f3ed9474fa342a
--- /dev/null
+++ b/src/Utilities.cpp
@@ -0,0 +1,90 @@
+#include "Utilities.h"
+
+// Calculates a rotation matrix from Euler angles
+void calculateRotationMatrixFromEuler(double euler[],
+ double rotationMatrix[]) {
+ double cos_a = cos(euler[0]);
+ double sin_a = sin(euler[0]);
+ double cos_b = cos(euler[1]);
+ double sin_b = sin(euler[1]);
+ double cos_c = cos(euler[2]);
+ double sin_c = sin(euler[2]);
+
+ rotationMatrix[0] = cos_b * cos_c;
+ rotationMatrix[1] = -cos_a * sin_c + sin_a * sin_b * cos_c;
+ rotationMatrix[2] = sin_a * sin_c + cos_a * sin_b * cos_c;
+ rotationMatrix[3] = cos_b * sin_c;
+ rotationMatrix[4] = cos_a * cos_c + sin_a * sin_b * sin_c;
+ rotationMatrix[5] = -sin_a * cos_c + cos_a * sin_b * sin_c;
+ rotationMatrix[6] = -sin_b;
+ rotationMatrix[7] = sin_a * cos_b;
+ rotationMatrix[8] = cos_a * cos_b;
+}
+
+
+// uses a quaternion to calclate a rotation matrix.
+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;
+ q[2] /= norm;
+ q[3] /= norm;
+
+ rotationMatrix[0] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
+ rotationMatrix[1] = 2 * (q[0] * q[1] - q[2] * q[3]);
+ rotationMatrix[2] = 2 * (q[0] * q[2] + q[1] * q[3]);
+ rotationMatrix[3] = 2 * (q[0] * q[1] + q[2] * q[3]);
+ rotationMatrix[4] = -q[0] * q[0] + q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
+ rotationMatrix[5] = 2 * (q[1] * q[2] - q[0] * q[3]);
+ rotationMatrix[6] = 2 * (q[0] * q[2] - q[1] * q[3]);
+ rotationMatrix[7] = 2 * (q[1] * q[2] + q[0] * q[3]);
+ 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[]) {
+ double detSample = (sample - 1.0) * sampleSumming + startingSample;
+ double m11 = iTransL[1];
+ double m12 = iTransL[2];
+ double m21 = iTransS[1];
+ double m22 = iTransS[2];
+ double t1 = line + lineOffset - lineOrigin - iTransL[0];
+ double t2 = detSample - sampleOrigin - iTransS[0];
+ double determinant = m11 * m22 - m12 * m21;
+ double p11 = m11 / determinant;
+ double p12 = -m12 / determinant;
+ 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);
+};
+
+// 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;
+}