Unverified Commit f098d9a7 authored by Jesse Mapel's avatar Jesse Mapel Committed by GitHub
Browse files

Initial frame jitter (#399)

parent d274dad7

include/usgscsm/UsgsAstroFrameSensorModel.h

+4 −1
Original line number Diff line number Diff line
@@ -365,6 +365,9 @@ class UsgsAstroFrameSensorModel : public csm::RasterGM, 
  std::vector<double> m_iTransS;

  std::vector<double> m_iTransL;

  std::vector<double> m_boresight;

  std::vector<double> m_lineJitter;

  std::vector<double> m_sampleJitter;

  std::vector<double> m_lineTimes;

  double m_majorAxis;

  double m_minorAxis;

  double m_focalLength;

include/usgscsm/Utilities.h

+13 −0
Original line number Diff line number Diff line
@@ -24,6 +24,19 @@ void computeDistortedFocalPlaneCoordinates( 
    const double &startingLine, const double iTransS[], const double iTransL[],

    double &distortedX, double &distortedY);



void removeJitter(

    const double &line, const double &sample,

    const std::vector<double> lineJitterCoeffs, const std::vector<double> sampleJitterCoeffs,

    const std::vector<double> lineTimes,

    double &dejitteredLine, double &dejitteredSample);



void addJitter(

    const double &line, const double &sample,

    const double &tolerance, const int &maxIts,

    const std::vector<double> lineJitterCoeffs, const std::vector<double> sampleJitterCoeffs,

    const std::vector<double> lineTimes,

    double &jitteredLine, double &jitteredSample);



void computePixel(const double &distortedX, const double &distortedY,

                  const double &sampleOrigin, const double &lineOrigin,

                  const double &sampleSumming, const double &lineSumming,

src/UsgsAstroFrameSensorModel.cpp

+47 −10
Original line number Diff line number Diff line
@@ -81,6 +81,9 @@ void UsgsAstroFrameSensorModel::reset() { 
  m_iTransS = std::vector<double>(3, 0.0);

  m_iTransL = std::vector<double>(3, 0.0);

  m_boresight = std::vector<double>(3, 0.0);

  m_lineJitter.clear();

  m_sampleJitter.clear();

  m_lineTimes.clear();

  m_parameterType =

      std::vector<csm::param::Type>(NUM_PARAMETERS, csm::param::REAL);

  m_referencePointXyz.x = 0;
@@ -163,6 +166,18 @@ csm::ImageCoord UsgsAstroFrameSensorModel::groundToImage( 
               m_startingDetectorSample, m_startingDetectorLine, &m_iTransS[0],

               &m_iTransL[0], line, sample);



  // Optionally apply rolling shutter jitter correction

  if (m_lineTimes.size()) {

    double jitteredLine, jitteredSample;

    addJitter(

        line, sample,

        desired_precision, 20,

        m_lineJitter, m_sampleJitter, m_lineTimes,

        jitteredLine, jitteredSample);

    line = jitteredLine;

    sample = jitteredSample;

  }



  MESSAGE_LOG("Computed groundToImage for {}, {}, {} as line, sample: {}, {}",

              groundPt.x, groundPt.y, groundPt.z, line, sample);
@@ -211,8 +226,13 @@ csm::EcefCoord UsgsAstroFrameSensorModel::imageToGround( 
      m[0][0], m[0][1], m[0][2], m[1][0], m[1][1], m[1][2], m[2][0], m[2][1],

      m[2][2]);



  // Apply the principal point offset, assuming the pp is given in pixels

  double xl, yl, zl;

  // Optionally apply rolling shutter jitter correction

  if (m_lineTimes.size()) {

    double dejitteredLine, dejitteredSample;

    removeJitter(line, sample, m_lineJitter, m_sampleJitter, m_lineTimes, dejitteredLine, dejitteredSample);

    line = dejitteredLine;

    sample = dejitteredSample;

  }



  // Convert from the pixel space into the metric space

  double x_camera, y_camera;
@@ -229,6 +249,7 @@ csm::EcefCoord UsgsAstroFrameSensorModel::imageToGround( 
              undistortedY);



  // Now back from distorted mm to pixels

  double xl, yl, zl;

  xl = m[0][0] * undistortedX + m[0][1] * undistortedY -

       m[0][2] * -m_focalLength;

  yl = m[1][0] * undistortedX + m[1][1] * undistortedY -
@@ -737,6 +758,9 @@ std::string UsgsAstroFrameSensorModel::getModelState() const { 
      {"m_transY", {m_transY[0], m_transY[1], m_transY[2]}},

      {"m_iTransS", {m_iTransS[0], m_iTransS[1], m_iTransS[2]}},

      {"m_iTransL", {m_iTransL[0], m_iTransL[1], m_iTransL[2]}},

      {"m_lineJitter", m_lineJitter},

      {"m_sampleJitter", m_sampleJitter},

      {"m_lineTimes", m_lineTimes},

      {"m_majorAxis", m_majorAxis},

      {"m_minorAxis", m_minorAxis},

      {"m_spacecraftVelocity",
@@ -959,6 +983,12 @@ void UsgsAstroFrameSensorModel::replaceModelState( 
      m_originalHalfSamples = state.at("m_originalHalfSamples").get<double>();

    if (state.find("m_pixelPitch") != state.end())

      m_pixelPitch = state.at("m_pixelPitch").get<double>();

    if (state.find("m_lineJitter") != state.end())

      m_lineJitter = state.at("m_lineJitter").get<std::vector<double>>();

    if (state.find("m_sampleJitter") != state.end())

      m_sampleJitter = state.at("m_sampleJitter").get<std::vector<double>>();

    if (state.find("m_lineTimes") != state.end())

      m_lineTimes = state.at("m_lineTimes").get<std::vector<double>>();



  } catch (std::out_of_range &e) {

    MESSAGE_LOG("State keywords required to generate sensor model missing: " +
@@ -1150,6 +1180,13 @@ std::string UsgsAstroFrameSensorModel::constructStateFromIsd( 
  state["m_iTransL"] = ale::getFocal2PixelLines(parsedIsd);

  state["m_iTransS"] = ale::getFocal2PixelSamples(parsedIsd);



  // optional rolling shutter jitter

  if (parsedIsd.find("jitter") != parsedIsd.end()) {

    state["m_lineJitter"] = parsedIsd.at("jitter").at("lineJitterCoefficients");

    state["m_sampleJitter"] = parsedIsd.at("jitter").at("sampleJitterCoefficients");

    state["m_lineTimes"] = parsedIsd.at("jitter").at("lineExposureTimes");

  }



  // We don't pass the pixel to focal plane transformation so invert the

  // focal plane to pixel transformation

  try {

src/Utilities.cpp

+82 −0
Original line number Diff line number Diff line 
#include "Utilities.h"





#include <Error.h>

#include <cmath>

#include <stack>
@@ -88,6 +89,87 @@ void computeDistortedFocalPlaneCoordinates( 
  distortedY = p21 * t1 + p22 * t2;

}





// Compute the de-jittered pixel coordinate given a jittered image coordinate

// a set of jitter coefficients and line exposure times for rolling shutter.

// Jitter coefficients are in largest power first order. There is no constant

// coefficient. For example {1, 2, 3} would correspond to 1*t^3 + 2*t&2 + 3*t.

void removeJitter(

    const double &line, const double &sample,

    const std::vector<double> lineJitterCoeffs, const std::vector<double> sampleJitterCoeffs,

    const std::vector<double> lineTimes,

    double &dejitteredLine, double &dejitteredSample) {

  // Check input

  if (lineJitterCoeffs.size() != sampleJitterCoeffs.size() ||

      lineTimes.size() == 0) {

    throw csm::Error(

        csm::Error::INDEX_OUT_OF_RANGE,

        "Jitter coefficient vectors must be the same size.",

        "removeJitter");

  }

  if (lineTimes.size() == 0) {

    throw csm::Error(

        csm::Error::INDEX_OUT_OF_RANGE,

        "Line exposure times must be non-empty.",

        "removeJitter");

  }

  double lineJitter = 0;

  double sampleJitter = 0;

  // Bound line index to the vector of line exposure times;

  double time = lineTimes[std::max(std::min((int)std::round(line), (int)lineTimes.size()), 1) - 1];

  for (unsigned int n = 0; n < lineJitterCoeffs.size(); n++) {

    double timeTerm = pow(time, lineJitterCoeffs.size() - n);

    lineJitter += lineJitterCoeffs[n] * timeTerm;

    sampleJitter += sampleJitterCoeffs[n] * timeTerm;

  }

  dejitteredLine = line - lineJitter;

  dejitteredSample = sample - sampleJitter;



  return;

}





// Compute the jittered pixel coordinate given a de-jittered image coordinate

// a set of jitter coefficients and line exposure times for rolling shutter.

// Jitter coefficients are in largest power first order. There is no constant

// coefficient. For example {1, 2, 3} would correspond to 1*t^3 + 2*t&2 + 3*t.

// This uses an iterative method so a tolerance and maximum number of iteration

// are required to determine when to stop iterating.

void addJitter(

    const double &line, const double &sample,

    const double &tolerance, const int &maxIts,

    const std::vector<double> lineJitterCoeffs, const std::vector<double> sampleJitterCoeffs,

    const std::vector<double> lineTimes,

    double &jitteredLine, double &jitteredSample) {

  int iteration = 0;

  double dejitteredLine = line - 1;

  double dejitteredSample = sample - 1;

  double currentLine = line;

  double currentSample =  sample;



  while (iteration < maxIts) {

    removeJitter(

        currentLine, currentSample,

        lineJitterCoeffs, sampleJitterCoeffs, lineTimes,

        dejitteredLine, dejitteredSample);



    if (fabs(dejitteredLine - line) < tolerance &&

        fabs(dejitteredSample - sample) < tolerance) {

      break;

    }



    currentLine = line + currentLine - dejitteredLine;

    currentSample = sample + currentSample - dejitteredSample;

    iteration++;

  }



  jitteredLine = currentLine;

  jitteredSample = currentSample;



  return;

}





// Compute the image pixel for a distorted focal plane coordinate

// in - line

// in - sample

tests/Fixtures.h

+30 −2
Original line number Diff line number Diff line
@@ -131,6 +131,34 @@ class OrbitalFrameSensorModel : public ::testing::Test { 
  }

};



class JitterFrameSensorModel : public ::testing::Test {

 protected:

  csm::Isd jitterIsd;

  csm::Isd isd;

  std::shared_ptr<csm::RasterGM> jitterModel;

  std::shared_ptr<csm::RasterGM> model;



  void SetUp() override {

    csm::Model *genericModel = nullptr;



    UsgsAstroPlugin cameraPlugin;



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



    genericModel = cameraPlugin.constructModelFromISD(

      isd, UsgsAstroFrameSensorModel::_SENSOR_MODEL_NAME);

    model = std::shared_ptr<csm::RasterGM>(dynamic_cast<csm::RasterGM *>(genericModel));

    ASSERT_NE(model, nullptr);



    jitterIsd.setFilename("data/jitterFramer.img");



    genericModel = cameraPlugin.constructModelFromISD(

      jitterIsd, UsgsAstroFrameSensorModel::_SENSOR_MODEL_NAME);

    jitterModel = std::shared_ptr<csm::RasterGM>(dynamic_cast<csm::RasterGM *>(genericModel));

    ASSERT_NE(jitterModel, nullptr);

  }

};



class FrameIsdTest : public ::testing::Test {

 protected:

  csm::Isd isd;