Unverified Commit a53f9cfe authored by Jesse Mapel's avatar Jesse Mapel Committed by GitHub
Browse files

Adds SAR imaging loci functions and fixes imagetoground (#286) 

* Added ellipsoid image to ground

* Fixed loci functions

* Added look direction and loci tests

* Moved ellipsoid height to utilities
parent 87d32c56

include/usgscsm/UsgsAstroLsSensorModel.h

+0 −9
Original line number Diff line number Diff line
@@ -1018,15 +1018,6 @@ private: 
      double&       z,

      int&          mode ) const;



   // Computes the height above ellipsoid for an input ECF coordinate

   void computeElevation (

      const double& x,

      const double& y,

      const double& z,

      double&       height,

      double&       achieved_precision,

      const double& desired_precision) const;



   // determines the sensor velocity accounting for parameter adjustments.

   void getAdjSensorPosVel(

      const double& time,

include/usgscsm/UsgsAstroSarSensorModel.h

+2 −0
Original line number Diff line number Diff line
@@ -210,7 +210,9 @@ 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 getSunPosition(const double imageTime) const;



    std::vector<double> getRangeCoefficients(double time) const;



    ////////////////////////////

include/usgscsm/Utilities.h

+9 −0
Original line number Diff line number Diff line
@@ -85,6 +85,15 @@ double secantRoot( 
    double epsilon = 1e-10,

    int maxIterations = 30);



double computeEllipsoidElevation(

    double x,

    double y,

    double z,

    double semiMajor,

    double semiMinor,

    double desired_precision = 0.001,

    double* achieved_precision = nullptr);



// Vector operations

csm::EcefVector operator*(double scalar, const csm::EcefVector &vec);

csm::EcefVector operator*(const csm::EcefVector &vec, double scalar);

src/UsgsAstroLsSensorModel.cpp

+12 −79
Original line number Diff line number Diff line
@@ -1018,8 +1018,9 @@ csm::EcefLocus UsgsAstroLsSensorModel::imageToProximateImagingLocus( 
   double z = ground_pt.z;



   // Elevation at input ground point

   double height, aPrec;

   computeElevation(x, y, z, height, aPrec, desired_precision);

   double height = computeEllipsoidElevation(

         x, y, z,

         m_majorAxis, m_minorAxis, desired_precision);



   // Ground point on object ray with the same elevation

   csm::EcefCoord gp1 = imageToGround(
@@ -1050,8 +1051,9 @@ csm::EcefLocus UsgsAstroLsSensorModel::imageToProximateImagingLocus( 
   gp2.y = gp1.y + scale * dy2;

   gp2.z = gp1.z + scale * dz2;



   double hLocus;

   computeElevation(gp2.x, gp2.y, gp2.z, hLocus, aPrec, desired_precision);

   double hLocus = computeEllipsoidElevation(

         gp2.x, gp2.y, gp2.z,

         m_majorAxis, m_minorAxis, desired_precision);

   locus.point = imageToGround(

         image_pt, hLocus, desired_precision, achieved_precision, warnings);
@@ -2051,76 +2053,6 @@ void UsgsAstroLsSensorModel::lightAberrationCorr( 
                               "{} {} {}", dxl, dyl, dzl)

}



//***************************************************************************

// UsgsAstroLsSensorModel::computeElevation

//***************************************************************************

void UsgsAstroLsSensorModel::computeElevation(

   const double& x,

   const double& y,

   const double& z,

   double&       height,

   double&       achieved_precision,

   const double& desired_precision) const

{

   MESSAGE_LOG(m_logger, "Calculating computeElevation for {} {} {}"

                               "with desired precision {}",

                              x, y, z, desired_precision)

   // Compute elevation given xyz

   // Requires semi-major-axis and eccentricity-square

   const int MKTR = 10;

   double ecc_sqr = 1.0 - m_minorAxis * m_minorAxis / m_majorAxis / m_majorAxis;

   double ep2 = 1.0 - ecc_sqr;

   double d2 = x * x + y * y;

   double d = sqrt(d2);

   double h = 0.0;

   int ktr = 0;

   double hPrev, r;



   // Suited for points near equator

   if (d >= z)

   {

      double tt, zz, n;

      double tanPhi = z / d;

      do

      {

         hPrev = h;

         tt = tanPhi * tanPhi;

         r = m_majorAxis / sqrt(1.0 + ep2 * tt);

         zz = z + r * ecc_sqr * tanPhi;

         n = r * sqrt(1.0 + tt);

         h = sqrt(d2 + zz * zz) - n;

         tanPhi = zz / d;

         ktr++;

      } while (MKTR > ktr && fabs(h - hPrev) > desired_precision);

      MESSAGE_LOG(m_logger, "computeElevation: point is near equator")

   }



   // Suited for points near the poles

   else

   {

      double cc, dd, nn;

      double cotPhi = d / z;

      do

      {

         hPrev = h;

         cc = cotPhi * cotPhi;

         r = m_majorAxis / sqrt(ep2 + cc);

         dd = d - r * ecc_sqr * cotPhi;

         nn = r * sqrt(1.0 + cc) * ep2;

         h = sqrt(dd * dd + z * z) - nn;

         cotPhi = dd / z;

         ktr++;

      } while (MKTR > ktr && fabs(h - hPrev) > desired_precision);

      MESSAGE_LOG(m_logger, "computeElevation: point is near poles")

   }



   height = h;

   achieved_precision = fabs(h - hPrev);

   MESSAGE_LOG(m_logger, "computeElevation: height {} with achieved"

                               "precision of {}",

                                height, achieved_precision)

}



//***************************************************************************

// UsgsAstroLsSensorModel::losEllipsoidIntersect

//**************************************************************************
@@ -2173,7 +2105,7 @@ void UsgsAstroLsSensorModel::losEllipsoidIntersect( 
   }

   double scale, scale1, h, slope;

   double sprev, hprev;

   double aPrec, sTerm;

   double sTerm;

   int ktr = 0;



   // Compute ground point vector
@@ -2187,7 +2119,7 @@ void UsgsAstroLsSensorModel::losEllipsoidIntersect( 
   x = xc + scale * xl;

   y = yc + scale * yl;

   z = zc + scale * zl;

   computeElevation(x, y, z, h, aPrec, desired_precision);

   h = computeEllipsoidElevation(x, y, z, m_majorAxis, m_minorAxis, desired_precision);

   slope = -1;



   achieved_precision = fabs(height - h);
@@ -2515,9 +2447,10 @@ void UsgsAstroLsSensorModel::setLinearApproximation() 


  csm::EcefCoord refPt = getReferencePoint();



  double height, aPrec;

  double desired_precision = 0.01;

  computeElevation(refPt.x, refPt.y, refPt.z, height, aPrec, desired_precision);

  double height = computeEllipsoidElevation(

        refPt.x, refPt.y, refPt.z,

        m_majorAxis, m_minorAxis, desired_precision);

  if (std::isnan(height))

  {

    MESSAGE_LOG(m_logger, "setLinearApproximation: computeElevation of"

src/UsgsAstroSarSensorModel.cpp

+75 −11
Original line number Diff line number Diff line
@@ -513,14 +513,29 @@ csm::EcefCoord UsgsAstroSarSensorModel::imageToGround( 
  double inTrackComp = dot(spacecraftPosition, vHat);



  // Compute the substituted values

  // TODO properly handle ellipsoid radii

  double radiusSqr = m_majorAxis * m_majorAxis;

  // Iterate to find proper radius value

  double pointRadius = m_majorAxis + height;

  double radiusSqr;

  double pointHeight;

  csm::EcefVector groundVec;

  do {

    radiusSqr = pointRadius * pointRadius;

    double alpha = (radiusSqr - slantRange * slantRange - positionMag * positionMag) / (2 * nadirComp);

  // TODO use right/left look to determine +/-

  double beta = -sqrt(slantRange * slantRange - alpha * alpha);

  csm::EcefVector groundVec = alpha * tHat + beta * uHat + spacecraftPosition;

    double beta = sqrt(slantRange * slantRange - alpha * alpha);

    if (m_lookDirection == LEFT) {

      beta *= -1;

    }

    groundVec = alpha * tHat + beta * uHat + spacecraftPosition;

    pointHeight = computeEllipsoidElevation(

        groundVec.x, groundVec.y, groundVec.z,

        m_majorAxis, m_minorAxis);

    pointRadius -= (pointHeight - height);

  } while(fabs(pointHeight - height) > desiredPrecision);





  return csm::EcefCoord(groundVec.x, groundVec.y, groundVec.z);

  csm::EcefCoord groundPt(groundVec.x, groundVec.y, groundVec.z);



  return groundPt;

}



csm::EcefCoordCovar UsgsAstroSarSensorModel::imageToGround(
@@ -611,8 +626,33 @@ csm::EcefLocus UsgsAstroSarSensorModel::imageToProximateImagingLocus( 
    double* achievedPrecision,

    csm::WarningList* warnings) const

{

  return csm::EcefLocus(0.0, 0.0, 0.0,

                        0.0, 0.0, 0.0);

  // 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 sensor to ground point 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 groundVec(groundPt.x, groundPt.y, groundPt.z);

  csm::EcefVector lookVec = normalized(rejection(groundVec - spacecraftPosition, spacecraftVelocity));

  csm::EcefVector closestVec = spacecraftPosition + slantRange * lookVec;





  // 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);



  return csm::EcefLocus(closestVec.x, closestVec.y, closestVec.z,

                        tangent.x,    tangent.y,    tangent.z);

}



csm::EcefLocus UsgsAstroSarSensorModel::imageToRemoteImagingLocus(
@@ -621,8 +661,32 @@ csm::EcefLocus UsgsAstroSarSensorModel::imageToRemoteImagingLocus( 
    double* achievedPrecision,

    csm::WarningList* warnings) const

{

  return csm::EcefLocus(0.0, 0.0, 0.0,

                        0.0, 0.0, 0.0);

  // 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;





  // 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);



  return csm::EcefLocus(closestVec.x, closestVec.y, closestVec.z,

                        tangent.x,    tangent.y,    tangent.z);

}



csm::ImageCoord UsgsAstroSarSensorModel::getImageStart() const