/** 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.
*
* @param ux distorted focal plane x in millimeters
* @param uy distorted focal plane y in millimeters
* @param uz distorted focal plane z in millimeters
*
* @return conversion was successful
*/
bool CameraGroundMap::SetFocalPlane(const double ux, const double uy,
double uz) {
NaifStatus::CheckErrors();
SpiceDouble lookC[3];
lookC[0] = ux;
lookC[1] = uy;
lookC[2] = uz;
SpiceDouble unitLookC[3];
vhat_c(lookC,unitLookC);
NaifStatus::CheckErrors();
return p_camera->SetLookDirection(unitLookC);
}
/** 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.
*
* @param ux Distorted focal plane x in millimeters
* @param uy Distorted focal plane y in millimeters
* @param uz Distorted focal plane z in millimeters
*
* @return @b bool Indicates whether the conversion was successful
*
* @internal
* @history 2008-01-02 Tracie Sucharski - Check incoming pixel for validity
* against edge of pixel not center.
* @history 2008-02-05 Tracie Sucharski, Replaced unitVector files with
* Rick McCloskey's code to calculate look
* direction
* @history 2008-02-14 Tracie Sucharski, Change imgSamp/imgLine to double
* so that fractional pixels are used in calculations.
*/
bool VimsGroundMap::SetFocalPlane(const double ux, const double uy,
const double uz) {
p_ux = ux;
p_uy = uy;
p_uz = uz;
double imgSamp = ux;
double imgLine = uy;
if ((imgLine < 0.5) || (imgLine > p_camera->ParentLines() + 0.5) ||
(imgSamp < 0.5) || (imgSamp > p_camera->ParentSamples() + 0.5)) {
return false;
}
imgLine--;
imgSamp--;
// does interline_delay & exposure-duration account for summing modes?
// if not, won't use p_parentLine/p_parentSample
double et = 0.;
if (p_channel == "VIS") {
et = (p_etStart + ((p_irExp * p_swathWidth) - p_visExp) / 2.) +
((imgLine + 0.5) * p_visExp);
}
else if (p_channel == "IR") {
et = (double)p_etStart +
(imgLine * p_camera->ParentSamples() * p_irExp) +
(imgLine * p_interlineDelay) + ((imgSamp + 0.5) * p_irExp);
}
p_camera->setTime(et);
// get Look Direction
SpiceDouble lookC[3];
LookDirection(lookC);
SpiceDouble unitLookC[3];
vhat_c(lookC, unitLookC);
return p_camera->SetLookDirection(unitLookC);
}
/** Compute ground position from slant range
*
* @param ux Slant range distance
* @param uy Doppler shift (always 0.0)
* @param uz Not used
*
* @return conversion was successful
*/
bool RadarGroundMap::SetFocalPlane(const double ux, const double uy,
double uz) {
SpiceRotation *bodyFrame = p_camera->BodyRotation();
SpicePosition *spaceCraft = p_camera->InstrumentPosition();
// Get spacecraft position and velocity to create a state vector
std::vector<double> Ssc(6);
// Load the state into Ssc
vequ_c ( (SpiceDouble *) &(spaceCraft->Coordinate()[0]), &Ssc[0]);
vequ_c ( (SpiceDouble *) &(spaceCraft->Velocity()[0]), &Ssc[3]);
// Rotate state vector to body-fixed
std::vector<double> bfSsc(6);
bfSsc = bodyFrame->ReferenceVector(Ssc);
// Extract body-fixed position and velocity
std::vector<double> Vsc(3);
std::vector<double> Xsc(3);
vequ_c ( &bfSsc[0], (SpiceDouble *) &(Xsc[0]) );
vequ_c ( &bfSsc[3], (SpiceDouble *) &(Vsc[0]) );
// Compute intrack, crosstrack, and radial coordinate
SpiceDouble i[3];
vhat_c (&Vsc[0],i);
SpiceDouble c[3];
SpiceDouble dp;
dp = vdot_c(&Xsc[0],i);
SpiceDouble p[3],q[3];
vscl_c(dp,i,p);
vsub_c(&Xsc[0],p,q);
vhat_c(q,c);
SpiceDouble r[3];
vcrss_c(i,c,r);
// What is the initial guess for R
double radii[3];
p_camera->Radii(radii);
SpiceDouble R = radii[0];
SpiceDouble lastR = DBL_MAX;
SpiceDouble rlat;
SpiceDouble rlon;
SpiceDouble lat = DBL_MAX;
SpiceDouble lon = DBL_MAX;
double slantRangeSqr = (ux * p_rangeSigma) / 1000.;
slantRangeSqr = slantRangeSqr*slantRangeSqr;
SpiceDouble X[3];
int iter = 0;
do {
double normXsc = vnorm_c(&Xsc[0]);
double alpha = (R*R - slantRangeSqr - normXsc*normXsc) /
(2.0 * vdot_c(&Xsc[0],c));
double arg = slantRangeSqr - alpha*alpha;
if (arg < 0.0) return false;
double beta = sqrt(arg);
if (p_lookDirection == Radar::Left) beta *= -1.0;
SpiceDouble alphac[3],betar[3];
vscl_c(alpha,c,alphac);
vscl_c(beta,r,betar);
vadd_c(alphac,betar,alphac);
vadd_c(&Xsc[0],alphac,X);
// Convert X to lat,lon
lastR = R;
reclat_c(X,&R,&lon,&lat);
rlat = lat*180.0/Isis::PI;
rlon = lon*180.0/Isis::PI;
R = GetRadius(rlat,rlon);
iter++;
}
while (fabs(R-lastR) > p_tolerance && iter < 30);
if (fabs(R-lastR) > p_tolerance) return false;
lat = lat*180.0/Isis::PI;
lon = lon*180.0/Isis::PI;
while (lon < 0.0) lon += 360.0;
// Compute body fixed look direction
std::vector<double> lookB;
lookB.resize(3);
lookB[0] = X[0] - Xsc[0];
lookB[1] = X[1] - Xsc[1];
lookB[2] = X[2] - Xsc[2];
std::vector<double> lookJ = bodyFrame->J2000Vector(lookB);
SpiceRotation *cameraFrame = p_camera->InstrumentRotation();
std::vector<double> lookC = cameraFrame->ReferenceVector(lookJ);
SpiceDouble unitLookC[3];
vhat_c(&lookC[0],unitLookC);
p_camera->SetLookDirection(unitLookC);
return p_camera->Sensor::SetUniversalGround(lat,lon);
}
/** 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
*
* @return conversion was successful
*/
bool RadarGroundMap::SetGround(const double lat, const double lon, const double radius) {
// Get the ground point in rectangular coordinates (X)
SpiceDouble X[3];
SpiceDouble rlat = lat*Isis::PI/180.0;
SpiceDouble rlon = lon*Isis::PI/180.0;
latrec_c(radius,rlon,rlat,X);
// Compute lower bound for Doppler shift
double et1 = p_camera->Spice::CacheStartTime();
p_camera->Sensor::SetEphemerisTime(et1);
double xv1 = ComputeXv(X);
// Compute upper bound for Doppler shift
double et2 = p_camera->Spice::CacheEndTime();
p_camera->Sensor::SetEphemerisTime(et2);
double xv2 = ComputeXv(X);
// Make sure we bound root (xv = 0.0)
if ((xv1 < 0.0) && (xv2 < 0.0)) return false;
if ((xv1 > 0.0) && (xv2 > 0.0)) return false;
// Order the bounds
double fl,fh,xl,xh;
if (xv1 < xv2) {
fl = xv1;
fh = xv2;
xl = et1;
xh = et2;
}
else {
fl = xv2;
fh = xv1;
xl = et2;
xh = et1;
}
// Iterate a max of 30 times
for (int j=0; j<30; j++) {
// Use the secant method to guess the next et
double etGuess = xl + (xh - xl) * fl / (fl - fh);
// Compute the guessed Doppler shift. Hopefully
// this guess converges to zero at some point
p_camera->Sensor::SetEphemerisTime(etGuess);
double fGuess = ComputeXv(X);
// Update the bounds
double delTime;
if (fGuess < 0.0) {
delTime = xl - etGuess;
xl = etGuess;
fl = fGuess;
}
else {
delTime = xh - etGuess;
xh = etGuess;
fh = fGuess;
}
// See if we are done
if ((fabs(delTime) <= p_timeTolerance) || (fGuess == 0.0)) {
SpiceRotation *bodyFrame = p_camera->BodyRotation();
SpicePosition *spaceCraft = p_camera->InstrumentPosition();
// Get body fixed spacecraft velocity and position
std::vector<double> Ssc(6);
// Load the state into Ssc and rotate to body-fixed
vequ_c ( (SpiceDouble *) &(spaceCraft->Coordinate()[0]), &Ssc[0]);
vequ_c ( (SpiceDouble *) &(spaceCraft->Velocity()[0]), &Ssc[3]);
std::vector<double> bfSsc(6);
bfSsc = bodyFrame->ReferenceVector(Ssc);
// Extract the body-fixed position and velocity from the state
std::vector<double> Vsc(3);
std::vector<double> Xsc(3);
vequ_c ( &bfSsc[0], (SpiceDouble *) &(Xsc[0]) );
vequ_c ( &bfSsc[3], (SpiceDouble *) &(Vsc[0]) );
// Determine if focal plane coordinate falls on the correct side of the
// spacecraft. Radar has both left and right look directions. Make sure
// the coordinate is on the same side as the look direction. This is done
// by (X - S) . (V x S) where X=ground point vector, S=spacecraft position
// vector, and V=velocity vector. If the dot product is greater than 0, then
// the point is on the right side. If the dot product is less than 0, then
// the point is on the left side. If the dot product is 0, then the point is
// directly under the spacecraft (neither left or right) and is invalid.
SpiceDouble vout1[3];
SpiceDouble vout2[3];
SpiceDouble dp;
vsub_c(X,&Xsc[0],vout1);
vcrss_c(&Vsc[0],&Xsc[0],vout2);
dp = vdot_c(vout1,vout2);
if (dp > 0.0 && p_lookDirection == Radar::Left) return false;
if (dp < 0.0 && p_lookDirection == Radar::Right) return false;
if (dp == 0.0) return false;
// Compute body fixed look direction
std::vector<double> lookB;
lookB.resize(3);
lookB[0] = X[0] - Xsc[0];
lookB[1] = X[1] - Xsc[1];
lookB[2] = X[2] - Xsc[2];
std::vector<double> lookJ = bodyFrame->J2000Vector(lookB);
SpiceRotation *cameraFrame = p_camera->InstrumentRotation();
std::vector<double> lookC = cameraFrame->ReferenceVector(lookJ);
SpiceDouble unitLookC[3];
vhat_c(&lookC[0],unitLookC);
p_camera->SetLookDirection(unitLookC);
p_camera->SetFocalLength(p_slantRange*1000.0);
p_focalPlaneX = p_slantRange / p_rangeSigma;
p_focalPlaneY = 0.0;
return true;
}
}
return false;
}
