diff --git a/isis/tests/TestCsmModel.cpp b/isis/tests/TestCsmModel.cpp index dc4acf6cd5..42f2fcdae7 100644 --- a/isis/tests/TestCsmModel.cpp +++ b/isis/tests/TestCsmModel.cpp @@ -1,6 +1,7 @@ #include "TestCsmModel.h" #include #include +#include #include using json = nlohmann::json; @@ -9,24 +10,29 @@ const std::string TestCsmModel::SENSOR_MODEL_NAME = "TestCsmModel"; // Sensor model Parameter names const std::vector TestCsmModel::PARAM_NAMES = { - "test_param_one", - "test_param_two" + "center_latitude", + "center_longitude", + "scale", // pixels per degree }; // Sensor model Parameter units const std::vector TestCsmModel::PARAM_UNITS = { - "m", - "rad" +// "unitless", // Are these used/defined in an enum or anything like this? + "rad", // TODO: or degree? + "rad", + "pixels per degree", }; // Sensor model Parameter Types const std::vector TestCsmModel::PARAM_TYPES = { - csm::param::FICTITIOUS, + csm::param::REAL, + csm::param::REAL, csm::param::REAL }; // Sensor model Parameter sharing criteria const std::vector TestCsmModel::PARAM_SHARING_CRITERIA = { + csm::SharingCriteria(), csm::SharingCriteria(), csm::SharingCriteria() }; @@ -37,6 +43,8 @@ const std::vector TestCsmModel::PARAM_SHARING_CRITERIA = { */ TestCsmModel::TestCsmModel() { m_param_values.resize(TestCsmModel::PARAM_NAMES.size(), 0.0); + m_param_sigmas.resize(TestCsmModel::PARAM_NAMES.size(), 0.0); + m_noAdjustments = std::vector(TestCsmModel::PARAM_NAMES.size(), 0.0); }; @@ -175,7 +183,9 @@ std::string TestCsmModel::getSensorMode() const { * @return std::string reference date and time */ std::string TestCsmModel::getReferenceDateAndTime() const { - return "20000101T115959Z"; + std::string timeString; + timeString = "20000101T12000" + std::to_string(m_referenceTime) + "Z"; + return timeString; } @@ -186,9 +196,16 @@ std::string TestCsmModel::getReferenceDateAndTime() const { */ std::string TestCsmModel::getModelState() const { json state; + state["reference_time"] = m_referenceTime; + for (size_t param_index = 0; param_index < m_param_values.size(); param_index++) { state[TestCsmModel::PARAM_NAMES[param_index]] = m_param_values[param_index]; } + + state["center_latitude_sigma"] = m_param_sigmas[0]; + state["center_longitude_sigma"] = m_param_sigmas[1]; + state["scale_sigma"] = m_param_sigmas[2]; + return TestCsmModel::SENSOR_MODEL_NAME + "\n" + state.dump(); } @@ -201,8 +218,20 @@ std::string TestCsmModel::getModelState() const { void TestCsmModel::replaceModelState(const std::string& argState) { // Get the JSON substring json state = json::parse(argState.substr(argState.find("\n") + 1)); + + // set reference time + m_referenceTime = state.at("reference_time"); + + // set parameter values for (size_t param_index = 0; param_index < m_param_values.size(); param_index++) { - m_param_values[param_index] = state.at(TestCsmModel::PARAM_NAMES[param_index]); + // No error-checking, but that's probably fine for now. + m_param_values[param_index] = double(state.at(TestCsmModel::PARAM_NAMES[param_index])); + } + + // set parameter sigmas + for (size_t sigma_index = 0; sigma_index < m_param_sigmas.size(); sigma_index++) { + // No error-checking, but that's probably fine for now. + m_param_sigmas[sigma_index] = double(state.at(TestCsmModel::PARAM_NAMES[sigma_index]+"_sigma")); } } @@ -221,14 +250,19 @@ std::string TestCsmModel::constructStateFromIsd(const csm::Isd isd){ if (isdFile.fail()) { std::cout << "Could not open file: " << filename << std::endl; } - json parsedIsd; isdFile >> parsedIsd; - // Only extract the first 2 parameters from the file + // TODO: modified this so no longer true. Breaks existing tests? *Only extract the first 2 parameters from the file* json state; + state["reference_time"] = parsedIsd.at("reference_time"); for (size_t param_index = 0; param_index < m_param_values.size(); param_index++) { state[TestCsmModel::PARAM_NAMES[param_index]] = parsedIsd.at(TestCsmModel::PARAM_NAMES[param_index]); } + state["center_latitude_sigma"] = parsedIsd.at("center_latitude_sigma"); + state["center_longitude_sigma"] = parsedIsd.at("center_longitude_sigma"); + state["scale_sigma"] = parsedIsd.at("scale_sigma"); + + std::cout << "state output: " << TestCsmModel::SENSOR_MODEL_NAME + "\n" + state.dump() << std::endl;; return TestCsmModel::SENSOR_MODEL_NAME + "\n" + state.dump(); } @@ -380,9 +414,9 @@ void TestCsmModel::setParameterType(int index, csm::param::Type pType) { */ double TestCsmModel::getParameterCovariance(int index1, int index2) const { - // default to identity covariance matrix + // Just return variances along the diagonal if (index1 == index2) { - return 1.0; + return m_param_sigmas[index1]*m_param_sigmas[index1]; } return 0.0; } @@ -484,15 +518,35 @@ std::vector TestCsmModel::getCrossCovarianceMatrix( return covariance; } -// csm::RasterGM methods csm::ImageCoord TestCsmModel::groundToImage(const csm::EcefCoord& groundPt, double desiredPrecision, double* achievedPrecision, csm::WarningList* warnings) const { - return csm::ImageCoord(0.0,0.0); + + return groundToImage(groundPt, m_noAdjustments, desiredPrecision, achievedPrecision, warnings); } +csm::ImageCoord TestCsmModel::groundToImage(const csm::EcefCoord& groundPt, const std::vector &adjustments, + double desiredPrecision, + double* achievedPrecision, + csm::WarningList* warnings) const { + csm::ImageCoord imageCoord; + + double center_lat = getValue(0, adjustments); + double center_longitude = getValue(1, adjustments); + double scale = getValue(2, adjustments); + + double R = 1000000; + double lat = asin(groundPt.z/R); + double lon = acos(groundPt.x/(R*cos(lat))); + + imageCoord.samp = (lon - center_longitude)*scale + getImageSize().samp/2.0; + imageCoord.line = (lat - center_lat)*scale + getImageSize().line/2.0; + + return imageCoord; +} + csm::ImageCoordCovar TestCsmModel::groundToImage(const csm::EcefCoordCovar& groundPt, double desiredPrecision, double* achievedPrecision, @@ -506,7 +560,21 @@ csm::EcefCoord TestCsmModel::imageToGround(const csm::ImageCoord& imagePt, double desiredPrecision, double* achievedPrecision, csm::WarningList* warnings) const { - return csm::EcefCoord(0.0, 0.0, 0.0); + csm::EcefCoord groundPt; + + double center_lat = m_param_values[0]; + double center_longitude = m_param_values[1]; + double scale = m_param_values[2]; + + double lon = center_longitude + (imagePt.samp - getImageSize().samp/2.0)/scale; + double lat = center_lat + (imagePt.line - getImageSize().line/2.0)/scale; + + double R = 1000000.0; // TODO: getfromElliposid? + groundPt.x = R * cos(lat) * cos(lon); + groundPt.y = R * cos(lat) * sin(lon); + groundPt.z = R * sin(lat); + + return groundPt; } csm::EcefCoordCovar TestCsmModel::imageToGround(const csm::ImageCoordCovar& imagePt, @@ -515,7 +583,8 @@ csm::EcefCoordCovar TestCsmModel::imageToGround(const csm::ImageCoordCovar& imag double desiredPrecision, double* achievedPrecision, csm::WarningList* warnings) const { - return csm::EcefCoordCovar(0.0, 0.0, 0.0); + csm::EcefCoordCovar groundPt; + return groundPt; } @@ -525,6 +594,8 @@ csm::EcefLocus TestCsmModel::imageToProximateImagingLocus( double desiredPrecision, double* achievedPrecision, csm::WarningList* warnings) const { + + // TODO: not required to implement for testing, but return the ground point as the point for the locus return csm::EcefLocus(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); } @@ -534,7 +605,32 @@ csm::EcefLocus TestCsmModel::imageToRemoteImagingLocus( double desiredPrecision, double* achievedPrecision, csm::WarningList* warnings) const { - return csm::EcefLocus(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); + + // Convert: center lat, lon, radius+altitude to x,y,z and use that for s/c position + csm::EcefCoord sensorPosition; + + double center_lat = m_param_values[0]; + double center_longitude = m_param_values[1]; + double scale = m_param_values[2]; + + double lon = center_longitude + (imagePt.samp - getImageSize().samp/2.0)/scale; + double lat = center_lat + (imagePt.line - getImageSize().line/2.0)/scale; + + double altitude = 10000; // TODO: more realistic value? + double R = 1000000.0 + altitude; // TODO: getfromElliposid? // only line different from imageToGround. + sensorPosition.x = R * cos(lat) * cos(lon); + sensorPosition.y = R * cos(lat) * sin(lon); + sensorPosition.z = R * sin(lat); + + // Look vector = scale_to_unit_vector(groundPt - sensorPosition) + csm::EcefCoord groundPt = imageToGround(imagePt); + std::vector look(3); + look[0] = groundPt.x - sensorPosition.x; + look[1] = groundPt.y - sensorPosition.y; + look[2] = groundPt.z - sensorPosition.z; + double length = sqrt(look[0]*look[0] + look[1]*look[1] + look[2]*look[2]); + + return csm::EcefLocus(sensorPosition.x, sensorPosition.y, sensorPosition.z, look[0]/length, look[1]/length, look[2]/length); } @@ -544,7 +640,8 @@ csm::ImageCoord TestCsmModel::getImageStart() const { csm::ImageVector TestCsmModel::getImageSize() const { - return csm::ImageVector(0.0, 0.0); + // todo: should probably come from input ISD + return csm::ImageVector(1024, 1024); } @@ -571,7 +668,23 @@ double TestCsmModel::getImageTime(const csm::ImageCoord& imagePt) const { csm::EcefCoord TestCsmModel::getSensorPosition(const csm::ImageCoord& imagePt) const { - return csm::EcefCoord(0.0, 0.0, 0.0); + // Convert: center lat, lon, radius+altitude to x,y,z and use that for s/c position + csm::EcefCoord sensorPosition; + + double center_lat = m_param_values[0]; + double center_longitude = m_param_values[1]; + double scale = m_param_values[2]; + + double lon = center_longitude + (imagePt.samp - getImageSize().samp/2.0)/scale; + double lat = center_lat + (imagePt.line - getImageSize().line/2.0)/scale; + + double altitude = 10000; // TODO: more realistic value? + double R = 1000000.0 + altitude; // TODO: getfromElliposid? // only line different from imageToGround. + sensorPosition.x = R * cos(lat) * cos(lon); + sensorPosition.y = R * cos(lat) * sin(lon); + sensorPosition.z = R * sin(lat); + + return sensorPosition; } @@ -596,7 +709,8 @@ csm::RasterGM::SensorPartials TestCsmModel::computeSensorPartials( double desiredPrecision, double* achievedPrecision, csm::WarningList* warnings) const { - return csm::RasterGM::SensorPartials(0.0, 0.0); + csm::ImageCoord imagePt = groundToImage(groundPt, desiredPrecision, achievedPrecision); + return computeSensorPartials(index, imagePt, groundPt, desiredPrecision, achievedPrecision); } @@ -607,14 +721,69 @@ csm::RasterGM::SensorPartials TestCsmModel::computeSensorPartials( double desiredPrecision, double* achievedPrecision, csm::WarningList* warnings) const { - - return csm::RasterGM::SensorPartials(0.0, 0.0); + + double delta = 1.0; + + // latitude/longitude + if (index == 2) { + delta = 0.5; + } + else { + // scale + delta = 0.0035; + } + + std::vector parameterAdjustments(getNumParameters(), 0.0); + parameterAdjustments[index] = delta; + + csm::ImageCoord imagePt1 = groundToImage(groundPt, parameterAdjustments, desiredPrecision, achievedPrecision); + + csm::RasterGM::SensorPartials partials; + + partials.first = (imagePt1.line - imagePt.line) / delta; + partials.second = (imagePt1.samp - imagePt.samp) / delta; + + return partials; } std::vector TestCsmModel::computeGroundPartials(const csm::EcefCoord& groundPt) const { - std::vector vec = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - return vec; + // Partial of line, sample wrt X, Y, Z + double x = groundPt.x; + double y = groundPt.y; + double z = groundPt.z; + + csm::ImageCoord ipB = groundToImage(groundPt); + csm::EcefCoord nextPoint = imageToGround(csm::ImageCoord(ipB.line + 1, ipB.samp + 1)); + + double dx = nextPoint.x - x; + double dy = nextPoint.y - y; + double dz = nextPoint.z - z; + + double pixelGroundSize = sqrt((dx * dx + dy * dy + dz * dz) / 2.0); + + // If the ground size is too small, try the opposite direction + if (pixelGroundSize < 1e-10) { + nextPoint = imageToGround(csm::ImageCoord(ipB.line - 1, ipB.samp - 1)); + dx = nextPoint.x - x; + dy = nextPoint.y - y; + dz = nextPoint.z - z; + pixelGroundSize = sqrt((dx * dx + dy * dy + dz * dz) / 2.0); + } + + csm::ImageCoord ipX = groundToImage(csm::EcefCoord(x + pixelGroundSize, y, z)); + csm::ImageCoord ipY = groundToImage(csm::EcefCoord(x, y + pixelGroundSize, z)); + csm::ImageCoord ipZ = groundToImage(csm::EcefCoord(x, y, z + pixelGroundSize)); + + std::vector partials(6, 0.0); + partials[0] = (ipX.line - ipB.line) / pixelGroundSize; + partials[3] = (ipX.samp - ipB.samp) / pixelGroundSize; + partials[1] = (ipY.line - ipB.line) / pixelGroundSize; + partials[4] = (ipY.samp - ipB.samp) / pixelGroundSize; + partials[2] = (ipZ.line - ipB.line) / pixelGroundSize; + partials[5] = (ipZ.samp - ipB.samp) / pixelGroundSize; + + return partials; } const csm::CorrelationModel& TestCsmModel::getCorrelationModel() const { @@ -628,3 +797,12 @@ std::vector TestCsmModel::getUnmodeledCrossCovariance( return vec; } + +csm::Ellipsoid TestCsmModel::getEllipsoid() const { + return csm::Ellipsoid(1000000, 1000000); +} + + +double TestCsmModel::getValue(int index, const std::vector &adjustments) const { + return m_param_values[index] + adjustments[index]; +} diff --git a/isis/tests/TestCsmModel.h b/isis/tests/TestCsmModel.h index cb0ffb2cf0..d59d40be51 100644 --- a/isis/tests/TestCsmModel.h +++ b/isis/tests/TestCsmModel.h @@ -7,6 +7,7 @@ #include "csm/Plugin.h" #include "csm/Version.h" #include "csm/CorrelationModel.h" +#include "csm/SettableEllipsoid.h" #include @@ -16,7 +17,7 @@ * * @author 2020-12-08 Kristin Berry */ -class TestCsmModel : public csm::RasterGM { +class TestCsmModel : public csm::RasterGM, public csm::SettableEllipsoid { public: // Static variables that describe the model static const std::string SENSOR_MODEL_NAME; @@ -82,13 +83,20 @@ class TestCsmModel : public csm::RasterGM { double* achievedPrecision = NULL, csm::WarningList* warnings = NULL) const; + virtual csm::ImageCoord groundToImage(const csm::EcefCoord& groundPt, + const std::vector &adjustments, + double desiredPrecision = 0.001, + double* achievedPrecision = NULL, + csm::WarningList* warnings = NULL) const; + + virtual csm::ImageCoordCovar groundToImage(const csm::EcefCoordCovar& groundPt, double desiredPrecision = 0.001, double* achievedPrecision = NULL, csm::WarningList* warnings = NULL) const; virtual csm::EcefCoord imageToGround(const csm::ImageCoord& imagePt, - double height, + double height = 0.0, double desiredPrecision = 0.001, double* achievedPrecision = NULL, csm::WarningList* warnings = NULL) const; @@ -153,9 +161,13 @@ class TestCsmModel : public csm::RasterGM { virtual std::vector getUnmodeledCrossCovariance( const csm::ImageCoord& pt1, const csm::ImageCoord& pt2) const; - + virtual csm::Ellipsoid getEllipsoid() const; + double getValue(int index, const std::vector &adjustments) const; private: std::vector m_param_values; //! Parameter values associated with the sensor model + std::vector m_param_sigmas; //! Sigma values associated with the sensor model parameters csm::NoCorrelationModel m_correlationModel; + double m_referenceTime; + std::vector m_noAdjustments; }; #endif