CameraGroundMap.cpp

Go to the documentation of this file.

#include "CameraGroundMap.h"

#include <QDebug>

#include <iostream>

#include "NaifStatus.h"
#include "SurfacePoint.h"
#include "Latitude.h"
#include "Longitude.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,
                                      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(); //  m
      // 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) {

    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???

    // 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
    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];

    // 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???
    double upsB[3], upB[3], dist;
    vminus_c((SpiceDouble *) &lookB[0], upsB);
    unorm_c(upsB, upsB, &dist);
    unorm_c(pB, upB, &dist);
    double angle = vdot_c(upB, upsB);
    double emission;
    if(angle > 1) {
      emission = 0;
    }
    else if(angle < -1) {
      emission = 180.;
    }
    else {
      emission = acos(angle) * 180.0 / Isis::PI;
    }
    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);

    // 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);
  }


  //  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

    // 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);

    SpicePosition *instPos = p_camera->instrumentPosition();

    std::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);
    *dx = fl * DQuotient(lookC, d_lookC, 0);
    *dy = fl * DQuotient(lookC, d_lookC, 1);
    return true;
  }

  //  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

    // 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);

    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;
  }

  bool CameraGroundMap::GetdXYdPoint(std::vector<double> d_lookB, double *dx, double *dy) {

    //  TODO  add a check to make sure p_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);

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

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


  std::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();

    std::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(std::vector<double> &look,
                                    std::vector<double> &dlook,
                                    int index) {
    return (look[2] * dlook[index] - look[index] * dlook[2]) /
           (look[2] * look[2]);
  }
}