From 51bc20aab8ae129951f3066820a2190fd4ebd0ff Mon Sep 17 00:00:00 2001 From: Jesse Mapel <jmapel@usgs.gov> Date: Fri, 5 Feb 2021 16:43:04 -0700 Subject: [PATCH] Added precision setting in locus methods --- src/UsgsAstroFrameSensorModel.cpp | 28 +++++++++++++++++-- src/UsgsAstroLsSensorModel.cpp | 4 +++ src/UsgsAstroSarSensorModel.cpp | 8 ++++++ tests/FrameCameraTests.cpp | 46 +++++++++++++++++++++++++++++++ tests/LineScanCameraTests.cpp | 15 ++++++++-- tests/SarTests.cpp | 20 ++++++++++++-- 6 files changed, 113 insertions(+), 8 deletions(-) diff --git a/src/UsgsAstroFrameSensorModel.cpp b/src/UsgsAstroFrameSensorModel.cpp index d7942b5..33720d3 100644 --- a/src/UsgsAstroFrameSensorModel.cpp +++ b/src/UsgsAstroFrameSensorModel.cpp @@ -276,12 +276,32 @@ csm::EcefLocus UsgsAstroFrameSensorModel::imageToProximateImagingLocus( const csm::ImageCoord &imagePt, const csm::EcefCoord &groundPt, double desiredPrecision, double *achievedPrecision, csm::WarningList *warnings) const { - // Ignore the ground point? MESSAGE_LOG( "Computing imageToProximateImagingLocus(No ground) for point {}, {}, {}, " "with desired precision {}", imagePt.line, imagePt.samp, desiredPrecision); - return imageToRemoteImagingLocus(imagePt); + + // The locus is a straight line so we can just use the remote locus to + // get the direction + csm::EcefLocus remoteLocus = imageToRemoteImagingLocus( + imagePt, desiredPrecision, achievedPrecision, warnings); + + // Find the point on the locus closest to the input ground point + // The direction vector is already normalized so we can skip a division + csm::EcefVector positionToGround( + groundPt.x - remoteLocus.point.x, + groundPt.y - remoteLocus.point.y, + groundPt.z - remoteLocus.point.z); + csm::EcefVector positionToClosest = + dot(remoteLocus.direction, positionToGround) * remoteLocus.direction; + + return csm::EcefLocus( + remoteLocus.point.x + positionToClosest.x, + remoteLocus.point.y + positionToClosest.y, + remoteLocus.point.z + positionToClosest.z, + remoteLocus.direction.x, + remoteLocus.direction.y, + remoteLocus.direction.z); } csm::EcefLocus UsgsAstroFrameSensorModel::imageToRemoteImagingLocus( @@ -320,6 +340,10 @@ csm::EcefLocus UsgsAstroFrameSensorModel::imageToRemoteImagingLocus( sqrt(lookB[0] * lookB[0] + lookB[1] * lookB[1] + lookB[2] * lookB[2]); std::vector<double> lookBUnit{lookB[0] / mag, lookB[1] / mag, lookB[2] / mag}; + if (achievedPrecision) { + *achievedPrecision = 0.0; + } + return csm::EcefLocus(m_currentParameterValue[0], m_currentParameterValue[1], m_currentParameterValue[2], lookBUnit[0], lookBUnit[1], lookBUnit[2]); diff --git a/src/UsgsAstroLsSensorModel.cpp b/src/UsgsAstroLsSensorModel.cpp index 1b45dcb..f28f766 100644 --- a/src/UsgsAstroLsSensorModel.cpp +++ b/src/UsgsAstroLsSensorModel.cpp @@ -1010,6 +1010,10 @@ csm::EcefLocus UsgsAstroLsSensorModel::imageToRemoteImagingLocus( locus.direction.x = -locus.direction.x; locus.direction.y = -locus.direction.y; locus.direction.z = -locus.direction.z; + + if (achieved_precision) { + *achieved_precision = 0.0; + } return locus; } diff --git a/src/UsgsAstroSarSensorModel.cpp b/src/UsgsAstroSarSensorModel.cpp index 523f190..0d37d27 100644 --- a/src/UsgsAstroSarSensorModel.cpp +++ b/src/UsgsAstroSarSensorModel.cpp @@ -658,6 +658,10 @@ csm::EcefLocus UsgsAstroSarSensorModel::imageToProximateImagingLocus( } tangent = normalized(tangent); + if (achievedPrecision) { + *achievedPrecision = 0.0; + } + return csm::EcefLocus(closestVec.x, closestVec.y, closestVec.z, tangent.x, tangent.y, tangent.z); } @@ -689,6 +693,10 @@ csm::EcefLocus UsgsAstroSarSensorModel::imageToRemoteImagingLocus( } tangent = normalized(tangent); + if (achievedPrecision) { + *achievedPrecision = 0.0; + } + return csm::EcefLocus(closestVec.x, closestVec.y, closestVec.z, tangent.x, tangent.y, tangent.z); } diff --git a/tests/FrameCameraTests.cpp b/tests/FrameCameraTests.cpp index a6f2acb..197c30c 100644 --- a/tests/FrameCameraTests.cpp +++ b/tests/FrameCameraTests.cpp @@ -130,6 +130,52 @@ TEST_F(OrbitalFrameSensorModel, GroundPartials) { EXPECT_DOUBLE_EQ(partials[5], 0.0); } +TEST_F(OrbitalFrameSensorModel, ImageToRemoteImagingLocus) { + csm::ImageCoord imagePt(8.0, 8.0); + double precision; + csm::WarningList warnings; + csm::EcefLocus locus = sensorModel->imageToRemoteImagingLocus( + imagePt, 0.001, &precision, &warnings); + csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); + double lookX = groundPt.x - locus.point.x; + double lookY = groundPt.y - locus.point.y; + double lookZ = groundPt.z - locus.point.z; + double lookMag = sqrt(lookX * lookX + lookY * lookY + lookZ * lookZ); + lookX /= lookMag; + lookY /= lookMag; + lookZ /= lookMag; + EXPECT_NEAR(locus.direction.x, lookX, 1e-10); + EXPECT_NEAR(locus.direction.y, lookY, 1e-10); + EXPECT_NEAR(locus.direction.z, lookZ, 1e-10); + EXPECT_NEAR(locus.point.x, 1050000.0, 1e-10); + EXPECT_NEAR(locus.point.y, 0.0, 1e-10); + EXPECT_NEAR(locus.point.z, 0.0, 1e-10); + EXPECT_LT(precision, 0.001); + EXPECT_TRUE(warnings.empty()); +} + +TEST_F(OrbitalFrameSensorModel, ImageToProximateImagingLocus) { + csm::ImageCoord imagePt(8.0, 8.0); + csm::EcefCoord groundPt(1005000.0, 1000.0, 2000.0); + double precision; + csm::WarningList warnings; + csm::EcefLocus remoteLocus = sensorModel->imageToRemoteImagingLocus(imagePt); + csm::EcefLocus locus = sensorModel->imageToProximateImagingLocus( + imagePt, groundPt, 0.001, &precision, &warnings); + // The locus is a straight line so the proximate and remote should have the + // same direction + EXPECT_DOUBLE_EQ(locus.direction.x, remoteLocus.direction.x); + EXPECT_DOUBLE_EQ(locus.direction.y, remoteLocus.direction.y); + EXPECT_DOUBLE_EQ(locus.direction.z, remoteLocus.direction.z); + // The locus is the X-axis so the closest point is just the X component of + // the ground point. + EXPECT_NEAR(locus.point.x, 1005000.0, 1e-10); + EXPECT_NEAR(locus.point.y, 0.0, 1e-10); + EXPECT_NEAR(locus.point.z, 0.0, 1e-10); + EXPECT_LT(precision, 0.001); + EXPECT_TRUE(warnings.empty()); +} + // **************************************************************************** // Modified sensor model tests // **************************************************************************** diff --git a/tests/LineScanCameraTests.cpp b/tests/LineScanCameraTests.cpp index f55f965..b767a54 100644 --- a/tests/LineScanCameraTests.cpp +++ b/tests/LineScanCameraTests.cpp @@ -55,9 +55,11 @@ TEST_F(ConstVelocityLineScanSensorModel, OffBody) { TEST_F(ConstVelocityLineScanSensorModel, ProximateImageLocus) { csm::ImageCoord imagePt(8.0, 8.0); csm::EcefCoord groundPt(10, 2, 1); + double precision; + csm::WarningList warnings; csm::EcefLocus remoteLocus = sensorModel->imageToRemoteImagingLocus(imagePt); - csm::EcefLocus locus = - sensorModel->imageToProximateImagingLocus(imagePt, groundPt); + csm::EcefLocus locus = sensorModel->imageToProximateImagingLocus( + imagePt, groundPt, 0.001, &precision, &warnings); double locusToGroundX = locus.point.x - groundPt.x; double locusToGroundY = locus.point.y - groundPt.y; double locusToGroundZ = locus.point.z - groundPt.z; @@ -68,11 +70,16 @@ TEST_F(ConstVelocityLineScanSensorModel, ProximateImageLocus) { locusToGroundY * locus.direction.y + locusToGroundZ * locus.direction.z, 0.0, 1e-10); + EXPECT_LT(precision, 0.001); + EXPECT_TRUE(warnings.empty()); } TEST_F(ConstVelocityLineScanSensorModel, RemoteImageLocus) { csm::ImageCoord imagePt(8.5, 8.0); - csm::EcefLocus locus = sensorModel->imageToRemoteImagingLocus(imagePt); + double precision; + csm::WarningList warnings; + csm::EcefLocus locus = sensorModel->imageToRemoteImagingLocus( + imagePt, 0.001, &precision, &warnings); csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); double lookX = groundPt.x - locus.point.x; double lookY = groundPt.y - locus.point.y; @@ -87,6 +94,8 @@ TEST_F(ConstVelocityLineScanSensorModel, RemoteImageLocus) { EXPECT_NEAR(locus.point.x, 1000.0, 1e-10); EXPECT_NEAR(locus.point.y, 0.0, 1e-10); EXPECT_NEAR(locus.point.z, 0.0, 1e-10); + EXPECT_LT(precision, 0.001); + EXPECT_TRUE(warnings.empty()); } TEST_F(ConstVelocityLineScanSensorModel, calculateAttitudeCorrection) { diff --git a/tests/SarTests.cpp b/tests/SarTests.cpp index 78e53d8..cd6b21e 100644 --- a/tests/SarTests.cpp +++ b/tests/SarTests.cpp @@ -91,27 +91,41 @@ TEST_F(SarSensorModel, computeGroundPartials) { } TEST_F(SarSensorModel, imageToProximateImagingLocus) { + double precision; + csm::WarningList warnings; csm::EcefLocus locus = sensorModel->imageToProximateImagingLocus( csm::ImageCoord(500.0, 500.0), csm::EcefCoord(1737287.8590770673, -5403.280537826621, - -3849.9796183814506)); + -3849.9796183814506), + 0.001, + &precision, + &warnings); EXPECT_NEAR(locus.point.x, 1737388.1260092105, 1e-2); EXPECT_NEAR(locus.point.y, -5403.0102509726485, 1e-2); EXPECT_NEAR(locus.point.z, -3749.9801945280433, 1e-2); EXPECT_NEAR(locus.direction.x, 0.002701478402694769, 1e-5); EXPECT_NEAR(locus.direction.y, -0.9999963509835628, 1e-5); EXPECT_NEAR(locus.direction.z, -5.830873570731962e-06, 1e-5); + EXPECT_LT(precision, 0.001); + EXPECT_TRUE(warnings.empty()); } TEST_F(SarSensorModel, imageToRemoteImagingLocus) { - csm::EcefLocus locus = - sensorModel->imageToRemoteImagingLocus(csm::ImageCoord(500.0, 500.0)); + double precision; + csm::WarningList warnings; + csm::EcefLocus locus = sensorModel->imageToRemoteImagingLocus( + csm::ImageCoord(500.0, 500.0), + 0.001, + &precision, + &warnings); EXPECT_NEAR(locus.point.x, 1737380.8279381434, 1e-3); EXPECT_NEAR(locus.point.y, 0.0, 1e-3); EXPECT_NEAR(locus.point.z, -3749.964442364465, 1e-3); EXPECT_NEAR(locus.direction.x, 0.0, 1e-8); EXPECT_NEAR(locus.direction.y, -1.0, 1e-8); EXPECT_NEAR(locus.direction.z, 0.0, 1e-8); + EXPECT_LT(precision, 0.001); + EXPECT_TRUE(warnings.empty()); } TEST_F(SarSensorModel, adjustedPositionVelocity) { -- GitLab