From 24b2382de86c47993b19f549c2a5e21bfa2f61bd Mon Sep 17 00:00:00 2001 From: acpaquette <acp263@nau.edu> Date: Fri, 15 Feb 2019 13:10:06 -0700 Subject: [PATCH] Extracts distortion models from the Line Scanner and Framer cameras into its own function set (#172) * Moved transverse distortion from line scanner into new file * Updated fixtures for distortion tests * Forgot a semicolon * Extracted radial destortion, and built associated tests * Removed invertDistortion commented code from Linescanner * Reconstructed distortion functions return results * Incorperated invertDistortion function * Corrected spelling, and removed commented out functions * Renamed all dpoint variables to distortionPoint * Removed computeUndistortedFocalPlaneCoordinates function * Renamed distortionPoint to undistortedPoint and distortedPoint where applicable * Fixed typo * Removed doc string parameters * Further extracted transverse destortion out of the Framer --- CMakeLists.txt | 5 +- include/usgscsm/Distortion.h | 25 ++ include/usgscsm/UsgsAstroFrameSensorModel.h | 9 +- include/usgscsm/UsgsAstroLsSensorModel.h | 11 - src/Distortion.cpp | 254 ++++++++++++++++++++ src/UsgsAstroFrameSensorModel.cpp | 220 ++--------------- src/UsgsAstroLsSensorModel.cpp | 93 +------ tests/CMakeLists.txt | 3 +- tests/DistortionTests.cpp | 154 ++++++++++++ tests/Fixtures.h | 2 + tests/FrameCameraTests.cpp | 159 +----------- 11 files changed, 474 insertions(+), 461 deletions(-) create mode 100644 include/usgscsm/Distortion.h create mode 100644 src/Distortion.cpp create mode 100644 tests/DistortionTests.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 76ab4a6..9e5226c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,7 +18,7 @@ else() PATH_SUFFIXES "csm" PATHS $ENV{CONDA_PREFIX}/include/) find_library(CSM_LIBRARY csmapi PATHS $ENV{CONDA_PREFIX}/lib) - + message("--Found CSM Library: ${CSM_LIBRARY}") message("--Found CSM Include Directory: ${CSM_INCLUDE_DIR}") endif(BUILD_CSM) @@ -26,7 +26,8 @@ endif(BUILD_CSM) add_library(usgscsm SHARED src/UsgsAstroPlugin.cpp src/UsgsAstroFrameSensorModel.cpp - src/UsgsAstroLsSensorModel.cpp) + src/UsgsAstroLsSensorModel.cpp + src/Distortion.cpp) set_target_properties(usgscsm PROPERTIES VERSION ${PROJECT_VERSION} diff --git a/include/usgscsm/Distortion.h b/include/usgscsm/Distortion.h new file mode 100644 index 0000000..6ae0d04 --- /dev/null +++ b/include/usgscsm/Distortion.h @@ -0,0 +1,25 @@ +#ifndef Distortion_h +#define Distortion_h + +#include <vector> +#include <math.h> +#include <tuple> + +// Transverse Distortion +std::tuple<double, double> removeDistortion(double dx, double dy, + const std::vector<double> &odtX, const std::vector<double> &odtY); + +std::vector<std::vector<double>> distortionJacobian(double x, double y, + const std::vector<double> &odtX, const std::vector<double> &odtY); + +std::tuple<double, double> distortionFunction(double ux, double uy, + const std::vector<double> &odtX, const std::vector<double> &odtY); + +// Radial Distortion +std::tuple<double, double> removeDistortion(double inFocalPlaneX, double inFocalPlaneY, + const double opticalDistCoef[3], double tolerance = 1.0E-6); + +std::tuple<double, double> invertDistortion(double inFocalPlaneX, double inFocalPlaneY, + const double opticalDistCoef[3], double desiredPrecision, double tolerance = 1.0E-6); + +#endif diff --git a/include/usgscsm/UsgsAstroFrameSensorModel.h b/include/usgscsm/UsgsAstroFrameSensorModel.h index d9a62d7..3a823e7 100644 --- a/include/usgscsm/UsgsAstroFrameSensorModel.h +++ b/include/usgscsm/UsgsAstroFrameSensorModel.h @@ -8,6 +8,7 @@ #include <gtest/gtest.h> #include "RasterGM.h" #include "CorrelationModel.h" +#include "Distortion.h" #include <json.hpp> using json = nlohmann::json; @@ -320,12 +321,6 @@ protected: FRIEND_TEST(FrameSensorModel, setFocalPlane_AlternatingOnes); FRIEND_TEST(FrameSensorModel, distortMe_AlternatingOnes); - virtual bool setFocalPlane(double dx,double dy,double &undistortedX,double &undistortedY) const; - virtual void distortionFunction(double ux, double uy, double &dx, double &dy) const; - virtual void distortionJacobian(double x, double y, double &Jxx, - double &Jxy, double &Jyx, double &Jyy) const; - - private: // Input parameters @@ -373,7 +368,7 @@ protected: int m_nSamples; int m_nParameters; - csm::EcefCoord m_referencePointXyz; + csm::EcefCoord m_referencePointXyz; json _state; static const int _NUM_STATE_KEYWORDS; diff --git a/include/usgscsm/UsgsAstroLsSensorModel.h b/include/usgscsm/UsgsAstroLsSensorModel.h index 6ec28f1..3cd5f31 100644 --- a/include/usgscsm/UsgsAstroLsSensorModel.h +++ b/include/usgscsm/UsgsAstroLsSensorModel.h @@ -924,12 +924,6 @@ private: double& distortedLine, double& distortedSample) const; - void computeUndistortedFocalPlaneCoordinates( - const double& distortedFocalPlaneX, - const double& distortedFocalPlaneY, - double& undistortedFocalPlaneX, - double& undistortedFocalPlaneY) const; - void calculateRotationMatrixFromQuaternions( const double& time, double cameraToBody[9]) const; @@ -949,11 +943,6 @@ private: const std::vector<double>& adj, double attCorr[9]) const; - void reconstructSensorDistortion( - double& focalX, - double& focalY, - const double& desiredPrecision) const; - // This method computes the imaging locus. // imaging locus : set of ground points associated with an image pixel. void losToEcf( diff --git a/src/Distortion.cpp b/src/Distortion.cpp new file mode 100644 index 0000000..103aa5c --- /dev/null +++ b/src/Distortion.cpp @@ -0,0 +1,254 @@ +#include "Distortion.h" + +/** + * @brief Compute undistorted focal plane x/y. + * + * Computes undistorted focal plane (x,y) coordinates given a distorted focal plane (x,y) + * coordinate. The undistorted coordinates are solved for using the Newton-Raphson + * method for root-finding if the distortionFunction method is invoked. + * + * @param dx distorted focal plane x in millimeters + * @param dy distorted focal plane y in millimeters + * @param undistortedX The undistorted x coordinate, in millimeters. + * @param undistortedY The undistorted y coordinate, in millimeters. + * + * @return if the conversion was successful + * @todo Review the tolerance and maximum iterations of the root- + * finding algorithm. + * @todo Review the handling of non-convergence of the root-finding + * algorithm. + * @todo Add error handling for near-zero determinant. +*/ +std::tuple<double, double> removeDistortion(double dx, double dy, + const std::vector<double> &odtX, const std::vector<double> &odtY) { + // Solve the distortion equation using the Newton-Raphson method. + // Set the error tolerance to about one millionth of a NAC pixel. + const double tol = 1.4E-5; + + // The maximum number of iterations of the Newton-Raphson method. + const int maxTries = 60; + + double x; + double y; + std::tuple<double, double> undistortedPoint(dx, dy); + std::tuple<double, double> distortedPoint; + + // Initial guess at the root + x = dx; + y = dy; + + distortedPoint = distortionFunction(x, y, odtX, odtY); + + for (int count = 1; ((fabs(std::get<0>(distortedPoint)) +fabs(std::get<1>(distortedPoint))) > tol) && (count < maxTries); count++) { + + distortedPoint = distortionFunction(x, y, odtX, odtY); + + // fx = dx - fx; + // fy = dy - fy; + distortedPoint = std::make_tuple(dx - std::get<0>(distortedPoint), dy - std::get<1>(distortedPoint)); + + std::vector<std::vector<double>> jacobian; + + jacobian = distortionJacobian(x, y, odtX, odtY); + + // Jxx * Jyy - Jxy * Jyx + double determinant = jacobian[0][0] * jacobian[1][1] - jacobian[0][1] * jacobian[1][0]; + if (fabs(determinant) < 1E-6) { + undistortedPoint = std::make_tuple(x, y); + // + // Near-zero determinant. Add error handling here. + // + //-- Just break out and return with no convergence + return undistortedPoint; + } + + //x = x + (Jyy * fx - Jxy * fy) + x = x + (jacobian[1][1] * std::get<0>(distortedPoint) - jacobian[0][1] * std::get<1>(distortedPoint)) / determinant; + // y = y + (Jxx * fy - Jyx * fx) + y = y + (jacobian[0][0] * std::get<1>(distortedPoint) - jacobian[1][0] * std::get<0>(distortedPoint)) / determinant; + } + + if ( (fabs(std::get<0>(distortedPoint)) + fabs(std::get<1>(distortedPoint))) <= tol) { + // The method converged to a root. + undistortedPoint = std::make_tuple(x, y); + } + // Otherwise method did not converge to a root within the maximum + // number of iterations. Return with no distortion. + return undistortedPoint; +} + +/** + * @description Jacobian of the distortion function. The Jacobian was computed + * algebraically from the function described in the distortionFunction + * method. + * + * @param x + * @param y + * @param odtX opticalDistCoef In X + * @param odtY opticalDistCoef In Y + * + * @returns jacobian a jacobian vector of vectors as + [0][0]: xx, [0][1]: xy + [1][0]: yx, [1][1]: yy + */ + +std::vector<std::vector<double>> distortionJacobian(double x, double y, + const std::vector<double> &odtX, const std::vector<double> &odtY) { + + double d_dx[10]; + d_dx[0] = 0; + d_dx[1] = 1; + d_dx[2] = 0; + d_dx[3] = 2 * x; + d_dx[4] = y; + d_dx[5] = 0; + d_dx[6] = 3 * x * x; + d_dx[7] = 2 * x * y; + d_dx[8] = y * y; + d_dx[9] = 0; + double d_dy[10]; + d_dy[0] = 0; + d_dy[1] = 0; + d_dy[2] = 1; + d_dy[3] = 0; + d_dy[4] = x; + d_dy[5] = 2 * y; + d_dy[6] = 0; + d_dy[7] = x * x; + d_dy[8] = 2 * x * y; + d_dy[9] = 3 * y * y; + + std::vector<std::vector<double>> jacobian(2, std::vector<double>(2)); + + jacobian[0][0] = 0; + jacobian[0][1] = 0; + jacobian[1][0] = 0; + jacobian[1][1] = 0; + + for (int i = 0; i < 10; i++) { + jacobian[0][0] = jacobian[0][0] + d_dx[i] * odtX[i]; + jacobian[0][1] = jacobian[0][1] + d_dy[i] * odtX[i]; + jacobian[1][0] = jacobian[1][0] + d_dx[i] * odtY[i]; + jacobian[1][1] = jacobian[1][1] + d_dy[i] * odtY[i]; + } + + return jacobian; +} + +/** + * @description Compute distorted focal plane (dx,dy) coordinate given an undistorted focal + * plane (ux,uy) coordinate. This uses the third order Taylor approximation to the + * distortion model. + * + * @param ux Undistored x + * @param uy Undistored y + * @param odtX opticalDistCoef In X + * @param odtY opticalDistCoef In Y + * + * @returns distortedPoint Newly adjusted focal plane coordinates as an x, y tuple + */ +std::tuple<double, double> distortionFunction(double ux, double uy, + const std::vector<double> &odtX, const std::vector<double> &odtY) { + + double f[10]; + f[0] = 1; + f[1] = ux; + f[2] = uy; + f[3] = ux * ux; + f[4] = ux * uy; + f[5] = uy * uy; + f[6] = ux * ux * ux; + f[7] = ux * ux * uy; + f[8] = ux * uy * uy; + f[9] = uy * uy * uy; + + std::tuple<double, double> distortedPoint(0.0, 0.0); + for (int i = 0; i < 10; i++) { + distortedPoint = std::make_tuple(std::get<0>(distortedPoint) + f[i] * odtX[i], + std::get<1>(distortedPoint) + f[i] * odtY[i]); + } + + return distortedPoint; +} + +/** + * @description Compute undistorted focal plane coordinate given a distorted + * coordinate set and the distortion coefficients + * + * @param inFocalPlaneX Distorted x + * @param inFocalPlaneY Distorted y + * @param opticalDistCoef distortion coefficients + * + * @returns undistortedPoint Newly adjusted focal plane coordinates as an x, y tuple + */ +std::tuple<double, double> removeDistortion(double inFocalPlaneX, double inFocalPlaneY, + const double opticalDistCoef[3], double tolerance) { + double rr = inFocalPlaneX * inFocalPlaneX + inFocalPlaneY * inFocalPlaneY; + std::tuple<double, double> undistortedPoint(inFocalPlaneX, inFocalPlaneY); + + if (rr > tolerance) + { + double dr = opticalDistCoef[0] + (rr * (opticalDistCoef[1] + rr * opticalDistCoef[2])); + undistortedPoint = std::make_tuple(inFocalPlaneX * (1.0 - dr), inFocalPlaneY * (1.0 - dr)); + } + + return undistortedPoint; +} + +/** + * @description Compute undistorted focal plane coordinate given a distorted + * focal plane coordinate. This method works by iteratively adding distortion + * until the new distorted point, r, undistorts to within a tolerance of the + * original point, rp. + * + * @param inFocalPlaneX Distorted x + * @param inFocalPlaneY Distorted y + * @param opticalDistCoef Distortion coefficients + * @param desiredPrecision Convergence precision + * @param tolerance Tolerance of r^2 + * + * @returns undistortedPoint Newly adjusted focal plane coordinates as an x, y tuple + */ +std::tuple<double, double> invertDistortion(double inFocalPlaneX, double inFocalPlaneY, + const double opticalDistCoef[3], double desiredPrecision, double tolerance) { + double rp2 = (inFocalPlaneX * inFocalPlaneX) + + (inFocalPlaneY * inFocalPlaneY); + std::tuple<double, double> undistortedPoint; + + if (rp2 > tolerance) { + double rp = sqrt(rp2); + // Compute first fractional distortion using rp + double drOverR = opticalDistCoef[0] + + (rp2 * (opticalDistCoef[1] + (rp2 * opticalDistCoef[2]))); + // Compute first distorted point estimate, r + double r = rp + (drOverR * rp); + double r_prev, r2_prev; + int iteration = 0; + do { + // Don't get in an end-less loop. This algorithm should + // converge quickly. If not then we are probably way outside + // of the focal plane. Just set the distorted position to the + // undistorted position. Also, make sure the focal plane is less + // than 1km, it is unreasonable for it to grow larger than that. + if (iteration >= 15 || r > 1E9) { + drOverR = 0.0; + break; + } + + r_prev = r; + r2_prev = r * r; + + // Compute new fractional distortion: + drOverR = opticalDistCoef[0] + + (r2_prev * (opticalDistCoef[1] + (r2_prev * opticalDistCoef[2]))); + + // Compute new estimate of r + r = rp + (drOverR * r_prev); + iteration++; + } + while (fabs(r * (1 - drOverR) - rp) > desiredPrecision); + undistortedPoint = std::make_tuple(inFocalPlaneX / (1.0 - drOverR), + inFocalPlaneY / (1.0 - drOverR)); + } + return undistortedPoint; +} diff --git a/src/UsgsAstroFrameSensorModel.cpp b/src/UsgsAstroFrameSensorModel.cpp index 1f9dc37..03f45a7 100644 --- a/src/UsgsAstroFrameSensorModel.cpp +++ b/src/UsgsAstroFrameSensorModel.cpp @@ -1,4 +1,5 @@ #include "UsgsAstroFrameSensorModel.h" +#include "Distortion.h" #include <iomanip> #include <iostream> @@ -134,14 +135,14 @@ csm::ImageCoord UsgsAstroFrameSensorModel::groundToImage( undistortedy = (f * (m[0][1] * xo + m[1][1] * yo + m[2][1] * zo)/denom) + m_linePp; // Apply the distortion to the line/sample location and then convert back to line/sample - double distortedx, distortedy; - distortionFunction(undistortedx, undistortedy, distortedx, distortedy); + std::tuple<double, double> distortedPoint; + distortedPoint = distortionFunction(undistortedx, undistortedy, m_odtX, m_odtY); // Convert distorted mm into line/sample double sample, line; - sample = m_iTransS[0] + m_iTransS[1] * distortedx + m_iTransS[2] * distortedy + m_ccdCenter[1]; - line = m_iTransL[0] + m_iTransL[1] * distortedx + m_iTransL[2] * distortedy + m_ccdCenter[0]; + sample = m_iTransS[0] + m_iTransS[1] * std::get<0>(distortedPoint) + m_iTransS[2] * std::get<1>(distortedPoint) + m_ccdCenter[1]; + line = m_iTransL[0] + m_iTransL[1] * std::get<0>(distortedPoint) + m_iTransL[2] * std::get<1>(distortedPoint) + m_ccdCenter[0]; return csm::ImageCoord(line, sample); } @@ -157,9 +158,9 @@ csm::ImageCoordCovar UsgsAstroFrameSensorModel::groundToImage(const csm::EcefCoo gp.y = groundPt.y; gp.z = groundPt.z; - csm::ImageCoord ip = groundToImage( - gp, desiredPrecision, achievedPrecision, warnings); - csm::ImageCoordCovar result(ip.line, ip.samp); + csm::ImageCoord ip = groundToImage( + gp, desiredPrecision, achievedPrecision, warnings); + csm::ImageCoordCovar result(ip.line, ip.samp); // This is a partial, incorrect implementation to test if SocetGXP needs // this method implemented in order to load the sensor. return result; @@ -192,18 +193,13 @@ csm::EcefCoord UsgsAstroFrameSensorModel::imageToGround(const csm::ImageCoord &i x_camera = m_transX[0] + m_transX[1] * (lo - line_center) + m_transX[2] * (so - sample_center); // Apply the distortion model (remove distortion) - double undistorted_cameraX, undistorted_cameraY = 0.0; - - setFocalPlane(x_camera, y_camera, undistorted_cameraX, undistorted_cameraY); + std::tuple<double, double> undistortedPoint; + undistortedPoint = removeDistortion(x_camera, y_camera, m_odtX, m_odtY); // Now back from distorted mm to pixels - double udx, udy; //distorted line and sample - udx = undistorted_cameraX; - udy = undistorted_cameraY; - - xl = m[0][0] * udx + m[0][1] * udy - m[0][2] * -m_focalLength; - yl = m[1][0] * udx + m[1][1] * udy - m[1][2] * -m_focalLength; - zl = m[2][0] * udx + m[2][1] * udy - m[2][2] * -m_focalLength; + xl = m[0][0] * std::get<0>(undistortedPoint) + m[0][1] * std::get<1>(undistortedPoint) - m[0][2] * - m_focalLength; + yl = m[1][0] * std::get<0>(undistortedPoint) + m[1][1] * std::get<1>(undistortedPoint) - m[1][2] * - m_focalLength; + zl = m[2][0] * std::get<0>(undistortedPoint) + m[2][1] * std::get<1>(undistortedPoint) - m[2][2] * - m_focalLength; double x, y, z; double xc, yc, zc; @@ -251,15 +247,13 @@ csm::EcefLocus UsgsAstroFrameSensorModel::imageToRemoteImagingLocus(const csm::I double focalPlaneY = m_transY[0] + m_transY[1] * row + m_transY[2] * col; // Distort - double undistortedFocalPlaneX = focalPlaneX; - double undistortedFocalPlaneY = focalPlaneY; - - setFocalPlane(focalPlaneX, focalPlaneY, undistortedFocalPlaneX, undistortedFocalPlaneY); + std::tuple<double, double> undistortedPoint; + undistortedPoint = removeDistortion(focalPlaneX, focalPlaneY, m_odtX, m_odtY); // Get rotation matrix and transform to a body-fixed frame double m[3][3]; calcRotationMatrix(m); - std::vector<double> lookC { undistortedFocalPlaneX, undistortedFocalPlaneY, m_focalLength }; + std::vector<double> lookC { std::get<0>(undistortedPoint), std::get<1>(undistortedPoint), m_focalLength }; std::vector<double> lookB { m[0][0] * lookC[0] + m[0][1] * lookC[1] + m[0][2] * lookC[2], m[1][0] * lookC[0] + m[1][1] * lookC[1] + m[1][2] * lookC[2], @@ -652,8 +646,8 @@ std::string UsgsAstroFrameSensorModel::getModelState() const { {"m_currentParameterValue", m_currentParameterValue}, {"m_imageIdentifier", m_imageIdentifier}, {"m_collectionIdentifier", m_collectionIdentifier}, - {"m_referencePointXyz", {m_referencePointXyz.x, - m_referencePointXyz.y, + {"m_referencePointXyz", {m_referencePointXyz.x, + m_referencePointXyz.y, m_referencePointXyz.z}}, {"m_currentParameterCovariance", m_currentParameterCovariance} }; @@ -964,7 +958,7 @@ std::string UsgsAstroFrameSensorModel::constructStateFromIsd(const std::string& state["m_referencePointXyz"] = std::vector<double>(3, 0.0); state["m_currentParameterCovariance"] = std::vector<double>(NUM_PARAMETERS*NUM_PARAMETERS,0.0); - state["m_collectionIdentifier"] = ""; + state["m_collectionIdentifier"] = ""; std::cerr << "Constants Set!" << std::endl; @@ -1107,7 +1101,7 @@ std::vector<double> UsgsAstroFrameSensorModel::getCrossCovarianceMatrix( const GeometricModel &comparisonModel, csm::param::Set pSet, const GeometricModelList &otherModels) const { - + // No correlation between models. const std::vector<int>& indices = getParameterSetIndices(pSet); size_t num_rows = indices.size(); @@ -1207,180 +1201,6 @@ void UsgsAstroFrameSensorModel::losEllipsoidIntersect( } -/** - * @brief Compute undistorted focal plane x/y. - * - * Computes undistorted focal plane (x,y) coordinates given a distorted focal plane (x,y) - * coordinate. The undistorted coordinates are solved for using the Newton-Raphson - * method for root-finding if the distortionFunction method is invoked. - * - * @param dx distorted focal plane x in millimeters - * @param dy distorted focal plane y in millimeters - * @param undistortedX The undistorted x coordinate, in millimeters. - * @param undistortedY The undistorted y coordinate, in millimeters. - * - * @return if the conversion was successful - * @todo Review the tolerance and maximum iterations of the root- - * finding algorithm. - * @todo Review the handling of non-convergence of the root-finding - * algorithm. - * @todo Add error handling for near-zero determinant. -*/ -bool UsgsAstroFrameSensorModel::setFocalPlane(double dx,double dy, - double &undistortedX, - double &undistortedY ) const { - - // Solve the distortion equation using the Newton-Raphson method. - // Set the error tolerance to about one millionth of a NAC pixel. - const double tol = 1.4E-5; - - // The maximum number of iterations of the Newton-Raphson method. - const int maxTries = 60; - - double x; - double y; - double fx; - double fy; - double Jxx; - double Jxy; - double Jyx; - double Jyy; - - // Initial guess at the root - x = dx; - y = dy; - - distortionFunction(x, y, fx, fy); - - - for (int count = 1; ((fabs(fx) +fabs(fy)) > tol) && (count < maxTries); count++) { - - this->distortionFunction(x, y, fx, fy); - - fx = dx - fx; - fy = dy - fy; - - distortionJacobian(x, y, Jxx, Jxy, Jyx, Jyy); - - double determinant = Jxx * Jyy - Jxy * Jyx; - if (fabs(determinant) < 1E-6) { - - undistortedX = x; - undistortedY = y; - // - // Near-zero determinant. Add error handling here. - // - //-- Just break out and return with no convergence - return false; - } - - x = x + (Jyy * fx - Jxy * fy) / determinant; - y = y + (Jxx * fy - Jyx * fx) / determinant; - } - - if ( (fabs(fx) + fabs(fy)) <= tol) { - // The method converged to a root. - undistortedX = x; - undistortedY = y; - return true; - - } - else { - // The method did not converge to a root within the maximum - // number of iterations. Return with no distortion. - undistortedX = dx; - undistortedY = dy; - return false; - } - return true; -} - - -/** - * @description Jacobian of the distortion function. The Jacobian was computed - * algebraically from the function described in the distortionFunction - * method. - * - * @param x - * @param y - * @param Jxx Partial_xx - * @param Jxy Partial_xy - * @param Jyx Partial_yx - * @param Jyy Partial_yy - */ -void UsgsAstroFrameSensorModel::distortionJacobian(double x, double y, double &Jxx, double &Jxy, - double &Jyx, double &Jyy) const { - - double d_dx[10]; - d_dx[0] = 0; - d_dx[1] = 1; - d_dx[2] = 0; - d_dx[3] = 2 * x; - d_dx[4] = y; - d_dx[5] = 0; - d_dx[6] = 3 * x * x; - d_dx[7] = 2 * x * y; - d_dx[8] = y * y; - d_dx[9] = 0; - double d_dy[10]; - d_dy[0] = 0; - d_dy[1] = 0; - d_dy[2] = 1; - d_dy[3] = 0; - d_dy[4] = x; - d_dy[5] = 2 * y; - d_dy[6] = 0; - d_dy[7] = x * x; - d_dy[8] = 2 * x * y; - d_dy[9] = 3 * y * y; - - Jxx = 0.0; - Jxy = 0.0; - Jyx = 0.0; - Jyy = 0.0; - - for (int i = 0; i < 10; i++) { - Jxx = Jxx + d_dx[i] * m_odtX[i]; - Jxy = Jxy + d_dy[i] * m_odtX[i]; - Jyx = Jyx + d_dx[i] * m_odtY[i]; - Jyy = Jyy + d_dy[i] * m_odtY[i]; - } -} - - - -/** - * @description Compute distorted focal plane (dx,dy) coordinate given an undistorted focal - * plane (ux,uy) coordinate. This describes the third order Taylor approximation to the - * distortion model. - * - * @param ux Undistored x - * @param uy Undistored y - * @param dx Result distorted x - * @param dy Result distorted y - */ -void UsgsAstroFrameSensorModel::distortionFunction(double ux, double uy, double &dx, double &dy) const { - - double f[10]; - f[0] = 1; - f[1] = ux; - f[2] = uy; - f[3] = ux * ux; - f[4] = ux * uy; - f[5] = uy * uy; - f[6] = ux * ux * ux; - f[7] = ux * ux * uy; - f[8] = ux * uy * uy; - f[9] = uy * uy * uy; - - dx = 0.0; - dy = 0.0; - for (int i = 0; i < 10; i++) { - dx = dx + f[i] * m_odtX[i]; - dy = dy + f[i] * m_odtY[i]; - } -} - /***** Helper Functions *****/ double UsgsAstroFrameSensorModel::getValue( diff --git a/src/UsgsAstroLsSensorModel.cpp b/src/UsgsAstroLsSensorModel.cpp index c476c4c..ad040c9 100644 --- a/src/UsgsAstroLsSensorModel.cpp +++ b/src/UsgsAstroLsSensorModel.cpp @@ -19,6 +19,7 @@ #define USGS_SENSOR_LIBRARY #include "UsgsAstroLsSensorModel.h" +#include "Distortion.h" #include <algorithm> #include <iostream> @@ -1677,26 +1678,6 @@ void UsgsAstroLsSensorModel::computeDistortedFocalPlaneCoordinates(const double& distortedSample = p21 * t1 + p22 * t2; } -// Compute un-distorted image coordinates in mm / apply lens distortion correction -void UsgsAstroLsSensorModel::computeUndistortedFocalPlaneCoordinates(const double &distortedFocalPlaneX, const double& distortedFocalPlaneY, double& undistortedFocalPlaneX, double& undistortedFocalPlaneY) const{ - undistortedFocalPlaneX = distortedFocalPlaneX; - undistortedFocalPlaneY = distortedFocalPlaneY; - if (m_opticalDistCoef[0] != 0.0 || - m_opticalDistCoef[1] != 0.0 || - m_opticalDistCoef[2] != 0.0) - { - double rr = distortedFocalPlaneX * distortedFocalPlaneX - + distortedFocalPlaneY * distortedFocalPlaneY; - if (rr > 1.0E-6) - { - double dr = m_opticalDistCoef[0] + (rr * (m_opticalDistCoef[1] - + rr * m_opticalDistCoef[2])); - undistortedFocalPlaneX = distortedFocalPlaneX * (1.0 - dr); - undistortedFocalPlaneY = distortedFocalPlaneY * (1.0 - dr); - } - } -}; - // Define imaging ray in image space (In other words, create a look vector in camera space) void UsgsAstroLsSensorModel::createCameraLookVector(const double& undistortedFocalPlaneX, const double& undistortedFocalPlaneY, const std::vector<double>& adj, double cameraLook[]) const{ @@ -1779,59 +1760,6 @@ void UsgsAstroLsSensorModel::calculateAttitudeCorrection(const double& time, con } -// This method works by iteratively adding distortion until the new distorted -// point, r, undistorts to within a tolerance of the original point, rp. -void UsgsAstroLsSensorModel::reconstructSensorDistortion( - double& focalX, - double& focalY, - const double& desiredPrecision) const -{ - if (m_opticalDistCoef[0] != 0.0 || - m_opticalDistCoef[1] != 0.0 || - m_opticalDistCoef[2] != 0.0) - { - double rp2 = (focalX * focalX) + (focalY * focalY); - double tolerance = 1.0E-6; - if (rp2 > tolerance) { - double rp = sqrt(rp2); - // Compute first fractional distortion using rp - double drOverR = m_opticalDistCoef[0] - + (rp2 * (m_opticalDistCoef[1] + (rp2 * m_opticalDistCoef[2]))); - // Compute first distorted point estimate, r - double r = rp + (drOverR * rp); - double r_prev, r2_prev; - int iteration = 0; - do { - // Don't get in an end-less loop. This algorithm should - // converge quickly. If not then we are probably way outside - // of the focal plane. Just set the distorted position to the - // undistorted position. Also, make sure the focal plane is less - // than 1km, it is unreasonable for it to grow larger than that. - if (iteration >= 15 || r > 1E9) { - drOverR = 0.0; - break; - } - - r_prev = r; - r2_prev = r * r; - - // Compute new fractional distortion: - drOverR = m_opticalDistCoef[0] - + (r2_prev * (m_opticalDistCoef[1] + (r2_prev * m_opticalDistCoef[2]))); - - // Compute new estimate of r - r = rp + (drOverR * r_prev); - iteration++; - } - while (fabs(r * (1 - drOverR) - rp) > desiredPrecision); - - focalX = focalX / (1.0 - drOverR); - focalY = focalY / (1.0 - drOverR); - } - } -} - - //*************************************************************************** // UsgsAstroLsSensorModel::losToEcf //*************************************************************************** @@ -1850,7 +1778,7 @@ void UsgsAstroLsSensorModel::losToEcf( double& bodyLookZ) const // output line-of-sight z coordinate { //# private_func_description - // Computes image ray (look vector) in ecf coordinate system. + // Computes image ray (look vector) in ecf coordinate system. // Compute adjusted sensor position and velocity double time = getImageTime(csm::ImageCoord(line, sample)); @@ -1866,12 +1794,12 @@ void UsgsAstroLsSensorModel::losToEcf( computeDistortedFocalPlaneCoordinates(fractionalLine, sampleUSGSFull, natFocalPlaneX, natFocalPlaneY); // Remove lens distortion - double focalPlaneX, focalPlaneY; - computeUndistortedFocalPlaneCoordinates(natFocalPlaneX, natFocalPlaneY, focalPlaneX, focalPlaneY); + std::tuple<double, double> undistortedPoint; + undistortedPoint = removeDistortion(natFocalPlaneX, natFocalPlaneY, m_opticalDistCoef); // Define imaging ray (look vector) in camera space double cameraLook[3]; - createCameraLookVector(focalPlaneX, focalPlaneY, adj, cameraLook); + createCameraLookVector(std::get<0>(undistortedPoint), std::get<1>(undistortedPoint), adj, cameraLook); // Apply attitude correction double attCorr[9]; @@ -2459,17 +2387,18 @@ csm::ImageCoord UsgsAstroLsSensorModel::computeViewingPixel( double lookScale = m_focal / adjustedLookZ; double focalX = adjustedLookX * lookScale; double focalY = adjustedLookY * lookScale; + std::tuple<double, double> undistortedPoint; // Invert distortion - reconstructSensorDistortion(focalX, focalY, desiredPrecision); + undistortedPoint = invertDistortion(focalX, focalY, m_opticalDistCoef, desiredPrecision); // Convert to detector line and sample double detectorLine = m_iTransL[0] - + m_iTransL[1] * focalX - + m_iTransL[2] * focalY; + + m_iTransL[1] * std::get<0>(undistortedPoint) + + m_iTransL[2] * std::get<1>(undistortedPoint); double detectorSample = m_iTransS[0] - + m_iTransS[1] * focalX - + m_iTransS[2] * focalY; + + m_iTransS[1] * std::get<0>(undistortedPoint) + + m_iTransS[2] * std::get<1>(undistortedPoint); // Convert to image sample line double line = detectorLine + m_detectorLineOrigin - m_detectorLineOffset diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 881f738..e3567d1 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -4,7 +4,8 @@ cmake_minimum_required(VERSION 3.10) add_executable(runCSMCameraModelTests PluginTests.cpp FrameCameraTests.cpp - LineScanCameraTests.cpp) + LineScanCameraTests.cpp + DistortionTests.cpp) if(WIN32) option(CMAKE_USE_WIN32_THREADS_INIT "using WIN32 threads" ON) option(gtest_disable_pthreads "Disable uses of pthreads in gtest." ON) diff --git a/tests/DistortionTests.cpp b/tests/DistortionTests.cpp new file mode 100644 index 0000000..b21971e --- /dev/null +++ b/tests/DistortionTests.cpp @@ -0,0 +1,154 @@ +#include "Distortion.h" + +#include <gtest/gtest.h> + +#include "Fixtures.h" + +// NOTE: The imagePt format is (Lines,Samples) + +INSTANTIATE_TEST_CASE_P(JacobianTest,ImageCoordParameterizedTest, + ::testing::Values(csm::ImageCoord(2.5,2.5),csm::ImageCoord(7.5,7.5))); + +TEST_P(ImageCoordParameterizedTest, JacobianTest) { + std::vector<double> odtX = {1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0}; + std::vector<double> odtY = {0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0}; + + double Jxx,Jxy,Jyx,Jyy; + + csm::ImageCoord imagePt = GetParam(); + std::vector<std::vector<double>> jacobian; + jacobian = distortionJacobian(imagePt.samp, imagePt.line, odtX, odtY); + + // Jxx * Jyy - Jxy * Jyx + double determinant = fabs(jacobian[0][0] * jacobian[1][1] - jacobian[0][1] * jacobian[1][0]); + EXPECT_GT(determinant,1e-3); +} + +TEST(Transverse, Jacobian1) { + csm::ImageCoord imagePt(7.5, 7.5); + + std::vector<double> odtX = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0}; + std::vector<double> odtY = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,1.0}; + + std::vector<std::vector<double>> jacobian; + jacobian = distortionJacobian(imagePt.samp, imagePt.line, odtX, odtY); + + EXPECT_NEAR(jacobian[0][0],56.25,1e-8 ); + EXPECT_NEAR(jacobian[0][1],112.5,1e-8); + EXPECT_NEAR(jacobian[1][0],56.25,1e-8); + EXPECT_NEAR(jacobian[1][1],281.25,1e-8); +} + +TEST(Transverse, distortMe_AlternatingOnes) { + csm::ImageCoord imagePt(7.5, 7.5); + + std::vector<double> odtX = {1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0}; + std::vector<double> odtY = {0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0}; + + double dx,dy; + std::tuple<double, double> distortedPoint; + distortedPoint = distortionFunction(imagePt.samp, imagePt.line, odtX, odtY); + + EXPECT_NEAR(std::get<0>(distortedPoint),908.5,1e-8 ); + EXPECT_NEAR(std::get<1>(distortedPoint),963.75,1e-8); +} + +TEST(Transverse, distortMe_AllCoefficientsOne) { + csm::ImageCoord imagePt(7.5, 7.5); + + std::vector<double> odtX = {1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0}; + std::vector<double> odtY = {1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0}; + + double dx,dy; + std::tuple<double, double> distortedPoint; + distortedPoint = distortionFunction(imagePt.samp, imagePt.line, odtX, odtY); + + EXPECT_NEAR(std::get<0>(distortedPoint),1872.25,1e-8 ); + EXPECT_NEAR(std::get<1>(distortedPoint),1872.25,1e-8); +} + +TEST(transverse, removeDistortion1) { + csm::ImageCoord imagePt(7.5, 7.5); + + std::vector<double> odtX = {0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; + std::vector<double> odtY = {0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; + + std::tuple<double, double> undistortedPoint; + undistortedPoint = removeDistortion(imagePt.samp, imagePt.line, odtX, odtY); + + EXPECT_NEAR(imagePt.samp,7.5,1e-8 ); + EXPECT_NEAR(imagePt.line,7.5,1e-8); + EXPECT_NEAR(imagePt.line,std::get<0>(undistortedPoint),1e-8); + EXPECT_NEAR(imagePt.samp,std::get<1>(undistortedPoint),1e-8); +} + +TEST(transverse, removeDistortion_AllCoefficientsOne) { + csm::ImageCoord imagePt(1872.25, 1872.25); + + std::vector<double> odtX = {1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0}; + std::vector<double> odtY = {1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0}; + + std::tuple<double, double> undistortedPoint; + undistortedPoint = removeDistortion(imagePt.samp, imagePt.line, odtX, odtY); + + // The Jacobian is singular, so the setFocalPlane should break out of it's iteration and + // returns the same distorted coordinates that were passed in. + EXPECT_NEAR(std::get<0>(undistortedPoint),imagePt.samp,1e-8 ); + EXPECT_NEAR(std::get<1>(undistortedPoint),imagePt.line,1e-8); +} + +TEST(transverse, removeDistortion_AlternatingOnes) { + csm::ImageCoord imagePt(963.75, 908.5); + + std::vector<double> odtX = {1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0}; + std::vector<double> odtY = {0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0}; + + std::tuple<double, double> undistortedPoint; + undistortedPoint = removeDistortion(imagePt.samp, imagePt.line, odtX, odtY); + + EXPECT_NEAR(std::get<0>(undistortedPoint),7.5,1e-8 ); + EXPECT_NEAR(std::get<1>(undistortedPoint),7.5,1e-8); +} + +TEST(Radial, testRemoveDistortion) { + csm::ImageCoord imagePt(0.0, 4.0); + + double dx, dy; + double coeffs[3] = {0, 0, 0}; + std::tuple<double, double> undistortedPoint; + + undistortedPoint = removeDistortion(imagePt.samp, imagePt.line, coeffs); + + EXPECT_NEAR(std::get<0>(undistortedPoint),4,1e-8); + EXPECT_NEAR(std::get<1>(undistortedPoint),0,1e-8); +} + +// If coeffs are 0 then this will have the same result as removeDistortion +// with 0 distortion coefficients +TEST(Radial, testInverseDistortion){ + csm::ImageCoord imagePt(0.0, 4.0); + + double dx, dy; + double desiredPrecision = 0.01; + double coeffs[3] = {0, 0, 0}; + std::tuple<double, double> undistortedPoint; + + undistortedPoint = invertDistortion(imagePt.samp, imagePt.line, coeffs, desiredPrecision); + + EXPECT_NEAR(std::get<0>(undistortedPoint),4,1e-8); + EXPECT_NEAR(std::get<1>(undistortedPoint),0,1e-8); +} + +TEST(Radial, testInverseOnesCoeffs){ + csm::ImageCoord imagePt(0.0, 4.0); + + double dx, dy; + double desiredPrecision = 0.01; + double coeffs[3] = {1, 1, 1}; + std::tuple<double, double> undistortedPoint; + + undistortedPoint = invertDistortion(imagePt.samp, imagePt.line, coeffs, desiredPrecision); + + EXPECT_NEAR(std::get<0>(undistortedPoint),4,1e-8); + EXPECT_NEAR(std::get<1>(undistortedPoint),0,1e-8); +} diff --git a/tests/Fixtures.h b/tests/Fixtures.h index fc61f71..e372b12 100644 --- a/tests/Fixtures.h +++ b/tests/Fixtures.h @@ -80,6 +80,8 @@ class ConstVelLineScanIsdTest : public ::testing::Test { } }; +class ImageCoordParameterizedTest : public ::testing::TestWithParam<csm::ImageCoord> {}; + class FramerParameterizedTest : public ::testing::TestWithParam<csm::ImageCoord> { protected: diff --git a/tests/FrameCameraTests.cpp b/tests/FrameCameraTests.cpp index f3a22bc..d41b461 100644 --- a/tests/FrameCameraTests.cpp +++ b/tests/FrameCameraTests.cpp @@ -7,34 +7,6 @@ #include "Fixtures.h" using json = nlohmann::json; - -INSTANTIATE_TEST_CASE_P(JacobianTest,FramerParameterizedTest, - ::testing::Values(csm::ImageCoord(2.5,2.5),csm::ImageCoord(7.5,7.5))); - -TEST_P(FramerParameterizedTest, JacobianTest) { - - UsgsAstroFrameSensorModel* sensorModel = createModel(isd); - std::string modelState = sensorModel->getModelState(); - auto state = json::parse(modelState); - - state["m_odtX"] = {1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0}; - state["m_odtY"] = {0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0}; - sensorModel->replaceModelState(state.dump()); - - double Jxx,Jxy,Jyx,Jyy; - ASSERT_NE(sensorModel, nullptr); - - csm::ImageCoord imagePt1 = GetParam(); -// cout << "[" << imagePt1.samp << "," << imagePt1.line << "]"<< endl; - sensorModel->distortionJacobian(imagePt1.samp, imagePt1.line, Jxx, Jxy,Jyx,Jyy); - - double determinant = fabs(Jxx*Jyy - Jxy*Jyx); - EXPECT_GT(determinant,1e-3); - - delete sensorModel; - sensorModel=NULL; -} - // NOTE: The imagePt format is (Lines,Samples) TEST_F(FrameSensorModel, State) { @@ -89,7 +61,7 @@ TEST_F(FrameSensorModel, OffBody3) { TEST_F(FrameSensorModel, getReferencePoint) { csm::EcefCoord groundPt = sensorModel->getReferencePoint(); EXPECT_EQ(groundPt.x, 0.0); - EXPECT_EQ(groundPt.y, 0.0); + EXPECT_EQ(groundPt.y, 0.0); EXPECT_EQ(groundPt.z, 0.0); } @@ -105,135 +77,6 @@ TEST_F(FrameSensorModel, getImageIdentifier) { EXPECT_EQ("simpleFramerISD", sensorModel->getImageIdentifier()); } -TEST_F(FrameSensorModel, setFocalPlane1) { - csm::ImageCoord imagePt(7.5, 7.5); - double ux,uy; - - std::string modelState = sensorModel->getModelState(); - auto state = json::parse(modelState); - - state["m_odtX"] = {0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; - state["m_odtY"] = {0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; - sensorModel->replaceModelState(state.dump()); - - ASSERT_NE(sensorModel, nullptr); - sensorModel->setFocalPlane(imagePt.samp, imagePt.line, ux, uy); - EXPECT_NEAR(imagePt.samp,7.5,1e-8 ); - EXPECT_NEAR(imagePt.line,7.5,1e-8); - delete sensorModel; - sensorModel = NULL; -} - - -TEST_F(FrameSensorModel, Jacobian1) { - csm::ImageCoord imagePt(7.5, 7.5); - - std::string modelState = sensorModel->getModelState(); - auto state = json::parse(modelState); - state["m_odtX"] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0}; - state["m_odtY"] = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,1.0}; - sensorModel->replaceModelState(state.dump()); - - double Jxx,Jxy,Jyx,Jyy; - - ASSERT_NE(sensorModel, nullptr); - sensorModel->distortionJacobian(imagePt.samp, imagePt.line, Jxx, Jxy,Jyx,Jyy); - - EXPECT_NEAR(Jxx,56.25,1e-8 ); - EXPECT_NEAR(Jxy,112.5,1e-8); - EXPECT_NEAR(Jyx,56.25,1e-8); - EXPECT_NEAR(Jyy,281.25,1e-8); - - delete sensorModel; - sensorModel = NULL; -} - - -TEST_F(FrameSensorModel, distortMe_AllCoefficientsOne) { - csm::ImageCoord imagePt(7.5, 7.5); - - std::string modelState = sensorModel->getModelState(); - auto state = json::parse(modelState); - state["m_odtX"] = {1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0}; - state["m_odtY"] = {1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0}; - sensorModel->replaceModelState(state.dump()); - - double dx,dy; - ASSERT_NE(sensorModel, nullptr); - sensorModel->distortionFunction(imagePt.samp, imagePt.line,dx,dy ); - - EXPECT_NEAR(dx,1872.25,1e-8 ); - EXPECT_NEAR(dy,1872.25,1e-8); - - delete sensorModel; - sensorModel = NULL; -} - -TEST_F(FrameSensorModel, setFocalPlane_AllCoefficientsOne) { - csm::ImageCoord imagePt(1872.25, 1872.25); - - std::string modelState = sensorModel->getModelState(); - auto state = json::parse(modelState); - state["m_odtX"] = {1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0}; - state["m_odtY"] = {1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0}; - sensorModel->replaceModelState(state.dump()); - - double ux,uy; - ASSERT_NE(sensorModel, nullptr); - sensorModel->setFocalPlane(imagePt.samp, imagePt.line,ux,uy ); - - // The Jacobian is singular, so the setFocalPlane should break out of it's iteration and - // returns the same distorted coordinates that were passed in. - EXPECT_NEAR(ux,imagePt.samp,1e-8 ); - EXPECT_NEAR(uy,imagePt.line,1e-8); - - delete sensorModel; - sensorModel = NULL; -} - - -TEST_F(FrameSensorModel, distortMe_AlternatingOnes) { - csm::ImageCoord imagePt(7.5, 7.5); - - std::string modelState = sensorModel->getModelState(); - auto state = json::parse(modelState); - state["m_odtX"] = {1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0}; - state["m_odtY"] = {0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0}; - sensorModel->replaceModelState(state.dump()); - - double dx,dy; - ASSERT_NE(sensorModel, nullptr); - sensorModel->distortionFunction(imagePt.samp, imagePt.line,dx,dy ); - - EXPECT_NEAR(dx,908.5,1e-8 ); - EXPECT_NEAR(dy,963.75,1e-8); - - delete sensorModel; - sensorModel = NULL; -} - - -TEST_F(FrameSensorModel, setFocalPlane_AlternatingOnes) { - csm::ImageCoord imagePt(963.75, 908.5); - - std::string modelState = sensorModel->getModelState(); - auto state = json::parse(modelState); - state["m_odtX"] = {1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0}; - state["m_odtY"] = {0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0}; - sensorModel->replaceModelState(state.dump()); - - double ux,uy; - ASSERT_NE(sensorModel, nullptr); - - sensorModel->setFocalPlane(imagePt.samp, imagePt.line,ux,uy ); - - EXPECT_NEAR(ux,7.5,1e-8 ); - EXPECT_NEAR(uy,7.5,1e-8); - - delete sensorModel; - sensorModel = NULL; -} - // Focal Length Tests: TEST_F(FrameStateTest, FL500_OffBody4) { -- GitLab