Hide normalizing of distorted coordiantes in the lens distortion model (3a1bb8aa) · Commits · aflab / astrogeology / Usgscsm

include/usgscsm/Distortion.h

+2 −0

Original line number	Diff line number	Diff line
		@@ -19,11 +19,13 @@ void computeTransverseDistortion(double ux, double uy, double &dx, double &dy,

		void removeDistortion(double dx, double dy, double &ux, double &uy,
		std::vector<double> const& opticalDistCoeffs,
		double focalLength,
		DistortionType distortionType,
		const double tolerance = 1.0E-6);

		void applyDistortion(double ux, double uy, double &dx, double &dy,
		std::vector<double> const& opticalDistCoeffs,
		double focalLength,
		DistortionType distortionType,
		const double desiredPrecision = 1.0E-6,
		const double tolerance = 1.0E-6);

src/Distortion.cpp

+15 −8

Original line number	Diff line number	Diff line
		@@ -84,13 +84,14 @@ void computeTransverseDistortion(double ux, double uy, double &dx, double &dy,
		}
		}

		// Compute distorted focal plane coordinates given undistorted coordinates. Use
		// the radial-tangential distortion model with 5 coefficients (k1, k2, k3 for
		// radial distortion, and p1, p2 for tangential distortion). This was tested to
		// give the same results as the OpenCV distortion model, by invoking the
		// function cv::projectPoints() (with zero rotation, zero translation, and
		// identity camera matrix). The parameters are stored in opticalDistCoeffs
		// in the order: [k1, k2, p1, p2, k3].
		// Compute distorted normalized focal plane coordinates given undistorted
		// normalized coordinates. Use the radial-tangential distortion model with 5
		// coefficients (k1, k2, k3 for radial distortion, and p1, p2 for tangential
		// distortion). This was tested to give the same results as the OpenCV
		// distortion model, by invoking the function cv::projectPoints() (with zero
		// rotation, zero translation, and identity camera matrix). The parameters are
		// stored in opticalDistCoeffs in the order: [k1, k2, p1, p2, k3].
		// Reference: https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html
		void computeRadTanDistortion(double ux, double uy, double &dx, double &dy,
		std::vector<double> const& opticalDistCoeffs) {

		@@ -120,7 +121,7 @@ void computeRadTanDistortion(double ux, double uy, double &dx, double &dy,
		+ (p1 * (r2 + 2.0 * y * y) + 2.0 * p2 * x * y);
		}

		// Compute the jacobian for radtan distortion
		// Compute the jacobian for radtan distortion at given normalized coordinates
		void radTanDistortionJacobian(double x, double y, double *jacobian,
		std::vector<double> const& opticalDistCoeffs) {

		@@ -155,6 +156,7 @@ void radTanDistortionJacobian(double x, double y, double *jacobian,

		void removeDistortion(double dx, double dy, double &ux, double &uy,
		std::vector<double> const& opticalDistCoeffs,
		double focalLength,
		DistortionType distortionType, const double tolerance) {
		ux = dx;
		uy = dy;
		@@ -343,8 +345,10 @@ void removeDistortion(double dx, double dy, double &ux, double &uy,
		// with the radtan model. See computeRadTanDistortion() for more details.
		case RADTAN:
		{
		dx /= focalLength; dy /= focalLength; // Find normalized coordinates
		newtonRaphson(dx, dy, ux, uy, opticalDistCoeffs, distortionType, tolerance,
		computeRadTanDistortion, radTanDistortionJacobian);
		ux = focalLength; uy = focalLength; // Convert back to pixel coordinates

		}
		break;
		@@ -354,6 +358,7 @@ void removeDistortion(double dx, double dy, double &ux, double &uy,

		void applyDistortion(double ux, double uy, double &dx, double &dy,
		std::vector<double> const& opticalDistCoeffs,
		double focalLength,
		DistortionType distortionType,
		const double desiredPrecision, const double tolerance) {
		dx = ux;
		@@ -634,7 +639,9 @@ void applyDistortion(double ux, double uy, double &dx, double &dy,
		// with the RADTAN model. See computeRadTanDistortion() for more details.
		case RADTAN:
		{
		ux /= focalLength; uy /= focalLength; // Find normalized coordinates
		computeRadTanDistortion(ux, uy, dx, dy, opticalDistCoeffs);
		dx = focalLength; dy = focalLength; // Convert back to pixel coordinates
		}
		break;

src/UsgsAstroFrameSensorModel.cpp

+3 −17

Original line number	Diff line number	Diff line
		@@ -172,12 +172,8 @@ csm::ImageCoord UsgsAstroFrameSensorModel::groundToImage(
		// Apply the distortion to the line/sample location and then convert back to
		// line/sample
		double distortedX, distortedY;
		// Divide by focal length
		undistortedx /= f; undistortedy /= f;
		applyDistortion(undistortedx, undistortedy, distortedX, distortedY,
		m_opticalDistCoeffs, m_distortionType);
		// Apply multiplication by focal length
		distortedX = f; distortedY = f;
		m_opticalDistCoeffs, m_focalLength, m_distortionType);

		MESSAGE_LOG(
		spdlog::level::trace,
		@@ -272,16 +268,10 @@ csm::EcefCoord UsgsAstroFrameSensorModel::imageToGround(
		m_detectorLineSumming, m_startingDetectorSample, m_startingDetectorLine,
		&m_iTransS[0], &m_iTransL[0], x_camera, y_camera);

		// divide x_camera and y_camera by focal length
		x_camera /= m_focalLength; y_camera /= m_focalLength;

		// Apply the distortion model (remove distortion)
		double undistortedX, undistortedY;
		removeDistortion(x_camera, y_camera, undistortedX, undistortedY,
		m_opticalDistCoeffs, m_distortionType);

		// Multiply by focal length
		undistortedX = m_focalLength; undistortedY = m_focalLength;
		m_opticalDistCoeffs, m_focalLength, m_distortionType);

		MESSAGE_LOG(
		spdlog::level::trace,
		@@ -405,13 +395,9 @@ csm::EcefLocus UsgsAstroFrameSensorModel::imageToRemoteImagingLocus(

		// Distort
		double undistortedFocalPlaneX, undistortedFocalPlaneY;
		// divide by focal length
		focalPlaneX /= m_focalLength; focalPlaneY /= m_focalLength;
		removeDistortion(focalPlaneX, focalPlaneY, undistortedFocalPlaneX,
		undistortedFocalPlaneY, m_opticalDistCoeffs,
		m_distortionType);
		// Multiply by focal length
		undistortedFocalPlaneX = m_focalLength; undistortedFocalPlaneY = m_focalLength;
		m_focalLength, m_distortionType);

		MESSAGE_LOG(
		spdlog::level::trace,

src/UsgsAstroLsSensorModel.cpp

+4 −3

Original line number	Diff line number	Diff line
		@@ -774,7 +774,8 @@ csm::ImageCoord UsgsAstroLsSensorModel::groundToImage(
		// Invert distortion
		double distortedFocalX, distortedFocalY;
		applyDistortion(detectorView[0], detectorView[1], distortedFocalX,
		distortedFocalY, m_opticalDistCoeffs, m_distortionType,
		distortedFocalY, m_opticalDistCoeffs, m_focalLength,
		m_distortionType,
		desiredPrecision);
		MESSAGE_LOG(
		spdlog::level::trace,
		@@ -1876,7 +1877,7 @@ void UsgsAstroLsSensorModel::losToEcf(
		double undistortedFocalPlaneX, undistortedFocalPlaneY;
		removeDistortion(distortedFocalPlaneX, distortedFocalPlaneY,
		undistortedFocalPlaneX, undistortedFocalPlaneY,
		m_opticalDistCoeffs, m_distortionType);
		m_opticalDistCoeffs, m_focalLength, m_distortionType);
		MESSAGE_LOG(
		spdlog::level::trace,
		"losToEcf: undistorted focal plane coordinate {} {}",
		@@ -2800,7 +2801,7 @@ double UsgsAstroLsSensorModel::calcDetectorLineErr(double t, csm::ImageCoord con
		// Invert distortion
		double distortedFocalX, distortedFocalY;
		applyDistortion(detectorView[0], detectorView[1], distortedFocalX,
		distortedFocalY, m_opticalDistCoeffs, m_distortionType);
		distortedFocalY, m_opticalDistCoeffs, m_focalLength, m_distortionType);

		// Convert to detector line
		double detectorLine = m_iTransL[0] + m_iTransL[1] * distortedFocalX +

src/UsgsAstroPushFrameSensorModel.cpp

+3 −3

Original line number	Diff line number	Diff line
		@@ -762,8 +762,8 @@ csm::ImageCoord UsgsAstroPushFrameSensorModel::groundToImage(
		// Invert distortion
		double distortedFocalX, distortedFocalY;
		applyDistortion(focalCoord[0], focalCoord[1], distortedFocalX,
		distortedFocalY, m_opticalDistCoeffs, m_distortionType,
		desiredPrecision);
		distortedFocalY, m_opticalDistCoeffs, m_focalLength,
		m_distortionType, desiredPrecision);

		// Convert to detector line and sample
		double detectorLine = m_iTransL[0] + m_iTransL[1] * distortedFocalX +
		@@ -1793,7 +1793,7 @@ void UsgsAstroPushFrameSensorModel::losToEcf(
		double undistortedFocalPlaneX, undistortedFocalPlaneY;
		removeDistortion(distortedFocalPlaneX, distortedFocalPlaneY,
		undistortedFocalPlaneX, undistortedFocalPlaneY,
		m_opticalDistCoeffs, m_distortionType);
		m_opticalDistCoeffs, m_focalLength, m_distortionType);
		MESSAGE_LOG("losToEcf: undistorted focal plane coordinate {} {}",
		undistortedFocalPlaneX, undistortedFocalPlaneY)