Unverified Commit 5ef20008 authored by Oleg Alexandrov's avatar Oleg Alexandrov Committed by GitHub
Browse files

Fix for the linescan sensor, the step size can be too big when iterating (#358) 

* Fix for the linescan sensor, the step size can be too big when iterating

* The previous fix was too agressive

* Since m_detectorLineSumming is used in many places, use a  dedicated step size var

* Use secant method to find the right line
parent 7fb9c5b7

include/usgscsm/UsgsAstroLsSensorModel.h

+8 −1
Original line number Diff line number Diff line
@@ -977,6 +977,13 @@ class UsgsAstroLsSensorModel : public csm::RasterGM, 
  // Compute the determinant of a 3x3 matrix

  double determinant3x3(double mat[9]) const;

  

  // A function whose value will be 0 when the line a given ground

  // point projects into is found. The obtained line will be

  // approxPt.line + t.

  double calcDetectorLineErr(double t, csm::ImageCoord const& approxPt,

                             const csm::EcefCoord& groundPt,

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

  

  csm::NoCorrelationModel _no_corr_model;  // A way to report no correlation

                                           // between images is supported

  std::vector<double>

src/UsgsAstroLsSensorModel.cpp

+55 −60
Original line number Diff line number Diff line
@@ -648,36 +648,6 @@ void UsgsAstroLsSensorModel::updateState() { 


  // Set focal length variance

  m_covariance[15 * num_params + 15] = positionVariance;



  // Test if a pixel projected to the ground and projected back

  // returns to the original location. If not, flip the sign of

  // m_iTransL. It is very important here to pick a pixel which is not

  // an integer, as this test may succeed for integer pixels and fail

  // for non-integer ones. Also, need to pick the answer with the

  // smallest achieved precision, even when neither of them is smaller

  // than the desired precision.



  lineCtr = round(m_nLines / 2.0) + 0.5;

  sampCtr = round(m_nSamples / 2.0) + 0.5;



  double desiredPrecision = 0.001;

  ip = csm::ImageCoord(lineCtr, sampCtr);

  csm::EcefCoord xyz = imageToGround(ip, refHeight);



  double achievedPrecision1 = 1.0;

  // Will use m_iTransL on the next line

  ip = groundToImage(xyz, desiredPrecision, &achievedPrecision1);



  double achievedPrecision2 = 1.0;

  for (int it = 0; it < sizeof(m_iTransL)/sizeof(double); it++)

    m_iTransL[it] = -m_iTransL[it]; // use a flipped m_iTransL

  ip = groundToImage(xyz, desiredPrecision, &achievedPrecision2);



  if (std::abs(achievedPrecision1) <= std::abs(achievedPrecision2)) {

    // Flip back m_iTransL as the original was better

    for (int it = 0; it < sizeof(m_iTransL)/sizeof(double); it++)

      m_iTransL[it] = -m_iTransL[it];

  }

}



//---------------------------------------------------------------------------
@@ -707,36 +677,38 @@ csm::ImageCoord UsgsAstroLsSensorModel::groundToImage( 
    const csm::EcefCoord& groundPt, const std::vector<double>& adj,

    double desiredPrecision, double* achievedPrecision,

    csm::WarningList* warnings) const {

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

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

  

  csm::ImageCoord approxPt;

  computeLinearApproximation(groundPt, approxPt);



  std::vector<double> detectorView;

  double detectorLine = m_nLines;

  double detectorSample = 0;

  double count = 0;

  double timei;

  // Search for the (line, sample) coordinate that views a given

  // ground point. Set this up as a root-finding problem and use the

  // secant method.

  int count = 0;

  double t0 = 0.0;

  double lineErr0 = calcDetectorLineErr(t0, approxPt, groundPt, adj);

  double t1 = lineErr0 * 0.1; // need to stay close or else there's overshooting

  double lineErr1 = calcDetectorLineErr(t1, approxPt, groundPt, adj);

  while (std::abs(lineErr1) > desiredPrecision && ++count < 15) {



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

    timei = getImageTime(approxPt);

    detectorView = computeDetectorView(timei, groundPt, adj);

    if (lineErr1 == lineErr0) 

      break; // avoid division by 0

    

    // Convert to detector line

    detectorLine = m_iTransL[0] + m_iTransL[1] * detectorView[0] +

                   m_iTransL[2] * detectorView[1] + m_detectorLineOrigin -

                   m_startingDetectorLine;

    detectorLine /= m_detectorLineSumming;

    // Secant method update

    // https://en.wikipedia.org/wiki/Secant_method

    double t2 = t1 - lineErr1 * (t1 - t0) / (lineErr1 - lineErr0);

    double lineErr2 = calcDetectorLineErr(t2, approxPt, groundPt, adj);

    

    // Convert to image line

    approxPt.line += detectorLine;

    // Update for the next step

    t0 = t1; lineErr0 = lineErr1;

    t1 = t2; lineErr1 = lineErr2;

  }



  timei = getImageTime(approxPt);

  detectorView = computeDetectorView(timei, groundPt, adj);

  // Update the line with the found value

  approxPt.line += t1;



  double timei = getImageTime(approxPt);

  std::vector<double> detectorView = computeDetectorView(timei, groundPt, adj);



  // Invert distortion

  double distortedFocalX, distortedFocalY;
@@ -745,9 +717,9 @@ csm::ImageCoord UsgsAstroLsSensorModel::groundToImage( 
                  desiredPrecision);



  // Convert to detector line and sample

  detectorLine = m_iTransL[0] + m_iTransL[1] * distortedFocalX +

  double detectorLine = m_iTransL[0] + m_iTransL[1] * distortedFocalX +

                        m_iTransL[2] * distortedFocalY;

  detectorSample = m_iTransS[0] + m_iTransS[1] * distortedFocalX +

  double detectorSample = m_iTransS[0] + m_iTransS[1] * distortedFocalX +

                          m_iTransS[2] * distortedFocalY;

  // Convert to image sample line

  double finalUpdate =
@@ -766,7 +738,7 @@ csm::ImageCoord UsgsAstroLsSensorModel::groundToImage( 
              approxPt.samp)



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

      (abs(finalUpdate) > desiredPrecision)) {

      (std::abs(finalUpdate) > desiredPrecision)) {

    warnings->push_back(csm::Warning(

        csm::Warning::PRECISION_NOT_MET, "Desired precision not achieved.",

        "UsgsAstroLsSensorModel::groundToImage()"));
@@ -2650,3 +2622,26 @@ csm::EcefVector UsgsAstroLsSensorModel::getSunPosition( 
  }

  return sunPosition;

}



// A function whose value will be 0 when the line a given ground point

// projects into is found. The obtained line will be approxPt.line +

// t.

double UsgsAstroLsSensorModel::calcDetectorLineErr(double t, csm::ImageCoord const& approxPt,

                                                   const csm::EcefCoord& groundPt,

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

  

  csm::ImageCoord currPt = approxPt;

  currPt.line += t;

    

  double timei = getImageTime(currPt);

  std::vector<double> detectorView = computeDetectorView(timei, groundPt, adj);

  

  // Convert to detector line

  double detectorLine = m_iTransL[0] + m_iTransL[1] * detectorView[0] +

    m_iTransL[2] * detectorView[1] + m_detectorLineOrigin -

    m_startingDetectorLine;

  detectorLine /= m_detectorLineSumming;

    

  return detectorLine;

}