Skip to content

Commit

Permalink
Adds more capabilities to TestCsmModel (#4527)
Browse files Browse the repository at this point in the history 
* Updated to get new isd/state form to test jigsaw parsing

* Add functional image-to-ground, ground-to-image, and isd-parsing to TestCsmModel

* Remove temporary csminit test changes

* Updated variable name for clarity
  • Loading branch information
@krlberry
krlberry authored Jun 15, 2021
1 parent 474c6f9 commit eef4037
Show file tree
Hide file tree
Showing 2 changed files with 216 additions and 26 deletions.
224 changes: 201 additions & 23 deletions isis/tests/TestCsmModel.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#include "TestCsmModel.h"
#include <fstream>
#include <iostream>
#include <string>
#include <nlohmann/json.hpp>
using json = nlohmann::json;

Expand All @@ -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()
};
Expand All @@ -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);
};


Expand Down Expand Up @@ -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;
}


Expand All @@ -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();
}

Expand All @@ -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"));
}
}

Expand All @@ -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();
}

Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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,
Expand All @@ -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,
Expand All @@ -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;
}


Expand All @@ -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);
}

Expand All @@ -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);
}


Expand All @@ -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);
}


Expand All @@ -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;
}


Expand All @@ -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);
}


Expand All @@ -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<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 {
Expand All @@ -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];
}
Loading

0 comments on commit eef4037

Please sign in to comment.