LineScanCameraGroundMap.cpp

Go to the documentation of this file.

#include "LineScanCameraGroundMap.h"

#include <iostream>
#include <iomanip>

#include <QTime>
#include <QList>
#include <QFile>
#include <QTextStream>

#include "IException.h"
#include "IString.h"
#include "Camera.h"
#include "CameraDistortionMap.h"
#include "CameraFocalPlaneMap.h"
#include "Distance.h"
#include "LineScanCameraDetectorMap.h"
#include "iTime.h"
#include "Latitude.h"
#include "Longitude.h"
#include "Statistics.h"
#include "SurfacePoint.h"
#include "FunctionTools.h"


using namespace std;
using namespace Isis;

bool ptXLessThan(const QList<double> l1, const QList<double> l2);

class LineOffsetFunctor : 
  public std::unary_function<double, double > {
  public:

    LineOffsetFunctor(Isis::Camera *camera, const Isis::SurfacePoint &surPt) {
      m_camera = camera;
      m_surfacePoint = surPt;
    }


    ~LineOffsetFunctor() {}


    double operator()(double et) {
      double lookC[3] = {0.0, 0.0, 0.0};
      double ux = 0.0;
      double uy = 0.0;
      double dx = 0.0;
      double dy = 0.0;

      // Verify the time is with the cache bounds
      double startTime = m_camera->cacheStartTime().Et();
      double endTime = m_camera->cacheEndTime().Et();
      if (et < startTime || et > endTime) {
        IString msg = "Ephemeris time passed to LineOffsetFunctor is not within the image "
                      "cache bounds";
        throw IException(IException::Programmer, msg, _FILEINFO_);
      }
     
      m_camera->Sensor::setTime(et);
 
      // Set ground
      if (!m_camera->Sensor::SetGround(m_surfacePoint, false)) {
        IString msg = "Sensor::SetGround failed for surface point in LineScanCameraGroundMap.cpp"
                      " LineOffsetFunctor";
        throw IException(IException::Programmer, msg, _FILEINFO_);
      }
   
      // Calculate the undistorted focal plane coordinates
      m_camera->Sensor::LookDirection(lookC);
      ux = m_camera->FocalLength() * lookC[0] / lookC[2];
      uy = m_camera->FocalLength() * lookC[1] / lookC[2];


      // This was replaced with the code below to get Chandrayaan M3 to work.
      // SetUndistortedFocalPlane was failing a majority of the time, causing most SetGround calls
      // to fail. Even when it did succeed, it was producing non-continous return values.
        // Set the undistorted focal plane coordinates
      //  if (!m_camera->DistortionMap()->SetUndistortedFocalPlane(ux, uy)) {
      //    IString msg = "DistortionMap::SetUndistoredFocalPlane failed for surface point in "
      //                  "LineScanCameraGroundMap.cpp LineOffsetFunctor";
      //    throw IException(IException::Programmer, msg, _FILEINFO_);
      //  }

        // Get the natural (distorted focal plane coordinates)
      //  dx = m_camera->DistortionMap()->FocalPlaneX();
      //  dy = m_camera->DistortionMap()->FocalPlaneY();
      //  std::cout << "use dist" << std::endl;
      //}


      // Try to use SetUndistortedFocalPlane, if that does not work use the distorted x,y
      // under the assumption (bad|good) that extrapolating the distortion
      // is causing the distorted x,y to be way off the sensor, and thus not very good anyway.
      if (m_camera->DistortionMap()->SetUndistortedFocalPlane(ux, uy)) {
        // Get the natural (distorted focal plane coordinates)
        dx = m_camera->DistortionMap()->FocalPlaneX();
        dy = m_camera->DistortionMap()->FocalPlaneY();
      }
      else {
        dx = ux;
        dy = uy;
      }

      if (!m_camera->FocalPlaneMap()->SetFocalPlane(dx, dy)) {
        IString msg = "FocalPlaneMap::SetFocalPlane failed for surface point in "
                      "LineScanCameraGroundMap.cpp LineOffsetFunctor";
        throw IException(IException::Programmer, msg, _FILEINFO_);
      }     

      // Return the offset
      return (m_camera->FocalPlaneMap()->DetectorLineOffset() - 
              m_camera->FocalPlaneMap()->DetectorLine());
    }


  private:
    SurfacePoint m_surfacePoint;
    Camera* m_camera;
};


