New cahvor distortion model

include/usgscsm/Distortion.h

@@ -6,7 +6,7 @@
 #include <tuple>
 #include <vector>
 
-enum DistortionType { RADIAL, TRANSVERSE, KAGUYALISM, DAWNFC, LROLROCNAC };
+enum DistortionType { RADIAL, TRANSVERSE, KAGUYALISM, DAWNFC, LROLROCNAC, CAHVOR };
 
 // Transverse Distortion
 void distortionJacobian(double x, double y, double *jacobian,

src/Distortion.cpp

@@ -297,6 +297,28 @@ void removeDistortion(double dx, double dy, double &ux, double &uy,
 
       return;
     } break;
+
+    // Compute undistorted focal plane coordinate given a distorted
+    // coordinate set and the distortion coefficients along with an
+    // x, and y offset as the fourth and fifth element
+    case CAHVOR:
+    {
+      double shiftedDx = dx - opticalDistCoeffs[3];
+      double shiftedDy = dy - opticalDistCoeffs[4];
+      double rr = shiftedDx * shiftedDx + shiftedDy * shiftedDy;
+
+      if (rr > tolerance)
+      {
+        double dr = opticalDistCoeffs[0] +
+                    (rr * (opticalDistCoeffs[1] + rr * opticalDistCoeffs[2]));
+
+        ux = shiftedDx * (1.0 - dr);
+        uy = shiftedDy * (1.0 - dr);
+        ux += opticalDistCoeffs[3];
+        uy += opticalDistCoeffs[4];
+      }
+    }
+    break;
   }
 }
 
@@ -519,5 +541,64 @@ void applyDistortion(double ux, double uy, double &dx, double &dy,
 
       return;
     } break;
+
+    // Compute undistorted focal plane coordinate given a distorted
+    // focal plane coordinate. This case works by iteratively adding distortion
+    // until the new distorted point, r, undistorts to within a tolerance of the
+    // original point, rp. Also applies an initial offset with an
+    // x, and y offset as the fourth and fifth element
+    // This is untested manually
+    case CAHVOR:
+    {
+      double shiftedUx = ux - opticalDistCoeffs[3];
+      double shiftedUy = uy - opticalDistCoeffs[4];
+      double rp2 = (ux * ux) + (uy * uy);
+
+      if (rp2 > tolerance)
+      {
+        double rp = sqrt(rp2);
+        // Compute first fractional distortion using rp
+        double drOverR =
+            opticalDistCoeffs[0] +
+            (rp2 * (opticalDistCoeffs[1] + (rp2 * opticalDistCoeffs[2])));
+
+        // Compute first distorted point estimate, r
+        double r = rp + (drOverR * rp);
+        double r_prev, r2_prev;
+        int iteration = 0;
+
+        do
+        {
+          // Don't get in an end-less loop.  This algorithm should
+          // converge quickly.  If not then we are probably way outside
+          // of the focal plane.  Just set the distorted position to the
+          // undistorted position. Also, make sure the focal plane is less
+          // than 1km, it is unreasonable for it to grow larger than that.
+          if (iteration >= 20 || r > 1E9)
+          {
+            drOverR = 0.0;
+            break;
+          }
+
+          r_prev = r;
+          r2_prev = r * r;
+
+          // Compute new fractional distortion:
+          drOverR = opticalDistCoeffs[0] +
+                    (r2_prev *
+                     (opticalDistCoeffs[1] + (r2_prev * opticalDistCoeffs[2])));
+
+          // Compute new estimate of r
+          r = rp + (drOverR * r_prev);
+          iteration++;
+        } while (fabs(r - r_prev) > desiredPrecision);
+
+        dx = ux / (1.0 - drOverR);
+        dy = uy / (1.0 - drOverR);
+        dx += opticalDistCoeffs[3];
+        dy += opticalDistCoeffs[4];
+      }
+    }
+    break;
   }
 }

src/UsgsAstroFrameSensorModel.cpp

