Commit 0bddf1da authored by acpaquette's avatar acpaquette Committed by Jesse Mapel
Browse files

New groundToImage method (#260) 



* Added line and point comp and distance

* Finished linearization

* Fixed removing wrong line

* More fixes

* First pass at detector coords

* Added line coefficient computation

* Updated for the line search algorithem

* Added final touches to the groundToImage algorithm

* Removed old groundtoimage code

* Removed some prints

* Somewhat working groundToImage stuffs

* Fixed orbital ls

* Fixed old line

* Added warnings, moved things around, added some prints

* More prints

* Fixed sensor angles

* Removed constAngular tests

* Commented out new algorithm section and removed prints

* Updated expected precision to be the default precision passed to groundtoimage

* Updated tests to check for precision

* Addressed PR feedback

* Added old logger message back in

* Removed utlity code from algorithm

* Changes based on feedback and other small comment based changes

Co-authored-by: default avatarJesse Mapel <jam826@nau.edu>
parent b2ea3a26

include/usgscsm/UsgsAstroLsSensorModel.h

+2 −8
Original line number Diff line number Diff line
@@ -108,11 +108,6 @@ public: 
   std::vector<double> m_positions;

   std::vector<double> m_velocities;

   std::vector<double> m_quaternions;

   std::vector<int> m_detectorNodes;

   std::vector<double> m_detectorXCoords;

   std::vector<double> m_detectorYCoords;

   std::vector<double> m_detectorLineCoeffs;

   double m_averageDetectorSize;

   std::vector<double> m_currentParameterValue;

   std::vector<csm::param::Type> m_parameterType;

   csm::EcefCoord m_referencePointXyz;
@@ -1043,11 +1038,10 @@ private: 


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

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

   csm::ImageCoord computeViewingPixel(

   std::vector<double> computeDetectorView(

      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 double& desiredPrecision // Desired precision for distortion inversion

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

   ) const;



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

include/usgscsm/Utilities.h

+0 −19
Original line number Diff line number Diff line
@@ -11,25 +11,6 @@ 
#include <json/json.hpp>



#include <Warning.h>



double distanceToLine(double x, double y,

                      double a, double b, double c);



double distanceToPlane(double x, double y, double z,

                       double a, double b, double c, double d);



void line(double x1, double y1, double x2, double y2,

          double &a, double &b, double &c);



void plane(double x0, double y0, double z0,

           double v1x, double v1y, double v1z,

           double v2x, double v2y, double v2z,

           double &a, double &b, double &c, double &d);



std::vector<int> fitLinearApproximation(const std::vector<double> &x,

                                        const std::vector<double> &y,

                                        double tolerance);



// methods pulled out of los2ecf and computeViewingPixel



// for now, put everything in here.

src/UsgsAstroLsSensorModel.cpp

+71 −179
Original line number Diff line number Diff line
@@ -516,11 +516,6 @@ void UsgsAstroLsSensorModel::reset() 
  m_positions.clear();                        // 42

  m_velocities.clear();                      // 43

  m_quaternions.clear();                     // 44

  m_detectorNodes.clear();

  m_detectorXCoords.clear();

  m_detectorYCoords.clear();

  m_detectorLineCoeffs.clear();

  m_averageDetectorSize = 0.0;



  m_currentParameterValue.assign(NUM_PARAMETERS,0.0);

  m_parameterType.assign(NUM_PARAMETERS,csm::param::REAL);
@@ -614,37 +609,6 @@ void UsgsAstroLsSensorModel::updateState() 
   MESSAGE_LOG(m_logger, "updateState: half time duration set to {}",

                               m_halfTime)



   // compute linearized detector coordinates

   m_detectorXCoords.resize(m_nSamples);

   m_detectorYCoords.resize(m_nSamples);



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

     computeDistortedFocalPlaneCoordinates(

          0.5, i,

          m_detectorSampleOrigin, m_detectorLineOrigin,

          m_detectorSampleSumming, 1.0,

          m_startingSample, 0.0,

          m_iTransS, m_iTransL,

          m_detectorXCoords[i], m_detectorYCoords[i]

     );

   }

   m_averageDetectorSize = 0;

   for (int i = 1; i < m_nSamples; i++) {

     double xDist = m_detectorXCoords[i] - m_detectorXCoords[i-1];

     double yDist = m_detectorYCoords[i] - m_detectorYCoords[i-1];

     m_averageDetectorSize += sqrt(xDist*xDist + yDist*yDist);

   }

   m_averageDetectorSize /= (m_nSamples-1);

   m_detectorNodes = fitLinearApproximation(m_detectorXCoords, m_detectorYCoords,

                                            m_averageDetectorSize);



   m_detectorLineCoeffs.resize(3 * (m_detectorNodes.size() - 1));

   for (size_t i = 0; i + 1 < m_detectorNodes.size(); i++) {

     line(m_detectorXCoords[m_detectorNodes[i]], m_detectorYCoords[m_detectorNodes[i]],

          m_detectorXCoords[m_detectorNodes[i+1]], m_detectorYCoords[m_detectorNodes[i+1]],

          m_detectorLineCoeffs[3*i], m_detectorLineCoeffs[3*i+1], m_detectorLineCoeffs[3*i+2]);

   }



   // Parameter covariance, hardcoded accuracy values

   int num_params = NUM_PARAMETERS;

   int num_paramsSquare = num_params * num_params;
@@ -687,129 +651,76 @@ csm::ImageCoord UsgsAstroLsSensorModel::groundToImage( 
// UsgsAstroLsSensorModel::groundToImage (internal version)

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

csm::ImageCoord UsgsAstroLsSensorModel::groundToImage(

   const csm::EcefCoord& ground_pt,

   const csm::EcefCoord& groundPt,

   const std::vector<double>& adj,

   double                desired_precision,

   double*               achieved_precision,

   double                desiredPrecision,

   double*               achievedPrecision,

   csm::WarningList*     warnings) const

{

   MESSAGE_LOG(m_logger, "Computing groundToImage (with adjustments) for {}, {}, {}, with desired precision {}",

              ground_pt.x, ground_pt.y, ground_pt.z, desired_precision);

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

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

   // line. Since this is looking for a zero we subtract 0.5 each time the offsets

   // are calculated. This allows it to converge on the center of the pixel where

   // there is only one correct answer instead of the top or bottom of the pixel

   // where there are two correct answers.



   // 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 < m_nLines) {

      ++approxNextPoint.line;

   }

   else {

      --approxNextPoint.line;

   }

   double height, aPrec;

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

   csm::EcefCoord approxIntersect = imageToGround(approxPoint, height);

   csm::EcefCoord approxNextIntersect = imageToGround(approxNextPoint, height);

   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 secant method search on the image lines

   double sampCtr = m_nSamples / 2.0;

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

   double lastTime = getImageTime(csm::ImageCoord(m_nLines, sampCtr));

   // See comment above for why (- 0.5)

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

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

   MESSAGE_LOG(m_logger, "groundToImage: Initial firstOffset {}, lastOffset {}", firstOffset, lastOffset)



   double closestLine;

   csm::ImageCoord detectorCoord;



   // Start secant method search

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

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

                      / (lastOffset - firstOffset);

      // Because time across the image is not continuous, find the exposure closest

      // to the computed nextTime and use that.



      // I.E. if the computed nextTime is 0.3, and the middle exposure times for

      // lines are 0.07, 0.17, 0.27, 0.37, and 0.47; then use 0.27 because it is

      // the closest middle exposure time.

      auto referenceTimeIt = std::upper_bound(m_intTimeStartTimes.begin(),

                                              m_intTimeStartTimes.end(),

                                              nextTime);

      if (referenceTimeIt != m_intTimeStartTimes.begin()) {

         --referenceTimeIt;

      }



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

      MESSAGE_LOG(m_logger, "groundToImage: Find reference time, line number {}, start time {}, duration {} ",

                  m_intTimeLines[referenceIndex], m_intTimeStartTimes[referenceIndex], m_intTimes[referenceIndex])

      double computedLine = (nextTime - m_intTimeStartTimes[referenceIndex]) / m_intTimes[referenceIndex]

                          + m_intTimeLines[referenceIndex];

      // Remove -0.5 once ISIS linescanrate is fixed

      closestLine = floor(computedLine - 0.5);

      nextTime = getImageTime(csm::ImageCoord(closestLine, sampCtr));



      detectorCoord = computeViewingPixel(nextTime, ground_pt, adj, pixelPrec/2);

      double nextOffset = detectorCoord.line - 0.5;



      // remove the farthest away node

      if (fabs(firstTime - nextTime) > fabs(lastTime - nextTime)) {

        firstTime = nextTime;

        firstOffset = nextOffset;

      }

      else {

        lastTime = nextTime;

        lastOffset = nextOffset;

      }

      MESSAGE_LOG(m_logger, "groundToImage: Loop firstOffset {}, firstTime {}, lastOffset {}, lastTime {}, nextOffset {}, nextTime {}",

                  firstOffset, firstTime, lastOffset, lastTime, nextOffset, nextTime)

   // This method first uses a linear approximation to get an initial point.

   // Then the detector offset for the line is continuously computed and

   // applied to the line until we achieve the desired precision.



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

        MESSAGE_LOG(m_logger, "groundToImage: Final firstOffset {}, lastOffset {}, pixelPre {}", firstOffset, lastOffset, pixelPrec)

        break;

      }

   csm::ImageCoord approxPt;

   computeLinearApproximation(groundPt, approxPt);



   std::vector<double> detectorView;

   double detectorLine = m_nLines;

   double count = 0;

   double timei;



   while(abs(detectorLine) > desiredPrecision && ++count < 15) {

     timei = getImageTime(approxPt);

     detectorView = computeDetectorView(timei, groundPt, adj);



     // Convert to detector line

     detectorLine = m_iTransL[0]

                  + m_iTransL[1] * detectorView[0]

                  + m_iTransL[2] * detectorView[1];



     // Convert to image line

     approxPt.line += detectorLine;

   }



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

   csm::ImageCoord calculatedPixel(detectorCoord.line + closestLine, detectorCoord.samp);

   timei = getImageTime(approxPt);

   detectorView = computeDetectorView(timei, groundPt, adj);



   // Reintersect to ensure the image point actually views the ground point.

   csm::EcefCoord calculatedPoint = imageToGround(calculatedPixel, height);

   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;

   // Invert distortion

   double distortedFocalX, distortedFocalY;

   applyDistortion(detectorView[0], detectorView[1], distortedFocalX, distortedFocalY,

                   m_opticalDistCoeffs, m_distortionType, desiredPrecision);



   if (achieved_precision) {

      *achieved_precision = sqrt(len);

   // Convert to detector line and sample

   detectorLine = m_iTransL[0]

                + m_iTransL[1] * distortedFocalX

                + m_iTransL[2] * distortedFocalY;

   double detectorSample = m_iTransS[0]

                         + m_iTransS[1] * distortedFocalX

                         + m_iTransS[2] * distortedFocalY;



   // Convert to image sample line

   approxPt.line += detectorLine;

   approxPt.samp = (detectorSample + m_detectorSampleOrigin - m_startingSample)

                 / m_detectorSampleSumming;



   if (achievedPrecision) {

     *achievedPrecision = detectorLine;

   }



   double preSquare = desired_precision * desired_precision;

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

     MESSAGE_LOG(m_logger, "groundToImage: Desired precision not achieved {}", preSquare)

     std::stringstream msg;

     msg << "Desired precision not achieved. ";

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

     warnings->push_back(csm::Warning(csm::Warning::PRECISION_NOT_MET,

                         msg.str().c_str(),

   MESSAGE_LOG(m_logger, "groundToImage: image line sample {} {}",

                                approxPt.line, approxPt.samp)



   if (warnings && (desiredPrecision > 0.0) && (abs(detectorLine) > desiredPrecision))

   {

      warnings->push_back(

         csm::Warning(

            csm::Warning::PRECISION_NOT_MET,

            "Desired precision not achieved.",

            "UsgsAstroLsSensorModel::groundToImage()"));

   }

   MESSAGE_LOG(m_logger, "groundToImage: Final pixel line {}, sample {}", calculatedPixel.line, calculatedPixel.samp)



   return calculatedPixel;

   return approxPt;

}



//***************************************************************************
@@ -1356,7 +1267,7 @@ double UsgsAstroLsSensorModel::getImageTime( 
   const csm::ImageCoord& image_pt) const

{

   // Remove 0.5 after ISIS dependency in the linescanrate is gone

   double lineFull = floor(image_pt.line) + 0.5;

   double lineFull = image_pt.line;



   auto referenceLineIt = std::upper_bound(m_intTimeLines.begin(),

                                           m_intTimeLines.end(),
@@ -1969,12 +1880,11 @@ void UsgsAstroLsSensorModel::losToEcf( 
   // USGS image convention: UL pixel center == (1.0, 1.0)

   double sampleCSMFull = sample;

   double sampleUSGSFull = sampleCSMFull;

   double fractionalLine = line - floor(line);



   // Compute distorted image coordinates in mm (sample, line on image (pixels) -> focal plane

   double distortedFocalPlaneX, distortedFocalPlaneY;

   computeDistortedFocalPlaneCoordinates(

         fractionalLine, sampleUSGSFull,

         m_detectorLineOrigin, sampleUSGSFull,

         m_detectorSampleOrigin, m_detectorLineOrigin,

         m_detectorSampleSumming, 1.0,

         m_startingSample, 0.0,
@@ -2369,6 +2279,7 @@ void UsgsAstroLsSensorModel::getAdjSensorPosVel( 
   double sensVelNom[3];

   lagrangeInterp(m_numPositions/3, &m_velocities[0], m_t0Ephem, m_dtEphem,

      time, 3, nOrder, sensVelNom);



   MESSAGE_LOG(m_logger, "getAdjSensorPosVel: using {} order Lagrange",

                                nOrder)
@@ -2437,18 +2348,18 @@ void UsgsAstroLsSensorModel::getAdjSensorPosVel( 




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

// UsgsAstroLineScannerSensorModel::computeViewingPixel

// UsgsAstroLineScannerSensorModel::computeDetectorView

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

csm::ImageCoord UsgsAstroLsSensorModel::computeViewingPixel(

std::vector<double> UsgsAstroLsSensorModel::computeDetectorView(

   const double& time,

   const csm::EcefCoord& groundPoint,

   const std::vector<double>& adj,

   const double& desiredPrecision) const

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

{

  MESSAGE_LOG(m_logger, "Computing computeViewingPixel (with adjusments)"

  MESSAGE_LOG(m_logger, "Computing computeDetectorView (with adjusments)"

                              "for ground point {} {} {} at time {} ",

                              groundPoint.x, groundPoint.y, groundPoint.z, time)





  // Helper function to compute the CCD pixel that views a ground point based

  // on the exterior orientation at a given time.
@@ -2460,7 +2371,7 @@ csm::ImageCoord UsgsAstroLsSensorModel::computeViewingPixel( 
   double bodyLookX = groundPoint.x - xc;

   double bodyLookY = groundPoint.y - yc;

   double bodyLookZ = groundPoint.z - zc;

   MESSAGE_LOG(m_logger, "computeViewingPixel: look vector {} {} {}",

   MESSAGE_LOG(m_logger, "computeDetectorView: look vector {} {} {}",

                                bodyLookX, bodyLookY, bodyLookZ)



   // Rotate the look vector into the camera reference frame
@@ -2479,7 +2390,7 @@ csm::ImageCoord UsgsAstroLsSensorModel::computeViewingPixel( 
   double cameraLookZ = bodyToCamera[2] * bodyLookX

                      + bodyToCamera[5] * bodyLookY

                      + bodyToCamera[8] * bodyLookZ;

   MESSAGE_LOG(m_logger, "computeViewingPixel: look vector (camrea ref frame)"

   MESSAGE_LOG(m_logger, "computeDetectorView: look vector (camrea ref frame)"

                               "{} {} {}",

                                cameraLookX, cameraLookY, cameraLookZ)
@@ -2497,7 +2408,7 @@ csm::ImageCoord UsgsAstroLsSensorModel::computeViewingPixel( 
   double adjustedLookZ = attCorr[2] * cameraLookX

                        + attCorr[5] * cameraLookY

                        + attCorr[8] * cameraLookZ;

   MESSAGE_LOG(m_logger, "computeViewingPixel: adjusted look vector"

   MESSAGE_LOG(m_logger, "computeDetectorView: adjusted look vector"

                               "{} {} {}",

                                adjustedLookX, adjustedLookY, adjustedLookZ)
@@ -2505,31 +2416,12 @@ csm::ImageCoord UsgsAstroLsSensorModel::computeViewingPixel( 
   double lookScale = (m_focalLength  + getValue(15, adj)) / adjustedLookZ;

   double focalX = adjustedLookX * lookScale;

   double focalY = adjustedLookY * lookScale;

   double distortedFocalX, distortedFocalY;

   MESSAGE_LOG(m_logger, "computeViewingPixel: focal plane coordinates"



   MESSAGE_LOG(m_logger, "computeDetectorView: focal plane coordinates"

                         "x:{} y:{} scale:{}",

                         focalX, focalY, lookScale)



   // Invert distortion

   applyDistortion(focalX, focalY, distortedFocalX, distortedFocalY,

                   m_opticalDistCoeffs, m_distortionType, desiredPrecision);



   // Convert to detector line and sample

   double detectorLine = m_iTransL[0]

                       + m_iTransL[1] * distortedFocalX

                       + m_iTransL[2] * distortedFocalY;

   double detectorSample = m_iTransS[0]

                         + m_iTransS[1] * distortedFocalX

                         + m_iTransS[2] * distortedFocalY;



   // Convert to image sample line

   double line = detectorLine + m_detectorLineOrigin;

   double sample = (detectorSample + m_detectorSampleOrigin - m_startingSample)

                 / m_detectorSampleSumming;

   MESSAGE_LOG(m_logger, "computeViewingPixel: image line sample {} {}",

                                line, sample)



   return csm::ImageCoord(line, sample);

   return std::vector<double> {focalX, focalY};

}

src/Utilities.cpp

+0 −96
Original line number Diff line number Diff line
@@ -7,102 +7,6 @@ 


using json = nlohmann::json;



// Calculate the signed distance from a 2D point to a line given in standard form

//

// --NOTE-- This function assumes that the coefficients of the line have been reduced

//          so that sqrt(a^2 + b^2) = 1

double distanceToLine(double x, double y,

                      double a, double b, double c) {

  return a*x + b*y + c;

}



// Calculate the distance from a 3D point to a plane given in standard form

//

// --NOTE-- This function assumes that the coefficients of the plane have been reduced

//          so that sqrt(a^2 + b^2 + c^2) = 1

double distanceToPlane(double x, double y, double z,

                       double a, double b, double c, double d) {

  return std::abs(a*x + b*y + c*z + d);

}



// Calculate the standard form coefficients of a line that passes through two points

//

// --NOTE-- The coefficients have been reduced such that sqrt(a^2 + b^2) = 1

void line(double x1, double y1, double x2, double y2,

          double &a, double &b, double &c) {

  a = y1 - y2;

  b = x2 - x1;

  c = x1*y2 - x2*y1;

  double scale = sqrt(a*a + b*b);

  a /= scale;

  b /= scale;

  c /= scale;

}



// Calculate the standard form coefficients of a plane that passes through a point

// and contains two vectors.

//

// --NOTE-- The coefficients have been reduced such that sqrt(a^2 + b^2 + c^2) = 1

void plane(double x0, double y0, double z0,

           double v1x, double v1y, double v1z,

           double v2x, double v2y, double v2z,

           double &a, double &b, double &c, double &d) {

  // Compute normal vector and scale

  a = v1y*v2z - v1z*v2y;

  b = v1z*v2x - v1x*v2z;

  c = v1x*v2y - v1y*v2x;

  double scale = sqrt(a*a + b*b + c*c);

  a /= scale;

  b /= scale;

  c /= scale;

  // Shift to point

  d = - (a*x0 + b*y0 + c*z0);

}







// Fit a piecewise-linear approximations to 2D data within a tolerance

//

// Returns a vector of node indices

std::vector<int> fitLinearApproximation(const std::vector<double> &x,

                                        const std::vector<double> &y,

                                        double tolerance) {

  std::vector<int> nodes;

  nodes.push_back(0);



  std::stack<std::pair<int, int>> workStack;

  workStack.push(std::make_pair(0, x.size() - 1));

  while (!workStack.empty()) {

    std::pair<int, int> range = workStack.top();

    workStack.pop();

    double a, b, c;

    line(x[range.first], y[range.first], x[range.second], y[range.second], a, b, c);

    double maxError = 0;

    int maxIndex = (range.second + range.first) / 2;

    for (int i = range.first + 1; i < range.second; i++) {

      double error = std::abs(distanceToLine(x[i], y[i], a, b, c));

      if (error > maxError) {

        maxError = error;

        maxIndex = i;

      }

    }

    // If the max error is greater than the tolerance, split at the largest error

    // Do not split if the range only contains two nodes

    if (maxError > tolerance && range.second - range.first > 1) {

      // Append the second range and then the first range

      // so that nodes are added in the same order they came in

      workStack.push(std::make_pair(maxIndex, range.second));

      workStack.push(std::make_pair(range.first, maxIndex));

    }

    else {

      // segment is good so append last point to nodes

      nodes.push_back(range.second);

    }

  }



  return nodes;

}



// Calculates a rotation matrix from Euler angles

// in - euler[3]

// out - rotationMatrix[9]

tests/Fixtures.h

+0 −27
Original line number Diff line number Diff line
@@ -240,33 +240,6 @@ class ConstVelocityLineScanSensorModel : public ::testing::Test { 
      }

};



class ConstAngularVelocityLineScanSensorModel : public ::testing::Test {

   protected:

      csm::Isd isd;

      UsgsAstroLsSensorModel *sensorModel;



      void SetUp() override {

         sensorModel = NULL;



         isd.setFilename("data/constAngularVelocityLineScan.img");

         UsgsAstroPlugin cameraPlugin;



         csm::Model *model = cameraPlugin.constructModelFromISD(

               isd,

               "USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL");

         sensorModel = dynamic_cast<UsgsAstroLsSensorModel *>(model);



         ASSERT_NE(sensorModel, nullptr);

      }



      void TearDown() override {

         if (sensorModel) {

            delete sensorModel;

            sensorModel = NULL;

         }

      }

};



class OrbitalLineScanSensorModel : public ::testing::Test {

   protected:

      csm::Isd isd;