Skip to content
Snippets Groups Projects
Commit df461d79 authored by acpaquette's avatar acpaquette Committed by Jesse Mapel
Browse files

New cahvor distortion model

parent 7728eebb
Branches
Tags
No related merge requests found
......@@ -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,
......
......@@ -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;
}
}
......@@ -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>() *
......
......@@ -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(
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment