More stubs (#285)

* Added actual methods

* Made default build type release

* Position and Velocity API done

* Fixes from review

CMakeLists.txt

@@ -5,6 +5,14 @@ include(GNUInstallDirs)
 
 set(CMAKE_CXX_STANDARD 11)
 
+# Set a default build type if none was specified
+set(default_build_type "Release")
+if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES)
+  message(STATUS "Setting build type to '${default_build_type}' as none was specified.")
+  set(CMAKE_BUILD_TYPE "${default_build_type}" CACHE
+      STRING "Choose the type of build." FORCE)
+endif()
+
 # Optional build or link against CSM
 option (BUILD_CSM "Build the CSM library" ON)
 if(BUILD_CSM)

include/usgscsm/UsgsAstroSarSensorModel.h

@@ -195,6 +195,12 @@ class UsgsAstroSarSensorModel : public csm::RasterGM, virtual public csm::Settab
 
     virtual void setEllipsoid(const csm::Ellipsoid &ellipsoid);
 
+    ////////////////////
+    // Helper methods //
+    ////////////////////
+    void determineSensorCovarianceInImageSpace(
+       csm::EcefCoord &gp,
+       double          sensor_cov[4]) const;
     double dopplerShift(csm::EcefCoord groundPt, double tolerance) const;
 
     double slantRange(csm::EcefCoord surfPt, double time) const;
@@ -204,7 +210,7 @@ class UsgsAstroSarSensorModel : public csm::RasterGM, virtual public csm::Settab
     double groundRangeToSlantRange(double groundRange, const std::vector<double> &coeffs) const;
 
     csm::EcefVector getSpacecraftPosition(double time) const;
-    csm::EcefVector getSpacecraftVelocity(double time) const;
+    csm::EcefVector getSunPosition(const double imageTime) const;
     std::vector<double> getRangeCoefficients(double time) const;
 
     ////////////////////////////

src/UsgsAstroSarSensorModel.cpp

