From 24b2382de86c47993b19f549c2a5e21bfa2f61bd Mon Sep 17 00:00:00 2001
From: acpaquette <acp263@nau.edu>
Date: Fri, 15 Feb 2019 13:10:06 -0700
Subject: [PATCH] Extracts distortion models from the Line Scanner and Framer
 cameras into its own function set (#172)

* Moved transverse distortion from line scanner into new file

* Updated fixtures for distortion tests

* Forgot a semicolon

* Extracted radial destortion, and built associated tests

* Removed invertDistortion commented code from Linescanner

* Reconstructed distortion functions return results

* Incorperated invertDistortion function

* Corrected spelling, and removed commented out functions

* Renamed all dpoint variables to distortionPoint

* Removed computeUndistortedFocalPlaneCoordinates function

* Renamed distortionPoint to undistortedPoint and distortedPoint where applicable

* Fixed typo

* Removed doc string parameters

* Further extracted transverse destortion out of the Framer
---
 CMakeLists.txt                              |   5 +-
 include/usgscsm/Distortion.h                |  25 ++
 include/usgscsm/UsgsAstroFrameSensorModel.h |   9 +-
 include/usgscsm/UsgsAstroLsSensorModel.h    |  11 -
 src/Distortion.cpp                          | 254 ++++++++++++++++++++
 src/UsgsAstroFrameSensorModel.cpp           | 220 ++---------------
 src/UsgsAstroLsSensorModel.cpp              |  93 +------
 tests/CMakeLists.txt                        |   3 +-
 tests/DistortionTests.cpp                   | 154 ++++++++++++
 tests/Fixtures.h                            |   2 +
 tests/FrameCameraTests.cpp                  | 159 +-----------
 11 files changed, 474 insertions(+), 461 deletions(-)
 create mode 100644 include/usgscsm/Distortion.h
 create mode 100644 src/Distortion.cpp
 create mode 100644 tests/DistortionTests.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 76ab4a6..9e5226c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -18,7 +18,7 @@ else()
 			    PATH_SUFFIXES "csm"
 			    PATHS $ENV{CONDA_PREFIX}/include/)
   find_library(CSM_LIBRARY csmapi PATHS $ENV{CONDA_PREFIX}/lib)
-  
+
   message("--Found CSM Library: ${CSM_LIBRARY}")
   message("--Found CSM Include Directory: ${CSM_INCLUDE_DIR}")
 endif(BUILD_CSM)
@@ -26,7 +26,8 @@ endif(BUILD_CSM)
 add_library(usgscsm SHARED
             src/UsgsAstroPlugin.cpp
             src/UsgsAstroFrameSensorModel.cpp
-            src/UsgsAstroLsSensorModel.cpp)
+            src/UsgsAstroLsSensorModel.cpp
+            src/Distortion.cpp)
 
 set_target_properties(usgscsm PROPERTIES
     VERSION ${PROJECT_VERSION}
diff --git a/include/usgscsm/Distortion.h b/include/usgscsm/Distortion.h
new file mode 100644
index 0000000..6ae0d04
--- /dev/null
+++ b/include/usgscsm/Distortion.h
@@ -0,0 +1,25 @@
+#ifndef Distortion_h
+#define Distortion_h
+
+#include <vector>
+#include <math.h>
+#include <tuple>
+
+// Transverse Distortion
+std::tuple<double, double> removeDistortion(double dx, double dy,
+                        const std::vector<double> &odtX, const std::vector<double> &odtY);
+
+std::vector<std::vector<double>> distortionJacobian(double x, double y,
+                        const std::vector<double> &odtX, const std::vector<double> &odtY);
+
+std::tuple<double, double> distortionFunction(double ux, double uy,
+                        const std::vector<double> &odtX, const std::vector<double> &odtY);
+
+// Radial Distortion
+std::tuple<double, double> removeDistortion(double inFocalPlaneX, double inFocalPlaneY,
+                        const double opticalDistCoef[3], double tolerance = 1.0E-6);
+
+std::tuple<double, double> invertDistortion(double inFocalPlaneX, double inFocalPlaneY,
+                        const double opticalDistCoef[3], double desiredPrecision, double tolerance = 1.0E-6);
+
+#endif
diff --git a/include/usgscsm/UsgsAstroFrameSensorModel.h b/include/usgscsm/UsgsAstroFrameSensorModel.h
index d9a62d7..3a823e7 100644
--- a/include/usgscsm/UsgsAstroFrameSensorModel.h
+++ b/include/usgscsm/UsgsAstroFrameSensorModel.h
@@ -8,6 +8,7 @@
 #include <gtest/gtest.h>
 #include "RasterGM.h"
 #include "CorrelationModel.h"
+#include "Distortion.h"
 
 #include <json.hpp>
 using json = nlohmann::json;
@@ -320,12 +321,6 @@ protected:
     FRIEND_TEST(FrameSensorModel, setFocalPlane_AlternatingOnes);
     FRIEND_TEST(FrameSensorModel, distortMe_AlternatingOnes);
 
-    virtual bool setFocalPlane(double dx,double dy,double &undistortedX,double &undistortedY) const;
-    virtual void distortionFunction(double ux, double uy, double &dx, double &dy) const;
-    virtual void distortionJacobian(double x, double y, double &Jxx,
-                                    double &Jxy, double &Jyx, double &Jyy) const;
-
-
 
   private:
     // Input parameters
@@ -373,7 +368,7 @@ protected:
     int m_nSamples;
     int m_nParameters;
 
-    csm::EcefCoord m_referencePointXyz; 
+    csm::EcefCoord m_referencePointXyz;
 
     json _state;
     static const int         _NUM_STATE_KEYWORDS;
diff --git a/include/usgscsm/UsgsAstroLsSensorModel.h b/include/usgscsm/UsgsAstroLsSensorModel.h
index 6ec28f1..3cd5f31 100644
--- a/include/usgscsm/UsgsAstroLsSensorModel.h
+++ b/include/usgscsm/UsgsAstroLsSensorModel.h
@@ -924,12 +924,6 @@ private:
        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;
@@ -949,11 +943,6 @@ private:
        const std::vector<double>& adj,
        double attCorr[9]) const;
 
