CSMCamera.cpp

 
/* SPDX-License-Identifier: CC0-1.0 */
 
#include "CSMCamera.h"
#include "CameraSkyMap.h"
 
#include <fstream>
#include <iostream>
#include <iomanip>
 
#include <QDebug>
#include <QList>
#include <QPointF>
#include <QString>
 
#include "Blob.h"
#include "CameraDetectorMap.h"
#include "CameraDistortionMap.h"
#include "CameraFocalPlaneMap.h"
#include "Constants.h"
#include "Displacement.h"
#include "Distance.h"
#include "FileName.h"
#include "IException.h"
#include "IString.h"
#include "iTime.h"
#include "Latitude.h"
#include "Longitude.h"
#include "LinearAlgebra.h"
#include "NaifStatus.h"
#include "SpecialPixel.h"
#include "SurfacePoint.h"
 
#include "csm/Warning.h"
#include "csm/Error.h"
#include "csm/Plugin.h"
#include "csm/Ellipsoid.h"
#include "csm/SettableEllipsoid.h"
 
using namespace std;
 
namespace Isis {
 
  CSMCamera::CSMCamera(Cube &cube) : Camera(cube) {
    Blob state("CSMState", "String");
    cube.read(state);
    PvlObject &blobLabel = state.Label();
    QString pluginName = blobLabel.findKeyword("PluginName")[0];
    QString modelName = blobLabel.findKeyword("ModelName")[0];
    QString stateString = QString::fromUtf8(state.getBuffer(), state.Size());
    init(cube, pluginName, modelName, stateString);
  }
 
 
  void CSMCamera::init(Cube &cube, QString pluginName, QString modelName, QString stateString){
    const csm::Plugin *plugin = csm::Plugin::findPlugin(pluginName.toStdString());
    if (!plugin) {
      QStringList availablePlugins;
      for (const csm::Plugin *plugin: csm::Plugin::getList()) {
        availablePlugins.append(QString::fromStdString(plugin->getPluginName()));
      }
      QString msg = "Failed to find plugin [" + pluginName + "] for image [" + cube.fileName() +
                    "]. Check that the corresponding CSM plugin library is in the directory "
                    "specified by your IsisPreferences. Loaded plugins [" +
                    availablePlugins.join(", ") + "].";
      throw IException(IException::User, msg, _FILEINFO_);
    }
    if (!plugin->canModelBeConstructedFromState(modelName.toStdString(), stateString.toStdString())) {
      QString msg = "CSM state string attached to image [" + cube.fileName() + "] cannot "
                    "be converted to a [" + modelName + "] using [" + pluginName + "].";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
    m_model = dynamic_cast<csm::RasterGM*>(plugin->constructModelFromState(stateString.toStdString()));
    // If the dynamic cast failed, raise an exception
    if (!m_model) {
      QString msg = "Failed to convert CSM Model to RasterGM.";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    m_instrumentNameLong = QString::fromStdString(m_model->getSensorIdentifier());
    m_instrumentNameShort = QString::fromStdString(m_model->getSensorIdentifier());
    m_spacecraftNameLong = QString::fromStdString(m_model->getPlatformIdentifier());
    m_spacecraftNameShort = QString::fromStdString(m_model->getPlatformIdentifier());
 
    QString timeString = QString::fromStdString(m_model->getReferenceDateAndTime());
    // Strip the UTC time zone indicator for iTime
    timeString.remove("Z");
    m_refTime.setUtc(timeString);
 
    setTarget(*cube.label());
  }
 
 
  bool CSMCamera::SetImage(const double sample, const double line) {
    // Save off the line & sample
    p_childSample = sample;
    p_childLine = line;
 
    csm::ImageCoord imagePt;
    isisToCsmPixel(p_alphaCube->AlphaLine(line), p_alphaCube->AlphaSample(sample), imagePt);
    double achievedPrecision = 0;
    csm::WarningList warnings;
    csm::EcefLocus imageLocus;
    try {
      imageLocus = m_model->imageToRemoteImagingLocus(imagePt,
                                                      0.001,
                                                      &achievedPrecision,
                                                      &warnings);
    }
    catch (csm::Error &e) {
      return false;
    }
 
    // Check for issues on the CSM end
    if (achievedPrecision > 0.001) {
      return false;
    }
    if (!warnings.empty()) {
      for (csm::Warning warning : warnings) {
        if (warning.getWarning() == csm::Warning::IMAGE_COORD_OUT_OF_BOUNDS){
          return false;
        }
      }
    }
 
    // ISIS sensors work in Kilometers, CSM works in meters
    std::vector<double> obsPosition = {imageLocus.point.x / 1000.0,
                                       imageLocus.point.y / 1000.0,
                                       imageLocus.point.z / 1000.0};
    std::vector<double> locusVec = {imageLocus.direction.x,
                                    imageLocus.direction.y,
                                    imageLocus.direction.z};
 
    // Save off the look vector
    m_lookB[0] = locusVec[0];
    m_lookB[1] = locusVec[1];
    m_lookB[2] = locusVec[2];
    m_newLookB = true;
 
    // Check for a ground intersection
    if(!target()->shape()->intersectSurface(obsPosition, locusVec)) {
      return false;
    }
 
    p_pointComputed = true;
    if (!m_et) {
      m_et = new iTime();
    }
    *m_et = m_refTime + m_model->getImageTime(imagePt);
    return true;
  }
 
 
  bool CSMCamera::SetUniversalGround(const double latitude, const double longitude) {
    return SetGround(
        Latitude(latitude, Angle::Degrees),
        Longitude(longitude, Angle::Degrees));
  }
 
 
  bool CSMCamera::SetUniversalGround(const double latitude, const double longitude, double radius) {
    return SetGround(SurfacePoint(
        Latitude(latitude, Angle::Degrees),
        Longitude(longitude, Angle::Degrees),
        Distance(radius, Distance::Meters)));
  }
 
 
  bool CSMCamera::SetGround(Latitude latitude, Longitude longitude) {
    ShapeModel *shape = target()->shape();
    Distance localRadius;
 
    if (shape->name() != "Plane") { // this is the normal behavior
      localRadius = LocalRadius(latitude, longitude);
    }
    else {
      localRadius = Distance(latitude.degrees(),Distance::Kilometers);
      latitude = Latitude(0.,Angle::Degrees);
    }
 
    if (!localRadius.isValid()) {
      target()->shape()->clearSurfacePoint();
      return false;
    }
 
    return SetGround(SurfacePoint(latitude, longitude, localRadius));
  }
 
 
  bool CSMCamera::SetGround(const SurfacePoint & surfacePt) {
    ShapeModel *shape = target()->shape();
    if (!surfacePt.Valid()) {
      shape->clearSurfacePoint();
      return false;
    }
 
    bool validBackProject = true;
 
    // Back project through the CSM model
    csm::ImageCoord imagePt;
    double achievedPrecision = 0;
    csm::WarningList warnings;
    csm::EcefCoord groundPt = isisToCsmGround(surfacePt);
    try {
      imagePt = m_model->groundToImage(groundPt, 0.01, &achievedPrecision, &warnings);
    }
    catch (csm::Error &e) {
      validBackProject = false;
    }
    if (achievedPrecision > 0.01) {
      validBackProject = false;
    }
    if (!warnings.empty()) {
      for (csm::Warning warning : warnings) {
        if (warning.getWarning() == csm::Warning::IMAGE_COORD_OUT_OF_BOUNDS){
          validBackProject = false;
        }
      }
    }
 
    // Check for occlusion
    double line, sample;
    csmToIsisPixel(imagePt, line, sample);
    csm::EcefLocus imageLocus = m_model->imageToRemoteImagingLocus(imagePt);
    std::vector<double> sensorPosition = {imageLocus.point.x, imageLocus.point.y, imageLocus.point.z};
    shape->clearSurfacePoint();
    shape->intersectSurface(surfacePt,
                            sensorPosition,
                            true);
    if (!shape->hasIntersection()) {
      validBackProject = false;
    }
 
    // If the back projection was successful, then save it
    if (validBackProject) {
      m_lookB[0] = imageLocus.direction.x;
      m_lookB[1] = imageLocus.direction.y;
      m_lookB[2] = imageLocus.direction.z;
      m_newLookB = true;
      p_childSample = p_alphaCube->BetaSample(sample);
      p_childLine = p_alphaCube->BetaLine(line);
      p_pointComputed = true;
      shape->setHasIntersection(true);
      if (!m_et) {
        m_et = new iTime();
      }
      *m_et = m_refTime + m_model->getImageTime(imagePt);
      return true;
    }
 
    // Otherwise reset
    shape->clearSurfacePoint();
    return false;
  }
 
 
  double CSMCamera::LineResolution() {
    vector<double> imagePartials = ImagePartials();
    return sqrt(imagePartials[0]*imagePartials[0] +
                imagePartials[2]*imagePartials[2] +
                imagePartials[4]*imagePartials[4]);
  }
 
 
  double CSMCamera::SampleResolution() {
    vector<double> imagePartials = ImagePartials();
    return sqrt(imagePartials[1]*imagePartials[1] +
                imagePartials[3]*imagePartials[3] +
                imagePartials[5]*imagePartials[5]);
  }
 
 
  double CSMCamera::DetectorResolution() {
    // Redo the line and sample resolution calculations because it avoids
    // a call to ImagePartials which could be a costly call
    vector<double> imagePartials = ImagePartials();
    double lineRes =  sqrt(imagePartials[0]*imagePartials[0] +
                           imagePartials[2]*imagePartials[2] +
                           imagePartials[4]*imagePartials[4]);
    double sampRes =  sqrt(imagePartials[1]*imagePartials[1] +
                           imagePartials[3]*imagePartials[3] +
                           imagePartials[5]*imagePartials[5]);
    return (sampRes + lineRes) / 2.0;
  }
 
 
  double CSMCamera::ObliqueLineResolution() {
    // CSM resolution is always the oblique resolution so just return it
    return LineResolution();
  }
 
 
  double CSMCamera::ObliqueSampleResolution() {
    // CSM resolution is always the oblique resolution so just return it
    return SampleResolution();
  }
 
 
  double CSMCamera::ObliqueDetectorResolution() {
    // CSM resolution is always the oblique resolution so just return it
    return DetectorResolution();
  }
 
 
  double CSMCamera::parentLine() const {
    return p_alphaCube->AlphaLine(Line());
  }
 
 
  double CSMCamera::parentSample() const {
    return p_alphaCube->AlphaSample(Sample());
  }
 
 
  void CSMCamera::instrumentBodyFixedPosition(double p[3]) const {
    std::vector<double> position = sensorPositionBodyFixed();
    p[0] = position[0];
    p[1] = position[1];
    p[2] = position[2];
  }
 
 
  std::vector<double> CSMCamera::sensorPositionBodyFixed() const {
    return sensorPositionBodyFixed(parentLine(), parentSample());
  }
 
 
  std::vector<double> CSMCamera::sensorPositionBodyFixed(double line, double sample) const {
    csm::ImageCoord imagePt;
    isisToCsmPixel(line, sample, imagePt);
    csm::EcefCoord sensorPosition =  m_model->getSensorPosition(imagePt);
    // CSM uses meters, but ISIS wants this in Km
    std::vector<double> result {
        sensorPosition.x / 1000.0,
        sensorPosition.y / 1000.0,
        sensorPosition.z / 1000.0};
    return result;
  }
 
 
  void CSMCamera::subSpacecraftPoint(double &lat, double &lon) {
    subSpacecraftPoint(lat, lon, parentLine(), parentSample());
  }
 
 
  void CSMCamera::subSpacecraftPoint(double &lat, double &lon, double line, double sample) {
    // Get s/c position from CSM because it is vector from center of body to that
    vector<double> sensorPosition = sensorPositionBodyFixed(line, sample);
    SurfacePoint surfacePoint(
        Displacement(sensorPosition[0], Displacement::Kilometers),
        Displacement(sensorPosition[1], Displacement::Kilometers),
        Displacement(sensorPosition[2], Displacement::Kilometers));
    lat = surfacePoint.GetLatitude().degrees();
    lon = surfacePoint.GetLongitude().degrees();
  }
 
 
  vector<double> CSMCamera::ImagePartials() {
    return ImagePartials(GetSurfacePoint());
  }
 
 
  vector<double> CSMCamera::ImagePartials(SurfacePoint groundPoint) {
    csm::EcefCoord groundCoord = isisToCsmGround(groundPoint);
    vector<double> groundPartials = m_model->computeGroundPartials(groundCoord);
 
    // Jacobian format is
    // line WRT X  line WRT Y  line WRT Z
    // samp WRT X  samp WRT Y  samp WRT Z
    LinearAlgebra::Matrix groundMatrix(2, 3);
    groundMatrix(0,0) = groundPartials[0];
    groundMatrix(0,1) = groundPartials[1];
    groundMatrix(0,2) = groundPartials[2];
    groundMatrix(1,0) = groundPartials[3];
    groundMatrix(1,1) = groundPartials[4];
    groundMatrix(1,2) = groundPartials[5];
 
    LinearAlgebra::Matrix imageMatrix = LinearAlgebra::pseudoinverse(groundMatrix);
 
    vector<double> imagePartials = {imageMatrix(0,0),
                                    imageMatrix(0,1),
                                    imageMatrix(1,0),
                                    imageMatrix(1,1),
                                    imageMatrix(2,0),
                                    imageMatrix(2,1)};
    return imagePartials;
  }
 
 
  vector<double> CSMCamera::GroundPartials() {
    return GroundPartials(GetSurfacePoint());
  }
 
 
  vector<double> CSMCamera::GroundPartials(SurfacePoint groundPoint) {
    csm::EcefCoord groundCoord = isisToCsmGround(groundPoint);
    vector<double> groundPartials = m_model->computeGroundPartials(groundCoord);
    return groundPartials;
  }
 
 
  void CSMCamera::setTarget(Pvl label) {
    Target *target = new Target(label);
 
    // get radii from CSM
    csm::Ellipsoid targetEllipsoid = csm::SettableEllipsoid::getEllipsoid(m_model);
    std::vector<Distance> radii  = {Distance(targetEllipsoid.getSemiMajorRadius(), Distance::Meters),
                                    Distance(targetEllipsoid.getSemiMajorRadius(), Distance::Meters),
                                    Distance(targetEllipsoid.getSemiMinorRadius(), Distance::Meters)};
    target->setRadii(radii);
 
    // Target needs to be able to access the camera to do things like
    // compute resolution
    target->setSpice(this);
 
    if (m_target) {
      delete m_target;
      m_target = nullptr;
    }
 
    m_target = target;
  }
 
 
  void CSMCamera::isisToCsmPixel(double line, double sample, csm::ImageCoord &csmPixel) const {
    csmPixel.line = line - 0.5;
    csmPixel.samp = sample - 0.5;
  }
 
 
  void CSMCamera::csmToIsisPixel(csm::ImageCoord csmPixel, double &line, double &sample) const {
    line = csmPixel.line + 0.5;
    sample = csmPixel.samp + 0.5;
  }
 
 
  csm::EcefCoord CSMCamera::isisToCsmGround(const SurfacePoint &groundPt) const {
    return csm::EcefCoord(groundPt.GetX().meters(),
                          groundPt.GetY().meters(),
                          groundPt.GetZ().meters());
  }
 
 
  SurfacePoint CSMCamera::csmToIsisGround(const csm::EcefCoord &groundPt) const {
    return SurfacePoint(Displacement(groundPt.x, Displacement::Meters),
                        Displacement(groundPt.y, Displacement::Meters),
                        Displacement(groundPt.z, Displacement::Meters));
  }
 
 
  double CSMCamera::PhaseAngle() const {
    csm::EcefCoord groundPt = isisToCsmGround(GetSurfacePoint());
    csm::EcefVector sunEcefVec = m_model->getIlluminationDirection(groundPt);
    // ISIS wants the position of the sun, not just the vector from the ground
    // point to the sun. So, we approximate this by adding in the ground point.
    // ISIS wants this in Km so convert
    std::vector<double> sunVec = {
        (groundPt.x - sunEcefVec.x) / 1000.0,
        (groundPt.y - sunEcefVec.y) / 1000.0,
        (groundPt.z - sunEcefVec.z) / 1000.0};
    return target()->shape()->phaseAngle(sensorPositionBodyFixed(), sunVec);
  }
 
 
  double CSMCamera::EmissionAngle() const {
    return target()->shape()->emissionAngle(sensorPositionBodyFixed());
  }
 
 
  double CSMCamera::IncidenceAngle() const {
    csm::EcefCoord groundPt = isisToCsmGround(GetSurfacePoint());
    csm::EcefVector sunEcefVec = m_model->getIlluminationDirection(groundPt);
    // ISIS wants the position of the sun, not just the vector from the ground
    // point to the sun. So, we approximate this by adding in the ground point.
    // ISIS wants this in Km so convert
    std::vector<double> sunVec = {
        (groundPt.x - sunEcefVec.x) / 1000.0,
        (groundPt.y - sunEcefVec.y) / 1000.0,
        (groundPt.z - sunEcefVec.z) / 1000.0};
    return target()->shape()->incidenceAngle(sunVec);
  }
 
 
  double CSMCamera::SlantDistance() const {
    std::vector<double> sensorPosition = sensorPositionBodyFixed();
    SurfacePoint groundPoint = GetSurfacePoint();
 
    std::vector<double> sensorToGround = {
        groundPoint.GetX().kilometers() - (sensorPosition[0]),
        groundPoint.GetY().kilometers() - (sensorPosition[1]),
        groundPoint.GetZ().kilometers() - (sensorPosition[2])};
 
    return sqrt(
        sensorToGround[0] * sensorToGround[0] +
        sensorToGround[1] * sensorToGround[1] +
        sensorToGround[2] * sensorToGround[2]);
  }
 
 
  double CSMCamera::targetCenterDistance() const {
    std::vector<double> sensorPosition = sensorPositionBodyFixed();
    return sqrt(
        sensorPosition[0] * sensorPosition[0] +
        sensorPosition[1] * sensorPosition[1] +
        sensorPosition[2] * sensorPosition[2]);
  }
 
 
  std::vector<int> CSMCamera::getParameterIndices(csm::param::Set paramSet) const {
    return m_model->getParameterSetIndices(paramSet);
  }
 
 
  std::vector<int> CSMCamera::getParameterIndices(csm::param::Type paramType) const {
    std::vector<int> parameterIndices;
    for (int i = 0; i < m_model->getNumParameters(); i++) {
      if (m_model->getParameterType(i) == paramType) {
        parameterIndices.push_back(i);
      }
    }
    return parameterIndices;
  }
 
 
  std::vector<int> CSMCamera::getParameterIndices(QStringList paramList) const {
    std::vector<int> parameterIndices;
    QStringList failedParams;
    for (int i = 0; i < paramList.size(); i++) {
      bool found = false;
      for (int j = 0; j < m_model->getNumParameters(); j++) {
        if (QString::compare(QString::fromStdString(m_model->getParameterName(j)).trimmed(),
                             paramList[i].trimmed(),
                             Qt::CaseInsensitive) == 0) {
          parameterIndices.push_back(j);
          found = true;
          break;
        }
      }
 
      if (!found) {
        failedParams.push_back(paramList[i]);
      }
    }
 
    if (!failedParams.empty()) {
      QString msg = "Failed to find indices for the following parameters [" +
                    failedParams.join(",") + "].";
      throw IException(IException::User, msg, _FILEINFO_);
    }
    return parameterIndices;
  }
 
 
  void CSMCamera::applyParameterCorrection(int index, double correction) {
    double currentValue = m_model->getParameterValue(index);
    m_model->setParameterValue(index, currentValue + correction);
  }
 
 
  double CSMCamera::getParameterCovariance(int index1, int index2) {
    return m_model->getParameterCovariance(index1, index2);
  }
 
 
  vector<double> CSMCamera::getSensorPartials(int index, SurfacePoint groundPoint) {
    // csm::SensorPartials holds (line, sample) in order for each parameter
   csm::EcefCoord groundCoord = isisToCsmGround(groundPoint);
   std::pair<double, double> partials = m_model->computeSensorPartials(index, groundCoord);
   vector<double> partialsVector = {partials.first, partials.second};
 
   return partialsVector;
  }
 
 
  QString CSMCamera::getParameterName(int index) {
    return QString::fromStdString(m_model->getParameterName(index));
  }
 
 
  double CSMCamera::getParameterValue(int index) {
    return m_model->getParameterValue(index);
  }
 
 
  QString CSMCamera::getParameterUnits(int index) {
    return QString::fromStdString(m_model->getParameterUnits(index));
  }
 
 
  QString CSMCamera::getModelState() const {
    return QString::fromStdString(m_model->getModelState());
  }
 
 
  void CSMCamera::setTime(const iTime &time) {
    QString msg = "Setting the image time is not supported for CSM camera models";
    throw IException(IException::Programmer, msg, _FILEINFO_);
  }
 
 
  void CSMCamera::subSolarPoint(double &lat, double &lon) {
    QString msg = "Sub solar point is not supported for CSM camera models";
    throw IException(IException::Programmer, msg, _FILEINFO_);
  }
 
 
  QList<QPointF> CSMCamera::PixelIfovOffsets() {
    QString msg = "Pixel Field of View is not supported for CSM camera models";
    throw IException(IException::User, msg, _FILEINFO_);
  }
 
 
  void CSMCamera::sunPosition(double p[3]) const {
    QString msg = "Sun position is not supported for CSM camera models";
    throw IException(IException::Programmer, msg, _FILEINFO_);
  }
 
 
  SpicePosition *CSMCamera::sunPosition() const {
    QString msg = "Sun position is not supported for CSM camera models";
    throw IException(IException::Programmer, msg, _FILEINFO_);
  }
 
 
  SpicePosition *CSMCamera::instrumentPosition() const {
    QString msg = "Instrument position is not supported for CSM camera models";
    throw IException(IException::Programmer, msg, _FILEINFO_);
  }
 
 
  SpiceRotation *CSMCamera::bodyRotation() const {
    QString msg = "Target body orientation is not supported for CSM camera models";
    throw IException(IException::Programmer, msg, _FILEINFO_);
  }
 
 
  SpiceRotation *CSMCamera::instrumentRotation() const {
    QString msg = "Instrument orientation is not supported for CSM camera models";
    throw IException(IException::Programmer, msg, _FILEINFO_);
  }
 
 
  void CSMCamera::computeSolarLongitude(iTime et) {
    QString msg = "Solar longitude is not supported for CSM camera models";
    throw IException(IException::Programmer, msg, _FILEINFO_);
  }
 
 
  double CSMCamera::SolarDistance() const {
    QString msg = "Solar distance is not supported for CSM camera models";
    throw IException(IException::Programmer, msg, _FILEINFO_);
  }
 
 
  double CSMCamera::RightAscension() {
    QString msg = "Right Ascension is not supported for CSM camera models";
    throw IException(IException::Programmer, msg, _FILEINFO_);
  }
 
 
  double CSMCamera::Declination() {
    QString msg = "Declination is not supported for CSM camera models";
    throw IException(IException::Programmer, msg, _FILEINFO_);
  }
}