class SensorSurfacePointDistanceFunctor : 
  public std::unary_function<double, double > {

  public:
    SensorSurfacePointDistanceFunctor(Isis::Camera *camera, const Isis::SurfacePoint &surPt) {
      m_camera = camera;
      surfacePoint = surPt;
    }


    ~SensorSurfacePointDistanceFunctor() {}


    double operator()(double et) {
      double s[3], p[3];

      //verify the time is with the cache bounds
      double startTime = m_camera->cacheStartTime().Et();
      double endTime = m_camera->cacheEndTime().Et();

      if (et < startTime || et > endTime) {
        IString msg = "Ephemeris time passed to SensorSurfacePointDistanceFunctor is not within the image "
                      "cache bounds";
        throw IException(IException::Programmer, msg, _FILEINFO_);
      }
      m_camera->Sensor::setTime(et);
      if (!m_camera->Sensor::SetGround(surfacePoint, false)) {
         IString msg = "Sensor::SetGround failed for surface point in LineScanCameraGroundMap.cpp"
                       "SensorSurfacePointDistanceFunctor";
      }
      m_camera->instrumentPosition(s);
      m_camera->Coordinate(p);
      return sqrt((s[0] - p[0]) * (s[0] - p[0]) +
                  (s[1] - p[1]) * (s[1] - p[1]) +
                  (s[2] - p[2]) * (s[2] - p[2]) );  //distance
    }

  private:
    SurfacePoint surfacePoint;
    Camera* m_camera;
};


namespace Isis {

  LineScanCameraGroundMap::LineScanCameraGroundMap(Camera *cam) : CameraGroundMap(cam) {}


  LineScanCameraGroundMap::~LineScanCameraGroundMap() {}

  bool LineScanCameraGroundMap::SetGround(const Latitude &lat,
      const Longitude &lon) {
    Distance radius(p_camera->LocalRadius(lat, lon));

    if (radius.isValid()) {
      return SetGround(SurfacePoint(lat, lon, radius));
    }
    else {
      return false;
    }
  }


  bool LineScanCameraGroundMap::SetGround(const SurfacePoint &surfacePoint, const int &approxLine) {
    FindFocalPlaneStatus status = FindFocalPlane(approxLine, surfacePoint);
    if (status == Success) return true;
    //if(status == Failure) return false;
    return false;
  }
  

  bool LineScanCameraGroundMap::SetGround(const SurfacePoint &surfacePoint) {
    FindFocalPlaneStatus status = FindFocalPlane(-1, surfacePoint);

    if (status == Success) return true;

    return false;
  }


  double LineScanCameraGroundMap::FindSpacecraftDistance(int line,
      const SurfacePoint &surfacePoint) {

    CameraDetectorMap *detectorMap = p_camera->DetectorMap();
    detectorMap->SetParent(p_camera->ParentSamples() / 2, line);
    if (!p_camera->Sensor::SetGround(surfacePoint, false)) {
      return DBL_MAX;
    }

    return p_camera->SlantDistance();
  }


