Projected Line Scanner photogrametry updates (67e0d966) · Commits · aflab / astrogeology / Usgscsm

src/UsgsAstroProjectedLsSensorModel.cpp

+11 −339

Original line number	Diff line number	Diff line
		@@ -289,58 +289,12 @@ csm::ImageCoord UsgsAstroProjectedLsSensorModel::groundToImage(
		csm::ImageCoordCovar UsgsAstroProjectedLsSensorModel::groundToImage(
		const csm::EcefCoordCovar &groundPt, double desired_precision,
		double achieved_precision, csm::WarningList warnings) const {
		MESSAGE_LOG(
		spdlog::level::debug,
		"Computing groundToImage(Covar) for {}, {}, {}, with desired precision "
		"{}",
		groundPt.x, groundPt.y, groundPt.z, desired_precision);
		// Ground to image with error propagation
		// Compute corresponding image point
		csm::EcefCoord gp;
		gp.x = groundPt.x;
		gp.y = groundPt.y;
		gp.z = groundPt.z;

		csm::ImageCoord ip =
		groundToImage(gp, desired_precision, achieved_precision, warnings);
		csm::ImageCoordCovar result(ip.line, ip.samp);

		// // Compute partials ls wrt XYZ
		// std::vector<double> prt(6, 0.0);
		// prt = UsgsAstroLsSensorModel::computeGroundPartials(groundPt);

		// // Error propagation
		// double ltx, lty, ltz;
		// double stx, sty, stz;
		// ltx = prt[0] * groundPt.covariance[0] + prt[1] * groundPt.covariance[3] +
		// prt[2] * groundPt.covariance[6];
		// lty = prt[0] * groundPt.covariance[1] + prt[1] * groundPt.covariance[4] +
		// prt[2] * groundPt.covariance[7];
		// ltz = prt[0] * groundPt.covariance[2] + prt[1] * groundPt.covariance[5] +
		// prt[2] * groundPt.covariance[8];
		// stx = prt[3] * groundPt.covariance[0] + prt[4] * groundPt.covariance[3] +
		// prt[5] * groundPt.covariance[6];
		// sty = prt[3] * groundPt.covariance[1] + prt[4] * groundPt.covariance[4] +
		// prt[5] * groundPt.covariance[7];
		// stz = prt[3] * groundPt.covariance[2] + prt[4] * groundPt.covariance[5] +
		// prt[5] * groundPt.covariance[8];

		// double gp_cov[4]; // Input gp cov in image space
		// gp_cov[0] = ltx * prt[0] + lty * prt[1] + ltz * prt[2];
		// gp_cov[1] = ltx * prt[3] + lty * prt[4] + ltz * prt[5];
		// gp_cov[2] = stx * prt[0] + sty * prt[1] + stz * prt[2];
		// gp_cov[3] = stx * prt[3] + sty * prt[4] + stz * prt[5];

		// std::vector<double> unmodeled_cov = getUnmodeledError(ip);
		// double sensor_cov[4]; // sensor cov in image space
		// determineSensorCovarianceInImageSpace(gp, sensor_cov);

		// result.covariance[0] = gp_cov[0] + unmodeled_cov[0] + sensor_cov[0];
		// result.covariance[1] = gp_cov[1] + unmodeled_cov[1] + sensor_cov[1];
		// result.covariance[2] = gp_cov[2] + unmodeled_cov[2] + sensor_cov[2];
		// result.covariance[3] = gp_cov[3] + unmodeled_cov[3] + sensor_cov[3];

		return result;
		csm::ImageCoordCovar imageCoordCovar = UsgsAstroLsSensorModel::groundToImage(groundPt, desired_precision, achieved_precision, warnings);
		csm::ImageCoord projImagePt = groundToImage(groundPt);

		imageCoordCovar.line = projImagePt.line;
		imageCoordCovar.samp = projImagePt.samp;
		return imageCoordCovar;
		}

		//***************************************************************************
		@@ -417,7 +371,10 @@ csm::EcefLocus UsgsAstroProjectedLsSensorModel::imageToProximateImagingLocus(
		const csm::ImageCoord& image_pt, const csm::EcefCoord& ground_pt,
		double desired_precision, double* achieved_precision,
		csm::WarningList* warnings) const {
		csm::ImageCoord cameraImagePt = UsgsAstroLsSensorModel::groundToImage(ground_pt);
		csm::EcefCoord projGround = imageToGround(image_pt, 0);
		csm::ImageCoord cameraImagePt = UsgsAstroLsSensorModel::groundToImage(projGround);
		// std::cout.precision(17);
		// std::cout << cameraImagePt.line << ", " <<

		return UsgsAstroLsSensorModel::imageToProximateImagingLocus(cameraImagePt, ground_pt, desired_precision, achieved_precision, warnings);
		}
		@@ -496,69 +453,6 @@ std::string UsgsAstroProjectedLsSensorModel::getReferenceDateAndTime() const {
		return ephemTimeToCalendarTime(ephemTime);
		}

		//***************************************************************************
		// UsgsAstroProjectedLsSensorModel::getImageTime
		//***************************************************************************
		double UsgsAstroProjectedLsSensorModel::getImageTime(
		const csm::ImageCoord& image_pt) const {
		// Convert imagept to camera imagept
		// proj -> ecef -> groundToImage
		MESSAGE_LOG(
		spdlog::level::debug,
		"getImageTime for image line {}",
		image_pt.line)
		double lineFull = image_pt.line;

		auto referenceLineIt =
		std::upper_bound(m_intTimeLines.begin(), m_intTimeLines.end(), lineFull);
		if (referenceLineIt != m_intTimeLines.begin()) {
		--referenceLineIt;
		}
		size_t referenceIndex =
		std::distance(m_intTimeLines.begin(), referenceLineIt);

		// Adding 0.5 to the line results in center exposure time for a given line
		double time = m_intTimeStartTimes[referenceIndex] +
		m_intTimes[referenceIndex] *
		(lineFull - m_intTimeLines[referenceIndex] + 0.5);

		MESSAGE_LOG(
		spdlog::level::debug,
		"getImageTime is {}",
		time)

		return time;
		}

		//***************************************************************************
		// UsgsAstroProjectedLsSensorModel::getSensorPosition
		//***************************************************************************
		csm::EcefCoord UsgsAstroProjectedLsSensorModel::getSensorPosition(
		const csm::ImageCoord& imagePt) const {
		// Convert imagept to camera imagept
		// proj -> ecef -> groundToImage
		MESSAGE_LOG(
		spdlog::level::debug,
		"getSensorPosition at image coord ({}, {})",
		imagePt.line, imagePt.samp)

		return UsgsAstroLsSensorModel::getSensorPosition(getImageTime(imagePt));
		}

		//***************************************************************************
		// UsgsAstroProjectedLsSensorModel::getSensorVelocity
		//***************************************************************************
		csm::EcefVector UsgsAstroProjectedLsSensorModel::getSensorVelocity(
		const csm::ImageCoord& imagePt) const {
		// Convert imagept to camera imagept
		// proj -> ecef -> groundToImage
		MESSAGE_LOG(
		spdlog::level::debug,
		"getSensorVelocity at image coord ({}, {})",
		imagePt.line, imagePt.samp);
		return UsgsAstroLsSensorModel::getSensorVelocity(getImageTime(imagePt));
		}

		//---------------------------------------------------------------------------
		// Sensor Model Parameters
		//---------------------------------------------------------------------------
		@@ -712,33 +606,6 @@ UsgsAstroProjectedLsSensorModel::getValidImageRange() const {
		// image in a zero based system.
		}

		//***************************************************************************
		// UsgsAstroProjectedLsSensorModel::getIlluminationDirection
		//***************************************************************************
		csm::EcefVector UsgsAstroProjectedLsSensorModel::getIlluminationDirection(
		const csm::EcefCoord& groundPt) const {
		MESSAGE_LOG(
		spdlog::level::debug,
		"Accessing illumination direction of ground point"
		"{} {} {}.",
		groundPt.x, groundPt.y, groundPt.z);

		csm::EcefVector sunPosition =
		getSunPosition(getImageTime(groundToImage(groundPt)));
		csm::EcefVector illuminationDirection =
		csm::EcefVector(groundPt.x - sunPosition.x, groundPt.y - sunPosition.y,
		groundPt.z - sunPosition.z);

		double scale = sqrt(illuminationDirection.x * illuminationDirection.x +
		illuminationDirection.y * illuminationDirection.y +
		illuminationDirection.z * illuminationDirection.z);

		illuminationDirection.x /= scale;
		illuminationDirection.y /= scale;
		illuminationDirection.z /= scale;
		return illuminationDirection;
		}

		//---------------------------------------------------------------------------
		// Error Correction
		//---------------------------------------------------------------------------
		@@ -899,201 +766,6 @@ csm::Version UsgsAstroProjectedLsSensorModel::getVersion() const {
		return csm::Version(1, 0, 0);
		}

		//***************************************************************************
		// UsgsAstroLineScannerSensorModel::getEllipsoid
		//***************************************************************************
		csm::Ellipsoid UsgsAstroProjectedLsSensorModel::getEllipsoid() const {
		return csm::Ellipsoid(m_majorAxis, m_minorAxis);
		}

		void UsgsAstroProjectedLsSensorModel::setEllipsoid(const csm::Ellipsoid& ellipsoid) {
		m_majorAxis = ellipsoid.getSemiMajorRadius();
		m_minorAxis = ellipsoid.getSemiMinorRadius();
		}

		//***************************************************************************
		// UsgsAstroLineScannerSensorModel::getValue
		//***************************************************************************
		double UsgsAstroProjectedLsSensorModel::getValue(
		int index, const std::vector<double>& adjustments) const {
		return m_currentParameterValue[index] + adjustments[index];
		}

		//***************************************************************************
		// Functions pulled out of losToEcf and computeViewingPixel
		// **************************************************************************
		void UsgsAstroProjectedLsSensorModel::getQuaternions(const double& time,
		double q[4]) const {
		int nOrder = 8;
		if (m_platformFlag == 0) nOrder = 4;
		int nOrderQuat = nOrder;
		if (m_numQuaternions < 6 && nOrder == 8) nOrderQuat = 4;

		MESSAGE_LOG(
		spdlog::level::debug,
		"Calculating getQuaternions for time {} with {}"
		"order lagrange",
		time, nOrder)
		lagrangeInterp(m_numQuaternions / 4, &m_quaternions[0], m_t0Quat, m_dtQuat,
		time, 4, nOrderQuat, q);
		}

		//***************************************************************************
		// UsgsAstroLineScannerSensorModel::calculateAttitudeCorrection
		//***************************************************************************
		void UsgsAstroProjectedLsSensorModel::calculateAttitudeCorrection(
		const double& time, const std::vector<double>& adj,
		double attCorr[9]) const {
		MESSAGE_LOG(
		spdlog::level::debug,
		"Computing calculateAttitudeCorrection (with adjustment)"
		"for time {}",
		time)
		double aTime = time - m_t0Quat;
		double euler[3];
		double nTime = aTime / m_halfTime;
		double nTime2 = nTime * nTime;
		euler[0] = (getValue(6, adj) + getValue(9, adj) * nTime +
		getValue(12, adj) * nTime2) /
		m_flyingHeight;
		euler[1] = (getValue(7, adj) + getValue(10, adj) * nTime +
		getValue(13, adj) * nTime2) /
		m_flyingHeight;
		euler[2] = (getValue(8, adj) + getValue(11, adj) * nTime +
		getValue(14, adj) * nTime2) /
		m_halfSwath;
		MESSAGE_LOG(
		spdlog::level::trace,
		"calculateAttitudeCorrection: euler {} {} {}",
		euler[0], euler[1], euler[2])

		calculateRotationMatrixFromEuler(euler, attCorr);
		}

		//***************************************************************************
		// UsgsAstroLineScannerSensorModel::computeProjectiveApproximation
		//***************************************************************************
		void UsgsAstroProjectedLsSensorModel::computeProjectiveApproximation(const csm::EcefCoord& gp,
		csm::ImageCoord& ip) const {
		MESSAGE_LOG(
		spdlog::level::debug,
		"Computing projective approximation for ground point ({}, {}, {})",
		gp.x, gp.y, gp.z);
		if (m_useApproxInitTrans) {
		std::vector<double> const& u = m_projTransCoeffs; // alias, to save on typing

		double x = gp.x, y = gp.y, z = gp.z;
		double line_den = 1 + u[4] * x + u[5] * y + u[6] * z;
		double samp_den = 1 + u[11] * x + u[12] * y + u[13] * z;

		// Sanity checks. Ensure we don't divide by 0 and that the numbers are valid.
		if (line_den == 0.0 \|\| std::isnan(line_den) \|\| std::isinf(line_den) \|\|
		samp_den == 0.0 \|\| std::isnan(samp_den) \|\| std::isinf(samp_den)) {

		ip.line = m_nLines / 2.0;
		ip.samp = m_nSamples / 2.0;
		MESSAGE_LOG(
		spdlog::level::warn,
		"Computing initial guess with constant approx line/2 and sample/2");

		return;
		}

		// Apply the formula
		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;
		double numCols = m_nSamples;
		if (ip.line < 0.0) ip.line = 0.0;
		if (ip.line > numRows) ip.line = numRows;

		if (ip.samp < 0.0) ip.samp = 0.0;
		if (ip.samp > numCols) ip.samp = numCols;
		} else {
		ip.line = m_nLines / 2.0;
		ip.samp = m_nSamples / 2.0;
		}
		MESSAGE_LOG(
		spdlog::level::debug,
		"Projective approximation ({}, {})",
		ip.line, ip.samp);
		}

		//***************************************************************************
		// UsgsAstroLineScannerSensorModel::createProjectiveApproximation
		//***************************************************************************
		void UsgsAstroProjectedLsSensorModel::createProjectiveApproximation() {
		MESSAGE_LOG(
		spdlog::level::debug,
		"Calculating createProjectiveApproximation");

		// Use 9 points (9*4 eventual matrix rows) as we need to fit 14 variables.
		const int numPts = 9;
		double u_factors[numPts] = {0.0, 0.0, 0.0, 0.5, 0.5, 0.5, 1.0, 1.0, 1.0};
		double v_factors[numPts] = {0.0, 0.5, 1.0, 0.0, 0.5, 1.0, 0.0, 0.5, 1.0};

		csm::EcefCoord refPt = getReferencePoint();

		double desired_precision = 0.01;
		double height = computeEllipsoidElevation(
		refPt.x, refPt.y, refPt.z, m_majorAxis, m_minorAxis, desired_precision);
		if (std::isnan(height)) {
		MESSAGE_LOG(
		spdlog::level::warn,
		"createProjectiveApproximation: computeElevation of "
		"reference point {} {} {} with 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;
		}
		MESSAGE_LOG(
		spdlog::level::trace,
		"createProjectiveApproximation: computeElevation of "
		"reference point {} {} {} with desired precision "
		"{} 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;

		std::vector<csm::ImageCoord> ip(2 * numPts);
		std::vector<csm::EcefCoord> gp(2 * numPts);

		// Sample at two heights above the ellipsoid in order to get a
		// reliable estimate of the relationship between image points and
		// ground points.

		for (int i = 0; i < numPts; i++) {
		ip[i].line = u_factors[i] * numImageRows;
		ip[i].samp = v_factors[i] * numImageCols;
		gp[i] = imageToGround(ip[i], height);
		}

		double delta_z = 100.0;
		height += delta_z;
		for (int i = 0; i < numPts; i++) {
		ip[i + numPts].line = u_factors[i] * numImageRows;
		ip[i + numPts].samp = v_factors[i] * numImageCols;
		gp[i + numPts] = imageToGround(ip[i + numPts], height);
		}

		usgscsm::computeBestFitProjectiveTransform(ip, gp, m_projTransCoeffs);
		m_useApproxInitTrans = true;

		MESSAGE_LOG(
		spdlog::level::debug,
		"Completed createProjectiveApproximation");
		}

		//***************************************************************************
		// UsgsAstroLineScannerSensorModel::constructStateFromIsd
		//***************************************************************************