Merged CameraGroundMap changes (new functions, target body support,... (3f50efb0) · Commits · aflab / astrogeology / ISIS3

isis/src/base/objs/CameraGroundMap/CameraGroundMap.cpp

+273 −110

Original line number	Diff line number	Diff line
		@@ -22,38 +22,48 @@
		*/
		#include "CameraGroundMap.h"

		#include <iostream>

		#include <QDebug>

		#include <iostream>
		#include <SpiceUsr.h>

		#include "NaifStatus.h"
		#include "SurfacePoint.h"
		#include "IException.h"
		#include "Latitude.h"
		#include "Longitude.h"
		#include "NaifStatus.h"
		#include "SurfacePoint.h"
		#include "Target.h"

		using namespace std;

		namespace Isis {

		/**
		* Constructor
		*
		* @param parent Pointer to camera to be used for mapping with ground
		*/
		CameraGroundMap::CameraGroundMap(Camera *parent) {
		p_camera = parent;
		p_camera->SetGroundMap(this);
		}

		/** Compute ground position from focal plane coordinate

		/**
		* Compute ground position from focal plane coordinate
		*
		* This method will compute the ground position given an
		* undistorted focal plane coordinate. Note that the latitude/longitude
		* value can be obtained from the camera class passed into the constructor.
		* value can be obtained from the camera pointer passed into the constructor.
		*
		* @param ux distorted focal plane x in millimeters
		* @param uy distorted focal plane y in millimeters
		* @param uz distorted focal plane z in millimeters
		* @param ux undistorted focal plane x in millimeters
		* @param uy undistorted focal plane y in millimeters
		* @param uz undistorted focal plane z in millimeters
		*
		* @return conversion was successful
		* @return @b bool If conversion was successful
		*/
		bool CameraGroundMap::SetFocalPlane(const double ux, const double uy,
		double uz) {
		bool CameraGroundMap::SetFocalPlane(const double ux, const double uy, const double uz) {
		NaifStatus::CheckErrors();

		SpiceDouble lookC[3];
		@@ -70,19 +80,23 @@ namespace Isis {
		return result;
		}

		/** Compute undistorted focal plane coordinate from ground position

		/**
		* Compute undistorted focal plane coordinate from ground position
		*
		* @param lat planetocentric latitude in degrees
		* @param lon planetocentric longitude in degrees
		*
		* @return conversion was successful
		* @return @b bool If conversion was successful
		*/
		bool CameraGroundMap::SetGround(const Latitude &lat, const Longitude &lon) {
		if (p_camera->target()->shape()->name() == "Plane") {
		double radius = lat.degrees(); // m
		double radius = lat.degrees();
		// double azimuth = lon.degrees();
		Latitude lat(0., Angle::Degrees);
		if (radius < 0.0) radius = 0.0; // TODO: massive, temporary kluge to get around testing latitude at -90 in caminfo app (are there more issues like this? Probably)KE
		if (radius < 0.0) radius = 0.0; // TODO: massive, temporary kluge to get around testing
		// latitude at -90 in caminfo app (are there
		// more issues like this? Probably)KE
		if (p_camera->Sensor::SetGround(SurfacePoint(lat, lon, Distance(radius, Distance::Meters)))) {
		LookCtoFocalPlaneXY();
		return true;
		@@ -101,7 +115,10 @@ namespace Isis {
		return false;
		}

		//! Compute undistorted focal plane coordinate from camera look vector

		/**
		* Compute undistorted focal plane coordinate from camera look vector
		*/
		void CameraGroundMap::LookCtoFocalPlaneXY() {
		double lookC[3];
		p_camera->Sensor::LookDirection(lookC);
		@@ -114,13 +131,13 @@ namespace Isis {
		p_focalPlaneY = lookC[1] * scale;
		}

		/** Compute undistorted focal plane coordinate from ground position that includes a local radius

		/**
		* Compute undistorted focal plane coordinate from ground position that includes a local radius
		*
		* @param lat planetocentric latitude in degrees
		* @param lon planetocentric longitude in degrees
		* @param radius local radius in meters
		* @param surfacePoint Surface point (ground position)
		*
		* @return conversion was successful
		* @return @b bool If conversion was successful
		*/
		bool CameraGroundMap::SetGround(const SurfacePoint &surfacePoint) {
		if (p_camera->Sensor::SetGround(surfacePoint)) {
		@@ -132,20 +149,24 @@ namespace Isis {
		}


		/** Compute undistorted focal plane coordinate from ground position using current Spice from SetImage call
		/**
		* Compute undistorted focal plane coordinate from ground position using current Spice
		* from SetImage call
		*
		* This method will compute the undistorted focal plane coordinate for
		* a ground position, using the current Spice settings (time and kernels)
		* without resetting the current point values for lat/lon/radius/p_pB/x/y. The
		* class value for p_look is set by this method.
		* without resetting the current point values for lat/lon/radius/m_pB/x/y. The
		* class value for m_lookJ is set by this method.
		*
		* @param point
		* @param point[in] Surface point (ground position)
		* @param cudx[out] Pointer to computed undistored x focal plane coordinate
		* @param cudy[out] Pointer to computed undistored y focal plane coordinate
		*
		* @return conversion was successful
		* @return @b bool If conversion was successful
		*/
		bool CameraGroundMap::GetXY(const SurfacePoint &point, double cudx, double cudy) {

		double pB[3];
		vector<double> pB(3);
		pB[0] = point.GetX().kilometers();
		pB[1] = point.GetY().kilometers();
		pB[2] = point.GetZ().kilometers();
		@@ -157,23 +178,29 @@ namespace Isis {

		// Should a check be added to make sure SetImage has been called???

		// Compute the look vector in body-fixed coordinates
		// double pB[3]; // Point on surface
		// latrec_c(radius / 1000.0, lon * Isis::PI / 180.0, lat * Isis::PI / 180.0, pB);

		// Get spacecraft vector in body-fixed coordinates
		// Get spacecraft vector in j2000 coordinates
		SpiceRotation *bodyRot = p_camera->bodyRotation();
		SpiceRotation *instRot = p_camera->instrumentRotation();
		std::vector<double> sB = bodyRot->ReferenceVector(p_camera->instrumentPosition()->Coordinate());
		std::vector<double> lookB(3);
		for(int ic = 0; ic < 3; ic++) lookB[ic] = pB[ic] - sB[ic];
		vector<double> pJ = bodyRot->J2000Vector(pB);
		vector<double> sJ = p_camera->instrumentPosition()->Coordinate();

		// Calculate lookJ
		vector<double> lookJ(3);
		for (int ic = 0; ic < 3; ic++) {
		lookJ[ic] = pJ[ic] - sJ[ic];
		}

		// Save pB for target body partial derivative calculations NEW *** DAC 8-14-2015
		m_pB = pB;

		// Check for point on back of planet by checking to see if surface point is viewable (test emission angle)
		// Check for point on back of planet by checking to see if surface point is viewable
		// (test emission angle)
		// During iterations, we may not want to do the back of planet test???
		vector<double> lookB = bodyRot->ReferenceVector(lookJ);
		double upsB[3], upB[3], dist;
		vminus_c((SpiceDouble *) &lookB[0], upsB);
		unorm_c(upsB, upsB, &dist);
		unorm_c(pB, upB, &dist);
		unorm_c((SpiceDouble *) &pB[0], upB, &dist);
		double angle = vdot_c(upB, upsB);
		double emission;
		if (angle > 1) {
		@@ -185,13 +212,16 @@ namespace Isis {
		else {
		emission = acos(angle) * 180.0 / Isis::PI;
		}
		if(fabs(emission) > 90.) return false;

		if (fabs(emission) > 90.) {
		return false;
		}

		// Get the look vector in the camera frame and the instrument rotation
		p_lookJ.resize(3);
		p_lookJ = p_camera->bodyRotation()->J2000Vector(lookB);
		std::vector <double> lookC(3);
		lookC = instRot->ReferenceVector(p_lookJ);
		m_lookJ.resize(3);
		m_lookJ = lookJ;
		vector<double> lookC(3);
		lookC = instRot->ReferenceVector(m_lookJ);

		// Get focal length with direction for scaling coordinates
		double fl = p_camera->DistortionMap()->UndistortedFocalPlaneZ();
		@@ -202,20 +232,22 @@ namespace Isis {
		}




		/** Compute undistorted focal plane coordinate from ground position using current Spice from SetImage call
		/**
		* Compute undistorted focal plane coordinate from ground position using current Spice
		* from SetImage call
		*
		* This method will compute the undistorted focal plane coordinate for
		* a ground position, using the current Spice settings (time and kernels)
		* without resetting the current point values for lat/lon/radius/p_pB/x/y. The
		* class value for p_look is set by this method.
		* without resetting the current point values for lat/lon/radius/m_pB/x/y. The
		* class value for m_lookJ is set by this method.
		*
		* @param lat Latitude in degrees
		* @param lon Longitude in degrees
		* @param radius
		* @param lat[in] Latitude in degrees
		* @param lon[in] Longitude in degrees
		* @param radius[in] Radius in meters
		* @param cudx[out] Pointer to computed undistored x focal plane coordinate
		* @param cudy[out] Pointer to computed undistored y focal plane coordinate
		*
		* @return conversion was successful
		* @return @b bool If conversion was successful
		*/
		bool CameraGroundMap::GetXY(const double lat, const double lon,
		const double radius, double cudx, double cudy) {
		@@ -226,104 +258,156 @@ namespace Isis {
		}


		/** Compute derivative w/r to position of focal plane coordinate from ground position using current Spice from SetImage call
		/**
		* Compute derivative w/r to position of focal plane coordinate from ground position
		* using current Spice from SetImage call
		*
		* This method will compute the derivative of the undistorted focal plane coordinate for
		* a ground position with respect to a spacecraft position coordinate, using the current
		* Spice settings (time and kernels) without resetting the current point values for lat/lon/radius/x/y.
		* Spice settings (time and kernels) without resetting the current point values for
		* lat/lon/radius/x/y.
		*
		* @param varType enumerated partial type (definitions in SpicePosition)
		* @param coefIndex coefficient index of fit polynomial
		* @param *dx pointer to partial derivative of undistorted focal plane x
		* @param *dy pointer to partial derivative of undistorted focal plane y
		* @param varType[in] enumerated partial type (definitions in SpicePosition)
		* @param coefIndex[in] coefficient index of fit polynomial
		* @param *dx[out] pointer to partial derivative of undistorted focal plane x
		* @param *dy[out] pointer to partial derivative of undistorted focal plane y
		*
		* @return conversion was successful
		* @return @b bool If conversion was successful
		*/
		// also have a GetDxyDorientation and a GetDxyDpoint
		bool CameraGroundMap::GetdXYdPosition(const SpicePosition::PartialType varType, int coefIndex,
		double dx, double dy) {

		// TODO add a check to make sure p_lookJ has been set
		//TODO add a check to make sure m_lookJ has been set

		// Get directional fl for scaling coordinates
		double fl = p_camera->DistortionMap()->UndistortedFocalPlaneZ();

		// Rotate look vector into camera frame
		SpiceRotation *instRot = p_camera->instrumentRotation();
		std::vector <double> lookC(3);
		lookC = instRot->ReferenceVector(p_lookJ);
		vector<double> lookC(3);
		lookC = instRot->ReferenceVector(m_lookJ);

		SpicePosition *instPos = p_camera->instrumentPosition();

		std::vector<double> d_lookJ = instPos->CoordinatePartial(varType, coefIndex);
		vector<double> d_lookJ = instPos->CoordinatePartial(varType, coefIndex);
		for (int j = 0; j < 3; j++) d_lookJ[j] *= -1.0;
		std::vector<double> d_lookC = instRot->ReferenceVector(d_lookJ);
		vector<double> d_lookC = instRot->ReferenceVector(d_lookJ);
		dx = fl DQuotient(lookC, d_lookC, 0);
		dy = fl DQuotient(lookC, d_lookC, 1);
		return true;
		}

		/** Compute derivative of focal plane coordinate w/r to orientation from ground position using current Spice from SetImage call

		/**
		* Compute derivative of focal plane coordinate w/r to instrument using current state from
		* SetImage call
		*
		* This method will compute the derivative of the undistorted focal plane coordinate for
		* a ground position with respect to the instrument orientation, using the current Spice
		* settings (time and kernels) without resetting the current point values for lat/lon/radius/x/y.
		*
		* @param varType enumerated partial type (definitions in SpicePosition)
		* @param coefIndex coefficient index of fit polynomial
		* @param *dx pointer to partial derivative of undistorted focal plane x
		* @param *dy pointer to partial derivative of undistorted focal plane y
		* @param varType[in] enumerated partial type (definitions in SpicePosition)
		* @param coefIndex[in] coefficient index of fit polynomial
		* @param *dx[out] pointer to partial derivative of undistorted focal plane x
		* @param *dy[out] pointer to partial derivative of undistorted focal plane y
		*
		* @return conversion was successful
		* @return @b bool If conversion was successful
		*/
		// also have a GetDxyDorientation and a GetDxyDpoint
		bool CameraGroundMap::GetdXYdOrientation(const SpiceRotation::PartialType varType, int coefIndex,
		double dx, double dy) {

		// TODO add a check to make sure p_lookJ has been set
		//TODO add a check to make sure m_lookJ has been set

		// Get directional fl for scaling coordinates
		double fl = p_camera->DistortionMap()->UndistortedFocalPlaneZ();

		// Rotate look vector into camera frame
		// Rotate J2000 look vector into camera frame
		SpiceRotation *instRot = p_camera->instrumentRotation();
		std::vector <double> lookC(3);
		lookC = instRot->ReferenceVector(p_lookJ);
		vector<double> lookC(3);
		lookC = instRot->ReferenceVector(m_lookJ);

		// Rotate J2000 look vector into camera frame through the derivative rotation
		vector<double> d_lookC = instRot->ToReferencePartial(m_lookJ, varType, coefIndex);

		std::vector<double> d_lookC = instRot->ToReferencePartial(p_lookJ, varType, coefIndex);
		dx = fl DQuotient(lookC, d_lookC, 0);
		dy = fl DQuotient(lookC, d_lookC, 1);
		return true;
		}

		/** Compute derivative of focal plane coordinate w/r to ground point from ground position using current Spice from SetImage call

		/**
		* Compute derivative of focal plane coordinate w/r to target body using current state
		*
		* This method will compute the derivative of the undistorted focal plane coordinate for
		* a ground position with respect to the target body orientation, using the current Spice
		* settings (time and kernels) without resetting the current point values for lat/lon/radius/x/y.
		*
		* @param varType[in] enumerated partial type (definitions in SpicePosition)
		* @param coefIndex[in] coefficient index of fit polynomial
		* @param *dx[out] pointer to partial derivative of undistorted focal plane x
		* @param *dy[out] pointer to partial derivative of undistorted focal plane y
		*
		* @return @b bool If conversion was successful
		*/
		bool CameraGroundMap::GetdXYdTOrientation(const SpiceRotation::PartialType varType, int coefIndex,
		double dx, double dy) {

		//TODO add a check to make sure m_pB and m_lookJ have been set.
		// 0. calculate or save from previous GetXY call lookB. We need toJ2000Partial that is
		// like a derivative form of J2000Vector
		// 1. we will call d_lookJ = bodyrot->toJ2000Partial (Make sure the partials are correct for
		// the target body orientation matrix.
		// 2. we will then call d_lookC = instRot->ReferenceVector(d_lookJ)
		// 3. the rest should be the same.

		// Get directional fl for scaling coordinates
		double fl = p_camera->DistortionMap()->UndistortedFocalPlaneZ();

		// Rotate body-fixed look vector into J2000 through the derivative rotation
		SpiceRotation *bodyRot = p_camera->bodyRotation();
		SpiceRotation *instRot = p_camera->instrumentRotation();
		vector<double> dlookJ = bodyRot->toJ2000Partial(m_pB, varType, coefIndex);
		vector<double> lookC(3);
		vector<double> dlookC(3);

		// Rotate both the J2000 look vector and the derivative J2000 look vector into the camera
		lookC = instRot->ReferenceVector(m_lookJ);
		dlookC = instRot->ReferenceVector(dlookJ);

		dx = fl DQuotient(lookC, dlookC, 0);
		dy = fl DQuotient(lookC, dlookC, 1);
		return true;
		}


		/**
		* Compute derivative of focal plane coordinate w/r to ground point using current state
		*
		* This method will compute the derivative of the undistorted focal plane coordinate for
		* a ground position with respect to lat, lon, or radius, using the current Spice settings (time and kernels)
		* without resetting the current point values for lat/lon/radius/x/y.
		* a ground position with respect to lat, lon, or radius, using the current Spice settings (time
		* and kernels) without resetting the current point values for lat/lon/radius/x/y.
		*
		* @param varType enumerated partial type (definitions in SpicePosition)
		* @param coefIndex coefficient index of fit polynomial
		* @param *dx pointer to partial derivative of undistorted focal plane x
		* @param *dy pointer to partial derivative of undistorted focal plane y
		* @param d_pB Point vector
		* @param *dx[out] pointer to partial derivative of undistorted focal plane x
		* @param *dy[out] pointer to partial derivative of undistorted focal plane y
		*
		* @return conversion was successful
		*/
		bool CameraGroundMap::GetdXYdPoint(std::vector<double> d_lookB, double dx, double dy) {
		bool CameraGroundMap::GetdXYdPoint(vector<double> d_pB, double dx, double dy) {

		// TODO add a check to make sure p_lookJ has been set
		// TODO add a check to make sure m_lookJ has been set

		// Get directional fl for scaling coordinates
		double fl = p_camera->DistortionMap()->UndistortedFocalPlaneZ();

		// Rotate look vector into camera frame
		SpiceRotation *instRot = p_camera->instrumentRotation();
		std::vector <double> lookC(3);
		lookC = instRot->ReferenceVector(p_lookJ);
		vector<double> lookC(3);
		lookC = instRot->ReferenceVector(m_lookJ);

		SpiceRotation *bodyRot = p_camera->bodyRotation();
		std::vector<double> d_lookJ = bodyRot->J2000Vector(d_lookB);
		std::vector<double> d_lookC = instRot->ReferenceVector(d_lookJ);
		vector<double> d_lookJ = bodyRot->J2000Vector(d_pB);
		vector<double> d_lookC = instRot->ReferenceVector(d_lookJ);

		dx = fl DQuotient(lookC, d_lookC, 0);
		dy = fl DQuotient(lookC, d_lookC, 1);
		@@ -331,16 +415,95 @@ namespace Isis {
		}


		/** Compute derivative with respect to indicated variable of conversion function from lat/lon/rad to rectangular coord
		/**
		* Compute derivative of focal plane coordinate w/r to one of the ellipsoidal radii (a, b, or c)
		*
		* @param lat planetocentric latitude in degrees
		* @param lon planetocentric longitude in degrees
		* @param radius local radius in meters
		* This method will compute the derivative of the undistorted focal plane coordinate for
		* a ground position with respect to the a (major axis), b (minor axis), or c (polar axis) radius,
		* using the current Spice settings (time and kernels) without resetting the current point
		* values for lat/lon/radius/x/y.
		*
		* @param spoint Surface point whose derivative is to be evalutated
		* @param raxis Radius axis enumerated partial type (definitions in this header)
		*
		* @throws IException::Programmer "Invalid partial type for this method"
		*
		* @return @b vector<double> partialDerivative of body-fixed point with respect to selected
		* ellipsoid axis
		*/
		vector<double> CameraGroundMap::EllipsoidPartial(SurfacePoint spoint, PartialType raxis) {
		double rlat = spoint.GetLatitude().radians();
		double rlon = spoint.GetLongitude().radians();
		double sinLon = sin(rlon);
		double cosLon = cos(rlon);
		double sinLat = sin(rlat);
		double cosLat = cos(rlat);

		vector<double> v(3);

		switch (raxis) {
		case WRT_MajorAxis:
		v[0] = cosLat * cosLon;
		v[1] = 0.0;
		v[2] = 0.0;
		break;
		case WRT_MinorAxis:
		v[0] = 0.0;
		v[1] = cosLat * sinLon;
		v[2] = 0.0;
		break;
		case WRT_PolarAxis:
		v[0] = 0.0;
		v[1] = 0.0;
		v[2] = sinLat;
		break;
		default:
		QString msg = "Invalid partial type for this method";
		throw IException(IException::Programmer, msg, _FILEINFO_);
		}

		return v;
		}


		/**
		* Compute derivative of focal plane coordinate w/r to mean of the ellipsoidal radii (a, b, c)
		*
		* This method will compute the derivative of the undistorted focal plane coordinate for
		* a ground position with respect to the mean of the a (major axis), b (minor axis), and c
		* (polar axis) radius, using the current Spice settings (time and kernels) without resetting the
		* current point values for lat/lon/radius/x/y.
		*
		* @param spoint Surface point whose derivative is to be evalutated
		* @param meanRadius Computed mean of radii
		*
		* @return @b vector<double> partialDerivative of body-fixed point with respect to mean radius
		* TODO This method assumes the radii of all points in the adjustment have been set identically
		* to the ???
		*/
		vector<double> CameraGroundMap::MeanRadiusPartial(SurfacePoint spoint, Distance meanRadius) {
		double radkm = meanRadius.kilometers();

		vector<double> v(3);

		v[0] = spoint.GetX().kilometers() / radkm;
		v[1] = spoint.GetY().kilometers() / radkm;
		v[2] = spoint.GetZ().kilometers() / radkm;

		return v;
		}


		/**
		* Compute derivative with respect to indicated variable of conversion function from lat/lon/rad
		* to rectangular coord
		*
		* @param spoint Surface point (ground position)
		* @param wrt take derivative with respect to this value
		*
		* @return partialDerivative
		* @return @b vector<double> partialDerivative Computed derivative
		*/
		std::vector<double> CameraGroundMap::PointPartial(SurfacePoint spoint, PartialType wrt) {
		vector<double> CameraGroundMap::PointPartial(SurfacePoint spoint, PartialType wrt) {
		double rlat = spoint.GetLatitude().radians();
		double rlon = spoint.GetLongitude().radians();
		double sinLon = sin(rlon);
		@@ -349,7 +512,7 @@ namespace Isis {
		double cosLat = cos(rlat);
		double radkm = spoint.GetLocalRadius().kilometers();

		std::vector<double> v(3);
		vector<double> v(3);
		if (wrt == WRT_Latitude) {
		v[0] = -radkm * sinLat * cosLon;
		v[1] = -radkm * sinLon * sinLat;
		@@ -380,10 +543,10 @@ namespace Isis {
		* @param dlook derivative of look vector in camera frame
		* @param index vector value to differentiate
		*
		* @return derivative
		* @return @b double derivative Computed derivative
		*/
		double CameraGroundMap::DQuotient(std::vector<double> &look,
		std::vector<double> &dlook,
		double CameraGroundMap::DQuotient(vector<double> &look,
		vector<double> &dlook,
		int index) {
		return (look[2] * dlook[index] - look[index] * dlook[2]) /
		(look[2] * look[2]);

isis/src/base/objs/CameraGroundMap/CameraGroundMap.h

+50 −29

File changed.

Preview size limit exceeded, changes collapsed.