From 51bc20aab8ae129951f3066820a2190fd4ebd0ff Mon Sep 17 00:00:00 2001
From: Jesse Mapel <jmapel@usgs.gov>
Date: Fri, 5 Feb 2021 16:43:04 -0700
Subject: [PATCH] Added precision setting in locus methods

---
 src/UsgsAstroFrameSensorModel.cpp | 28 +++++++++++++++++--
 src/UsgsAstroLsSensorModel.cpp    |  4 +++
 src/UsgsAstroSarSensorModel.cpp   |  8 ++++++
 tests/FrameCameraTests.cpp        | 46 +++++++++++++++++++++++++++++++
 tests/LineScanCameraTests.cpp     | 15 ++++++++--
 tests/SarTests.cpp                | 20 ++++++++++++--
 6 files changed, 113 insertions(+), 8 deletions(-)

diff --git a/src/UsgsAstroFrameSensorModel.cpp b/src/UsgsAstroFrameSensorModel.cpp
index d7942b5..33720d3 100644
--- a/src/UsgsAstroFrameSensorModel.cpp
+++ b/src/UsgsAstroFrameSensorModel.cpp
@@ -276,12 +276,32 @@ csm::EcefLocus UsgsAstroFrameSensorModel::imageToProximateImagingLocus(
     const csm::ImageCoord &imagePt, const csm::EcefCoord &groundPt,
     double desiredPrecision, double *achievedPrecision,
     csm::WarningList *warnings) const {
-  // Ignore the ground point?
   MESSAGE_LOG(
       "Computing imageToProximateImagingLocus(No ground) for point {}, {}, {}, "
       "with desired precision {}",
       imagePt.line, imagePt.samp, desiredPrecision);
-  return imageToRemoteImagingLocus(imagePt);
+
+  // The locus is a straight line so we can just use the remote locus to
+  // get the direction
+  csm::EcefLocus remoteLocus = imageToRemoteImagingLocus(
+      imagePt, desiredPrecision, achievedPrecision, warnings);
+
+  // Find the point on the locus closest to the input ground point
+  // The direction vector is already normalized so we can skip a division
+  csm::EcefVector positionToGround(
+      groundPt.x - remoteLocus.point.x,
+      groundPt.y - remoteLocus.point.y,
+      groundPt.z - remoteLocus.point.z);
+  csm::EcefVector positionToClosest =
+      dot(remoteLocus.direction, positionToGround) * remoteLocus.direction;
+
+  return csm::EcefLocus(
+      remoteLocus.point.x + positionToClosest.x,
+      remoteLocus.point.y + positionToClosest.y,
+      remoteLocus.point.z + positionToClosest.z,
+      remoteLocus.direction.x,
+      remoteLocus.direction.y,
+      remoteLocus.direction.z);
 }
 
 csm::EcefLocus UsgsAstroFrameSensorModel::imageToRemoteImagingLocus(
@@ -320,6 +340,10 @@ csm::EcefLocus UsgsAstroFrameSensorModel::imageToRemoteImagingLocus(
       sqrt(lookB[0] * lookB[0] + lookB[1] * lookB[1] + lookB[2] * lookB[2]);
   std::vector<double> lookBUnit{lookB[0] / mag, lookB[1] / mag, lookB[2] / mag};
 
+  if (achievedPrecision) {
+    *achievedPrecision = 0.0;
+  }
+
   return csm::EcefLocus(m_currentParameterValue[0], m_currentParameterValue[1],
                         m_currentParameterValue[2], lookBUnit[0], lookBUnit[1],
                         lookBUnit[2]);
diff --git a/src/UsgsAstroLsSensorModel.cpp b/src/UsgsAstroLsSensorModel.cpp
index 1b45dcb..f28f766 100644
--- a/src/UsgsAstroLsSensorModel.cpp
+++ b/src/UsgsAstroLsSensorModel.cpp
@@ -1010,6 +1010,10 @@ csm::EcefLocus UsgsAstroLsSensorModel::imageToRemoteImagingLocus(
   locus.direction.x = -locus.direction.x;
   locus.direction.y = -locus.direction.y;
   locus.direction.z = -locus.direction.z;
+
+  if (achieved_precision) {
+    *achieved_precision = 0.0;
+  }
   return locus;
 }
 
diff --git a/src/UsgsAstroSarSensorModel.cpp b/src/UsgsAstroSarSensorModel.cpp
index 523f190..0d37d27 100644
--- a/src/UsgsAstroSarSensorModel.cpp
+++ b/src/UsgsAstroSarSensorModel.cpp
@@ -658,6 +658,10 @@ csm::EcefLocus UsgsAstroSarSensorModel::imageToProximateImagingLocus(
   }
   tangent = normalized(tangent);
 
+  if (achievedPrecision) {
+    *achievedPrecision = 0.0;
+  }
+
   return csm::EcefLocus(closestVec.x, closestVec.y, closestVec.z, tangent.x,
                         tangent.y, tangent.z);
 }
@@ -689,6 +693,10 @@ csm::EcefLocus UsgsAstroSarSensorModel::imageToRemoteImagingLocus(
   }
   tangent = normalized(tangent);
 
+  if (achievedPrecision) {
+    *achievedPrecision = 0.0;
+  }
+
   return csm::EcefLocus(closestVec.x, closestVec.y, closestVec.z, tangent.x,
                         tangent.y, tangent.z);
 }
