Merge branch 'master' of github.com:USGS-Astrogeology/CSM-CameraModel

      double&       vy,

      double&       vz) const;

   // Computes the imaging locus that would view a ground point at a specific

   // time. Computationally, this is the opposite of losToEcf.

   csm::ImageCoord computeViewingPixel(

      const double& time,   // The time to use the EO at

      const csm::EcefCoord& groundPoint,      // The ground coordinate

      const std::vector<double>& adj // Parameter Adjustments for partials

   ) const;

   // The linear approximation for the sensor model is used as the starting point

   // for iterative rigorous calculations.

   void computeLinearApproximation(

   int          m_PlatformFlag;                   // 7

   int          m_AberrFlag;                      // 8

   int          m_AtmRefFlag;                     // 9

   double       m_IntTime;                        // 10

   std::vector<double> m_IntTimeLines;

   std::vector<double> m_IntTimeStartTimes;

   std::vector<double> m_IntTimes;

   double       m_StartingEphemerisTime;          // 11

   double       m_CenterEphemerisTime;            // 12

   double       m_DetectorSampleSumming;          // 13

       STA_PLATFORM_FLAG,

       STA_ABERR_FLAG,

       STA_ATMREF_FLAG,

       STA_INT_TIME,

       STA_INT_TIME_LINES,

       STA_INT_TIME_START_TIMES,

       STA_INT_TIMES,

       STA_STARTING_EPHEMERIS_TIME,

       STA_CENTER_EPHEMERIS_TIME,

       STA_DETECTOR_SAMPLE_SUMMING,

   "STA_PLATFORM_FLAG",

   "STA_ABERR_FLAG",

   "STA_ATMREF_FLAG",

   "STA_INT_TIME",

   "STA_INT_TIME_LINES",

   "STA_INT_TIME_START_TIMES",

   "STA_INT_TIMES",

   "STA_STARTING_EPHEMERIS_TIME",

   "STA_CENTER_EPHEMERIS_TIME",

   "STA_DETECTOR_SAMPLE_SUMMING",

   state.m_PlatformFlag = atoi(image_support_data.param("PLATFORM").c_str());

   state.m_AberrFlag = atoi(image_support_data.param("ABERR").c_str());

   state.m_AtmRefFlag = atoi(image_support_data.param("ATMREF").c_str());

   state.m_IntTime = atof(image_support_data.param("INT_TIME").c_str());

   state.m_StartingEphemerisTime = atof(image_support_data.param("STARTING_EPHEMERIS_TIME").c_str());

   state.m_CenterEphemerisTime = atof(image_support_data.param("CENTER_EPHEMERIS_TIME").c_str());

   if (image_support_data.param("NUMBER_OF_INT_TIMES").empty()) {

     state.m_IntTimeLines = {0.5};

     state.m_IntTimeStartTimes = {state.m_StartingEphemerisTime - state.m_CenterEphemerisTime};

     state.m_IntTimes = {atof(image_support_data.param("INT_TIME").c_str())};

   }

   else {

     int numIntTimes = atoi(image_support_data.param("NUMBER_OF_INT_TIMES").c_str());

     for (int i = 0; i < numIntTimes; i++) {

       state.m_IntTimeLines.push_back(atof(image_support_data.param("INT_TIME", i*3).c_str()));

       state.m_IntTimeStartTimes.push_back(atof(image_support_data.param("INT_TIME", i*3 + 1).c_str()));

       state.m_IntTimes.push_back(atof(image_support_data.param("INT_TIME", i*3 + 2).c_str()));

     }

   }

   state.m_CenterEphemerisTime = atof(image_support_data.param("CENTER_EPHEMERIS_TIME").c_str());

   state.m_DetectorSampleSumming = atoi(image_support_data.param("DETECTOR_SAMPLE_SUMMING").c_str());

   state.m_StartingSample = atoi(image_support_data.param("STARTING_SAMPLE").c_str());

   state.m_IkCode = atoi(image_support_data.param("IKCODE").c_str());

#include "genericls/UsgsAstroLsSensorModel.h"

#include <algorithm>

#include <iostream>

#include <sstream>

#include <math.h>

   _data.m_HalfSwath = _data.m_Gsd * _data.m_TotalSamples / 2.0;

   // Compute half time duration

   _data.m_HalfTime = _data.m_IntTime * _data.m_TotalLines / 2.0;

   double fullImageTime = _data.m_IntTimeStartTimes.back() - _data.m_IntTimeStartTimes.front()

                          + _data.m_IntTimes.back() * (_data.m_TotalLines - _data.m_IntTimeLines.back());

   _data.m_HalfTime = fullImageTime / 2.0;

   // Parameter covariance, hardcoded accuracy values

   int num_params = _data.NUM_PARAMETERS;

   double*               achieved_precision,

   csm::WarningList*     warnings) const

