Unverified Commit 9414c80a authored by Oleg Alexandrov's avatar Oleg Alexandrov Committed by GitHub
Browse files

Add the ability to apply a transform to a linescan model (#340) 

* Add the ablility to apply a rotation and translation to a linescan model state

* Make applyTransformToState static

Conflicts:
	include/usgscsm/UsgsAstroLsSensorModel.h
	src/UsgsAstroLsSensorModel.cpp

* Add a test for applyTransformToState and make it public

Minor tidying

Rm unneeded whitespace

Reword comment
parent 1df52c1f

include/usgscsm/UsgsAstroLsSensorModel.h

+6 −0
Original line number Diff line number Diff line
@@ -154,6 +154,12 @@ class UsgsAstroLsSensorModel : public csm::RasterGM, 


  virtual std::string getModelState() const;



  // Apply a rotation and translation to a state string. The effect is

  // to transform the position and orientation of the camera in ECEF

  // coordinates.

  static void applyTransformToState(ale::Rotation const& r, ale::Vec3d const& t,

                                    std::string& stateString);

  

  // Set the sensor model based on the input state data

  void set(const std::string& state_data);

include/usgscsm/Utilities.h

+8 −0
Original line number Diff line number Diff line
@@ -10,6 +10,9 @@ 


#include <nlohmann/json.hpp>



#include <ale/Rotation.h>

#include <ale/Vectors.h>



#include <Warning.h>

#include <csm.h>
@@ -168,4 +171,9 @@ std::vector<double> getSensorOrientations(nlohmann::json isd, 
double getWavelength(nlohmann::json isd, csm::WarningList *list = nullptr);

nlohmann::json stateAsJson(std::string modelState);



// Apply transforms to orientations and vectors

void applyRotationToQuatVec(ale::Rotation const& r, std::vector<double> & quaternions);

void applyRotationTranslationToXyzVec(ale::Rotation const& r, ale::Vec3d const& t,

                                      std::vector<double> & xyz);



#endif  // INCLUDE_USGSCSM_UTILITIES_H_

src/UsgsAstroLsSensorModel.cpp

+31 −2
Original line number Diff line number Diff line
@@ -441,6 +441,37 @@ std::string UsgsAstroLsSensorModel::getModelState() const { 
  return stateString;

}



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

// UsgsAstroLineScannerSensorModel::applyTransformToState

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

void UsgsAstroLsSensorModel::applyTransformToState(ale::Rotation const& r, ale::Vec3d const& t,

                                                   std::string& stateString) {



  nlohmann::json j = stateAsJson(stateString);



  // Apply rotation to quaternions

  std::vector<double> quaternions = j["m_quaternions"].get<std::vector<double>>();

  applyRotationToQuatVec(r, quaternions);

  j["m_quaternions"] = quaternions;

  

  // Apply rotation and translation to positions

  std::vector<double> positions = j["m_positions"].get<std::vector<double>>();;

  applyRotationTranslationToXyzVec(r, t, positions);

  j["m_positions"] = positions;

  

  // Apply rotation to velocities. The translation does not get applied. 

  ale::Vec3d zero_t(0, 0, 0);

  std::vector<double> velocities = j["m_velocities"].get<std::vector<double>>();;

  applyRotationTranslationToXyzVec(r, zero_t, velocities);

  j["m_velocities"] = velocities;

  

  // We do not change the Sun position or velocity. The idea is that

  // the Sun is so far, that minor camera adjustments won't affect

  // where the Sun is.



  // Update the state string

  stateString = getModelNameFromModelState(stateString) + "\n" + j.dump();

}

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

// UsgsAstroLineScannerSensorModel::reset

//***************************************************************************
@@ -2233,8 +2264,6 @@ std::string UsgsAstroLsSensorModel::constructStateFromIsd( 
  json jsonIsd = json::parse(imageSupportData);

  csm::WarningList* parsingWarnings = new csm::WarningList;



  int num_params = NUM_PARAMETERS;



  state["m_modelName"] = ale::getSensorModelName(jsonIsd);

  state["m_imageIdentifier"] = ale::getImageId(jsonIsd);

  state["m_sensorName"] = ale::getSensorName(jsonIsd);

src/Utilities.cpp

+46 −0
Original line number Diff line number Diff line
@@ -1323,3 +1323,49 @@ json stateAsJson(std::string modelState) { 
  }

  return json::parse(modelState.begin() + found, modelState.end());

}



// Apply a rotation to a vector of quaternions.

void applyRotationToQuatVec(ale::Rotation const& r, std::vector<double> & quaternions) {

  int num_quat = quaternions.size();

  

  for (int it = 0; it < num_quat/4; it++) {



    double * q = &quaternions[4*it];



    // Note that quaternion in q is stored in order x, y, z, w, while

    // the rotation matrix wants it as w, x, y, z.

    

    std::vector<double> trans_q = (r * ale::Rotation(q[3], q[0], q[1], q[2])).toQuaternion();



    // Modify in-place

    q[0] = trans_q[1];

    q[1] = trans_q[2];

    q[2] = trans_q[3];

    q[3] = trans_q[0];

  }

}



// Apply a rotation and translation to a vector of xyz vectors.

void applyRotationTranslationToXyzVec(ale::Rotation const& r, ale::Vec3d const& t,

                                      std::vector<double> & xyz) {

  

  int num_xyz = xyz.size();

  for (int it = 0; it < num_xyz/3; it++) {

    

    double * p = &xyz[3*it];



    ale::Vec3d p_vec(p[0], p[1], p[2]);

    

    // Apply the rotation

    p_vec = r(p_vec);



    // Apply the translation

    p_vec += t;



    // Modify in-place

    p[0] = p_vec.x;

    p[1] = p_vec.y;

    p[2] = p_vec.z;

    

  }

}

tests/LineScanCameraTests.cpp

+23 −0
Original line number Diff line number Diff line
@@ -22,6 +22,29 @@ TEST_F(ConstVelocityLineScanSensorModel, State) { 
  EXPECT_EQ(sensorModel->getModelState(), modelState);

}



// Apply the identity transform to a state

TEST_F(ConstVelocityLineScanSensorModel, ApplyTransformToState) {

  std::string modelState = sensorModel->getModelState();



  ale::Rotation r; // identity rotation

  ale::Vec3d t;    // zero translation



  // The input state has some "-0" values which get transformed by

  // applying the identity transform into "0", which results in the

  // state string changing. Hence, for this test, compare the results

  // of applying the identity transform once vs twice, which are the

  // same.



  // First application

  UsgsAstroLsSensorModel::applyTransformToState(r, t, modelState);



  // Second application

  std::string modelState2 = modelState;

  UsgsAstroLsSensorModel::applyTransformToState(r, t, modelState2);



  EXPECT_EQ(modelState, modelState2);

}



// Fly by tests



TEST_F(ConstVelocityLineScanSensorModel, Center) {