diff --git a/tests/FrameCameraTests.cpp b/tests/FrameCameraTests.cpp
index a6f2acb..197c30c 100644
--- a/tests/FrameCameraTests.cpp
+++ b/tests/FrameCameraTests.cpp
@@ -130,6 +130,52 @@ TEST_F(OrbitalFrameSensorModel, GroundPartials) {
   EXPECT_DOUBLE_EQ(partials[5], 0.0);
 }
 
+TEST_F(OrbitalFrameSensorModel, ImageToRemoteImagingLocus) {
+  csm::ImageCoord imagePt(8.0, 8.0);
+  double precision;
+  csm::WarningList warnings;
+  csm::EcefLocus locus = sensorModel->imageToRemoteImagingLocus(
+      imagePt, 0.001, &precision, &warnings);
+  csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0);
+  double lookX = groundPt.x - locus.point.x;
+  double lookY = groundPt.y - locus.point.y;
+  double lookZ = groundPt.z - locus.point.z;
+  double lookMag = sqrt(lookX * lookX + lookY * lookY + lookZ * lookZ);
+  lookX /= lookMag;
+  lookY /= lookMag;
+  lookZ /= lookMag;
+  EXPECT_NEAR(locus.direction.x, lookX, 1e-10);
+  EXPECT_NEAR(locus.direction.y, lookY, 1e-10);
+  EXPECT_NEAR(locus.direction.z, lookZ, 1e-10);
+  EXPECT_NEAR(locus.point.x, 1050000.0, 1e-10);
+  EXPECT_NEAR(locus.point.y, 0.0, 1e-10);
+  EXPECT_NEAR(locus.point.z, 0.0, 1e-10);
+  EXPECT_LT(precision, 0.001);
+  EXPECT_TRUE(warnings.empty());
+}
+
+TEST_F(OrbitalFrameSensorModel, ImageToProximateImagingLocus) {
+  csm::ImageCoord imagePt(8.0, 8.0);
+  csm::EcefCoord groundPt(1005000.0, 1000.0, 2000.0);
+  double precision;
+  csm::WarningList warnings;
+  csm::EcefLocus remoteLocus = sensorModel->imageToRemoteImagingLocus(imagePt);
+  csm::EcefLocus locus = sensorModel->imageToProximateImagingLocus(
+      imagePt, groundPt, 0.001, &precision, &warnings);
+  // The locus is a straight line so the proximate and remote should have the
+  // same direction
+  EXPECT_DOUBLE_EQ(locus.direction.x, remoteLocus.direction.x);
+  EXPECT_DOUBLE_EQ(locus.direction.y, remoteLocus.direction.y);
+  EXPECT_DOUBLE_EQ(locus.direction.z, remoteLocus.direction.z);
+  // The locus is the X-axis so the closest point is just the X component of
+  // the ground point.
+  EXPECT_NEAR(locus.point.x, 1005000.0, 1e-10);
+  EXPECT_NEAR(locus.point.y, 0.0, 1e-10);
+  EXPECT_NEAR(locus.point.z, 0.0, 1e-10);
+  EXPECT_LT(precision, 0.001);
+  EXPECT_TRUE(warnings.empty());
+}
+
 // ****************************************************************************
 //   Modified sensor model tests
 // ****************************************************************************