{

   //# func_description

   //  Computes line and sample given the ground coordinates in ECF cs.

   //  The solution is iterative and repeatedly calls the routine

   //  imageToPlane. If convergence is not achieved, a warning is issued.

   // Initialize variables

   const int MKTR = 10;

   const double DELTA_IMAGE = 1.0;

   double preSquare = desired_precision * desired_precision;

   int mode = -1;

   // Initialize line, sample to center, CSM convention

   csm::ImageCoord ip;

   computeLinearApproximation(ground_pt, ip);

   double lineTemp = ip.line;

   double sampleTemp = ip.samp;

   // Compute ground elevation

   double height, aPrec;

   double x = ground_pt.x;

   double y = ground_pt.y;

   double z = ground_pt.z;

   computeElevation(x, y, z, height, aPrec, desired_precision);

   // Compute ground delta for the seed line and sample.

   double xSeed = x;

   double ySeed = y;

   double zSeed = z;

   imageToPlane(lineTemp, sampleTemp, height, adj,

      xSeed, ySeed, zSeed, mode);

   double dx = x - xSeed;

   double dy = y - ySeed;

   double dz = z - zSeed;

   double len = dx * dx + dy * dy + dz * dz;

   // Iterate until ground delta is acceptable.

   double xLine, yLine, zLine;

   double xPLine, yPLine, zPLine;

   double xSample, ySample, zSample;

   double xPSamp, yPSamp, zPSamp;

   double dLine, dSamp, det;

   int ktr = 0;

   while (preSquare < len && ktr < MKTR)

   {

      // Check for convergence

      mode = -1;

      ktr++;

      // Compute partial of ground coordinates w.r.t. line

      xLine = xSeed;

      yLine = ySeed;

      zLine = zSeed;

      mode = -1;

      imageToPlane(lineTemp + DELTA_IMAGE, sampleTemp, height, adj,

         xLine, yLine, zLine, mode);

      xPLine = xLine - xSeed;

      yPLine = yLine - ySeed;

      zPLine = zLine - zSeed;

      // Compute partial of ground coordinates w.r.t. sample

      xSample = xSeed;

      ySample = ySeed;

      zSample = zSeed;

      mode = -1;

      imageToPlane(lineTemp, sampleTemp + DELTA_IMAGE, height, adj,

         xSample, ySample, zSample, mode);

      xPSamp = xSample - xSeed;

      yPSamp = ySample - ySeed;

      zPSamp = zSample - zSeed;

      // Compute delta line and sample by fixing one coordinate

      // axis.  This axis has the largest image ray component and

      // not updated in the imageToPlane() routine.  Since

      // imageToPlane() updates only two of the three coordinates.

      // Therefore, one of dx, dy, dz must be = 0.0 exactly.

      // The following if else if should work just fine.

      if (0.0 == dx)

      {

         det = yPLine * zPSamp - yPSamp * zPLine;

         dLine = (zPSamp * dy - yPSamp * dz);

         dSamp = (yPLine * dz - zPLine * dy);

      }

      else if (0.0 == dy)

      {

         det = xPLine * zPSamp - xPSamp * zPLine;

         dLine = (zPSamp * dx - xPSamp * dz);

         dSamp = (xPLine * dz - zPLine * dx);

      }

      else  if (0.0 == dz)

      {

         det = xPLine * yPSamp - xPSamp * yPLine;

         dLine = (yPSamp * dx - xPSamp * dy);

         dSamp = (xPLine * dy - yPLine * dx);

      }

      else

      {

   // Search for the line, sample coordinate that viewed a given ground point.

   // This method uses an iterative bisection method to search for the image

   // line.

   //

   // For a given search window, this routine involves projecting the

   // ground point onto the focal plane based on the instrument orientation

   // at the start and end of the search window. Then, it computes the focal

   // plane intersection at a mid-point of the search window. Then, it reduces

   // the search window based on the signs of the intersected line offsets from

   // the center of the ccd. For example, if the line offset is -145 at the

   // start of the window, 10 at the mid point, and 35 at the end of the search

   // window, the window will be reduced to the start of the old window to the

   // middle of the old window.

   //

   // In order to achieve faster convergence, the mid point is calculated

   // using the False Position Method instead of simple bisection. This method

   // uses the zero of the line between the two ends of the search window for

   // the mid point instead of a simple bisection. In most cases, this will

   // converge significantly faster, but it can be slower than simple bisection

   // in some situations.

   // Start bisection search on the image lines

   double sampCtr = _data.m_TotalSamples / 2.0;

   double firstTime = getImageTime(csm::ImageCoord(0.0, sampCtr));

   double lastTime = getImageTime(csm::ImageCoord(_data.m_TotalLines, sampCtr));

   double firstOffset = computeViewingPixel(firstTime, ground_pt, adj).line - 0.5;

   double lastOffset = computeViewingPixel(lastTime, ground_pt, adj).line - 0.5;

   // Check if both offsets have the same sign.

   // This means there is not guaranteed to be a zero.

   if ((firstOffset > 0) != (lastOffset < 0)) {

        throw csm::Error(

           csm::Error::ALGORITHM,

            "Undefined case.",

           "Ground point is not viewed by the image.",

           "UsgsAstroLsSensorModel::groundToImage");

   }

      // Update line and sample estimates

      if (0.0 == det)

      {

         throw csm::Error(

            csm::Error::ALGORITHM,

            "Divide by zero.",

            "UsgsAstroLsSensorModel::groundToImage");

   // Convert the ground precision to pixel precision so we can

   // check for convergence without re-intersecting

   csm::ImageCoord approxPoint;

   computeLinearApproximation(ground_pt, approxPoint);

   csm::ImageCoord approxNextPoint = approxPoint;

   if (approxNextPoint.line + 1 < _data.m_TotalLines) {

      ++approxNextPoint.line;

   }

   else {

      --approxNextPoint.line;

   }

   csm::EcefCoord approxIntersect = imageToGround(approxPoint, _data.m_RefElevation);

   csm::EcefCoord approxNextIntersect = imageToGround(approxNextPoint, _data.m_RefElevation);

   double lineDX = approxNextIntersect.x - approxIntersect.x;

   double lineDY = approxNextIntersect.y - approxIntersect.y;

   double lineDZ = approxNextIntersect.z - approxIntersect.z;

   double approxLineRes = sqrt(lineDX * lineDX + lineDY * lineDY + lineDZ * lineDZ);

   // Increase the precision by a small amount to ensure the desired precision is met

   double pixelPrec = desired_precision / approxLineRes * 0.9;

   // Start bisection search for zero

   for (int it = 0; it < 30; it++) {

      double nextTime = ((firstTime * lastOffset) - (lastTime * firstOffset))

                      / (lastOffset - firstOffset);

      double nextOffset = computeViewingPixel(nextTime, ground_pt, adj).line - 0.5;

      // We're looking for a zero, so check that either firstLine and middleLine have

      // opposite signs, or middleLine and lastLine have opposite signs.

      if ((firstOffset > 0) == (nextOffset < 0)) {

         lastTime = nextTime;

         lastOffset = nextOffset;

      }

      else {

         firstTime = nextTime;

         firstOffset = nextOffset;

      }

      if (fabs(lastOffset - firstOffset) < pixelPrec) {

         break;

      }

   }

      lineTemp += (dLine / det / DELTA_IMAGE);

      sampleTemp += (dSamp / det / DELTA_IMAGE);

      // Update ground delta

      xSeed = x;

      ySeed = y;

      zSeed = z;

      mode = -1;

      imageToPlane(lineTemp, sampleTemp, height, adj,

         xSeed, ySeed, zSeed, mode);

      dx = x - xSeed;

      dy = y - ySeed;

      dz = z - zSeed;

      len = dx * dx + dy * dy + dz * dz;

   // Check that the desired precision was met

   }  // while (preSquare < len)

   double computedTime = ((firstTime * lastOffset) - (lastTime * firstOffset))

                       / (lastOffset - firstOffset);

   csm::ImageCoord calculatedPixel = computeViewingPixel(computedTime, ground_pt, adj);

   // The computed viewing line is the detector line, so we need to convert that to image lines

   auto referenceTimeIt = std::upper_bound(_data.m_IntTimeStartTimes.begin(),

                                           _data.m_IntTimeStartTimes.end(),

                                           computedTime);

   if (referenceTimeIt != _data.m_IntTimeStartTimes.begin()) {

      --referenceTimeIt;

   }

   size_t referenceIndex = std::distance(_data.m_IntTimeStartTimes.begin(), referenceTimeIt);

   calculatedPixel.line += _data.m_IntTimeLines[referenceIndex] - 1

                         + (computedTime - _data.m_IntTimeStartTimes[referenceIndex])

                         / _data.m_IntTimes[referenceIndex];

   csm::EcefCoord calculatedPoint = imageToGround(calculatedPixel, _data.m_RefElevation);

   double dx = ground_pt.x - calculatedPoint.x;

   double dy = ground_pt.y - calculatedPoint.y;

   double dz = ground_pt.z - calculatedPoint.z;

   double len = dx * dx + dy * dy + dz * dz;

   // If the final correction is greater than 10 meters,

   // the solution is not valid enough to report even with a warning

   if (len > 100.0)

   {

   if (len > 100.0) {

      throw csm::Error(

         csm::Error::ALGORITHM,

         "Did not converge.",

         "UsgsAstroLsSensorModel::groundToImage");

   }

   if (achieved_precision)

   if (achieved_precision) {

      *achieved_precision = sqrt(len);

   }

   if (warnings && (desired_precision > 0.0) && (preSquare < len))

   {

   double preSquare = desired_precision * desired_precision;

   if (warnings && (desired_precision > 0.0) && (preSquare < len)) {

      std::stringstream msg;

      msg << "Desired precision not achieved. ";

      msg << len << "  " << preSquare << "\n";

         "UsgsAstroLsSensorModel::groundToImage()"));

   }

   return csm::ImageCoord(lineTemp, sampleTemp);

   return calculatedPixel;

}

//***************************************************************************

{

   // Flip image taken backwards

   double line1 = image_pt.line;

   if (_data.m_ImageFlipFlag == 1)

      line1 = _data.m_TotalLines - 1 - image_pt.line;

   // CSM image convention: UL pixel center == (0.5, 0.5)

   // USGS image convention: UL pixel center == (1.0, 1.0)

   double lineCSMFull = line1 + _data.m_OffsetLines;

   double lineUSGSFull = lineCSMFull + 0.5;

   // USGS timing reference leftmost-side 1st full row

   double timeUsgs = _data.m_IntTime * (lineUSGSFull - 0.5)

      + _data.m_StartingEphemerisTime;

   // These calculation assumes that the values in the integration time

   // vectors are in terms of ISIS' pixels

   auto referenceLineIt = std::upper_bound(_data.m_IntTimeLines.begin(),

                                           _data.m_IntTimeLines.end(),

                                           lineUSGSFull);

   if (referenceLineIt != _data.m_IntTimeLines.begin()) {

      --referenceLineIt;

   }

   size_t referenceIndex = std::distance(_data.m_IntTimeLines.begin(), referenceLineIt);

   double time = _data.m_IntTimeStartTimes[referenceIndex]

      + _data.m_IntTimes[referenceIndex] * (lineUSGSFull - _data.m_IntTimeLines[referenceIndex]);

   // Translate time origin to image geometric center

   // Same as ephemeris and attitude time origin

   return timeUsgs - _data.m_CenterEphemerisTime;

   return time;

}

   double sampleCSMFull = sample + _data.m_OffsetSamples;

   double sampleUSGSFull = sampleCSMFull + 0.5;

   double fractionalLine = line - floor(line) - 0.5;

   // Compute distorted image coordinates in mm

   double m12 = _data.m_ITransL[2];

   double m21 = _data.m_ITransS[1];

   double m22 = _data.m_ITransS[2];

   double t1 = _data.m_DetectorLineOffset - _data.m_DetectorLineOrigin - _data.m_ITransL[0];

   double t1 = fractionalLine + _data.m_DetectorLineOffset

               - _data.m_DetectorLineOrigin - _data.m_ITransL[0];

   double t2 = isisDetSample - _data.m_DetectorSampleOrigin - _data.m_ITransS[0];

   double determinant = m11 * m22 - m12 * m21;

   double p11 = m11 / determinant;

   double p12 = -m12 / determinant;

   double p21 = -m21 / determinant;

   double p22 = m22 / determinant;

   double isisNatFocalPlaneX = p11 * t1 + p21 * t2;

   double isisNatFocalPlaneX = p11 * t1 + p12 * t2;

   double isisNatFocalPlaneY = p21 * t1 + p22 * t2;

   // Remove lens distortion

}

//***************************************************************************

// UsgsAstroLineScannerSensorModel::computeViewingPixel

//***************************************************************************

csm::ImageCoord UsgsAstroLsSensorModel::computeViewingPixel(

   const double& time,

   const csm::EcefCoord& groundPoint,

   const std::vector<double>& adj) const

{

   // Get the exterior orientation

   double xc, yc, zc, vx, vy, vz;

   getAdjSensorPosVel(time, adj, xc, yc, zc, vx, vy, vz);

   // Compute the look vector

   double bodyLookX = groundPoint.x - xc;

   double bodyLookY = groundPoint.y - yc;

   double bodyLookZ = groundPoint.z - zc;

   // Rotate the look vector into the camera reference frame

   int nOrder = 8;

   if (_data.m_PlatformFlag == 0)

      nOrder = 4;

   int nOrderQuat = nOrder;

   if (_data.m_NumQuaternions < 6 && nOrder == 8)

      nOrderQuat = 4;

   double q[4];

   lagrangeInterp(

      _data.m_NumQuaternions, &_data.m_Quaternions[0], _data.m_T0Quat, _data.m_DtQuat,

      time, 4, nOrderQuat, q);

   double norm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);

   // Divide by the negative norm for 0 through 2 to invert the quaternion

   q[0] /= -norm;

   q[1] /= -norm;

   q[2] /= -norm;

   q[3] /= norm;

   double bodyToCamera[9];

   bodyToCamera[0] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];

   bodyToCamera[1] = 2 * (q[0] * q[1] - q[2] * q[3]);

   bodyToCamera[2] = 2 * (q[0] * q[2] + q[1] * q[3]);

   bodyToCamera[3] = 2 * (q[0] * q[1] + q[2] * q[3]);

   bodyToCamera[4] = -q[0] * q[0] + q[1] * q[1] - q[2] * q[2] + q[3] * q[3];

   bodyToCamera[5] = 2 * (q[1] * q[2] - q[0] * q[3]);

   bodyToCamera[6] = 2 * (q[0] * q[2] - q[1] * q[3]);

   bodyToCamera[7] = 2 * (q[1] * q[2] + q[0] * q[3]);

   bodyToCamera[8] = -q[0] * q[0] - q[1] * q[1] + q[2] * q[2] + q[3] * q[3];

   double cameraLookX = bodyToCamera[0] * bodyLookX

                      + bodyToCamera[1] * bodyLookY

                      + bodyToCamera[2] * bodyLookZ;

   double cameraLookY = bodyToCamera[3] * bodyLookX

                      + bodyToCamera[4] * bodyLookY

                      + bodyToCamera[5] * bodyLookZ;

   double cameraLookZ = bodyToCamera[6] * bodyLookX

                      + bodyToCamera[7] * bodyLookY

                      + bodyToCamera[8] * bodyLookZ;

   // Invert the attitude correction

   double aTime = time - _data.m_T0Quat;

   double euler[3];

   double nTime = aTime / _data.m_HalfTime;

   double nTime2 = nTime * nTime;

   euler[0] =

      (getValue(6, adj) + getValue(9, adj)* nTime + getValue(12, adj)* nTime2) / _data.m_FlyingHeight;

   euler[1] =

      (getValue(7, adj) + getValue(10, adj)* nTime + getValue(13, adj)* nTime2) / _data.m_FlyingHeight;

   euler[2] =

      (getValue(8, adj) + getValue(11, adj)* nTime + getValue(14, adj)* nTime2) / _data.m_HalfSwath;

   double cos_a = cos(euler[0]);

   double sin_a = sin(euler[0]);

   double cos_b = cos(euler[1]);

   double sin_b = sin(euler[1]);

   double cos_c = cos(euler[2]);

   double sin_c = sin(euler[2]);

   double attCorr[9];

   attCorr[0] = cos_b * cos_c;

   attCorr[1] = -cos_a * sin_c + sin_a * sin_b * cos_c;

   attCorr[2] = sin_a * sin_c + cos_a * sin_b * cos_c;

   attCorr[3] = cos_b * sin_c;

   attCorr[4] = cos_a * cos_c + sin_a * sin_b * sin_c;

   attCorr[5] = -sin_a * cos_c + cos_a * sin_b * sin_c;

   attCorr[6] = -sin_b;

   attCorr[7] = sin_a * cos_b;

   attCorr[8] = cos_a * cos_b;

   double adjustedLookX = attCorr[0] * cameraLookX

                        + attCorr[3] * cameraLookY

                        + attCorr[6] * cameraLookZ;

   double adjustedLookY = attCorr[1] * cameraLookX

                        + attCorr[4] * cameraLookY

                        + attCorr[7] * cameraLookZ;

   double adjustedLookZ = attCorr[2] * cameraLookX

                        + attCorr[5] * cameraLookY

                        + attCorr[8] * cameraLookZ;

   // Invert the boresight correction

   double correctedLookX = _data.m_MountingMatrix[0] * adjustedLookX

                         + _data.m_MountingMatrix[3] * adjustedLookY

                         + _data.m_MountingMatrix[6] * adjustedLookZ;