diff --git a/include/usgscsm/UsgsAstroLsSensorModel.h b/include/usgscsm/UsgsAstroLsSensorModel.h
index ad688d1e27a8e06a42ad9890b02d944ca58e4007..783aa78016daf4f34bd1cb4242c62844ecc22a4d 100644
--- a/include/usgscsm/UsgsAstroLsSensorModel.h
+++ b/include/usgscsm/UsgsAstroLsSensorModel.h
@@ -916,7 +916,41 @@ private:
double* achievedPrecision = NULL,
csm::WarningList* warnings = NULL) const;
- // This method computes the imaging locus.
+ // methods pulled out of los2ecf
+
+ void computeDistortedFocalPlaneCoordinates(
+ const double& line,
+ const double& sample,
+ double& distortedLine,
+ double& distortedSample) const;
+
+ void computeUndistortedFocalPlaneCoordinates(
+ const double& distortedFocalPlaneX,
+ const double& distortedFocalPlaneY,
+ double& undistortedFocalPlaneX,
+ double& undistortedFocalPlaneY) const;
+
+ void calculateRotationMatrixFromQuaternions(
+ const double& time,
+ double cameraToBody[9]) const;
+
+ void calculateRotationMatrixFromEuler(
+ double euler[],
+ double rotationMatrix[]) const;
+
+ void createCameraLookVector(
+ const double& undistortedFocalPlaneX,
+ const double& undistortedFocalPlaneY,
+ const std::vector<double>& adj,
+ double cameraLook[]) 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(
const double& line, // CSM image convention
const double& sample, // UL pixel center == (0.5, 0.5)
@@ -927,9 +961,9 @@ private:
double& vx, // output sensor x velocity
double& vy, // output sensor y velocity
double& vz, // output sensor z cvelocity
- double& xl, // output line-of-sight x coordinate
- double& yl, // output line-of-sight y coordinate
- double& zl ) const;
+ double& bodyFixedX, // output line-of-sight x coordinate
+ double& bodyFixedY, // output line-of-sight y coordinate
+ double& bodyFixedZ ) const;
// Computes the LOS correction due to light aberration
void lightAberrationCorr(
diff --git a/src/UsgsAstroLsSensorModel.cpp b/src/UsgsAstroLsSensorModel.cpp
index 96c49c85a7835496c63a22a4d112cd26ef6c5e77..5a2494f2f3198d80dbbea6dd7489bb10c85eb5b5 100644
--- a/src/UsgsAstroLsSensorModel.cpp
+++ b/src/UsgsAstroLsSensorModel.cpp
@@ -84,7 +84,6 @@ const std::string UsgsAstroLsSensorModel::_STATE_KEYWORD[] =
"m_detectorSampleOrigin",
"m_detectorLineOrigin",
"m_detectorLineOffset",
- "m_mountingMatrix",
"m_semiMajorAxis",
"m_semiMinorAxis",
"m_referenceDateAndTime",
@@ -167,9 +166,6 @@ void UsgsAstroLsSensorModel::replaceModelState(const std::string &stateString )
m_detectorSampleOrigin = j["m_detectorSampleOrigin"];
m_detectorLineOrigin = j["m_detectorLineOrigin"];
m_detectorLineOffset = j["m_detectorLineOffset"];
- for (int i = 0; i < 9; i++) {
- m_mountingMatrix[i] = j["m_mountingMatrix"][i];
- }
m_semiMajorAxis = j["m_semiMajorAxis"];
m_semiMinorAxis = j["m_semiMinorAxis"];
m_referenceDateAndTime = j["m_referenceDateAndTime"];
@@ -279,7 +275,6 @@ std::string UsgsAstroLsSensorModel::getModelState() const {
state["m_detectorSampleOrigin"] = m_detectorSampleOrigin;
state["m_detectorLineOrigin"] = m_detectorLineOrigin;
state["m_detectorLineOffset"] = m_detectorLineOffset;
- state["m_mountingMatrix"] = std::vector<double>(m_mountingMatrix, m_mountingMatrix+9);
state["m_semiMajorAxis"] = m_semiMajorAxis;
state["m_semiMinorAxis"] = m_semiMinorAxis;
state["m_referenceDateAndTime"] = m_referenceDateAndTime;
@@ -361,15 +356,6 @@ void UsgsAstroLsSensorModel::reset()
m_detectorSampleOrigin = 2500.0; // 23
m_detectorLineOrigin = 0.0; // 24
m_detectorLineOffset = 0.0; // 25
- m_mountingMatrix[0] = 1.0; // 26
- m_mountingMatrix[1] = 0.0; // 26
- m_mountingMatrix[2] = 0.0; // 26
- m_mountingMatrix[3] = 0.0; // 26
- m_mountingMatrix[4] = 1.0; // 26
- m_mountingMatrix[5] = 0.0; // 26
- m_mountingMatrix[6] = 0.0; // 26
- m_mountingMatrix[7] = 0.0; // 26
- m_mountingMatrix[8] = 1.0; // 26
m_semiMajorAxis = 3400000.0; // 27
m_semiMinorAxis = 3350000.0; // 28
m_referenceDateAndTime = ""; // 30
@@ -1666,6 +1652,133 @@ double UsgsAstroLsSensorModel::getValue(
return m_parameterVals[index] + adjustments[index];
}
+
+//***************************************************************************
+// Functions pulled out of losToEcf
+// **************************************************************************
+
+// Compute distorted focalPlane coordinates in mm
+void UsgsAstroLsSensorModel::computeDistortedFocalPlaneCoordinates(const double& line, const double& sample, double& distortedLine, double& distortedSample) const{
+ double isisDetSample = (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 = isisDetSample - 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;
+}
+
+// Compute un-distorted image coordinates in mm / apply lens distortion correction
+void UsgsAstroLsSensorModel::computeUndistortedFocalPlaneCoordinates(const double &distortedFocalPlaneX, const double& distortedFocalPlaneY, double& undistortedFocalPlaneX, double& undistortedFocalPlaneY) const{
+ undistortedFocalPlaneX = distortedFocalPlaneX;
+ undistortedFocalPlaneY = distortedFocalPlaneY;
+ if (m_opticalDistCoef[0] != 0.0 ||
+ m_opticalDistCoef[1] != 0.0 ||
+ m_opticalDistCoef[2] != 0.0)
+ {
+ double rr = distortedFocalPlaneX * distortedFocalPlaneX
+ + distortedFocalPlaneY * distortedFocalPlaneY;
+ if (rr > 1.0E-6)
+ {
+ double dr = m_opticalDistCoef[0] + (rr * (m_opticalDistCoef[1]
+ + rr * m_opticalDistCoef[2]));
+ undistortedFocalPlaneX = distortedFocalPlaneX * (1.0 - dr);
+ undistortedFocalPlaneY = distortedFocalPlaneY * (1.0 - dr);
+ }
+ }
+};
+
+
+// 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_isisZDirection;
+ cameraLook[1] = -undistortedFocalPlaneY * m_isisZDirection;
+ 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 {
+ 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;
+}
+
+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;
+ double nTime2 = nTime * nTime;
+ euler[0] =
+ (getValue(6, adj) + getValue(9, adj)* nTime + getValue(12, adj)* nTime2) / m_flyingHeight;
+ euler[1] =
+ (getValue(7, adj) + getValue(10, adj)* nTime + getValue(13, adj)* nTime2) / m_flyingHeight;
+ euler[2] =
+ (getValue(8, adj) + getValue(11, adj)* nTime + getValue(14, adj)* nTime2) / m_halfSwath;
+
+ calculateRotationMatrixFromEuler(euler, attCorr);
+}
+
+
//***************************************************************************
// UsgsAstroLsSensorModel::losToEcf
//***************************************************************************
@@ -1679,12 +1792,13 @@ void UsgsAstroLsSensorModel::losToEcf(
double& vx, // output sensor x velocity
double& vy, // output sensor y velocity
double& vz, // output sensor z velocity
- double& xl, // output line-of-sight x coordinate
- double& yl, // output line-of-sight y coordinate
- double& zl) const // output line-of-sight z coordinate
+ double& bodyLookX, // output line-of-sight x coordinate
+ double& bodyLookY, // output line-of-sight y coordinate
+ double& bodyLookZ) const // output line-of-sight z coordinate
{
//# private_func_description
- // Computes image ray in ecf coordinate system.
+ // Computes image ray (look vector) in ecf coordinate system.
+ // Compute adjusted sensor position and velocity
double time = getImageTime(csm::ImageCoord(line, sample));
getAdjSensorPosVel(time, adj, xc, yc, zc, vx, vy, vz);
@@ -1694,144 +1808,46 @@ void UsgsAstroLsSensorModel::losToEcf(
double sampleUSGSFull = sampleCSMFull;
double fractionalLine = line - floor(line);
- // Compute distorted image coordinates in mm
-
- double isisDetSample = (sampleUSGSFull - 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 = fractionalLine + m_detectorLineOffset
- - m_detectorLineOrigin - m_iTransL[0];
- double t2 = isisDetSample - 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;
- double isisNatFocalPlaneX = p11 * t1 + p12 * t2;
- double isisNatFocalPlaneY = p21 * t1 + p22 * t2;
+ // Compute distorted image coordinates in mm (sample, line on image (pixels) -> focal plane
+ double isisNatFocalPlaneX, isisNatFocalPlaneY;
+ computeDistortedFocalPlaneCoordinates(fractionalLine, sampleUSGSFull, isisNatFocalPlaneX, isisNatFocalPlaneY);
// Remove lens distortion
- double isisFocalPlaneX = isisNatFocalPlaneX;
- double isisFocalPlaneY = isisNatFocalPlaneY;
- if (m_opticalDistCoef[0] != 0.0 ||
- m_opticalDistCoef[1] != 0.0 ||
- m_opticalDistCoef[2] != 0.0)
- {
- double rr = isisNatFocalPlaneX * isisNatFocalPlaneX
- + isisNatFocalPlaneY * isisNatFocalPlaneY;
- if (rr > 1.0E-6)
- {
- double dr = m_opticalDistCoef[0] + (rr * (m_opticalDistCoef[1]
- + rr * m_opticalDistCoef[2]));
- isisFocalPlaneX = isisNatFocalPlaneX * (1.0 - dr);
- isisFocalPlaneY = isisNatFocalPlaneY * (1.0 - dr);
- }
- }
-
- // Define imaging ray in image space
-
- double losIsis[3];
- losIsis[0] = -isisFocalPlaneX * m_isisZDirection;
- losIsis[1] = -isisFocalPlaneY * m_isisZDirection;
- losIsis[2] = -m_focal * (1.0 - getValue(15, adj) / m_halfSwath);
- double isisMag = sqrt(losIsis[0] * losIsis[0]
- + losIsis[1] * losIsis[1]
- + losIsis[2] * losIsis[2]);
- losIsis[0] /= isisMag;
- losIsis[1] /= isisMag;
- losIsis[2] /= isisMag;
-
- // Apply boresight correction
-
- double losApl[3];
- losApl[0] =
- m_mountingMatrix[0] * losIsis[0]
- + m_mountingMatrix[1] * losIsis[1]
- + m_mountingMatrix[2] * losIsis[2];
- losApl[1] =
- m_mountingMatrix[3] * losIsis[0]
- + m_mountingMatrix[4] * losIsis[1]
- + m_mountingMatrix[5] * losIsis[2];
- losApl[2] =
- m_mountingMatrix[6] * losIsis[0]
- + m_mountingMatrix[7] * losIsis[1]
- + m_mountingMatrix[8] * losIsis[2];
+ double isisFocalPlaneX, isisFocalPlaneY;
+ computeUndistortedFocalPlaneCoordinates(isisNatFocalPlaneX, isisNatFocalPlaneY, isisFocalPlaneX, isisFocalPlaneY);
+
+ // Define imaging ray (look vector) in camera space
+ double cameraLook[3];
+ createCameraLookVector(isisFocalPlaneX, isisFocalPlaneY, adj, cameraLook);
// Apply attitude correction
+ double attCorr[9];
+ calculateAttitudeCorrection(time, adj, attCorr);
- double aTime = time - m_t0Quat;
- double euler[3];
- double nTime = aTime / m_halfTime;
- double nTime2 = nTime * nTime;
- euler[0] =
- (getValue(6, adj) + getValue(9, adj)* nTime + getValue(12, adj)* nTime2) / m_flyingHeight;
- euler[1] =
- (getValue(7, adj) + getValue(10, adj)* nTime + getValue(13, adj)* nTime2) / m_flyingHeight;
- euler[2] =
- (getValue(8, adj) + getValue(11, adj)* nTime + getValue(14, adj)* nTime2) / m_halfSwath;
- 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]);
- double plFromApl[9];
- plFromApl[0] = cos_b * cos_c;
- plFromApl[1] = -cos_a * sin_c + sin_a * sin_b * cos_c;
- plFromApl[2] = sin_a * sin_c + cos_a * sin_b * cos_c;
- plFromApl[3] = cos_b * sin_c;
- plFromApl[4] = cos_a * cos_c + sin_a * sin_b * sin_c;
- plFromApl[5] = -sin_a * cos_c + cos_a * sin_b * sin_c;
- plFromApl[6] = -sin_b;
- plFromApl[7] = sin_a * cos_b;
- plFromApl[8] = cos_a * cos_b;
-
- double losPl[3];
- losPl[0] = plFromApl[0] * losApl[0] + plFromApl[1] * losApl[1]
- + plFromApl[2] * losApl[2];
- losPl[1] = plFromApl[3] * losApl[0] + plFromApl[4] * losApl[1]
- + plFromApl[5] * losApl[2];
- losPl[2] = plFromApl[6] * losApl[0] + plFromApl[7] * losApl[1]
- + plFromApl[8] * losApl[2];
-
- // Apply rotation matrix from sensor quaternions
+ double correctedCameraLook[3];
+ correctedCameraLook[0] = attCorr[0] * cameraLook[0]
+ + attCorr[1] * cameraLook[1]
+ + attCorr[2] * cameraLook[2];
+ correctedCameraLook[1] = attCorr[3] * cameraLook[0]
+ + attCorr[4] * cameraLook[1]
+ + attCorr[5] * cameraLook[2];
+ correctedCameraLook[2] = attCorr[6] * cameraLook[0]
+ + attCorr[7] * cameraLook[1]
+ + attCorr[8] * cameraLook[2];
- 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;
- double ecfFromPl[9];
- ecfFromPl[0] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
- ecfFromPl[1] = 2 * (q[0] * q[1] - q[2] * q[3]);
- ecfFromPl[2] = 2 * (q[0] * q[2] + q[1] * q[3]);
- ecfFromPl[3] = 2 * (q[0] * q[1] + q[2] * q[3]);
- ecfFromPl[4] = -q[0] * q[0] + q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
- ecfFromPl[5] = 2 * (q[1] * q[2] - q[0] * q[3]);
- ecfFromPl[6] = 2 * (q[0] * q[2] - q[1] * q[3]);
- ecfFromPl[7] = 2 * (q[1] * q[2] + q[0] * q[3]);
- ecfFromPl[8] = -q[0] * q[0] - q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
-
-
- xl = ecfFromPl[0] * losPl[0] + ecfFromPl[1] * losPl[1]
- + ecfFromPl[2] * losPl[2];
- yl = ecfFromPl[3] * losPl[0] + ecfFromPl[4] * losPl[1]
- + ecfFromPl[5] * losPl[2];
- zl = ecfFromPl[6] * losPl[0] + ecfFromPl[7] * losPl[1]
- + ecfFromPl[8] * losPl[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 cameraToBody[9];
+ calculateRotationMatrixFromQuaternions(time, cameraToBody);
+
+ bodyLookX = cameraToBody[0] * correctedCameraLook[0]
+ + cameraToBody[1] * correctedCameraLook[1]
+ + cameraToBody[2] * correctedCameraLook[2];
+ bodyLookY = cameraToBody[3] * correctedCameraLook[0]
+ + cameraToBody[4] * correctedCameraLook[1]
+ + cameraToBody[5] * correctedCameraLook[2];
+ bodyLookZ = cameraToBody[6] * correctedCameraLook[0]
+ + cameraToBody[7] * correctedCameraLook[1]
+ + cameraToBody[8] * correctedCameraLook[2];
}
@@ -2370,69 +2386,25 @@ csm::ImageCoord UsgsAstroLsSensorModel::computeViewingPixel(
double bodyLookZ = groundPoint.z - zc;
// Rotate the look vector into the camera reference frame
- 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]);
- // Divide by the negative norm for 0 through 2 to invert the quaternion
- q[0] /= -norm;
- q[1] /= -norm;
- q[2] /= -norm;
- q[3] /= norm;
double bodyToCamera[9];
- bodyToCamera[0] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
- bodyToCamera[1] = 2 * (q[0] * q[1] - q[2] * q[3]);
- bodyToCamera[2] = 2 * (q[0] * q[2] + q[1] * q[3]);
- bodyToCamera[3] = 2 * (q[0] * q[1] + q[2] * q[3]);
- bodyToCamera[4] = -q[0] * q[0] + q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
- bodyToCamera[5] = 2 * (q[1] * q[2] - q[0] * q[3]);
- bodyToCamera[6] = 2 * (q[0] * q[2] - q[1] * q[3]);
- bodyToCamera[7] = 2 * (q[1] * q[2] + q[0] * q[3]);
- bodyToCamera[8] = -q[0] * q[0] - q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
+ calculateRotationMatrixFromQuaternions(time, bodyToCamera);
+
+ // Apply transpose of matrix to rotate body->camera
double cameraLookX = bodyToCamera[0] * bodyLookX
- + bodyToCamera[1] * bodyLookY
- + bodyToCamera[2] * bodyLookZ;
- double cameraLookY = bodyToCamera[3] * bodyLookX
+ + bodyToCamera[3] * bodyLookY
+ + bodyToCamera[6] * bodyLookZ;
+ double cameraLookY = bodyToCamera[1] * bodyLookX
+ bodyToCamera[4] * bodyLookY
- + bodyToCamera[5] * bodyLookZ;
- double cameraLookZ = bodyToCamera[6] * bodyLookX
- + bodyToCamera[7] * bodyLookY
+ + bodyToCamera[7] * bodyLookZ;
+ double cameraLookZ = bodyToCamera[2] * bodyLookX
+ + bodyToCamera[5] * bodyLookY
+ bodyToCamera[8] * bodyLookZ;
// Invert the attitude correction
- double aTime = time - m_t0Quat;
- double euler[3];
- double nTime = aTime / m_halfTime;
- double nTime2 = nTime * nTime;
- euler[0] =
- (getValue(6, adj) + getValue(9, adj)* nTime + getValue(12, adj)* nTime2) / m_flyingHeight;
- euler[1] =
- (getValue(7, adj) + getValue(10, adj)* nTime + getValue(13, adj)* nTime2) / m_flyingHeight;
- euler[2] =
- (getValue(8, adj) + getValue(11, adj)* nTime + getValue(14, adj)* nTime2) / m_halfSwath;
- 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]);
double attCorr[9];
- attCorr[0] = cos_b * cos_c;
- attCorr[1] = -cos_a * sin_c + sin_a * sin_b * cos_c;
- attCorr[2] = sin_a * sin_c + cos_a * sin_b * cos_c;
- attCorr[3] = cos_b * sin_c;
- attCorr[4] = cos_a * cos_c + sin_a * sin_b * sin_c;
- attCorr[5] = -sin_a * cos_c + cos_a * sin_b * sin_c;
- attCorr[6] = -sin_b;
- attCorr[7] = sin_a * cos_b;
- attCorr[8] = cos_a * cos_b;
+ calculateAttitudeCorrection(time, adj, attCorr);
+
+ // Apply transpose of matrix to invert the attidue correction
double adjustedLookX = attCorr[0] * cameraLookX
+ attCorr[3] * cameraLookY
+ attCorr[6] * cameraLookZ;
@@ -2443,21 +2415,10 @@ csm::ImageCoord UsgsAstroLsSensorModel::computeViewingPixel(
+ attCorr[5] * cameraLookY
+ attCorr[8] * cameraLookZ;
- // Invert the boresight correction
- double correctedLookX = m_mountingMatrix[0] * adjustedLookX
- + m_mountingMatrix[3] * adjustedLookY
- + m_mountingMatrix[6] * adjustedLookZ;
- double correctedLookY = m_mountingMatrix[1] * adjustedLookX
- + m_mountingMatrix[4] * adjustedLookY
- + m_mountingMatrix[7] * adjustedLookZ;
- double correctedLookZ = m_mountingMatrix[2] * adjustedLookX
- + m_mountingMatrix[5] * adjustedLookY
- + m_mountingMatrix[8] * adjustedLookZ;
-
// Convert to focal plane coordinate
- double lookScale = m_focal / correctedLookZ;
- double focalX = correctedLookX * lookScale;
- double focalY = correctedLookY * lookScale;
+ double lookScale = m_focal / adjustedLookZ;
+ double focalX = adjustedLookX * lookScale;
+ double focalY = adjustedLookY * lookScale;
// Invert distortion
// This method works by iteratively adding distortion until the new distorted
@@ -2732,24 +2693,6 @@ std::string UsgsAstroLsSensorModel::constructStateFromIsd(const std::string imag
state["m_detectorLineOrigin"] = isd.at("detector_center").at("line");
state["m_detectorLineOffset"] = 0;
- double cos_a = cos(0);
- double sin_a = sin(0);
- double cos_b = cos(0);
- double sin_b = sin(0);
- double cos_c = cos(0);
- double sin_c = sin(0);
-
- state["m_mountingMatrix"] = json::array();
- state["m_mountingMatrix"][0] = cos_b * cos_c;
- state["m_mountingMatrix"][1] = -cos_a * sin_c + sin_a * sin_b * cos_c;
- state["m_mountingMatrix"][2] = sin_a * sin_c + cos_a * sin_b * cos_c;
- state["m_mountingMatrix"][3] = cos_b * sin_c;
- state["m_mountingMatrix"][4] = cos_a * cos_c + sin_a * sin_b * sin_c;
- state["m_mountingMatrix"][5] = -sin_a * cos_c + cos_a * sin_b * sin_c;
- state["m_mountingMatrix"][6] = -sin_b;
- state["m_mountingMatrix"][7] = sin_a * cos_b;
- state["m_mountingMatrix"][8] = cos_a * cos_b;
-
state["m_dtEphem"] = isd.at("dt_ephemeris");
state["m_t0Ephem"] = isd.at("t0_ephemeris");
state["m_dtQuat"] = isd.at("dt_quaternion");