Commit 6af0cfae authored by Jesse Mapel's avatar Jesse Mapel Committed by Jesse Mapel
Browse files

Updated ls to re-interp states

parent da685687

src/UsgsAstroLsSensorModel.cpp

+42 −35
Original line number Diff line number Diff line
@@ -2286,17 +2286,50 @@ std::string UsgsAstroLsSensorModel::constructStateFromIsd( 
  state["m_referencePointXyz"] = std::vector<double>(3, 0.0);

  MESSAGE_LOG("m_referencePointXyz: {} ", state["m_referencePointXyz"].dump())



  // Sun position and velocity are required for getIlluminationDirection

  // Sun position and sensor position use the same times so compute that now

  ale::States inst_state = ale::getInstrumentPosition(jsonIsd);

  std::vector<double> ephemTime = inst_state.getTimes();

  double startTime = ephemTime.front();

  double stopTime = ephemTime.back();

  // Force at least 25 nodes to help with lagrange interpolation

  // These also have to be equally spaced for lagrange interpolation

  if (ephemTime.size() < 25) {

    ephemTime.resize(25);

  }

  double timeStep = (stopTime - startTime) / (ephemTime.size() - 1);

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

    ephemTime[i] = startTime + i * timeStep;

  }



  try {

    state["m_dtEphem"] = timeStep;

    MESSAGE_LOG("m_dtEphem: {} ", state["m_dtEphem"].dump())

  } catch (...) {

    parsingWarnings->push_back(csm::Warning(

        csm::Warning::DATA_NOT_AVAILABLE, "dt_ephemeris not in ISD",

        "UsgsAstroFrameSensorModel::constructStateFromIsd()"));

    MESSAGE_LOG("m_dtEphem not in ISD")

  }



  try {

    state["m_t0Ephem"] = startTime - ale::getCenterTime(jsonIsd);

    MESSAGE_LOG("t0_ephemeris: {}", state["m_t0Ephem"].dump())

  } catch (...) {

    parsingWarnings->push_back(csm::Warning(

        csm::Warning::DATA_NOT_AVAILABLE, "t0_ephemeris not in ISD",

        "UsgsAstroFrameSensorModel::constructStateFromIsd()"));

    MESSAGE_LOG("t0_ephemeris not in ISD")

  }



  ale::States sunState = ale::getSunPosition(jsonIsd);

  std::vector<ale::State> sunStates = sunState.getStates();

  std::vector<double> ephemTime = sunState.getTimes();

  ale::Orientations j2000_to_target = ale::getBodyRotation(jsonIsd);

  ale::State rotatedSunState;

  ale::State interpSunState, rotatedSunState;

  std::vector<double> sunPositions = {};

  std::vector<double> sunVelocities = {};



  for (int i = 0; i < ephemTime.size(); i++) {

    rotatedSunState = j2000_to_target.rotateStateAt(ephemTime[i], sunStates[i]);

    interpSunState = sunState.getState(ephemTime[i], ale::SPLINE);

    rotatedSunState = j2000_to_target.rotateStateAt(ephemTime[i], interpSunState);

    // ALE operates in Km and we want m

    sunPositions.push_back(rotatedSunState.position.x * 1000);

    sunPositions.push_back(rotatedSunState.position.y * 1000);
@@ -2363,41 +2396,17 @@ std::string UsgsAstroLsSensorModel::constructStateFromIsd( 
      state["m_detectorSampleOrigin"].dump(),

      state["m_detectorLineOrigin"].dump())



  ale::States inst_state = ale::getInstrumentPosition(jsonIsd);

  // These are exlusive to LineScanners, leave them here for now.

  ephemTime = inst_state.getTimes();

  try {

    state["m_dtEphem"] = (ephemTime[ephemTime.size() - 1] - ephemTime[0]) /

                         (ephemTime.size() - 1);

    MESSAGE_LOG("m_dtEphem: {} ", state["m_dtEphem"].dump())

  } catch (...) {

    parsingWarnings->push_back(csm::Warning(

        csm::Warning::DATA_NOT_AVAILABLE, "dt_ephemeris not in ISD",

        "UsgsAstroFrameSensorModel::constructStateFromIsd()"));

    MESSAGE_LOG("m_dtEphem not in ISD")

  }



  try {

    state["m_t0Ephem"] = ephemTime[0] - ale::getCenterTime(jsonIsd);

    MESSAGE_LOG("t0_ephemeris: {}", state["m_t0Ephem"].dump())

  } catch (...) {

    parsingWarnings->push_back(csm::Warning(

        csm::Warning::DATA_NOT_AVAILABLE, "t0_ephemeris not in ISD",

        "UsgsAstroFrameSensorModel::constructStateFromIsd()"));

    MESSAGE_LOG("t0_ephemeris not in ISD")

  }



  ale::Orientations j2000_to_sensor = ale::getInstrumentPointing(jsonIsd);

  ephemTime = inst_state.getTimes();

  std::vector<ale::State> instStates = inst_state.getStates();

  ale::State rotatedInstState;

  ale::State rotatedInstStateInv;

  ale::State interpInstState, rotatedInstState;

  std::vector<double> positions = {};

  std::vector<double> velocities = {};



  for (int i = 0; i < ephemTime.size(); i++) {

    interpInstState = inst_state.getState(ephemTime[i], ale::SPLINE);

    rotatedInstState =

        j2000_to_target.rotateStateAt(ephemTime[i], instStates[i], ale::SLERP);

        j2000_to_target.rotateStateAt(ephemTime[i], interpInstState, ale::SLERP);

    // ALE operates in Km and we want m

    positions.push_back(rotatedInstState.position.x * 1000);

    positions.push_back(rotatedInstState.position.y * 1000);
@@ -2562,10 +2571,8 @@ csm::EcefVector UsgsAstroLsSensorModel::getSunPosition( 
  // If there are multiple positions, use Lagrange interpolation

  if ((numSunPositions / 3) > 1) {

    double sunPos[3];

    double endTime = m_t0Ephem + (m_dtEphem * ((m_numPositions / 3)));

    double sun_dtEphem = (endTime - m_t0Ephem) / (numSunPositions / 3);

    lagrangeInterp(numSunPositions / 3, &m_sunPosition[0], m_t0Ephem,

                   sun_dtEphem, imageTime, 3, 8, sunPos);

                   m_dtEphem, imageTime, 3, 8, sunPos);

    sunPosition.x = sunPos[0];

    sunPosition.y = sunPos[1];

    sunPosition.z = sunPos[2];

tests/LineScanCameraTests.cpp

+2 −2
Original line number Diff line number Diff line
@@ -192,9 +192,9 @@ TEST_F(OrbitalLineScanSensorModel, getSunPositionStationary) { 
TEST_F(OrbitalLineScanSensorModel, Center) {

  csm::ImageCoord imagePt(8.5, 8.0);

  csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0);

  EXPECT_NEAR(groundPt.x, 999999.67040488799, 1e-9);

  EXPECT_NEAR(groundPt.x, 999999.67004351015, 1e-9);

  EXPECT_NEAR(groundPt.y, 0.0, 1e-9);

  EXPECT_NEAR(groundPt.z, -811.90523782723039, 1e-9);

  EXPECT_NEAR(groundPt.z, -812.35021438796923, 1e-9);

}



TEST_F(OrbitalLineScanSensorModel, Inversion) {

tests/data/orbitalLineScan.json

+19 −19
Original line number Diff line number Diff line
@@ -61,22 +61,22 @@ 
      [1049.99892857, 0.0, -1.49999949]

    ],

    "velocities": [

      [-0.0, 0.0, -1050.0],

      [-0.1, 0.0, -1049.99999524],

      [-0.2, 0.0, -1049.99998095],

      [-0.3, 0.0, -1049.99995714],

      [-0.39999999, 0.0 ,-1049.99992381],

      [-0.49999998, 0.0 ,-1049.99988095],

      [-0.59999997, 0.0 ,-1049.99982857],

      [-0.69999995, 0.0 ,-1049.99976667],

      [-0.79999992, 0.0 ,-1049.99969524],

      [-0.89999989, 0.0 ,-1049.99961429],

      [-0.99999985, 0.0 ,-1049.99952381],

      [-1.0999998, 0.0 ,-1049.99942381],

      [-1.19999974, 0.0 ,-1049.99931429],

      [-1.29999967, 0.0 ,-1049.99919524],

      [-1.39999959, 0.0 ,-1049.99906667],

      [-1.49999949, 0.0, -1049.99892857]

      [-0.0, 0.0, -1.0500],

      [-0.1, 0.0, -1.04999999524],

      [-0.2, 0.0, -1.04999998095],

      [-0.3, 0.0, -1.04999995714],

      [-0.39999999, 0.0 ,-1.04999992381],

      [-0.49999998, 0.0 ,-1.04999988095],

      [-0.59999997, 0.0 ,-1.04999982857],

      [-0.69999995, 0.0 ,-1.04999976667],

      [-0.79999992, 0.0 ,-1.04999969524],

      [-0.89999989, 0.0 ,-1.04999961429],

      [-0.99999985, 0.0 ,-1.04999952381],

      [-1.0999998, 0.0 ,-1.04999942381],

      [-1.19999974, 0.0 ,-1.04999931429],

      [-1.29999967, 0.0 ,-1.04999919524],

      [-1.39999959, 0.0 ,-1.04999906667],

      [-1.49999949, 0.0, -1.04999892857]

    ],

    "ephemeris_times": [

      999.2,
@@ -225,9 +225,9 @@ 
    ],

    "velocities": [

      [0.125, 0.125, 0.125],

      [0.125, 0.125, 0.12],

      [0.125, 0.125, 0.12],

      [0.125, 0.125, 0.12]

      [0.125, 0.125, 0.125],

      [0.125, 0.125, 0.125],

      [0.125, 0.125, 0.125]

    ],

    "ephemeris_times": [

      999.2,