Added most of imageToGround (#284) (f40dd961) · Commits · aflab / astrogeology / Usgscsm

include/usgscsm/UsgsAstroSarSensorModel.h

+2 −0

Original line number	Diff line number	Diff line
		@@ -201,6 +201,8 @@ class UsgsAstroSarSensorModel : public csm::RasterGM, virtual public csm::Settab

		double slantRangeToGroundRange(const csm::EcefCoord& groundPt, double time, double slantRange, double tolerance) const;

		double groundRangeToSlantRange(double groundRange, const std::vector<double> &coeffs) const;

		csm::EcefVector getSpacecraftPosition(double time) const;
		csm::EcefVector getSpacecraftVelocity(double time) const;
		std::vector<double> getRangeCoefficients(double time) const;

include/usgscsm/Utilities.h

+16 −0

Original line number	Diff line number	Diff line
		@@ -10,6 +10,7 @@

		#include <json/json.hpp>

		#include <csm.h>
		#include <Warning.h>
		// methods pulled out of los2ecf and computeViewingPixel

		@@ -84,6 +85,21 @@ double secantRoot(
		double epsilon = 1e-10,
		int maxIterations = 30);

		// Vector operations
		csm::EcefVector operator*(double scalar, const csm::EcefVector &vec);
		csm::EcefVector operator*(const csm::EcefVector &vec, double scalar);
		csm::EcefVector operator/(const csm::EcefVector &vec, double scalar);
		csm::EcefVector operator+(const csm::EcefVector &vec1, const csm::EcefVector &vec2);
		csm::EcefVector operator-(const csm::EcefVector &vec1, const csm::EcefVector &vec2);
		double dot(const csm::EcefVector &vec1, const csm::EcefVector &vec2);
		csm::EcefVector cross(const csm::EcefVector &vec1, const csm::EcefVector &vec2);
		double norm(const csm::EcefVector &vec);
		csm::EcefVector normalized(const csm::EcefVector &vec);
		// The projection of vec1 onto vec2. The component of vec1 that is parallel to vec2
		csm::EcefVector projection(const csm::EcefVector &vec1, const csm::EcefVector &vec2);
		// The rejection of vec1 onto vec2. The component of vec1 that is orthogonal to vec2
		csm::EcefVector rejection(const csm::EcefVector &vec1, const csm::EcefVector &vec2);

		// Methods for checking/accessing the ISD

		double metric_conversion(double val, std::string from, std::string to="m");

src/UsgsAstroSarSensorModel.cpp

+44 −19

Original line number	Diff line number	Diff line
		@@ -360,20 +360,15 @@ double UsgsAstroSarSensorModel::dopplerShift(
		csm::EcefCoord groundPt,
		double tolerance) const
		{
		std::function<double(double)> dopplerShiftFunction = [this, groundPt](double time) {
		csm::EcefVector groundVec(groundPt.x ,groundPt.y, groundPt.z);
		std::function<double(double)> dopplerShiftFunction = [this, groundVec](double time) {
		csm::EcefVector spacecraftPosition = getSpacecraftPosition(time);
		csm::EcefVector spacecraftVelocity = getSpacecraftVelocity(time);
		double lookVector[3];
		csm::EcefVector lookVector = spacecraftPosition - groundVec;

		lookVector[0] = spacecraftPosition.x - groundPt.x;
		lookVector[1] = spacecraftPosition.y - groundPt.y;
		lookVector[2] = spacecraftPosition.z - groundPt.z;
		double slantRange = norm(lookVector);

		double slantRange = sqrt(pow(lookVector[0], 2) + pow(lookVector[1], 2) + pow(lookVector[2], 2));


		double dopplerShift = -2.0 * (lookVector[0]spacecraftVelocity.x + lookVector[1]spacecraftVelocity.y
		+ lookVector[2]spacecraftVelocity.z)/(slantRange m_wavelength);
		double dopplerShift = -2.0 * dot(lookVector, spacecraftVelocity) / (slantRange * m_wavelength);

		return dopplerShift;
		};
		@@ -386,13 +381,9 @@ double UsgsAstroSarSensorModel::dopplerShift(
		double UsgsAstroSarSensorModel::slantRange(csm::EcefCoord surfPt,
		double time) const
		{
		csm::EcefVector surfVec(surfPt.x ,surfPt.y, surfPt.z);
		csm::EcefVector spacecraftPosition = getSpacecraftPosition(time);
		double lookVector[3];

		lookVector[0] = spacecraftPosition.x - surfPt.x;
		lookVector[1] = spacecraftPosition.y - surfPt.y;
		lookVector[2] = spacecraftPosition.z - surfPt.z;
		return sqrt(pow(lookVector[0], 2) + pow(lookVector[1], 2) + pow(lookVector[2], 2));
		return norm(spacecraftPosition - surfVec);
		}

		double UsgsAstroSarSensorModel::slantRangeToGroundRange(
		@@ -405,8 +396,8 @@ double UsgsAstroSarSensorModel::slantRangeToGroundRange(

		// Calculates the ground range from the slant range.
		std::function<double(double)> slantRangeToGroundRangeFunction =
		[coeffs, slantRange](double groundRange){
		return slantRange - (coeffs[0] + groundRange * (coeffs[1] + groundRange * (coeffs[2] + groundRange * coeffs[3])));
		[this, coeffs, slantRange](double groundRange){
		return slantRange - groundRangeToSlantRange(groundRange, coeffs);
		};

		// Need to come up with an initial guess when solving for ground
		@@ -424,6 +415,11 @@ double UsgsAstroSarSensorModel::slantRangeToGroundRange(
		return brentRoot(minGroundRangeGuess, maxGroundRangeGuess, slantRangeToGroundRangeFunction, tolerance);
		}

		double UsgsAstroSarSensorModel::groundRangeToSlantRange(double groundRange, const std::vector<double> &coeffs) const
		{
		return coeffs[0] + groundRange * (coeffs[1] + groundRange * (coeffs[2] + groundRange * coeffs[3]));
		}


		csm::ImageCoordCovar UsgsAstroSarSensorModel::groundToImage(
		const csm::EcefCoordCovar& groundPt,
		@@ -443,7 +439,36 @@ csm::EcefCoord UsgsAstroSarSensorModel::imageToGround(
		double* achievedPrecision,
		csm::WarningList* warnings) const
		{
		return csm::EcefCoord(0.0, 0.0, 0.0);
		double time = m_startingEphemerisTime + (imagePt.line - 0.5) * m_exposureDuration;
		double groundRange = imagePt.samp * m_scaledPixelWidth;
		std::vector<double> coeffs = getRangeCoefficients(time);
		double slantRange = groundRangeToSlantRange(groundRange, coeffs);

		// Compute the in-track, cross-track, nadir, coordinate system to solve in
		csm::EcefVector spacecraftPosition = getSpacecraftPosition(time);
		double positionMag = norm(spacecraftPosition);
		csm::EcefVector spacecraftVelocity = getSpacecraftVelocity(time);
		// In-track unit vector
		csm::EcefVector vHat = normalized(spacecraftVelocity);
		// Nadir unit vector
		csm::EcefVector tHat = normalized(rejection(spacecraftPosition, vHat));
		// Cross-track unit vector
		csm::EcefVector uHat = cross(vHat, tHat);

		// Compute the spacecraft position in the new coordinate system
		// The cross-track unit vector is orthogonal to the position so we ignore it
		double nadirComp = dot(spacecraftPosition, tHat);
		double inTrackComp = dot(spacecraftPosition, vHat);

		// Compute the substituted values
		// TODO properly handle ellipsoid radii
		double radiusSqr = m_majorAxis * m_majorAxis;
		double alpha = (radiusSqr - slantRange * slantRange - positionMag * positionMag) / (2 * nadirComp);
		// TODO use right/left look to determine +/-
		double beta = -sqrt(slantRange * slantRange - alpha * alpha);
		csm::EcefVector groundVec = alpha * tHat + beta * uHat + spacecraftPosition;

		return csm::EcefCoord(groundVec.x, groundVec.y, groundVec.z);
		}

		csm::EcefCoordCovar UsgsAstroSarSensorModel::imageToGround(

src/Utilities.cpp

+68 −0

Original line number	Diff line number	Diff line
		@@ -405,6 +405,74 @@ double secantRoot(double lowerBound, double upperBound, std::function<double(dou
		}


		csm::EcefVector operator*(double scalar, const csm::EcefVector &vec)
		{
		return csm::EcefVector(
		scalar * vec.x,
		scalar * vec.y,
		scalar * vec.z);
		}

		csm::EcefVector operator*(const csm::EcefVector &vec, double scalar)
		{
		return scalar * vec;
		}

		csm::EcefVector operator/(const csm::EcefVector &vec, double scalar)
		{
		return 1.0 / scalar * vec;
		}

		csm::EcefVector operator+(const csm::EcefVector &vec1, const csm::EcefVector &vec2)
		{
		return csm::EcefVector(
		vec1.x + vec2.x,
		vec1.y + vec2.y,
		vec1.z + vec2.z);
		}

		csm::EcefVector operator-(const csm::EcefVector &vec1, const csm::EcefVector &vec2)
		{
		return csm::EcefVector(
		vec1.x - vec2.x,
		vec1.y - vec2.y,
		vec1.z - vec2.z);
		}

		double dot(const csm::EcefVector &vec1, const csm::EcefVector &vec2)
		{
		return vec1.x * vec2.x + vec1.y * vec2.y + vec1.z * vec2.z;
		}

		csm::EcefVector cross(const csm::EcefVector &vec1, const csm::EcefVector &vec2)
		{
		return csm::EcefVector(
		vec1.y * vec2.z - vec1.z * vec2.y,
		vec1.z * vec2.x - vec1.x * vec2.z,
		vec1.x * vec2.y - vec1.y * vec2.x);
		}

		double norm(const csm::EcefVector &vec)
		{
		return sqrt(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z);
		}

		csm::EcefVector normalized(const csm::EcefVector &vec)
		{
		return vec / norm(vec);
		}

		csm::EcefVector projection(const csm::EcefVector &vec1, const csm::EcefVector &vec2)
		{
		return dot(vec1, vec2) / dot(vec2, vec2) * vec2;
		}

		csm::EcefVector rejection(const csm::EcefVector &vec1, const csm::EcefVector &vec2)
		{
		return vec1 - projection(vec1, vec2);
		}


		// convert a measurement
		double metric_conversion(double val, std::string from, std::string to) {
		json typemap = {

tests/SarTests.cpp

+5 −4

Original line number	Diff line number	Diff line
		@@ -44,11 +44,12 @@ TEST_F(SarSensorModel, State) {
		}

		TEST_F(SarSensorModel, Center) {
		csm::ImageCoord imagePt(7.5, 7.5);
		csm::ImageCoord imagePt(500.0, 500.0);
		csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0);
		EXPECT_NEAR(groundPt.x, 0, 1e-8);
		EXPECT_NEAR(groundPt.y, 0, 1e-8);
		EXPECT_NEAR(groundPt.z, 0, 1e-8);
		// TODO these tolerances are bad
		EXPECT_NEAR(groundPt.x, 1737391.90602155, 1e-2);
		EXPECT_NEAR(groundPt.y, 3749.98835331, 1e-2);
		EXPECT_NEAR(groundPt.z, -3749.99708833, 1e-2);
		}

		TEST_F(SarSensorModel, GroundToImage) {