Adds more capabilities to TestCsmModel (#4527) (b382c973) · Commits · aflab / astrogeology / ISIS3

isis/tests/TestCsmModel.cpp

+201 −23

Original line number	Diff line number	Diff line
		#include "TestCsmModel.h"
		#include <fstream>
		#include <iostream>
		#include <string>
		#include <nlohmann/json.hpp>
		using json = nlohmann::json;

		@@ -9,24 +10,29 @@ const std::string TestCsmModel::SENSOR_MODEL_NAME = "TestCsmModel";

		// Sensor model Parameter names
		const std::vector<std::string> TestCsmModel::PARAM_NAMES = {
		"test_param_one",
		"test_param_two"
		"center_latitude",
		"center_longitude",
		"scale", // pixels per degree
		};

		// Sensor model Parameter units
		const std::vector<std::string> 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<csm::param::Type> TestCsmModel::PARAM_TYPES = {
		csm::param::FICTITIOUS,
		csm::param::REAL,
		csm::param::REAL,
		csm::param::REAL
		};

		// Sensor model Parameter sharing criteria
		const std::vector<csm::SharingCriteria> TestCsmModel::PARAM_SHARING_CRITERIA = {
		csm::SharingCriteria(),
		csm::SharingCriteria(),
		csm::SharingCriteria()
		};
		@@ -37,6 +43,8 @@ const std::vector<csm::SharingCriteria> 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<double>(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<double> 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<double> &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<double> 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);
		}


		@@ -608,13 +722,68 @@ csm::RasterGM::SensorPartials TestCsmModel::computeSensorPartials(
		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<double> 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<double> TestCsmModel::computeGroundPartials(const csm::EcefCoord& groundPt) const {
		std::vector<double> 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<double> 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<double> TestCsmModel::getUnmodeledCrossCovariance(
		return vec;
		}


		csm::Ellipsoid TestCsmModel::getEllipsoid() const {
		return csm::Ellipsoid(1000000, 1000000);
		}


		double TestCsmModel::getValue(int index, const std::vector<double> &adjustments) const {
		return m_param_values[index] + adjustments[index];
		}

isis/tests/TestCsmModel.h

+15 −3

Original line number	Diff line number	Diff line
		@@ -7,6 +7,7 @@
		#include "csm/Plugin.h"
		#include "csm/Version.h"
		#include "csm/CorrelationModel.h"
		#include "csm/SettableEllipsoid.h"

		#include <nlohmann/json.hpp>

		@@ -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<double> &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<double> getUnmodeledCrossCovariance(
		const csm::ImageCoord& pt1,
		const csm::ImageCoord& pt2) const;

		virtual csm::Ellipsoid getEllipsoid() const;
		double getValue(int index, const std::vector<double> &adjustments) const;
		private:
		std::vector<double> m_param_values; //! Parameter values associated with the sensor model
		std::vector<double> m_param_sigmas; //! Sigma values associated with the sensor model parameters
		csm::NoCorrelationModel m_correlationModel;
		double m_referenceTime;
		std::vector<double> m_noAdjustments;
		};
		#endif