diff --git a/include/usgscsm/UsgsAstroLsSensorModel.h b/include/usgscsm/UsgsAstroLsSensorModel.h
index 0a3b721b41ca213ac0dbc08c9cccf557949df292..3ce3880f1f380109a6b7c843b51736d03e0571d0 100644
--- a/include/usgscsm/UsgsAstroLsSensorModel.h
+++ b/include/usgscsm/UsgsAstroLsSensorModel.h
@@ -154,6 +154,12 @@ class UsgsAstroLsSensorModel : public csm::RasterGM,
virtual std::string getModelState() const;
+ // 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);
+
// Set the sensor model based on the input state data
void set(const std::string& state_data);
diff --git a/include/usgscsm/Utilities.h b/include/usgscsm/Utilities.h
index 2e41bfda26835801e2703db1328412b1dde2eeaf..84fabec5bfe9b5dd682fdb6e0198d63808db8a3b 100644
--- a/include/usgscsm/Utilities.h
+++ b/include/usgscsm/Utilities.h
@@ -10,6 +10,9 @@
#include <nlohmann/json.hpp>
+#include <ale/Rotation.h>
+#include <ale/Vectors.h>
+
#include <Warning.h>
#include <csm.h>
@@ -168,4 +171,9 @@ std::vector<double> getSensorOrientations(nlohmann::json isd,
double getWavelength(nlohmann::json isd, csm::WarningList *list = nullptr);
nlohmann::json stateAsJson(std::string modelState);
+// Apply transforms to orientations and vectors
+void applyRotationToQuatVec(ale::Rotation const& r, std::vector<double> & quaternions);
+void applyRotationTranslationToXyzVec(ale::Rotation const& r, ale::Vec3d const& t,
+ std::vector<double> & xyz);
+
#endif // INCLUDE_USGSCSM_UTILITIES_H_
diff --git a/src/UsgsAstroLsSensorModel.cpp b/src/UsgsAstroLsSensorModel.cpp
index a8a87f690ffebcc3410c84e8a000ddac2e80e6d5..b329d344c36b159d321b1bf5f7ebed42f04b94a7 100644
--- a/src/UsgsAstroLsSensorModel.cpp
+++ b/src/UsgsAstroLsSensorModel.cpp
@@ -441,6 +441,37 @@ std::string UsgsAstroLsSensorModel::getModelState() const {
return stateString;
}
+//***************************************************************************
+// UsgsAstroLineScannerSensorModel::applyTransformToState
+//***************************************************************************
+void UsgsAstroLsSensorModel::applyTransformToState(ale::Rotation const& r, ale::Vec3d const& t,
+ std::string& stateString) {
+
+ nlohmann::json j = stateAsJson(stateString);
+
+ // Apply rotation to quaternions
+ std::vector<double> quaternions = j["m_quaternions"].get<std::vector<double>>();
+ applyRotationToQuatVec(r, quaternions);
+ j["m_quaternions"] = quaternions;
+
+ // 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;
+
+ // 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;
+
+ // 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();
+}
//***************************************************************************
// UsgsAstroLineScannerSensorModel::reset
//***************************************************************************
@@ -2233,8 +2264,6 @@ std::string UsgsAstroLsSensorModel::constructStateFromIsd(
json jsonIsd = json::parse(imageSupportData);
csm::WarningList* parsingWarnings = new csm::WarningList;
- int num_params = NUM_PARAMETERS;
-
state["m_modelName"] = ale::getSensorModelName(jsonIsd);
state["m_imageIdentifier"] = ale::getImageId(jsonIsd);
state["m_sensorName"] = ale::getSensorName(jsonIsd);
diff --git a/src/Utilities.cpp b/src/Utilities.cpp
index 279c46dbf721cae6e270eb846dac6edf1ca4e389..df3e5d2ea50fce11281a50466a45e2ed1142c7b3 100644
--- a/src/Utilities.cpp
+++ b/src/Utilities.cpp
@@ -1323,3 +1323,49 @@ json stateAsJson(std::string modelState) {
}
return json::parse(modelState.begin() + found, modelState.end());
}
+
+// Apply a rotation to a vector of quaternions.
+void applyRotationToQuatVec(ale::Rotation const& r, std::vector<double> & quaternions) {
+ int num_quat = quaternions.size();
+
+ for (int it = 0; it < num_quat/4; it++) {
+
+ double * q = &quaternions[4*it];
+
+ // Note that quaternion in q is stored in order x, y, z, w, while
+ // the rotation matrix wants it as w, x, y, z.
+
+ std::vector<double> trans_q = (r * ale::Rotation(q[3], q[0], q[1], q[2])).toQuaternion();
+
+ // Modify in-place
+ q[0] = trans_q[1];
+ q[1] = trans_q[2];
+ q[2] = trans_q[3];
+ q[3] = trans_q[0];
+ }
+}
+
+// Apply a rotation and translation to a vector of xyz vectors.
+void applyRotationTranslationToXyzVec(ale::Rotation const& r, ale::Vec3d const& t,
+ std::vector<double> & xyz) {
+
+ int num_xyz = xyz.size();
+ for (int it = 0; it < num_xyz/3; it++) {
+
+ double * p = &xyz[3*it];
+
+ ale::Vec3d p_vec(p[0], p[1], p[2]);
+
+ // Apply the rotation
+ p_vec = r(p_vec);
+
+ // Apply the translation
+ p_vec += t;
+
+ // Modify in-place
+ p[0] = p_vec.x;
+ p[1] = p_vec.y;
+ p[2] = p_vec.z;
+
+ }
+}
diff --git a/tests/LineScanCameraTests.cpp b/tests/LineScanCameraTests.cpp
index a36be717904cc7a7521bf5eb8ad8fd324fcb252a..8ae9bd8dec2c9dd17fb1893f6bb110b234db42c0 100644
--- a/tests/LineScanCameraTests.cpp
+++ b/tests/LineScanCameraTests.cpp
@@ -22,6 +22,29 @@ TEST_F(ConstVelocityLineScanSensorModel, State) {
EXPECT_EQ(sensorModel->getModelState(), modelState);
}
+// Apply the identity transform to a state
+TEST_F(ConstVelocityLineScanSensorModel, 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
+ UsgsAstroLsSensorModel::applyTransformToState(r, t, modelState);
+
+ // Second application
+ std::string modelState2 = modelState;
+ UsgsAstroLsSensorModel::applyTransformToState(r, t, modelState2);
+
+ EXPECT_EQ(modelState, modelState2);
+}
+
// Fly by tests
TEST_F(ConstVelocityLineScanSensorModel, Center) {