  LineScanCameraGroundMap::FindFocalPlaneStatus
      LineScanCameraGroundMap::FindFocalPlane(const int &approxLine,
                                              const SurfacePoint &surfacePoint) {

    //CameraDistortionMap *distortionMap = p_camera->DistortionMap();
    //CameraFocalPlaneMap *focalMap = p_camera->FocalPlaneMap();

    double approxTime = 0;
    double approxOffset = 0;
    double lookC[3] = {0.0, 0.0, 0.0};
    double ux = 0.0;
    double uy = 0.0;
    //double dx = 0.0, dy = 0.0;
    //double s[3], p[3];
    const double cacheStart = p_camera->Spice::cacheStartTime().Et();
    const double cacheEnd = p_camera->Spice::cacheEndTime().Et();

    double lineRate = ((LineScanCameraDetectorMap *)p_camera->DetectorMap())->LineRate(); //line rate

    if (lineRate == 0.0) return Failure;

    LineOffsetFunctor offsetFunc(p_camera,surfacePoint);
    SensorSurfacePointDistanceFunctor distanceFunc(p_camera,surfacePoint);

    // METHOD #1
    // Use the line given as a start point for the secant method root search. 
    if (approxLine >= 0.5) {

      // convert the approxLine to an approximate time and offset
      p_camera->DetectorMap()->SetParent(p_camera->ParentSamples() / 2.0, approxLine);
      approxTime = p_camera->time().Et();
  
      approxOffset = offsetFunc(approxTime);

      // Check to see if there is no need to improve this root, it's good enough
      if (fabs(approxOffset) < 1e-2) { 
        p_camera->Sensor::setTime(approxTime);
        // check to make sure the point isn't behind the planet
        if (!p_camera->Sensor::SetGround(surfacePoint, true)) {
          return Failure;
        } 
        p_camera->Sensor::LookDirection(lookC);
        ux = p_camera->FocalLength() * lookC[0] / lookC[2];
        uy = p_camera->FocalLength() * lookC[1] / lookC[2];
     
        p_focalPlaneX = ux;
        p_focalPlaneY = uy;

        return Success;
      }

      double fl, fh, xl, xh;

      // starting times for the secant method, kept within the domain of the cache
      xh = approxTime;
      if (xh + lineRate < cacheEnd) {
        xl = xh + lineRate;
      }
      else {
        xl = xh - lineRate;
      }

      // starting offsets
      fh = approxOffset;  //the first is already calculated
      fl = offsetFunc(xl);

      // Iterate to refine the given approximate time that the instrument imaged the ground point
      for (int j=0; j < 10; j++) {
        if (fl-fh == 0.0) {
          return Failure;
        }
        double etGuess = xl + (xh - xl) * fl / (fl - fh);

        if (etGuess < cacheStart) etGuess = cacheStart;
        if (etGuess > cacheEnd) etGuess = cacheEnd;

        double f = offsetFunc(etGuess);


        // elliminate the node farthest away from the current best guess
        if (fabs( xl- etGuess) > fabs( xh - etGuess)) {  
          xl = etGuess;
          fl = f;
        }
        else {
          xh = etGuess;
          fh = f;
        }

        // See if we converged on the point so set up the undistorted focal plane values and return
        if (fabs(f) < 1e-2) {
          p_camera->Sensor::setTime(approxTime);
          // check to make sure the point isn't behind the planet
          if (!p_camera->Sensor::SetGround(surfacePoint, true)) {
            return Failure;
          } 
          p_camera->Sensor::LookDirection(lookC);
          ux = p_camera->FocalLength() * lookC[0] / lookC[2];
          uy = p_camera->FocalLength() * lookC[1] / lookC[2];
      
          p_focalPlaneX = ux;
          p_focalPlaneY = uy;

          return Success;       
        }
      } // End itteration using a guess
      // return Failure; // Removed to let the lagrange method try to find the line if secant fails
    } // End use a guess


    // METHOD #2
    // The guess or middle line did not work so try estimating with a quadratic
    // The offsets are typically quadratic, so three points will be used to approximate a quadratic
    // as a first order attempt to find the root location(s)

    // The three nodes to be used to approximate the quadratic
    double offsetNodes[3];
    double timeNodes[3];
    double timeAverage;
    double scale;
    QList<double> root;
    QList<double> offset;
    QList<double> dist;

    timeNodes[0] = cacheStart;
    timeNodes[2] = cacheEnd;
    timeNodes[1] = (cacheStart+cacheEnd) / 2.0; // middle time

    double quadPoly[3];
    double temp;

    for (int i=0; i<3; i++) {
      offsetNodes[i] = offsetFunc(timeNodes[i]);
    }
   
    // centralize and normalize the data for stability in root finding
    timeAverage = (timeNodes[0] + timeNodes[1] + timeNodes[2]) / 3.0;
    timeNodes[0] -= timeAverage;
    timeNodes[1] -= timeAverage;
    timeNodes[2] -= timeAverage;

    scale = 1.0 / sqrt((timeNodes[0] - timeNodes[2]) * 
                       (timeNodes[0] - timeNodes[2]) + 
                       (offsetNodes[0] - offsetNodes[2]) * 
                       (offsetNodes[0] - offsetNodes[2]));

    timeNodes[0] *= scale;
    timeNodes[1] *= scale;
    timeNodes[2] *= scale;

    offsetNodes[0] *= scale;
    offsetNodes[1] *= scale;
    offsetNodes[2] *= scale;

    // Use lagrange interpolating polynomials to find the coefficients of the quadratic, 
    // there are many ways to do this; I chose to do it this way because it is pretty straight 
    // forward and cheap
    quadPoly[0] = quadPoly[1] = quadPoly[2] = 0.0;

    temp = offsetNodes[0] / ((timeNodes[0] - timeNodes[1]) * (timeNodes[0] - timeNodes[2]));
    quadPoly[0] += temp;
    quadPoly[1] += temp * (-timeNodes[1] - timeNodes[2]);
    quadPoly[2] += temp * timeNodes[1] * timeNodes[2];

    temp = offsetNodes[1] / ((timeNodes[1] - timeNodes[0]) * (timeNodes[1] - timeNodes[2]));
    quadPoly[0] += temp;
    quadPoly[1] += temp * (-timeNodes[0] - timeNodes[2]);
    quadPoly[2] += temp * timeNodes[0] * timeNodes[2];
      
    temp = offsetNodes[2] / ((timeNodes[2] - timeNodes[0]) * (timeNodes[2] - timeNodes[1]));
    quadPoly[0] += temp;
    quadPoly[1] += temp * (-timeNodes[0] - timeNodes[1]);
    quadPoly[2] += temp * timeNodes[0] * timeNodes[1];

    // Now that we have the coefficients of the quadratic look for roots 
    // (see Numerical Recipes Third Edition page 227)
    temp = quadPoly[1] * quadPoly[1] - 4.0 * quadPoly[0] * quadPoly[2];  //discriminant

    // THIS IS A PREMATURE FAILURE RETURN. IT SHOULD TRY THE NEXT METHON BEFORE FAILING
    if (temp < 0.0) {
      return Failure;  // there are apparently not any real roots on this image
    }

    if (quadPoly[1] >= 0.0) {
      temp = -0.5 * (quadPoly[1] + sqrt(temp));
    }
    else {
      temp = -0.5 * (quadPoly[1] - sqrt(temp));
    }

    if (quadPoly[0] != 0.0) {
      root.push_back(temp/quadPoly[0]);
    }

    if (quadPoly[2] != 0.0) {
      root.push_back(quadPoly[2]/temp);
    }

    // check to see if the roots are in the time interval of the cache
    for (int i=root.size()-1; i>=0; i--) {
      if ( root[i] < timeNodes[0] || root[i] > timeNodes[2] ) {
        root.removeAt(i);
      }
    }
     
    // return the calculated roots to the original system
    for (int i=0; i<root.size(); i++) {
      root[i] = root[i]/scale + timeAverage;
    }

    // THIS IS A PREMATURE FAILURE RETURN. IT SHOULD TRY THE NEXT METHON BEFORE FAILING
    if (root.size() == 0) {
      return Failure;  // there are apparently not any roots on this image
    }

    // At the time of this writing ISIS made no attempt to support any sensors that were not "1 to 1".
    // Meaning they imaged the same point on the ground in multiple lines of the image
    // therefore we must somehow reduce multiple possible roots to a single one,  the legacy 
    // code (replaced with this code) did this based on distance from the sensor to the target
    // the shortest distance being the winner.  For legacy consistency I have used the same logic below.  

    for (int i=0; i<root.size(); i++) {  // Offset/dist calculation loop
      dist << distanceFunc(root[i]);
      offset << offsetFunc(root[i]);
    }

    // Save the root with the smallest dist
    {
      int j=0;   
      for (int i=1; i<root.size(); i++) {
        if (dist[i] < dist[j]) j=i;
      }      
        
      approxTime = root[j];  // Now we have our time
      approxOffset = offset[j];  // The offsets are saved to avoid recalculating it later
    }

    if (fabs(approxOffset) < 1.0e-2) { // No need to iteratively improve this root, it's good enough
      p_camera->Sensor::setTime(approxTime);

      // Check to make sure the point isn't behind the planet
      if (!p_camera->Sensor::SetGround(surfacePoint, true)) {
        return Failure;
      }
       
      p_camera->Sensor::LookDirection(lookC);
      ux = p_camera->FocalLength() * lookC[0] / lookC[2];
      uy = p_camera->FocalLength() * lookC[1] / lookC[2];
     
      p_focalPlaneX = ux;
      p_focalPlaneY = uy;

      return Success;
    }


    // METHOD #3
    // Estimated line and quadratic approximation insufficient, try Brent's method
    // The offsets are typically quadratic, so three points will be used to approximate a quadratic
    // as a first order attempt to find the root location(s)

    // The above sections are sufficient for finding the correct times for the vast majority of 
    // back projection solutions.  The following section exists for those few particularly
    // stuborn problems.  They are typically characterised by being significantly non-quadratic.
    // Examples mostly include images with very long exposure times.
    //
    // Further, while the preceeding sections is intended to be fast, this section is intended to be
    // thurough.  Brents method (Numerical Recipies 454 - 456) will be used to find all the roots,
    // that are bracketed by the five points defined in the quadratic solution method above.
    // The root with the shortest distance to the camera will be returned

    // Get everything ordered for iteration combine and sort the five already defined points
    QList <QList <double> > pts;

    for (int i=0; i<3; i++) {
      QList <double> pt;
      pt << timeNodes[i] / scale + timeAverage;
      pt << offsetNodes[i] / scale;
      pts << pt; 
    }

    for (int i=0; i<root.size(); i++) {
      QList <double> pt;
      pt << root[i];
      pt << offset[i];
      pts << pt; 
    }

    qSort(pts.begin(), pts.end(), ptXLessThan);
    
    root.clear();
    for (int i=1; i<pts.size(); i++) {
      // If the signs of the two offsets are not the same they bracket at least one root
      if ( (pts[i-1][1] > 0) - (pts[i-1][1] < 0) != (pts[i][1] > 0) - (pts[i][1] < 0) ) {
        double temp;
        if (FunctionTools::brentsRootFinder <LineOffsetFunctor> (offsetFunc, pts[i-1], pts[i],
                                                                 1.0e-3, 200, temp)) {
          root << temp;
        }
      }      
    }

    // Discard any roots that are looking through the planet
    for (int i = root.size()-1; i>=0; i--) {
      p_camera->Sensor::setTime(root[i]);
      //check to make sure the point isn't behind the planet
      if (!p_camera->Sensor::SetGround(surfacePoint, true)) {
        root.removeAt(i);
      }
    }

    // If none of the roots remain...
    if (root.size() == 0) {
      return Failure;
    }

    // Choose from the remaining roots, the solution with the smallest distance to target
    dist.clear();
    offset.clear();
    for (int i=0; i<root.size(); i++) {  // Offset/dist calculation loop
      dist << distanceFunc(root[i]);
      offset << offsetFunc(root[i]);
    }

    // Save the root with the smallest dist
    {
      int j=0;   
      for (int i=1; i<root.size(); i++) {
        if (dist[i] < dist[j]) j=i;
      }

      p_camera->Sensor::setTime(root[j]);
    }

    // No need to make sure the point isn't behind the planet, it was done above
    p_camera->Sensor::LookDirection(lookC);
    ux = p_camera->FocalLength() * lookC[0] / lookC[2];
    uy = p_camera->FocalLength() * lookC[1] / lookC[2];
     
    p_focalPlaneX = ux;
    p_focalPlaneY = uy;
       
    return Success;
  }
}


bool ptXLessThan(const QList<double> l1, const QList<double> l2) {
  return l1[0] < l2[0];
}