Bugfix in imageToRemoteImagingLocus (#361)

* Make imageToRemoteImagingLocus consistent with imageToGround

* Tune the logic and fix the test

src/UsgsAstroSarSensorModel.cpp

@@ -543,6 +543,10 @@ csm::EcefCoord UsgsAstroSarSensorModel::imageToGround(
     pointRadius -= (pointHeight - height);
   } while (fabs(pointHeight - height) > desiredPrecision);
 
+  if (achievedPrecision) {
+    *achievedPrecision = fabs(pointHeight - height);
+  }
+
   csm::EcefCoord groundPt(groundVec.x, groundVec.y, groundVec.z);
 
   return groundPt;
@@ -669,36 +673,24 @@ csm::EcefLocus UsgsAstroSarSensorModel::imageToProximateImagingLocus(
 csm::EcefLocus UsgsAstroSarSensorModel::imageToRemoteImagingLocus(
     const csm::ImageCoord& imagePt, double desiredPrecision,
     double* achievedPrecision, csm::WarningList* warnings) const {
-  // Compute the slant range
-  double time =
-      m_startingEphemerisTime + (imagePt.line - 0.5) * m_exposureDuration;
-  double groundRange = imagePt.samp * m_scaledPixelWidth;
-  std::vector<double> coeffs = getRangeCoefficients(time);
-  double slantRange = groundRangeToSlantRange(groundRange, coeffs);
 
-  // Project the negative sensor position vector onto the 0 doppler plane
-  // then compute the closest point at the slant range to that
-  csm::EcefVector spacecraftPosition = getSpacecraftPosition(time);
-  csm::EcefVector spacecraftVelocity = getSensorVelocity(time);
-  csm::EcefVector lookVec =
-      normalized(rejection(-1 * spacecraftPosition, spacecraftVelocity));
-  csm::EcefVector closestVec = spacecraftPosition + slantRange * lookVec;
+  // Find the sensor coordinates
+  csm::EcefCoord sensorPt = getSensorPosition(imagePt);
 
-  // Compute the tangent at the closest point
-  csm::EcefVector tangent;
-  if (m_lookDirection == LEFT) {
-    tangent = cross(spacecraftVelocity, lookVec);
-  } else {
-    tangent = cross(lookVec, spacecraftVelocity);
-  }
-  tangent = normalized(tangent);
+  // Compute the intersection of the ray traced through the
+  // current pixel with the datum.  
+  double height = 0.0; 
+  csm::EcefCoord groundPt = imageToGround(imagePt, height, desiredPrecision,
+                                        achievedPrecision, warnings);
 
-  if (achievedPrecision) {
-    *achievedPrecision = 0.0;
-  }
+  // Normalized direction from camera to ground
+  csm::EcefVector dir
+    = normalized(csm::EcefVector(groundPt.x - sensorPt.x,
+                                 groundPt.y - sensorPt.y,
+                                 groundPt.z - sensorPt.z));
                  
-  return csm::EcefLocus(closestVec.x, closestVec.y, closestVec.z, tangent.x,
-                        tangent.y, tangent.z);
+  return csm::EcefLocus(groundPt.x, groundPt.y, groundPt.z,
+                        dir.x, dir.y, dir.z);
 }
 
 csm::ImageCoord UsgsAstroSarSensorModel::getImageStart() const {

tests/SarTests.cpp

@@ -113,17 +113,28 @@ TEST_F(SarSensorModel, imageToProximateImagingLocus) {
 TEST_F(SarSensorModel, imageToRemoteImagingLocus) {
   double precision;
   csm::WarningList warnings;
+  csm::ImageCoord imagePt(500.0, 500.0);
   csm::EcefLocus locus = sensorModel->imageToRemoteImagingLocus(
-      csm::ImageCoord(500.0, 500.0),
+      imagePt,
       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);
+
+  csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0);
+  csm::EcefCoord sensorPt = sensorModel->getSensorPosition(imagePt);
+
+  double lookX = groundPt.x - sensorPt.x;
+  double lookY = groundPt.y - sensorPt.y;
+  double lookZ = groundPt.z - sensorPt.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_LT(precision, 0.001);
   EXPECT_TRUE(warnings.empty());
 }