diff --git a/include/usgscsm/UsgsAstroSarSensorModel.h b/include/usgscsm/UsgsAstroSarSensorModel.h index ed72d66f3d6dad260b126cbc644ced61614bbfe2..f7cba8dbec338f74739768d55c8b3522f0cdd5b6 100644 --- a/include/usgscsm/UsgsAstroSarSensorModel.h +++ b/include/usgscsm/UsgsAstroSarSensorModel.h @@ -191,6 +191,16 @@ class UsgsAstroSarSensorModel : public csm::RasterGM, virtual public csm::Settab virtual void setEllipsoid(const csm::Ellipsoid &ellipsoid); + double dopplerShift(csm::EcefCoord groundPt, double tolerance) const; + + double slantRange(csm::EcefCoord surfPt, double time) const; + + double slantRangeToGroundRange(const csm::EcefCoord& groundPt, double time, double slantRange, double tolerance) const; + + csm::EcefVector getSpacecraftPosition(double time) const; + csm::EcefVector getSpacecraftVelocity(double time) const; + std::vector<double> getRangeCoefficients(double time) const; + //////////////////////////// // Model static variables // //////////////////////////// @@ -216,6 +226,7 @@ class UsgsAstroSarSensorModel : public csm::RasterGM, virtual public csm::Settab double m_scaledPixelWidth; double m_startingEphemerisTime; double m_centerEphemerisTime; + double m_endingEphemerisTime; double m_majorAxis; double m_minorAxis; std::string m_platformIdentifier; @@ -228,6 +239,7 @@ class UsgsAstroSarSensorModel : public csm::RasterGM, virtual public csm::Settab double m_dtEphem; double m_t0Ephem; std::vector<double> m_scaleConversionCoefficients; + std::vector<double> m_scaleConversionTimes; std::vector<double> m_positions; std::vector<double> m_velocities; std::vector<double> m_currentParameterValue; @@ -236,6 +248,7 @@ class UsgsAstroSarSensorModel : public csm::RasterGM, virtual public csm::Settab std::vector<double> m_covariance; std::vector<double> m_sunPosition; std::vector<double> m_sunVelocity; + double m_wavelength; }; #endif diff --git a/include/usgscsm/Utilities.h b/include/usgscsm/Utilities.h index d7d89eec99e4384459cd4bd3365b0c4dd9ec67b8..9765b269d23594a5bdb188c6aa2334204e3ebc22 100644 --- a/include/usgscsm/Utilities.h +++ b/include/usgscsm/Utilities.h @@ -74,9 +74,16 @@ void lagrangeInterp ( double brentRoot( double lowerBound, double upperBound, - double (*func)(double), + std::function<double(double)> func, double epsilon = 1e-10); +double secantRoot( + double lowerBound, + double upperBound, + std::function<double(double)> func, + double epsilon = 1e-10, + int maxIterations = 30); + // Methods for checking/accessing the ISD double metric_conversion(double val, std::string from, std::string to="m"); @@ -89,12 +96,14 @@ int getTotalLines(nlohmann::json isd, csm::WarningList *list=nullptr); int getTotalSamples(nlohmann::json isd, csm::WarningList *list=nullptr); double getStartingTime(nlohmann::json isd, csm::WarningList *list=nullptr); double getCenterTime(nlohmann::json isd, csm::WarningList *list=nullptr); +double getEndingTime(nlohmann::json isd, csm::WarningList *list=nullptr); std::vector<double> getIntegrationStartLines(nlohmann::json isd, csm::WarningList *list=nullptr); std::vector<double> getIntegrationStartTimes(nlohmann::json isd, csm::WarningList *list=nullptr); std::vector<double> getIntegrationTimes(nlohmann::json isd, csm::WarningList *list=nullptr); double getExposureDuration(nlohmann::json isd, csm::WarningList *list=nullptr); double getScaledPixelWidth(nlohmann::json isd, csm::WarningList *list=nullptr); std::vector<double> getScaleConversionCoefficients(nlohmann::json isd, csm::WarningList *list=nullptr); +std::vector<double> getScaleConversionTimes(nlohmann::json isd, csm::WarningList *list=nullptr); int getSampleSumming(nlohmann::json isd, csm::WarningList *list=nullptr); int getLineSumming(nlohmann::json isd, csm::WarningList *list=nullptr); double getFocalLength(nlohmann::json isd, csm::WarningList *list=nullptr); @@ -117,5 +126,5 @@ std::vector<double> getSunVelocities(nlohmann::json isd, csm::WarningList *list= std::vector<double> getSensorPositions(nlohmann::json isd, csm::WarningList *list=nullptr); std::vector<double> getSensorVelocities(nlohmann::json isd, csm::WarningList *list=nullptr); std::vector<double> getSensorOrientations(nlohmann::json isd, csm::WarningList *list=nullptr); - +double getWavelength(nlohmann::json isd, csm::WarningList *list=nullptr); #endif diff --git a/src/UsgsAstroSarSensorModel.cpp b/src/UsgsAstroSarSensorModel.cpp index 6b8dd0894d909494802c1fc21b7ed7782b1cd3b4..b2782cfc2b77376d20940f72d979c20ba137ef1e 100644 --- a/src/UsgsAstroSarSensorModel.cpp +++ b/src/UsgsAstroSarSensorModel.cpp @@ -1,13 +1,18 @@ #include "UsgsAstroSarSensorModel.h" #include "Utilities.h" +#include <functional> +#include <iomanip> #include <string.h> +#include <cmath> #include <json/json.hpp> using json = nlohmann::json; using namespace std; +#define MESSAGE_LOG(logger, ...) if (logger) { logger->info(__VA_ARGS__); } + const string UsgsAstroSarSensorModel::_SENSOR_MODEL_NAME = "USGS_ASTRO_SAR_SENSOR_MODEL"; const int UsgsAstroSarSensorModel::NUM_PARAMETERS = 6; const string UsgsAstroSarSensorModel::PARAMETER_NAME[] = @@ -63,6 +68,7 @@ string UsgsAstroSarSensorModel::constructStateFromIsd( state["m_centerEphemerisTime"] = getCenterTime(isd, parsingWarnings); state["m_startingEphemerisTime"] = getStartingTime(isd, parsingWarnings); + state["m_endingEphemerisTime"] = getEndingTime(isd, parsingWarnings); state["m_exposureDuration"] = getExposureDuration(isd, parsingWarnings); @@ -108,6 +114,8 @@ string UsgsAstroSarSensorModel::constructStateFromIsd( // SAR specific values state["m_scaledPixelWidth"] = getScaledPixelWidth(isd, parsingWarnings); state["m_scaleConversionCoefficients"] = getScaleConversionCoefficients(isd, parsingWarnings); + state["m_scaleConversionTimes"] = getScaleConversionTimes(isd, parsingWarnings); + state["m_wavelength"] = getWavelength(isd, parsingWarnings); // Default to identity covariance state["m_covariance"] = @@ -120,10 +128,14 @@ string UsgsAstroSarSensorModel::constructStateFromIsd( if (warnings) { warnings->insert(warnings->end(), parsingWarnings->begin(), parsingWarnings->end()); } - delete parsingWarnings; + std::string message = "ISD is invalid for creating the sensor model with error ["; + csm::Warning warn = parsingWarnings->front(); + message += warn.getMessage(); + message += "]"; parsingWarnings = nullptr; + delete parsingWarnings; throw csm::Error(csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE, - "ISD is invalid for creating the sensor model.", + message, "UsgsAstroSarSensorModel::constructStateFromIsd"); } @@ -178,6 +190,7 @@ void UsgsAstroSarSensorModel::reset() m_scaledPixelWidth = 0; m_startingEphemerisTime = 0; m_centerEphemerisTime = 0; + m_endingEphemerisTime = 0; m_majorAxis = 0; m_minorAxis = 0; m_platformIdentifier = "Unknown"; @@ -189,6 +202,7 @@ void UsgsAstroSarSensorModel::reset() m_dtEphem = 0; m_t0Ephem = 0; m_scaleConversionCoefficients.clear(); + m_scaleConversionTimes.clear(); m_positions.clear(); m_velocities.clear(); m_currentParameterValue = vector<double>(NUM_PARAMETERS, 0.0); @@ -199,6 +213,7 @@ void UsgsAstroSarSensorModel::reset() m_covariance = vector<double>(NUM_PARAMETERS * NUM_PARAMETERS,0.0); m_sunPosition.clear(); m_sunVelocity.clear(); + m_wavelength = 0; } void UsgsAstroSarSensorModel::replaceModelState(const string& argState) @@ -214,8 +229,10 @@ void UsgsAstroSarSensorModel::replaceModelState(const string& argState) m_nSamples = stateJson["m_nSamples"]; m_exposureDuration = stateJson["m_exposureDuration"]; m_scaledPixelWidth = stateJson["m_scaledPixelWidth"]; + m_wavelength = stateJson["m_wavelength"]; m_startingEphemerisTime = stateJson["m_startingEphemerisTime"]; m_centerEphemerisTime = stateJson["m_centerEphemerisTime"]; + m_endingEphemerisTime = stateJson["m_endingEphemerisTime"]; m_majorAxis = stateJson["m_majorAxis"]; m_minorAxis = stateJson["m_minorAxis"]; m_platformIdentifier = stateJson["m_platformIdentifier"].get<string>(); @@ -225,6 +242,7 @@ void UsgsAstroSarSensorModel::replaceModelState(const string& argState) m_dtEphem = stateJson["m_dtEphem"]; m_t0Ephem = stateJson["m_t0Ephem"]; m_scaleConversionCoefficients = stateJson["m_scaleConversionCoefficients"].get<vector<double>>(); + m_scaleConversionTimes = stateJson["m_scaleConversionTimes"].get<vector<double>>(); m_positions = stateJson["m_positions"].get<vector<double>>(); m_velocities = stateJson["m_velocities"].get<vector<double>>(); m_currentParameterValue = stateJson["m_currentParameterValue"].get<vector<double>>(); @@ -255,6 +273,7 @@ string UsgsAstroSarSensorModel::getModelState() const state["m_sunVelocity"] = m_sunVelocity; state["m_centerEphemerisTime"] = m_centerEphemerisTime; state["m_startingEphemerisTime"] = m_startingEphemerisTime; + state["m_endingEphemerisTime"] = m_endingEphemerisTime; state["m_exposureDuration"] = m_exposureDuration; state["m_dtEphem"] = m_dtEphem; state["m_t0Ephem"] = m_t0Ephem; @@ -268,23 +287,117 @@ string UsgsAstroSarSensorModel::getModelState() const state["m_minElevation"] = m_minElevation; state["m_maxElevation"] = m_maxElevation; state["m_scaledPixelWidth"] = m_scaledPixelWidth; + state["m_wavelength"] = m_wavelength; state["m_scaleConversionCoefficients"] = m_scaleConversionCoefficients; + state["m_scaleConversionTimes"] = m_scaleConversionTimes; state["m_covariance"] = m_covariance; return state.dump(); } - csm::ImageCoord UsgsAstroSarSensorModel::groundToImage( const csm::EcefCoord& groundPt, double desiredPrecision, double* achievedPrecision, - csm::WarningList* warnings) const + csm::WarningList* warnings) const { + + //MESSAGE_LOG(this->m_logger, "Computing groundToImage(ImageCoord) for {}, {}, {}, with desired precision {}", + // 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); + + // 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 + double groundTolerance = m_scaledPixelWidth * desiredPrecision / 2.0; + double groundRange = slantRangeToGroundRange(groundPt, time, slantRangeValue, groundTolerance); + + double line = (time - m_startingEphemerisTime) / m_exposureDuration + 0.5; + double sample = groundRange / m_scaledPixelWidth; + return csm::ImageCoord(line, sample); + } catch (std::exception& error) { + std::string message = "Could not calculate groundToImage, with error ["; + message += error.what(); + message += "]"; + throw csm::Error(csm::Error::UNKNOWN_ERROR, message, "groundToImage"); + } +} + +// Calculate the root +double UsgsAstroSarSensorModel::dopplerShift( + csm::EcefCoord groundPt, + double tolerance) const { - return csm::ImageCoord(0.0, 0.0); + std::function<double(double)> dopplerShiftFunction = [this, groundPt](double time) { + csm::EcefVector spacecraftPosition = getSpacecraftPosition(time); + csm::EcefVector spacecraftVelocity = getSpacecraftVelocity(time); + double lookVector[3]; + + lookVector[0] = spacecraftPosition.x - groundPt.x; + lookVector[1] = spacecraftPosition.y - groundPt.y; + lookVector[2] = spacecraftPosition.z - groundPt.z; + + double slantRange = sqrt(pow(lookVector[0], 2) + pow(lookVector[1], 2) + pow(lookVector[2], 2)); + + + double dopplerShift = -2.0 * (lookVector[0]*spacecraftVelocity.x + lookVector[1]*spacecraftVelocity.y + + lookVector[2]*spacecraftVelocity.z)/(slantRange * m_wavelength); + + return dopplerShift; + }; + + // Do root-finding for "dopplerShift" + return brentRoot(m_startingEphemerisTime, m_endingEphemerisTime, dopplerShiftFunction, tolerance); +} + + +double UsgsAstroSarSensorModel::slantRange(csm::EcefCoord surfPt, + double time) const +{ + csm::EcefVector spacecraftPosition = getSpacecraftPosition(time); + double lookVector[3]; + + lookVector[0] = spacecraftPosition.x - surfPt.x; + lookVector[1] = spacecraftPosition.y - surfPt.y; + lookVector[2] = spacecraftPosition.z - surfPt.z; + return sqrt(pow(lookVector[0], 2) + pow(lookVector[1], 2) + pow(lookVector[2], 2)); } +double UsgsAstroSarSensorModel::slantRangeToGroundRange( + const csm::EcefCoord& groundPt, + double time, + double slantRange, + double tolerance) const +{ + std::vector<double> coeffs = getRangeCoefficients(time); + + // Calculates the ground range from the slant range. + std::function<double(double)> slantRangeToGroundRangeFunction = + [coeffs, slantRange](double groundRange){ + return slantRange - (coeffs[0] + groundRange * (coeffs[1] + groundRange * (coeffs[2] + groundRange * coeffs[3]))); + }; + + // Need to come up with an initial guess when solving for ground + // range given slant range. Compute the ground range at the + // near and far edges of the image by evaluating the sample-to- + // ground-range equation: groundRange=(sample-1)*scaled_pixel_width + // at the edges of the image. We also need to add some padding to + // allow for solving for coordinates that are slightly outside of + // the actual image area. Use sample=-0.25*image_samples and + // sample=1.25*image_samples. + double minGroundRangeGuess = (-0.25 * m_nSamples - 1.0) * m_scaledPixelWidth; + double maxGroundRangeGuess = (1.25 * m_nSamples - 1.0) * m_scaledPixelWidth; + + // Tolerance to 1/20th of a pixel for now. + return brentRoot(minGroundRangeGuess, maxGroundRangeGuess, slantRangeToGroundRangeFunction, tolerance); +} + + csm::ImageCoordCovar UsgsAstroSarSensorModel::groundToImage( const csm::EcefCoordCovar& groundPt, double desiredPrecision, @@ -657,3 +770,71 @@ void UsgsAstroSarSensorModel::setEllipsoid(const csm::Ellipsoid &ellipsoid) m_majorAxis = ellipsoid.getSemiMajorRadius(); m_minorAxis = ellipsoid.getSemiMinorRadius(); } + +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; +} + +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(); + std::vector<double> interpCoeffs; + + double endTime = m_scaleConversionTimes.back(); + if ((numCoeffs/4) > 1) { + double coefficients[4]; + double dtEphem = (endTime - m_scaleConversionTimes[0]) / (m_scaleConversionCoefficients.size()/4); + lagrangeInterp(m_scaleConversionCoefficients.size()/4, &m_scaleConversionCoefficients[0], m_scaleConversionTimes[0], dtEphem, + time, 4, 8, coefficients); + interpCoeffs.push_back(coefficients[0]); + interpCoeffs.push_back(coefficients[1]); + interpCoeffs.push_back(coefficients[2]); + interpCoeffs.push_back(coefficients[3]); + } + else { + interpCoeffs.push_back(m_scaleConversionCoefficients[0]); + interpCoeffs.push_back(m_scaleConversionCoefficients[1]); + interpCoeffs.push_back(m_scaleConversionCoefficients[2]); + interpCoeffs.push_back(m_scaleConversionCoefficients[3]); + } + return interpCoeffs; +} diff --git a/src/Utilities.cpp b/src/Utilities.cpp index 29963774c84f5329874345204425b706288525e1..a532b8b83a075973812220f78c6a59bb9febc1cb 100644 --- a/src/Utilities.cpp +++ b/src/Utilities.cpp @@ -280,14 +280,14 @@ void lagrangeInterp( double brentRoot( double lowerBound, double upperBound, - double (*func)(double), + std::function<double(double)> func, double epsilon) { double counterPoint = lowerBound; double currentPoint = upperBound; double counterFunc = func(counterPoint); double currentFunc = func(currentPoint); if (counterFunc * currentFunc > 0.0) { - throw std::invalid_argument("Function values at the boundaries have the same sign."); + throw std::invalid_argument("Function values at the boundaries have the same sign [brentRoot]."); } if (fabs(counterFunc) < fabs(currentFunc)) { std::swap(counterPoint, currentPoint); @@ -304,7 +304,7 @@ double brentRoot( do { // Inverse quadratic interpolation - if (counterFunc != previousFunc && counterFunc != currentFunc) { + if (counterFunc != previousFunc && counterFunc != currentFunc && currentFunc != previousFunc) { nextPoint = (counterPoint * currentFunc * previousFunc) / ((counterFunc - currentFunc) * (counterFunc - previousFunc)); nextPoint += (currentPoint * counterFunc * previousFunc) / ((currentFunc - counterFunc) * (currentFunc - previousFunc)); nextPoint += (previousPoint * currentFunc * counterFunc) / ((previousFunc - counterFunc) * (previousFunc - currentFunc)); @@ -345,6 +345,66 @@ double brentRoot( return nextPoint; } +double secantRoot(double lowerBound, double upperBound, std::function<double(double)> func, + double epsilon, int maxIters) { + bool found = false; + + double x0 = lowerBound; + double x1 = upperBound; + double f0 = func(x0); + double f1 = func(x1); + double diff = 0; + double x2 = 0; + double f2 = 0; + + std::cout << "f0, f1: " << f0 << ", " << f1 << std::endl; + + // Make sure we bound the root (f = 0.0) + if (f0 * f1 > 0.0) { + throw std::invalid_argument("Function values at the boundaries have the same sign [secantRoot]."); + } + + // Order the bounds + if (f1 < f0) { + std::swap(x0, x1); + std::swap(f0, f1); + } + + for (int iteration=0; iteration < maxIters; iteration++) { + x2 = x1 - f1 * (x1 - x0)/(f1 - f0); + f2 = func(x2); + + // Update the bounds for the next iteration + if (f2 < 0.0) { + diff = x1 - x2; + x1 = x2; + f1 = f2; + } + else { + diff = x0 - x2; + x0 = x2; + f0 = f2; + } + + // Check to see if we're done + if ((fabs(diff) <= epsilon) || (f2 == 0.0) ) { + found = true; + break; + } + } + + if (found) { + return x2; + } + else { + throw csm::Error( + csm::Error::UNKNOWN_ERROR, + "Could not find a root of the function using the secant method", + "secantRoot"); + } +} + + // convert a measurement double metric_conversion(double val, std::string from, std::string to) { json typemap = { @@ -512,6 +572,23 @@ double getCenterTime(json isd, csm::WarningList *list) { return time; } +double getEndingTime(json isd, csm::WarningList *list) { + double time = 0.0; + try { + time = isd.at("ending_ephemeris_time"); + } + catch (...) { + if (list) { + list->push_back( + csm::Warning( + csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the ending image time.", + "Utilities::getEndingTime()")); + } + } + return time; +} + std::vector<double> getIntegrationStartLines(json isd, csm::WarningList *list) { std::vector<double> lines; try { @@ -1138,19 +1215,58 @@ double getScaledPixelWidth(nlohmann::json isd, csm::WarningList *list) { return width; } +std::vector<double> getScaleConversionTimes(nlohmann::json isd, csm::WarningList *list) { + std::vector<double> time; + try { + time = isd.at("range_conversion_times").get<std::vector<double>>(); + } + catch (...) { + if (list) { + list->push_back( + csm::Warning( + csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the range conversion times.", + "Utilities::getScaleConversionTimes()")); + } + } + return time; +} + std::vector<double> getScaleConversionCoefficients(nlohmann::json isd, csm::WarningList *list) { std::vector<double> coefficients; try { - coefficients = isd.at("range_conversion_coefficients").get<std::vector<double>>(); + for (auto& location : isd.at("range_conversion_coefficients")){ + coefficients.push_back(location[0]); + coefficients.push_back(location[1]); + coefficients.push_back(location[2]); + coefficients.push_back(location[3]); + } } catch (...) { if (list) { list->push_back( csm::Warning( csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the range conversion coefficients and times.", + "Could not parse the range conversion coefficients.", "Utilities::getScaleConversionCoefficients()")); } } return coefficients; } + +double getWavelength(json isd, csm::WarningList *list) { + double wavelength = 0.0; + try { + wavelength = isd.at("wavelength"); + } + catch (...) { + if (list) { + list->push_back( + csm::Warning( + csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the wavelength.", + "Utilities::getWavelength()")); + } + } + return wavelength; +} diff --git a/tests/Fixtures.h b/tests/Fixtures.h index 0b400a80982321b046bc5fc9f721822cab3ac508..608e83a29d9293b5fa2da9c087c9b1d9a8a1a9c5 100644 --- a/tests/Fixtures.h +++ b/tests/Fixtures.h @@ -319,4 +319,30 @@ class SarIsdTest : public ::testing::Test { } }; +class SarSensorModel : public ::testing::Test { + protected: + csm::Isd isd; + UsgsAstroSarSensorModel *sensorModel; + + void SetUp() override { + sensorModel = NULL; + + isd.setFilename("data/orbitalSar.img"); + UsgsAstroPlugin sarCameraPlugin; + + csm::Model *model = sarCameraPlugin.constructModelFromISD( + isd, + "USGS_ASTRO_SAR_SENSOR_MODEL"); + sensorModel = dynamic_cast<UsgsAstroSarSensorModel *>(model); + ASSERT_NE(sensorModel, nullptr); + } + + void TearDown() override { + if (sensorModel) { + delete sensorModel; + sensorModel = NULL; + } + } +}; + #endif diff --git a/tests/ISDParsingTests.cpp b/tests/ISDParsingTests.cpp index 736cc11d8059696f1965954bfb66f813aade5b8f..6db50862d597f560bc7c46e1cd28e587fcdf22ff 100644 --- a/tests/ISDParsingTests.cpp +++ b/tests/ISDParsingTests.cpp @@ -324,9 +324,9 @@ TEST(ISDParsing, getScaledPixelWidth) { TEST(ISDParsing, getScaleConversionCoefficients) { json isd = {{"range_conversion_coefficients", - {300, 1, 0.1, 0.01, - 400, 2, 0.2, 0.02, - 500, 3, 0.3, 0.03}}}; + {{300, 1, 0.1, 0.01}, + {400, 2, 0.2, 0.02}, + {500, 3, 0.3, 0.03}}}}; std::vector<double> coefficients = getScaleConversionCoefficients(isd); ASSERT_EQ(coefficients.size(), 12); EXPECT_EQ(coefficients[0], 300); @@ -342,3 +342,14 @@ TEST(ISDParsing, getScaleConversionCoefficients) { EXPECT_EQ(coefficients[10], 0.3); EXPECT_EQ(coefficients[11], 0.03); } + +TEST(ISDParsing, getScaleConversionTimes) { + json isd = {{"range_conversion_times", + {100, 200, 300, -400}}}; + std::vector<double> times = getScaleConversionTimes(isd); + ASSERT_EQ(times.size(), 4); + EXPECT_EQ(times[0], 100); + EXPECT_EQ(times[1], 200); + EXPECT_EQ(times[2], 300); + EXPECT_EQ(times[3], -400); +} diff --git a/tests/SarTests.cpp b/tests/SarTests.cpp index 500fa9bdeeca0452222fdf561f7bbfa111acb6ba..59cb3727297c9d69176436cb1311b30dec4ecf9e 100644 --- a/tests/SarTests.cpp +++ b/tests/SarTests.cpp @@ -3,6 +3,7 @@ #include "UsgsAstroSarSensorModel.h" #include "Warning.h" +#include "Fixtures.h" #include <json/json.hpp> #include <gtest/gtest.h> @@ -33,3 +34,49 @@ TEST(SarTests, stateFromIsd) { } EXPECT_TRUE(warnings.empty()); } + +TEST_F(SarSensorModel, State) { + std::string modelState = sensorModel->getModelState(); + EXPECT_NO_THROW( + sensorModel->replaceModelState(modelState) + ); + EXPECT_EQ(sensorModel->getModelState(), modelState); +} + +TEST_F(SarSensorModel, Center) { + csm::ImageCoord imagePt(7.5, 7.5); + csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); + EXPECT_NEAR(groundPt.x, 0, 1e-8); + EXPECT_NEAR(groundPt.y, 0, 1e-8); + EXPECT_NEAR(groundPt.z, 0, 1e-8); +} + +TEST_F(SarSensorModel, GroundToImage) { + csm::EcefCoord groundPt(1737391.90602155, 3749.98835331, -3749.99708833); + csm::ImageCoord imagePt = sensorModel->groundToImage(groundPt, 0.001); + EXPECT_NEAR(imagePt.line, 500.0, 0.001); + // Due to position interpolation, the sample is slightly less accurate than the line + EXPECT_NEAR(imagePt.samp, 500.0, 0.002); +} + +TEST_F(SarSensorModel, spacecraftPosition) { + csm::EcefVector position = sensorModel->getSpacecraftPosition(-0.0025); + EXPECT_NEAR(position.x, 3.73740000e+06, 1e-8); + EXPECT_NEAR(position.y, 0.00000000e+00, 1e-8); + EXPECT_NEAR(position.z, 0.00000000e+00, 1e-8); +} + +TEST_F(SarSensorModel, spacecraftVelocity) { + csm::EcefVector velocity = sensorModel->getSpacecraftVelocity(-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); +} + +TEST_F(SarSensorModel, getRangeCoefficients) { + std::vector<double> coeffs = sensorModel->getRangeCoefficients(-0.0025); + EXPECT_NEAR(coeffs[0], 2000000.0000039602, 1e-8); + EXPECT_NEAR(coeffs[1], -1.0504347070801814e-08, 1e-8); + EXPECT_NEAR(coeffs[2], 5.377926500384916e-07, 1e-8); + EXPECT_NEAR(coeffs[3], -1.3072206620088014e-15, 1e-8); +} diff --git a/tests/UtilitiesTests.cpp b/tests/UtilitiesTests.cpp index f1918bdf24539c3ab3e0c21275212835ceec9cc1..2400cb372e26edefc98a50b1418f81b831f0349d 100644 --- a/tests/UtilitiesTests.cpp +++ b/tests/UtilitiesTests.cpp @@ -9,6 +9,7 @@ #include <math.h> #include <stdexcept> +#include <functional> using json = nlohmann::json; @@ -339,12 +340,21 @@ TEST(UtilitiesTests, lagrangeInterp2D) { EXPECT_DOUBLE_EQ(outputValue[1], 1.5); } -double testPoly(double x) { - return (x - 2) * (x + 1) * (x + 7); -}; - TEST(UtilitiesTests, brentRoot) { + std::function<double(double)> testPoly = [](double x) { + return (x - 2) * (x + 1) * (x + 7); + }; + EXPECT_NEAR(brentRoot(1.0, 3.0, testPoly, 1e-10), 2.0, 1e-10); EXPECT_NEAR(brentRoot(0.0, -3.0, testPoly, 1e-10), -1.0, 1e-10); EXPECT_THROW(brentRoot(-3.0, 3.0, testPoly), std::invalid_argument); } + +TEST(UtilitiesTests, secantRoot) { + std::function<double(double)> testPoly = [](double x) { + return (x - 2) * (x + 1) * (x + 7); + }; + EXPECT_NEAR(secantRoot(1.0, 3.0, testPoly, 1e-10), 2.0, 1e-10); + EXPECT_NEAR(secantRoot(0.0, -3.0, testPoly, 1e-10), -1.0, 1e-10); + EXPECT_THROW(secantRoot(-1000.0, 1000.0, testPoly), csm::Error); +} diff --git a/tests/data/orbitalSar.json b/tests/data/orbitalSar.json index 8e07ccd1d8251f05e1b380ee56f7f9f28ac0e1a5..95e1a9fe62de76a4f6759e7f5617e5dae0270c91 100644 --- a/tests/data/orbitalSar.json +++ b/tests/data/orbitalSar.json @@ -16,76 +16,78 @@ "wavelength" : 0.125, "line_exposure_duration" : 0.005, "scaled_pixel_width" : 7.5, - "t0_ephemeris": 0.0, + "t0_ephemeris": -0.0025, "dt_ephemeris": 0.25, "sensor_position": { "positions": - [[ 3.73740000e+06, 0.00000000e+00, -0.00000000e+00], - [ 3.73739991e+06, 0.00000000e+00, -8.06679515e+02], - [ 3.73739965e+06, 0.00000000e+00, -1.61335899e+03], - [ 3.73739922e+06, 0.00000000e+00, -2.42003839e+03], - [ 3.73739861e+06, 0.00000000e+00, -3.22671768e+03], - [ 3.73739782e+06, 0.00000000e+00, -4.03339682e+03], - [ 3.73739687e+06, 0.00000000e+00, -4.84007577e+03], - [ 3.73739573e+06, 0.00000000e+00, -5.64675450e+03], - [ 3.73739443e+06, 0.00000000e+00, -6.45343296e+03], - [ 3.73739295e+06, 0.00000000e+00, -7.26011112e+03], - [ 3.73739129e+06, 0.00000000e+00, -8.06678895e+03], - [ 3.73738947e+06, 0.00000000e+00, -8.87346640e+03], - [ 3.73738746e+06, 0.00000000e+00, -9.68014343e+03], - [ 3.73738529e+06, 0.00000000e+00, -1.04868200e+04], - [ 3.73738294e+06, 0.00000000e+00, -1.12934961e+04], - [ 3.73738041e+06, 0.00000000e+00, -1.21001717e+04], - [ 3.73737771e+06, 0.00000000e+00, -1.29068467e+04], - [ 3.73737484e+06, 0.00000000e+00, -1.37135211e+04], - [ 3.73737179e+06, 0.00000000e+00, -1.45201949e+04], - [ 3.73736857e+06, 0.00000000e+00, -1.53268679e+04], - [ 3.73736518e+06, 0.00000000e+00, -1.61335403e+04]], + [[3737400.0, 0.0, -0.0], + [3737399.912943243, 0.0, -806.6795148600813], + [3737399.651772976, 0.0, -1613.3589921395437], + [3737399.216489211, 0.0, -2420.03839425777], + [3737398.607091969, 0.0, -3226.7176836341473], + [3737397.823581278, 0.0, -4033.396822688065], + [3737396.8659571735, 0.0, -4840.075773838925], + [3737395.734219702, 0.0, -5646.754499506133], + [3737394.4283689144, 0.0, -6453.432962109106], + [3737392.9484048723, 0.0, -7260.111124067275], + [3737391.2943276456, 0.0, -8066.788947800083], + [3737389.4661373096, 0.0, -8873.466395726995], + [3737387.4638339505, 0.0, -9680.14343026748], + [3737385.287417662, 0.0, -10486.820013841041], + [3737382.936888544, 0.0, -11293.496108867193], + [3737380.4122467074, 0.0, -12100.17167776548], + [3737377.713492269, 0.0, -12906.846682955462], + [3737374.840625355, 0.0, -13713.521086856732], + [3737371.7936460995, 0.0, -14520.194851888911], + [3737368.5725546437, 0.0, -15326.867940471646], + [3737365.177351138, 0.0, -16133.540315024615]], "velocities": - [[-0.00000000e+00, 0.00000000e+00, -3.73740000e+06], - [-8.06679515e+02, 0.00000000e+00, -3.73739991e+06], - [-1.61335899e+03, 0.00000000e+00, -3.73739965e+06], - [-2.42003839e+03, 0.00000000e+00, -3.73739922e+06], - [-3.22671768e+03, 0.00000000e+00, -3.73739861e+06], - [-4.03339682e+03, 0.00000000e+00, -3.73739782e+06], - [-4.84007577e+03, 0.00000000e+00, -3.73739687e+06], - [-5.64675450e+03, 0.00000000e+00, -3.73739573e+06], - [-6.45343296e+03, 0.00000000e+00, -3.73739443e+06], - [-7.26011112e+03, 0.00000000e+00, -3.73739295e+06], - [-8.06678895e+03, 0.00000000e+00, -3.73739129e+06], - [-8.87346640e+03, 0.00000000e+00, -3.73738947e+06], - [-9.68014343e+03, 0.00000000e+00, -3.73738746e+06], - [-1.04868200e+04, 0.00000000e+00, -3.73738529e+06], - [-1.12934961e+04, 0.00000000e+00, -3.73738294e+06], - [-1.21001717e+04, 0.00000000e+00, -3.73738041e+06], - [-1.29068467e+04, 0.00000000e+00, -3.73737771e+06], - [-1.37135211e+04, 0.00000000e+00, -3.73737484e+06], - [-1.45201949e+04, 0.00000000e+00, -3.73737179e+06], - [-1.53268679e+04, 0.00000000e+00, -3.73736857e+06], - [-1.61335403e+04, 0.00000000e+00, -3.73736518e+06]], + [[-0.0, 0.0, -3737400.0], + [-806.6795148600813, 0.0, -3737399.912943243], + [-1613.3589921395437, 0.0, -3737399.651772976], + [-2420.03839425777, 0.0, -3737399.216489211], + [-3226.7176836341473, 0.0, -3737398.607091969], + [-4033.396822688065, 0.0, -3737397.823581278], + [-4840.075773838925, 0.0, -3737396.8659571735], + [-5646.754499506133, 0.0, -3737395.734219702], + [-6453.432962109106, 0.0, -3737394.4283689144], + [-7260.111124067275, 0.0, -3737392.9484048723], + [-8066.788947800083, 0.0, -3737391.2943276456], + [-8873.466395726995, 0.0, -3737389.4661373096], + [-9680.14343026748, 0.0, -3737387.4638339505], + [-10486.820013841041, 0.0, -3737385.287417662], + [-11293.496108867193, 0.0, -3737382.936888544], + [-12100.17167776548, 0.0, -3737380.4122467074], + [-12906.846682955462, 0.0, -3737377.713492269], + [-13713.521086856732, 0.0, -3737374.840625355], + [-14520.194851888911, 0.0, -3737371.7936460995], + [-15326.867940471646, 0.0, -3737368.5725546437], + [-16133.540315024615, 0.0, -3737365.177351138]], "unit": "m" }, + "range_conversion_times": [0.0, 0.25, 0.5, 0.75, 1.0, 1.25, 1.5, 1.75, 2.0, 2.25, 2.5, 2.75, 3.0, + 3.25, 3.5, 3.75, 4.0, 4.25, 4.5, 4.75], "range_conversion_coefficients" : [ - 0.0, 7.99423808710000e+04, 6.92122900000000e-01, 3.40193700000000e-06, -2.39924200000000e-11, - 0.25, 7.99423795700000e+04, 6.90795300000000e-01, 3.41377900000000e-06, -2.40431400000000e-11, - 0.5, 7.99423784790000e+04, 6.89923300000000e-01, 3.42142300000000e-06, -2.40714100000000e-11, - 0.75, 7.99423782970000e+04, 6.89091700000000e-01, 3.42877300000000e-06, -2.41021200000000e-11, - 1.0, 7.99423774910000e+04, 6.88258400000000e-01, 3.43615000000000e-06, -2.41318800000000e-11, - 1.25, 7.99423754720000e+04, 6.87466400000000e-01, 3.44310000000000e-06, -2.41596100000000e-11, - 1.5, 7.99423758230000e+04, 6.86672900000000e-01, 3.45009800000000e-06, -2.41882400000000e-11, - 1.75, 7.99423758680000e+04, 6.85920500000000e-01, 3.45672500000000e-06, -2.42163000000000e-11, - 2.0, 7.99423737430000e+04, 6.85166800000000e-01, 3.46335100000000e-06, -2.42427900000000e-11, - 2.25, 7.99423758150000e+04, 6.84453800000000e-01, 3.46968000000000e-06, -2.42686600000000e-11, - 2.5, 7.99423733480000e+04, 6.83783000000000e-01, 3.47554900000000e-06, -2.42912800000000e-11, - 2.75, 7.99423731540000e+04, 6.83111300000000e-01, 3.48141500000000e-06, -2.43158100000000e-11, - 3.0, 7.99423745960000e+04, 6.82480900000000e-01, 3.48693900000000e-06, -2.43373700000000e-11, - 3.25, 7.99423723580000e+04, 6.81849000000000e-01, 3.49252600000000e-06, -2.43599300000000e-11, - 3.5, 7.99423734150000e+04, 6.81259600000000e-01, 3.49765400000000e-06, -2.43799400000000e-11, - 3.75, 7.99423726230000e+04, 6.80711700000000e-01, 3.50247300000000e-06, -2.43988600000000e-11, - 4.0, 7.99423708550000e+04, 6.80163500000000e-01, 3.50722300000000e-06, -2.44181200000000e-11, - 4.25, 7.99423717720000e+04, 6.79614100000000e-01, 3.51201600000000e-06, -2.44370400000000e-11, - 4.5, 7.99423706730000e+04, 6.79149600000000e-01, 3.51607500000000e-06, -2.44520600000000e-11, - 4.75, 7.99423710670000e+04, 6.78430200000000e-01, 3.52233100000000e-06, -2.44771700000000e-11 + [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15], + [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15], + [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15], + [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15], + [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15], + [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15], + [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15], + [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15], + [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15], + [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15], + [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15], + [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15], + [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15], + [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15], + [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15], + [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15], + [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15], + [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15], + [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15], + [2000000.0000039602, -1.0504347070801814e-08, 5.377926500384916e-07, -1.3072206620088014e-15] ], "sun_position": { "positions": [