@@ -321,7 +321,7 @@ csm::EcefLocus UsgsAstroFrameSensorModel::imageToProximateImagingLocus(
     csm::WarningList *warnings) const {
   MESSAGE_LOG(
       spdlog::level::info,
-      "Computing imageToProximateImagingLocus(No ground) for point {}, {}, {}, "
+      "Computing imageToProximateImagingLocus(No ground) for point {}, {}, "
       "with desired precision {}",
       imagePt.line, imagePt.samp, desiredPrecision);
 
@@ -362,7 +362,7 @@ csm::EcefLocus UsgsAstroFrameSensorModel::imageToRemoteImagingLocus(
     double *achievedPrecision, csm::WarningList *warnings) const {
   MESSAGE_LOG(
       spdlog::level::info,
-      "Computing imageToProximateImagingLocus for {}, {}, {}, with desired "
+      "Computing imageToRemoteImagingLocus for {}, {} with desired "
       "precision {}",
       imagePt.line, imagePt.samp, desiredPrecision);
   // Find the line,sample on the focal plane (mm)
@@ -373,15 +373,30 @@ csm::EcefLocus UsgsAstroFrameSensorModel::imageToRemoteImagingLocus(
       m_startingDetectorLine, &m_iTransS[0], &m_iTransL[0], focalPlaneX,
       focalPlaneY);
 
+  MESSAGE_LOG(
+      spdlog::level::trace,
+      "Found focalPlaneX: {}, and focalPlaneY: {}",
+      focalPlaneX, focalPlaneY);
+      
   // Distort
   double undistortedFocalPlaneX, undistortedFocalPlaneY;
   removeDistortion(focalPlaneX, focalPlaneY, undistortedFocalPlaneX,
                    undistortedFocalPlaneY, m_opticalDistCoeffs,
                    m_distortionType);
 
+  MESSAGE_LOG(
+      spdlog::level::trace,
+      "Found undistortedFocalPlaneX: {}, and undistortedFocalPlaneY: {}",
+      undistortedFocalPlaneX, undistortedFocalPlaneY);
+
   // Get rotation matrix and transform to a body-fixed frame
   double m[3][3];
   calcRotationMatrix(m);
+  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]);
   std::vector<double> lookC{undistortedFocalPlaneX, undistortedFocalPlaneY,
                             m_focalLength};
   std::vector<double> lookB{
@@ -1250,6 +1265,7 @@ std::string UsgsAstroFrameSensorModel::constructStateFromIsd(
   // We don't pass the pixel to focal plane transformation so invert the
   // focal plane to pixel transformation
   try {
+    // "focal2pixel_lines":[0,136.4988453771169,0],"focal2pixel_samples":[136.4988453771169,0,0]
     double determinant = state["m_iTransL"][1].get<double>() *
                              state["m_iTransS"][2].get<double>() -
                          state["m_iTransL"][2].get<double>() *

src/Utilities.cpp

@@ -1053,6 +1053,8 @@ DistortionType getDistortionModel(int aleDistortionModel,
       return DistortionType::DAWNFC;
     } else if (aleDistortionType == ale::DistortionType::LROLROCNAC) {
       return DistortionType::LROLROCNAC;
+    }else if (aleDistortionType == ale::DistortionType::CAHVOR) {
+      return DistortionType::CAHVOR;
     }
   } catch (...) {
     if (list) {
@@ -1193,6 +1195,24 @@ std::vector<double> getDistortionCoeffs(json isd, csm::WarningList *list) {
         coefficients = std::vector<double>(1, 0.0);
       }
     } break;
+    case DistortionType::CAHVOR: {
+      try {
+        coefficients = isd.at("optical_distortion")
+                           .at("cahvor")
+                           .at("coefficients")
+                           .get<std::vector<double>>();
+
+        return coefficients;
+      } catch (...) {
+        if (list) {
+          list->push_back(csm::Warning(
+              csm::Warning::DATA_NOT_AVAILABLE,
+              "Could not parse the radial distortion model coefficients.",
+              "Utilities::getDistortion()"));
+        }
+        coefficients = std::vector<double>(6, 0.0);
+      }
+    } break;
   }
   if (list) {
     list->push_back(