Commit aaafa013 authored by Kristin's avatar Kristin Committed by Summer Stapleton
Browse files

Pulled losToEcf shared functions out into Utilities class (#174) 

* Initial steps to pull common camera model or math utility functions into a separate Utilities file.

* dos2unix line endings again

* ../include/usgscsm/UsgsAstroLsSensorModel.h

* updates to modularized functions

* dos2unix UsgsAstroFrameSensorModel

* Forgot to actually add the Utilities class

* Keep needed to unix2dos to re-fix line ending caracters

* We have some dos line ending and some unix line endings in our code
parent 24b2382d

CMakeLists.txt

+2 −1
Original line number Diff line number Diff line
@@ -27,7 +27,8 @@ add_library(usgscsm SHARED 
            src/UsgsAstroPlugin.cpp

            src/UsgsAstroFrameSensorModel.cpp

            src/UsgsAstroLsSensorModel.cpp

            src/Distortion.cpp)

            src/Distortion.cpp

	    src/Utilities.cpp)



set_target_properties(usgscsm PROPERTIES

    VERSION ${PROJECT_VERSION}

include/usgscsm/UsgsAstroLsSensorModel.h

+6 −21
Original line number Diff line number Diff line
@@ -31,7 +31,6 @@ 
#include <SettableEllipsoid.h>

#include <CorrelationModel.h>





class UsgsAstroLsSensorModel : public csm::RasterGM, virtual public csm::SettableEllipsoid

{

public:
@@ -916,27 +915,13 @@ private: 
      double* achievedPrecision = NULL,

      csm::WarningList* warnings = NULL) const;



   // methods pulled out of los2ecf and computeViewingPixel



   void computeDistortedFocalPlaneCoordinates(

       const double& line,

       const double& sample,

       double& distortedLine,

       double& distortedSample) const;



   void calculateRotationMatrixFromQuaternions(

       const double& time,

       double cameraToBody[9]) const;



   void calculateRotationMatrixFromEuler(

       double euler[],

       double rotationMatrix[]) const;

   void reconstructSensorDistortion(

     double& focalX,

     double& focalY,

     const double& desiredPrecision) const;



   void createCameraLookVector(

       const double& undistortedFocalPlaneX,

       const double& undistortedFocalPlaneY,

       const std::vector<double>& adj,

       double cameraLook[]) const;

   void getQuaternions(const double& time,

                       double quaternion[4]) const;



   void calculateAttitudeCorrection(

       const double& time,

include/usgscsm/Utilities.h

0 → 100644
+48 −0
Original line number Diff line number Diff line 
#ifndef Utilities_h

#define Utilities_h



#include <vector>

#include <math.h>

#include <tuple>



// methods pulled out of los2ecf and computeViewingPixel



// for now, put everything in here. 

// TODO: later, consider if it makes sense to pull sample/line offsets out

// Compute distorted focalPlane coordinates in mm

std::tuple<double, double> computeDistortedFocalPlaneCoordinates(

  const double& line,

  const double& sample,

  const double& sampleOrigin,

  const double& lineOrigin, 

  const double& sampleSumming,

  const double& startingSample,

  const double& lineOffset,

  const double iTransS[],

  const double iTransL[]);

    

void calculateRotationMatrixFromQuaternions(

  double quaternions[4],

  double cameraToBody[9]);



void calculateRotationMatrixFromEuler(

  double euler[],

  double rotationMatrix[]);



void createCameraLookVector(

  const double& undistortedFocalPlaneX,

  const double& undistortedFocalPlaneY,

  const double& zDirection,

  const double& focalLength,

  const double& focalLengthBias,

  const double& halfSwath,

  double cameraLook[]);



//void calculateAttitudeCorrection(

//  const double& time,

//  

//  double attCorr[9]);

//



#endif

src/UsgsAstroLsSensorModel.cpp

+14 −83
Original line number Diff line number Diff line
@@ -19,6 +19,7 @@ 
#define USGS_SENSOR_LIBRARY



#include "UsgsAstroLsSensorModel.h"

#include "Utilities.h"

#include "Distortion.h"



#include <algorithm>
@@ -1658,92 +1659,18 @@ double UsgsAstroLsSensorModel::getValue( 
// Functions pulled out of losToEcf and computeViewingPixel

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



// Compute distorted focalPlane coordinates in mm

void UsgsAstroLsSensorModel::computeDistortedFocalPlaneCoordinates(const double& line, const double& sample, double& distortedLine, double& distortedSample) const{

  double detSample = (sample - 1.0)

      * m_detectorSampleSumming + m_startingSample;

  double m11 = m_iTransL[1];

  double m12 = m_iTransL[2];

  double m21 = m_iTransS[1];

  double m22 = m_iTransS[2];

  double t1 = line + m_detectorLineOffset

               - m_detectorLineOrigin - m_iTransL[0];

  double t2 = detSample - m_detectorSampleOrigin - m_iTransS[0];

  double determinant = m11 * m22 - m12 * m21;

  double p11 = m11 / determinant;

  double p12 = -m12 / determinant;

  double p21 = -m21 / determinant;

  double p22 = m22 / determinant;

  distortedLine = p11 * t1 + p12 * t2;

  distortedSample = p21 * t1 + p22 * t2;

}





// Define imaging ray in image space (In other words, create a look vector in camera space)

void UsgsAstroLsSensorModel::createCameraLookVector(const double& undistortedFocalPlaneX, const double& undistortedFocalPlaneY, const std::vector<double>& adj, double cameraLook[]) const{

   cameraLook[0] = -undistortedFocalPlaneX * m_zDirection;

   cameraLook[1] = -undistortedFocalPlaneY * m_zDirection;

   cameraLook[2] = -m_focal * (1.0 - getValue(15, adj) / m_halfSwath);

   double magnitude = sqrt(cameraLook[0] * cameraLook[0]

                  + cameraLook[1] * cameraLook[1]

                  + cameraLook[2] * cameraLook[2]);

   cameraLook[0] /= magnitude;

   cameraLook[1] /= magnitude;

   cameraLook[2] /= magnitude;

};





// Given a time and a flag to indicate whether the a->b or b->a rotation should be calculated

// uses the quaternions in the m_quaternions member to calclate a rotation matrix.

void UsgsAstroLsSensorModel::calculateRotationMatrixFromQuaternions(const double& time, double rotationMatrix[9]) const {

void UsgsAstroLsSensorModel::getQuaternions(const double& time, double q[4]) const{

  int nOrder = 8;

  if (m_platformFlag == 0)

     nOrder = 4;

  int nOrderQuat = nOrder;

  if (m_numQuaternions < 6 && nOrder == 8)

     nOrderQuat = 4;

  double q[4];

  lagrangeInterp(

     m_numQuaternions, &m_quaternions[0], m_t0Quat, m_dtQuat,

     time, 4, nOrderQuat, q);

  double norm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);

  q[0] /= norm;

  q[1] /= norm;

  q[2] /= norm;

  q[3] /= norm;



  rotationMatrix[0] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];

  rotationMatrix[1] = 2 * (q[0] * q[1] - q[2] * q[3]);

  rotationMatrix[2] = 2 * (q[0] * q[2] + q[1] * q[3]);

  rotationMatrix[3] = 2 * (q[0] * q[1] + q[2] * q[3]);

  rotationMatrix[4] = -q[0] * q[0] + q[1] * q[1] - q[2] * q[2] + q[3] * q[3];

  rotationMatrix[5] = 2 * (q[1] * q[2] - q[0] * q[3]);

  rotationMatrix[6] = 2 * (q[0] * q[2] - q[1] * q[3]);

  rotationMatrix[7] = 2 * (q[1] * q[2] + q[0] * q[3]);

  rotationMatrix[8] = -q[0] * q[0] - q[1] * q[1] + q[2] * q[2] + q[3] * q[3];

};



// Calculates a rotation matrix from Euler angles

void UsgsAstroLsSensorModel::calculateRotationMatrixFromEuler(double euler[],

                                                              double rotationMatrix[]) const {

  double cos_a = cos(euler[0]);

  double sin_a = sin(euler[0]);

  double cos_b = cos(euler[1]);

  double sin_b = sin(euler[1]);

  double cos_c = cos(euler[2]);

  double sin_c = sin(euler[2]);



  rotationMatrix[0] = cos_b * cos_c;

  rotationMatrix[1] = -cos_a * sin_c + sin_a * sin_b * cos_c;

  rotationMatrix[2] = sin_a * sin_c + cos_a * sin_b * cos_c;

  rotationMatrix[3] = cos_b * sin_c;

  rotationMatrix[4] = cos_a * cos_c + sin_a * sin_b * sin_c;

  rotationMatrix[5] = -sin_a * cos_c + cos_a * sin_b * sin_c;

  rotationMatrix[6] = -sin_b;

  rotationMatrix[7] = sin_a * cos_b;

  rotationMatrix[8] = cos_a * cos_b;

     m_numQuaternions, &m_quaternions[0], m_t0Quat, m_dtQuat, time, 4, nOrderQuat, q);

}





void UsgsAstroLsSensorModel::calculateAttitudeCorrection(const double& time, const std::vector<double>& adj, double attCorr[9]) const {

  double aTime = time - m_t0Quat;

  double euler[3];
@@ -1790,16 +1717,16 @@ void UsgsAstroLsSensorModel::losToEcf( 
   double fractionalLine = line - floor(line);



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

   double natFocalPlaneX, natFocalPlaneY;

   computeDistortedFocalPlaneCoordinates(fractionalLine, sampleUSGSFull, natFocalPlaneX, natFocalPlaneY);

   std::tuple<double, double> natFocalPlane;

   natFocalPlane = computeDistortedFocalPlaneCoordinates(fractionalLine, sampleUSGSFull, m_detectorSampleOrigin, m_detectorLineOrigin, m_detectorSampleSumming, m_startingSample, m_detectorLineOffset, m_iTransS, m_iTransL);



   // Remove lens distortion

   std::tuple<double, double> undistortedPoint;

   undistortedPoint = removeDistortion(natFocalPlaneX, natFocalPlaneY, m_opticalDistCoef);

   undistortedPoint = removeDistortion(std::get<0>(natFocalPlane), std::get<1>(natFocalPlane), m_opticalDistCoef);



  // Define imaging ray (look vector) in camera space

   double cameraLook[3];

   createCameraLookVector(std::get<0>(undistortedPoint), std::get<1>(undistortedPoint), adj, cameraLook);

   createCameraLookVector(std::get<0>(undistortedPoint), std::get<1>(undistortedPoint), m_zDirection, m_focal, getValue(15, adj), m_halfSwath, cameraLook); 



   // Apply attitude correction

   double attCorr[9];
@@ -1817,8 +1744,10 @@ void UsgsAstroLsSensorModel::losToEcf( 
                          + attCorr[8] * cameraLook[2];



// Rotate the look vector into the body fixed frame from the camera reference frame by applying the rotation matrix from the sensor quaternions

   double quaternions[4];

   getQuaternions(time, quaternions); 

   double cameraToBody[9];

   calculateRotationMatrixFromQuaternions(time, cameraToBody);

   calculateRotationMatrixFromQuaternions(quaternions, cameraToBody);



   bodyLookX = cameraToBody[0] * correctedCameraLook[0]

             + cameraToBody[1] * correctedCameraLook[1]
@@ -2354,8 +2283,10 @@ csm::ImageCoord UsgsAstroLsSensorModel::computeViewingPixel( 
   double bodyLookZ = groundPoint.z - zc;



   // Rotate the look vector into the camera reference frame

   double quaternions[4];

   getQuaternions(time, quaternions); 

   double bodyToCamera[9];

   calculateRotationMatrixFromQuaternions(time, bodyToCamera);

   calculateRotationMatrixFromQuaternions(quaternions, bodyToCamera);



   // Apply transpose of matrix to rotate body->camera

   double cameraLookX = bodyToCamera[0] * bodyLookX

src/Utilities.cpp

0 → 100644
+90 −0
Original line number Diff line number Diff line 
#include "Utilities.h"



// Calculates a rotation matrix from Euler angles

void calculateRotationMatrixFromEuler(double euler[],

                                      double rotationMatrix[]) {

  double cos_a = cos(euler[0]);

  double sin_a = sin(euler[0]);

  double cos_b = cos(euler[1]);

  double sin_b = sin(euler[1]);

  double cos_c = cos(euler[2]);

  double sin_c = sin(euler[2]);



  rotationMatrix[0] = cos_b * cos_c;

  rotationMatrix[1] = -cos_a * sin_c + sin_a * sin_b * cos_c;

  rotationMatrix[2] = sin_a * sin_c + cos_a * sin_b * cos_c;

  rotationMatrix[3] = cos_b * sin_c;

  rotationMatrix[4] = cos_a * cos_c + sin_a * sin_b * sin_c;

  rotationMatrix[5] = -sin_a * cos_c + cos_a * sin_b * sin_c;

  rotationMatrix[6] = -sin_b;

  rotationMatrix[7] = sin_a * cos_b;

  rotationMatrix[8] = cos_a * cos_b;

}





// uses a quaternion to calclate a rotation matrix.

void calculateRotationMatrixFromQuaternions(double q[4], double rotationMatrix[9]) {

  double norm = sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);

  q[0] /= norm;

  q[1] /= norm;

  q[2] /= norm;

  q[3] /= norm;



  rotationMatrix[0] = q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];

  rotationMatrix[1] = 2 * (q[0] * q[1] - q[2] * q[3]);

  rotationMatrix[2] = 2 * (q[0] * q[2] + q[1] * q[3]);

  rotationMatrix[3] = 2 * (q[0] * q[1] + q[2] * q[3]);

  rotationMatrix[4] = -q[0] * q[0] + q[1] * q[1] - q[2] * q[2] + q[3] * q[3];

  rotationMatrix[5] = 2 * (q[1] * q[2] - q[0] * q[3]);

  rotationMatrix[6] = 2 * (q[0] * q[2] - q[1] * q[3]);

  rotationMatrix[7] = 2 * (q[1] * q[2] + q[0] * q[3]);

  rotationMatrix[8] = -q[0] * q[0] - q[1] * q[1] + q[2] * q[2] + q[3] * q[3];

}



std::tuple<double, double> computeDistortedFocalPlaneCoordinates(

  const double& line,

  const double& sample,

  const double& sampleOrigin,

  const double& lineOrigin, 

  const double& sampleSumming,

  const double& startingSample,

  const double& lineOffset,

  const double iTransS[],

  const double iTransL[]) {

  double detSample = (sample - 1.0) * sampleSumming + startingSample;

  double m11 = iTransL[1];

  double m12 = iTransL[2];

  double m21 = iTransS[1];

  double m22 = iTransS[2];

  double t1 = line + lineOffset - lineOrigin - iTransL[0];

  double t2 = detSample - sampleOrigin - iTransS[0];

  double determinant = m11 * m22 - m12 * m21;

  double p11 = m11 / determinant;

  double p12 = -m12 / determinant;

  double p21 = -m21 / determinant;

  double p22 = m22 / determinant;



  double distortedLine = p11 * t1 + p12 * t2;

  double distortedSample = p21 * t1 + p22 * t2;

  return std::make_tuple(distortedLine, distortedSample);  

};



// Define imaging ray in image space (In other words, create a look vector in camera space)

void createCameraLookVector(

  const double& undistortedFocalPlaneX,

  const double& undistortedFocalPlaneY,

  const double& zDirection,

  const double& focalLength,

  const double& focalLengthBias,

  const double& halfSwath,

  double cameraLook[]) {

   cameraLook[0] = -undistortedFocalPlaneX * zDirection;

   cameraLook[1] = -undistortedFocalPlaneY * zDirection;

   cameraLook[2] = -focalLength * (1.0 - focalLengthBias / halfSwath);

   double magnitude = sqrt(cameraLook[0] * cameraLook[0]

                  + cameraLook[1] * cameraLook[1]

                  + cameraLook[2] * cameraLook[2]);

   cameraLook[0] /= magnitude;

   cameraLook[1] /= magnitude;

   cameraLook[2] /= magnitude;

}

src/UsgsAstroFrameSensorModel.cpp

+1 −1

File changed.

Contains only whitespace changes.