From dedf7a251c09628bebcebdacfae6fde6b0d7f073 Mon Sep 17 00:00:00 2001
From: Kristin <kberry@usgs.gov>
Date: Tue, 21 Jul 2020 16:00:06 -0700
Subject: [PATCH] Added adjustable parameters. (#288)
* Non-compiling rough-draft of groundToImage and helper functions
* Add adjustable parameters
* Add adj to functions
* Update based on comments, add rest of structure needed for adjustable paramters, added computeSensorParitals
* Update based on feedback and problems from merging with dev
---
include/usgscsm/UsgsAstroSarSensorModel.h | 35 ++--
src/UsgsAstroSarSensorModel.cpp | 198 +++++++++++++++-------
src/Utilities.cpp | 1 -
tests/SarTests.cpp | 15 ++
4 files changed, 172 insertions(+), 77 deletions(-)
diff --git a/include/usgscsm/UsgsAstroSarSensorModel.h b/include/usgscsm/UsgsAstroSarSensorModel.h
index 17e27db..eb9d27c 100644
--- a/include/usgscsm/UsgsAstroSarSensorModel.h
+++ b/include/usgscsm/UsgsAstroSarSensorModel.h
@@ -35,6 +35,13 @@ class UsgsAstroSarSensorModel : public csm::RasterGM, virtual public csm::Settab
double* achievedPrecision = NULL,
csm::WarningList* warnings = NULL) const;
+ virtual csm::ImageCoord groundToImage(
+ const csm::EcefCoord& groundPt,
+ const std::vector<double> adjustments,
+ double desired_precision = 0.001,
+ double* achieved_precision = NULL,
+ csm::WarningList* warnings = NULL) const;
+
virtual csm::ImageCoordCovar groundToImage(
const csm::EcefCoordCovar& groundPt,
double desiredPrecision = 0.001,
@@ -81,14 +88,24 @@ class UsgsAstroSarSensorModel : public csm::RasterGM, virtual public csm::Settab
virtual double getImageTime(const csm::ImageCoord& imagePt) const;
+ virtual csm::EcefVector getSpacecraftPosition(double time) const;
+
+ virtual csm::EcefVector getAdjustedSpacecraftPosition(double time, std::vector<double> adj) const;
+
virtual csm::EcefCoord getSensorPosition(const csm::ImageCoord& imagePt) const;
virtual csm::EcefCoord getSensorPosition(double time) const;
+ virtual csm::EcefCoord getAdjustedSensorPosition(double time,
+ std::vector<double> adjustments) const;
+
virtual csm::EcefVector getSensorVelocity(const csm::ImageCoord& imagePt) const;
virtual csm::EcefVector getSensorVelocity(double time) const;
+ virtual csm::EcefVector getAdjustedSensorVelocity(double time,
+ std::vector<double> adjustments) const;
+
virtual csm::RasterGM::SensorPartials computeSensorPartials(
int index,
const csm::EcefCoord& groundPt,
@@ -104,13 +121,6 @@ class UsgsAstroSarSensorModel : public csm::RasterGM, virtual public csm::Settab
double* achievedPrecision = NULL,
csm::WarningList* warnings = NULL) const;
- virtual std::vector<csm::RasterGM::SensorPartials> computeAllSensorPartials(
- const csm::EcefCoord& groundPt,
- csm::param::Set pSet = csm::param::VALID,
- double desiredPrecision = 0.001,
- double* achievedPrecision = NULL,
- csm::WarningList* warnings = NULL) const;
-
virtual std::vector<double> computeGroundPartials(const csm::EcefCoord& groundPt) const;
virtual const csm::CorrelationModel& getCorrelationModel() const;
@@ -202,20 +212,18 @@ class UsgsAstroSarSensorModel : public csm::RasterGM, virtual public csm::Settab
////////////////////
void determineSensorCovarianceInImageSpace(
csm::EcefCoord &gp,
- double sensor_cov[4]) const;
- double dopplerShift(csm::EcefCoord groundPt, double tolerance) const;
+ double sensor_cov[4]) const;
+ double dopplerShift(csm::EcefCoord groundPt, double tolerance, std::vector<double> adj) const;
- double slantRange(csm::EcefCoord surfPt, double time) const;
+ double slantRange(csm::EcefCoord surfPt, double time, std::vector<double> adj) const;
double slantRangeToGroundRange(const csm::EcefCoord& groundPt, double time, double slantRange, double tolerance) const;
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;
+ double getValue(int index, const std::vector<double> &adjustments) const;
////////////////////////////
// Model static variables //
@@ -266,6 +274,7 @@ class UsgsAstroSarSensorModel : public csm::RasterGM, virtual public csm::Settab
std::vector<double> m_sunVelocity;
double m_wavelength;
LookDirection m_lookDirection;
+ std::vector<double> m_noAdjustments;
std::shared_ptr<spdlog::logger> m_logger = spdlog::get("usgscsm_logger");
};
diff --git a/src/UsgsAstroSarSensorModel.cpp b/src/UsgsAstroSarSensorModel.cpp
index 48ae733..0cb53e4 100644
--- a/src/UsgsAstroSarSensorModel.cpp
+++ b/src/UsgsAstroSarSensorModel.cpp
@@ -17,12 +17,12 @@ const string UsgsAstroSarSensorModel::_SENSOR_MODEL_NAME = "USGS_ASTRO_SAR_SENSO
const int UsgsAstroSarSensorModel::NUM_PARAMETERS = 6;
const string UsgsAstroSarSensorModel::PARAMETER_NAME[] =
{
- "IT Pos. Bias ", // 0
- "CT Pos. Bias ", // 1
- "Rad Pos. Bias ", // 2
- "X Vel. Bias ", // 3
- "Y Vel. Bias ", // 4
- "Z Vel. Bias " // 5
+ "X Pos. Bias ", // 0
+ "Y Pos. Bias ", // 1
+ "Z Pos. Bias ", // 2
+ "X Vel. Bias ", // 3
+ "Y Vel. Bias ", // 4
+ "Z Vel. Bias " // 5
};
const int UsgsAstroSarSensorModel::NUM_PARAM_TYPES = 4;
@@ -223,6 +223,7 @@ void UsgsAstroSarSensorModel::reset() {
m_sunPosition.clear();
m_sunVelocity.clear();
m_wavelength = 0;
+ m_noAdjustments = std::vector<double>(NUM_PARAMETERS,0.0);
}
void UsgsAstroSarSensorModel::replaceModelState(const string& argState){
@@ -355,15 +356,32 @@ csm::ImageCoord UsgsAstroSarSensorModel::groundToImage(
double* achievedPrecision,
csm::WarningList* warnings) const {
- MESSAGE_LOG("Computing groundToImage(ImageCoord) for {}, {}, {}, with desired precision {}",
+ MESSAGE_LOG("Computing groundToImage(ImageCoord) for {}, {}, {}, with desired precision {}"
+ "No adjustments.",
+ groundPt.x, groundPt.y, groundPt.z, desiredPrecision);
+
+ csm::ImageCoord imagePt = groundToImage(groundPt, m_noAdjustments, desiredPrecision, achievedPrecision, warnings);
+ return imagePt;
+}
+
+csm::ImageCoord UsgsAstroSarSensorModel::groundToImage(
+ const csm::EcefCoord& groundPt,
+ const std::vector<double> adj,
+ double desiredPrecision,
+ double* achievedPrecision,
+ csm::WarningList* warnings) const {
+
+ MESSAGE_LOG("Computing groundToImage(ImageCoord) for {}, {}, {}, with desired precision {}, and "
+ "adjustments.",
groundPt.x, groundPt.y, groundPt.z, desiredPrecision);
// Find time of closest approach to groundPt and the corresponding slant range by finding
// the root of the doppler shift frequency
try {
double timeTolerance = m_exposureDuration * desiredPrecision / 2.0;
- double time = dopplerShift(groundPt, timeTolerance);
- double slantRangeValue = slantRange(groundPt, time);
+
+ double time = dopplerShift(groundPt, timeTolerance, adj);
+ double slantRangeValue = slantRange(groundPt, time, adj);
// Find the ground range, based on the ground-range-to-slant-range polynomial defined by the
// range coefficient set, with a time closest to the calculated time of closest approach
@@ -373,7 +391,8 @@ csm::ImageCoord UsgsAstroSarSensorModel::groundToImage(
double line = (time - m_startingEphemerisTime) / m_exposureDuration + 0.5;
double sample = groundRange / m_scaledPixelWidth;
return csm::ImageCoord(line, sample);
- } catch (std::exception& error) {
+ }
+ catch (std::exception& error) {
std::string message = "Could not calculate groundToImage, with error [";
message += error.what();
message += "]";
@@ -382,16 +401,18 @@ csm::ImageCoord UsgsAstroSarSensorModel::groundToImage(
}
}
+
// Calculate the root
double UsgsAstroSarSensorModel::dopplerShift(
csm::EcefCoord groundPt,
- double tolerance) const {
- MESSAGE_LOG("Calculating doppler shift with: {}, {}, {}, and tolerance {}.",
- groundPt.x, groundPt.y, groundPt.z, tolerance);
+ double tolerance,
+ const std::vector<double> adj) const {
+ MESSAGE_LOG("Calculating doppler shift with: {}, {}, {}, and tolerance {}.",
+ groundPt.x, groundPt.y, groundPt.z, tolerance);
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 = getSensorVelocity(time);
+ std::function<double(double)> dopplerShiftFunction = [this, groundVec, adj](double time) {
+ csm::EcefVector spacecraftPosition = getAdjustedSpacecraftPosition(time, adj);
+ csm::EcefVector spacecraftVelocity = getAdjustedSensorVelocity(time, adj);
csm::EcefVector lookVector = spacecraftPosition - groundVec;
double slantRange = norm(lookVector);
@@ -403,16 +424,17 @@ double UsgsAstroSarSensorModel::dopplerShift(
// Do root-finding for "dopplerShift"
double timePadding = m_exposureDuration * m_nLines * 0.25;
- return brentRoot(m_startingEphemerisTime - timePadding, m_endingEphemerisTime + timePadding, dopplerShiftFunction, tolerance);
+ return brentRoot(m_startingEphemerisTime - timePadding, m_endingEphemerisTime + timePadding,
+ dopplerShiftFunction, tolerance);
}
double UsgsAstroSarSensorModel::slantRange(csm::EcefCoord surfPt,
- double time) const {
+ double time, std::vector<double> adj) const {
MESSAGE_LOG("Calculating slant range with: {}, {}, {}, and time {}.",
surfPt.x, surfPt.y, surfPt.z, time);
csm::EcefVector surfVec(surfPt.x ,surfPt.y, surfPt.z);
- csm::EcefVector spacecraftPosition = getSpacecraftPosition(time);
+ csm::EcefVector spacecraftPosition = getAdjustedSpacecraftPosition(time, adj);
return norm(spacecraftPosition - surfVec);
}
@@ -533,7 +555,6 @@ csm::EcefCoord UsgsAstroSarSensorModel::imageToGround(
double nadirComp = dot(spacecraftPosition, tHat);
double inTrackComp = dot(spacecraftPosition, vHat);
- // Compute the substituted values
// Iterate to find proper radius value
double pointRadius = m_majorAxis + height;
double radiusSqr;
@@ -745,26 +766,79 @@ double UsgsAstroSarSensorModel::getImageTime(const csm::ImageCoord& imagePt) con
return m_startingEphemerisTime + (imagePt.line - 0.5) * m_exposureDuration;
}
+csm::EcefVector UsgsAstroSarSensorModel::getSpacecraftPosition(double time) const {
+ MESSAGE_LOG("getSpacecraftPosition at {} without adjustments", time)
+ csm::EcefCoord spacecraftPosition = getSensorPosition(time);
+ return csm::EcefVector(spacecraftPosition.x, spacecraftPosition.y, spacecraftPosition.z);
+}
+
+csm::EcefVector UsgsAstroSarSensorModel::getAdjustedSpacecraftPosition(double time, std::vector<double> adj)
+const {
+ MESSAGE_LOG("getSpacecraftPosition at {} with adjustments", time)
+ csm::EcefCoord spacecraftPosition = getAdjustedSensorPosition(time, adj);
+ return csm::EcefVector(spacecraftPosition.x, spacecraftPosition.y, spacecraftPosition.z);
+}
+
+csm::EcefCoord UsgsAstroSarSensorModel::getSensorPosition(double time) const
+{
+ MESSAGE_LOG("getSensorPosition at {}.", time)
+ csm::EcefCoord sensorPosition = getAdjustedSensorPosition(time, m_noAdjustments);
+ return sensorPosition;
+}
+
+
csm::EcefCoord UsgsAstroSarSensorModel::getSensorPosition(const csm::ImageCoord& imagePt) const
{
+ MESSAGE_LOG("getSensorPosition at {}, {}.", imagePt.samp, imagePt.line);
double time = getImageTime(imagePt);
return getSensorPosition(time);
}
-csm::EcefCoord UsgsAstroSarSensorModel::getSensorPosition(double time) const
+
+csm::EcefCoord UsgsAstroSarSensorModel::getAdjustedSensorPosition(double time,
+ std::vector<double> adj) const
{
- csm::EcefVector sensorVector = getSpacecraftPosition(time);
- return csm::EcefCoord(sensorVector.x, sensorVector.y, sensorVector.z);
+ MESSAGE_LOG("getSensorPosition at {}. With adjustments: {}.", time);
+ int numPositions = m_positions.size();
+ csm::EcefVector spacecraftPosition = csm::EcefVector();
+
+ // If there are multiple positions, use Lagrange interpolation
+ if ((numPositions/3) > 1) {
+ double position[3];
+ lagrangeInterp(numPositions/3, &m_positions[0], m_t0Ephem, m_dtEphem,
+ time, 3, 8, position);
+ spacecraftPosition.x = position[0] + getValue(0, adj);
+ spacecraftPosition.y = position[1] + getValue(1, adj);
+ spacecraftPosition.z = position[2] + getValue(2, adj);
+ }
+ else {
+ spacecraftPosition.x = m_positions[0] + getValue(0, adj);
+ spacecraftPosition.y = m_positions[1] + getValue(1, adj);
+ spacecraftPosition.z = m_positions[2] + getValue(2, adj);
+ }
+ return csm::EcefCoord(spacecraftPosition.x, spacecraftPosition.y, spacecraftPosition.z);
}
+
csm::EcefVector UsgsAstroSarSensorModel::getSensorVelocity(const csm::ImageCoord& imagePt) const
{
+ MESSAGE_LOG("getSensorVelocity at {}, {}. No adjustments.", imagePt.samp, imagePt.line);
double time = getImageTime(imagePt);
return getSensorVelocity(time);
}
csm::EcefVector UsgsAstroSarSensorModel::getSensorVelocity(double time) const
{
+ MESSAGE_LOG("getSensorVelocity at {}. Without adjustments.", time);
+ csm::EcefVector spacecraftVelocity = getAdjustedSensorVelocity(time, m_noAdjustments);
+ return spacecraftVelocity;
+}
+
+csm::EcefVector UsgsAstroSarSensorModel::getAdjustedSensorVelocity(double time,
+ std::vector<double> adj) const
+{
+
+ MESSAGE_LOG("getSensorVelocity at {}. With adjustments.", time);
int numVelocities = m_velocities.size();
csm::EcefVector spacecraftVelocity = csm::EcefVector();
@@ -773,14 +847,14 @@ csm::EcefVector UsgsAstroSarSensorModel::getSensorVelocity(double time) const
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];
+ spacecraftVelocity.x = velocity[0] + getValue(3, adj);
+ spacecraftVelocity.y = velocity[1] + getValue(4, adj);
+ spacecraftVelocity.z = velocity[2] + getValue(5, adj);
}
else {
- spacecraftVelocity.x = m_velocities[0];
- spacecraftVelocity.y = m_velocities[1];
- spacecraftVelocity.z = m_velocities[2];
+ spacecraftVelocity.x = m_velocities[0] + getValue(3, adj);
+ spacecraftVelocity.y = m_velocities[1] + getValue(4, adj);
+ spacecraftVelocity.z = m_velocities[2] + getValue(5, adj);
}
return spacecraftVelocity;
}
@@ -792,7 +866,17 @@ csm::RasterGM::SensorPartials UsgsAstroSarSensorModel::computeSensorPartials(
double* achievedPrecision,
csm::WarningList* warnings) const
{
- return csm::RasterGM::SensorPartials(0.0, 0.0);
+
+ MESSAGE_LOG("Calculating computeSensorPartials for ground point {}, {}, {} with desired precision {}",
+ groundPt.x, groundPt.y, groundPt.z, desiredPrecision)
+
+ // Compute image coordinate first
+ csm::ImageCoord imgPt = groundToImage(
+ groundPt, desiredPrecision, achievedPrecision);
+
+ // Call overloaded function
+ return computeSensorPartials(
+ index, imgPt, groundPt, desiredPrecision, achievedPrecision, warnings);
}
csm::RasterGM::SensorPartials UsgsAstroSarSensorModel::computeSensorPartials(
@@ -803,19 +887,26 @@ csm::RasterGM::SensorPartials UsgsAstroSarSensorModel::computeSensorPartials(
double* achievedPrecision,
csm::WarningList* warnings) const
{
- return csm::RasterGM::SensorPartials(0.0, 0.0);
-}
+ MESSAGE_LOG("Calculating computeSensorPartials (with image points {}, {}) for ground point {}, {}, "
+ "{} with desired precision {}",
+ imagePt.line, imagePt.samp, groundPt.x, groundPt.y, groundPt.z, desiredPrecision)
-vector<csm::RasterGM::SensorPartials> UsgsAstroSarSensorModel::computeAllSensorPartials(
- const csm::EcefCoord& groundPt,
- csm::param::Set pSet,
- double desiredPrecision,
- double* achievedPrecision,
- csm::WarningList* warnings) const
-{
- return vector<csm::RasterGM::SensorPartials>(NUM_PARAMETERS, csm::RasterGM::SensorPartials(0.0, 0.0));
+ // Compute numerical partials wrt specific parameter
+
+ const double DELTA = m_scaledPixelWidth;
+ std::vector<double> adj(UsgsAstroSarSensorModel::NUM_PARAMETERS, 0.0);
+ adj[index] = DELTA;
+
+ csm::ImageCoord adjImgPt = groundToImage(
+ groundPt, adj, desiredPrecision, achievedPrecision, warnings);
+
+ double linePartial = (adjImgPt.line - imagePt.line) / DELTA;
+ double samplePartial = (adjImgPt.samp - imagePt.samp) / DELTA;
+
+ return csm::RasterGM::SensorPartials(linePartial, samplePartial);
}
+
vector<double> UsgsAstroSarSensorModel::computeGroundPartials(const csm::EcefCoord& groundPt) const
{
double GND_DELTA = m_scaledPixelWidth;
@@ -1097,28 +1188,6 @@ void UsgsAstroSarSensorModel::determineSensorCovarianceInImageSpace(
}
}
-csm::EcefVector UsgsAstroSarSensorModel::getSpacecraftPosition(double time) const{
- int numPositions = m_positions.size();
- csm::EcefVector spacecraftPosition = csm::EcefVector();
-
- // If there are multiple positions, use Lagrange interpolation
- if ((numPositions/3) > 1) {
- double position[3];
- lagrangeInterp(numPositions/3, &m_positions[0], m_t0Ephem, m_dtEphem,
- time, 3, 8, position);
- spacecraftPosition.x = position[0];
- spacecraftPosition.y = position[1];
- spacecraftPosition.z = position[2];
- }
- else {
- spacecraftPosition.x = m_positions[0];
- spacecraftPosition.y = m_positions[1];
- spacecraftPosition.z = m_positions[2];
- }
- // Can add third case if need be, but seems unlikely to come up for SAR
- return spacecraftPosition;
-}
-
std::vector<double> UsgsAstroSarSensorModel::getRangeCoefficients(double time) const {
int numCoeffs = m_scaleConversionCoefficients.size();
@@ -1146,7 +1215,6 @@ std::vector<double> UsgsAstroSarSensorModel::getRangeCoefficients(double time) c
csm::EcefVector UsgsAstroSarSensorModel::getSunPosition(const double imageTime) const
{
-
int numSunPositions = m_sunPosition.size();
int numSunVelocities = m_sunVelocity.size();
csm::EcefVector sunPosition = csm::EcefVector();
@@ -1178,3 +1246,7 @@ csm::EcefVector UsgsAstroSarSensorModel::getSunPosition(const double imageTime)
}
return sunPosition;
}
+
+double UsgsAstroSarSensorModel::getValue(int index, const std::vector<double> &adjustments) const {
+ return m_currentParameterValue[index] + adjustments[index];
+}
diff --git a/src/Utilities.cpp b/src/Utilities.cpp
index e452881..36e2632 100644
--- a/src/Utilities.cpp
+++ b/src/Utilities.cpp
@@ -462,7 +462,6 @@ double computeEllipsoidElevation(
return h;
}
-
csm::EcefVector operator*(double scalar, const csm::EcefVector &vec)
{
return csm::EcefVector(
diff --git a/tests/SarTests.cpp b/tests/SarTests.cpp
index 2efd10f..0209202 100644
--- a/tests/SarTests.cpp
+++ b/tests/SarTests.cpp
@@ -114,3 +114,18 @@ TEST_F(SarSensorModel, imageToRemoteImagingLocus) {
EXPECT_NEAR(locus.direction.y, -1.0, 1e-8);
EXPECT_NEAR(locus.direction.z, 0.0, 1e-8);
}
+
+TEST_F(SarSensorModel, adjustedPositionVelocity) {
+ std::vector<double> adjustments = {1000000.0, 0.2, -10.0, -20.0, 0.5, 2000000.0};
+ csm::EcefCoord sensorPosition = sensorModel->getSensorPosition(0.0);
+ csm::EcefVector sensorVelocity = sensorModel->getSensorVelocity(0.0);
+ csm::EcefCoord adjPosition = sensorModel->getAdjustedSensorPosition(0.0, adjustments);
+ csm::EcefVector adjVelocity = sensorModel->getAdjustedSensorVelocity(0.0, adjustments);
+
+ EXPECT_NEAR(adjPosition.x, sensorPosition.x + adjustments[0], 1e-2);
+ EXPECT_NEAR(adjPosition.y, sensorPosition.y + adjustments[1], 1e-2);
+ EXPECT_NEAR(adjPosition.z, sensorPosition.z + adjustments[2], 1e-2);
+ EXPECT_NEAR(adjVelocity.x, sensorVelocity.x + adjustments[3], 1e-8);
+ EXPECT_NEAR(adjVelocity.y, sensorVelocity.y + adjustments[4], 1e-8);
+ EXPECT_NEAR(adjVelocity.z, sensorVelocity.z + adjustments[5], 1e-2);
+}
--
GitLab