From 76a95abdc3f970f54c4df23067a50783075d105a Mon Sep 17 00:00:00 2001
From: Jesse Mapel <jmapel@usgs.gov>
Date: Fri, 26 Apr 2019 10:27:14 -0700
Subject: [PATCH] Normalized quaternion in framer (#229)
* Normalized quaternion in framer
* Made quaternion delta betta
* Clarified comment about parameter partial delta
---
src/UsgsAstroFrameSensorModel.cpp | 14 +++++++++++++-
1 file changed, 13 insertions(+), 1 deletion(-)
diff --git a/src/UsgsAstroFrameSensorModel.cpp b/src/UsgsAstroFrameSensorModel.cpp
index 67f80c0..7e622e2 100644
--- a/src/UsgsAstroFrameSensorModel.cpp
+++ b/src/UsgsAstroFrameSensorModel.cpp
@@ -500,7 +500,13 @@ csm::RasterGM::SensorPartials UsgsAstroFrameSensorModel::computeSensorPartials(i
with point: {}, {}, index: {}, and desiredPrecision: {}",
groundPt.x, groundPt.y, groundPt.z, imagePt.line, imagePt.samp,
index, desiredPrecision);
- const double delta = 1.0;
+ double delta = 1.0;
+ // The rotation parameters will usually be small (<1),
+ // so a delta of 1.0 is too small.
+ // Rotation parameters are indices 3-6
+ if (index > 2 && index < 7) {
+ delta = 0.01;
+ }
// Update the parameter
std::vector<double>adj(NUM_PARAMETERS, 0.0);
adj[index] = delta;
@@ -1207,6 +1213,12 @@ void UsgsAstroFrameSensorModel::calcRotationMatrix(
double z = m_currentParameterValue[5];
double w = m_currentParameterValue[6];
+ double norm = sqrt(x * x + y * y + z * z + w * w);
+ x /= norm;
+ y /= norm;
+ w /= norm;
+ z /= norm;
+
m[0][0] = w*w + x*x - y*y - z*z;
m[0][1] = 2 * (x*y - w*z);
m[0][2] = 2 * (w*y + x*z);
--
GitLab