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