diff --git a/include/usgscsm/UsgsAstroLsSensorModel.h b/include/usgscsm/UsgsAstroLsSensorModel.h
index 3ce3880f1f380109a6b7c843b51736d03e0571d0..ffb28049bcd8885765570de62045c6a4aea6e6ab 100644
--- a/include/usgscsm/UsgsAstroLsSensorModel.h
+++ b/include/usgscsm/UsgsAstroLsSensorModel.h
@@ -976,7 +976,14 @@ class UsgsAstroLsSensorModel : public csm::RasterGM,
// Compute the determinant of a 3x3 matrix
double determinant3x3(double mat[9]) const;
-
+
+ // A function whose value will be 0 when the line a given ground
+ // point projects into is found. The obtained line will be
+ // approxPt.line + t.
+ double calcDetectorLineErr(double t, csm::ImageCoord const& approxPt,
+ const csm::EcefCoord& groundPt,
+ const std::vector<double>& adj) const;
+
csm::NoCorrelationModel _no_corr_model; // A way to report no correlation
// between images is supported
std::vector<double>
diff --git a/src/UsgsAstroLsSensorModel.cpp b/src/UsgsAstroLsSensorModel.cpp
index 3b39ee66a24aacc705d8972c30e1a4a35c65fc0d..3ff95ab0b02a1bcff9d717e2f7fd978268a94446 100644
--- a/src/UsgsAstroLsSensorModel.cpp
+++ b/src/UsgsAstroLsSensorModel.cpp
@@ -648,36 +648,6 @@ void UsgsAstroLsSensorModel::updateState() {
// Set focal length variance
m_covariance[15 * num_params + 15] = positionVariance;
-
- // Test if a pixel projected to the ground and projected back
- // returns to the original location. If not, flip the sign of
- // m_iTransL. It is very important here to pick a pixel which is not
- // an integer, as this test may succeed for integer pixels and fail
- // for non-integer ones. Also, need to pick the answer with the
- // smallest achieved precision, even when neither of them is smaller
- // than the desired precision.
-
- lineCtr = round(m_nLines / 2.0) + 0.5;
- sampCtr = round(m_nSamples / 2.0) + 0.5;
-
- double desiredPrecision = 0.001;
- ip = csm::ImageCoord(lineCtr, sampCtr);
- csm::EcefCoord xyz = imageToGround(ip, refHeight);
-
- double achievedPrecision1 = 1.0;
- // Will use m_iTransL on the next line
- ip = groundToImage(xyz, desiredPrecision, &achievedPrecision1);
-
- double achievedPrecision2 = 1.0;
- for (int it = 0; it < sizeof(m_iTransL)/sizeof(double); it++)
- m_iTransL[it] = -m_iTransL[it]; // use a flipped m_iTransL
- ip = groundToImage(xyz, desiredPrecision, &achievedPrecision2);
-
- if (std::abs(achievedPrecision1) <= std::abs(achievedPrecision2)) {
- // Flip back m_iTransL as the original was better
- for (int it = 0; it < sizeof(m_iTransL)/sizeof(double); it++)
- m_iTransL[it] = -m_iTransL[it];
- }
}
//---------------------------------------------------------------------------
@@ -707,36 +677,38 @@ csm::ImageCoord UsgsAstroLsSensorModel::groundToImage(
const csm::EcefCoord& groundPt, const std::vector<double>& adj,
double desiredPrecision, double* achievedPrecision,
csm::WarningList* warnings) const {
- // Search for the line, sample coordinate that viewed a given ground point.
- // This method first uses a linear approximation to get an initial point.
- // Then the detector offset for the line is continuously computed and
- // applied to the line until we achieve the desired precision.
-
+
csm::ImageCoord approxPt;
computeLinearApproximation(groundPt, approxPt);
- std::vector<double> detectorView;
- double detectorLine = m_nLines;
- double detectorSample = 0;
- double count = 0;
- double timei;
-
- while (abs(detectorLine) > desiredPrecision && ++count < 15) {
- timei = getImageTime(approxPt);
- detectorView = computeDetectorView(timei, groundPt, adj);
-
- // Convert to detector line
- detectorLine = m_iTransL[0] + m_iTransL[1] * detectorView[0] +
- m_iTransL[2] * detectorView[1] + m_detectorLineOrigin -
- m_startingDetectorLine;
- detectorLine /= m_detectorLineSumming;
-
- // Convert to image line
- approxPt.line += detectorLine;
+ // Search for the (line, sample) coordinate that views a given
+ // ground point. Set this up as a root-finding problem and use the
+ // secant method.
+ int count = 0;
+ double t0 = 0.0;
+ double lineErr0 = calcDetectorLineErr(t0, approxPt, groundPt, adj);
+ double t1 = lineErr0 * 0.1; // need to stay close or else there's overshooting
+ double lineErr1 = calcDetectorLineErr(t1, approxPt, groundPt, adj);
+ while (std::abs(lineErr1) > desiredPrecision && ++count < 15) {
+
+ if (lineErr1 == lineErr0)
+ break; // avoid division by 0
+
+ // Secant method update
+ // https://en.wikipedia.org/wiki/Secant_method
+ double t2 = t1 - lineErr1 * (t1 - t0) / (lineErr1 - lineErr0);
+ double lineErr2 = calcDetectorLineErr(t2, approxPt, groundPt, adj);
+
+ // Update for the next step
+ t0 = t1; lineErr0 = lineErr1;
+ t1 = t2; lineErr1 = lineErr2;
}
- timei = getImageTime(approxPt);
- detectorView = computeDetectorView(timei, groundPt, adj);
+ // Update the line with the found value
+ approxPt.line += t1;
+
+ double timei = getImageTime(approxPt);
+ std::vector<double> detectorView = computeDetectorView(timei, groundPt, adj);
// Invert distortion
double distortedFocalX, distortedFocalY;
@@ -745,10 +717,10 @@ csm::ImageCoord UsgsAstroLsSensorModel::groundToImage(
desiredPrecision);
// Convert to detector line and sample
- detectorLine = m_iTransL[0] + m_iTransL[1] * distortedFocalX +
- m_iTransL[2] * distortedFocalY;
- detectorSample = m_iTransS[0] + m_iTransS[1] * distortedFocalX +
- m_iTransS[2] * distortedFocalY;
+ double detectorLine = m_iTransL[0] + m_iTransL[1] * distortedFocalX +
+ m_iTransL[2] * distortedFocalY;
+ double detectorSample = m_iTransS[0] + m_iTransS[1] * distortedFocalX +
+ m_iTransS[2] * distortedFocalY;
// Convert to image sample line
double finalUpdate =
(detectorLine + m_detectorLineOrigin - m_startingDetectorLine) /
@@ -766,7 +738,7 @@ csm::ImageCoord UsgsAstroLsSensorModel::groundToImage(
approxPt.samp)
if (warnings && (desiredPrecision > 0.0) &&
- (abs(finalUpdate) > desiredPrecision)) {
+ (std::abs(finalUpdate) > desiredPrecision)) {
warnings->push_back(csm::Warning(
csm::Warning::PRECISION_NOT_MET, "Desired precision not achieved.",
"UsgsAstroLsSensorModel::groundToImage()"));
@@ -2650,3 +2622,26 @@ csm::EcefVector UsgsAstroLsSensorModel::getSunPosition(
}
return sunPosition;
}
+
+// A function whose value will be 0 when the line a given ground point
+// projects into is found. The obtained line will be approxPt.line +
+// t.
+double UsgsAstroLsSensorModel::calcDetectorLineErr(double t, csm::ImageCoord const& approxPt,
+ const csm::EcefCoord& groundPt,
+ const std::vector<double>& adj) const {
+
+ csm::ImageCoord currPt = approxPt;
+ currPt.line += t;
+
+ double timei = getImageTime(currPt);
+ std::vector<double> detectorView = computeDetectorView(timei, groundPt, adj);
+
+ // Convert to detector line
+ double detectorLine = m_iTransL[0] + m_iTransL[1] * detectorView[0] +
+ m_iTransL[2] * detectorView[1] + m_detectorLineOrigin -
+ m_startingDetectorLine;
+ detectorLine /= m_detectorLineSumming;
+
+ return detectorLine;
+}
+