From 9414c80a487258504d9591b5c3ab8e763a9ba7c1 Mon Sep 17 00:00:00 2001
From: Oleg Alexandrov <oleg.alexandrov@gmail.com>
Date: Sun, 8 Aug 2021 14:22:39 -0700
Subject: [PATCH] Add the ability to apply a transform to a linescan model
(#340)
* Add the ablility to apply a rotation and translation to a linescan model state
* Make applyTransformToState static
Conflicts:
include/usgscsm/UsgsAstroLsSensorModel.h
src/UsgsAstroLsSensorModel.cpp
* Add a test for applyTransformToState and make it public
Minor tidying
Rm unneeded whitespace
Reword comment
---
include/usgscsm/UsgsAstroLsSensorModel.h | 6 ++++
include/usgscsm/Utilities.h | 8 +++++
src/UsgsAstroLsSensorModel.cpp | 33 +++++++++++++++--
src/Utilities.cpp | 46 ++++++++++++++++++++++++
tests/LineScanCameraTests.cpp | 23 ++++++++++++
5 files changed, 114 insertions(+), 2 deletions(-)
diff --git a/include/usgscsm/UsgsAstroLsSensorModel.h b/include/usgscsm/UsgsAstroLsSensorModel.h
index 0a3b721..3ce3880 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 2e41bfd..84fabec 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 a8a87f6..b329d34 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 279c46d..df3e5d2 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 a36be71..8ae9bd8 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) {
--
GitLab