From 19d6be9f1f5f38f0e3f65bbfa2845d76d3e521ec Mon Sep 17 00:00:00 2001
From: Austin Sanders <arsanders@usgs.gov>
Date: Tue, 30 Jul 2019 14:20:59 -0700
Subject: [PATCH] Fixed matrix inversion in
 computeDistortedFocalPlaneCoordinates

---
 src/Utilities.cpp        |  9 +++++++--
 tests/UtilitiesTests.cpp | 16 ++++++++++++++++
 2 files changed, 23 insertions(+), 2 deletions(-)

diff --git a/src/Utilities.cpp b/src/Utilities.cpp
index 50d892f..dec0545 100644
--- a/src/Utilities.cpp
+++ b/src/Utilities.cpp
@@ -87,13 +87,18 @@ void computeDistortedFocalPlaneCoordinates(
   double t1 = detLine - lineOrigin - iTransL[0];
   double t2 = detSample - sampleOrigin - iTransS[0];
   double determinant = m11 * m22 - m12 * m21;
-  double p11 = m11 / determinant;
+  double p11 = m22 / determinant;
   double p12 = -m12 / determinant;
   double p21 = -m21 / determinant;
-  double p22 = m22 / determinant;
+  double p22 = m11 / determinant;
 
   distortedX = p11 * t1 + p12 * t2;
   distortedY = p21 * t1 + p22 * t2;
+
+  std::cout << t1 << ", " << t2 << std::endl;
+  std::cout << p11 << ", " << p12 << std::endl;
+  std::cout << p21 << ", " << p22 << std::endl;
+
 };
 
 // Compue the image pixel for a distorted focal plane coordinate
diff --git a/tests/UtilitiesTests.cpp b/tests/UtilitiesTests.cpp
index 4da9a08..19ca11b 100644
--- a/tests/UtilitiesTests.cpp
+++ b/tests/UtilitiesTests.cpp
@@ -81,6 +81,22 @@ TEST(UtilitiesTests, computeDistortedFocalPlaneCoordinatesSumming) {
    EXPECT_DOUBLE_EQ(undistortedFocalPlaneY, 0);
 }
 
+TEST(UtilitiesTests, computeDistortedFocalPlaneCoordinatesAffine) {
+   double iTransS[] = {-10.0, 0.0, 0.1};
+   double iTransL[] = {10.0, -0.1, 0.0};
+   double undistortedFocalPlaneX, undistortedFocalPlaneY;
+   computeDistortedFocalPlaneCoordinates(
+         11.0, -9.0,
+         0.0, 0.0,
+         1.0, 1.0,
+         0.0, 0.0,
+         iTransS, iTransL,
+         undistortedFocalPlaneX, undistortedFocalPlaneY);
+   EXPECT_NEAR(undistortedFocalPlaneX, -10.0, 1e-12);
+   EXPECT_NEAR(undistortedFocalPlaneY, 10.0, 1e-12);
+}
+
+
 TEST(UtilitiesTests, computeDistortedFocalPlaneCoordinatesStart) {
    double iTransS[] = {0.0, 0.0, 10.0};
    double iTransL[] = {0.0, 10.0, 0.0};
-- 
GitLab