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

More stubs (#285) 

* Added actual methods

* Made default build type release

* Position and Velocity API done

* Fixes from review
parent f40dd961

CMakeLists.txt

+8 −0
Original line number Diff line number Diff line
@@ -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

+7 −1
Original line number Diff line number Diff line
@@ -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

+226 −42
Original line number Diff line number Diff line
@@ -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

+1 −1
Original line number Diff line number Diff line
@@ -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);