Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Adds more capabilities to TestCsmModel #4527

Merged
merged 4 commits into from
Jun 15, 2021
Merged

Adds more capabilities to TestCsmModel #4527

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
98b2147
Updated to get new isd/state form to test jigsaw parsing
krlberry Jun 11, 2021
fe99aea
Add functional image-to-ground, ground-to-image, and isd-parsing to T…
krlberry Jun 14, 2021
c847e8e
Remove temporary csminit test changes
krlberry Jun 14, 2021
8547ee1
Updated variable name for clarity
krlberry Jun 14, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
224 changes: 201 additions & 23 deletions isis/tests/TestCsmModel.cpp
View file
Open in desktop
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> adj(getNumParameters(), 0.0);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Would this benefit from a more descriptive name?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Absolutely! Thanks for the suggestion

adj[index] = delta;

csm::ImageCoord imagePt1 = groundToImage(groundPt, adj, 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