CameraGroundMap.cpp

 
/* SPDX-License-Identifier: CC0-1.0 */
#include "CameraGroundMap.h"
 
#include <iostream>
 
#include <QDebug>
 
#include <SpiceUsr.h>
 
#include "IException.h"
#include "Latitude.h"
#include "Longitude.h"
#include "NaifStatus.h"
#include "SurfacePoint.h"
#include "Target.h"
 
using namespace std;
 
namespace Isis {
 
  CameraGroundMap::CameraGroundMap(Camera *parent) {
    p_camera = parent;
    p_camera->SetGroundMap(this);
  }
 
 
  bool CameraGroundMap::SetFocalPlane(const double ux, const double uy, const 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();
 
    bool result = p_camera->SetLookDirection(unitLookC);
    return result;
  }
 
 
  bool CameraGroundMap::SetGround(const Latitude &lat, const Longitude &lon) {
    if (p_camera->target()->shape()->name() == "Plane") {
      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 (p_camera->Sensor::SetGround(SurfacePoint(lat, lon, Distance(radius, Distance::Meters)))) {
         LookCtoFocalPlaneXY();
         return true;
      }
    }
    else {
      Distance radius(p_camera->LocalRadius(lat, lon));
      if (radius.isValid()) {
        if (p_camera->Sensor::SetGround(SurfacePoint(lat, lon, radius))) {
          LookCtoFocalPlaneXY();
          return true;
        }
      }
    }
 
    return false;
  }
 
 
  void CameraGroundMap::LookCtoFocalPlaneXY() {
    double lookC[3];
    p_camera->Sensor::LookDirection(lookC);
 
    //Get the fl as the z coordinate to handle instruments looking down the -z axis 2013-02-22.
    double focalLength = p_camera->DistortionMap()->UndistortedFocalPlaneZ();
    double scale = focalLength / lookC[2];
 
    p_focalPlaneX = lookC[0] * scale;
    p_focalPlaneY = lookC[1] * scale;
  }
 
 
  bool CameraGroundMap::SetGround(const SurfacePoint &surfacePoint) {
    if (p_camera->Sensor::SetGround(surfacePoint)) {
      LookCtoFocalPlaneXY();
      return true;
    }
 
    return false;
  }
 
 
  bool CameraGroundMap::GetXY(const SurfacePoint &point, double *cudx, 
                              double *cudy, bool test) {
 
    vector<double> pB(3);
    pB[0] = point.GetX().kilometers();
    pB[1] = point.GetY().kilometers();
    pB[2] = point.GetZ().kilometers();
 
    // Check for Sky images
    if (p_camera->target()->isSky()) {
      return false;
    }
 
    // Should a check be added to make sure SetImage has been called???
 
    // Get spacecraft vector in j2000 coordinates
    SpiceRotation *bodyRot = p_camera->bodyRotation();
    SpiceRotation *instRot = p_camera->instrumentRotation();
    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;
    
    // During iterations in the bundle adjustment do not do the back-of-planet test.
    // Failures are expected to happen during the bundle adjustment due to bad camera
    // pointing or position, poor a priori points, or inaccurate target body information.  For
    // instance, control points near the limb of an image often fail the test.  The hope is
    // that during the bundle adjustment, any variables causing points to fail the test will
    // be corrected.  If not, the point residuals will likely be large on a point that fails the
    // test.  The back-of-planet test is still a valid check for a control net diagnostic
    // program, but not for the bundle adjustment.
    // 
    // TODO It might be useful to have a separate diagnostic program test all points in a 
    //            control net to see if any of the control points fail the back-of-planet test on 
    //            any of the images.
 
    // Check for point on back of planet by checking to see if surface point is viewable 
    //   (test emission angle)
    if (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((SpiceDouble *) &pB[0], upB, &dist);
      double cosangle = vdot_c(upB, upsB);
      double emission;
      if (cosangle > 1) {
        emission = 0;
      }
      else if (cosangle < -1) {
        emission = 180.;
      }
      else {
        emission = acos(cosangle) * 180.0 / Isis::PI;
      }
 
      if (fabs(emission) > 90.) {
        return false;
      }
    }
 
    // Get the look vector in the camera frame and the instrument rotation
    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();
 
    *cudx = lookC[0] * fl / lookC[2];
    *cudy = lookC[1] * fl / lookC[2];
    return true;
  }
 
 
  bool CameraGroundMap::GetXY(const double lat, const double lon,
                              const double radius, double *cudx, double *cudy) {
    SurfacePoint spoint(Latitude(lat, Angle::Degrees),
                        Longitude(lon, Angle::Degrees),
                        Distance(radius, Distance::Meters));
    return GetXY(spoint, cudx, cudy);
  }
 
 
  bool CameraGroundMap::GetdXYdPosition(const SpicePosition::PartialType varType, int coefIndex,
                                        double *dx, double *dy) {
 
    //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();
    vector<double> lookC(3);
    lookC = instRot->ReferenceVector(m_lookJ);
 
    SpicePosition *instPos = p_camera->instrumentPosition();
 
    vector<double> d_lookJ = instPos->CoordinatePartial(varType, coefIndex);
    for (int j = 0; j < 3; j++) d_lookJ[j] *= -1.0;
    vector<double> d_lookC =  instRot->ReferenceVector(d_lookJ);
    *dx = fl * DQuotient(lookC, d_lookC, 0);
    *dy = fl * DQuotient(lookC, d_lookC, 1);
    return true;
  }
 
 
  bool CameraGroundMap::GetdXYdOrientation(const SpiceRotation::PartialType varType, int coefIndex,
                                           double *dx, double *dy) {
 
    //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 J2000 look vector into camera frame
    SpiceRotation *instRot = p_camera->instrumentRotation();
    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);
 
    *dx = fl * DQuotient(lookC, d_lookC, 0);
    *dy = fl * DQuotient(lookC, d_lookC, 1);
    return true;
  }
 
 
  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;
  }
 
 
  bool CameraGroundMap::GetdXYdPoint(vector<double> d_pB, double *dx, double *dy) {
 
    //  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();
    vector<double> lookC(3);
    lookC = instRot->ReferenceVector(m_lookJ);
 
    SpiceRotation *bodyRot = p_camera->bodyRotation();
    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);
    return true;
  }
 
 
  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;
  }
 
 
  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;
  }
 
 
  vector<double> CameraGroundMap::PointPartial(SurfacePoint spoint, PartialType wrt) {
    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);
    double radkm = spoint.GetLocalRadius().kilometers();
 
    vector<double> v(3);
    if (wrt == WRT_Latitude) {
      v[0] = -radkm * sinLat * cosLon;
      v[1] = -radkm * sinLon * sinLat;
      v[2] =  radkm * cosLat;
    }
    else if (wrt == WRT_Longitude) {
      v[0] = -radkm * cosLat * sinLon;
      v[1] =  radkm * cosLat * cosLon;
      v[2] =  0.0;
    }
    else {
      v[0] = cosLon * cosLat;
      v[1] = sinLon * cosLat;
      v[2] = sinLat;
    }
 
    return v;
  }
 
 
  double CameraGroundMap::DQuotient(vector<double> &look,
                                    vector<double> &dlook,
                                    int index) {
    return (look[2] * dlook[index] - look[index] * dlook[2]) /
           (look[2] * look[2]);
  }
}