Isis 3.0 Programmer Reference
Back | Home
RadarGroundMap.cpp
Go to the documentation of this file.
1 
23 #include "RadarGroundMap.h"
24 #include "iTime.h"
25 #include "Latitude.h"
26 #include "Longitude.h"
27 #include "Target.h"
28 
29 using namespace std;
30 
31 namespace Isis {
32  RadarGroundMap::RadarGroundMap(Camera *parent, Radar::LookDirection ldir,
33  double waveLength) :
34  CameraGroundMap(parent) {
35  p_camera = parent;
36  p_lookDirection = ldir;
37  p_waveLength = waveLength;
38 
39  // Angular tolerance based on radii and
40  // slant range (Focal length)
41 // Distance radii[3];
42 // p_camera->radii(radii);
43 // p_tolerance = p_camera->FocalLength() / radii[2];
44 // p_tolerance *= 180.0 / Isis::PI;
45  p_tolerance = .0001;
46 
47  // Compute a default time tolerance to a 1/20 of a pixel
48  double et1 = p_camera->Spice::cacheStartTime().Et();
49  double et2 = p_camera->Spice::cacheEndTime().Et();
50  p_timeTolerance = (et2 - et1) / p_camera->Lines() / 20.0;
51  }
52 
61  bool RadarGroundMap::SetFocalPlane(const double ux, const double uy,
62  double uz) {
63 
64  SpiceRotation *bodyFrame = p_camera->bodyRotation();
65  SpicePosition *spaceCraft = p_camera->instrumentPosition();
66 
67  // Get spacecraft position and velocity to create a state vector
68  std::vector<double> Ssc(6);
69  // Load the state into Ssc
70  vequ_c((SpiceDouble *) & (spaceCraft->Coordinate()[0]), &Ssc[0]);
71  vequ_c((SpiceDouble *) & (spaceCraft->Velocity()[0]), &Ssc[3]);
72 
73  // Rotate state vector to body-fixed
74  std::vector<double> bfSsc(6);
75  bfSsc = bodyFrame->ReferenceVector(Ssc);
76 
77  // Extract body-fixed position and velocity
78  std::vector<double> Vsc(3);
79  std::vector<double> Xsc(3);
80  vequ_c(&bfSsc[0], (SpiceDouble *) & (Xsc[0]));
81  vequ_c(&bfSsc[3], (SpiceDouble *) & (Vsc[0]));
82 
83  // Compute intrack, crosstrack, and radial coordinate
84  SpiceDouble i[3];
85  vhat_c(&Vsc[0], i);
86 
87  SpiceDouble c[3];
88  SpiceDouble dp;
89  dp = vdot_c(&Xsc[0], i);
90  SpiceDouble p[3], q[3];
91  vscl_c(dp, i, p);
92  vsub_c(&Xsc[0], p, q);
93  vhat_c(q, c);
94 
95  SpiceDouble r[3];
96  vcrss_c(i, c, r);
97 
98  // What is the initial guess for R
99  Distance radii[3];
100  p_camera->radii(radii);
101  SpiceDouble R = radii[0].kilometers();
102 
103  SpiceDouble lat = DBL_MAX;
104  SpiceDouble lon = DBL_MAX;
105 
106  double slantRangeSqr = (ux * p_rangeSigma) / 1000.; // convert to meters, then km
107  slantRangeSqr = slantRangeSqr * slantRangeSqr;
108  SpiceDouble X[3];
109 
110  // The iteration code was moved to its own method so that it can be run multiple times
111  // if necessary. The first iteration should suffice for those pixels that have shallow
112  // slopes. For those pixels that lie on steep slopes (up to 2x the incidence angle), then
113  // an additional iteration call is needed. In the future, we may need to add more calls
114  // to the iteration method if the slope is greater than 2x the incidence angle. The
115  // slope variable will need to be halved each time the iteration method is called until
116  // a solution is found. So, for example, if we needed to call the iteration method a third
117  // time, the slope variable would be set to .25.
118  bool useSlopeEqn = false;
119  double slope = .5;
120  bool success = Iterate(R,slantRangeSqr,c,r,X,lat,lon,Xsc,useSlopeEqn,slope);
121 
122  if(!success) {
123  R = radii[0].kilometers();
124  useSlopeEqn = true;
125  success = Iterate(R,slantRangeSqr,c,r,X,lat,lon,Xsc,useSlopeEqn,slope);
126  }
127 
128  if(!success) return false;
129 
130  lat = lat * 180.0 / Isis::PI;
131  lon = lon * 180.0 / Isis::PI;
132  while(lon < 0.0) lon += 360.0;
133 
134  // Compute body fixed look direction
135  std::vector<double> lookB;
136  lookB.resize(3);
137  lookB[0] = X[0] - Xsc[0];
138  lookB[1] = X[1] - Xsc[1];
139  lookB[2] = X[2] - Xsc[2];
140 
141  std::vector<double> lookJ = bodyFrame->J2000Vector(lookB);
142  SpiceRotation *cameraFrame = p_camera->instrumentRotation();
143  std::vector<double> lookC = cameraFrame->ReferenceVector(lookJ);
144 
145  SpiceDouble unitLookC[3];
146  vhat_c(&lookC[0], unitLookC);
147 
148  return p_camera->Sensor::SetUniversalGround(lat, lon);
149  }
150 
159  bool RadarGroundMap::Iterate(SpiceDouble &R, const double &slantRangeSqr, const SpiceDouble c[],
160  const SpiceDouble r[], SpiceDouble X[], SpiceDouble &lat,
161  SpiceDouble &lon, const std::vector<double> &Xsc,
162  const bool &useSlopeEqn, const double &slope) {
163 
164  lat = DBL_MAX;
165  lon = DBL_MAX;
166  SpiceDouble lastR = DBL_MAX;
167  SpiceDouble rlat;
168  SpiceDouble rlon;
169  int iter = 0;
170  do {
171  double normXsc = vnorm_c(&Xsc[0]);
172  double alpha = (R * R - slantRangeSqr - normXsc * normXsc) /
173  (2.0 * vdot_c(&Xsc[0], c));
174 
175  double arg = slantRangeSqr - alpha * alpha;
176  if(arg < 0.0) return false;
177 
178  double beta = sqrt(arg);
179  if(p_lookDirection == Radar::Left) beta *= -1.0;
180 
181  SpiceDouble alphac[3], betar[3];
182  vscl_c(alpha, c, alphac);
183  vscl_c(beta, r, betar);
184 
185  vadd_c(alphac, betar, alphac);
186  vadd_c(&Xsc[0], alphac, X);
187 
188  // Convert X to lat,lon
189  lastR = R;
190  reclat_c(X, &R, &lon, &lat);
191 
192  rlat = lat * 180.0 / Isis::PI;
193  rlon = lon * 180.0 / Isis::PI;
194  if(useSlopeEqn) {
195  R = lastR + slope * (p_camera->LocalRadius(rlat, rlon).kilometers() - lastR);
196  } else {
197  R = p_camera->LocalRadius(rlat, rlon).kilometers();
198  }
199 
200  iter++;
201  }
202  while(fabs(R - lastR) > p_tolerance && iter < 100);
203 
204  if(fabs(R - lastR) > p_tolerance) return false;
205 
206  return true;
207  }
208 
216  bool RadarGroundMap::SetGround(const Latitude &lat, const Longitude &lon) {
217  Distance localRadius(p_camera->LocalRadius(lat, lon));
218 
219  if(!localRadius.isValid()) {
220  return false;
221  }
222 
223  return SetGround(SurfacePoint(lat, lon, p_camera->LocalRadius(lat, lon)));
224  }
225 
234  bool RadarGroundMap::SetGround(const SurfacePoint &surfacePoint) {
235  // Get the ground point in rectangular coordinates (X)
236  if(!surfacePoint.Valid()) return false;
237 
238  SpiceDouble X[3];
239  surfacePoint.ToNaifArray(X);
240 
241  // Compute lower bound for Doppler shift
242  double et1 = p_camera->Spice::cacheStartTime().Et();
243  p_camera->Sensor::setTime(et1);
244  double xv1 = ComputeXv(X);
245 
246  // Compute upper bound for Doppler shift
247  double et2 = p_camera->Spice::cacheEndTime().Et();
248  p_camera->Sensor::setTime(et2);
249  double xv2 = ComputeXv(X);
250 
251  // Make sure we bound root (xv = 0.0)
252  if((xv1 < 0.0) && (xv2 < 0.0)) return false;
253  if((xv1 > 0.0) && (xv2 > 0.0)) return false;
254 
255  // Order the bounds
256  double fl, fh, xl, xh;
257  if(xv1 < xv2) {
258  fl = xv1;
259  fh = xv2;
260  xl = et1;
261  xh = et2;
262  }
263  else {
264  fl = xv2;
265  fh = xv1;
266  xl = et2;
267  xh = et1;
268  }
269 
270  // Iterate a max of 30 times
271  for(int j = 0; j < 30; j++) {
272  // Use the secant method to guess the next et
273  double etGuess = xl + (xh - xl) * fl / (fl - fh);
274 
275  // Compute the guessed Doppler shift. Hopefully
276  // this guess converges to zero at some point
277  p_camera->Sensor::setTime(etGuess);
278  double fGuess = ComputeXv(X);
279 
280  // Update the bounds
281  double delTime;
282  if(fGuess < 0.0) {
283  delTime = xl - etGuess;
284  xl = etGuess;
285  fl = fGuess;
286  }
287  else {
288  delTime = xh - etGuess;
289  xh = etGuess;
290  fh = fGuess;
291  }
292 
293  // See if we are done
294  if((fabs(delTime) <= p_timeTolerance) || (fGuess == 0.0)) {
295  SpiceRotation *bodyFrame = p_camera->bodyRotation();
296  SpicePosition *spaceCraft = p_camera->instrumentPosition();
297 
298  // Get body fixed spacecraft velocity and position
299  std::vector<double> Ssc(6);
300 
301  // Load the state into Ssc and rotate to body-fixed
302  vequ_c((SpiceDouble *) & (spaceCraft->Coordinate()[0]), &Ssc[0]);
303  vequ_c((SpiceDouble *) & (spaceCraft->Velocity()[0]), &Ssc[3]);
304  std::vector<double> bfSsc(6);
305  bfSsc = bodyFrame->ReferenceVector(Ssc);
306 
307  // Extract the body-fixed position and velocity from the state
308  std::vector<double> Vsc(3);
309  std::vector<double> Xsc(3);
310  vequ_c(&bfSsc[0], (SpiceDouble *) & (Xsc[0]));
311  vequ_c(&bfSsc[3], (SpiceDouble *) & (Vsc[0]));
312 
313  // Determine if focal plane coordinate falls on the correct side of the
314  // spacecraft. Radar has both left and right look directions. Make sure
315  // the coordinate is on the same side as the look direction. This is done
316  // by (X - S) . (V x S) where X=ground point vector, S=spacecraft position
317  // vector, and V=velocity vector. If the dot product is greater than 0, then
318  // the point is on the right side. If the dot product is less than 0, then
319  // the point is on the left side. If the dot product is 0, then the point is
320  // directly under the spacecraft (neither left or right) and is invalid.
321  SpiceDouble vout1[3];
322  SpiceDouble vout2[3];
323  SpiceDouble dp;
324  vsub_c(X, &Xsc[0], vout1);
325  vcrss_c(&Vsc[0], &Xsc[0], vout2);
326  dp = vdot_c(vout1, vout2);
327  if(dp > 0.0 && p_lookDirection == Radar::Left) return false;
328  if(dp < 0.0 && p_lookDirection == Radar::Right) return false;
329  if(dp == 0.0) return false;
330 
331 
332 
333  // Compute body fixed look direction
334  std::vector<double> lookB;
335  lookB.resize(3);
336  lookB[0] = X[0] - Xsc[0];
337  lookB[1] = X[1] - Xsc[1];
338  lookB[2] = X[2] - Xsc[2];
339 
340  std::vector<double> lookJ = bodyFrame->J2000Vector(lookB);
341  SpiceRotation *cameraFrame = p_camera->instrumentRotation(); //this is the pointer to the camera's SpiceRotation/instrumentatrotation object
342  std::vector<double> lookC = cameraFrame->ReferenceVector(lookJ);
343 
344  SpiceDouble unitLookC[3];
345  vhat_c(&lookC[0], unitLookC);
346 
347  p_camera->SetFocalLength(p_slantRange * 1000.0); // p_slantRange is km so focal length is in m
348  p_focalPlaneX = p_slantRange * 1000.0 / p_rangeSigma; // km to meters and scaled to focal plane
349  p_focalPlaneY = 0.0;
350  p_camera->target()->shape()->setSurfacePoint(surfacePoint); // Added 2-11-2013 by DAC
351 
352  // set the sensor's ground point and also makes it possible to calculate m_ra & m_dec
353 
354  return p_camera->Sensor::SetGround(surfacePoint, true);
355  }
356  }
357 
358  return false;
359  }
360 
361 
373  bool RadarGroundMap::GetXY(const SurfacePoint &spoint, double *cudx,
374  double *cudy) {
375 
376  // Get the ground point in rectangular body-fixed coordinates (X)
377  double X[3];
378  X[0] = spoint.GetX().kilometers();
379  X[1] = spoint.GetY().kilometers();
380  X[2] = spoint.GetZ().kilometers();
381 
382  // Compute body-fixed look vector
383  SpiceRotation *bodyFrame = p_camera->bodyRotation();
384  SpicePosition *spaceCraft = p_camera->instrumentPosition();
385 
386  std::vector<double> sJ(6); // Spacecraft state vector (position and velocity) in J2000 frame
387  // Load the state into sJ
388  vequ_c((SpiceDouble *) & (spaceCraft->Coordinate()[0]), &sJ[0]);
389  vequ_c((SpiceDouble *) & (spaceCraft->Velocity()[0]), &sJ[3]);
390 
391  // Rotate the state to body-fixed
392  p_sB.resize(6);
393  p_sB = bodyFrame->ReferenceVector(sJ);
394 
395  // Extract the body-fixed position and velocity
396  SpiceDouble VsB[3];
397  SpiceDouble PsB[3];
398  vequ_c(&p_sB[0], PsB);
399  vequ_c(&p_sB[3], VsB);
400 
401  p_lookB.resize(3);
402  vsub_c(X, PsB, &p_lookB[0]);
403 
404  p_groundSlantRange = vnorm_c(&p_lookB[0]); // km
405  p_groundDopplerFreq = 2. / p_waveLength / p_groundSlantRange * vdot_c(&p_lookB[0], &VsB[0]);
406  *cudx = p_groundSlantRange * 1000.0 / p_rangeSigma; // to meters, then to focal plane coord
407  *cudy = p_groundDopplerFreq / p_dopplerSigma; // htx to focal plane coord
408 
409  return true;
410  }
411 
412 
413  double RadarGroundMap::ComputeXv(SpiceDouble X[3]) {
414  // Get the spacecraft position (Xsc) and velocity (Vsc) in body fixed
415  // coordinates
416  SpiceRotation *bodyFrame = p_camera->bodyRotation();
417  SpicePosition *spaceCraft = p_camera->instrumentPosition();
418 
419  // Load the state into Ssc
420  std::vector<double> Ssc(6);
421  vequ_c((SpiceDouble *) & (spaceCraft->Coordinate()[0]), &Ssc[0]);
422  vequ_c((SpiceDouble *) & (spaceCraft->Velocity()[0]), &Ssc[3]);
423 
424  // Rotate the state to body-fixed
425  std::vector<double> bfSsc(6);
426  bfSsc = bodyFrame->ReferenceVector(Ssc);
427 
428  // Extract the body-fixed position and velocity
429  std::vector<double> Vsc(3);
430  std::vector<double> Xsc(3);
431  vequ_c(&bfSsc[0], &Xsc[0]);
432  vequ_c(&bfSsc[3], &Vsc[0]);
433 
434  // Compute the slant range
435  SpiceDouble lookB[3];
436  vsub_c(&Xsc[0], X, lookB);
437  p_slantRange = vnorm_c(lookB); // units are km
438 
439  // Compute and return xv = -2 * (point - observer) dot (point velocity - observer velocity) / (slantRange*wavelength)
440  // In body-fixed coordinates, the point velocity = 0. so the equation becomes
441  // double xv = 2.0 * vdot_c(lookB,&Vsc[0]) / (vnorm_c(lookB) * WaveLength() );
442  double xv = -2.0 * vdot_c(lookB, &Vsc[0]) / (vnorm_c(lookB) * WaveLength()); // - is applied to lookB above
443  return xv;
444  }
445 
446 
460  // d_slantRange = (lookB dot d_lookB) / slantRange
461  // d_dopplerFrequency = -dopplerFrequency/slantRange*d_slantRange -
462  // 2./wavelength/slantRange*(d_lookB dot vlookB) -
463  // 2./wavelength/slantRange*(lookB dot d_vlookB)
464 
465  // Add the partial for the x coordinate of the position (differentiating
466  // point(x,y,z) - spacecraftPosition(x,y,z) in body-fixed and the velocity
467  // Load the derivative of the state into d_lookJ
468  bool RadarGroundMap::GetdXYdPosition(const SpicePosition::PartialType varType, int coefIndex,
469  double *dx, double *dy) {
470  SpicePosition *instPos = p_camera->instrumentPosition();
471  SpiceRotation *bodyRot = p_camera->bodyRotation();
472 
473  std::vector <double> d_lookJ(6);
474  vequ_c(&(instPos->CoordinatePartial(varType, coefIndex))[0], &d_lookJ[0]);
475  vequ_c(&(instPos->VelocityPartial(varType, coefIndex))[0], &d_lookJ[3]);
476 
477  std::vector<double> d_lookB = bodyRot->ReferenceVector(d_lookJ);
478 
479  double d_slantRange = (-1.) * vdot_c(&p_lookB[0], &d_lookB[0]) / p_groundSlantRange;
480  double d_dopplerFreq = (-1.) * p_groundDopplerFreq * d_slantRange / p_groundSlantRange -
481  2. / p_waveLength / p_groundSlantRange * vdot_c(&d_lookB[0], &p_sB[3]) +
482  2. / p_waveLength / p_groundSlantRange * vdot_c(&p_lookB[0], &d_lookB[3]);
483 
484  *dx = d_slantRange * 1000.0 / p_rangeSigma;// km to meters, then to focal plane coord
485  *dy = d_dopplerFreq / p_dopplerSigma; // htz scaled to focal plane
486 
487  return true;
488  }
489 
503  bool RadarGroundMap::GetdXYdPoint(std::vector<double> d_lookB,
504  double *dx, double *dy) {
505 
506  // TODO add a check to make sure p_lookB has been set
507 
508  double d_slantRange = vdot_c(&p_lookB[0], &d_lookB[0]) / p_groundSlantRange; // km
509  // After switching to J2000, the last term will not be 0. as it is in body-fixed
510 // double d_dopplerFreq = p_groundDopplerFreq*d_slantRange/p_groundSlantRange // Ken
511 // + 2./p_waveLength/p_groundSlantRange*vdot_c(&d_lookB[0], &p_sB[3]);
512  double d_dopplerFreq = (-1.) * p_groundDopplerFreq * d_slantRange / p_groundSlantRange
513  + 2. / p_waveLength / p_groundSlantRange * vdot_c(&d_lookB[0], &p_sB[3]);
514 // + 2./p_wavelength/slantRange*vdot_c(&p_lookB[0], 0);
515 
516  *dx = d_slantRange * 1000.0 / p_rangeSigma;
517  *dy = d_dopplerFreq / p_dopplerSigma;
518 
519  return true;
520  }
521 
522 }
This class defines a body-fixed surface point.
Definition: SurfacePoint.h:86
virtual bool SetFocalPlane(const double ux, const double uy, const double uz)
Compute ground position from slant range.
double p_focalPlaneY
Camera&#39;s y focal plane coordinate.
double p_focalPlaneX
Camera&#39;s x focal plane coordinate.
double p_groundDopplerFreq
units are hertz
SpiceRotation * instrumentRotation() const
Accessor method for the instrument rotation.
Definition: Spice.cpp:1476
void radii(Distance r[3]) const
Returns the radii of the body in km.
Definition: Spice.cpp:850
const double PI(3.14159265358979323846)
The mathematical constant PI.
void instrumentPosition(double p[3]) const
Returns the spacecraft position in body-fixed frame km units.
Definition: Spice.cpp:742
double p_slantRange
units are km
This class is designed to encapsulate the concept of a Latitude.
Definition: Latitude.h:59
virtual bool SetGround(const Latitude &lat, const Longitude &lon)
Compute undistorted focal plane coordinate from ground position.
double p_rangeSigma
Scaling factor to convert meters to focal plane coord.
std::vector< double > CoordinatePartial(SpicePosition::PartialType partialVar, int coeffIndex)
Set the coefficients of a polynomial fit to each of the three coordinates of the position vector for ...
void ToNaifArray(double naifOutput[3]) const
A naif array is a c-style array of size 3.
Distance measurement, usually in meters.
Definition: Distance.h:47
This class is designed to encapsulate the concept of a Longitude.
Definition: Longitude.h:52
Target * target() const
Returns a pointer to the target object.
Definition: Spice.cpp:1277
void SetFocalLength(double v)
Sets the focal length.
Definition: Camera.cpp:3064
std::vector< double > ReferenceVector(const std::vector< double > &jVec)
Given a direction vector in J2000, return a reference frame direction.
SpiceRotation * bodyRotation() const
Accessor method for the body rotation.
Definition: Spice.cpp:1465
double p_dopplerSigma
Scaling factor to convert hertz to focal plane coord.
double kilometers() const
Get the displacement in kilometers.
const std::vector< double > & Coordinate()
Return the current J2000 position.
bool Iterate(SpiceDouble &R, const double &slantRangeSqr, const SpiceDouble c[], const SpiceDouble r[], SpiceDouble X[], SpiceDouble &lat, SpiceDouble &lon, const std::vector< double > &Xsc, const bool &useSlopeEqn, const double &slope)
Iteration loop for computing ground position from slant range.
Obtain SPICE rotation information for a body.
Unless noted otherwise, the portions of Isis written by the USGS are public domain.
Distance LocalRadius() const
Returns the local radius at the intersection point.
Definition: Sensor.cpp:282
double WaveLength()
Return the wavelength.
std::vector< double > J2000Vector(const std::vector< double > &rVec)
Given a direction vector in the reference frame, return a J2000 direction.
std::vector< double > VelocityPartial(SpicePosition::PartialType partialVar, int coeffIndex)
Compute the derivative of the velocity with respect to the specified variable.
void setSurfacePoint(const SurfacePoint &surfacePoint)
Set surface intersection point.
Definition: ShapeModel.cpp:476
double p_groundSlantRange
units are km
virtual bool GetdXYdPoint(std::vector< double > d_lookB, double *dx, double *dy)
Compute derivative of focal plane coordinate w/r to ground point from ground position using current S...
Obtain SPICE position information for a body.
const std::vector< double > & Velocity()
Return the current J2000 velocity.
virtual bool GetdXYdPosition(const SpicePosition::PartialType varType, int coefIndex, double *cudx, double *cudy)
Compute derivative w/r to position of focal plane coordinate from ground position using current Spice...
double kilometers() const
Get the distance in kilometers.
Definition: Distance.cpp:118
ShapeModel * shape() const
Return the shape.
Definition: Target.cpp:592
virtual bool GetXY(const SurfacePoint &spoint, double *cudx, double *cudy)
Compute undistorted focal plane coordinate from ground position using current Spice from SetImage cal...
Unless noted otherwise, the portions of Isis written by the USGS are public domain.

U.S. Department of the Interior | U.S. Geological Survey
ISIS | Privacy & Disclaimers | Astrogeology Research Program
To contact us, please post comments and questions on the ISIS Support Center
File Modified: 07/12/2023 23:28:00