diff --git a/include/usgscsm/UsgsAstroSarSensorModel.h b/include/usgscsm/UsgsAstroSarSensorModel.h index 3855911d171b88928cd8559a1eb74ee2097f141a..118f9f4eb07deaf7900027417a4f5deca7e00bb9 100644 --- a/include/usgscsm/UsgsAstroSarSensorModel.h +++ b/include/usgscsm/UsgsAstroSarSensorModel.h @@ -201,6 +201,8 @@ class UsgsAstroSarSensorModel : public csm::RasterGM, virtual public csm::Settab double slantRangeToGroundRange(const csm::EcefCoord& groundPt, double time, double slantRange, double tolerance) const; + double groundRangeToSlantRange(double groundRange, const std::vector<double> &coeffs) const; + csm::EcefVector getSpacecraftPosition(double time) const; csm::EcefVector getSpacecraftVelocity(double time) const; std::vector<double> getRangeCoefficients(double time) const; diff --git a/include/usgscsm/Utilities.h b/include/usgscsm/Utilities.h index 51fb78d07abab4eb8cb8812a45584199a620f0ad..b1d2abe9a4ccd0b3c253aca2d94835d2a2c0b521 100644 --- a/include/usgscsm/Utilities.h +++ b/include/usgscsm/Utilities.h @@ -10,6 +10,7 @@ #include <json/json.hpp> +#include <csm.h> #include <Warning.h> // methods pulled out of los2ecf and computeViewingPixel @@ -84,6 +85,21 @@ double secantRoot( double epsilon = 1e-10, int maxIterations = 30); +// Vector operations +csm::EcefVector operator*(double scalar, const csm::EcefVector &vec); +csm::EcefVector operator*(const csm::EcefVector &vec, double scalar); +csm::EcefVector operator/(const csm::EcefVector &vec, double scalar); +csm::EcefVector operator+(const csm::EcefVector &vec1, const csm::EcefVector &vec2); +csm::EcefVector operator-(const csm::EcefVector &vec1, const csm::EcefVector &vec2); +double dot(const csm::EcefVector &vec1, const csm::EcefVector &vec2); +csm::EcefVector cross(const csm::EcefVector &vec1, const csm::EcefVector &vec2); +double norm(const csm::EcefVector &vec); +csm::EcefVector normalized(const csm::EcefVector &vec); +// The projection of vec1 onto vec2. The component of vec1 that is parallel to vec2 +csm::EcefVector projection(const csm::EcefVector &vec1, const csm::EcefVector &vec2); +// The rejection of vec1 onto vec2. The component of vec1 that is orthogonal to vec2 +csm::EcefVector rejection(const csm::EcefVector &vec1, const csm::EcefVector &vec2); + // Methods for checking/accessing the ISD double metric_conversion(double val, std::string from, std::string to="m"); diff --git a/src/UsgsAstroSarSensorModel.cpp b/src/UsgsAstroSarSensorModel.cpp index 8f44b94c7b506436a42e20e4977f975d69633c24..c6c2bf6cf764984ea9dd39122d2442ef1a13b3ac 100644 --- a/src/UsgsAstroSarSensorModel.cpp +++ b/src/UsgsAstroSarSensorModel.cpp @@ -360,20 +360,15 @@ double UsgsAstroSarSensorModel::dopplerShift( csm::EcefCoord groundPt, double tolerance) const { - std::function<double(double)> dopplerShiftFunction = [this, groundPt](double time) { + csm::EcefVector groundVec(groundPt.x ,groundPt.y, groundPt.z); + std::function<double(double)> dopplerShiftFunction = [this, groundVec](double time) { csm::EcefVector spacecraftPosition = getSpacecraftPosition(time); csm::EcefVector spacecraftVelocity = getSpacecraftVelocity(time); - double lookVector[3]; + csm::EcefVector lookVector = spacecraftPosition - groundVec; - lookVector[0] = spacecraftPosition.x - groundPt.x; - lookVector[1] = spacecraftPosition.y - groundPt.y; - lookVector[2] = spacecraftPosition.z - groundPt.z; + double slantRange = norm(lookVector); - double slantRange = sqrt(pow(lookVector[0], 2) + pow(lookVector[1], 2) + pow(lookVector[2], 2)); - - - double dopplerShift = -2.0 * (lookVector[0]*spacecraftVelocity.x + lookVector[1]*spacecraftVelocity.y - + lookVector[2]*spacecraftVelocity.z)/(slantRange * m_wavelength); + double dopplerShift = -2.0 * dot(lookVector, spacecraftVelocity) / (slantRange * m_wavelength); return dopplerShift; }; @@ -386,13 +381,9 @@ double UsgsAstroSarSensorModel::dopplerShift( double UsgsAstroSarSensorModel::slantRange(csm::EcefCoord surfPt, double time) const { + csm::EcefVector surfVec(surfPt.x ,surfPt.y, surfPt.z); csm::EcefVector spacecraftPosition = getSpacecraftPosition(time); - double lookVector[3]; - - lookVector[0] = spacecraftPosition.x - surfPt.x; - lookVector[1] = spacecraftPosition.y - surfPt.y; - lookVector[2] = spacecraftPosition.z - surfPt.z; - return sqrt(pow(lookVector[0], 2) + pow(lookVector[1], 2) + pow(lookVector[2], 2)); + return norm(spacecraftPosition - surfVec); } double UsgsAstroSarSensorModel::slantRangeToGroundRange( @@ -405,8 +396,8 @@ double UsgsAstroSarSensorModel::slantRangeToGroundRange( // Calculates the ground range from the slant range. std::function<double(double)> slantRangeToGroundRangeFunction = - [coeffs, slantRange](double groundRange){ - return slantRange - (coeffs[0] + groundRange * (coeffs[1] + groundRange * (coeffs[2] + groundRange * coeffs[3]))); + [this, coeffs, slantRange](double groundRange){ + return slantRange - groundRangeToSlantRange(groundRange, coeffs); }; // Need to come up with an initial guess when solving for ground @@ -424,6 +415,11 @@ double UsgsAstroSarSensorModel::slantRangeToGroundRange( return brentRoot(minGroundRangeGuess, maxGroundRangeGuess, slantRangeToGroundRangeFunction, tolerance); } +double UsgsAstroSarSensorModel::groundRangeToSlantRange(double groundRange, const std::vector<double> &coeffs) const +{ + return coeffs[0] + groundRange * (coeffs[1] + groundRange * (coeffs[2] + groundRange * coeffs[3])); +} + csm::ImageCoordCovar UsgsAstroSarSensorModel::groundToImage( const csm::EcefCoordCovar& groundPt, @@ -443,7 +439,36 @@ csm::EcefCoord UsgsAstroSarSensorModel::imageToGround( double* achievedPrecision, csm::WarningList* warnings) const { - return csm::EcefCoord(0.0, 0.0, 0.0); + double time = m_startingEphemerisTime + (imagePt.line - 0.5) * m_exposureDuration; + double groundRange = imagePt.samp * m_scaledPixelWidth; + std::vector<double> coeffs = getRangeCoefficients(time); + double slantRange = groundRangeToSlantRange(groundRange, coeffs); + + // Compute the in-track, cross-track, nadir, coordinate system to solve in + csm::EcefVector spacecraftPosition = getSpacecraftPosition(time); + double positionMag = norm(spacecraftPosition); + csm::EcefVector spacecraftVelocity = getSpacecraftVelocity(time); + // In-track unit vector + csm::EcefVector vHat = normalized(spacecraftVelocity); + // Nadir unit vector + csm::EcefVector tHat = normalized(rejection(spacecraftPosition, vHat)); + // Cross-track unit vector + csm::EcefVector uHat = cross(vHat, tHat); + + // Compute the spacecraft position in the new coordinate system + // The cross-track unit vector is orthogonal to the position so we ignore it + double nadirComp = dot(spacecraftPosition, tHat); + double inTrackComp = dot(spacecraftPosition, vHat); + + // Compute the substituted values + // TODO properly handle ellipsoid radii + double radiusSqr = m_majorAxis * m_majorAxis; + double alpha = (radiusSqr - slantRange * slantRange - positionMag * positionMag) / (2 * nadirComp); + // TODO use right/left look to determine +/- + double beta = -sqrt(slantRange * slantRange - alpha * alpha); + csm::EcefVector groundVec = alpha * tHat + beta * uHat + spacecraftPosition; + + return csm::EcefCoord(groundVec.x, groundVec.y, groundVec.z); } csm::EcefCoordCovar UsgsAstroSarSensorModel::imageToGround( diff --git a/src/Utilities.cpp b/src/Utilities.cpp index 08101a85b3b0f719e30631dcf039969706ac7908..38cbfa7cdbcbef78a8e48d3bb9c6a25990fe1102 100644 --- a/src/Utilities.cpp +++ b/src/Utilities.cpp @@ -405,6 +405,74 @@ double secantRoot(double lowerBound, double upperBound, std::function<double(dou } +csm::EcefVector operator*(double scalar, const csm::EcefVector &vec) +{ + return csm::EcefVector( + scalar * vec.x, + scalar * vec.y, + scalar * vec.z); +} + +csm::EcefVector operator*(const csm::EcefVector &vec, double scalar) +{ + return scalar * vec; +} + +csm::EcefVector operator/(const csm::EcefVector &vec, double scalar) +{ + return 1.0 / scalar * vec; +} + +csm::EcefVector operator+(const csm::EcefVector &vec1, const csm::EcefVector &vec2) +{ + return csm::EcefVector( + vec1.x + vec2.x, + vec1.y + vec2.y, + vec1.z + vec2.z); +} + +csm::EcefVector operator-(const csm::EcefVector &vec1, const csm::EcefVector &vec2) +{ + return csm::EcefVector( + vec1.x - vec2.x, + vec1.y - vec2.y, + vec1.z - vec2.z); +} + +double dot(const csm::EcefVector &vec1, const csm::EcefVector &vec2) +{ + return vec1.x * vec2.x + vec1.y * vec2.y + vec1.z * vec2.z; +} + +csm::EcefVector cross(const csm::EcefVector &vec1, const csm::EcefVector &vec2) +{ + return csm::EcefVector( + vec1.y * vec2.z - vec1.z * vec2.y, + vec1.z * vec2.x - vec1.x * vec2.z, + vec1.x * vec2.y - vec1.y * vec2.x); +} + +double norm(const csm::EcefVector &vec) +{ + return sqrt(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z); +} + +csm::EcefVector normalized(const csm::EcefVector &vec) +{ + return vec / norm(vec); +} + +csm::EcefVector projection(const csm::EcefVector &vec1, const csm::EcefVector &vec2) +{ + return dot(vec1, vec2) / dot(vec2, vec2) * vec2; +} + +csm::EcefVector rejection(const csm::EcefVector &vec1, const csm::EcefVector &vec2) +{ + return vec1 - projection(vec1, vec2); +} + + // convert a measurement double metric_conversion(double val, std::string from, std::string to) { json typemap = { diff --git a/tests/SarTests.cpp b/tests/SarTests.cpp index 59cb3727297c9d69176436cb1311b30dec4ecf9e..5cff6a0e63666d5045f1eaed463af66d7729ad85 100644 --- a/tests/SarTests.cpp +++ b/tests/SarTests.cpp @@ -44,11 +44,12 @@ TEST_F(SarSensorModel, State) { } TEST_F(SarSensorModel, Center) { - csm::ImageCoord imagePt(7.5, 7.5); + csm::ImageCoord imagePt(500.0, 500.0); csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); - EXPECT_NEAR(groundPt.x, 0, 1e-8); - EXPECT_NEAR(groundPt.y, 0, 1e-8); - EXPECT_NEAR(groundPt.z, 0, 1e-8); + // TODO these tolerances are bad + EXPECT_NEAR(groundPt.x, 1737391.90602155, 1e-2); + EXPECT_NEAR(groundPt.y, 3749.98835331, 1e-2); + EXPECT_NEAR(groundPt.z, -3749.99708833, 1e-2); } TEST_F(SarSensorModel, GroundToImage) { diff --git a/tests/UtilitiesTests.cpp b/tests/UtilitiesTests.cpp index 2400cb372e26edefc98a50b1418f81b831f0349d..ba98ca736c969b2544047a23dcb11c1cc3d6af21 100644 --- a/tests/UtilitiesTests.cpp +++ b/tests/UtilitiesTests.cpp @@ -358,3 +358,137 @@ TEST(UtilitiesTests, secantRoot) { EXPECT_NEAR(secantRoot(0.0, -3.0, testPoly, 1e-10), -1.0, 1e-10); EXPECT_THROW(secantRoot(-1000.0, 1000.0, testPoly), csm::Error); } + +TEST(UtilitiesTests, vectorProduct) { + csm::EcefVector vec(1.0, 2.0, 3.0); + csm::EcefVector leftVec = 2.0 * vec; + csm::EcefVector rightVec = vec * 2; + EXPECT_DOUBLE_EQ(leftVec.x, 2.0); + EXPECT_DOUBLE_EQ(leftVec.y, 4.0); + EXPECT_DOUBLE_EQ(leftVec.z, 6.0); + EXPECT_DOUBLE_EQ(rightVec.x, 2.0); + EXPECT_DOUBLE_EQ(rightVec.y, 4.0); + EXPECT_DOUBLE_EQ(rightVec.z, 6.0); +} + +TEST(UtilitiesTests, vectorDivision) { + csm::EcefVector vec(2.0, 4.0, 6.0); + csm::EcefVector divVec = vec / 2.0; + EXPECT_DOUBLE_EQ(divVec.x, 1.0); + EXPECT_DOUBLE_EQ(divVec.y, 2.0); + EXPECT_DOUBLE_EQ(divVec.z, 3.0); +} + +TEST(UtilitiesTests, vectorAddition) { + csm::EcefVector vec1(2.0, 4.0, 6.0); + csm::EcefVector vec2(1.0, 2.0, 3.0); + csm::EcefVector sumVec = vec1 + vec2; + EXPECT_DOUBLE_EQ(sumVec.x, 3.0); + EXPECT_DOUBLE_EQ(sumVec.y, 6.0); + EXPECT_DOUBLE_EQ(sumVec.z, 9.0); +} + +TEST(UtilitiesTests, vectorSubtraction) { + csm::EcefVector vec1(2.0, 4.0, 6.0); + csm::EcefVector vec2(1.0, 2.0, 3.0); + csm::EcefVector diffVec = vec1 - vec2; + EXPECT_DOUBLE_EQ(diffVec.x, 1.0); + EXPECT_DOUBLE_EQ(diffVec.y, 2.0); + EXPECT_DOUBLE_EQ(diffVec.z, 3.0); +} + +TEST(UtilitiesTests, vectorDot) { + csm::EcefVector unitX(1.0, 0.0, 0.0); + csm::EcefVector unitY(0.0, 1.0, 0.0); + csm::EcefVector unitZ(0.0, 0.0, 1.0); + csm::EcefVector testVec(1.0, 2.0, 3.0); + EXPECT_DOUBLE_EQ(dot(testVec, unitX), 1.0); + EXPECT_DOUBLE_EQ(dot(testVec, unitY), 2.0); + EXPECT_DOUBLE_EQ(dot(testVec, unitZ), 3.0); +} + +TEST(UtilitiesTests, vectorCross) { + csm::EcefVector unitX(1.0, 0.0, 0.0); + csm::EcefVector unitY(0.0, 1.0, 0.0); + csm::EcefVector unitZ(0.0, 0.0, 1.0); + + csm::EcefVector unitXY = cross(unitX, unitY); + EXPECT_DOUBLE_EQ(unitXY.x, 0.0); + EXPECT_DOUBLE_EQ(unitXY.y, 0.0); + EXPECT_DOUBLE_EQ(unitXY.z, 1.0); + + + csm::EcefVector unitYX = cross(unitY, unitX); + EXPECT_DOUBLE_EQ(unitYX.x, 0.0); + EXPECT_DOUBLE_EQ(unitYX.y, 0.0); + EXPECT_DOUBLE_EQ(unitYX.z, -1.0); + + csm::EcefVector unitXZ = cross(unitX, unitZ); + EXPECT_DOUBLE_EQ(unitXZ.x, 0.0); + EXPECT_DOUBLE_EQ(unitXZ.y, -1.0); + EXPECT_DOUBLE_EQ(unitXZ.z, 0.0); + + + csm::EcefVector unitZX = cross(unitZ, unitX); + EXPECT_DOUBLE_EQ(unitZX.x, 0.0); + EXPECT_DOUBLE_EQ(unitZX.y, 1.0); + EXPECT_DOUBLE_EQ(unitZX.z, 0.0); + + csm::EcefVector unitYZ = cross(unitY, unitZ); + EXPECT_DOUBLE_EQ(unitYZ.x, 1.0); + EXPECT_DOUBLE_EQ(unitYZ.y, 0.0); + EXPECT_DOUBLE_EQ(unitYZ.z, 0.0); + + + csm::EcefVector unitZY = cross(unitZ, unitY); + EXPECT_DOUBLE_EQ(unitZY.x, -1.0); + EXPECT_DOUBLE_EQ(unitZY.y, 0.0); + EXPECT_DOUBLE_EQ(unitZY.z, 0.0); +} + +TEST(UtilitiesTests, vectorNorm) { + csm::EcefVector testVec(1.0, 2.0, 3.0); + EXPECT_DOUBLE_EQ(norm(testVec), sqrt(14.0)); + csm::EcefVector normVec = normalized(testVec); + EXPECT_DOUBLE_EQ(normVec.x, 1.0 / sqrt(14.0)); + EXPECT_DOUBLE_EQ(normVec.y, 2.0 / sqrt(14.0)); + EXPECT_DOUBLE_EQ(normVec.z, 3.0 / sqrt(14.0)); +} + +TEST(UtilitiesTests, vectorProjection) { + csm::EcefVector unitX(1.0, 0.0, 0.0); + csm::EcefVector unitY(0.0, 1.0, 0.0); + csm::EcefVector unitZ(0.0, 0.0, 1.0); + + csm::EcefVector testVec(1.0, 2.0, 3.0); + + csm::EcefVector projX = projection(testVec, unitX); + EXPECT_DOUBLE_EQ(projX.x, 1.0); + EXPECT_DOUBLE_EQ(projX.y, 0.0); + EXPECT_DOUBLE_EQ(projX.z, 0.0); + + csm::EcefVector rejectX = rejection(testVec, unitX); + EXPECT_DOUBLE_EQ(rejectX.x, 0.0); + EXPECT_DOUBLE_EQ(rejectX.y, 2.0); + EXPECT_DOUBLE_EQ(rejectX.z, 3.0); + + csm::EcefVector projY = projection(testVec, unitY); + EXPECT_DOUBLE_EQ(projY.x, 0.0); + EXPECT_DOUBLE_EQ(projY.y, 2.0); + EXPECT_DOUBLE_EQ(projY.z, 0.0); + + csm::EcefVector rejectY = rejection(testVec, unitY); + EXPECT_DOUBLE_EQ(rejectY.x, 1.0); + EXPECT_DOUBLE_EQ(rejectY.y, 0.0); + EXPECT_DOUBLE_EQ(rejectY.z, 3.0); + + csm::EcefVector projZ = projection(testVec, unitZ); + EXPECT_DOUBLE_EQ(projZ.x, 0.0); + EXPECT_DOUBLE_EQ(projZ.y, 0.0); + EXPECT_DOUBLE_EQ(projZ.z, 3.0); + + csm::EcefVector rejectZ = rejection(testVec, unitZ); + EXPECT_DOUBLE_EQ(rejectZ.x, 1.0); + EXPECT_DOUBLE_EQ(rejectZ.y, 2.0); + EXPECT_DOUBLE_EQ(rejectZ.z, 0.0); +}