diff --git a/include/usgscsm/UsgsAstroFrameSensorModel.h b/include/usgscsm/UsgsAstroFrameSensorModel.h
index 575cc11ad511a5270ff5344e96bb30722e5ac493..b24d4ea62d15e67d57a03869450565d5eb76c4f8 100644
--- a/include/usgscsm/UsgsAstroFrameSensorModel.h
+++ b/include/usgscsm/UsgsAstroFrameSensorModel.h
@@ -34,8 +34,16 @@ class UsgsAstroFrameSensorModel : public csm::RasterGM,
double *achievedPrecision = NULL,
csm::WarningList *warnings = NULL) const;
+ static std::string getModelNameFromModelState(const std::string& model_state);
+
std::string constructStateFromIsd(const std::string &jsonIsd,
csm::WarningList *warnings);
+
+ // Apply a rotation and translation to a state string. The effect is
+ // to transform the position and orientation of the camera in ECEF
+ // coordinates.
+ static void applyTransformToState(ale::Rotation const& r, ale::Vec3d const& t,
+ std::string& stateString);
void reset();
virtual csm::ImageCoordCovar groundToImage(
diff --git a/include/usgscsm/UsgsAstroSarSensorModel.h b/include/usgscsm/UsgsAstroSarSensorModel.h
index d5dac3f5620bba4c53f8255543e88b0733efd397..31d29bfe70e6b80fbfbf86d47a2c6e7e45e286ef 100644
--- a/include/usgscsm/UsgsAstroSarSensorModel.h
+++ b/include/usgscsm/UsgsAstroSarSensorModel.h
@@ -5,6 +5,8 @@
#include <RasterGM.h>
#include <SettableEllipsoid.h>
+#include "ale/Rotation.h"
+
#include "spdlog/spdlog.h"
class UsgsAstroSarSensorModel : public csm::RasterGM,
@@ -24,8 +26,14 @@ class UsgsAstroSarSensorModel : public csm::RasterGM,
std::string constructStateFromIsd(const std::string imageSupportData,
csm::WarningList* list);
- std::string getModelNameFromModelState(const std::string& model_state);
+ static std::string getModelNameFromModelState(const std::string& model_state);
+ // Apply a rotation and translation to a state string. The effect is
+ // to transform the position and orientation of the camera in ECEF
+ // coordinates.
+ static void applyTransformToState(ale::Rotation const& r, ale::Vec3d const& t,
+ std::string& stateString);
+
virtual csm::ImageCoord groundToImage(
const csm::EcefCoord& groundPt, double desiredPrecision = 0.001,
double* achievedPrecision = NULL,
diff --git a/src/UsgsAstroFrameSensorModel.cpp b/src/UsgsAstroFrameSensorModel.cpp
index eb1fdbf8f399106d3b2f3f64309f4ccbe8787d7b..763af338ca1a9d792fee9fc3d7aa430042b72691 100644
--- a/src/UsgsAstroFrameSensorModel.cpp
+++ b/src/UsgsAstroFrameSensorModel.cpp
@@ -697,6 +697,32 @@ std::string UsgsAstroFrameSensorModel::getReferenceDateAndTime() const {
return ephemTimeToCalendarTime(ephemTime);
}
+std::string UsgsAstroFrameSensorModel::getModelNameFromModelState(
+ const std::string& model_state) {
+ // Parse the string to JSON
+ auto j = stateAsJson(model_state);
+ // If model name cannot be determined, return a blank string
+ std::string model_name;
+
+ if (j.find("m_modelName") != j.end()) {
+ model_name = j["m_modelName"];
+ } else {
+ csm::Error::ErrorType aErrorType = csm::Error::INVALID_SENSOR_MODEL_STATE;
+ std::string aMessage = "No 'm_modelName' key in the model state object.";
+ std::string aFunction = "UsgsAstroFramePlugin::getModelNameFromModelState";
+ csm::Error csmErr(aErrorType, aMessage, aFunction);
+ throw(csmErr);
+ }
+ if (model_name != _SENSOR_MODEL_NAME) {
+ csm::Error::ErrorType aErrorType = csm::Error::SENSOR_MODEL_NOT_SUPPORTED;
+ std::string aMessage = "Sensor model not supported.";
+ std::string aFunction = "UsgsAstroFramePlugin::getModelNameFromModelState()";
+ csm::Error csmErr(aErrorType, aMessage, aFunction);
+ throw(csmErr);
+ }
+ return model_name;
+}
+
std::string UsgsAstroFrameSensorModel::getModelState() const {
MESSAGE_LOG("Dumping model state");
json state = {
@@ -749,6 +775,56 @@ std::string UsgsAstroFrameSensorModel::getModelState() const {
return stateString;
}
+void UsgsAstroFrameSensorModel::applyTransformToState(ale::Rotation const& r, ale::Vec3d const& t,
+ std::string& stateString) {
+
+ nlohmann::json j = stateAsJson(stateString);
+
+ // The vector having the rotation and translation from camera to
+ // ECEF
+ std::vector<double> currentParameterValue = j["m_currentParameterValue"];
+
+ // Extract the quaternions from the frame camera, apply the
+ // rotation, and put them back.
+ std::vector<double> quaternions;
+ for (size_t it = 0; it < 4; it++)
+ quaternions.push_back(currentParameterValue[it + 3]);
+ applyRotationToQuatVec(r, quaternions);
+ for (size_t it = 0; it < 4; it++)
+ currentParameterValue[it + 3] = quaternions[it];
+
+ // Extract the positions from the frame camera, apply to them the
+ // rotation and translation, and put them back.
+ std::vector<double> positions;
+ for (size_t it = 0; it < 3; it++)
+ positions.push_back(currentParameterValue[it]);
+ applyRotationTranslationToXyzVec(r, t, positions);
+ for (size_t it = 0; it < 3; it++)
+ currentParameterValue[it] = positions[it];
+
+ // Put the transformed vector back in the json state.
+ j["m_currentParameterValue"] = currentParameterValue;
+
+ // Apply rotation to velocities. The translation does not get
+ // applied.
+ std::vector<double> velocities = j["m_spacecraftVelocity"];
+ ale::Vec3d zero_t(0, 0, 0);
+ applyRotationTranslationToXyzVec(r, zero_t, velocities);
+ j["m_spacecraftVelocity"] = velocities;
+
+ // Apply the transform to the reference point.
+ std::vector<double> refPt = j["m_referencePointXyz"];
+ applyRotationTranslationToXyzVec(r, t, refPt);
+ j["m_referencePointXyz"] = refPt;
+
+ // We do not change the Sun position or velocity. The idea is that
+ // the Sun is so far, that minor camera adjustments won't affect
+ // where the Sun is.
+
+ // Update the state string
+ stateString = getModelNameFromModelState(stateString) + "\n" + j.dump(2);
+}
+
bool UsgsAstroFrameSensorModel::isValidModelState(
const std::string &stringState, csm::WarningList *warnings) {
MESSAGE_LOG("Checking if model has valid state");
@@ -871,6 +947,19 @@ void UsgsAstroFrameSensorModel::replaceModelState(
imageToGround(csm::ImageCoord(m_nLines / 2.0, m_nSamples / 2.0));
m_currentParameterCovariance =
state.at("m_currentParameterCovariance").get<std::vector<double>>();
+
+ // These are optional and may not exist in all state files
+ if (state.find("m_maxElevation") != state.end())
+ m_maxElevation = state.at("m_maxElevation").get<double>();
+ if (state.find("m_minElevation") != state.end())
+ m_minElevation = state.at("m_minElevation").get<double>();
+ if (state.find("m_originalHalfLines") != state.end())
+ m_originalHalfLines = state.at("m_originalHalfLines").get<double>();
+ if (state.find("m_originalHalfSamples") != state.end())
+ m_originalHalfSamples = state.at("m_originalHalfSamples").get<double>();
+ if (state.find("m_pixelPitch") != state.end())
+ m_pixelPitch = state.at("m_pixelPitch").get<double>();
+
} catch (std::out_of_range &e) {
MESSAGE_LOG("State keywords required to generate sensor model missing: " +
std::string(e.what()) + "\nUsing model string: " + stringState +
@@ -1348,7 +1437,7 @@ void UsgsAstroFrameSensorModel::losEllipsoidIntersect(
// If quadTerm is negative, the image ray does not
// intersect the ellipsoid. Setting the quadTerm to
// zero means solving for a point on the ray nearest
- // the surface of the ellisoid.
+ // the surface of the ellipsoid.
if (0.0 > quadTerm) {
quadTerm = 0.0;
diff --git a/src/UsgsAstroLsSensorModel.cpp b/src/UsgsAstroLsSensorModel.cpp
index 208e47ba98cd7956bd2d4b6adbca3867f5036b59..bd1a7fb33eb7a26adf75a937072ab8fa76177c82 100644
--- a/src/UsgsAstroLsSensorModel.cpp
+++ b/src/UsgsAstroLsSensorModel.cpp
@@ -468,6 +468,11 @@ void UsgsAstroLsSensorModel::applyTransformToState(ale::Rotation const& r, ale::
applyRotationTranslationToXyzVec(r, zero_t, velocities);
j["m_velocities"] = velocities;
+ // Apply the transform to the reference point
+ std::vector<double> refPt = j["m_referencePointXyz"];
+ applyRotationTranslationToXyzVec(r, t, refPt);
+ j["m_referencePointXyz"] = refPt;
+
// We do not change the Sun position or velocity. The idea is that
// the Sun is so far, that minor camera adjustments won't affect
// where the Sun is.
@@ -475,6 +480,7 @@ void UsgsAstroLsSensorModel::applyTransformToState(ale::Rotation const& r, ale::
// Update the state string
stateString = getModelNameFromModelState(stateString) + "\n" + j.dump(2);
}
+
//***************************************************************************
// UsgsAstroLineScannerSensorModel::reset
//***************************************************************************
diff --git a/src/UsgsAstroSarSensorModel.cpp b/src/UsgsAstroSarSensorModel.cpp
index fe1017fa5b8c97786bb72c93fa405f0801abcaa8..93b29d10107284e4e95f2a09e128c68d4231908d 100644
--- a/src/UsgsAstroSarSensorModel.cpp
+++ b/src/UsgsAstroSarSensorModel.cpp
@@ -208,7 +208,6 @@ string UsgsAstroSarSensorModel::constructStateFromIsd(
string UsgsAstroSarSensorModel::getModelNameFromModelState(
const string& model_state) {
- MESSAGE_LOG("Getting model name from model state: {}", model_state);
// Parse the string to JSON
auto j = stateAsJson(model_state);
// If model name cannot be determined, return a blank string
@@ -220,7 +219,6 @@ string UsgsAstroSarSensorModel::getModelNameFromModelState(
csm::Error::ErrorType aErrorType = csm::Error::INVALID_SENSOR_MODEL_STATE;
string aMessage = "No 'm_modelName' key in the model state object.";
string aFunction = "UsgsAstroSarSensorModel::getModelNameFromModelState";
- MESSAGE_LOG(aMessage);
csm::Error csmErr(aErrorType, aMessage, aFunction);
throw(csmErr);
}
@@ -228,7 +226,6 @@ string UsgsAstroSarSensorModel::getModelNameFromModelState(
csm::Error::ErrorType aErrorType = csm::Error::SENSOR_MODEL_NOT_SUPPORTED;
string aMessage = "Sensor model not supported.";
string aFunction = "UsgsAstroSarSensorModel::getModelNameFromModelState()";
- MESSAGE_LOG(aMessage);
csm::Error csmErr(aErrorType, aMessage, aFunction);
throw(csmErr);
}
@@ -399,6 +396,40 @@ string UsgsAstroSarSensorModel::getModelState() const {
return stateString;
}
+//***************************************************************************
+// UsgsAstroSarSensorModel::applyTransformToState
+//***************************************************************************
+void UsgsAstroSarSensorModel::applyTransformToState(ale::Rotation const& r, ale::Vec3d const& t,
+ std::string& stateString) {
+
+ nlohmann::json j = stateAsJson(stateString);
+
+ // Apply rotation and translation to positions
+ std::vector<double> positions = j["m_positions"].get<std::vector<double>>();
+ applyRotationTranslationToXyzVec(r, t, positions);
+ j["m_positions"] = positions;
+
+ // Note that the SAR sensor does not have quaternions
+
+ // Apply rotation to velocities. The translation does not get applied.
+ ale::Vec3d zero_t(0, 0, 0);
+ std::vector<double> velocities = j["m_velocities"].get<std::vector<double>>();
+ applyRotationTranslationToXyzVec(r, zero_t, velocities);
+ j["m_velocities"] = velocities;
+
+ // Apply the transform to the reference point
+ std::vector<double> refPt = j["m_referencePointXyz"];
+ applyRotationTranslationToXyzVec(r, t, refPt);
+ j["m_referencePointXyz"] = refPt;
+
+ // We do not change the Sun position or velocity. The idea is that
+ // the Sun is so far, that minor camera adjustments won't affect
+ // where the Sun is.
+
+ // Update the state string
+ stateString = getModelNameFromModelState(stateString) + "\n" + j.dump(2);
+}
+
csm::ImageCoord UsgsAstroSarSensorModel::groundToImage(
const csm::EcefCoord& groundPt, double desiredPrecision,
double* achievedPrecision, csm::WarningList* warnings) const {
diff --git a/src/Utilities.cpp b/src/Utilities.cpp
index 030e19a8cac6d10093d4e02313ed1be12bf62d1d..e0c552e8f1215baca337737e24c7a87579798c5f 100644
--- a/src/Utilities.cpp
+++ b/src/Utilities.cpp
@@ -55,7 +55,7 @@ void calculateRotationMatrixFromQuaternions(double q[4],
rotationMatrix[8] = -q[0] * q[0] - q[1] * q[1] + q[2] * q[2] + q[3] * q[3];
}
-// Compue the distorted focal plane coordinate for a given image pixel
+// Compute the distorted focal plane coordinate for a given image pixel
// in - line
// in - sample
// in - sampleOrigin - the origin of the ccd coordinate system relative to the
@@ -88,7 +88,7 @@ void computeDistortedFocalPlaneCoordinates(
distortedY = p21 * t1 + p22 * t2;
}
-// Compue the image pixel for a distorted focal plane coordinate
+// Compute the image pixel for a distorted focal plane coordinate
// in - line
// in - sample
// in - sampleOrigin - the origin of the ccd coordinate system relative to the
diff --git a/tests/FrameCameraTests.cpp b/tests/FrameCameraTests.cpp
index 663a406e79f55bcf5f0f44dbdb920b0829f26381..535ff30aced20521f394612a9eb806852d630058 100644
--- a/tests/FrameCameraTests.cpp
+++ b/tests/FrameCameraTests.cpp
@@ -19,6 +19,19 @@ TEST_F(FrameSensorModel, State) {
EXPECT_EQ(sensorModel->getModelState(), modelState);
}
+// Apply the identity transform to a state
+TEST_F(FrameSensorModel, ApplyTransformToState) {
+ std::string modelState = sensorModel->getModelState();
+
+ ale::Rotation r; // identity rotation
+ ale::Vec3d t; // zero translation
+
+ std::string modelState_out = modelState;
+ UsgsAstroFrameSensorModel::applyTransformToState(r, t, modelState_out);
+
+ EXPECT_EQ(modelState, modelState_out);
+}
+
// centered and slightly off-center:
TEST_F(FrameSensorModel, Center) {
csm::ImageCoord imagePt(7.5, 7.5);
diff --git a/tests/SarTests.cpp b/tests/SarTests.cpp
index 71b0a0e087243b96b0620e4ca48d1b47e22f21ee..a7cf8bf8644f0946cfb3ebd9d00cc5907e08bc58 100644
--- a/tests/SarTests.cpp
+++ b/tests/SarTests.cpp
@@ -78,6 +78,29 @@ TEST_F(SarSensorModel, State) {
EXPECT_TRUE(differences.empty());
}
+// Apply the identity transform to a state
+TEST_F(SarSensorModel, ApplyTransformToState) {
+ std::string modelState = sensorModel->getModelState();
+
+ ale::Rotation r; // identity rotation
+ ale::Vec3d t; // zero translation
+
+ // The input state has some "-0" values which get transformed by
+ // applying the identity transform into "0", which results in the
+ // state string changing. Hence, for this test, compare the results
+ // of applying the identity transform once vs twice, which are the
+ // same.
+
+ // First application
+ UsgsAstroSarSensorModel::applyTransformToState(r, t, modelState);
+
+ // Second application
+ std::string modelState2 = modelState;
+ UsgsAstroSarSensorModel::applyTransformToState(r, t, modelState2);
+
+ EXPECT_EQ(modelState, modelState2);
+}
+
TEST_F(SarSensorModel, Center) {
csm::ImageCoord imagePt(500.0, 500.0);
csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0);