Merge pull request #436 from acpaquette/kaguya_fix

Line Scan groundToImage Fix

src/UsgsAstroFrameSensorModel.cpp

@@ -153,6 +153,11 @@ csm::ImageCoord UsgsAstroFrameSensorModel::groundToImage(
   // Camera rotation matrix
   double m[3][3];
   calcRotationMatrix(m, adjustments);
+  MESSAGE_LOG(
+      spdlog::level::trace,
+      "Calculated rotation matrix [{}, {}, {}], [{}, {}, {}], [{}, {}, {}]",
+      m[0][0], m[0][1], m[0][2], m[1][0], m[1][1], m[1][2], m[2][0], m[2][1],
+      m[2][2]);
 
   // Sensor position
   double undistortedx, undistortedy, denom;
@@ -160,12 +165,21 @@ csm::ImageCoord UsgsAstroFrameSensorModel::groundToImage(
   undistortedx = (f * (m[0][0] * xo + m[1][0] * yo + m[2][0] * zo) / denom);
   undistortedy = (f * (m[0][1] * xo + m[1][1] * yo + m[2][1] * zo) / denom);
 
+  MESSAGE_LOG(
+      spdlog::level::trace,
+      "Found undistortedX: {}, and undistortedY: {}",
+      undistortedx, undistortedy);
   // Apply the distortion to the line/sample location and then convert back to
   // line/sample
   double distortedX, distortedY;
   applyDistortion(undistortedx, undistortedy, distortedX, distortedY,
                   m_opticalDistCoeffs, m_distortionType);
 
+  MESSAGE_LOG(
+      spdlog::level::trace,
+      "Found distortedX: {}, and distortedY: {}",
+      distortedX, distortedY);
+
   // Convert distorted mm into line/sample
   double sample, line;
   computePixel(distortedX, distortedY, m_ccdCenter[1], m_ccdCenter[0],

src/UsgsAstroLsSensorModel.cpp

@@ -721,6 +721,10 @@ csm::ImageCoord UsgsAstroLsSensorModel::groundToImage(
 
   csm::ImageCoord approxPt;
   computeProjectiveApproximation(groundPt, approxPt);
+  MESSAGE_LOG(
+      spdlog::level::trace,
+      "Computed Proj Approximation: {}, {}",
+      approxPt.line, approxPt.samp);
 
   // Search for the (line, sample) coordinate that views a given
   // ground point. Set this up as a root-finding problem and use the
@@ -730,6 +734,10 @@ csm::ImageCoord UsgsAstroLsSensorModel::groundToImage(
   double lineErr0 = calcDetectorLineErr(t0, approxPt, groundPt, adj);
   double t1 = 0.1;
   double lineErr1 = calcDetectorLineErr(t1, approxPt, groundPt, adj);
+  MESSAGE_LOG(
+      spdlog::level::trace,
+      "Initial Line Error: {}, {}",
+      lineErr0, lineErr1);
   while (std::abs(lineErr1) > desiredPrecision && ++count < 15) {
 
     if (lineErr1 == lineErr0)
@@ -743,25 +751,45 @@ csm::ImageCoord UsgsAstroLsSensorModel::groundToImage(
     // Update for the next step
     t0 = t1; lineErr0 = lineErr1;
     t1 = t2; lineErr1 = lineErr2;
+    MESSAGE_LOG(
+        spdlog::level::trace,
+        "{} Line Error and (t0, t1): {}, {}, {}, {}",
+        count, lineErr0, lineErr1, t0, t1);
   }
 
   // Update the line with the found value
   approxPt.line += t1;
+  MESSAGE_LOG(
+      spdlog::level::trace,
+      "After line Approximation: {}, {}",
+      approxPt.line, approxPt.samp);
 
   double timei = getImageTime(approxPt);
   std::vector<double> detectorView = computeDetectorView(timei, groundPt, adj);
+  MESSAGE_LOG(
+      spdlog::level::trace,
+      "Detector View: {}, and undistortedY: {}",
+      detectorView[0], detectorView[1]);
 
   // Invert distortion
   double distortedFocalX, distortedFocalY;
   applyDistortion(detectorView[0], detectorView[1], distortedFocalX,
                   distortedFocalY, m_opticalDistCoeffs, m_distortionType,
                   desiredPrecision);
+  MESSAGE_LOG(
+      spdlog::level::trace,
+      "Distorted X, Y: {}, and undistortedY: {}",
+      distortedFocalX, distortedFocalY);
 
   // Convert to detector line and sample
   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;
+  MESSAGE_LOG(
+      spdlog::level::trace,
+      "Detector Line, Sample: {}, and undistortedY: {}",
+      detectorLine, detectorSample);
   // Convert to image sample line
   double finalUpdate =
       (detectorLine + m_detectorLineOrigin - m_startingDetectorLine) /
@@ -2263,6 +2291,11 @@ void UsgsAstroLsSensorModel::computeProjectiveApproximation(const csm::EcefCoord
     ip.line = (u[0] + u[1] * x + u[2] * y + u[3]  * z) / line_den;
     ip.samp = (u[7] + u[8] * x + u[9] * y + u[10] * z) / samp_den;
 
+    MESSAGE_LOG(
+        spdlog::level::debug,
+        "Projective approximation before bounding ({}, {})",
+        ip.line, ip.samp);
+
     // Since this is valid only over the image,
     // don't let the result go beyond the image border.
     double numRows = m_nLines;
@@ -2305,8 +2338,8 @@ void UsgsAstroLsSensorModel::createProjectiveApproximation() {
         spdlog::level::warn,
         "createProjectiveApproximation: computeElevation of "
         "reference point {} {} {} with desired precision "
-        "{} returned nan height; nonprojective",
-        refPt.x, refPt.y, refPt.z, desired_precision);
+        "{} and major/minor radii {}, {} returned nan height; nonprojective",
+        refPt.x, refPt.y, refPt.z, desired_precision, m_majorAxis, m_minorAxis);
     m_useApproxInitTrans = false;
     return;
   }
@@ -2314,8 +2347,8 @@ void UsgsAstroLsSensorModel::createProjectiveApproximation() {
       spdlog::level::trace,
       "createProjectiveApproximation: computeElevation of "
       "reference point {} {} {} with desired precision "
-      "{} returned {} height",
-      refPt.x, refPt.y, refPt.z, desired_precision, height);
+      "{} and major/minor radii {}, {} returned {} height",
+      refPt.x, refPt.y, refPt.z, desired_precision, m_majorAxis, m_minorAxis, height);
 
   double numImageRows = m_nLines;
   double numImageCols = m_nSamples;
@@ -2764,9 +2797,14 @@ double UsgsAstroLsSensorModel::calcDetectorLineErr(double t, csm::ImageCoord con
   double timei = getImageTime(currPt);
   std::vector<double> detectorView = computeDetectorView(timei, groundPt, adj);
 
+  // Invert distortion
+  double distortedFocalX, distortedFocalY;
+  applyDistortion(detectorView[0], detectorView[1], distortedFocalX,
+                  distortedFocalY, m_opticalDistCoeffs, m_distortionType);
+
   // Convert to detector line
-  double detectorLine = m_iTransL[0] + m_iTransL[1] * detectorView[0] +
-    m_iTransL[2] * detectorView[1] + m_detectorLineOrigin -
+  double detectorLine = m_iTransL[0] + m_iTransL[1] * distortedFocalX +
+                        m_iTransL[2] * distortedFocalY + m_detectorLineOrigin -
                         m_startingDetectorLine;
   detectorLine /= m_detectorLineSumming;