From 4131a93d966f5d9da7c141d1285e87952a8d9700 Mon Sep 17 00:00:00 2001
From: Jesse Mapel <jmapel@usgs.gov>
Date: Fri, 10 Apr 2020 17:07:26 -0700
Subject: [PATCH] Fixed constant rotations in Orientations (#350)
---
include/ale/Isd.h | 2 --
include/ale/Orientations.h | 3 --
src/Orientations.cpp | 20 ++++++------
src/Util.cpp | 7 ++--
tests/ctests/IsdTests.cpp | 2 --
tests/ctests/OrientationsTests.cpp | 52 ++++++++++++++++++++++++++++++
6 files changed, 64 insertions(+), 22 deletions(-)
diff --git a/include/ale/Isd.h b/include/ale/Isd.h
index d1126e7..41e489c 100644
--- a/include/ale/Isd.h
+++ b/include/ale/Isd.h
@@ -62,8 +62,6 @@ namespace ale {
PositionInterpolation interpMethod;
- Rotation const_rotation;
-
States inst_pos;
States sun_pos;
diff --git a/include/ale/Orientations.h b/include/ale/Orientations.h
index 5f437db..3a253d1 100644
--- a/include/ale/Orientations.h
+++ b/include/ale/Orientations.h
@@ -21,7 +21,6 @@ namespace ale {
const std::vector<Rotation> &rotations,
const std::vector<double> ×,
const std::vector<Vec3d> &avs = std::vector<ale::Vec3d>(),
- const int refFrame = 1,
const Rotation &constRot = Rotation(1, 0, 0, 0),
const std::vector<int> const_frames = std::vector<int>(),
const std::vector<int> time_dependent_frames = std::vector<int>()
@@ -40,7 +39,6 @@ namespace ale {
std::vector<double> getTimes() const;
std::vector<int> getConstantFrames() const;
std::vector<int> getTimeDependentFrames() const;
- int getReferenceFrame() const;
Rotation getConstantRotation() const;
/**
@@ -89,7 +87,6 @@ namespace ale {
std::vector<int> m_timeDepFrames;
std::vector<int> m_constFrames;
Rotation m_constRotation;
- int m_refFrame;
};
}
diff --git a/src/Orientations.cpp b/src/Orientations.cpp
index c2d0200..004b2c0 100644
--- a/src/Orientations.cpp
+++ b/src/Orientations.cpp
@@ -8,12 +8,11 @@ namespace ale {
const std::vector<Rotation> &rotations,
const std::vector<double> ×,
const std::vector<Vec3d> &avs,
- const int refFrame,
const Rotation &const_rot,
const std::vector<int> const_frames,
const std::vector<int> time_dependent_frames
) :
- m_rotations(rotations), m_avs(avs), m_times(times), m_refFrame(refFrame), m_timeDepFrames(time_dependent_frames), m_constFrames(const_frames), m_constRotation(const_rot) {
+ m_rotations(rotations), m_avs(avs), m_times(times), m_timeDepFrames(time_dependent_frames), m_constFrames(const_frames), m_constRotation(const_rot) {
if (m_rotations.size() < 2 || m_times.size() < 2) {
throw std::invalid_argument("There must be at least two rotations and times.");
}
@@ -48,10 +47,6 @@ namespace ale {
return m_constFrames;
}
- int Orientations::getReferenceFrame() const {
- return m_refFrame;
- }
-
Rotation Orientations::getConstantRotation() const {
return m_constRotation;
}
@@ -62,7 +57,7 @@ namespace ale {
) const {
int interpIndex = interpolationIndex(m_times, time);
double t = (time - m_times[interpIndex]) / (m_times[interpIndex + 1] - m_times[interpIndex]);
- return m_rotations[interpIndex].interpolate(m_rotations[interpIndex + 1], t, interpType);
+ return m_constRotation * m_rotations[interpIndex].interpolate(m_rotations[interpIndex + 1], t, interpType);
}
@@ -80,6 +75,9 @@ namespace ale {
bool invert
) const {
Rotation interpRot = interpolate(time, interpType);
+ if (invert) {
+ interpRot = interpRot.inverse();
+ }
return interpRot(vector);
}
@@ -109,8 +107,10 @@ namespace ale {
std::vector<Rotation> mergedRotations;
std::vector<Vec3d> mergedAvs;
for (double time: mergedTimes) {
+ // interpolate includes the constant rotation, so invert it to undo that
+ Rotation inverseConst = m_constRotation.inverse();
Rotation rhsRot = rhs.interpolate(time);
- mergedRotations.push_back(interpolate(time)*rhsRot);
+ mergedRotations.push_back(inverseConst*interpolate(time)*rhsRot);
Vec3d combinedAv = rhsRot.inverse()(interpolateAV(time));
Vec3d rhsAv = rhs.interpolateAV(time);
combinedAv.x += rhsAv.x;
@@ -133,10 +133,10 @@ namespace ale {
updatedRotations.push_back(m_rotations[i]*rhs);
}
- Rotation inverse = rhs.inverse();
+ Rotation inverseRhs = rhs.inverse();
std::vector<Vec3d> updatedAvs;
for (size_t i = 0; i < m_avs.size(); i++) {
- updatedAvs.push_back(inverse(m_avs[i]));
+ updatedAvs.push_back(inverseRhs(m_avs[i]));
}
m_rotations = updatedRotations;
diff --git a/src/Util.cpp b/src/Util.cpp
index 15776d4..60c448f 100644
--- a/src/Util.cpp
+++ b/src/Util.cpp
@@ -534,7 +534,6 @@ Orientations getInstrumentPointing(json isd) {
std::vector<Rotation> rotations = getJsonQuatArray(pointing.at("quaternions"));
std::vector<double> times = getJsonArray<double>(pointing.at("ephemeris_times"));
std::vector<Vec3d> velocities = getJsonVec3dArray(pointing.at("angular_velocities"));
- int refFrame = pointing.at("reference_frame").get<int>();
std::vector<int> constFrames;
if (pointing.find("constant_frames") != pointing.end()){
@@ -553,7 +552,7 @@ Orientations getInstrumentPointing(json isd) {
Rotation constRot(rotArray);
- Orientations orientation(rotations, times, velocities, refFrame, constRot, constFrames, timeDepFrames);
+ Orientations orientation(rotations, times, velocities, constRot, constFrames, timeDepFrames);
return orientation;
@@ -569,8 +568,6 @@ Orientations getBodyRotation(json isd) {
std::vector<double> times = getJsonArray<double>(bodrot.at("ephemeris_times"));
std::vector<Vec3d> velocities = getJsonVec3dArray(bodrot.at("angular_velocities"));
- int refFrame = bodrot.at("reference_frame").get<int>();
-
std::vector<int> constFrames;
if (bodrot.find("constant_frames") != bodrot.end()){
constFrames = getJsonArray<int>(bodrot.at("constant_frames"));
@@ -588,7 +585,7 @@ Orientations getBodyRotation(json isd) {
Rotation constRot(rotArray);
- Orientations orientation(rotations, times, velocities, refFrame, constRot, constFrames, timeDepFrames);
+ Orientations orientation(rotations, times, velocities, constRot, constFrames, timeDepFrames);
return orientation;
} catch (...) {
diff --git a/tests/ctests/IsdTests.cpp b/tests/ctests/IsdTests.cpp
index 5c001bf..b76070b 100644
--- a/tests/ctests/IsdTests.cpp
+++ b/tests/ctests/IsdTests.cpp
@@ -268,7 +268,6 @@ TEST(Isd, GetInstrumentPointing) {
std::vector<int> timeDepFrames = instPointing.getTimeDependentFrames();
ASSERT_EQ(rotations.size(), 2);
- ASSERT_EQ(instPointing.getReferenceFrame(), 2);
ASSERT_EQ(instPointing.getTimes()[0], 300);
ASSERT_EQ(instPointing.getTimes()[1], 600);
@@ -339,7 +338,6 @@ TEST(Isd, GetBodyRotation) {
std::vector<int> timeDepFrames = bodyRot.getTimeDependentFrames();
ASSERT_EQ(rotations.size(), 2);
- ASSERT_EQ(bodyRot.getReferenceFrame(), 2);
ASSERT_EQ(bodyRot.getTimes()[0], 300);
ASSERT_EQ(bodyRot.getTimes()[1], 600);
diff --git a/tests/ctests/OrientationsTests.cpp b/tests/ctests/OrientationsTests.cpp
index 6eb56fc..0b9d511 100644
--- a/tests/ctests/OrientationsTests.cpp
+++ b/tests/ctests/OrientationsTests.cpp
@@ -29,6 +29,18 @@ class OrientationTest : public ::testing::Test {
Orientations orientations;
};
+class ConstOrientationTest : public OrientationTest{
+ protected:
+ void SetUp() override {
+ OrientationTest::SetUp();
+ constRotation = Rotation(0, 1, 0, 0);
+ constOrientations = Orientations(rotations, times, avs, constRotation);
+ }
+
+ Rotation constRotation;
+ Orientations constOrientations;
+};
+
TEST_F(OrientationTest, ConstructorAccessors) {
vector<Rotation> outputRotations = orientations.getRotations();
vector<double> outputTimes = orientations.getTimes();
@@ -130,3 +142,43 @@ TEST_F(OrientationTest, OrientationMultiplication) {
EXPECT_EQ(expectedQuats[i][3], quats[3]);
}
}
+
+TEST_F(ConstOrientationTest, RotateAt) {
+ Vec3d rotatedX = constRotation(orientations.rotateVectorAt(0.0, Vec3d(1.0, 0.0, 0.0)));
+ Vec3d constRotatedX = constOrientations.rotateVectorAt(0.0, Vec3d(1.0, 0.0, 0.0));
+ EXPECT_NEAR(rotatedX.x, constRotatedX.x, 1e-10);
+ EXPECT_NEAR(rotatedX.y, constRotatedX.y, 1e-10);
+ EXPECT_NEAR(rotatedX.z, constRotatedX.z, 1e-10);
+ Vec3d rotatedY = constRotation(orientations.rotateVectorAt(0.0, Vec3d(0.0, 1.0, 0.0)));
+ Vec3d constRotatedY = constOrientations.rotateVectorAt(0.0, Vec3d(0.0, 1.0, 0.0));
+ EXPECT_NEAR(rotatedY.x, constRotatedY.x, 1e-10);
+ EXPECT_NEAR(rotatedY.y, constRotatedY.y, 1e-10);
+ EXPECT_NEAR(rotatedY.z, constRotatedY.z, 1e-10);
+ Vec3d rotatedZ = constRotation(orientations.rotateVectorAt(0.0, Vec3d(0.0, 0.0, 1.0)));
+ Vec3d constRotatedZ = constOrientations.rotateVectorAt(0.0, Vec3d(0.0, 0.0, 1.0));
+ EXPECT_NEAR(rotatedZ.x, constRotatedZ.x, 1e-10);
+ EXPECT_NEAR(rotatedZ.y, constRotatedZ.y, 1e-10);
+ EXPECT_NEAR(rotatedZ.z, constRotatedZ.z, 1e-10);
+}
+
+TEST_F(ConstOrientationTest, OrientationMultiplication) {
+ constOrientations *= orientations;
+ vector<Rotation> outputRotations = constOrientations.getRotations();
+ vector<vector<double>> expectedQuats = {
+ {-0.5, 0.5, 0.5, 0.5},
+ {-0.5,-0.5,-0.5,-0.5},
+ { 1.0, 0.0, 0.0, 0.0}
+ };
+ for (size_t i = 0; i < outputRotations.size(); i++) {
+ vector<double> quats = outputRotations[i].toQuaternion();
+ EXPECT_EQ(expectedQuats[i][0], quats[0]);
+ EXPECT_EQ(expectedQuats[i][1], quats[1]);
+ EXPECT_EQ(expectedQuats[i][2], quats[2]);
+ EXPECT_EQ(expectedQuats[i][3], quats[3]);
+ }
+ vector<double> constQuats = constOrientations.getConstantRotation().toQuaternion();
+ EXPECT_EQ(constQuats[0], 0);
+ EXPECT_EQ(constQuats[1], 1);
+ EXPECT_EQ(constQuats[2], 0);
+ EXPECT_EQ(constQuats[3], 0);
+}
--
GitLab