-   void reconstructSensorDistortion(
-       double& focalX,
-       double& focalY,
-       const double& desiredPrecision) const;
-
 // This method computes the imaging locus.
 // imaging locus : set of ground points associated with an image pixel.
    void losToEcf(
diff --git a/src/Distortion.cpp b/src/Distortion.cpp
new file mode 100644
index 0000000..103aa5c
--- /dev/null
+++ b/src/Distortion.cpp
@@ -0,0 +1,254 @@
+#include "Distortion.h"
+
+/**
+ * @brief Compute undistorted focal plane x/y.
+ *
+ * Computes undistorted focal plane (x,y) coordinates given a distorted focal plane (x,y)
+ * coordinate. The undistorted coordinates are solved for using the Newton-Raphson
+ * method for root-finding if the distortionFunction method is invoked.
+ *
+ * @param dx distorted focal plane x in millimeters
+ * @param dy distorted focal plane y in millimeters
+ * @param undistortedX The undistorted x coordinate, in millimeters.
+ * @param undistortedY The undistorted y coordinate, in millimeters.
+ *
+ * @return if the conversion was successful
+ * @todo Review the tolerance and maximum iterations of the root-
+ *       finding algorithm.
+ * @todo Review the handling of non-convergence of the root-finding
+ *       algorithm.
+ * @todo Add error handling for near-zero determinant.
+*/
+std::tuple<double, double> removeDistortion(double dx, double dy,
+                        const std::vector<double> &odtX, const std::vector<double> &odtY) {
+  // Solve the distortion equation using the Newton-Raphson method.
+  // Set the error tolerance to about one millionth of a NAC pixel.
+  const double tol = 1.4E-5;
+
+  // The maximum number of iterations of the Newton-Raphson method.
+  const int maxTries = 60;
+
+  double x;
+  double y;
+  std::tuple<double, double> undistortedPoint(dx, dy);
+  std::tuple<double, double> distortedPoint;
+
+  // Initial guess at the root
+  x = dx;
+  y = dy;
+
+  distortedPoint = distortionFunction(x, y, odtX, odtY);
+
+  for (int count = 1; ((fabs(std::get<0>(distortedPoint)) +fabs(std::get<1>(distortedPoint))) > tol) && (count < maxTries); count++) {
+
+    distortedPoint = distortionFunction(x, y, odtX, odtY);
+
+    // fx = dx - fx;
+    // fy = dy - fy;
+    distortedPoint = std::make_tuple(dx - std::get<0>(distortedPoint), dy - std::get<1>(distortedPoint));
+
+    std::vector<std::vector<double>> jacobian;
+
+    jacobian = distortionJacobian(x, y, odtX, odtY);
+
+    // Jxx * Jyy - Jxy * Jyx
+    double determinant = jacobian[0][0] * jacobian[1][1] - jacobian[0][1] * jacobian[1][0];
+    if (fabs(determinant) < 1E-6) {
+      undistortedPoint = std::make_tuple(x, y);
+      //
+      // Near-zero determinant. Add error handling here.
+      //
+      //-- Just break out and return with no convergence
+      return undistortedPoint;
+    }
+
+    //x = x + (Jyy * fx - Jxy * fy)
+    x = x + (jacobian[1][1] * std::get<0>(distortedPoint) - jacobian[0][1] * std::get<1>(distortedPoint)) / determinant;
+    // y = y + (Jxx * fy - Jyx * fx)
+    y = y + (jacobian[0][0] * std::get<1>(distortedPoint) - jacobian[1][0] * std::get<0>(distortedPoint)) / determinant;
+  }
+
+  if ( (fabs(std::get<0>(distortedPoint)) + fabs(std::get<1>(distortedPoint))) <= tol) {
+    // The method converged to a root.
+    undistortedPoint = std::make_tuple(x, y);
+  }
+  // Otherwise method did not converge to a root within the maximum
+  // number of iterations. Return with no distortion.
+  return undistortedPoint;
+}
+
+/**
+ * @description Jacobian of the distortion function. The Jacobian was computed
+ * algebraically from the function described in the distortionFunction
+ * method.
+ *
+ * @param x
+ * @param y
+ * @param odtX opticalDistCoef In X
+ * @param odtY opticalDistCoef In Y
+ *
+ * @returns jacobian a jacobian vector of vectors as
+                     [0][0]: xx, [0][1]: xy
+                     [1][0]: yx, [1][1]: yy
+ */
+
+std::vector<std::vector<double>> distortionJacobian(double x, double y,
+                        const std::vector<double> &odtX, const std::vector<double> &odtY) {
+
+  double d_dx[10];
+  d_dx[0] = 0;
+  d_dx[1] = 1;
+  d_dx[2] = 0;
+  d_dx[3] = 2 * x;
+  d_dx[4] = y;
+  d_dx[5] = 0;
+  d_dx[6] = 3 * x * x;
+  d_dx[7] = 2 * x * y;
+  d_dx[8] = y * y;
+  d_dx[9] = 0;
+  double d_dy[10];
+  d_dy[0] = 0;
+  d_dy[1] = 0;
+  d_dy[2] = 1;
+  d_dy[3] = 0;
+  d_dy[4] = x;
+  d_dy[5] = 2 * y;
+  d_dy[6] = 0;
+  d_dy[7] = x * x;
+  d_dy[8] = 2 * x * y;
+  d_dy[9] = 3 * y * y;
+
+  std::vector<std::vector<double>> jacobian(2, std::vector<double>(2));
+
+  jacobian[0][0] = 0;
+  jacobian[0][1] = 0;
+  jacobian[1][0] = 0;
+  jacobian[1][1] = 0;
+
+  for (int i = 0; i < 10; i++) {
+    jacobian[0][0] = jacobian[0][0] + d_dx[i] * odtX[i];
+    jacobian[0][1] = jacobian[0][1] + d_dy[i] * odtX[i];
+    jacobian[1][0] = jacobian[1][0] + d_dx[i] * odtY[i];
+    jacobian[1][1] = jacobian[1][1] + d_dy[i] * odtY[i];
+  }
+
+  return jacobian;
+}
+
+/**
+ * @description Compute distorted focal plane (dx,dy) coordinate  given an undistorted focal
+ * plane (ux,uy) coordinate. This uses the third order Taylor approximation to the
+ * distortion model.
+ *
+ * @param ux Undistored x
+ * @param uy Undistored y
+ * @param odtX opticalDistCoef In X
+ * @param odtY opticalDistCoef In Y
+ *
+ * @returns distortedPoint Newly adjusted focal plane coordinates as an x, y tuple
+ */
+std::tuple<double, double> distortionFunction(double ux, double uy,
+  const std::vector<double> &odtX, const std::vector<double> &odtY) {
+
+  double f[10];
+  f[0] = 1;
+  f[1] = ux;
+  f[2] = uy;
+  f[3] = ux * ux;
+  f[4] = ux * uy;
+  f[5] = uy * uy;
+  f[6] = ux * ux * ux;
+  f[7] = ux * ux * uy;
+  f[8] = ux * uy * uy;
+  f[9] = uy * uy * uy;
+
+  std::tuple<double, double> distortedPoint(0.0, 0.0);
+  for (int i = 0; i < 10; i++) {
+    distortedPoint = std::make_tuple(std::get<0>(distortedPoint) + f[i] * odtX[i],
+                             std::get<1>(distortedPoint) + f[i] * odtY[i]);
+  }
+
+  return distortedPoint;
+}
+
+/**
+ * @description Compute undistorted focal plane coordinate given a distorted
+ * coordinate set and the distortion coefficients
+ *
+ * @param inFocalPlaneX Distorted x
+ * @param inFocalPlaneY Distorted y
+ * @param opticalDistCoef distortion coefficients
+ *
+ * @returns undistortedPoint Newly adjusted focal plane coordinates as an x, y tuple
+ */
+std::tuple<double, double> removeDistortion(double inFocalPlaneX, double inFocalPlaneY,
+  const double opticalDistCoef[3], double tolerance) {
+  double rr = inFocalPlaneX * inFocalPlaneX + inFocalPlaneY * inFocalPlaneY;
+  std::tuple<double, double> undistortedPoint(inFocalPlaneX, inFocalPlaneY);
+
+  if (rr > tolerance)
+  {
+    double dr = opticalDistCoef[0] + (rr * (opticalDistCoef[1] + rr * opticalDistCoef[2]));
+    undistortedPoint = std::make_tuple(inFocalPlaneX * (1.0 - dr), inFocalPlaneY * (1.0 - dr));
+  }
+
+  return undistortedPoint;
+}
+
+/**
+ * @description Compute undistorted focal plane coordinate given a distorted
+ * focal plane coordinate. This method works by iteratively adding distortion
+ * until the new distorted point, r, undistorts to within a tolerance of the
+ * original point, rp.
+ *
+ * @param inFocalPlaneX Distorted x
+ * @param inFocalPlaneY Distorted y
+ * @param opticalDistCoef Distortion coefficients
+ * @param desiredPrecision Convergence precision
+ * @param tolerance Tolerance of r^2
+ *
+ * @returns undistortedPoint Newly adjusted focal plane coordinates as an x, y tuple
+ */
+std::tuple<double, double> invertDistortion(double inFocalPlaneX, double inFocalPlaneY,
+  const double opticalDistCoef[3], double desiredPrecision, double tolerance) {
+  double rp2 = (inFocalPlaneX * inFocalPlaneX) +
+               (inFocalPlaneY * inFocalPlaneY);
+  std::tuple<double, double> undistortedPoint;
+
+  if (rp2 > tolerance) {
+    double rp = sqrt(rp2);
+    // Compute first fractional distortion using rp
+    double drOverR = opticalDistCoef[0]
+                  + (rp2 * (opticalDistCoef[1] + (rp2 * opticalDistCoef[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 >= 15 || r > 1E9) {
+        drOverR = 0.0;
+        break;
+      }
+
+      r_prev = r;
+      r2_prev = r * r;
+
+      // Compute new fractional distortion:
+      drOverR = opticalDistCoef[0]
+             + (r2_prev * (opticalDistCoef[1] + (r2_prev * opticalDistCoef[2])));
+
+      // Compute new estimate of r
+      r = rp + (drOverR * r_prev);
+      iteration++;
+    }
+    while (fabs(r * (1 - drOverR) - rp) > desiredPrecision);
+    undistortedPoint = std::make_tuple(inFocalPlaneX / (1.0 - drOverR),
+                                       inFocalPlaneY / (1.0 - drOverR));
+  }
+  return undistortedPoint;
+}
diff --git a/src/UsgsAstroFrameSensorModel.cpp b/src/UsgsAstroFrameSensorModel.cpp
index 1f9dc37..03f45a7 100644
--- a/src/UsgsAstroFrameSensorModel.cpp
+++ b/src/UsgsAstroFrameSensorModel.cpp
@@ -1,4 +1,5 @@
 #include "UsgsAstroFrameSensorModel.h"
+#include "Distortion.h"
 
 #include <iomanip>
 #include <iostream>
@@ -134,14 +135,14 @@ csm::ImageCoord UsgsAstroFrameSensorModel::groundToImage(
   undistortedy = (f * (m[0][1] * xo + m[1][1] * yo + m[2][1] * zo)/denom) + m_linePp;
 
   // Apply the distortion to the line/sample location and then convert back to line/sample
-  double distortedx, distortedy;
-  distortionFunction(undistortedx, undistortedy, distortedx, distortedy);
+  std::tuple<double, double> distortedPoint;
+  distortedPoint = distortionFunction(undistortedx, undistortedy, m_odtX, m_odtY);
 
 
   // Convert distorted mm into line/sample
   double sample, line;
-  sample = m_iTransS[0] + m_iTransS[1] * distortedx + m_iTransS[2] * distortedy + m_ccdCenter[1];
-  line =   m_iTransL[0] + m_iTransL[1] * distortedx + m_iTransL[2] * distortedy + m_ccdCenter[0];
+  sample = m_iTransS[0] + m_iTransS[1] * std::get<0>(distortedPoint) + m_iTransS[2] * std::get<1>(distortedPoint) + m_ccdCenter[1];
+  line =   m_iTransL[0] + m_iTransL[1] * std::get<0>(distortedPoint) + m_iTransL[2] * std::get<1>(distortedPoint) + m_ccdCenter[0];
 
   return csm::ImageCoord(line, sample);
 }
@@ -157,9 +158,9 @@ csm::ImageCoordCovar UsgsAstroFrameSensorModel::groundToImage(const csm::EcefCoo
     gp.y = groundPt.y;
     gp.z = groundPt.z;
 
-    csm::ImageCoord ip = groundToImage(                                          
-      gp, desiredPrecision, achievedPrecision, warnings);                     
-   csm::ImageCoordCovar result(ip.line, ip.samp);                                    
+    csm::ImageCoord ip = groundToImage(
+      gp, desiredPrecision, achievedPrecision, warnings);
+   csm::ImageCoordCovar result(ip.line, ip.samp);
    // This is a partial, incorrect implementation to test if SocetGXP needs
    // this method implemented in order to load the sensor.
    return result;
@@ -192,18 +193,13 @@ csm::EcefCoord UsgsAstroFrameSensorModel::imageToGround(const csm::ImageCoord &i
   x_camera = m_transX[0] + m_transX[1] * (lo - line_center) + m_transX[2] * (so - sample_center);
 
   // Apply the distortion model (remove distortion)
-  double undistorted_cameraX, undistorted_cameraY = 0.0;
-
-  setFocalPlane(x_camera, y_camera, undistorted_cameraX, undistorted_cameraY);
+  std::tuple<double, double> undistortedPoint;
+  undistortedPoint = removeDistortion(x_camera, y_camera, m_odtX, m_odtY);
 
   // Now back from distorted mm to pixels
-  double udx, udy; //distorted line and sample
-  udx = undistorted_cameraX;
-  udy = undistorted_cameraY;
-
-  xl = m[0][0] * udx + m[0][1] * udy - m[0][2] * -m_focalLength;
-  yl = m[1][0] * udx + m[1][1] * udy - m[1][2] * -m_focalLength;
-  zl = m[2][0] * udx + m[2][1] * udy - m[2][2] * -m_focalLength;
+  xl = m[0][0] * std::get<0>(undistortedPoint) + m[0][1] * std::get<1>(undistortedPoint) - m[0][2] * - m_focalLength;
+  yl = m[1][0] * std::get<0>(undistortedPoint) + m[1][1] * std::get<1>(undistortedPoint) - m[1][2] * - m_focalLength;
+  zl = m[2][0] * std::get<0>(undistortedPoint) + m[2][1] * std::get<1>(undistortedPoint) - m[2][2] * - m_focalLength;
 
   double x, y, z;
   double xc, yc, zc;
@@ -251,15 +247,13 @@ csm::EcefLocus UsgsAstroFrameSensorModel::imageToRemoteImagingLocus(const csm::I
   double focalPlaneY = m_transY[0] + m_transY[1] * row + m_transY[2] * col;
 
   // Distort
-  double undistortedFocalPlaneX = focalPlaneX;
-  double undistortedFocalPlaneY = focalPlaneY;
-
-  setFocalPlane(focalPlaneX, focalPlaneY, undistortedFocalPlaneX, undistortedFocalPlaneY);
+  std::tuple<double, double> undistortedPoint;
+  undistortedPoint = removeDistortion(focalPlaneX, focalPlaneY, m_odtX, m_odtY);
 
   // Get rotation matrix and transform to a body-fixed frame
   double m[3][3];
   calcRotationMatrix(m);
-  std::vector<double> lookC { undistortedFocalPlaneX, undistortedFocalPlaneY, m_focalLength };
+  std::vector<double> lookC { std::get<0>(undistortedPoint), std::get<1>(undistortedPoint), m_focalLength };
   std::vector<double> lookB {
     m[0][0] * lookC[0] + m[0][1] * lookC[1] + m[0][2] * lookC[2],
     m[1][0] * lookC[0] + m[1][1] * lookC[1] + m[1][2] * lookC[2],
@@ -652,8 +646,8 @@ std::string UsgsAstroFrameSensorModel::getModelState() const {
       {"m_currentParameterValue", m_currentParameterValue},
       {"m_imageIdentifier", m_imageIdentifier},
       {"m_collectionIdentifier", m_collectionIdentifier},
-      {"m_referencePointXyz", {m_referencePointXyz.x, 
-                               m_referencePointXyz.y, 
+      {"m_referencePointXyz", {m_referencePointXyz.x,
+                               m_referencePointXyz.y,
                                m_referencePointXyz.z}},
       {"m_currentParameterCovariance", m_currentParameterCovariance}
     };
@@ -964,7 +958,7 @@ std::string UsgsAstroFrameSensorModel::constructStateFromIsd(const std::string&
 
       state["m_referencePointXyz"] = std::vector<double>(3, 0.0);
       state["m_currentParameterCovariance"] = std::vector<double>(NUM_PARAMETERS*NUM_PARAMETERS,0.0);
-      state["m_collectionIdentifier"] = ""; 
+      state["m_collectionIdentifier"] = "";
 
       std::cerr << "Constants Set!" << std::endl;
 
@@ -1107,7 +1101,7 @@ std::vector<double> UsgsAstroFrameSensorModel::getCrossCovarianceMatrix(
     const GeometricModel &comparisonModel,
     csm::param::Set pSet,
     const GeometricModelList &otherModels) const {
-   
+
    // No correlation between models.
    const std::vector<int>& indices = getParameterSetIndices(pSet);
    size_t num_rows = indices.size();
@@ -1207,180 +1201,6 @@ void UsgsAstroFrameSensorModel::losEllipsoidIntersect(
 }
 
 
-/**
- * @brief Compute undistorted focal plane x/y.
- *
- * Computes undistorted focal plane (x,y) coordinates given a distorted focal plane (x,y)
- * coordinate. The undistorted coordinates are solved for using the Newton-Raphson
- * method for root-finding if the distortionFunction method is invoked.
- *
- * @param dx distorted focal plane x in millimeters
- * @param dy distorted focal plane y in millimeters
- * @param undistortedX The undistorted x coordinate, in millimeters.
- * @param undistortedY The undistorted y coordinate, in millimeters.
- *
- * @return if the conversion was successful
- * @todo Review the tolerance and maximum iterations of the root-
- *       finding algorithm.
- * @todo Review the handling of non-convergence of the root-finding
- *       algorithm.
- * @todo Add error handling for near-zero determinant.
-*/
-bool UsgsAstroFrameSensorModel::setFocalPlane(double dx,double dy,
-                                       double &undistortedX,
-                                       double &undistortedY ) const {
-
-  // Solve the distortion equation using the Newton-Raphson method.
-  // Set the error tolerance to about one millionth of a NAC pixel.
-  const double tol = 1.4E-5;
-
-  // The maximum number of iterations of the Newton-Raphson method.
-  const int maxTries = 60;
-
-  double x;
-  double y;
-  double fx;
-  double fy;
-  double Jxx;
-  double Jxy;
-  double Jyx;
-  double Jyy;
-
-  // Initial guess at the root
-  x = dx;
-  y = dy;
-
-  distortionFunction(x, y, fx, fy);
-
-
-  for (int count = 1; ((fabs(fx) +fabs(fy)) > tol) && (count < maxTries); count++) {
-
-    this->distortionFunction(x, y, fx, fy);
-
-    fx = dx - fx;
-    fy = dy - fy;
-
-    distortionJacobian(x, y, Jxx, Jxy, Jyx, Jyy);
-
-    double determinant = Jxx * Jyy - Jxy * Jyx;
-    if (fabs(determinant) < 1E-6) {
-
-      undistortedX = x;
-      undistortedY = y;
-      //
-      // Near-zero determinant. Add error handling here.
-      //
-      //-- Just break out and return with no convergence
-      return false;
-    }
-
-    x = x + (Jyy * fx - Jxy * fy) / determinant;
-    y = y + (Jxx * fy - Jyx * fx) / determinant;
-  }
-
-  if ( (fabs(fx) + fabs(fy)) <= tol) {
-    // The method converged to a root.
-    undistortedX = x;
-    undistortedY = y;
-    return true;
-
-  }
-  else {
-    // The method did not converge to a root within the maximum
-    // number of iterations. Return with no distortion.
-    undistortedX = dx;
-    undistortedY = dy;
-    return false;
-  }
-  return true;
-}
-
-
-/**
- * @description Jacobian of the distortion function. The Jacobian was computed
- * algebraically from the function described in the distortionFunction
- * method.
- *
- * @param x
- * @param y
- * @param Jxx  Partial_xx
- * @param Jxy  Partial_xy
- * @param Jyx  Partial_yx
- * @param Jyy  Partial_yy
- */
-void UsgsAstroFrameSensorModel::distortionJacobian(double x, double y, double &Jxx, double &Jxy,
-                                            double &Jyx, double &Jyy) const {
-
-  double d_dx[10];
-  d_dx[0] = 0;
-  d_dx[1] = 1;
-  d_dx[2] = 0;
-  d_dx[3] = 2 * x;
-  d_dx[4] = y;
-  d_dx[5] = 0;
-  d_dx[6] = 3 * x * x;
-  d_dx[7] = 2 * x * y;
-  d_dx[8] = y * y;
-  d_dx[9] = 0;
-  double d_dy[10];
-  d_dy[0] = 0;
-  d_dy[1] = 0;
-  d_dy[2] = 1;
-  d_dy[3] = 0;
-  d_dy[4] = x;
-  d_dy[5] = 2 * y;
-  d_dy[6] = 0;
-  d_dy[7] = x * x;
-  d_dy[8] = 2 * x * y;
-  d_dy[9] = 3 * y * y;
-
-  Jxx = 0.0;
-  Jxy = 0.0;
-  Jyx = 0.0;
-  Jyy = 0.0;
-
-  for (int i = 0; i < 10; i++) {
-    Jxx = Jxx + d_dx[i] * m_odtX[i];
-    Jxy = Jxy + d_dy[i] * m_odtX[i];
-    Jyx = Jyx + d_dx[i] * m_odtY[i];
-    Jyy = Jyy + d_dy[i] * m_odtY[i];
-  }
-}
-
-
-
-/**
- * @description Compute distorted focal plane (dx,dy) coordinate  given an undistorted focal
- * plane (ux,uy) coordinate. This describes the third order Taylor approximation to the
- * distortion model.
- *
- * @param ux Undistored x
- * @param uy Undistored y
- * @param dx Result distorted x
- * @param dy Result distorted y
- */
-void UsgsAstroFrameSensorModel::distortionFunction(double ux, double uy, double &dx, double &dy) const {
-
-  double f[10];
-  f[0] = 1;
-  f[1] = ux;
-  f[2] = uy;
-  f[3] = ux * ux;
-  f[4] = ux * uy;
-  f[5] = uy * uy;
-  f[6] = ux * ux * ux;
-  f[7] = ux * ux * uy;
-  f[8] = ux * uy * uy;
-  f[9] = uy * uy * uy;
-
-  dx = 0.0;
-  dy = 0.0;
-  for (int i = 0; i < 10; i++) {
-    dx = dx + f[i] * m_odtX[i];
-    dy = dy + f[i] * m_odtY[i];
-  }
-}
-
 /***** Helper Functions *****/
 
 double UsgsAstroFrameSensorModel::getValue(
diff --git a/src/UsgsAstroLsSensorModel.cpp b/src/UsgsAstroLsSensorModel.cpp
index c476c4c..ad040c9 100644
--- a/src/UsgsAstroLsSensorModel.cpp
+++ b/src/UsgsAstroLsSensorModel.cpp
@@ -19,6 +19,7 @@
 #define USGS_SENSOR_LIBRARY
 
 #include "UsgsAstroLsSensorModel.h"
+#include "Distortion.h"
 
 #include <algorithm>
 #include <iostream>
@@ -1677,26 +1678,6 @@ void UsgsAstroLsSensorModel::computeDistortedFocalPlaneCoordinates(const double&
   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{
@@ -1779,59 +1760,6 @@ void UsgsAstroLsSensorModel::calculateAttitudeCorrection(const double& time, con
 }
 
 
-// This method works by iteratively adding distortion until the new distorted
-// point, r, undistorts to within a tolerance of the original point, rp.
-void UsgsAstroLsSensorModel::reconstructSensorDistortion(
-    double& focalX,
-    double& focalY,
-    const double& desiredPrecision) const
-{
-  if (m_opticalDistCoef[0] != 0.0 ||
-     m_opticalDistCoef[1] != 0.0 ||
-     m_opticalDistCoef[2] != 0.0)
-   {
-     double rp2 = (focalX * focalX) + (focalY * focalY);
-     double tolerance = 1.0E-6;
-     if (rp2 > tolerance) {
-       double rp = sqrt(rp2);
-       // Compute first fractional distortion using rp
-       double drOverR = m_opticalDistCoef[0]
-                      + (rp2 * (m_opticalDistCoef[1] + (rp2 * m_opticalDistCoef[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 >= 15 || r > 1E9) {
-           drOverR = 0.0;
-           break;
-         }
-
-         r_prev = r;
-         r2_prev = r * r;
-
-         // Compute new fractional distortion:
-         drOverR = m_opticalDistCoef[0]
-                 + (r2_prev * (m_opticalDistCoef[1] + (r2_prev * m_opticalDistCoef[2])));
-
-         // Compute new estimate of r
-         r = rp + (drOverR * r_prev);
-         iteration++;
-       }
-       while (fabs(r * (1 - drOverR) - rp) > desiredPrecision);
-
-       focalX = focalX / (1.0 - drOverR);
-       focalY = focalY / (1.0 - drOverR);
-     }
-   }
-}
-
-
 //***************************************************************************
 // UsgsAstroLsSensorModel::losToEcf
 //***************************************************************************
@@ -1850,7 +1778,7 @@ void UsgsAstroLsSensorModel::losToEcf(
    double&       bodyLookZ) const  // output line-of-sight z coordinate
 {
    //# private_func_description
-   //  Computes image ray (look vector) 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));
@@ -1866,12 +1794,12 @@ void UsgsAstroLsSensorModel::losToEcf(
    computeDistortedFocalPlaneCoordinates(fractionalLine, sampleUSGSFull, natFocalPlaneX, natFocalPlaneY);
 
    // Remove lens distortion
-   double focalPlaneX, focalPlaneY;
-   computeUndistortedFocalPlaneCoordinates(natFocalPlaneX, natFocalPlaneY, focalPlaneX, focalPlaneY);
+   std::tuple<double, double> undistortedPoint;
+   undistortedPoint = removeDistortion(natFocalPlaneX, natFocalPlaneY, m_opticalDistCoef);
 
   // Define imaging ray (look vector) in camera space
    double cameraLook[3];
-   createCameraLookVector(focalPlaneX, focalPlaneY, adj, cameraLook);
+   createCameraLookVector(std::get<0>(undistortedPoint), std::get<1>(undistortedPoint), adj, cameraLook);
 
    // Apply attitude correction
    double attCorr[9];
@@ -2459,17 +2387,18 @@ csm::ImageCoord UsgsAstroLsSensorModel::computeViewingPixel(
    double lookScale = m_focal / adjustedLookZ;
    double focalX = adjustedLookX * lookScale;
    double focalY = adjustedLookY * lookScale;
+   std::tuple<double, double> undistortedPoint;
 
    // Invert distortion
-   reconstructSensorDistortion(focalX, focalY, desiredPrecision);
+   undistortedPoint = invertDistortion(focalX, focalY, m_opticalDistCoef, desiredPrecision);
 
    // Convert to detector line and sample
    double detectorLine = m_iTransL[0]
-                       + m_iTransL[1] * focalX
-                       + m_iTransL[2] * focalY;
+                       + m_iTransL[1] * std::get<0>(undistortedPoint)
+                       + m_iTransL[2] * std::get<1>(undistortedPoint);
    double detectorSample = m_iTransS[0]
-                         + m_iTransS[1] * focalX
-                         + m_iTransS[2] * focalY;
+                         + m_iTransS[1] * std::get<0>(undistortedPoint)
+                         + m_iTransS[2] * std::get<1>(undistortedPoint);
 
    // Convert to image sample line
    double line = detectorLine + m_detectorLineOrigin - m_detectorLineOffset
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 881f738..e3567d1 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -4,7 +4,8 @@ cmake_minimum_required(VERSION 3.10)
 add_executable(runCSMCameraModelTests
                PluginTests.cpp
                FrameCameraTests.cpp
-               LineScanCameraTests.cpp)
+               LineScanCameraTests.cpp
+               DistortionTests.cpp)
 if(WIN32)
   option(CMAKE_USE_WIN32_THREADS_INIT "using WIN32 threads" ON)
   option(gtest_disable_pthreads "Disable uses of pthreads in gtest." ON)
diff --git a/tests/DistortionTests.cpp b/tests/DistortionTests.cpp
new file mode 100644
index 0000000..b21971e
--- /dev/null
+++ b/tests/DistortionTests.cpp
@@ -0,0 +1,154 @@
+#include "Distortion.h"
+
+#include <gtest/gtest.h>
+
+#include "Fixtures.h"
+
+// NOTE: The imagePt format is (Lines,Samples)
+
+INSTANTIATE_TEST_CASE_P(JacobianTest,ImageCoordParameterizedTest,
+                        ::testing::Values(csm::ImageCoord(2.5,2.5),csm::ImageCoord(7.5,7.5)));
+
+TEST_P(ImageCoordParameterizedTest, JacobianTest) {
+   std::vector<double> odtX = {1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0};
+   std::vector<double> odtY = {0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0};
+
+   double Jxx,Jxy,Jyx,Jyy;
+
+   csm::ImageCoord imagePt = GetParam();
+   std::vector<std::vector<double>> jacobian;
+   jacobian = distortionJacobian(imagePt.samp, imagePt.line, odtX, odtY);
+
+   // Jxx * Jyy - Jxy * Jyx
+   double determinant = fabs(jacobian[0][0] * jacobian[1][1] - jacobian[0][1] * jacobian[1][0]);
+   EXPECT_GT(determinant,1e-3);
+}
+
+TEST(Transverse, Jacobian1) {
+  csm::ImageCoord imagePt(7.5, 7.5);
+
+  std::vector<double> odtX = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0};
+  std::vector<double> odtY = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,1.0};
+
+  std::vector<std::vector<double>> jacobian;
+  jacobian = distortionJacobian(imagePt.samp, imagePt.line, odtX, odtY);
+
+  EXPECT_NEAR(jacobian[0][0],56.25,1e-8 );
+  EXPECT_NEAR(jacobian[0][1],112.5,1e-8);
+  EXPECT_NEAR(jacobian[1][0],56.25,1e-8);
+  EXPECT_NEAR(jacobian[1][1],281.25,1e-8);
+}
+
+TEST(Transverse, distortMe_AlternatingOnes) {
+  csm::ImageCoord imagePt(7.5, 7.5);
+
+  std::vector<double> odtX = {1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0};
+  std::vector<double> odtY = {0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0};
+
+  double dx,dy;
+  std::tuple<double, double> distortedPoint;
+  distortedPoint = distortionFunction(imagePt.samp, imagePt.line, odtX, odtY);
+
+  EXPECT_NEAR(std::get<0>(distortedPoint),908.5,1e-8 );
+  EXPECT_NEAR(std::get<1>(distortedPoint),963.75,1e-8);
+}
+
+TEST(Transverse,  distortMe_AllCoefficientsOne) {
+  csm::ImageCoord imagePt(7.5, 7.5);
+
+  std::vector<double> odtX = {1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0};
+  std::vector<double> odtY = {1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0};
+
+  double dx,dy;
+  std::tuple<double, double> distortedPoint;
+  distortedPoint = distortionFunction(imagePt.samp, imagePt.line, odtX, odtY);
+
+  EXPECT_NEAR(std::get<0>(distortedPoint),1872.25,1e-8 );
+  EXPECT_NEAR(std::get<1>(distortedPoint),1872.25,1e-8);
+}
+
+TEST(transverse, removeDistortion1) {
+  csm::ImageCoord imagePt(7.5, 7.5);
+
+  std::vector<double> odtX = {0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
+  std::vector<double> odtY = {0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
+
+  std::tuple<double, double> undistortedPoint;
+  undistortedPoint = removeDistortion(imagePt.samp, imagePt.line, odtX, odtY);
+
+  EXPECT_NEAR(imagePt.samp,7.5,1e-8 );
+  EXPECT_NEAR(imagePt.line,7.5,1e-8);
+  EXPECT_NEAR(imagePt.line,std::get<0>(undistortedPoint),1e-8);
+  EXPECT_NEAR(imagePt.samp,std::get<1>(undistortedPoint),1e-8);
+}
+
+TEST(transverse, removeDistortion_AllCoefficientsOne) {
+  csm::ImageCoord imagePt(1872.25, 1872.25);
+
+  std::vector<double> odtX = {1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0};
+  std::vector<double> odtY = {1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0};
+
+  std::tuple<double, double> undistortedPoint;
+  undistortedPoint = removeDistortion(imagePt.samp, imagePt.line, odtX, odtY);
+
+  // The Jacobian is singular, so the setFocalPlane should break out of it's iteration and
+  // returns the same distorted coordinates that were passed in.
+  EXPECT_NEAR(std::get<0>(undistortedPoint),imagePt.samp,1e-8 );
+  EXPECT_NEAR(std::get<1>(undistortedPoint),imagePt.line,1e-8);
+}
+
+TEST(transverse, removeDistortion_AlternatingOnes) {
+  csm::ImageCoord imagePt(963.75, 908.5);
+
+  std::vector<double> odtX = {1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0};
+  std::vector<double> odtY = {0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0};
+
+  std::tuple<double, double> undistortedPoint;
+  undistortedPoint = removeDistortion(imagePt.samp, imagePt.line, odtX, odtY);
+
+  EXPECT_NEAR(std::get<0>(undistortedPoint),7.5,1e-8 );
+  EXPECT_NEAR(std::get<1>(undistortedPoint),7.5,1e-8);
+}
+
+TEST(Radial, testRemoveDistortion) {
+  csm::ImageCoord imagePt(0.0, 4.0);
+
+  double dx, dy;
+  double coeffs[3] = {0, 0, 0};
+  std::tuple<double, double> undistortedPoint;
+
+  undistortedPoint = removeDistortion(imagePt.samp, imagePt.line, coeffs);
+
+  EXPECT_NEAR(std::get<0>(undistortedPoint),4,1e-8);
+  EXPECT_NEAR(std::get<1>(undistortedPoint),0,1e-8);
+}
+
+// If coeffs are 0 then this will have the same result as removeDistortion
+// with 0 distortion coefficients
+TEST(Radial, testInverseDistortion){
+  csm::ImageCoord imagePt(0.0, 4.0);
+
+  double dx, dy;
+  double desiredPrecision = 0.01;
+  double coeffs[3] = {0, 0, 0};
+  std::tuple<double, double> undistortedPoint;
+
+  undistortedPoint = invertDistortion(imagePt.samp, imagePt.line, coeffs, desiredPrecision);
+
+  EXPECT_NEAR(std::get<0>(undistortedPoint),4,1e-8);
+  EXPECT_NEAR(std::get<1>(undistortedPoint),0,1e-8);
+}
+
+TEST(Radial, testInverseOnesCoeffs){
+  csm::ImageCoord imagePt(0.0, 4.0);
+
+  double dx, dy;
+  double desiredPrecision = 0.01;
+  double coeffs[3] = {1, 1, 1};
+  std::tuple<double, double> undistortedPoint;
+
+  undistortedPoint = invertDistortion(imagePt.samp, imagePt.line, coeffs, desiredPrecision);
+
+  EXPECT_NEAR(std::get<0>(undistortedPoint),4,1e-8);
+  EXPECT_NEAR(std::get<1>(undistortedPoint),0,1e-8);
+}
diff --git a/tests/Fixtures.h b/tests/Fixtures.h
index fc61f71..e372b12 100644
--- a/tests/Fixtures.h
+++ b/tests/Fixtures.h
@@ -80,6 +80,8 @@ class ConstVelLineScanIsdTest : public ::testing::Test {
    }
 };
 
+class ImageCoordParameterizedTest : public ::testing::TestWithParam<csm::ImageCoord> {};
+
 class FramerParameterizedTest : public ::testing::TestWithParam<csm::ImageCoord> {
 
 protected:
diff --git a/tests/FrameCameraTests.cpp b/tests/FrameCameraTests.cpp
index f3a22bc..d41b461 100644
--- a/tests/FrameCameraTests.cpp
+++ b/tests/FrameCameraTests.cpp
@@ -7,34 +7,6 @@
 #include "Fixtures.h"
 
 using json = nlohmann::json;
-
-INSTANTIATE_TEST_CASE_P(JacobianTest,FramerParameterizedTest,
-                        ::testing::Values(csm::ImageCoord(2.5,2.5),csm::ImageCoord(7.5,7.5)));
-
-TEST_P(FramerParameterizedTest, JacobianTest) {
-
-   UsgsAstroFrameSensorModel* sensorModel = createModel(isd);
-   std::string modelState = sensorModel->getModelState();
-   auto state = json::parse(modelState);
-
-   state["m_odtX"] = {1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0};
-   state["m_odtY"] = {0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0};
-   sensorModel->replaceModelState(state.dump());
-
-   double Jxx,Jxy,Jyx,Jyy;
-   ASSERT_NE(sensorModel, nullptr);
-
-   csm::ImageCoord imagePt1 = GetParam();
-//   cout << "[" << imagePt1.samp << "," << imagePt1.line << "]"<< endl;
-   sensorModel->distortionJacobian(imagePt1.samp, imagePt1.line, Jxx, Jxy,Jyx,Jyy);
-
-   double determinant = fabs(Jxx*Jyy - Jxy*Jyx);
-   EXPECT_GT(determinant,1e-3);
-
-   delete sensorModel;
-   sensorModel=NULL;
-}
-
 // NOTE: The imagePt format is (Lines,Samples)
 
 TEST_F(FrameSensorModel, State) {
@@ -89,7 +61,7 @@ TEST_F(FrameSensorModel, OffBody3) {
 TEST_F(FrameSensorModel, getReferencePoint) {
   csm::EcefCoord groundPt = sensorModel->getReferencePoint();
   EXPECT_EQ(groundPt.x, 0.0);
-  EXPECT_EQ(groundPt.y, 0.0);   
+  EXPECT_EQ(groundPt.y, 0.0);
   EXPECT_EQ(groundPt.z, 0.0);
 }
 
@@ -105,135 +77,6 @@ TEST_F(FrameSensorModel, getImageIdentifier) {
   EXPECT_EQ("simpleFramerISD", sensorModel->getImageIdentifier());
 }
 
-TEST_F(FrameSensorModel, setFocalPlane1) {
-  csm::ImageCoord imagePt(7.5, 7.5);
-  double ux,uy;
-
-  std::string modelState = sensorModel->getModelState();
-  auto state = json::parse(modelState);
-
-  state["m_odtX"] = {0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
-  state["m_odtY"] = {0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
-  sensorModel->replaceModelState(state.dump());
-
-  ASSERT_NE(sensorModel, nullptr);
-  sensorModel->setFocalPlane(imagePt.samp, imagePt.line, ux, uy);
-  EXPECT_NEAR(imagePt.samp,7.5,1e-8 );
-  EXPECT_NEAR(imagePt.line,7.5,1e-8);
-  delete sensorModel;
-  sensorModel = NULL;
-}
-
-
-TEST_F(FrameSensorModel, Jacobian1) {
-  csm::ImageCoord imagePt(7.5, 7.5);
-
-  std::string modelState = sensorModel->getModelState();
-  auto state = json::parse(modelState);
-  state["m_odtX"] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0};
-  state["m_odtY"] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,1.0};
-  sensorModel->replaceModelState(state.dump());
-
-  double Jxx,Jxy,Jyx,Jyy;
-
-  ASSERT_NE(sensorModel, nullptr);
-  sensorModel->distortionJacobian(imagePt.samp, imagePt.line, Jxx, Jxy,Jyx,Jyy);
-
-  EXPECT_NEAR(Jxx,56.25,1e-8 );
-  EXPECT_NEAR(Jxy,112.5,1e-8);
-  EXPECT_NEAR(Jyx,56.25,1e-8);
-  EXPECT_NEAR(Jyy,281.25,1e-8);
-
-  delete sensorModel;
-  sensorModel = NULL;
-}
-
-
-TEST_F(FrameSensorModel, distortMe_AllCoefficientsOne) {
-  csm::ImageCoord imagePt(7.5, 7.5);
-
-  std::string modelState = sensorModel->getModelState();
-  auto state = json::parse(modelState);
-  state["m_odtX"] = {1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0};
-  state["m_odtY"] = {1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0};
-  sensorModel->replaceModelState(state.dump());
-
-  double dx,dy;
-  ASSERT_NE(sensorModel, nullptr);
-  sensorModel->distortionFunction(imagePt.samp, imagePt.line,dx,dy );
-
-  EXPECT_NEAR(dx,1872.25,1e-8 );
-  EXPECT_NEAR(dy,1872.25,1e-8);
-
-  delete sensorModel;
-  sensorModel = NULL;
-}
-
-TEST_F(FrameSensorModel, setFocalPlane_AllCoefficientsOne) {
-  csm::ImageCoord imagePt(1872.25, 1872.25);
-
-  std::string modelState = sensorModel->getModelState();
-  auto state = json::parse(modelState);
-  state["m_odtX"] = {1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0};
-  state["m_odtY"] = {1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0};
-  sensorModel->replaceModelState(state.dump());
-
-  double ux,uy;
-  ASSERT_NE(sensorModel, nullptr);
-  sensorModel->setFocalPlane(imagePt.samp, imagePt.line,ux,uy );
-
-  // The Jacobian is singular, so the setFocalPlane should break out of it's iteration and
-  // returns the same distorted coordinates that were passed in.
-  EXPECT_NEAR(ux,imagePt.samp,1e-8 );
-  EXPECT_NEAR(uy,imagePt.line,1e-8);
-
-  delete sensorModel;
-  sensorModel = NULL;
-}
-
-
-TEST_F(FrameSensorModel, distortMe_AlternatingOnes) {
-  csm::ImageCoord imagePt(7.5, 7.5);
-
-  std::string modelState = sensorModel->getModelState();
-  auto state = json::parse(modelState);
-  state["m_odtX"] = {1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0};
-  state["m_odtY"] = {0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0};
-  sensorModel->replaceModelState(state.dump());
-
-  double dx,dy;
-  ASSERT_NE(sensorModel, nullptr);
-  sensorModel->distortionFunction(imagePt.samp, imagePt.line,dx,dy );
-
-  EXPECT_NEAR(dx,908.5,1e-8 );
-  EXPECT_NEAR(dy,963.75,1e-8);
-
-  delete sensorModel;
-  sensorModel = NULL;
-}
-
-
-TEST_F(FrameSensorModel, setFocalPlane_AlternatingOnes) {
-  csm::ImageCoord imagePt(963.75, 908.5);
-
-  std::string modelState = sensorModel->getModelState();
-  auto state = json::parse(modelState);
-  state["m_odtX"] = {1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0};
-  state["m_odtY"] = {0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0};
-  sensorModel->replaceModelState(state.dump());
-
-  double ux,uy;
-  ASSERT_NE(sensorModel, nullptr);
-
-  sensorModel->setFocalPlane(imagePt.samp, imagePt.line,ux,uy );
-
-  EXPECT_NEAR(ux,7.5,1e-8 );
-  EXPECT_NEAR(uy,7.5,1e-8);
-
-  delete sensorModel;
-  sensorModel = NULL;
-}
-
 
 // Focal Length Tests:
 TEST_F(FrameStateTest, FL500_OffBody4) {
-- 
GitLab