diff --git a/tests/LineScanCameraTests.cpp b/tests/LineScanCameraTests.cpp
index f55f965..b767a54 100644
--- a/tests/LineScanCameraTests.cpp
+++ b/tests/LineScanCameraTests.cpp
@@ -55,9 +55,11 @@ TEST_F(ConstVelocityLineScanSensorModel, OffBody) {
 TEST_F(ConstVelocityLineScanSensorModel, ProximateImageLocus) {
   csm::ImageCoord imagePt(8.0, 8.0);
   csm::EcefCoord groundPt(10, 2, 1);
+  double precision;
+  csm::WarningList warnings;
   csm::EcefLocus remoteLocus = sensorModel->imageToRemoteImagingLocus(imagePt);
-  csm::EcefLocus locus =
-      sensorModel->imageToProximateImagingLocus(imagePt, groundPt);
+  csm::EcefLocus locus = sensorModel->imageToProximateImagingLocus(
+      imagePt, groundPt, 0.001, &precision, &warnings);
   double locusToGroundX = locus.point.x - groundPt.x;
   double locusToGroundY = locus.point.y - groundPt.y;
   double locusToGroundZ = locus.point.z - groundPt.z;
@@ -68,11 +70,16 @@ TEST_F(ConstVelocityLineScanSensorModel, ProximateImageLocus) {
                   locusToGroundY * locus.direction.y +
                   locusToGroundZ * locus.direction.z,
               0.0, 1e-10);
+  EXPECT_LT(precision, 0.001);
+  EXPECT_TRUE(warnings.empty());
 }
 
 TEST_F(ConstVelocityLineScanSensorModel, RemoteImageLocus) {
   csm::ImageCoord imagePt(8.5, 8.0);
-  csm::EcefLocus locus = sensorModel->imageToRemoteImagingLocus(imagePt);
+  double precision;
+  csm::WarningList warnings;
+  csm::EcefLocus locus = sensorModel->imageToRemoteImagingLocus(
+      imagePt, 0.001, &precision, &warnings);
   csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0);
   double lookX = groundPt.x - locus.point.x;
   double lookY = groundPt.y - locus.point.y;
@@ -87,6 +94,8 @@ TEST_F(ConstVelocityLineScanSensorModel, RemoteImageLocus) {
   EXPECT_NEAR(locus.point.x, 1000.0, 1e-10);
   EXPECT_NEAR(locus.point.y, 0.0, 1e-10);
   EXPECT_NEAR(locus.point.z, 0.0, 1e-10);
+  EXPECT_LT(precision, 0.001);
+  EXPECT_TRUE(warnings.empty());
 }
 
 TEST_F(ConstVelocityLineScanSensorModel, calculateAttitudeCorrection) {
diff --git a/tests/SarTests.cpp b/tests/SarTests.cpp
index 78e53d8..cd6b21e 100644
--- a/tests/SarTests.cpp
+++ b/tests/SarTests.cpp
@@ -91,27 +91,41 @@ TEST_F(SarSensorModel, computeGroundPartials) {
 }
 
 TEST_F(SarSensorModel, imageToProximateImagingLocus) {
+  double precision;
+  csm::WarningList warnings;
   csm::EcefLocus locus = sensorModel->imageToProximateImagingLocus(
       csm::ImageCoord(500.0, 500.0),
       csm::EcefCoord(1737287.8590770673, -5403.280537826621,
-                     -3849.9796183814506));
+                     -3849.9796183814506),
+      0.001,
+      &precision,
+      &warnings);
   EXPECT_NEAR(locus.point.x, 1737388.1260092105, 1e-2);
   EXPECT_NEAR(locus.point.y, -5403.0102509726485, 1e-2);
   EXPECT_NEAR(locus.point.z, -3749.9801945280433, 1e-2);
   EXPECT_NEAR(locus.direction.x, 0.002701478402694769, 1e-5);
   EXPECT_NEAR(locus.direction.y, -0.9999963509835628, 1e-5);
   EXPECT_NEAR(locus.direction.z, -5.830873570731962e-06, 1e-5);
+  EXPECT_LT(precision, 0.001);
+  EXPECT_TRUE(warnings.empty());
 }
 
 TEST_F(SarSensorModel, imageToRemoteImagingLocus) {
-  csm::EcefLocus locus =
-      sensorModel->imageToRemoteImagingLocus(csm::ImageCoord(500.0, 500.0));
+  double precision;
+  csm::WarningList warnings;
+  csm::EcefLocus locus = sensorModel->imageToRemoteImagingLocus(
+      csm::ImageCoord(500.0, 500.0),
+      0.001,
+      &precision,
+      &warnings);
   EXPECT_NEAR(locus.point.x, 1737380.8279381434, 1e-3);
   EXPECT_NEAR(locus.point.y, 0.0, 1e-3);
   EXPECT_NEAR(locus.point.z, -3749.964442364465, 1e-3);
   EXPECT_NEAR(locus.direction.x, 0.0, 1e-8);
   EXPECT_NEAR(locus.direction.y, -1.0, 1e-8);
   EXPECT_NEAR(locus.direction.z, 0.0, 1e-8);
+  EXPECT_LT(precision, 0.001);
+  EXPECT_TRUE(warnings.empty());
 }
 
 TEST_F(SarSensorModel, adjustedPositionVelocity) {
-- 
GitLab