diff --git a/include/usgscsm/UsgsAstroLsSensorModel.h b/include/usgscsm/UsgsAstroLsSensorModel.h index f935cd2014612265ecb8c2b1aff858bac3303d8c..8a6c3e054cc36c270b1323fe3d083018495c6fde 100644 --- a/include/usgscsm/UsgsAstroLsSensorModel.h +++ b/include/usgscsm/UsgsAstroLsSensorModel.h @@ -123,6 +123,10 @@ public: std::vector<double> m_covariance; int m_imageFlipFlag; + std::vector<double> m_sunPosition; + std::vector<double> m_sunVelocity; + + // Define logging pointer and file content std::string m_logFile; std::shared_ptr<spdlog::logger> m_logger; @@ -908,6 +912,15 @@ public: const std::vector<double>& adj, double attCorr[9]) const; + virtual csm::EcefVector getSunPosition( + const double imageTime) const; + //> This method returns the position of the sun at the time the image point + // was recorded. If multiple sun positions are available, the method uses + // lagrange interpolation. If one sun position and at least one sun velocity + // are available, then the position is calculated using linear extrapolation. + // If only one sun position is available, then that value is returned. + + private: void determineSensorCovarianceInImageSpace( diff --git a/include/usgscsm/Utilities.h b/include/usgscsm/Utilities.h index 94459d9d4ed1bda9d0f1b2fadfd4a18b79c2f603..c5250aa935852d42ce823c1af919efee118a50b0 100644 --- a/include/usgscsm/Utilities.h +++ b/include/usgscsm/Utilities.h @@ -121,6 +121,7 @@ DistortionType getDistortionModel(nlohmann::json isd, csm::WarningList *list=nul std::vector<double> getDistortionCoeffs(nlohmann::json isd, csm::WarningList *list=nullptr); std::vector<double> getRadialDistortion(nlohmann::json isd, csm::WarningList *list=nullptr); std::vector<double> getSunPositions(nlohmann::json isd, csm::WarningList *list=nullptr); +std::vector<double> getSunVelocities(nlohmann::json isd, csm::WarningList *list=nullptr); 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); diff --git a/src/UsgsAstroLsSensorModel.cpp b/src/UsgsAstroLsSensorModel.cpp index 5fa07bd67f94230746daa82d2eb2a969de2a4526..dc0b7801308167affbbe89d12a36f92ddfe20b78 100644 --- a/src/UsgsAstroLsSensorModel.cpp +++ b/src/UsgsAstroLsSensorModel.cpp @@ -102,6 +102,8 @@ const std::string UsgsAstroLsSensorModel::_STATE_KEYWORD[] = "m_currentParameterValue", "m_parameterType", "m_referencePointXyz", + "m_sunPosition", + "m_sunVelocity", "m_gsd", "m_flyingHeight", "m_halfSwath", @@ -255,6 +257,8 @@ void UsgsAstroLsSensorModel::replaceModelState(const std::string &stateString ) m_quaternions = j["m_quaternions"].get<std::vector<double>>(); m_currentParameterValue = j["m_currentParameterValue"].get<std::vector<double>>(); m_covariance = j["m_covariance"].get<std::vector<double>>(); + m_sunPosition = j["m_sunPosition"].get<std::vector<double>>(); + m_sunVelocity = j["m_sunVelocity"].get<std::vector<double>>(); m_logFile = j["m_logFile"].get<std::string>(); if (m_logFile.empty()) { @@ -443,6 +447,12 @@ std::string UsgsAstroLsSensorModel::getModelState() const { m_referencePointXyz.x, m_referencePointXyz.y, m_referencePointXyz.z) + state["m_sunPosition"] = m_sunPosition; + MESSAGE_LOG(m_logger, "num sun positions: {} ", m_sunPosition.size()) + + state["m_sunVelocity"] = m_sunVelocity; + MESSAGE_LOG(m_logger, "num sun velocities: {} ", m_sunVelocity.size()) + state["m_logFile"] = m_logFile; return state.dump(); @@ -518,6 +528,10 @@ void UsgsAstroLsSensorModel::reset() m_referencePointXyz.x = 0.0; m_referencePointXyz.y = 0.0; m_referencePointXyz.z = 0.0; + + m_sunPosition = std::vector<double>(3, 0.0); + m_sunVelocity = std::vector<double>(3, 0.0); + m_gsd = 1.0; m_flyingHeight = 1000.0; m_halfSwath = 1000.0; @@ -1668,14 +1682,23 @@ UsgsAstroLsSensorModel::getValidImageRange() const csm::EcefVector UsgsAstroLsSensorModel::getIlluminationDirection( const csm::EcefCoord& groundPt) const { - MESSAGE_LOG(m_logger, "Accessing illimination direction of ground point" - "{} {} {}." - "Illimination direction is not supported, throwing exception", + MESSAGE_LOG(m_logger, "Accessing illumination direction of ground point" + "{} {} {}.", groundPt.x, groundPt.y, groundPt.z); - throw csm::Error( - csm::Error::UNSUPPORTED_FUNCTION, - "Unsupported function", - "UsgsAstroLsSensorModel::getIlluminationDirection"); + + csm::EcefVector sunPosition = getSunPosition(getImageTime(groundToImage(groundPt))); + csm::EcefVector illuminationDirection = csm::EcefVector(groundPt.x - sunPosition.x, + groundPt.y - sunPosition.y, + groundPt.z - sunPosition.z); + + double scale = sqrt(illuminationDirection.x * illuminationDirection.x + + illuminationDirection.y * illuminationDirection.y + + illuminationDirection.z * illuminationDirection.z); + + illuminationDirection.x /= scale; + illuminationDirection.y /= scale; + illuminationDirection.z /= scale; + return illuminationDirection; } //--------------------------------------------------------------------------- @@ -2755,6 +2778,10 @@ std::string UsgsAstroLsSensorModel::constructStateFromIsd(const std::string imag MESSAGE_LOG(m_logger, "m_referencePointXyz: {} ", state["m_referencePointXyz"].dump()) + // sun_position and velocity are required for getIlluminationDirection + state["m_sunPosition"]= getSunPositions(isd, parsingWarnings); + state["m_sunVelocity"]= getSunVelocities(isd, parsingWarnings); + // leave these be for now. state["m_gsd"] = 1.0; state["m_flyingHeight"] = 1000.0; @@ -2800,6 +2827,7 @@ std::string UsgsAstroLsSensorModel::constructStateFromIsd(const std::string imag state["m_detectorSampleOrigin"].dump(), state["m_detectorLineOrigin"].dump()) + // These are exlusive to LineScanners, leave them here for now. try { state["m_dtEphem"] = isd.at("dt_ephemeris"); @@ -2942,3 +2970,40 @@ std::shared_ptr<spdlog::logger> UsgsAstroLsSensorModel::getLogger() { void UsgsAstroLsSensorModel::setLogger(std::shared_ptr<spdlog::logger> logger) { m_logger = logger; } + + +csm::EcefVector UsgsAstroLsSensorModel::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_dtEphem * ((m_numPositions/3))); + 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/src/Utilities.cpp b/src/Utilities.cpp index 5c17d131b6b27bc124fcbc1fdddb968b701b786f..1f0b5907e9f9d8bc17dd7be1ffd4310041f2bfb4 100644 --- a/src/Utilities.cpp +++ b/src/Utilities.cpp @@ -1039,6 +1039,30 @@ std::vector<double> getSunPositions(json isd, csm::WarningList *list) { return positions; } +std::vector<double> getSunVelocities(json isd, csm::WarningList *list) { + std::vector<double> velocities; + try { + json jayson = isd.at("sun_position"); + json unit = jayson.at("unit"); + for (auto& location : jayson.at("velocities")) { + velocities.push_back(metric_conversion(location[0].get<double>(), unit.get<std::string>())); + velocities.push_back(metric_conversion(location[1].get<double>(), unit.get<std::string>())); + velocities.push_back(metric_conversion(location[2].get<double>(), unit.get<std::string>())); + } + } + catch (...) { + std::cout<<"Could not parse sun velocity"<<std::endl; + if (list) { + list->push_back( + csm::Warning( + csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the sun velocities.", + "Utilities::getSunVelocities()")); + } + } + return velocities; +} + std::vector<double> getSensorPositions(json isd, csm::WarningList *list) { std::vector<double> positions; try { diff --git a/tests/LineScanCameraTests.cpp b/tests/LineScanCameraTests.cpp index 619baec3196009a70fce5952ba63ff219965d752..80de543b7bcbe24be65cf348e9e5eddea3833e52 100644 --- a/tests/LineScanCameraTests.cpp +++ b/tests/LineScanCameraTests.cpp @@ -122,6 +122,75 @@ TEST_F(ConstVelocityLineScanSensorModel, calculateAttitudeCorrection) { EXPECT_NEAR(attCorr[8], 0, 1e-8); } + +TEST_F(OrbitalLineScanSensorModel, getIlluminationDirectionStationary) { + // Get state information, replace sun position / velocity to hit third case: + // One position, no velocity. + std::string state = sensorModel->getModelState(); + json jState = json::parse(state); + jState["m_sunPosition"] = std::vector<double>{100.0,100.0,100.0}; + jState["m_sunVelocity"] = std::vector<double>{}; + sensorModel->replaceModelState(jState.dump()); + + csm::ImageCoord imagePt(8.5,8); + csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); + csm::EcefVector direction = sensorModel->getIlluminationDirection(groundPt); + + // Calculate expected sun direction + // These are the ground point coordinates minus constant sun positions. + double expected_x = 999899.680000017; + double expected_y = -100; + double expected_z = -899.99991466668735; + + //normalize + double scale = sqrt((expected_x * expected_x) + (expected_y * expected_y) + (expected_z * expected_z)); + expected_x /= scale; + expected_y /= scale; + expected_z /= scale; + + EXPECT_DOUBLE_EQ(direction.x, expected_x); + EXPECT_DOUBLE_EQ(direction.y, expected_y); + EXPECT_DOUBLE_EQ(direction.z, expected_z); +} + +TEST_F(OrbitalLineScanSensorModel, getSunPositionLagrange){ + std::cout<<sensorModel->m_t0Ephem<<std::endl; + csm::EcefVector sunPosition = sensorModel->getSunPosition(-.6); + EXPECT_DOUBLE_EQ(sunPosition.x, 125); + EXPECT_DOUBLE_EQ(sunPosition.y, 125); + EXPECT_DOUBLE_EQ(sunPosition.z, 125); +} + +TEST_F(OrbitalLineScanSensorModel, getSunPositionLinear){ + // Get state information, replace sun position / velocity to hit third case: + // One position, no velocity. + std::string state = sensorModel->getModelState(); + json jState = json::parse(state); + jState["m_sunPosition"] = std::vector<double>{100.0,100.0,100.0}; + jState["m_sunVelocity"] = std::vector<double>{50.0, 50.0, 50.0}; + sensorModel->replaceModelState(jState.dump()); + + csm::EcefVector sunPosition = sensorModel->getSunPosition(.5); + EXPECT_DOUBLE_EQ(sunPosition.x, 125); + EXPECT_DOUBLE_EQ(sunPosition.y, 125); + EXPECT_DOUBLE_EQ(sunPosition.z, 125); +} + +TEST_F(OrbitalLineScanSensorModel, getSunPositionStationary){ + // Get state information, replace sun position / velocity to hit third case: + // One position, no velocity. + std::string state = sensorModel->getModelState(); + json jState = json::parse(state); + jState["m_sunPosition"] = std::vector<double>{100.0,100.0,100.0}; + jState["m_sunVelocity"] = std::vector<double>{}; + sensorModel->replaceModelState(jState.dump()); + + csm::EcefVector sunPosition = sensorModel->getSunPosition(1); + EXPECT_DOUBLE_EQ(sunPosition.x, 100); + EXPECT_DOUBLE_EQ(sunPosition.y, 100); + EXPECT_DOUBLE_EQ(sunPosition.z, 100); +} + TEST_F(OrbitalLineScanSensorModel, Center) { csm::ImageCoord imagePt(8.5, 8.0); csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); diff --git a/tests/data/orbitalLineScan.json b/tests/data/orbitalLineScan.json index ba6455a8f2e3c937019af2ac619a687ab76a0203..f1e00928eae6c7dba80d78610d29e7a4a908e6de 100644 --- a/tests/data/orbitalLineScan.json +++ b/tests/data/orbitalLineScan.json @@ -103,7 +103,10 @@ "starting_detector_sample": 0, "sun_position": { "positions": [ - [100.0, 100.0, 0.0] + [100.0, 100.0, 100.0], + [150.0, 150.0, 150.0], + [200.0, 200.0, 200.0], + [250.0, 250.0, 250.0] ], "velocities": [ [0.0, 0.0, 0.0]