diff --git a/src/UsgsAstroFrameSensorModel.cpp b/src/UsgsAstroFrameSensorModel.cpp index c5e07fd19f3f1dccc593161570f4c3b8dda7573c..dc27565c7b76d4cb6aec3c9fb4a887ce3ff44eda 100644 --- a/src/UsgsAstroFrameSensorModel.cpp +++ b/src/UsgsAstroFrameSensorModel.cpp @@ -556,50 +556,40 @@ std::vector<double> UsgsAstroFrameSensorModel::computeGroundPartials(const csm:: &groundPt) const { MESSAGE_LOG(this->m_logger, "Computing ground partials for ground point {}, {}, {}", groundPt.x, groundPt.y, groundPt.z); - // Partials of line, sample wrt X, Y, Z - // Uses collinearity equations - std::vector<double> partials(6, 0.0); + // Partial of line, sample wrt X, Y, Z + double x = groundPt.x; + double y = groundPt.y; + double z = groundPt.z; - double m[3][3]; - calcRotationMatrix(m, m_noAdjustments); - - double xo, yo, zo; - xo = groundPt.x - m_currentParameterValue[0]; - yo = groundPt.y - m_currentParameterValue[1]; - zo = groundPt.z - m_currentParameterValue[2]; - - double u, v, w; - u = m[0][0] * xo + m[0][1] * yo + m[0][2] * zo; - v = m[1][0] * xo + m[1][1] * yo + m[1][2] * zo; - w = m[2][0] * xo + m[2][1] * yo + m[2][2] * zo; - - double fdw, udw, vdw; - fdw = m_focalLength / w; - udw = u / w; - vdw = v / w; - - double upx, vpx, wpx; - upx = m[0][0]; - vpx = m[1][0]; - wpx = m[2][0]; - partials[0] = -fdw * ( upx - udw * wpx ); - partials[3] = -fdw * ( vpx - vdw * wpx ); - - double upy, vpy, wpy; - upy = m[0][1]; - vpy = m[1][1]; - wpy = m[2][1]; - partials[1] = -fdw * ( upy - udw * wpy ); - partials[4] = -fdw * ( vpy - vdw * wpy ); - - double upz, vpz, wpz; - upz = m[0][2]; - vpz = m[1][2]; - wpz = m[2][2]; - partials[2] = -fdw * ( upz - udw * wpz ); - partials[5] = -fdw * ( vpz - vdw * wpz ); - - MESSAGE_LOG(this->m_logger, "Computing ground partials result line: {}, sample: {}, wrt: {} and x: {}, y: {}, z: {}", + csm::ImageCoord ipB = groundToImage(groundPt); + csm::EcefCoord nextPoint = imageToGround(csm::ImageCoord(ipB.line + 1, ipB.samp + 1)); + double dx = nextPoint.x - x; + double dy = nextPoint.y - y; + double dz = nextPoint.z - z; + double pixelGroundSize = sqrt((dx*dx + dy*dy + dz*dz) / 2.0); + + // If the ground size is too small, try the opposite direction + if (pixelGroundSize < 1e-10) { + nextPoint = imageToGround(csm::ImageCoord(ipB.line - 1, ipB.samp - 1)); + dx = nextPoint.x - x; + dy = nextPoint.y - y; + dz = nextPoint.z - z; + pixelGroundSize = sqrt((dx*dx + dy*dy + dz*dz) / 2.0); + } + + csm::ImageCoord ipX = groundToImage(csm::EcefCoord(x + pixelGroundSize, y, z)); + csm::ImageCoord ipY = groundToImage(csm::EcefCoord(x, y + pixelGroundSize, z)); + csm::ImageCoord ipZ = groundToImage(csm::EcefCoord(x, y, z + pixelGroundSize)); + + std::vector<double> partials(6, 0.0); + partials[0] = (ipX.line - ipB.line) / pixelGroundSize; + partials[3] = (ipX.samp - ipB.samp) / pixelGroundSize; + partials[1] = (ipY.line - ipB.line) / pixelGroundSize; + partials[4] = (ipY.samp - ipB.samp) / pixelGroundSize; + partials[2] = (ipZ.line - ipB.line) / pixelGroundSize; + partials[5] = (ipZ.samp - ipB.samp) / pixelGroundSize; + + MESSAGE_LOG(this->m_logger, "Computing ground partials results:\nLine: {}, {}, {}\nSample: {}, {}, {}", partials[0], partials[1], partials[2], partials[3], partials[4], partials[5]); return partials; @@ -854,7 +844,7 @@ void UsgsAstroFrameSensorModel::replaceModelState(const std::string& stringState m_sensorName = state.at("m_sensorName").get<std::string>(); m_collectionIdentifier = state.at("m_collectionIdentifier").get<std::string>(); // Set reference point to the center of the image - m_referencePointXyz = imageToGround(csm::ImageCoord(m_nLines / 2.0, m_nSamples / 2.0)); + m_referencePointXyz = imageToGround(csm::ImageCoord(m_nLines/2.0, m_nSamples/2.0)); m_currentParameterCovariance = state.at("m_currentParameterCovariance").get<std::vector<double>>(); m_logFile = state.at("m_logFile").get<std::string>(); if (m_logFile.empty()) { diff --git a/src/Utilities.cpp b/src/Utilities.cpp index eb5a674a3dff2b8d1a853522003193e31c6a56cf..fe4665585c6f0962c099d0ec20585e65dbe937a1 100644 --- a/src/Utilities.cpp +++ b/src/Utilities.cpp @@ -810,6 +810,8 @@ std::vector<double> getDistortionCoeffs(json isd, csm::WarningList *list) { "Utilities::getDistortion()")); } coefficients = std::vector<double>(20, 0.0); + coefficients[1] = 1.0; + coefficients[12] = 1.0; } } break; diff --git a/tests/Fixtures.h b/tests/Fixtures.h index a020188149cf80a087475773456fa682dd498717..afc69599164ee28782d41d6cc0592ba4a69488d1 100644 --- a/tests/Fixtures.h +++ b/tests/Fixtures.h @@ -62,6 +62,33 @@ class FrameSensorModel : public ::testing::Test { } }; +class OrbitalFrameSensorModel : public ::testing::Test { + protected: + csm::Isd isd; + UsgsAstroFrameSensorModel *sensorModel; + + void SetUp() override { + sensorModel = NULL; + + isd.setFilename("data/orbitalFramer.img"); + UsgsAstroPlugin frameCameraPlugin; + + csm::Model *model = frameCameraPlugin.constructModelFromISD( + isd, + "USGS_ASTRO_FRAME_SENSOR_MODEL"); + sensorModel = dynamic_cast<UsgsAstroFrameSensorModel *>(model); + + ASSERT_NE(sensorModel, nullptr); + } + + void TearDown() override { + if (sensorModel) { + delete sensorModel; + sensorModel = NULL; + } + } +}; + class FrameIsdTest : public ::testing::Test { protected: csm::Isd isd; diff --git a/tests/FrameCameraTests.cpp b/tests/FrameCameraTests.cpp index d2ec2a3e221ad7ed881c3380a50248f0748c62f4..31fd953646f4f77d240218d3c5b22ee31011f267 100644 --- a/tests/FrameCameraTests.cpp +++ b/tests/FrameCameraTests.cpp @@ -79,6 +79,37 @@ TEST_F(FrameSensorModel, getImageIdentifier) { EXPECT_EQ("simpleFramerISD", sensorModel->getImageIdentifier()); } +TEST_F(FrameSensorModel, Inversion) { + csm::ImageCoord imagePt1(9.0, 9.0); + csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt1, 0.0); + csm::ImageCoord imagePt2 = sensorModel->groundToImage(groundPt); + EXPECT_DOUBLE_EQ(imagePt1.line, imagePt2.line); + EXPECT_DOUBLE_EQ(imagePt1.samp, imagePt2.samp); +} + +TEST_F(OrbitalFrameSensorModel, Center) { + csm::ImageCoord imagePt(8.0, 8.0); + csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); + EXPECT_DOUBLE_EQ(groundPt.x, 1000000.0); + EXPECT_DOUBLE_EQ(groundPt.y, 0); + EXPECT_DOUBLE_EQ(groundPt.z, 0); +} + +TEST_F(OrbitalFrameSensorModel, GroundPartials) { + csm::EcefCoord groundPt(1000000.0, 0.0, 0.0); + std::vector<double> partials = sensorModel->computeGroundPartials(groundPt); + // Pixels are 100m + // lines are increasing z and samples are increasing y in body fixed + // lines partials should be 0 except for the z partial which should be 1/100 + // sample partials should be 0 except for the y partial which should be 1/100 + EXPECT_DOUBLE_EQ(partials[0], 0.0); + EXPECT_DOUBLE_EQ(partials[1], 0.0); + EXPECT_DOUBLE_EQ(partials[2], 1 / 100.0); + EXPECT_DOUBLE_EQ(partials[3], 0.0); + EXPECT_DOUBLE_EQ(partials[4], 1 / 100.0); + EXPECT_DOUBLE_EQ(partials[5], 0.0); +} + // Focal Length Tests: TEST_F(FrameStateTest, FL500_OffBody4) { diff --git a/tests/UtilitiesTests.cpp b/tests/UtilitiesTests.cpp index 549b9c09d78d69b017c24e9bca7b0833fdbbf387..4da9a089000541e8e7eac2b2b6d92d5882468a82 100644 --- a/tests/UtilitiesTests.cpp +++ b/tests/UtilitiesTests.cpp @@ -141,6 +141,21 @@ TEST(UtilitiesTests, computePixelStart) { EXPECT_DOUBLE_EQ(sample, 4.0); } +TEST(UtilitiesTests, computePixelStartSumming) { + double iTransS[] = {0.0, 0.0, 10.0}; + double iTransL[] = {0.0, 10.0, 0.0}; + double line, sample; + computePixel( + -0.5, -0.2, + 8.0, 8.0, + 2.0, 4.0, + 2.0, 1.0, + iTransS, iTransL, + line, sample); + EXPECT_DOUBLE_EQ(line, 0.5); + EXPECT_DOUBLE_EQ(sample, 2.0); +} + TEST(UtilitiesTests, createCameraLookVector) { double cameraLook[3]; createCameraLookVector(0, -0.4, 1, 50, cameraLook); diff --git a/tests/data/orbitalFramer.json b/tests/data/orbitalFramer.json new file mode 100644 index 0000000000000000000000000000000000000000..1ee313046f642b96fe8e3623638a52e5826f40df --- /dev/null +++ b/tests/data/orbitalFramer.json @@ -0,0 +1,59 @@ +{ + "name_model" : "USGS_ASTRO_FRAME_SENSOR_MODEL", + "name_platform" : "TEST_PLATFORM", + "name_sensor" : "TEST_SENSOR", + "center_ephemeris_time": 1000.0, + "detector_sample_summing": 1, + "detector_line_summing": 1, + "focal2pixel_lines": [0.0, 10.0, 0.0], + "focal2pixel_samples": [0.0, 0.0, 10.0], + "focal_length_model": { + "focal_length": 50 + }, + "image_lines": 16, + "image_samples": 16, + "detector_center" : { + "line" : 8.0, + "sample" : 8.0 + }, + "optical_distortion": { + "radial": { + "coefficients": [0.0, 0.0, 0.0] + } + }, + "radii": { + "semimajor": 1000, + "semiminor": 1000, + "unit":"km" + }, + "reference_height": { + "maxheight": 1000, + "minheight": -1000, + "unit": "m" + }, + "sensor_position": { + "unit": "km", + "positions": [ + [1050.0, 0.0, 0.0] + ], + "velocities": [ + [ 0.0, 0.0, -1.0] + ] + }, + "sensor_orientation": { + "quaternions": [ + [0.0, -0.707106781186547, 0, 0.707106781186547] + ] + }, + "starting_detector_line": 0, + "starting_detector_sample": 0, + "sun_position": { + "positions": [ + [100.0, 100.0, 0.0] + ], + "velocities": [ + [0.0, 0.0, 0.0] + ], + "unit": "m" + } +}