Merge pull request #436 from acpaquette/kaguya_fix (2331aaa4) · Commits · aflab / astrogeology / Usgscsm

src/UsgsAstroFrameSensorModel.cpp

+14 −0

Original line number	Diff line number	Diff line
		@@ -153,6 +153,11 @@ csm::ImageCoord UsgsAstroFrameSensorModel::groundToImage(
		// Camera rotation matrix
		double m[3][3];
		calcRotationMatrix(m, adjustments);
		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]);

		// Sensor position
		double undistortedx, undistortedy, denom;
		@@ -160,12 +165,21 @@ csm::ImageCoord UsgsAstroFrameSensorModel::groundToImage(
		undistortedx = (f * (m[0][0] * xo + m[1][0] * yo + m[2][0] * zo) / denom);
		undistortedy = (f * (m[0][1] * xo + m[1][1] * yo + m[2][1] * zo) / denom);

		MESSAGE_LOG(
		spdlog::level::trace,
		"Found undistortedX: {}, and undistortedY: {}",
		undistortedx, undistortedy);
		// Apply the distortion to the line/sample location and then convert back to
		// line/sample
		double distortedX, distortedY;
		applyDistortion(undistortedx, undistortedy, distortedX, distortedY,
		m_opticalDistCoeffs, m_distortionType);

		MESSAGE_LOG(
		spdlog::level::trace,
		"Found distortedX: {}, and distortedY: {}",
		distortedX, distortedY);

		// Convert distorted mm into line/sample
		double sample, line;
		computePixel(distortedX, distortedY, m_ccdCenter[1], m_ccdCenter[0],

src/UsgsAstroLsSensorModel.cpp

+49 −11

Original line number	Diff line number	Diff line
		@@ -721,6 +721,10 @@ csm::ImageCoord UsgsAstroLsSensorModel::groundToImage(

		csm::ImageCoord approxPt;
		computeProjectiveApproximation(groundPt, approxPt);
		MESSAGE_LOG(
		spdlog::level::trace,
		"Computed Proj Approximation: {}, {}",
		approxPt.line, approxPt.samp);

		// Search for the (line, sample) coordinate that views a given
		// ground point. Set this up as a root-finding problem and use the
		@@ -730,6 +734,10 @@ csm::ImageCoord UsgsAstroLsSensorModel::groundToImage(
		double lineErr0 = calcDetectorLineErr(t0, approxPt, groundPt, adj);
		double t1 = 0.1;
		double lineErr1 = calcDetectorLineErr(t1, approxPt, groundPt, adj);
		MESSAGE_LOG(
		spdlog::level::trace,
		"Initial Line Error: {}, {}",
		lineErr0, lineErr1);
		while (std::abs(lineErr1) > desiredPrecision && ++count < 15) {

		if (lineErr1 == lineErr0)
		@@ -743,25 +751,45 @@ csm::ImageCoord UsgsAstroLsSensorModel::groundToImage(
		// Update for the next step
		t0 = t1; lineErr0 = lineErr1;
		t1 = t2; lineErr1 = lineErr2;
		MESSAGE_LOG(
		spdlog::level::trace,
		"{} Line Error and (t0, t1): {}, {}, {}, {}",
		count, lineErr0, lineErr1, t0, t1);
		}

		// Update the line with the found value
		approxPt.line += t1;
		MESSAGE_LOG(
		spdlog::level::trace,
		"After line Approximation: {}, {}",
		approxPt.line, approxPt.samp);

		double timei = getImageTime(approxPt);
		std::vector<double> detectorView = computeDetectorView(timei, groundPt, adj);
		MESSAGE_LOG(
		spdlog::level::trace,
		"Detector View: {}, and undistortedY: {}",
		detectorView[0], detectorView[1]);

		// Invert distortion
		double distortedFocalX, distortedFocalY;
		applyDistortion(detectorView[0], detectorView[1], distortedFocalX,
		distortedFocalY, m_opticalDistCoeffs, m_distortionType,
		desiredPrecision);
		MESSAGE_LOG(
		spdlog::level::trace,
		"Distorted X, Y: {}, and undistortedY: {}",
		distortedFocalX, distortedFocalY);

		// Convert to detector line and sample
		double detectorLine = m_iTransL[0] + m_iTransL[1] * distortedFocalX +
		m_iTransL[2] * distortedFocalY;
		double detectorSample = m_iTransS[0] + m_iTransS[1] * distortedFocalX +
		m_iTransS[2] * distortedFocalY;
		MESSAGE_LOG(
		spdlog::level::trace,
		"Detector Line, Sample: {}, and undistortedY: {}",
		detectorLine, detectorSample);
		// Convert to image sample line
		double finalUpdate =
		(detectorLine + m_detectorLineOrigin - m_startingDetectorLine) /
		@@ -2263,6 +2291,11 @@ void UsgsAstroLsSensorModel::computeProjectiveApproximation(const csm::EcefCoord
		ip.line = (u[0] + u[1] * x + u[2] * y + u[3] * z) / line_den;
		ip.samp = (u[7] + u[8] * x + u[9] * y + u[10] * z) / samp_den;

		MESSAGE_LOG(
		spdlog::level::debug,
		"Projective approximation before bounding ({}, {})",
		ip.line, ip.samp);

		// Since this is valid only over the image,
		// don't let the result go beyond the image border.
		double numRows = m_nLines;
		@@ -2305,8 +2338,8 @@ void UsgsAstroLsSensorModel::createProjectiveApproximation() {
		spdlog::level::warn,
		"createProjectiveApproximation: computeElevation of "
		"reference point {} {} {} with desired precision "
		"{} returned nan height; nonprojective",
		refPt.x, refPt.y, refPt.z, desired_precision);
		"{} and major/minor radii {}, {} returned nan height; nonprojective",
		refPt.x, refPt.y, refPt.z, desired_precision, m_majorAxis, m_minorAxis);
		m_useApproxInitTrans = false;
		return;
		}
		@@ -2314,8 +2347,8 @@ void UsgsAstroLsSensorModel::createProjectiveApproximation() {
		spdlog::level::trace,
		"createProjectiveApproximation: computeElevation of "
		"reference point {} {} {} with desired precision "
		"{} returned {} height",
		refPt.x, refPt.y, refPt.z, desired_precision, height);
		"{} and major/minor radii {}, {} returned {} height",
		refPt.x, refPt.y, refPt.z, desired_precision, m_majorAxis, m_minorAxis, height);

		double numImageRows = m_nLines;
		double numImageCols = m_nSamples;
		@@ -2764,9 +2797,14 @@ double UsgsAstroLsSensorModel::calcDetectorLineErr(double t, csm::ImageCoord con
		double timei = getImageTime(currPt);
		std::vector<double> detectorView = computeDetectorView(timei, groundPt, adj);

		// Invert distortion
		double distortedFocalX, distortedFocalY;
		applyDistortion(detectorView[0], detectorView[1], distortedFocalX,
		distortedFocalY, m_opticalDistCoeffs, m_distortionType);

		// Convert to detector line
		double detectorLine = m_iTransL[0] + m_iTransL[1] * detectorView[0] +
		m_iTransL[2] * detectorView[1] + m_detectorLineOrigin -
		double detectorLine = m_iTransL[0] + m_iTransL[1] * distortedFocalX +
		m_iTransL[2] * distortedFocalY + m_detectorLineOrigin -
		m_startingDetectorLine;
		detectorLine /= m_detectorLineSumming;