diff --git a/include/usgscsm/Distortion.h b/include/usgscsm/Distortion.h
index b70859b9d24d7278bc88e75674b7dc7415d9a499..990f513617f0beda9ebd431719e9e02ce164477d 100644
--- a/include/usgscsm/Distortion.h
+++ b/include/usgscsm/Distortion.h
@@ -6,7 +6,7 @@
#include <tuple>
#include <vector>
-enum DistortionType { RADIAL, TRANSVERSE, KAGUYALISM, DAWNFC, LROLROCNAC };
+enum DistortionType { RADIAL, TRANSVERSE, KAGUYALISM, DAWNFC, LROLROCNAC, CAHVOR };
// Transverse Distortion
void distortionJacobian(double x, double y, double *jacobian,
diff --git a/src/Distortion.cpp b/src/Distortion.cpp
index 15803b672ea423267508625d4e71a42eea531671..5c08ed6b6ee1da0b264d689d91b5087b7591967f 100644
--- a/src/Distortion.cpp
+++ b/src/Distortion.cpp
@@ -297,6 +297,28 @@ void removeDistortion(double dx, double dy, double &ux, double &uy,
return;
} break;
+
+ // Compute undistorted focal plane coordinate given a distorted
+ // coordinate set and the distortion coefficients along with an
+ // x, and y offset as the fourth and fifth element
+ case CAHVOR:
+ {
+ double shiftedDx = dx - opticalDistCoeffs[3];
+ double shiftedDy = dy - opticalDistCoeffs[4];
+ double rr = shiftedDx * shiftedDx + shiftedDy * shiftedDy;
+
+ if (rr > tolerance)
+ {
+ double dr = opticalDistCoeffs[0] +
+ (rr * (opticalDistCoeffs[1] + rr * opticalDistCoeffs[2]));
+
+ ux = shiftedDx * (1.0 - dr);
+ uy = shiftedDy * (1.0 - dr);
+ ux += opticalDistCoeffs[3];
+ uy += opticalDistCoeffs[4];
+ }
+ }
+ break;
}
}
@@ -519,5 +541,64 @@ void applyDistortion(double ux, double uy, double &dx, double &dy,
return;
} break;
+
+ // Compute undistorted focal plane coordinate given a distorted
+ // focal plane coordinate. This case works by iteratively adding distortion
+ // until the new distorted point, r, undistorts to within a tolerance of the
+ // original point, rp. Also applies an initial offset with an
+ // x, and y offset as the fourth and fifth element
+ // This is untested manually
+ case CAHVOR:
+ {
+ double shiftedUx = ux - opticalDistCoeffs[3];
+ double shiftedUy = uy - opticalDistCoeffs[4];
+ double rp2 = (ux * ux) + (uy * uy);
+
+ if (rp2 > tolerance)
+ {
+ double rp = sqrt(rp2);
+ // Compute first fractional distortion using rp
+ double drOverR =
+ opticalDistCoeffs[0] +
+ (rp2 * (opticalDistCoeffs[1] + (rp2 * opticalDistCoeffs[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 >= 20 || r > 1E9)
+ {
+ drOverR = 0.0;
+ break;
+ }
+
+ r_prev = r;
+ r2_prev = r * r;
+
+ // Compute new fractional distortion:
+ drOverR = opticalDistCoeffs[0] +
+ (r2_prev *
+ (opticalDistCoeffs[1] + (r2_prev * opticalDistCoeffs[2])));
+
+ // Compute new estimate of r
+ r = rp + (drOverR * r_prev);
+ iteration++;
+ } while (fabs(r - r_prev) > desiredPrecision);
+
+ dx = ux / (1.0 - drOverR);
+ dy = uy / (1.0 - drOverR);
+ dx += opticalDistCoeffs[3];
+ dy += opticalDistCoeffs[4];
+ }
+ }
+ break;
}
}
diff --git a/src/UsgsAstroFrameSensorModel.cpp b/src/UsgsAstroFrameSensorModel.cpp
index f2548ae8430cf5841ee919b1345934c87823e8a4..03bb386826bc2043fd99126c29788b5994f77082 100644
--- a/src/UsgsAstroFrameSensorModel.cpp
+++ b/src/UsgsAstroFrameSensorModel.cpp
@@ -321,7 +321,7 @@ csm::EcefLocus UsgsAstroFrameSensorModel::imageToProximateImagingLocus(
csm::WarningList *warnings) const {
MESSAGE_LOG(
spdlog::level::info,
- "Computing imageToProximateImagingLocus(No ground) for point {}, {}, {}, "
+ "Computing imageToProximateImagingLocus(No ground) for point {}, {}, "
"with desired precision {}",
imagePt.line, imagePt.samp, desiredPrecision);
@@ -362,7 +362,7 @@ csm::EcefLocus UsgsAstroFrameSensorModel::imageToRemoteImagingLocus(
double *achievedPrecision, csm::WarningList *warnings) const {
MESSAGE_LOG(
spdlog::level::info,
- "Computing imageToProximateImagingLocus for {}, {}, {}, with desired "
+ "Computing imageToRemoteImagingLocus for {}, {} with desired "
"precision {}",
imagePt.line, imagePt.samp, desiredPrecision);
// Find the line,sample on the focal plane (mm)
@@ -373,15 +373,30 @@ csm::EcefLocus UsgsAstroFrameSensorModel::imageToRemoteImagingLocus(
m_startingDetectorLine, &m_iTransS[0], &m_iTransL[0], focalPlaneX,
focalPlaneY);
+ MESSAGE_LOG(
+ spdlog::level::trace,
+ "Found focalPlaneX: {}, and focalPlaneY: {}",
+ focalPlaneX, focalPlaneY);
+
// Distort
double undistortedFocalPlaneX, undistortedFocalPlaneY;
removeDistortion(focalPlaneX, focalPlaneY, undistortedFocalPlaneX,
undistortedFocalPlaneY, m_opticalDistCoeffs,
m_distortionType);
+ MESSAGE_LOG(
+ spdlog::level::trace,
+ "Found undistortedFocalPlaneX: {}, and undistortedFocalPlaneY: {}",
+ undistortedFocalPlaneX, undistortedFocalPlaneY);
+
// Get rotation matrix and transform to a body-fixed frame
double m[3][3];
calcRotationMatrix(m);
+ MESSAGE_LOG(
+ spdlog::level::trace,
+ "Calculated rotation matrix [{}, {}, {}], [{}, {}, {}], [{}, {}, {}]",
+ m[0][0], m[0][1], m[0][2], m[1][0], m[1][1], m[1][2], m[2][0], m[2][1],
+ m[2][2]);
std::vector<double> lookC{undistortedFocalPlaneX, undistortedFocalPlaneY,
m_focalLength};
std::vector<double> lookB{
@@ -1250,6 +1265,7 @@ std::string UsgsAstroFrameSensorModel::constructStateFromIsd(
// We don't pass the pixel to focal plane transformation so invert the
// focal plane to pixel transformation
try {
+ // "focal2pixel_lines":[0,136.4988453771169,0],"focal2pixel_samples":[136.4988453771169,0,0]
double determinant = state["m_iTransL"][1].get<double>() *
state["m_iTransS"][2].get<double>() -
state["m_iTransL"][2].get<double>() *
diff --git a/src/Utilities.cpp b/src/Utilities.cpp
index 5391b2d340946bb49b12135e2f3864f26a46b0a2..14ed72ee316e19b8583db27d2de3826557616ce2 100644
--- a/src/Utilities.cpp
+++ b/src/Utilities.cpp
@@ -1053,6 +1053,8 @@ DistortionType getDistortionModel(int aleDistortionModel,
return DistortionType::DAWNFC;
} else if (aleDistortionType == ale::DistortionType::LROLROCNAC) {
return DistortionType::LROLROCNAC;
+ }else if (aleDistortionType == ale::DistortionType::CAHVOR) {
+ return DistortionType::CAHVOR;
}
} catch (...) {
if (list) {
@@ -1193,6 +1195,24 @@ std::vector<double> getDistortionCoeffs(json isd, csm::WarningList *list) {
coefficients = std::vector<double>(1, 0.0);
}
} break;
+ case DistortionType::CAHVOR: {
+ try {
+ coefficients = isd.at("optical_distortion")
+ .at("cahvor")
+ .at("coefficients")
+ .get<std::vector<double>>();
+
+ return coefficients;
+ } catch (...) {
+ if (list) {
+ list->push_back(csm::Warning(
+ csm::Warning::DATA_NOT_AVAILABLE,
+ "Could not parse the radial distortion model coefficients.",
+ "Utilities::getDistortion()"));
+ }
+ coefficients = std::vector<double>(6, 0.0);
+ }
+ } break;
}
if (list) {
list->push_back(