@@ -363,7 +363,7 @@ double UsgsAstroSarSensorModel::dopplerShift(
    csm::EcefVector groundVec(groundPt.x ,groundPt.y, groundPt.z);
    std::function<double(double)> dopplerShiftFunction = [this, groundVec](double time) {
      csm::EcefVector spacecraftPosition = getSpacecraftPosition(time);
-     csm::EcefVector spacecraftVelocity = getSpacecraftVelocity(time);
+     csm::EcefVector spacecraftVelocity = getSensorVelocity(time);
      csm::EcefVector lookVector = spacecraftPosition - groundVec;
 
      double slantRange = norm(lookVector);
@@ -427,9 +427,61 @@ csm::ImageCoordCovar UsgsAstroSarSensorModel::groundToImage(
     double* achievedPrecision,
     csm::WarningList* warnings) const
 {
-  return csm::ImageCoordCovar(0.0, 0.0,
-                              1.0, 0.0,
-                                   1.0);
+  // Ground to image with error propagation
+  // Compute corresponding image point
+  csm::EcefCoord gp(groundPt);
+
+  csm::ImageCoord ip = groundToImage(gp, desiredPrecision, achievedPrecision, warnings);
+  csm::ImageCoordCovar result(ip.line, ip.samp);
+
+  // Compute partials ls wrt XYZ
+  std::vector<double> prt(6, 0.0);
+  prt = computeGroundPartials(groundPt);
+
+  // Error propagation
+  double ltx, lty, ltz;
+  double stx, sty, stz;
+  ltx =
+     prt[0] * groundPt.covariance[0] +
+     prt[1] * groundPt.covariance[3] +
+     prt[2] * groundPt.covariance[6];
+  lty =
+     prt[0] * groundPt.covariance[1] +
+     prt[1] * groundPt.covariance[4] +
+     prt[2] * groundPt.covariance[7];
+  ltz =
+     prt[0] * groundPt.covariance[2] +
+     prt[1] * groundPt.covariance[5] +
+     prt[2] * groundPt.covariance[8];
+  stx =
+     prt[3] * groundPt.covariance[0] +
+     prt[4] * groundPt.covariance[3] +
+     prt[5] * groundPt.covariance[6];
+  sty =
+     prt[3] * groundPt.covariance[1] +
+     prt[4] * groundPt.covariance[4] +
+     prt[5] * groundPt.covariance[7];
+  stz =
+     prt[3] * groundPt.covariance[2] +
+     prt[4] * groundPt.covariance[5] +
+     prt[5] * groundPt.covariance[8];
+
+  double gp_cov[4]; // Input gp cov in image space
+  gp_cov[0] = ltx * prt[0] + lty * prt[1] + ltz * prt[2];
+  gp_cov[1] = ltx * prt[3] + lty * prt[4] + ltz * prt[5];
+  gp_cov[2] = stx * prt[0] + sty * prt[1] + stz * prt[2];
+  gp_cov[3] = stx * prt[3] + sty * prt[4] + stz * prt[5];
+
+  std::vector<double> unmodeled_cov = getUnmodeledError(ip);
+  double sensor_cov[4]; // sensor cov in image space
+  determineSensorCovarianceInImageSpace(gp, sensor_cov);
+
+  result.covariance[0] = gp_cov[0] + unmodeled_cov[0] + sensor_cov[0];
+  result.covariance[1] = gp_cov[1] + unmodeled_cov[1] + sensor_cov[1];
+  result.covariance[2] = gp_cov[2] + unmodeled_cov[2] + sensor_cov[2];
+  result.covariance[3] = gp_cov[3] + unmodeled_cov[3] + sensor_cov[3];
+
+  return result;
 }
 
 csm::EcefCoord UsgsAstroSarSensorModel::imageToGround(
@@ -447,7 +499,7 @@ csm::EcefCoord UsgsAstroSarSensorModel::imageToGround(
   // Compute the in-track, cross-track, nadir, coordinate system to solve in
   csm::EcefVector spacecraftPosition = getSpacecraftPosition(time);
   double positionMag = norm(spacecraftPosition);
-  csm::EcefVector spacecraftVelocity = getSpacecraftVelocity(time);
+  csm::EcefVector spacecraftVelocity = getSensorVelocity(time);
   // In-track unit vector
   csm::EcefVector vHat = normalized(spacecraftVelocity);
   // Nadir unit vector
@@ -479,10 +531,77 @@ csm::EcefCoordCovar UsgsAstroSarSensorModel::imageToGround(
     double* achievedPrecision,
     csm::WarningList* warnings) const
 {
-  return csm::EcefCoordCovar(0.0, 0.0, 0.0,
-                             1.0, 0.0, 0.0,
-                                  1.0, 0.0,
-                                       1.0);
+  // Image to ground with error propagation
+  // Use numerical partials
+  const double DELTA_IMAGE = 1.0;
+  const double DELTA_GROUND = m_scaledPixelWidth;
+  csm::ImageCoord ip(imagePt.line, imagePt.samp);
+
+  csm::EcefCoord gp = imageToGround(ip, height, desiredPrecision, achievedPrecision, warnings);
+
+  // Compute numerical partials xyz wrt to lsh
+  ip.line = imagePt.line + DELTA_IMAGE;
+  ip.samp = imagePt.samp;
+  csm::EcefCoord gpl = imageToGround(ip, height, desiredPrecision);
+  double xpl = (gpl.x - gp.x) / DELTA_IMAGE;
+  double ypl = (gpl.y - gp.y) / DELTA_IMAGE;
+  double zpl = (gpl.z - gp.z) / DELTA_IMAGE;
+
+  ip.line = imagePt.line;
+  ip.samp = imagePt.samp + DELTA_IMAGE;
+  csm::EcefCoord gps = imageToGround(ip, height, desiredPrecision);
+  double xps = (gps.x - gp.x) / DELTA_IMAGE;
+  double yps = (gps.y - gp.y) / DELTA_IMAGE;
+  double zps = (gps.z - gp.z) / DELTA_IMAGE;
+
+  ip.line = imagePt.line;
+  ip.samp = imagePt.samp; // +DELTA_IMAGE;
+  csm::EcefCoord gph = imageToGround(ip, height + DELTA_GROUND, desiredPrecision);
+  double xph = (gph.x - gp.x) / DELTA_GROUND;
+  double yph = (gph.y - gp.y) / DELTA_GROUND;
+  double zph = (gph.z - gp.z) / DELTA_GROUND;
+
+  // Convert sensor covariance to image space
+  double sCov[4];
+  determineSensorCovarianceInImageSpace(gp, sCov);
+
+  std::vector<double> unmod = getUnmodeledError(imagePt);
+
+  double iCov[4];
+  iCov[0] = imagePt.covariance[0] + sCov[0] + unmod[0];
+  iCov[1] = imagePt.covariance[1] + sCov[1] + unmod[1];
+  iCov[2] = imagePt.covariance[2] + sCov[2] + unmod[2];
+  iCov[3] = imagePt.covariance[3] + sCov[3] + unmod[3];
+
+  // Temporary matrix product
+  double t00, t01, t02, t10, t11, t12, t20, t21, t22;
+  t00 = xpl * iCov[0] + xps * iCov[2];
+  t01 = xpl * iCov[1] + xps * iCov[3];
+  t02 = xph * heightVariance;
+  t10 = ypl * iCov[0] + yps * iCov[2];
+  t11 = ypl * iCov[1] + yps * iCov[3];
+  t12 = yph * heightVariance;
+  t20 = zpl * iCov[0] + zps * iCov[2];
+  t21 = zpl * iCov[1] + zps * iCov[3];
+  t22 = zph * heightVariance;
+
+  // Ground covariance
+  csm::EcefCoordCovar result;
+  result.x = gp.x;
+  result.y = gp.y;
+  result.z = gp.z;
+
+  result.covariance[0] = t00 * xpl + t01 * xps + t02 * xph;
+  result.covariance[1] = t00 * ypl + t01 * yps + t02 * yph;
+  result.covariance[2] = t00 * zpl + t01 * zps + t02 * zph;
+  result.covariance[3] = t10 * xpl + t11 * xps + t12 * xph;
+  result.covariance[4] = t10 * ypl + t11 * yps + t12 * yph;
+  result.covariance[5] = t10 * zpl + t11 * zps + t12 * zph;
+  result.covariance[6] = t20 * xpl + t21 * xps + t22 * xph;
+  result.covariance[7] = t20 * ypl + t21 * yps + t22 * yph;
+  result.covariance[8] = t20 * zpl + t21 * zps + t22 * zph;
+
+  return result;
 }
 
 csm::EcefLocus UsgsAstroSarSensorModel::imageToProximateImagingLocus(
@@ -513,47 +632,72 @@ csm::ImageCoord UsgsAstroSarSensorModel::getImageStart() const
 
 csm::ImageVector UsgsAstroSarSensorModel::getImageSize() const
 {
-  return csm::ImageVector(0.0, 0.0);
+  return csm::ImageVector(m_nLines, m_nSamples);
 }
 
 pair<csm::ImageCoord, csm::ImageCoord> UsgsAstroSarSensorModel::getValidImageRange() const
 {
-  return make_pair(csm::ImageCoord(0.0, 0.0), csm::ImageCoord(0.0, 0.0));
+  csm::ImageCoord start = getImageStart();
+  csm::ImageVector size = getImageSize();
+  return make_pair(start, csm::ImageCoord(start.line + size.line, start.samp + size.samp));
 }
 
 pair<double, double> UsgsAstroSarSensorModel::getValidHeightRange() const
 {
-  return make_pair(0.0, 0.0);
+  return make_pair(m_minElevation, m_maxElevation);
 }
 
 csm::EcefVector UsgsAstroSarSensorModel::getIlluminationDirection(const csm::EcefCoord& groundPt) const
 {
-  return csm::EcefVector(0.0, 0.0, 0.0);
+  csm::EcefVector groundVec(groundPt.x, groundPt.y, groundPt.z);
+  csm::EcefVector sunPosition = getSunPosition(getImageTime(groundToImage(groundPt)));
+  csm::EcefVector illuminationDirection = normalized(groundVec - sunPosition);
+  return illuminationDirection;
 }
 
 double UsgsAstroSarSensorModel::getImageTime(const csm::ImageCoord& imagePt) const
 {
-  return 0.0;
+  return m_startingEphemerisTime + (imagePt.line - 0.5) * m_exposureDuration;
 }
 
 csm::EcefCoord UsgsAstroSarSensorModel::getSensorPosition(const csm::ImageCoord& imagePt) const
 {
-  return csm::EcefCoord(0.0, 0.0, 0.0);
+  double time = getImageTime(imagePt);
+  return getSensorPosition(time);
 }
 
 csm::EcefCoord UsgsAstroSarSensorModel::getSensorPosition(double time) const
 {
-  return csm::EcefCoord(0.0, 0.0, 0.0);
+  csm::EcefVector sensorVector = getSpacecraftPosition(time);
+  return csm::EcefCoord(sensorVector.x, sensorVector.y, sensorVector.z);
 }
 
 csm::EcefVector UsgsAstroSarSensorModel::getSensorVelocity(const csm::ImageCoord& imagePt) const
 {
-  return csm::EcefVector(0.0, 0.0, 0.0);
+  double time = getImageTime(imagePt);
+  return getSensorVelocity(time);
 }
 
 csm::EcefVector UsgsAstroSarSensorModel::getSensorVelocity(double time) const
 {
-  return csm::EcefVector(0.0, 0.0, 0.0);
+  int numVelocities = m_velocities.size();
+  csm::EcefVector spacecraftVelocity = csm::EcefVector();
+
+  // If there are multiple positions, use Lagrange interpolation
+  if ((numVelocities/3) > 1) {
+    double velocity[3];
+    lagrangeInterp(numVelocities/3, &m_velocities[0], m_t0Ephem, m_dtEphem,
+                   time, 3, 8, velocity);
+    spacecraftVelocity.x = velocity[0];
+    spacecraftVelocity.y = velocity[1];
+    spacecraftVelocity.z = velocity[2];
+  }
+  else {
+    spacecraftVelocity.x = m_velocities[0];
+    spacecraftVelocity.y = m_velocities[1];
+    spacecraftVelocity.z = m_velocities[2];
+  }
+  return spacecraftVelocity;
 }
 
 csm::RasterGM::SensorPartials UsgsAstroSarSensorModel::computeSensorPartials(
@@ -823,6 +967,32 @@ void UsgsAstroSarSensorModel::setEllipsoid(const csm::Ellipsoid &ellipsoid)
    m_minorAxis = ellipsoid.getSemiMinorRadius();
 }
 
+void UsgsAstroSarSensorModel::determineSensorCovarianceInImageSpace(
+    csm::EcefCoord &gp,
+    double         sensor_cov[4] ) const
+{
+  int i, j, totalAdjParams;
+  totalAdjParams = getNumParameters();
+
+  std::vector<csm::RasterGM::SensorPartials> sensor_partials = computeAllSensorPartials(gp);
+
+  sensor_cov[0] = 0.0;
+  sensor_cov[1] = 0.0;
+  sensor_cov[2] = 0.0;
+  sensor_cov[3] = 0.0;
+
+  for (i = 0; i < totalAdjParams; i++)
+    {
+    for (j = 0; j < totalAdjParams; j++)
+    {
+      sensor_cov[0] += sensor_partials[i].first  * getParameterCovariance(i, j) * sensor_partials[j].first;
+      sensor_cov[1] += sensor_partials[i].second * getParameterCovariance(i, j) * sensor_partials[j].first;
+      sensor_cov[2] += sensor_partials[i].first  * getParameterCovariance(i, j) * sensor_partials[j].second;
+      sensor_cov[3] += sensor_partials[i].second * getParameterCovariance(i, j) * sensor_partials[j].second;
+    }
+  }
+}
+
 csm::EcefVector UsgsAstroSarSensorModel::getSpacecraftPosition(double time) const{
   int numPositions = m_positions.size();
   csm::EcefVector spacecraftPosition = csm::EcefVector();
@@ -845,27 +1015,6 @@ csm::EcefVector UsgsAstroSarSensorModel::getSpacecraftPosition(double time) cons
   return spacecraftPosition;
 }
 
-csm::EcefVector UsgsAstroSarSensorModel::getSpacecraftVelocity(double time) const {
-  int numVelocities = m_velocities.size();
-  csm::EcefVector spacecraftVelocity = csm::EcefVector();
-
-  // If there are multiple positions, use Lagrange interpolation
-  if ((numVelocities/3) > 1) {
-    double velocity[3];
-    lagrangeInterp(numVelocities/3, &m_velocities[0], m_t0Ephem, m_dtEphem,
-                   time, 3, 8, velocity);
-    spacecraftVelocity.x = velocity[0];
-    spacecraftVelocity.y = velocity[1];
-    spacecraftVelocity.z = velocity[2];
-  }
-  else {
-    spacecraftVelocity.x = m_velocities[0];
-    spacecraftVelocity.y = m_velocities[1];
-    spacecraftVelocity.z = m_velocities[2];
-  }
-  return spacecraftVelocity;
-}
-
 
 std::vector<double> UsgsAstroSarSensorModel::getRangeCoefficients(double time) const {
   int numCoeffs = m_scaleConversionCoefficients.size();
@@ -890,3 +1039,38 @@ std::vector<double> UsgsAstroSarSensorModel::getRangeCoefficients(double time) c
   }
   return interpCoeffs;
 }
+
+csm::EcefVector UsgsAstroSarSensorModel::getSunPosition(const double imageTime) const
+{
+
+  int numSunPositions = m_sunPosition.size();
+  int numSunVelocities = m_sunVelocity.size();
+  csm::EcefVector sunPosition = csm::EcefVector();
+
+  // If there are multiple positions, use Lagrange interpolation
+  if ((numSunPositions/3) > 1) {
+    double sunPos[3];
+    double endTime = m_t0Ephem + m_nLines * m_exposureDuration;
+    double sun_dtEphem = (endTime - m_t0Ephem) / (numSunPositions/3);
+    lagrangeInterp(numSunPositions/3, &m_sunPosition[0], m_t0Ephem, sun_dtEphem,
+                   imageTime, 3, 8, sunPos);
+    sunPosition.x = sunPos[0];
+    sunPosition.y = sunPos[1];
+    sunPosition.z = sunPos[2];
+  }
+  else if ((numSunVelocities/3) >= 1){
+    // If there is one position triple with at least one velocity triple
+    //  then the illumination direction is calculated via linear extrapolation.
+      sunPosition.x = (imageTime * m_sunVelocity[0] + m_sunPosition[0]);
+      sunPosition.y = (imageTime * m_sunVelocity[1] + m_sunPosition[1]);
+      sunPosition.z = (imageTime * m_sunVelocity[2] + m_sunPosition[2]);
+  }
+  else {
+    // If there is one position triple with no velocity triple, then the
+    //  illumination direction is the difference of the original vectors.
+      sunPosition.x = m_sunPosition[0];
+      sunPosition.y = m_sunPosition[1];
+      sunPosition.z = m_sunPosition[2];
+  }
+  return sunPosition;
+}

tests/SarTests.cpp

@@ -68,7 +68,7 @@ TEST_F(SarSensorModel, spacecraftPosition) {
 }
 
 TEST_F(SarSensorModel, spacecraftVelocity) {
-  csm::EcefVector velocity = sensorModel->getSpacecraftVelocity(-0.0025);
+  csm::EcefVector velocity = sensorModel->getSensorVelocity(-0.0025);
   EXPECT_NEAR(velocity.x, 0.00000000e+00, 1e-8);
   EXPECT_NEAR(velocity.y, 0.00000000e+00, 1e-8);
   EXPECT_NEAR(velocity.z, -3.73740000e+06, 1e-8);