From 70eda0af910b2733dcf904bb859b7938e5a50716 Mon Sep 17 00:00:00 2001
From: Jesse Mapel <jmapel@usgs.gov>
Date: Tue, 28 Apr 2020 15:49:47 -0700
Subject: [PATCH] More stubs (#285)
* Added actual methods
* Made default build type release
* Position and Velocity API done
* Fixes from review
---
CMakeLists.txt | 8 +
include/usgscsm/UsgsAstroSarSensorModel.h | 8 +-
src/UsgsAstroSarSensorModel.cpp | 268 ++++++++++++++++++----
tests/SarTests.cpp | 2 +-
4 files changed, 242 insertions(+), 44 deletions(-)
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 98a82ae..94cacb3 100644
--- a/CMakeLists.txt
+++ b/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)
diff --git a/include/usgscsm/UsgsAstroSarSensorModel.h b/include/usgscsm/UsgsAstroSarSensorModel.h
index 118f9f4..af79e94 100644
--- a/include/usgscsm/UsgsAstroSarSensorModel.h
+++ b/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;
////////////////////////////
diff --git a/src/UsgsAstroSarSensorModel.cpp b/src/UsgsAstroSarSensorModel.cpp
index c6c2bf6..7209180 100644
--- a/src/UsgsAstroSarSensorModel.cpp
+++ b/src/UsgsAstroSarSensorModel.cpp
@@ -231,12 +231,12 @@ void UsgsAstroSarSensorModel::replaceModelState(const string& argState)
m_nSamples = stateJson["m_nSamples"];
m_exposureDuration = stateJson["m_exposureDuration"];
m_scaledPixelWidth = stateJson["m_scaledPixelWidth"];
- std::string lookStr = stateJson["m_lookDirection"];
+ std::string lookStr = stateJson["m_lookDirection"];
if (lookStr.compare("right") == 0 ) {
- m_lookDirection = UsgsAstroSarSensorModel::RIGHT;
+ m_lookDirection = UsgsAstroSarSensorModel::RIGHT;
}
else if (lookStr.compare("left") == 0) {
- m_lookDirection = UsgsAstroSarSensorModel::LEFT;
+ m_lookDirection = UsgsAstroSarSensorModel::LEFT;
}
else {
std::string message = "Could not determine look direction from state";
@@ -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;
+}
diff --git a/tests/SarTests.cpp b/tests/SarTests.cpp
index 5cff6a0..a8279c7 100644
--- a/tests/SarTests.cpp
+++ b/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);
--
GitLab