diff --git a/include/usgscsm/Distortion.h b/include/usgscsm/Distortion.h index fccc1ed69..17d1a9142 100644 --- a/include/usgscsm/Distortion.h +++ b/include/usgscsm/Distortion.h @@ -1,19 +1,12 @@ #ifndef Distortion_h #define Distortion_h -#include #include -#include #include +#include +#include -enum DistortionType { - RADIAL, - TRANSVERSE, - KAGUYALISM, - DAWNFC, - LROLROCNAC -}; - +enum DistortionType { RADIAL, TRANSVERSE, KAGUYALISM, DAWNFC, LROLROCNAC }; // Transverse Distortion void distortionJacobian(double x, double y, double *jacobian, @@ -30,5 +23,6 @@ void removeDistortion(double dx, double dy, double &ux, double &uy, void applyDistortion(double ux, double uy, double &dx, double &dy, const std::vector opticalDistCoeffs, DistortionType distortionType, - const double desiredPrecision = 1.0E-6, const double tolerance = 1.0E-6); + const double desiredPrecision = 1.0E-6, + const double tolerance = 1.0E-6); #endif diff --git a/include/usgscsm/UsgsAstroFrameSensorModel.h b/include/usgscsm/UsgsAstroFrameSensorModel.h index 2b1376c9f..14ceea2dd 100644 --- a/include/usgscsm/UsgsAstroFrameSensorModel.h +++ b/include/usgscsm/UsgsAstroFrameSensorModel.h @@ -2,393 +2,395 @@ #ifndef UsgsAstroFrameSensorModel_h #define UsgsAstroFrameSensorModel_h +#include #include #include #include -#include "RasterGM.h" -#include #include "CorrelationModel.h" #include "Distortion.h" +#include "RasterGM.h" #include "Utilities.h" #include "spdlog/spdlog.h" - -class UsgsAstroFrameSensorModel : public csm::RasterGM, virtual public csm::SettableEllipsoid { +class UsgsAstroFrameSensorModel : public csm::RasterGM, + virtual public csm::SettableEllipsoid { // UsgsAstroFramePlugin needs to access private members friend class UsgsAstroFramePlugin; - public: - UsgsAstroFrameSensorModel(); - ~UsgsAstroFrameSensorModel(); - - - bool isValidModelState(const std::string& stringState, csm::WarningList *warnings); - bool isValidIsd(const std::string& stringIsd, csm::WarningList *warnings); - - virtual csm::ImageCoord groundToImage(const csm::EcefCoord &groundPt, - double desiredPrecision=0.001, - double *achievedPrecision=NULL, - csm::WarningList *warnings=NULL) const; - - - std::string constructStateFromIsd(const std::string& jsonIsd, csm::WarningList *warnings); - void reset(); - - virtual csm::ImageCoordCovar groundToImage(const csm::EcefCoordCovar &groundPt, - double desiredPrecision=0.001, - double *achievedPrecision=NULL, - csm::WarningList *warnings=NULL) const; - - virtual csm::ImageCoord groundToImage(const csm::EcefCoord& ground_pt, - const std::vector& adjustments, - double desired_precision=0.001, - double* achieved_precision=NULL, - csm::WarningList* warnings=NULL) const; - - /** - * This function determines if a sample, line intersects the target body and if so, where - * this intersection occurs in body-fixed coordinates. - * - * @param sample Sample of the input image. - * @param line Line of the input image. - * @param height ??? - * - * @return @b vector Returns the body-fixed X,Y,Z coordinates of the intersection. - * If no intersection, returns a 3-element vector of 0's. - */ - virtual csm::EcefCoord imageToGround(const csm::ImageCoord &imagePt, double height = 0.0, - double desiredPrecision=0.001, double *achievedPrecision=NULL, - csm::WarningList *warnings=NULL) const; - - virtual csm::EcefCoordCovar imageToGround(const csm::ImageCoordCovar &imagePt, double height, - double heightVariance, double desiredPrecision=0.001, - double *achievedPrecision=NULL, - csm::WarningList *warnings=NULL) const; - - virtual csm::EcefLocus imageToProximateImagingLocus(const csm::ImageCoord &imagePt, - const csm::EcefCoord &groundPt, - double desiredPrecision=0.001, - double *achievedPrecision=NULL, - csm::WarningList *warnings=NULL) const; - - virtual csm::EcefLocus imageToRemoteImagingLocus(const csm::ImageCoord &imagePt, - double desiredPrecision=0.001, - double *achievedPrecision=NULL, - csm::WarningList *warnings=NULL) const; - - virtual csm::ImageCoord getImageStart() const; - - virtual csm::ImageVector getImageSize() const; - - virtual std::pair getValidImageRange() const; - - virtual std::pair getValidHeightRange() const; - - /** - * Calculates the illumination vector (body-fixed, meters) from the sun to the given ground - * point. - * - * @param groundPt The ground point to find the illumination vector to. - * - * @return @b csm::EcefVector Returns the illumination vector from the sun to the ground point. - */ - virtual csm::EcefVector getIlluminationDirection(const csm::EcefCoord &groundPt) const; - - virtual double getImageTime(const csm::ImageCoord &imagePt) const; - - /** - * Determines the body-fixed sensor position for the given image coordinate. - * - * @param imagePt Image coordinate to find the sensor position for. - * - * @return @b csm::EcefCoord Returns the body-fixed sensor position. - * - * @throw csm::Error::BOUNDS "Image coordinate () out of bounds." - */ - virtual csm::EcefCoord getSensorPosition(const csm::ImageCoord &imagePt) const; - - virtual csm::EcefCoord getSensorPosition(double time) const; - - /** - * Determines the velocity of the sensor for the given image coordinate (in body-fixed frame). - * - * @param imagePt Image coordinate to find the sensor position for. - * - * @return @b csm::EcefVector Returns the sensor velocity in body-fixed frame. - * - * @throw csm::Error::BOUNDS "Image coordinate () out of bounds." - */ - - virtual csm::EcefVector getSensorVelocity(const csm::ImageCoord &imagePt) const; - - virtual csm::EcefVector getSensorVelocity(double time) const; - - virtual csm::RasterGM::SensorPartials computeSensorPartials(int index, - const csm::EcefCoord &groundPt, - double desiredPrecision=0.001, - double *achievedPrecision=NULL, - csm::WarningList *warnings=NULL) const; - - virtual csm::RasterGM::SensorPartials computeSensorPartials(int index, - const csm::ImageCoord &imagePt, - const csm::EcefCoord &groundPt, - double desiredPrecision=0.001, - double *achievedPrecision=NULL, - csm::WarningList *warnings=NULL) const; - - virtual std::vector computeAllSensorPartials (const csm::EcefCoord& groundPt, - csm::param::Set pSet = csm::param::VALID, - double desiredPrecision = 0.001, - double *achievedPrecision = NULL, - csm::WarningList* warnings = NULL) const; - - virtual std::vector computeAllSensorPartials (const csm::ImageCoord& imagePt, - const csm::EcefCoord& groundPt, - csm::param::Set pSet = csm::param::VALID, - double desiredPrecision = 0.001, - double *achievedPrecision = NULL, - csm::WarningList* warnings = NULL) const; - - virtual std::vector computeGroundPartials(const csm::EcefCoord &groundPt) const; - - virtual const csm::CorrelationModel &getCorrelationModel() const; - - virtual std::vector getUnmodeledCrossCovariance(const csm::ImageCoord &pt1, - const csm::ImageCoord &pt2) const; - - // IMPLEMENT MODEL PURE VIRTUALS - //--- - // Basic model information - //--- - virtual csm::Version getVersion() const; - //> This method returns the version of the model code. The Version - // object can be compared to other Version objects with its comparison - // operators. Not to be confused with the CSM API version. - //< - - virtual std::string getModelName() const; - //> This method returns a string identifying the name of the model. - //< - - virtual std::string getPedigree() const; - //> This method returns a string that identifies the sensor, - // the model type, its mode of acquisition and processing path. - // For example, an optical sensor model or a cubic rational polynomial - // model created from the same sensor's support data would produce - // different pedigrees for each case. - //< - - //--- - // Basic collection information - //--- - virtual std::string getImageIdentifier() const; - //> This method returns an identifier to uniquely indicate the imaging - // operation associated with this model. - // This is the primary identifier of the model. - // - // This method may return an empty string if the ID is unknown. - //< - - virtual void setImageIdentifier(const std::string& imageId, - csm::WarningList* warnings = NULL); - //> This method sets an identifier to uniquely indicate the imaging - // operation associated with this model. Typically used for models - // whose initialization does not produce an adequate identifier. - // - // If a non-NULL warnings argument is received, it will be populated - // as applicable. - //< - - virtual std::string getSensorIdentifier() const; - //> This method returns an identifier to indicate the specific sensor - // that was used to acquire the image. This ID must be unique among - // sensors for a given model name. It is used to determine parameter - // correlation and sharing. Equivalent to camera or mission ID. - // - // This method may return an empty string if the sensor ID is unknown. - //< - - virtual std::string getPlatformIdentifier() const; - //> This method returns an identifier to indicate the specific platform - // that was used to acquire the image. This ID must unique among - // platforms for a given model name. It is used to determine parameter - // correlation sharing. Equivalent to vehicle or aircraft tail number. - // - // This method may return an empty string if the platform ID is unknown. - //< - - virtual std::string getCollectionIdentifier() const; - //> This method returns an identifer to indicate a collection activity - // common to a set of images. This ID must be unique among collection - // activities for a given model name. It is used to determine parameter - // correlation and sharing. - //< - - virtual std::string getTrajectoryIdentifier() const; - //> This method returns an identifier to indicate a trajectory common - // to a set of images. This ID must be unique among trajectories - // for a given model name. It is used to determine parameter - // correlation and sharing. - //< - - virtual std::string getSensorType() const; - //> This method returns a description of the sensor type (EO, IR, SAR, - // etc). See csm.h for a list of common types. Should return - // CSM_SENSOR_TYPE_UNKNOWN if the sensor type is unknown. - //< - - virtual std::string getSensorMode() const; - //> This method returns a description of the sensor mode (FRAME, - // PUSHBROOM, SPOT, SCAN, etc). See csm.h for a list of common modes. - // Should return CSM_SENSOR_MODE_UNKNOWN if the sensor mode is unknown. - //< - - virtual std::string getReferenceDateAndTime() const; - //> This method returns an approximate date and time at which the - // image was taken. The returned string follows the ISO 8601 standard. - // - //- Precision Format Example - //- year yyyy "1961" - //- month yyyymm "196104" - //- day yyyymmdd "19610420" - //- hour yyyymmddThh "19610420T20" - //- minute yyyymmddThhmm "19610420T2000" - //- second yyyymmddThhmmss "19610420T200000" - //< - - //--- - // Sensor Model State - //--- - virtual std::string getModelState() const; - //> This method returns a string containing the data to exactly recreate - // the current model. It can be used to restore this model to a - // previous state with the replaceModelState method or create a new - // model object that is identical to this model. - // The string could potentially be saved to a file for later use. - // An empty string is returned if it is not possible to save the - // current state. - //< - - virtual void replaceModelState(const std::string& argState); - //> This method attempts to initialize the current model with the state - // given by argState. The argState argument can be a string previously - // retrieved from the getModelState method. - // - // If argState contains a valid state for the current model, - // the internal state of the model is updated. - // - // If the model cannot be updated to the given state, a csm::Error is - // thrown and the internal state of the model is undefined. - // - // If the argument state string is empty, the model remains unchanged. - //< - - // Implement methods from the SettableEllipsoid class - - virtual csm::Ellipsoid getEllipsoid() const; - //> This method returns the planetary ellipsoid. - //< - - virtual void setEllipsoid(const csm::Ellipsoid &ellipsoid); - //> This method sets the planetary ellipsoid. - //< - - // IMPLEMENT GEOMETRICMODEL PURE VIRTUALS - // See GeometricModel.h for documentation - virtual csm::EcefCoord getReferencePoint() const; - virtual void setReferencePoint(const csm::EcefCoord &groundPt); - virtual int getNumParameters() const; - virtual std::string getParameterName(int index) const; - virtual std::string getParameterUnits(int index) const; - virtual bool hasShareableParameters() const; - virtual bool isParameterShareable(int index) const; - virtual csm::SharingCriteria getParameterSharingCriteria(int index) const; - virtual double getParameterValue(int index) const; - virtual void setParameterValue(int index, double value); - virtual csm::param::Type getParameterType(int index) const; - virtual void setParameterType(int index, csm::param::Type pType); - virtual double getParameterCovariance(int index1, int index2) const; - virtual void setParameterCovariance(int index1, int index2, double covariance); - virtual int getNumGeometricCorrectionSwitches() const; - virtual std::string getGeometricCorrectionName(int index) const; - virtual void setGeometricCorrectionSwitch(int index, bool value, csm::param::Type pType); - virtual bool getGeometricCorrectionSwitch(int index) const; - virtual std::vector getCrossCovarianceMatrix( - const GeometricModel &comparisonModel, - csm::param::Set pSet = csm::param::VALID, - const GeometricModelList &otherModels = GeometricModelList()) const; - virtual std::shared_ptr getLogger(); - virtual void setLogger(std::string logName); - double getValue(int index, const std::vector &adjustments) const; - void calcRotationMatrix(double m[3][3]) const; - void calcRotationMatrix(double m[3][3], const std::vector &adjustments) const; - - void losEllipsoidIntersect (double height,double xc, - double yc, double zc, - double xl, double yl, - double zl, - double& x,double& y, double& z) const; - - static const std::string _SENSOR_MODEL_NAME; - - private: - // Input parameters - static const int m_numParameters; - static const std::string m_parameterName[]; - std::vector m_currentParameterValue; - std::vector m_currentParameterCovariance; - std::vector m_parameterType; - std::vector m_noAdjustments; - DistortionType m_distortionType; - std::vector m_opticalDistCoeffs; - std::vector m_transX; - std::vector m_transY; - std::vector m_spacecraftVelocity; - std::vector m_sunPosition; - std::vector m_ccdCenter; - std::vector m_iTransS; - std::vector m_iTransL; - std::vector m_boresight; - double m_majorAxis; - double m_minorAxis; - double m_focalLength; - double m_minElevation; - double m_maxElevation; - double m_startingDetectorSample; - double m_startingDetectorLine; - double m_detectorSampleSumming; - double m_detectorLineSumming; - std::string m_targetName; - std::string m_modelName; - std::string m_sensorName; - std::string m_platformName; - std::string m_imageIdentifier; - std::string m_collectionIdentifier; - double m_ifov; - std::string m_instrumentID; - double m_focalLengthEpsilon; - double m_originalHalfLines; - std::string m_spacecraftName; - double m_pixelPitch; - - double m_ephemerisTime; - double m_originalHalfSamples; - int m_nLines; - int m_nSamples; - int m_nParameters; - - csm::EcefCoord m_referencePointXyz; - - std::shared_ptr m_logger = spdlog::get("usgscsm_logger"); - - nlohmann::json _state; - static const int _NUM_STATE_KEYWORDS; - static const int NUM_PARAMETERS; - static const std::string _STATE_KEYWORD[]; - - csm::NoCorrelationModel _no_corr_model; - + public: + UsgsAstroFrameSensorModel(); + ~UsgsAstroFrameSensorModel(); + + bool isValidModelState(const std::string &stringState, + csm::WarningList *warnings); + bool isValidIsd(const std::string &stringIsd, csm::WarningList *warnings); + + virtual csm::ImageCoord groundToImage( + const csm::EcefCoord &groundPt, double desiredPrecision = 0.001, + double *achievedPrecision = NULL, + csm::WarningList *warnings = NULL) const; + + std::string constructStateFromIsd(const std::string &jsonIsd, + csm::WarningList *warnings); + void reset(); + + virtual csm::ImageCoordCovar groundToImage( + const csm::EcefCoordCovar &groundPt, double desiredPrecision = 0.001, + double *achievedPrecision = NULL, + csm::WarningList *warnings = NULL) const; + + virtual csm::ImageCoord groundToImage( + const csm::EcefCoord &ground_pt, const std::vector &adjustments, + double desired_precision = 0.001, double *achieved_precision = NULL, + csm::WarningList *warnings = NULL) const; + + /** + * This function determines if a sample, line intersects the target body and + * if so, where this intersection occurs in body-fixed coordinates. + * + * @param sample Sample of the input image. + * @param line Line of the input image. + * @param height ??? + * + * @return @b vector Returns the body-fixed X,Y,Z coordinates of the + * intersection. If no intersection, returns a 3-element vector of 0's. + */ + virtual csm::EcefCoord imageToGround(const csm::ImageCoord &imagePt, + double height = 0.0, + double desiredPrecision = 0.001, + double *achievedPrecision = NULL, + csm::WarningList *warnings = NULL) const; + + virtual csm::EcefCoordCovar imageToGround( + const csm::ImageCoordCovar &imagePt, double height, double heightVariance, + double desiredPrecision = 0.001, double *achievedPrecision = NULL, + csm::WarningList *warnings = NULL) const; + + virtual csm::EcefLocus imageToProximateImagingLocus( + const csm::ImageCoord &imagePt, const csm::EcefCoord &groundPt, + double desiredPrecision = 0.001, double *achievedPrecision = NULL, + csm::WarningList *warnings = NULL) const; + + virtual csm::EcefLocus imageToRemoteImagingLocus( + const csm::ImageCoord &imagePt, double desiredPrecision = 0.001, + double *achievedPrecision = NULL, + csm::WarningList *warnings = NULL) const; + + virtual csm::ImageCoord getImageStart() const; + + virtual csm::ImageVector getImageSize() const; + + virtual std::pair getValidImageRange() + const; + + virtual std::pair getValidHeightRange() const; + + /** + * Calculates the illumination vector (body-fixed, meters) from the sun to the + * given ground point. + * + * @param groundPt The ground point to find the illumination vector to. + * + * @return @b csm::EcefVector Returns the illumination vector from the sun to + * the ground point. + */ + virtual csm::EcefVector getIlluminationDirection( + const csm::EcefCoord &groundPt) const; + + virtual double getImageTime(const csm::ImageCoord &imagePt) const; + + /** + * Determines the body-fixed sensor position for the given image coordinate. + * + * @param imagePt Image coordinate to find the sensor position for. + * + * @return @b csm::EcefCoord Returns the body-fixed sensor position. + * + * @throw csm::Error::BOUNDS "Image coordinate () out of bounds." + */ + virtual csm::EcefCoord getSensorPosition( + const csm::ImageCoord &imagePt) const; + + virtual csm::EcefCoord getSensorPosition(double time) const; + + /** + * Determines the velocity of the sensor for the given image coordinate (in + * body-fixed frame). + * + * @param imagePt Image coordinate to find the sensor position for. + * + * @return @b csm::EcefVector Returns the sensor velocity in body-fixed frame. + * + * @throw csm::Error::BOUNDS "Image coordinate () out of bounds." + */ + + virtual csm::EcefVector getSensorVelocity( + const csm::ImageCoord &imagePt) const; + + virtual csm::EcefVector getSensorVelocity(double time) const; + + virtual csm::RasterGM::SensorPartials computeSensorPartials( + int index, const csm::EcefCoord &groundPt, + double desiredPrecision = 0.001, double *achievedPrecision = NULL, + csm::WarningList *warnings = NULL) const; + + virtual csm::RasterGM::SensorPartials computeSensorPartials( + int index, const csm::ImageCoord &imagePt, const csm::EcefCoord &groundPt, + double desiredPrecision = 0.001, double *achievedPrecision = NULL, + csm::WarningList *warnings = NULL) const; + + virtual std::vector computeAllSensorPartials( + const csm::EcefCoord &groundPt, csm::param::Set pSet = csm::param::VALID, + double desiredPrecision = 0.001, double *achievedPrecision = NULL, + csm::WarningList *warnings = NULL) const; + + virtual std::vector computeAllSensorPartials( + const csm::ImageCoord &imagePt, const csm::EcefCoord &groundPt, + csm::param::Set pSet = csm::param::VALID, double desiredPrecision = 0.001, + double *achievedPrecision = NULL, + csm::WarningList *warnings = NULL) const; + + virtual std::vector computeGroundPartials( + const csm::EcefCoord &groundPt) const; + + virtual const csm::CorrelationModel &getCorrelationModel() const; + + virtual std::vector getUnmodeledCrossCovariance( + const csm::ImageCoord &pt1, const csm::ImageCoord &pt2) const; + + // IMPLEMENT MODEL PURE VIRTUALS + //--- + // Basic model information + //--- + virtual csm::Version getVersion() const; + //> This method returns the version of the model code. The Version + // object can be compared to other Version objects with its comparison + // operators. Not to be confused with the CSM API version. + //< + + virtual std::string getModelName() const; + //> This method returns a string identifying the name of the model. + //< + + virtual std::string getPedigree() const; + //> This method returns a string that identifies the sensor, + // the model type, its mode of acquisition and processing path. + // For example, an optical sensor model or a cubic rational polynomial + // model created from the same sensor's support data would produce + // different pedigrees for each case. + //< + + //--- + // Basic collection information + //--- + virtual std::string getImageIdentifier() const; + //> This method returns an identifier to uniquely indicate the imaging + // operation associated with this model. + // This is the primary identifier of the model. + // + // This method may return an empty string if the ID is unknown. + //< + + virtual void setImageIdentifier(const std::string &imageId, + csm::WarningList *warnings = NULL); + //> This method sets an identifier to uniquely indicate the imaging + // operation associated with this model. Typically used for models + // whose initialization does not produce an adequate identifier. + // + // If a non-NULL warnings argument is received, it will be populated + // as applicable. + //< + + virtual std::string getSensorIdentifier() const; + //> This method returns an identifier to indicate the specific sensor + // that was used to acquire the image. This ID must be unique among + // sensors for a given model name. It is used to determine parameter + // correlation and sharing. Equivalent to camera or mission ID. + // + // This method may return an empty string if the sensor ID is unknown. + //< + + virtual std::string getPlatformIdentifier() const; + //> This method returns an identifier to indicate the specific platform + // that was used to acquire the image. This ID must unique among + // platforms for a given model name. It is used to determine parameter + // correlation sharing. Equivalent to vehicle or aircraft tail number. + // + // This method may return an empty string if the platform ID is unknown. + //< + + virtual std::string getCollectionIdentifier() const; + //> This method returns an identifer to indicate a collection activity + // common to a set of images. This ID must be unique among collection + // activities for a given model name. It is used to determine parameter + // correlation and sharing. + //< + + virtual std::string getTrajectoryIdentifier() const; + //> This method returns an identifier to indicate a trajectory common + // to a set of images. This ID must be unique among trajectories + // for a given model name. It is used to determine parameter + // correlation and sharing. + //< + + virtual std::string getSensorType() const; + //> This method returns a description of the sensor type (EO, IR, SAR, + // etc). See csm.h for a list of common types. Should return + // CSM_SENSOR_TYPE_UNKNOWN if the sensor type is unknown. + //< + + virtual std::string getSensorMode() const; + //> This method returns a description of the sensor mode (FRAME, + // PUSHBROOM, SPOT, SCAN, etc). See csm.h for a list of common modes. + // Should return CSM_SENSOR_MODE_UNKNOWN if the sensor mode is unknown. + //< + + virtual std::string getReferenceDateAndTime() const; + //> This method returns an approximate date and time at which the + // image was taken. The returned string follows the ISO 8601 standard. + // + //- Precision Format Example + //- year yyyy "1961" + //- month yyyymm "196104" + //- day yyyymmdd "19610420" + //- hour yyyymmddThh "19610420T20" + //- minute yyyymmddThhmm "19610420T2000" + //- second yyyymmddThhmmss "19610420T200000" + //< + + //--- + // Sensor Model State + //--- + virtual std::string getModelState() const; + //> This method returns a string containing the data to exactly recreate + // the current model. It can be used to restore this model to a + // previous state with the replaceModelState method or create a new + // model object that is identical to this model. + // The string could potentially be saved to a file for later use. + // An empty string is returned if it is not possible to save the + // current state. + //< + + virtual void replaceModelState(const std::string &argState); + //> This method attempts to initialize the current model with the state + // given by argState. The argState argument can be a string previously + // retrieved from the getModelState method. + // + // If argState contains a valid state for the current model, + // the internal state of the model is updated. + // + // If the model cannot be updated to the given state, a csm::Error is + // thrown and the internal state of the model is undefined. + // + // If the argument state string is empty, the model remains unchanged. + //< + + // Implement methods from the SettableEllipsoid class + + virtual csm::Ellipsoid getEllipsoid() const; + //> This method returns the planetary ellipsoid. + //< + + virtual void setEllipsoid(const csm::Ellipsoid &ellipsoid); + //> This method sets the planetary ellipsoid. + //< + + // IMPLEMENT GEOMETRICMODEL PURE VIRTUALS + // See GeometricModel.h for documentation + virtual csm::EcefCoord getReferencePoint() const; + virtual void setReferencePoint(const csm::EcefCoord &groundPt); + virtual int getNumParameters() const; + virtual std::string getParameterName(int index) const; + virtual std::string getParameterUnits(int index) const; + virtual bool hasShareableParameters() const; + virtual bool isParameterShareable(int index) const; + virtual csm::SharingCriteria getParameterSharingCriteria(int index) const; + virtual double getParameterValue(int index) const; + virtual void setParameterValue(int index, double value); + virtual csm::param::Type getParameterType(int index) const; + virtual void setParameterType(int index, csm::param::Type pType); + virtual double getParameterCovariance(int index1, int index2) const; + virtual void setParameterCovariance(int index1, int index2, + double covariance); + virtual int getNumGeometricCorrectionSwitches() const; + virtual std::string getGeometricCorrectionName(int index) const; + virtual void setGeometricCorrectionSwitch(int index, bool value, + csm::param::Type pType); + virtual bool getGeometricCorrectionSwitch(int index) const; + virtual std::vector getCrossCovarianceMatrix( + const GeometricModel &comparisonModel, + csm::param::Set pSet = csm::param::VALID, + const GeometricModelList &otherModels = GeometricModelList()) const; + virtual std::shared_ptr getLogger(); + virtual void setLogger(std::string logName); + double getValue(int index, const std::vector &adjustments) const; + void calcRotationMatrix(double m[3][3]) const; + void calcRotationMatrix(double m[3][3], + const std::vector &adjustments) const; + + void losEllipsoidIntersect(double height, double xc, double yc, double zc, + double xl, double yl, double zl, double &x, + double &y, double &z) const; + + static const std::string _SENSOR_MODEL_NAME; + + private: + // Input parameters + static const int m_numParameters; + static const std::string m_parameterName[]; + std::vector m_currentParameterValue; + std::vector m_currentParameterCovariance; + std::vector m_parameterType; + std::vector m_noAdjustments; + DistortionType m_distortionType; + std::vector m_opticalDistCoeffs; + std::vector m_transX; + std::vector m_transY; + std::vector m_spacecraftVelocity; + std::vector m_sunPosition; + std::vector m_ccdCenter; + std::vector m_iTransS; + std::vector m_iTransL; + std::vector m_boresight; + double m_majorAxis; + double m_minorAxis; + double m_focalLength; + double m_minElevation; + double m_maxElevation; + double m_startingDetectorSample; + double m_startingDetectorLine; + double m_detectorSampleSumming; + double m_detectorLineSumming; + std::string m_targetName; + std::string m_modelName; + std::string m_sensorName; + std::string m_platformName; + std::string m_imageIdentifier; + std::string m_collectionIdentifier; + double m_ifov; + std::string m_instrumentID; + double m_focalLengthEpsilon; + double m_originalHalfLines; + std::string m_spacecraftName; + double m_pixelPitch; + + double m_ephemerisTime; + double m_originalHalfSamples; + int m_nLines; + int m_nSamples; + int m_nParameters; + + csm::EcefCoord m_referencePointXyz; + + std::shared_ptr m_logger = spdlog::get("usgscsm_logger"); + + nlohmann::json _state; + static const int _NUM_STATE_KEYWORDS; + static const int NUM_PARAMETERS; + static const std::string _STATE_KEYWORD[]; + + csm::NoCorrelationModel _no_corr_model; }; #endif diff --git a/include/usgscsm/UsgsAstroLsSensorModel.h b/include/usgscsm/UsgsAstroLsSensorModel.h index f06e6b341..bbb5feb7a 100644 --- a/include/usgscsm/UsgsAstroLsSensorModel.h +++ b/include/usgscsm/UsgsAstroLsSensorModel.h @@ -2,7 +2,8 @@ // // UNCLASSIFIED // -// Copyright © 1989-2017 BAE Systems Information and Electronic Systems Integration Inc. +// Copyright © 1989-2017 BAE Systems Information and Electronic Systems +// Integration Inc. // ALL RIGHTS RESERVED // Use of this software product is governed by the terms of a license // agreement. The license agreement is found in the installation directory. @@ -27,1045 +28,979 @@ #ifndef __USGS_ASTRO_LINE_SCANNER_SENSORMODEL_H #define __USGS_ASTRO_LINE_SCANNER_SENSORMODEL_H +#include #include #include -#include #include "Distortion.h" #include "ale/Distortion.h" -#include "ale/States.h" #include "ale/Orientations.h" +#include "ale/States.h" #include "spdlog/spdlog.h" -class UsgsAstroLsSensorModel : public csm::RasterGM, virtual public csm::SettableEllipsoid -{ -public: - - // Initializes the class from state data as formatted - // in a string by the toString() method - void setState(const std::string &state); - - - virtual void replaceModelState(const std::string& stateString); - //> This method attempts to initialize the current model with the state - // given by argState. The argState argument can be a string previously - // retrieved from the getModelState method. - // - // If argState contains a valid state for the current model, - // the internal state of the model is updated. - // - // If the model cannot be updated to the given state, a csm::Error is - // thrown and the internal state of the model is undefined. - // - // If the argument state string is empty, the model remains unchanged. - //< - - - // This method checks to see if the model name is recognized - // in the input state string. - static std::string getModelNameFromModelState( - const std::string& model_state); - - std::string constructStateFromIsd(const std::string imageSupportData, csm::WarningList *list); - - // State data elements; - std::string m_imageIdentifier; - std::string m_sensorName; - int m_nLines; - int m_nSamples; - int m_platformFlag; - std::vector m_intTimeLines; - std::vector m_intTimeStartTimes; - std::vector m_intTimes; - double m_startingEphemerisTime; - double m_centerEphemerisTime; - double m_detectorSampleSumming; - double m_detectorLineSumming; - double m_startingDetectorSample; - double m_startingDetectorLine; - int m_ikCode; - double m_focalLength; - double m_zDirection; - DistortionType m_distortionType; - std::vector m_opticalDistCoeffs; - double m_iTransS[3]; - double m_iTransL[3]; - double m_detectorSampleOrigin; - double m_detectorLineOrigin; - double m_mountingMatrix[9]; - double m_majorAxis; - double m_minorAxis; - std::string m_referenceDateAndTime; - std::string m_platformIdentifier; - std::string m_sensorIdentifier; - std::string m_trajectoryIdentifier; - std::string m_collectionIdentifier; - double m_refElevation; - double m_minElevation; - double m_maxElevation; - double m_dtEphem; - double m_t0Ephem; - double m_dtQuat; - double m_t0Quat; - int m_numPositions; - int m_numQuaternions; - std::vector m_positions; - std::vector m_velocities; - std::vector m_quaternions; - std::vector m_currentParameterValue; - std::vector m_parameterType; - csm::EcefCoord m_referencePointXyz; - double m_gsd; - double m_flyingHeight; - double m_halfSwath; - double m_halfTime; - std::vector m_covariance; - int m_imageFlipFlag; - - std::vector m_sunPosition; - std::vector m_sunVelocity; - - - // Define logging pointer and file content - std::shared_ptr m_logger = spdlog::get("usgscsm_logger"); - - // Hardcoded - static const std::string _SENSOR_MODEL_NAME; // state date element 0 - - static const std::string _STATE_KEYWORD[]; - static const int NUM_PARAM_TYPES; - static const std::string PARAM_STRING_ALL[]; - static const csm::param::Type PARAM_CHAR_ALL[]; - static const int NUM_PARAMETERS; - static const std::string PARAMETER_NAME[]; - - // Set to default values - void reset(); - - //-------------------------------------------------------------- - // Constructors/Destructor - //-------------------------------------------------------------- - - UsgsAstroLsSensorModel(); - ~UsgsAstroLsSensorModel(); - - virtual std::string getModelState() const; - - - // Set the sensor model based on the input state data - void set( const std::string &state_data ); - - - //---------------------------------------------------------------- - // The following public methods are implementations of - // the methods inherited from RasterGM and SettableEllipsoid. - // These are defined in the CSM API. - //---------------------------------------------------------------- - - //--- - // Core Photogrammetry - //--- - virtual csm::ImageCoord groundToImage( - const csm::EcefCoord& groundPt, - double desiredPrecision = 0.001, +class UsgsAstroLsSensorModel : public csm::RasterGM, + virtual public csm::SettableEllipsoid { + public: + // Initializes the class from state data as formatted + // in a string by the toString() method + void setState(const std::string& state); + + virtual void replaceModelState(const std::string& stateString); + //> This method attempts to initialize the current model with the state + // given by argState. The argState argument can be a string previously + // retrieved from the getModelState method. + // + // If argState contains a valid state for the current model, + // the internal state of the model is updated. + // + // If the model cannot be updated to the given state, a csm::Error is + // thrown and the internal state of the model is undefined. + // + // If the argument state string is empty, the model remains unchanged. + //< + + // This method checks to see if the model name is recognized + // in the input state string. + static std::string getModelNameFromModelState(const std::string& model_state); + + std::string constructStateFromIsd(const std::string imageSupportData, + csm::WarningList* list); + + // State data elements; + std::string m_imageIdentifier; + std::string m_sensorName; + int m_nLines; + int m_nSamples; + int m_platformFlag; + std::vector m_intTimeLines; + std::vector m_intTimeStartTimes; + std::vector m_intTimes; + double m_startingEphemerisTime; + double m_centerEphemerisTime; + double m_detectorSampleSumming; + double m_detectorLineSumming; + double m_startingDetectorSample; + double m_startingDetectorLine; + int m_ikCode; + double m_focalLength; + double m_zDirection; + DistortionType m_distortionType; + std::vector m_opticalDistCoeffs; + double m_iTransS[3]; + double m_iTransL[3]; + double m_detectorSampleOrigin; + double m_detectorLineOrigin; + double m_mountingMatrix[9]; + double m_majorAxis; + double m_minorAxis; + std::string m_referenceDateAndTime; + std::string m_platformIdentifier; + std::string m_sensorIdentifier; + std::string m_trajectoryIdentifier; + std::string m_collectionIdentifier; + double m_refElevation; + double m_minElevation; + double m_maxElevation; + double m_dtEphem; + double m_t0Ephem; + double m_dtQuat; + double m_t0Quat; + int m_numPositions; + int m_numQuaternions; + std::vector m_positions; + std::vector m_velocities; + std::vector m_quaternions; + std::vector m_currentParameterValue; + std::vector m_parameterType; + csm::EcefCoord m_referencePointXyz; + double m_gsd; + double m_flyingHeight; + double m_halfSwath; + double m_halfTime; + std::vector m_covariance; + int m_imageFlipFlag; + + std::vector m_sunPosition; + std::vector m_sunVelocity; + + // Define logging pointer and file content + std::shared_ptr m_logger = spdlog::get("usgscsm_logger"); + + // Hardcoded + static const std::string _SENSOR_MODEL_NAME; // state date element 0 + + static const std::string _STATE_KEYWORD[]; + static const int NUM_PARAM_TYPES; + static const std::string PARAM_STRING_ALL[]; + static const csm::param::Type PARAM_CHAR_ALL[]; + static const int NUM_PARAMETERS; + static const std::string PARAMETER_NAME[]; + + // Set to default values + void reset(); + + //-------------------------------------------------------------- + // Constructors/Destructor + //-------------------------------------------------------------- + + UsgsAstroLsSensorModel(); + ~UsgsAstroLsSensorModel(); + + virtual std::string getModelState() const; + + // Set the sensor model based on the input state data + void set(const std::string& state_data); + + //---------------------------------------------------------------- + // The following public methods are implementations of + // the methods inherited from RasterGM and SettableEllipsoid. + // These are defined in the CSM API. + //---------------------------------------------------------------- + + //--- + // Core Photogrammetry + //--- + virtual csm::ImageCoord groundToImage( + const csm::EcefCoord& groundPt, double desiredPrecision = 0.001, double* achievedPrecision = NULL, csm::WarningList* warnings = NULL) const; - //> This method converts the given groundPt (x,y,z in ECEF meters) to a - // returned image coordinate (line, sample in full image space pixels). - // - // Iterative algorithms will use desiredPrecision, in meters, as the - // convergence criterion, otherwise it will be ignored. - // - // If a non-NULL achievedPrecision argument is received, it will be - // populated with the actual precision, in meters, achieved by iterative - // algorithms and 0.0 for deterministic algorithms. - // - // If a non-NULL warnings argument is received, it will be populated - // as applicable. - //< - - virtual csm::ImageCoordCovar groundToImage( - const csm::EcefCoordCovar& groundPt, - double desiredPrecision = 0.001, + //> This method converts the given groundPt (x,y,z in ECEF meters) to a + // returned image coordinate (line, sample in full image space pixels). + // + // Iterative algorithms will use desiredPrecision, in meters, as the + // convergence criterion, otherwise it will be ignored. + // + // If a non-NULL achievedPrecision argument is received, it will be + // populated with the actual precision, in meters, achieved by iterative + // algorithms and 0.0 for deterministic algorithms. + // + // If a non-NULL warnings argument is received, it will be populated + // as applicable. + //< + + virtual csm::ImageCoordCovar groundToImage( + const csm::EcefCoordCovar& groundPt, double desiredPrecision = 0.001, double* achievedPrecision = NULL, csm::WarningList* warnings = NULL) const; - //> This method converts the given groundPt (x,y,z in ECEF meters and - // corresponding 3x3 covariance in ECEF meters squared) to a returned - // image coordinate with covariance (line, sample in full image space - // pixels and corresponding 2x2 covariance in pixels squared). - // - // Iterative algorithms will use desiredPrecision, in meters, as the - // convergence criterion, otherwise it will be ignored. - // - // If a non-NULL achievedPrecision argument is received, it will be - // populated with the actual precision, in meters, achieved by iterative - // algorithms and 0.0 for deterministic algorithms. - // - // If a non-NULL warnings argument is received, it will be populated - // as applicable. - //< - - virtual csm::EcefCoord imageToGround( - const csm::ImageCoord& imagePt, - double height, - double desiredPrecision = 0.001, - double* achievedPrecision = NULL, - csm::WarningList* warnings = NULL) const; - //> This method converts the given imagePt (line,sample in full image - // space pixels) and given height (in meters relative to the WGS-84 - // ellipsoid) to a returned ground coordinate (x,y,z in ECEF meters). - // - // Iterative algorithms will use desiredPrecision, in meters, as the - // convergence criterion, otherwise it will be ignored. - // - // If a non-NULL achievedPrecision argument is received, it will be - // populated with the actual precision, in meters, achieved by iterative - // algorithms and 0.0 for deterministic algorithms. - // - // If a non-NULL warnings argument is received, it will be populated - // as applicable. - //< - - virtual csm::EcefCoordCovar imageToGround( - const csm::ImageCoordCovar& imagePt, - double height, - double heightVariance, - double desiredPrecision = 0.001, - double* achievedPrecision = NULL, + //> This method converts the given groundPt (x,y,z in ECEF meters and + // corresponding 3x3 covariance in ECEF meters squared) to a returned + // image coordinate with covariance (line, sample in full image space + // pixels and corresponding 2x2 covariance in pixels squared). + // + // Iterative algorithms will use desiredPrecision, in meters, as the + // convergence criterion, otherwise it will be ignored. + // + // If a non-NULL achievedPrecision argument is received, it will be + // populated with the actual precision, in meters, achieved by iterative + // algorithms and 0.0 for deterministic algorithms. + // + // If a non-NULL warnings argument is received, it will be populated + // as applicable. + //< + + virtual csm::EcefCoord imageToGround(const csm::ImageCoord& imagePt, + double height, + double desiredPrecision = 0.001, + double* achievedPrecision = NULL, + csm::WarningList* warnings = NULL) const; + //> This method converts the given imagePt (line,sample in full image + // space pixels) and given height (in meters relative to the WGS-84 + // ellipsoid) to a returned ground coordinate (x,y,z in ECEF meters). + // + // Iterative algorithms will use desiredPrecision, in meters, as the + // convergence criterion, otherwise it will be ignored. + // + // If a non-NULL achievedPrecision argument is received, it will be + // populated with the actual precision, in meters, achieved by iterative + // algorithms and 0.0 for deterministic algorithms. + // + // If a non-NULL warnings argument is received, it will be populated + // as applicable. + //< + + virtual csm::EcefCoordCovar imageToGround( + const csm::ImageCoordCovar& imagePt, double height, double heightVariance, + double desiredPrecision = 0.001, double* achievedPrecision = NULL, csm::WarningList* warnings = NULL) const; - //> This method converts the given imagePt (line, sample in full image - // space pixels and corresponding 2x2 covariance in pixels squared) - // and given height (in meters relative to the WGS-84 ellipsoid) and - // corresponding heightVariance (in meters) to a returned ground - // coordinate with covariance (x,y,z in ECEF meters and corresponding - // 3x3 covariance in ECEF meters squared). - // - // Iterative algorithms will use desiredPrecision, in meters, as the - // convergence criterion, otherwise it will be ignored. - // - // If a non-NULL achievedPrecision argument is received, it will be - // populated with the actual precision, in meters, achieved by iterative - // algorithms and 0.0 for deterministic algorithms. - // - // If a non-NULL warnings argument is received, it will be populated - // as applicable. - //< - - virtual csm::EcefLocus imageToProximateImagingLocus( - const csm::ImageCoord& imagePt, - const csm::EcefCoord& groundPt, - double desiredPrecision = 0.001, - double* achievedPrecision = NULL, + //> This method converts the given imagePt (line, sample in full image + // space pixels and corresponding 2x2 covariance in pixels squared) + // and given height (in meters relative to the WGS-84 ellipsoid) and + // corresponding heightVariance (in meters) to a returned ground + // coordinate with covariance (x,y,z in ECEF meters and corresponding + // 3x3 covariance in ECEF meters squared). + // + // Iterative algorithms will use desiredPrecision, in meters, as the + // convergence criterion, otherwise it will be ignored. + // + // If a non-NULL achievedPrecision argument is received, it will be + // populated with the actual precision, in meters, achieved by iterative + // algorithms and 0.0 for deterministic algorithms. + // + // If a non-NULL warnings argument is received, it will be populated + // as applicable. + //< + + virtual csm::EcefLocus imageToProximateImagingLocus( + const csm::ImageCoord& imagePt, const csm::EcefCoord& groundPt, + double desiredPrecision = 0.001, double* achievedPrecision = NULL, csm::WarningList* warnings = NULL) const; - //> This method, for the given imagePt (line, sample in full image space - // pixels), returns the position and direction of the imaging locus - // nearest the given groundPt (x,y,z in ECEF meters). - // - // Note that there are two opposite directions possible. Both are - // valid, so either can be returned; the calling application can convert - // to the other as necessary. - // - // Iterative algorithms will use desiredPrecision, in meters, as the - // convergence criterion for the locus position, otherwise it will be - // ignored. - // - // If a non-NULL achievedPrecision argument is received, it will be - // populated with the actual precision, in meters, achieved by iterative - // algorithms and 0.0 for deterministic algorithms. - // - // If a non-NULL warnings argument is received, it will be populated - // as applicable. - //< - - virtual csm::EcefLocus imageToRemoteImagingLocus( - const csm::ImageCoord& imagePt, - double desiredPrecision = 0.001, + //> This method, for the given imagePt (line, sample in full image space + // pixels), returns the position and direction of the imaging locus + // nearest the given groundPt (x,y,z in ECEF meters). + // + // Note that there are two opposite directions possible. Both are + // valid, so either can be returned; the calling application can convert + // to the other as necessary. + // + // Iterative algorithms will use desiredPrecision, in meters, as the + // convergence criterion for the locus position, otherwise it will be + // ignored. + // + // If a non-NULL achievedPrecision argument is received, it will be + // populated with the actual precision, in meters, achieved by iterative + // algorithms and 0.0 for deterministic algorithms. + // + // If a non-NULL warnings argument is received, it will be populated + // as applicable. + //< + + virtual csm::EcefLocus imageToRemoteImagingLocus( + const csm::ImageCoord& imagePt, double desiredPrecision = 0.001, double* achievedPrecision = NULL, csm::WarningList* warnings = NULL) const; - //> This method, for the given imagePt (line, sample in full image space - // pixels), returns the position and direction of the imaging locus - // at the sensor. - // - // Note that there are two opposite directions possible. Both are - // valid, so either can be returned; the calling application can convert - // to the other as necessary. - // - // Iterative algorithms will use desiredPrecision, in meters, as the - // convergence criterion for the locus position, otherwise it will be - // ignored. - // - // If a non-NULL achievedPrecision argument is received, it will be - // populated with the actual precision, in meters, achieved by iterative - // algorithms and 0.0 for deterministic algorithms. - // - // If a non-NULL warnings argument is received, it will be populated - // as applicable. - // - // Notes: - // - // The remote imaging locus is only well-defined for optical sensors. - // It is undefined for SAR sensors and might not be available for - // polynomial and other non-physical models. The - // imageToProximateImagingLocus method should be used instead where - // possible. - //< - - //--- - // Monoscopic Mensuration - //--- - virtual csm::ImageCoord getImageStart() const; - //> This method returns the starting coordinate (line, sample in full - // image space pixels) for the imaging operation. Typically (0,0). - //< - - virtual csm::ImageVector getImageSize() const; - //> This method returns the number of lines and samples in full image - // space pixels for the imaging operation. - // - // Note that the model might not be valid over the entire imaging - // operation. Use getValidImageRange() to get the valid range of image - // coordinates. - //< - - virtual std::pair getValidImageRange() const; - //> This method returns the minimum and maximum image coordinates - // (line, sample in full image space pixels), respectively, over which - // the current model is valid. The image coordinates define opposite - // corners of a rectangle whose sides are parallel to the line and - // sample axes. - // - // The valid image range does not always match the full image - // coverage as returned by the getImageStart and getImageSize methods. - // - // Used in conjunction with the getValidHeightRange method, it is - // possible to determine the full range of ground coordinates over which - // the model is valid. - //< - - virtual std::pair getValidHeightRange() const; - //> This method returns the minimum and maximum heights (in meters - // relative to WGS-84 ellipsoid), respectively, over which the model is - // valid. For example, a model for an airborne platform might not be - // designed to return valid coordinates for heights above the aircraft. - // - // If there are no limits defined for the model, (-99999.0,99999.0) - // will be returned. - //< - - virtual csm::EcefVector getIlluminationDirection(const csm::EcefCoord& groundPt) const; - //> This method returns a vector defining the direction of - // illumination at the given groundPt (x,y,z in ECEF meters). - // Note that there are two opposite directions possible. Both are - // valid, so either can be returned; the calling application can convert - // to the other as necessary. - //< - - //--- - // Time and Trajectory - //--- - virtual double getImageTime(const csm::ImageCoord& imagePt) const; - //> This method returns the time in seconds at which the pixel at the - // given imagePt (line, sample in full image space pixels) was captured - // - // The time provided is relative to the reference date and time given - // by the Model::getReferenceDateAndTime method. - //< - - virtual csm::EcefCoord getSensorPosition(const csm::ImageCoord& imagePt) const; - //> This method returns the position of the physical sensor - // (x,y,z in ECEF meters) when the pixel at the given imagePt - // (line, sample in full image space pixels) was captured. - // - // A csm::Error will be thrown if the sensor position is not available. - //< - - virtual csm::EcefCoord getSensorPosition(double time) const; - //> This method returns the position of the physical sensor - // (x,y,z meters ECEF) at the given time relative to the reference date - // and time given by the Model::getReferenceDateAndTime method. - //< - - virtual csm::EcefVector getSensorVelocity(const csm::ImageCoord& imagePt) const; - //> This method returns the velocity of the physical sensor - // (x,y,z in ECEF meters per second) when the pixel at the given imagePt - // (line, sample in full image space pixels) was captured. - //< - - virtual csm::EcefVector getSensorVelocity(double time) const; - //> This method returns the velocity of the physical sensor - // (x,y,z in ECEF meters per second ) at the given time relative to the - // reference date and time given by the Model::getReferenceDateAndTime - // method. - //< - - - virtual csm::RasterGM::SensorPartials computeSensorPartials( - int index, - const csm::EcefCoord& groundPt, - double desiredPrecision = 0.001, - double* achievedPrecision = NULL, + //> This method, for the given imagePt (line, sample in full image space + // pixels), returns the position and direction of the imaging locus + // at the sensor. + // + // Note that there are two opposite directions possible. Both are + // valid, so either can be returned; the calling application can convert + // to the other as necessary. + // + // Iterative algorithms will use desiredPrecision, in meters, as the + // convergence criterion for the locus position, otherwise it will be + // ignored. + // + // If a non-NULL achievedPrecision argument is received, it will be + // populated with the actual precision, in meters, achieved by iterative + // algorithms and 0.0 for deterministic algorithms. + // + // If a non-NULL warnings argument is received, it will be populated + // as applicable. + // + // Notes: + // + // The remote imaging locus is only well-defined for optical sensors. + // It is undefined for SAR sensors and might not be available for + // polynomial and other non-physical models. The + // imageToProximateImagingLocus method should be used instead where + // possible. + //< + + //--- + // Monoscopic Mensuration + //--- + virtual csm::ImageCoord getImageStart() const; + //> This method returns the starting coordinate (line, sample in full + // image space pixels) for the imaging operation. Typically (0,0). + //< + + virtual csm::ImageVector getImageSize() const; + //> This method returns the number of lines and samples in full image + // space pixels for the imaging operation. + // + // Note that the model might not be valid over the entire imaging + // operation. Use getValidImageRange() to get the valid range of image + // coordinates. + //< + + virtual std::pair getValidImageRange() + const; + //> This method returns the minimum and maximum image coordinates + // (line, sample in full image space pixels), respectively, over which + // the current model is valid. The image coordinates define opposite + // corners of a rectangle whose sides are parallel to the line and + // sample axes. + // + // The valid image range does not always match the full image + // coverage as returned by the getImageStart and getImageSize methods. + // + // Used in conjunction with the getValidHeightRange method, it is + // possible to determine the full range of ground coordinates over which + // the model is valid. + //< + + virtual std::pair getValidHeightRange() const; + //> This method returns the minimum and maximum heights (in meters + // relative to WGS-84 ellipsoid), respectively, over which the model is + // valid. For example, a model for an airborne platform might not be + // designed to return valid coordinates for heights above the aircraft. + // + // If there are no limits defined for the model, (-99999.0,99999.0) + // will be returned. + //< + + virtual csm::EcefVector getIlluminationDirection( + const csm::EcefCoord& groundPt) const; + //> This method returns a vector defining the direction of + // illumination at the given groundPt (x,y,z in ECEF meters). + // Note that there are two opposite directions possible. Both are + // valid, so either can be returned; the calling application can convert + // to the other as necessary. + //< + + //--- + // Time and Trajectory + //--- + virtual double getImageTime(const csm::ImageCoord& imagePt) const; + //> This method returns the time in seconds at which the pixel at the + // given imagePt (line, sample in full image space pixels) was captured + // + // The time provided is relative to the reference date and time given + // by the Model::getReferenceDateAndTime method. + //< + + virtual csm::EcefCoord getSensorPosition( + const csm::ImageCoord& imagePt) const; + //> This method returns the position of the physical sensor + // (x,y,z in ECEF meters) when the pixel at the given imagePt + // (line, sample in full image space pixels) was captured. + // + // A csm::Error will be thrown if the sensor position is not available. + //< + + virtual csm::EcefCoord getSensorPosition(double time) const; + //> This method returns the position of the physical sensor + // (x,y,z meters ECEF) at the given time relative to the reference date + // and time given by the Model::getReferenceDateAndTime method. + //< + + virtual csm::EcefVector getSensorVelocity( + const csm::ImageCoord& imagePt) const; + //> This method returns the velocity of the physical sensor + // (x,y,z in ECEF meters per second) when the pixel at the given imagePt + // (line, sample in full image space pixels) was captured. + //< + + virtual csm::EcefVector getSensorVelocity(double time) const; + //> This method returns the velocity of the physical sensor + // (x,y,z in ECEF meters per second ) at the given time relative to the + // reference date and time given by the Model::getReferenceDateAndTime + // method. + //< + + virtual csm::RasterGM::SensorPartials computeSensorPartials( + int index, const csm::EcefCoord& groundPt, + double desiredPrecision = 0.001, double* achievedPrecision = NULL, csm::WarningList* warnings = NULL) const; - //> This is one of two overloaded methods. This method takes only - // the necessary inputs. Some effieciency can be obtained by using the - // other method. Even more efficiency can be obtained by using the - // computeAllSensorPartials method. - // - // This method returns the partial derivatives of line and sample - // (in pixels per the applicable model parameter units), respectively, - // with respect to the model parameter given by index at the given - // groundPt (x,y,z in ECEF meters). - // - // Derived model implementations may wish to implement this method by - // calling the groundToImage method and passing the resulting image - // coordinate to the other computeSensorPartials method. - // - // If a non-NULL achievedPrecision argument is received, it will be - // populated with the highest actual precision, in meters, achieved by - // iterative algorithms and 0.0 for deterministic algorithms. - // - // If a non-NULL achievedPrecision argument is received, it will be - // populated with the actual precision, in meters, achieved by iterative - // algorithms and 0.0 for deterministic algorithms. - // - // If a non-NULL warnings argument is received, it will be populated - // as applicable. - //< - - virtual csm::RasterGM::SensorPartials computeSensorPartials( - int index, - const csm::ImageCoord& imagePt, - const csm::EcefCoord& groundPt, - double desiredPrecision = 0.001, - double* achievedPrecision = NULL, + //> This is one of two overloaded methods. This method takes only + // the necessary inputs. Some effieciency can be obtained by using the + // other method. Even more efficiency can be obtained by using the + // computeAllSensorPartials method. + // + // This method returns the partial derivatives of line and sample + // (in pixels per the applicable model parameter units), respectively, + // with respect to the model parameter given by index at the given + // groundPt (x,y,z in ECEF meters). + // + // Derived model implementations may wish to implement this method by + // calling the groundToImage method and passing the resulting image + // coordinate to the other computeSensorPartials method. + // + // If a non-NULL achievedPrecision argument is received, it will be + // populated with the highest actual precision, in meters, achieved by + // iterative algorithms and 0.0 for deterministic algorithms. + // + // If a non-NULL achievedPrecision argument is received, it will be + // populated with the actual precision, in meters, achieved by iterative + // algorithms and 0.0 for deterministic algorithms. + // + // If a non-NULL warnings argument is received, it will be populated + // as applicable. + //< + + virtual csm::RasterGM::SensorPartials computeSensorPartials( + int index, const csm::ImageCoord& imagePt, const csm::EcefCoord& groundPt, + double desiredPrecision = 0.001, double* achievedPrecision = NULL, csm::WarningList* warnings = NULL) const; - //> This is one of two overloaded methods. This method takes - // an input image coordinate for efficiency. Even more efficiency can - // be obtained by using the computeAllSensorPartials method. - // - // This method returns the partial derivatives of line and sample - // (in pixels per the applicable model parameter units), respectively, - // with respect to the model parameter given by index at the given - // groundPt (x,y,z in ECEF meters). - // - // The imagePt, corresponding to the groundPt, is given so that it does - // not need to be computed by the method. Results are unpredictable if - // the imagePt provided does not correspond to the result of calling the - // groundToImage method with the given groundPt. - // - // Implementations with iterative algorithms (typically ground-to-image - // calls) will use desiredPrecision, in meters, as the convergence - // criterion, otherwise it will be ignored. - // - // If a non-NULL achievedPrecision argument is received, it will be - // populated with the highest actual precision, in meters, achieved by - // iterative algorithms and 0.0 for deterministic algorithms. - // - // If a non-NULL warnings argument is received, it will be populated - // as applicable. - //< - - virtual std::vector computeAllSensorPartials( - const csm::EcefCoord& groundPt, - csm::param::Set pSet = csm::param::VALID, - double desiredPrecision = 0.001, - double* achievedPrecision = NULL, + //> This is one of two overloaded methods. This method takes + // an input image coordinate for efficiency. Even more efficiency can + // be obtained by using the computeAllSensorPartials method. + // + // This method returns the partial derivatives of line and sample + // (in pixels per the applicable model parameter units), respectively, + // with respect to the model parameter given by index at the given + // groundPt (x,y,z in ECEF meters). + // + // The imagePt, corresponding to the groundPt, is given so that it does + // not need to be computed by the method. Results are unpredictable if + // the imagePt provided does not correspond to the result of calling the + // groundToImage method with the given groundPt. + // + // Implementations with iterative algorithms (typically ground-to-image + // calls) will use desiredPrecision, in meters, as the convergence + // criterion, otherwise it will be ignored. + // + // If a non-NULL achievedPrecision argument is received, it will be + // populated with the highest actual precision, in meters, achieved by + // iterative algorithms and 0.0 for deterministic algorithms. + // + // If a non-NULL warnings argument is received, it will be populated + // as applicable. + //< + + virtual std::vector computeAllSensorPartials( + const csm::EcefCoord& groundPt, csm::param::Set pSet = csm::param::VALID, + double desiredPrecision = 0.001, double* achievedPrecision = NULL, csm::WarningList* warnings = NULL) const; - //> This is one of two overloaded methods. This method takes only - // the necessary inputs. Some effieciency can be obtained by using the - // other method. - // - // This method returns the partial derivatives of line and sample - // (in pixels per the applicable model parameter units), respectively, - // with respect to to each of the desired model parameters at the given - // groundPt (x,y,z in ECEF meters). Desired model parameters are - // indicated by the given pSet. - // - // Implementations with iterative algorithms (typically ground-to-image - // calls) will use desiredPrecision, in meters, as the convergence - // criterion, otherwise it will be ignored. - // - // If a non-NULL achievedPrecision argument is received, it will be - // populated with the highest actual precision, in meters, achieved by - // iterative algorithms and 0.0 for deterministic algorithms. - // - // If a non-NULL warnings argument is received, it will be populated - // as applicable. - // - // The value returned is a vector of pairs with line and sample partials - // for one model parameter in each pair. The indices of the - // corresponding model parameters can be found by calling the - // getParameterSetIndices method for the given pSet. - // - // Derived models may wish to implement this directly for efficiency, - // but an implementation is provided here that calls the - // computeSensorPartials method for each desired parameter index. - //< - - virtual std::vector computeAllSensorPartials( - const csm::ImageCoord& imagePt, - const csm::EcefCoord& groundPt, - csm::param::Set pSet = csm::param::VALID, - double desiredPrecision = 0.001, + //> This is one of two overloaded methods. This method takes only + // the necessary inputs. Some effieciency can be obtained by using the + // other method. + // + // This method returns the partial derivatives of line and sample + // (in pixels per the applicable model parameter units), respectively, + // with respect to to each of the desired model parameters at the given + // groundPt (x,y,z in ECEF meters). Desired model parameters are + // indicated by the given pSet. + // + // Implementations with iterative algorithms (typically ground-to-image + // calls) will use desiredPrecision, in meters, as the convergence + // criterion, otherwise it will be ignored. + // + // If a non-NULL achievedPrecision argument is received, it will be + // populated with the highest actual precision, in meters, achieved by + // iterative algorithms and 0.0 for deterministic algorithms. + // + // If a non-NULL warnings argument is received, it will be populated + // as applicable. + // + // The value returned is a vector of pairs with line and sample partials + // for one model parameter in each pair. The indices of the + // corresponding model parameters can be found by calling the + // getParameterSetIndices method for the given pSet. + // + // Derived models may wish to implement this directly for efficiency, + // but an implementation is provided here that calls the + // computeSensorPartials method for each desired parameter index. + //< + + virtual std::vector computeAllSensorPartials( + const csm::ImageCoord& imagePt, const csm::EcefCoord& groundPt, + csm::param::Set pSet = csm::param::VALID, double desiredPrecision = 0.001, double* achievedPrecision = NULL, csm::WarningList* warnings = NULL) const; - //> This is one of two overloaded methods. This method takes - // an input image coordinate for efficiency. - // - // This method returns the partial derivatives of line and sample - // (in pixels per the applicable model parameter units), respectively, - // with respect to to each of the desired model parameters at the given - // groundPt (x,y,z in ECEF meters). Desired model parameters are - // indicated by the given pSet. - // - // The imagePt, corresponding to the groundPt, is given so that it does - // not need to be computed by the method. Results are unpredictable if - // the imagePt provided does not correspond to the result of calling the - // groundToImage method with the given groundPt. - // - // Implementations with iterative algorithms (typically ground-to-image - // calls) will use desiredPrecision, in meters, as the convergence - // criterion, otherwise it will be ignored. - // - // If a non-NULL achievedPrecision argument is received, it will be - // populated with the highest actual precision, in meters, achieved by - // iterative algorithms and 0.0 for deterministic algorithms. - // - // If a non-NULL warnings argument is received, it will be populated - // as applicable. - // - // The value returned is a vector of pairs with line and sample partials - // for one model parameter in each pair. The indices of the - // corresponding model parameters can be found by calling the - // getParameterSetIndices method for the given pSet. - // - // Derived models may wish to implement this directly for efficiency, - // but an implementation is provided here that calls the - // computeSensorPartials method for each desired parameter index. - //< - - virtual std::vector computeGroundPartials(const csm::EcefCoord& groundPt) const; - //> This method returns the partial derivatives of line and sample - // (in pixels per meter) with respect to the given groundPt - // (x,y,z in ECEF meters). - // - // The value returned is a vector with six elements as follows: - // - //- [0] = line wrt x - //- [1] = line wrt y - //- [2] = line wrt z - //- [3] = sample wrt x - //- [4] = sample wrt y - //- [5] = sample wrt z - //< - - virtual const csm::CorrelationModel& getCorrelationModel() const; - //> This method returns a reference to a CorrelationModel. - // The CorrelationModel is used to determine the correlation between - // the model parameters of different models of the same type. - // These correlations are used to establish the "a priori" cross-covariance - // between images. While some applications (such as generation of a - // replacement sensor model) may wish to call this method directly, - // it is reccommended that the inherited method - // GeometricModel::getCrossCovarianceMatrix() be called instead. - //< - - virtual std::vector getUnmodeledCrossCovariance( - const csm::ImageCoord& pt1, - const csm::ImageCoord& pt2) const; - //> This method returns the 2x2 line and sample cross covariance - // (in pixels squared) between the given imagePt1 and imagePt2 for any - // model error not accounted for by the model parameters. The error is - // reported as the four terms of a 2x2 matrix, returned as a 4 element - // vector. - //< - - virtual csm::EcefCoord getReferencePoint() const; - //> This method returns the ground point indicating the general - // location of the image. - //< - - virtual void setReferencePoint(const csm::EcefCoord& groundPt); - //> This method sets the ground point indicating the general location - // of the image. - //< - - //--- - // Sensor Model Parameters - //--- - virtual int getNumParameters() const; - //> This method returns the number of adjustable parameters. - //< - - virtual std::string getParameterName(int index) const; - //> This method returns the name for the adjustable parameter - // indicated by the given index. - // - // If the index is out of range, a csm::Error may be thrown. - //< - - virtual std::string getParameterUnits(int index) const; - //> This method returns the units for the adjustable parameter - // indicated by the given index. This string is intended for human - // consumption, not automated analysis. Preferred unit names are: - // - //- meters "m" - //- centimeters "cm" - //- millimeters "mm" - //- micrometers "um" - //- nanometers "nm" - //- kilometers "km" - //- inches-US "inch" - //- feet-US "ft" - //- statute miles "mi" - //- nautical miles "nmi" - //- - //- radians "rad" - //- microradians "urad" - //- decimal degrees "deg" - //- arc seconds "arcsec" - //- arc minutes "arcmin" - //- - //- seconds "sec" - //- minutes "min" - //- hours "hr" - //- - //- steradian "sterad" - //- - //- none "unitless" - //- - //- lines per second "lines/sec" - //- samples per second "samples/sec" - //- frames per second "frames/sec" - //- - //- watts "watt" - //- - //- degrees Kelvin "K" - //- - //- gram "g" - //- kilogram "kg" - //- pound - US "lb" - //- - //- hertz "hz" - //- megahertz "mhz" - //- gigahertz "ghz" - // - // Units may be combined with "/" or "." to indicate division or - // multiplication. The caret symbol "^" can be used to indicate - // exponentiation. Thus "m.m" and "m^2" are the same and indicate - // square meters. The return "m/sec^2" indicates an acceleration in - // meters per second per second. - // - // Derived classes may choose to return additional unit names, as - // required. - //< - - virtual bool hasShareableParameters() const; - //> This method returns true if there exists at least one adjustable - // parameter on the model that is shareable. See the - // isParameterShareable() method. This method should return false if - // all calls to isParameterShareable() return false. - //< - - virtual bool isParameterShareable(int index) const; - //> This method returns a flag to indicate whether or not the adjustable - // parameter referenced by index is shareable across models. - //< - - virtual csm::SharingCriteria getParameterSharingCriteria(int index) const; - //> This method returns characteristics to indicate how the adjustable - // parameter referenced by index is shareable across models. - //< - - virtual double getParameterValue(int index) const; - //> This method returns the value of the adjustable parameter - // referenced by the given index. - //< - - virtual void setParameterValue(int index, double value); - //> This method sets the value for the adjustable parameter referenced by - // the given index. - //< - - virtual csm::param::Type getParameterType(int index) const; - //> This method returns the type of the adjustable parameter - // referenced by the given index. - //< - - virtual void setParameterType(int index, csm::param::Type pType); - //> This method sets the type of the adjustable parameter - // reference by the given index. - //< - - virtual std::shared_ptr getLogger(); - virtual void setLogger(std::string logName); - - - //--- - // Uncertainty Propagation - //--- - virtual double getParameterCovariance( - int index1, - int index2) const; - //> This method returns the covariance between the parameters - // referenced by index1 and index2. Variance of a single parameter - // is indicated by specifying the samve value for index1 and index2. - //< - - virtual void setParameterCovariance( - int index1, - int index2, - double covariance); - //> This method is used to set the covariance between the parameters - // referenced by index1 and index2. Variance of a single parameter - // is indicated by specifying the samve value for index1 and index2. - //< - - //--- - // Error Correction - //--- - virtual int getNumGeometricCorrectionSwitches() const; - //> This method returns the number of geometric correction switches - // implemented for the current model. - //< - - virtual std::string getGeometricCorrectionName(int index) const; - //> This method returns the name for the geometric correction switch - // referenced by the given index. - //< - - virtual void setGeometricCorrectionSwitch(int index, - bool value, - csm::param::Type pType); - //> This method is used to enable/disable the geometric correction switch - // referenced by the given index. - //< - - virtual bool getGeometricCorrectionSwitch(int index) const; - //> This method returns the value of the geometric correction switch - // referenced by the given index. - //< - - - virtual std::vector getCrossCovarianceMatrix( + //> This is one of two overloaded methods. This method takes + // an input image coordinate for efficiency. + // + // This method returns the partial derivatives of line and sample + // (in pixels per the applicable model parameter units), respectively, + // with respect to to each of the desired model parameters at the given + // groundPt (x,y,z in ECEF meters). Desired model parameters are + // indicated by the given pSet. + // + // The imagePt, corresponding to the groundPt, is given so that it does + // not need to be computed by the method. Results are unpredictable if + // the imagePt provided does not correspond to the result of calling the + // groundToImage method with the given groundPt. + // + // Implementations with iterative algorithms (typically ground-to-image + // calls) will use desiredPrecision, in meters, as the convergence + // criterion, otherwise it will be ignored. + // + // If a non-NULL achievedPrecision argument is received, it will be + // populated with the highest actual precision, in meters, achieved by + // iterative algorithms and 0.0 for deterministic algorithms. + // + // If a non-NULL warnings argument is received, it will be populated + // as applicable. + // + // The value returned is a vector of pairs with line and sample partials + // for one model parameter in each pair. The indices of the + // corresponding model parameters can be found by calling the + // getParameterSetIndices method for the given pSet. + // + // Derived models may wish to implement this directly for efficiency, + // but an implementation is provided here that calls the + // computeSensorPartials method for each desired parameter index. + //< + + virtual std::vector computeGroundPartials( + const csm::EcefCoord& groundPt) const; + //> This method returns the partial derivatives of line and sample + // (in pixels per meter) with respect to the given groundPt + // (x,y,z in ECEF meters). + // + // The value returned is a vector with six elements as follows: + // + //- [0] = line wrt x + //- [1] = line wrt y + //- [2] = line wrt z + //- [3] = sample wrt x + //- [4] = sample wrt y + //- [5] = sample wrt z + //< + + virtual const csm::CorrelationModel& getCorrelationModel() const; + //> This method returns a reference to a CorrelationModel. + // The CorrelationModel is used to determine the correlation between + // the model parameters of different models of the same type. + // These correlations are used to establish the "a priori" cross-covariance + // between images. While some applications (such as generation of a + // replacement sensor model) may wish to call this method directly, + // it is reccommended that the inherited method + // GeometricModel::getCrossCovarianceMatrix() be called instead. + //< + + virtual std::vector getUnmodeledCrossCovariance( + const csm::ImageCoord& pt1, const csm::ImageCoord& pt2) const; + //> This method returns the 2x2 line and sample cross covariance + // (in pixels squared) between the given imagePt1 and imagePt2 for any + // model error not accounted for by the model parameters. The error is + // reported as the four terms of a 2x2 matrix, returned as a 4 element + // vector. + //< + + virtual csm::EcefCoord getReferencePoint() const; + //> This method returns the ground point indicating the general + // location of the image. + //< + + virtual void setReferencePoint(const csm::EcefCoord& groundPt); + //> This method sets the ground point indicating the general location + // of the image. + //< + + //--- + // Sensor Model Parameters + //--- + virtual int getNumParameters() const; + //> This method returns the number of adjustable parameters. + //< + + virtual std::string getParameterName(int index) const; + //> This method returns the name for the adjustable parameter + // indicated by the given index. + // + // If the index is out of range, a csm::Error may be thrown. + //< + + virtual std::string getParameterUnits(int index) const; + //> This method returns the units for the adjustable parameter + // indicated by the given index. This string is intended for human + // consumption, not automated analysis. Preferred unit names are: + // + //- meters "m" + //- centimeters "cm" + //- millimeters "mm" + //- micrometers "um" + //- nanometers "nm" + //- kilometers "km" + //- inches-US "inch" + //- feet-US "ft" + //- statute miles "mi" + //- nautical miles "nmi" + //- + //- radians "rad" + //- microradians "urad" + //- decimal degrees "deg" + //- arc seconds "arcsec" + //- arc minutes "arcmin" + //- + //- seconds "sec" + //- minutes "min" + //- hours "hr" + //- + //- steradian "sterad" + //- + //- none "unitless" + //- + //- lines per second "lines/sec" + //- samples per second "samples/sec" + //- frames per second "frames/sec" + //- + //- watts "watt" + //- + //- degrees Kelvin "K" + //- + //- gram "g" + //- kilogram "kg" + //- pound - US "lb" + //- + //- hertz "hz" + //- megahertz "mhz" + //- gigahertz "ghz" + // + // Units may be combined with "/" or "." to indicate division or + // multiplication. The caret symbol "^" can be used to indicate + // exponentiation. Thus "m.m" and "m^2" are the same and indicate + // square meters. The return "m/sec^2" indicates an acceleration in + // meters per second per second. + // + // Derived classes may choose to return additional unit names, as + // required. + //< + + virtual bool hasShareableParameters() const; + //> This method returns true if there exists at least one adjustable + // parameter on the model that is shareable. See the + // isParameterShareable() method. This method should return false if + // all calls to isParameterShareable() return false. + //< + + virtual bool isParameterShareable(int index) const; + //> This method returns a flag to indicate whether or not the adjustable + // parameter referenced by index is shareable across models. + //< + + virtual csm::SharingCriteria getParameterSharingCriteria(int index) const; + //> This method returns characteristics to indicate how the adjustable + // parameter referenced by index is shareable across models. + //< + + virtual double getParameterValue(int index) const; + //> This method returns the value of the adjustable parameter + // referenced by the given index. + //< + + virtual void setParameterValue(int index, double value); + //> This method sets the value for the adjustable parameter referenced by + // the given index. + //< + + virtual csm::param::Type getParameterType(int index) const; + //> This method returns the type of the adjustable parameter + // referenced by the given index. + //< + + virtual void setParameterType(int index, csm::param::Type pType); + //> This method sets the type of the adjustable parameter + // reference by the given index. + //< + + virtual std::shared_ptr getLogger(); + virtual void setLogger(std::string logName); + + //--- + // Uncertainty Propagation + //--- + virtual double getParameterCovariance(int index1, int index2) const; + //> This method returns the covariance between the parameters + // referenced by index1 and index2. Variance of a single parameter + // is indicated by specifying the samve value for index1 and index2. + //< + + virtual void setParameterCovariance(int index1, int index2, + double covariance); + //> This method is used to set the covariance between the parameters + // referenced by index1 and index2. Variance of a single parameter + // is indicated by specifying the samve value for index1 and index2. + //< + + //--- + // Error Correction + //--- + virtual int getNumGeometricCorrectionSwitches() const; + //> This method returns the number of geometric correction switches + // implemented for the current model. + //< + + virtual std::string getGeometricCorrectionName(int index) const; + //> This method returns the name for the geometric correction switch + // referenced by the given index. + //< + + virtual void setGeometricCorrectionSwitch(int index, bool value, + csm::param::Type pType); + //> This method is used to enable/disable the geometric correction switch + // referenced by the given index. + //< + + virtual bool getGeometricCorrectionSwitch(int index) const; + //> This method returns the value of the geometric correction switch + // referenced by the given index. + //< + + virtual std::vector getCrossCovarianceMatrix( const csm::GeometricModel& comparisonModel, csm::param::Set pSet = csm::param::VALID, - const csm::GeometricModel::GeometricModelList& otherModels = csm::GeometricModel::GeometricModelList()) const; - //> This method returns a matrix containing the elements of the error - // cross covariance between this model and a given second model - // (comparisonModel). The set of cross covariance elements returned is - // indicated by pSet, which, by default, is all VALID parameters. - // - // If comparisonModel is the same as this model, the covariance for - // this model will be returned. It is equivalent to calling - // getParameterCovariance() for the same set of elements. Note that - // even if the cross covariance for a particular model type is always - // zero, the covariance for this model must still be supported. - // - // The otherModels list contains all of the models in the current - // photogrammetric process; some cross-covariance implementations are - // influenced by other models. It can be omitted if it is not needed - // by any models being used. - // - // The returned vector will logically be a two-dimensional matrix of - // covariances, though for simplicity it is stored in a one-dimensional - // vector (STL has no two-dimensional structure). The height (number of - // rows) of this matrix is the number of parameters on the current model, - // and the width (number of columns) is the number of parameters on - // the comparison model. Thus, the covariance between p1 on this model - // and p2 on the comparison model is found in index (N*p1 + p2) - // in the returned vector. N is the size of the vector returned by - // getParameterSetIndices() on the comparison model for the given pSet). - // - // Note that cross covariance is often zero. Non-zero cross covariance - // can occur for models created from the same sensor (or different - // sensors on the same platform). While cross covariances can result - // from a bundle adjustment involving multiple models, no mechanism - // currently exists within csm to "set" the cross covariance between - // models. It should thus be assumed that the returned cross covariance - // reflects the "un-adjusted" state of the models. - //< - - virtual csm::Version getVersion() const; - //> This method returns the version of the model code. The Version - // object can be compared to other Version objects with its comparison - // operators. Not to be confused with the CSM API version. - //< - - virtual std::string getModelName() const; - //> This method returns a string identifying the name of the model. - //< - - virtual std::string getPedigree() const; - //> This method returns a string that identifies the sensor, - // the model type, its mode of acquisition and processing path. - // For example, an optical sensor model or a cubic rational polynomial - // model created from the same sensor's support data would produce - // different pedigrees for each case. - //< - - //--- - // Basic collection information - //--- - virtual std::string getImageIdentifier() const; - //> This method returns an identifier to uniquely indicate the imaging - // operation associated with this model. - // This is the primary identifier of the model. - // - // This method may return an empty string if the ID is unknown. - //< - - virtual void setImageIdentifier( - const std::string& imageId, - csm::WarningList* warnings = NULL); - //> This method sets an identifier to uniquely indicate the imaging - // operation associated with this model. Typically used for models - // whose initialization does not produce an adequate identifier. - // - // If a non-NULL warnings argument is received, it will be populated - // as applicable. - //< - - virtual std::string getSensorIdentifier() const; - //> This method returns an identifier to indicate the specific sensor - // that was used to acquire the image. This ID must be unique among - // sensors for a given model name. It is used to determine parameter - // correlation and sharing. Equivalent to camera or mission ID. - // - // This method may return an empty string if the sensor ID is unknown. - //< - - virtual std::string getPlatformIdentifier() const; - //> This method returns an identifier to indicate the specific platform - // that was used to acquire the image. This ID must unique among - // platforms for a given model name. It is used to determine parameter - // correlation sharing. Equivalent to vehicle or aircraft tail number. - // - // This method may return an empty string if the platform ID is unknown. - //< - - virtual std::string getCollectionIdentifier() const; - //> This method returns an identifer to indicate a collection activity - // common to a set of images. This ID must be unique among collection - // activities for a given model name. It is used to determine parameter - // correlation and sharing. - //< - - virtual std::string getTrajectoryIdentifier() const; - //> This method returns an identifier to indicate a trajectory common - // to a set of images. This ID must be unique among trajectories - // for a given model name. It is used to determine parameter - // correlation and sharing. - //< - - virtual std::string getSensorType() const; - //> This method returns a description of the sensor type (EO, IR, SAR, - // etc). See csm.h for a list of common types. Should return - // CSM_SENSOR_TYPE_UNKNOWN if the sensor type is unknown. - //< - - virtual std::string getSensorMode() const; - //> This method returns a description of the sensor mode (FRAME, - // PUSHBROOM, SPOT, SCAN, etc). See csm.h for a list of common modes. - // Should return CSM_SENSOR_MODE_UNKNOWN if the sensor mode is unknown. - //< - - virtual std::string getReferenceDateAndTime() const; - //> This method returns an approximate date and time at which the - // image was taken. The returned string follows the ISO 8601 standard. - // - //- Precision Format Example - //- year yyyy "1961" - //- month yyyymm "196104" - //- day yyyymmdd "19610420" - //- hour yyyymmddThh "19610420T20" - //- minute yyyymmddThhmm "19610420T2000" - //- second yyyymmddThhmmss "19610420T200000" - //< - - //--- - // Sensor Model State - //--- - // virtual std::string setModelState(std::string stateString) const; - //> This method returns a string containing the data to exactly recreate - // the current model. It can be used to restore this model to a - // previous state with the replaceModelState method or create a new - // model object that is identical to this model. - // The string could potentially be saved to a file for later use. - // An empty string is returned if it is not possible to save the - // current state. - //< - - virtual csm::Ellipsoid getEllipsoid() const; - //> This method returns the planetary ellipsoid. - //< - - virtual void setEllipsoid( - const csm::Ellipsoid &ellipsoid); - //> This method sets the planetary ellipsoid. - //< - - void calculateAttitudeCorrection( - const double& time, - const std::vector& adj, - double attCorr[9]) const; - - virtual csm::EcefVector getSunPosition( - const double imageTime) const; - //> This method returns the position of the sun at the time the image point - // was recorded. If multiple sun positions are available, the method uses - // lagrange interpolation. If one sun position and at least one sun velocity - // are available, then the position is calculated using linear extrapolation. - // If only one sun position is available, then that value is returned. - - -private: - - void determineSensorCovarianceInImageSpace( - csm::EcefCoord &gp, - double sensor_cov[4]) const; - - // Some state data values not found in the support data require a - // sensor model in order to be set. - void updateState(); - - // This method returns the value of the specified adjustable parameter - // with the associated adjustment added in. - double getValue( - int index, - const std::vector &adjustments) const; - - // This private form of the g2i method is used to ensure thread safety. - virtual csm::ImageCoord groundToImage( - const csm::EcefCoord& groundPt, - const std::vector &adjustments, - double desiredPrecision = 0.001, - double* achievedPrecision = NULL, + const csm::GeometricModel::GeometricModelList& otherModels = + csm::GeometricModel::GeometricModelList()) const; + //> This method returns a matrix containing the elements of the error + // cross covariance between this model and a given second model + // (comparisonModel). The set of cross covariance elements returned is + // indicated by pSet, which, by default, is all VALID parameters. + // + // If comparisonModel is the same as this model, the covariance for + // this model will be returned. It is equivalent to calling + // getParameterCovariance() for the same set of elements. Note that + // even if the cross covariance for a particular model type is always + // zero, the covariance for this model must still be supported. + // + // The otherModels list contains all of the models in the current + // photogrammetric process; some cross-covariance implementations are + // influenced by other models. It can be omitted if it is not needed + // by any models being used. + // + // The returned vector will logically be a two-dimensional matrix of + // covariances, though for simplicity it is stored in a one-dimensional + // vector (STL has no two-dimensional structure). The height (number of + // rows) of this matrix is the number of parameters on the current model, + // and the width (number of columns) is the number of parameters on + // the comparison model. Thus, the covariance between p1 on this model + // and p2 on the comparison model is found in index (N*p1 + p2) + // in the returned vector. N is the size of the vector returned by + // getParameterSetIndices() on the comparison model for the given pSet). + // + // Note that cross covariance is often zero. Non-zero cross covariance + // can occur for models created from the same sensor (or different + // sensors on the same platform). While cross covariances can result + // from a bundle adjustment involving multiple models, no mechanism + // currently exists within csm to "set" the cross covariance between + // models. It should thus be assumed that the returned cross covariance + // reflects the "un-adjusted" state of the models. + //< + + virtual csm::Version getVersion() const; + //> This method returns the version of the model code. The Version + // object can be compared to other Version objects with its comparison + // operators. Not to be confused with the CSM API version. + //< + + virtual std::string getModelName() const; + //> This method returns a string identifying the name of the model. + //< + + virtual std::string getPedigree() const; + //> This method returns a string that identifies the sensor, + // the model type, its mode of acquisition and processing path. + // For example, an optical sensor model or a cubic rational polynomial + // model created from the same sensor's support data would produce + // different pedigrees for each case. + //< + + //--- + // Basic collection information + //--- + virtual std::string getImageIdentifier() const; + //> This method returns an identifier to uniquely indicate the imaging + // operation associated with this model. + // This is the primary identifier of the model. + // + // This method may return an empty string if the ID is unknown. + //< + + virtual void setImageIdentifier(const std::string& imageId, + csm::WarningList* warnings = NULL); + //> This method sets an identifier to uniquely indicate the imaging + // operation associated with this model. Typically used for models + // whose initialization does not produce an adequate identifier. + // + // If a non-NULL warnings argument is received, it will be populated + // as applicable. + //< + + virtual std::string getSensorIdentifier() const; + //> This method returns an identifier to indicate the specific sensor + // that was used to acquire the image. This ID must be unique among + // sensors for a given model name. It is used to determine parameter + // correlation and sharing. Equivalent to camera or mission ID. + // + // This method may return an empty string if the sensor ID is unknown. + //< + + virtual std::string getPlatformIdentifier() const; + //> This method returns an identifier to indicate the specific platform + // that was used to acquire the image. This ID must unique among + // platforms for a given model name. It is used to determine parameter + // correlation sharing. Equivalent to vehicle or aircraft tail number. + // + // This method may return an empty string if the platform ID is unknown. + //< + + virtual std::string getCollectionIdentifier() const; + //> This method returns an identifer to indicate a collection activity + // common to a set of images. This ID must be unique among collection + // activities for a given model name. It is used to determine parameter + // correlation and sharing. + //< + + virtual std::string getTrajectoryIdentifier() const; + //> This method returns an identifier to indicate a trajectory common + // to a set of images. This ID must be unique among trajectories + // for a given model name. It is used to determine parameter + // correlation and sharing. + //< + + virtual std::string getSensorType() const; + //> This method returns a description of the sensor type (EO, IR, SAR, + // etc). See csm.h for a list of common types. Should return + // CSM_SENSOR_TYPE_UNKNOWN if the sensor type is unknown. + //< + + virtual std::string getSensorMode() const; + //> This method returns a description of the sensor mode (FRAME, + // PUSHBROOM, SPOT, SCAN, etc). See csm.h for a list of common modes. + // Should return CSM_SENSOR_MODE_UNKNOWN if the sensor mode is unknown. + //< + + virtual std::string getReferenceDateAndTime() const; + //> This method returns an approximate date and time at which the + // image was taken. The returned string follows the ISO 8601 standard. + // + //- Precision Format Example + //- year yyyy "1961" + //- month yyyymm "196104" + //- day yyyymmdd "19610420" + //- hour yyyymmddThh "19610420T20" + //- minute yyyymmddThhmm "19610420T2000" + //- second yyyymmddThhmmss "19610420T200000" + //< + + //--- + // Sensor Model State + //--- + // virtual std::string setModelState(std::string stateString) const; + //> This method returns a string containing the data to exactly recreate + // the current model. It can be used to restore this model to a + // previous state with the replaceModelState method or create a new + // model object that is identical to this model. + // The string could potentially be saved to a file for later use. + // An empty string is returned if it is not possible to save the + // current state. + //< + + virtual csm::Ellipsoid getEllipsoid() const; + //> This method returns the planetary ellipsoid. + //< + + virtual void setEllipsoid(const csm::Ellipsoid& ellipsoid); + //> This method sets the planetary ellipsoid. + //< + + void calculateAttitudeCorrection(const double& time, + const std::vector& adj, + double attCorr[9]) const; + + virtual csm::EcefVector getSunPosition(const double imageTime) const; + //> This method returns the position of the sun at the time the image point + // was recorded. If multiple sun positions are available, the method uses + // lagrange interpolation. If one sun position and at least one sun velocity + // are available, then the position is calculated using linear extrapolation. + // If only one sun position is available, then that value is returned. + + private: + void determineSensorCovarianceInImageSpace(csm::EcefCoord& gp, + double sensor_cov[4]) const; + + // Some state data values not found in the support data require a + // sensor model in order to be set. + void updateState(); + + // This method returns the value of the specified adjustable parameter + // with the associated adjustment added in. + double getValue(int index, const std::vector& adjustments) const; + + // This private form of the g2i method is used to ensure thread safety. + virtual csm::ImageCoord groundToImage( + const csm::EcefCoord& groundPt, const std::vector& adjustments, + double desiredPrecision = 0.001, double* achievedPrecision = NULL, csm::WarningList* warnings = NULL) const; - void reconstructSensorDistortion( - double& focalX, - double& focalY, - const double& desiredPrecision) const; - - void getQuaternions(const double& time, - double quaternion[4]) const; - -// This method computes the imaging locus. -// imaging locus : set of ground points associated with an image pixel. - void losToEcf( - const double& line, // CSM image convention - const double& sample, // UL pixel center == (0.5, 0.5) - const std::vector& adj, // Parameter Adjustments for partials - double& xc, // output sensor x coordinate - double& yc, // output sensor y coordinate - double& zc, // output sensor z coordinate - double& vx, // output sensor x velocity - double& vy, // output sensor y velocity - double& vz, // output sensor z cvelocity - double& bodyFixedX, // output line-of-sight x coordinate - double& bodyFixedY, // output line-of-sight y coordinate - double& bodyFixedZ ) const; - - // Computes the LOS correction due to light aberration - void lightAberrationCorr( - const double& vx, - const double& vy, - const double& vz, - const double& xl, - const double& yl, - const double& zl, - double& dxl, - double& dyl, - double& dzl) const; - - // Intersects a LOS at a specified height above the ellipsoid. - void losEllipsoidIntersect ( - const double& height, - const double& xc, - const double& yc, - const double& zc, - const double& xl, - const double& yl, - const double& zl, - double& x, - double& y, - double& z, - double& achieved_precision, - const double& desired_precision) const; - - // Intersects the los with a specified plane. - void losPlaneIntersect ( - const double& xc, // input: camera x coordinate - const double& yc, // input: camera y coordinate - const double& zc, // input: camera z coordinate - const double& xl, // input: component x image ray - const double& yl, // input: component y image ray - const double& zl, // input: component z image ray - double& x, // input/output: ground x coordinate - double& y, // input/output: ground y coordinate - double& z, // input/output: ground z coordinate - int& mode ) const; // input: -1 fixed component to be computed - // 0(X), 1(Y), or 2(Z) fixed - // output: 0(X), 1(Y), or 2(Z) fixed - // Intersects a los associated with an image coordinate with a specified plane. - void imageToPlane( - const double& line, // CSM Origin UL corner of UL pixel - const double& sample, // CSM Origin UL corner of UL pixel - const double& height, - const std::vector &adj, - double& x, - double& y, - double& z, - int& mode ) const; - - // determines the sensor velocity accounting for parameter adjustments. - void getAdjSensorPosVel( - const double& time, - const std::vector &adj, - double& xc, - double& yc, - double& zc, - double& vx, - double& vy, - double& vz) const; - - // Computes the imaging locus that would view a ground point at a specific - // time. Computationally, this is the opposite of losToEcf. - std::vector computeDetectorView( - const double& time, // The time to use the EO at - const csm::EcefCoord& groundPoint, // The ground coordinate - const std::vector& adj // Parameter Adjustments for partials - ) const; - - // The linear approximation for the sensor model is used as the starting point - // for iterative rigorous calculations. - void computeLinearApproximation( - const csm::EcefCoord &gp, - csm::ImageCoord &ip) const; - - // Initial setup of the linear approximation - void setLinearApproximation(); - - // Compute the determinant of a 3x3 matrix - double determinant3x3(double mat[9]) const; - - - - csm::NoCorrelationModel _no_corr_model; // A way to report no correlation between images is supported - std::vector _no_adjustment; // A vector of zeros indicating no internal adjustment - - // The following support the linear approximation of the sensor model - double _u0; - double _du_dx; - double _du_dy; - double _du_dz; - double _v0; - double _dv_dx; - double _dv_dy; - double _dv_dz; - bool _linear; // flag indicating if linear approximation is useful. + void reconstructSensorDistortion(double& focalX, double& focalY, + const double& desiredPrecision) const; + + void getQuaternions(const double& time, double quaternion[4]) const; + + // This method computes the imaging locus. + // imaging locus : set of ground points associated with an image pixel. + void losToEcf( + const double& line, // CSM image convention + const double& sample, // UL pixel center == (0.5, 0.5) + const std::vector& adj, // Parameter Adjustments for partials + double& xc, // output sensor x coordinate + double& yc, // output sensor y coordinate + double& zc, // output sensor z coordinate + double& vx, // output sensor x velocity + double& vy, // output sensor y velocity + double& vz, // output sensor z cvelocity + double& bodyFixedX, // output line-of-sight x coordinate + double& bodyFixedY, // output line-of-sight y coordinate + double& bodyFixedZ) const; + + // Computes the LOS correction due to light aberration + void lightAberrationCorr(const double& vx, const double& vy, const double& vz, + const double& xl, const double& yl, const double& zl, + double& dxl, double& dyl, double& dzl) const; + + // Intersects a LOS at a specified height above the ellipsoid. + void losEllipsoidIntersect(const double& height, const double& xc, + const double& yc, const double& zc, + const double& xl, const double& yl, + const double& zl, double& x, double& y, double& z, + double& achieved_precision, + const double& desired_precision) const; + + // Intersects the los with a specified plane. + void losPlaneIntersect( + const double& xc, // input: camera x coordinate + const double& yc, // input: camera y coordinate + const double& zc, // input: camera z coordinate + const double& xl, // input: component x image ray + const double& yl, // input: component y image ray + const double& zl, // input: component z image ray + double& x, // input/output: ground x coordinate + double& y, // input/output: ground y coordinate + double& z, // input/output: ground z coordinate + int& mode) const; // input: -1 fixed component to be computed + // 0(X), 1(Y), or 2(Z) fixed + // output: 0(X), 1(Y), or 2(Z) fixed + // Intersects a los associated with an image coordinate with a specified + // plane. + void imageToPlane(const double& line, // CSM Origin UL corner of UL pixel + const double& sample, // CSM Origin UL corner of UL pixel + const double& height, const std::vector& adj, + double& x, double& y, double& z, int& mode) const; + + // determines the sensor velocity accounting for parameter adjustments. + void getAdjSensorPosVel(const double& time, const std::vector& adj, + double& xc, double& yc, double& zc, double& vx, + double& vy, double& vz) const; + + // Computes the imaging locus that would view a ground point at a specific + // time. Computationally, this is the opposite of losToEcf. + std::vector computeDetectorView( + const double& time, // The time to use the EO at + const csm::EcefCoord& groundPoint, // The ground coordinate + const std::vector& adj // Parameter Adjustments for partials + ) const; + + // The linear approximation for the sensor model is used as the starting point + // for iterative rigorous calculations. + void computeLinearApproximation(const csm::EcefCoord& gp, + csm::ImageCoord& ip) const; + + // Initial setup of the linear approximation + void setLinearApproximation(); + + // Compute the determinant of a 3x3 matrix + double determinant3x3(double mat[9]) const; + + csm::NoCorrelationModel _no_corr_model; // A way to report no correlation + // between images is supported + std::vector + _no_adjustment; // A vector of zeros indicating no internal adjustment + + // The following support the linear approximation of the sensor model + double _u0; + double _du_dx; + double _du_dy; + double _du_dz; + double _v0; + double _dv_dx; + double _dv_dy; + double _dv_dz; + bool _linear; // flag indicating if linear approximation is useful. }; #endif diff --git a/include/usgscsm/UsgsAstroPlugin.h b/include/usgscsm/UsgsAstroPlugin.h index 3536875b0..acb13092d 100644 --- a/include/usgscsm/UsgsAstroPlugin.h +++ b/include/usgscsm/UsgsAstroPlugin.h @@ -1,65 +1,64 @@ #ifndef UsgsAstroPlugin_h #define UsgsAstroPlugin_h - #include #include #include #include -#include "spdlog/spdlog.h" #include "spdlog/sinks/basic_file_sink.h" +#include "spdlog/spdlog.h" class UsgsAstroPlugin : public csm::Plugin { + public: + UsgsAstroPlugin(); + ~UsgsAstroPlugin(); - public: - UsgsAstroPlugin(); - ~UsgsAstroPlugin(); - - virtual std::string getStateFromISD(csm::Isd imageSupportData) const; - virtual std::string getPluginName() const; - virtual std::string getManufacturer() const; - virtual std::string getReleaseDate() const; - virtual csm::Version getCsmVersion() const; - virtual size_t getNumModels() const; - virtual std::string getModelName(size_t modelIndex) const; - virtual std::string getModelFamily(size_t modelIndex) const; - virtual csm::Version getModelVersion(const std::string &modelName) const; - virtual bool canModelBeConstructedFromState(const std::string &modelName, - const std::string &modelState, - csm::WarningList *warnings = NULL) const; - virtual bool canModelBeConstructedFromISD(const csm::Isd &imageSupportData, - const std::string &modelName, - csm::WarningList *warnings = NULL) const; - virtual csm::Model *constructModelFromState(const std::string &modelState, - csm::WarningList *warnings = NULL) const; - virtual csm::Model *constructModelFromISD(const csm::Isd &imageSupportData, - const std::string &modelName, - csm::WarningList *warnings = NULL) const; - virtual std::string getModelNameFromModelState(const std::string &modelState, - csm::WarningList *warnings = NULL) const; - virtual bool canISDBeConvertedToModelState(const csm::Isd &imageSupportData, - const std::string &modelName, - csm::WarningList *warnings = NULL) const; - virtual std::string convertISDToModelState(const csm::Isd &imageSupportData, - const std::string &modelName, - csm::WarningList *warnings = NULL) const; + virtual std::string getStateFromISD(csm::Isd imageSupportData) const; + virtual std::string getPluginName() const; + virtual std::string getManufacturer() const; + virtual std::string getReleaseDate() const; + virtual csm::Version getCsmVersion() const; + virtual size_t getNumModels() const; + virtual std::string getModelName(size_t modelIndex) const; + virtual std::string getModelFamily(size_t modelIndex) const; + virtual csm::Version getModelVersion(const std::string &modelName) const; + virtual bool canModelBeConstructedFromState( + const std::string &modelName, const std::string &modelState, + csm::WarningList *warnings = NULL) const; + virtual bool canModelBeConstructedFromISD( + const csm::Isd &imageSupportData, const std::string &modelName, + csm::WarningList *warnings = NULL) const; + virtual csm::Model *constructModelFromState( + const std::string &modelState, csm::WarningList *warnings = NULL) const; + virtual csm::Model *constructModelFromISD( + const csm::Isd &imageSupportData, const std::string &modelName, + csm::WarningList *warnings = NULL) const; + virtual std::string getModelNameFromModelState( + const std::string &modelState, csm::WarningList *warnings = NULL) const; + virtual bool canISDBeConvertedToModelState( + const csm::Isd &imageSupportData, const std::string &modelName, + csm::WarningList *warnings = NULL) const; + virtual std::string convertISDToModelState( + const csm::Isd &imageSupportData, const std::string &modelName, + csm::WarningList *warnings = NULL) const; - std::string loadImageSupportData(const csm::Isd &imageSupportDataOriginal) const; + std::string loadImageSupportData( + const csm::Isd &imageSupportDataOriginal) const; - // TODO when implementing, add any other necessary members. + // TODO when implementing, add any other necessary members. -private: - static const UsgsAstroPlugin m_registeredPlugin; - static const std::string _PLUGIN_NAME; - static const std::string _MANUFACTURER_NAME; - static const std::string _RELEASE_DATE; - static const int _N_SENSOR_MODELS; + private: + static const UsgsAstroPlugin m_registeredPlugin; + static const std::string _PLUGIN_NAME; + static const std::string _MANUFACTURER_NAME; + static const std::string _RELEASE_DATE; + static const int _N_SENSOR_MODELS; - typedef csm::Model* (*sensorConstructor)(void); - static std::map MODELS; - std::shared_ptr m_logger; + typedef csm::Model *(*sensorConstructor)(void); + static std::map MODELS; + std::shared_ptr m_logger; }; #endif diff --git a/include/usgscsm/UsgsAstroSarSensorModel.h b/include/usgscsm/UsgsAstroSarSensorModel.h index eb9d27c7e..d5dac3f56 100644 --- a/include/usgscsm/UsgsAstroSarSensorModel.h +++ b/include/usgscsm/UsgsAstroSarSensorModel.h @@ -1,282 +1,267 @@ #ifndef __USGS_ASTRO_SAR_SENSORMODEL_H #define __USGS_ASTRO_SAR_SENSORMODEL_H +#include #include #include -#include #include "spdlog/spdlog.h" -class UsgsAstroSarSensorModel : public csm::RasterGM, virtual public csm::SettableEllipsoid -{ - - public: - enum LookDirection { - LEFT = 0, - RIGHT = 1 - }; +class UsgsAstroSarSensorModel : public csm::RasterGM, + virtual public csm::SettableEllipsoid { + public: + enum LookDirection { LEFT = 0, RIGHT = 1 }; - UsgsAstroSarSensorModel(); - ~UsgsAstroSarSensorModel() {} + UsgsAstroSarSensorModel(); + ~UsgsAstroSarSensorModel() {} - void reset(); + void reset(); - virtual void replaceModelState(const std::string& argState); + virtual void replaceModelState(const std::string& argState); - virtual std::string getModelState() const; + virtual std::string getModelState() const; - std::string constructStateFromIsd(const std::string imageSupportData, csm::WarningList *list); + std::string constructStateFromIsd(const std::string imageSupportData, + csm::WarningList* list); - std::string getModelNameFromModelState(const std::string& model_state); + std::string getModelNameFromModelState(const std::string& model_state); - virtual csm::ImageCoord groundToImage( - const csm::EcefCoord& groundPt, - double desiredPrecision = 0.001, - double* achievedPrecision = NULL, - csm::WarningList* warnings = NULL) const; + virtual csm::ImageCoord groundToImage( + const csm::EcefCoord& groundPt, double desiredPrecision = 0.001, + double* achievedPrecision = NULL, + csm::WarningList* warnings = NULL) const; - virtual csm::ImageCoord groundToImage( - const csm::EcefCoord& groundPt, - const std::vector adjustments, - double desired_precision = 0.001, - double* achieved_precision = NULL, - csm::WarningList* warnings = NULL) const; + virtual csm::ImageCoord groundToImage( + const csm::EcefCoord& groundPt, const std::vector adjustments, + double desired_precision = 0.001, double* achieved_precision = NULL, + csm::WarningList* warnings = NULL) const; - virtual csm::ImageCoordCovar groundToImage( - const csm::EcefCoordCovar& groundPt, - double desiredPrecision = 0.001, - double* achievedPrecision = NULL, - csm::WarningList* warnings = NULL) const; + virtual csm::ImageCoordCovar groundToImage( + const csm::EcefCoordCovar& groundPt, double desiredPrecision = 0.001, + double* achievedPrecision = NULL, + csm::WarningList* warnings = NULL) const; - virtual csm::EcefCoord imageToGround( - const csm::ImageCoord& imagePt, - double height, - double desiredPrecision = 0.001, - double* achievedPrecision = NULL, - csm::WarningList* warnings = NULL) const; + virtual csm::EcefCoord imageToGround(const csm::ImageCoord& imagePt, + double height, + double desiredPrecision = 0.001, + double* achievedPrecision = NULL, + csm::WarningList* warnings = NULL) const; - virtual csm::EcefCoordCovar imageToGround( - const csm::ImageCoordCovar& imagePt, - double height, - double heightVariance, - double desiredPrecision = 0.001, - double* achievedPrecision = NULL, - csm::WarningList* warnings = NULL) const; + virtual csm::EcefCoordCovar imageToGround( + const csm::ImageCoordCovar& imagePt, double height, double heightVariance, + double desiredPrecision = 0.001, double* achievedPrecision = NULL, + csm::WarningList* warnings = NULL) const; - virtual csm::EcefLocus imageToProximateImagingLocus( - const csm::ImageCoord& imagePt, - const csm::EcefCoord& groundPt, - double desiredPrecision = 0.001, - double* achievedPrecision = NULL, - csm::WarningList* warnings = NULL) const; + virtual csm::EcefLocus imageToProximateImagingLocus( + const csm::ImageCoord& imagePt, const csm::EcefCoord& groundPt, + double desiredPrecision = 0.001, double* achievedPrecision = NULL, + csm::WarningList* warnings = NULL) const; - virtual csm::EcefLocus imageToRemoteImagingLocus( - const csm::ImageCoord& imagePt, - double desiredPrecision = 0.001, - double* achievedPrecision = NULL, - csm::WarningList* warnings = NULL) const; + virtual csm::EcefLocus imageToRemoteImagingLocus( + const csm::ImageCoord& imagePt, double desiredPrecision = 0.001, + double* achievedPrecision = NULL, + csm::WarningList* warnings = NULL) const; - virtual csm::ImageCoord getImageStart() const; + virtual csm::ImageCoord getImageStart() const; - virtual csm::ImageVector getImageSize() const; + virtual csm::ImageVector getImageSize() const; - virtual std::pair getValidImageRange() const; + virtual std::pair getValidImageRange() + const; - virtual std::pair getValidHeightRange() const; + virtual std::pair getValidHeightRange() const; - virtual csm::EcefVector getIlluminationDirection(const csm::EcefCoord& groundPt) const; + virtual csm::EcefVector getIlluminationDirection( + const csm::EcefCoord& groundPt) const; - virtual double getImageTime(const csm::ImageCoord& imagePt) const; + virtual double getImageTime(const csm::ImageCoord& imagePt) const; - virtual csm::EcefVector getSpacecraftPosition(double time) const; + virtual csm::EcefVector getSpacecraftPosition(double time) const; - virtual csm::EcefVector getAdjustedSpacecraftPosition(double time, std::vector adj) const; + virtual csm::EcefVector getAdjustedSpacecraftPosition( + double time, std::vector adj) const; - virtual csm::EcefCoord getSensorPosition(const csm::ImageCoord& imagePt) const; + virtual csm::EcefCoord getSensorPosition( + const csm::ImageCoord& imagePt) const; - virtual csm::EcefCoord getSensorPosition(double time) const; + virtual csm::EcefCoord getSensorPosition(double time) const; - virtual csm::EcefCoord getAdjustedSensorPosition(double time, - std::vector adjustments) const; + virtual csm::EcefCoord getAdjustedSensorPosition( + double time, std::vector adjustments) const; - virtual csm::EcefVector getSensorVelocity(const csm::ImageCoord& imagePt) const; + virtual csm::EcefVector getSensorVelocity( + const csm::ImageCoord& imagePt) const; - virtual csm::EcefVector getSensorVelocity(double time) const; + virtual csm::EcefVector getSensorVelocity(double time) const; - virtual csm::EcefVector getAdjustedSensorVelocity(double time, - std::vector adjustments) const; + virtual csm::EcefVector getAdjustedSensorVelocity( + double time, std::vector adjustments) const; - virtual csm::RasterGM::SensorPartials computeSensorPartials( - int index, - const csm::EcefCoord& groundPt, - double desiredPrecision = 0.001, - double* achievedPrecision = NULL, - csm::WarningList* warnings = NULL) const; + virtual csm::RasterGM::SensorPartials computeSensorPartials( + int index, const csm::EcefCoord& groundPt, + double desiredPrecision = 0.001, double* achievedPrecision = NULL, + csm::WarningList* warnings = NULL) const; - virtual csm::RasterGM::SensorPartials computeSensorPartials( - int index, - const csm::ImageCoord& imagePt, - const csm::EcefCoord& groundPt, - double desiredPrecision = 0.001, - double* achievedPrecision = NULL, - csm::WarningList* warnings = NULL) const; + virtual csm::RasterGM::SensorPartials computeSensorPartials( + int index, const csm::ImageCoord& imagePt, const csm::EcefCoord& groundPt, + double desiredPrecision = 0.001, double* achievedPrecision = NULL, + csm::WarningList* warnings = NULL) const; - virtual std::vector computeGroundPartials(const csm::EcefCoord& groundPt) const; + virtual std::vector computeGroundPartials( + const csm::EcefCoord& groundPt) const; - virtual const csm::CorrelationModel& getCorrelationModel() const; + virtual const csm::CorrelationModel& getCorrelationModel() const; - virtual std::vector getUnmodeledCrossCovariance( - const csm::ImageCoord& pt1, - const csm::ImageCoord& pt2) const; + virtual std::vector getUnmodeledCrossCovariance( + const csm::ImageCoord& pt1, const csm::ImageCoord& pt2) const; - virtual csm::EcefCoord getReferencePoint() const; + virtual csm::EcefCoord getReferencePoint() const; - virtual void setReferencePoint(const csm::EcefCoord& groundPt); + virtual void setReferencePoint(const csm::EcefCoord& groundPt); - virtual int getNumParameters() const; + virtual int getNumParameters() const; - virtual std::string getParameterName(int index) const; + virtual std::string getParameterName(int index) const; - virtual std::string getParameterUnits(int index) const; + virtual std::string getParameterUnits(int index) const; - virtual bool hasShareableParameters() const; + virtual bool hasShareableParameters() const; - virtual bool isParameterShareable(int index) const; + virtual bool isParameterShareable(int index) const; - virtual csm::SharingCriteria getParameterSharingCriteria(int index) const; + virtual csm::SharingCriteria getParameterSharingCriteria(int index) const; - virtual double getParameterValue(int index) const; + virtual double getParameterValue(int index) const; - virtual void setParameterValue(int index, double value); + virtual void setParameterValue(int index, double value); - virtual csm::param::Type getParameterType(int index) const; + virtual csm::param::Type getParameterType(int index) const; - virtual void setParameterType(int index, csm::param::Type pType); + virtual void setParameterType(int index, csm::param::Type pType); - virtual double getParameterCovariance( - int index1, - int index2) const; + virtual double getParameterCovariance(int index1, int index2) const; - virtual void setParameterCovariance( - int index1, - int index2, - double covariance); + virtual void setParameterCovariance(int index1, int index2, + double covariance); - virtual int getNumGeometricCorrectionSwitches() const; + virtual int getNumGeometricCorrectionSwitches() const; - virtual std::string getGeometricCorrectionName(int index) const; + virtual std::string getGeometricCorrectionName(int index) const; - virtual void setGeometricCorrectionSwitch(int index, - bool value, - csm::param::Type pType); + virtual void setGeometricCorrectionSwitch(int index, bool value, + csm::param::Type pType); - virtual bool getGeometricCorrectionSwitch(int index) const; + virtual bool getGeometricCorrectionSwitch(int index) const; - virtual std::vector getCrossCovarianceMatrix( - const csm::GeometricModel& comparisonModel, - csm::param::Set pSet = csm::param::VALID, - const csm::GeometricModel::GeometricModelList& otherModels = csm::GeometricModel::GeometricModelList()) const; + virtual std::vector getCrossCovarianceMatrix( + const csm::GeometricModel& comparisonModel, + csm::param::Set pSet = csm::param::VALID, + const csm::GeometricModel::GeometricModelList& otherModels = + csm::GeometricModel::GeometricModelList()) const; - virtual csm::Version getVersion() const; + virtual csm::Version getVersion() const; - virtual std::string getModelName() const; + virtual std::string getModelName() const; - virtual std::string getPedigree() const; + virtual std::string getPedigree() const; - virtual std::string getImageIdentifier() const; + virtual std::string getImageIdentifier() const; - virtual void setImageIdentifier( - const std::string& imageId, - csm::WarningList* warnings = NULL); + virtual void setImageIdentifier(const std::string& imageId, + csm::WarningList* warnings = NULL); - virtual std::string getSensorIdentifier() const; + virtual std::string getSensorIdentifier() const; - virtual std::string getPlatformIdentifier() const; + virtual std::string getPlatformIdentifier() const; - virtual std::string getCollectionIdentifier() const; + virtual std::string getCollectionIdentifier() const; - virtual std::string getTrajectoryIdentifier() const; + virtual std::string getTrajectoryIdentifier() const; - virtual std::string getSensorType() const; + virtual std::string getSensorType() const; - virtual std::string getSensorMode() const; + virtual std::string getSensorMode() const; - virtual std::string getReferenceDateAndTime() const; + virtual std::string getReferenceDateAndTime() const; - virtual csm::Ellipsoid getEllipsoid() const; + virtual csm::Ellipsoid getEllipsoid() const; - virtual void setEllipsoid(const csm::Ellipsoid &ellipsoid); + virtual void setEllipsoid(const csm::Ellipsoid& ellipsoid); - //////////////////// - // Helper methods // - //////////////////// - void determineSensorCovarianceInImageSpace( - csm::EcefCoord &gp, - double sensor_cov[4]) const; - double dopplerShift(csm::EcefCoord groundPt, double tolerance, std::vector adj) const; + //////////////////// + // Helper methods // + //////////////////// + void determineSensorCovarianceInImageSpace(csm::EcefCoord& gp, + double sensor_cov[4]) const; + double dopplerShift(csm::EcefCoord groundPt, double tolerance, + std::vector adj) const; - double slantRange(csm::EcefCoord surfPt, double time, std::vector adj) const; + double slantRange(csm::EcefCoord surfPt, double time, + std::vector adj) const; - double slantRangeToGroundRange(const csm::EcefCoord& groundPt, double time, double slantRange, double tolerance) const; + double slantRangeToGroundRange(const csm::EcefCoord& groundPt, double time, + double slantRange, double tolerance) const; - double groundRangeToSlantRange(double groundRange, const std::vector &coeffs) const; + double groundRangeToSlantRange(double groundRange, + const std::vector& coeffs) const; - csm::EcefVector getSunPosition(const double imageTime) const; - std::vector getRangeCoefficients(double time) const; - double getValue(int index, const std::vector &adjustments) const; + csm::EcefVector getSunPosition(const double imageTime) const; + std::vector getRangeCoefficients(double time) const; + double getValue(int index, const std::vector& adjustments) const; - //////////////////////////// - // Model static variables // - //////////////////////////// + //////////////////////////// + // Model static variables // + //////////////////////////// - static const std::string _SENSOR_MODEL_NAME; - static const int NUM_PARAM_TYPES; - static const std::string PARAM_STRING_ALL[]; - static const csm::param::Type PARAM_CHAR_ALL[]; - static const int NUM_PARAMETERS; - static const std::string PARAMETER_NAME[]; - csm::NoCorrelationModel _NO_CORR_MODEL; // A way to report no correlation between images is supported - std::vector _NO_ADJUSTMENT; + static const std::string _SENSOR_MODEL_NAME; + static const int NUM_PARAM_TYPES; + static const std::string PARAM_STRING_ALL[]; + static const csm::param::Type PARAM_CHAR_ALL[]; + static const int NUM_PARAMETERS; + static const std::string PARAMETER_NAME[]; + csm::NoCorrelationModel _NO_CORR_MODEL; // A way to report no correlation + // between images is supported + std::vector _NO_ADJUSTMENT; - /////////////////////////// - // Model state variables // - /////////////////////////// - std::string m_imageIdentifier; - std::string m_platformName; - std::string m_sensorName; - int m_nLines; - int m_nSamples; - double m_exposureDuration; - double m_scaledPixelWidth; - double m_startingEphemerisTime; - double m_centerEphemerisTime; - double m_endingEphemerisTime; - double m_majorAxis; - double m_minorAxis; - std::string m_platformIdentifier; - std::string m_sensorIdentifier; - std::string m_trajectoryIdentifier; - std::string m_collectionIdentifier; - double m_refElevation; - double m_minElevation; - double m_maxElevation; - double m_dtEphem; - double m_t0Ephem; - std::vector m_scaleConversionCoefficients; - std::vector m_scaleConversionTimes; - std::vector m_positions; - std::vector m_velocities; - std::vector m_currentParameterValue; - std::vector m_parameterType; - csm::EcefCoord m_referencePointXyz; - std::vector m_covariance; - std::vector m_sunPosition; - std::vector m_sunVelocity; - double m_wavelength; - LookDirection m_lookDirection; - std::vector m_noAdjustments; - - std::shared_ptr m_logger = spdlog::get("usgscsm_logger"); + /////////////////////////// + // Model state variables // + /////////////////////////// + std::string m_imageIdentifier; + std::string m_platformName; + std::string m_sensorName; + int m_nLines; + int m_nSamples; + double m_exposureDuration; + double m_scaledPixelWidth; + double m_startingEphemerisTime; + double m_centerEphemerisTime; + double m_endingEphemerisTime; + double m_majorAxis; + double m_minorAxis; + std::string m_platformIdentifier; + std::string m_sensorIdentifier; + std::string m_trajectoryIdentifier; + std::string m_collectionIdentifier; + double m_refElevation; + double m_minElevation; + double m_maxElevation; + double m_dtEphem; + double m_t0Ephem; + std::vector m_scaleConversionCoefficients; + std::vector m_scaleConversionTimes; + std::vector m_positions; + std::vector m_velocities; + std::vector m_currentParameterValue; + std::vector m_parameterType; + csm::EcefCoord m_referencePointXyz; + std::vector m_covariance; + std::vector m_sunPosition; + std::vector m_sunVelocity; + double m_wavelength; + LookDirection m_lookDirection; + std::vector m_noAdjustments; + + std::shared_ptr m_logger = spdlog::get("usgscsm_logger"); }; #endif diff --git a/include/usgscsm/Utilities.h b/include/usgscsm/Utilities.h index cb7f55160..15c166f50 100644 --- a/include/usgscsm/Utilities.h +++ b/include/usgscsm/Utilities.h @@ -3,168 +3,170 @@ #include "Distortion.h" -#include #include -#include #include +#include +#include #include -#include #include +#include // 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 void computeDistortedFocalPlaneCoordinates( - const double& line, - const double& sample, - const double& sampleOrigin, - const double& lineOrigin, - const double& sampleSumming, - const double& lineSumming, - const double& startingSample, - const double& startingLine, - const double iTransS[], - const double iTransL[], - double &distortedX, - double &distortedY); - -void computePixel( - const double& distortedX, - const double& distortedY, - const double& sampleOrigin, - const double& lineOrigin, - const double& sampleSumming, - const double& lineSumming, - const double& startingSample, - const double& startingLine, - const double iTransS[], - const double iTransL[], - double &line, - double &sample); - -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, - double cameraLook[]); - -void lagrangeInterp ( - const int& numTime, - const double* valueArray, - const double& startTime, - const double& delTime, - const double& time, - const int& vectorLength, - const int& i_order, - double* valueVector); + const double &line, const double &sample, const double &sampleOrigin, + const double &lineOrigin, const double &sampleSumming, + const double &lineSumming, const double &startingSample, + const double &startingLine, const double iTransS[], const double iTransL[], + double &distortedX, double &distortedY); + +void computePixel(const double &distortedX, const double &distortedY, + const double &sampleOrigin, const double &lineOrigin, + const double &sampleSumming, const double &lineSumming, + const double &startingSample, const double &startingLine, + const double iTransS[], const double iTransL[], double &line, + double &sample); + +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, + double cameraLook[]); + +void lagrangeInterp(const int &numTime, const double *valueArray, + const double &startTime, const double &delTime, + const double &time, const int &vectorLength, + const int &i_order, double *valueVector); // Brent's algorithm for finding the roots of a function -// Arguments are two inputs that bracket a root, the function, and a convergence tolerance -double brentRoot( - double lowerBound, - double upperBound, - std::function func, - double epsilon = 1e-10); +// Arguments are two inputs that bracket a root, the function, and a convergence +// tolerance +double brentRoot(double lowerBound, double upperBound, + std::function func, double epsilon = 1e-10); // Evaluate a polynomial function. -// Coefficients should be ordered least order to greatest I.E. {1, 2, 3} is 1 + 2x + 3x^2 -double evaluatePolynomial( - const std::vector &coeffs, - double x); +// Coefficients should be ordered least order to greatest I.E. {1, 2, 3} is 1 + +// 2x + 3x^2 +double evaluatePolynomial(const std::vector &coeffs, double x); // Evaluate the derivative of a polynomial function. -// Coefficients should be ordered least order to greatest I.E. {1, 2, 3} is 1 + 2x + 3x^2 -double evaluatePolynomialDerivative( - const std::vector &coeffs, - double x); +// Coefficients should be ordered least order to greatest I.E. {1, 2, 3} is 1 + +// 2x + 3x^2 +double evaluatePolynomialDerivative(const std::vector &coeffs, + double x); // Find a root of a polynomial using Newton's method. -// Coefficients should be ordered least order to greatest I.E. {1, 2, 3} is 1 + 2x + 3x^2 -double polynomialRoot( - const std::vector &coeffs, - double guess, - double threshold = 1e-10, - int maxIterations = 30); - -double computeEllipsoidElevation( - double x, - double y, - double z, - double semiMajor, - double semiMinor, - double desired_precision = 0.001, - double* achieved_precision = nullptr); +// Coefficients should be ordered least order to greatest I.E. {1, 2, 3} is 1 + +// 2x + 3x^2 +double polynomialRoot(const std::vector &coeffs, double guess, + double threshold = 1e-10, int maxIterations = 30); + +double computeEllipsoidElevation(double x, double y, double z, double semiMajor, + double semiMinor, + double desired_precision = 0.001, + double *achieved_precision = nullptr); // Vector operations csm::EcefVector operator*(double scalar, const csm::EcefVector &vec); csm::EcefVector operator*(const csm::EcefVector &vec, double scalar); csm::EcefVector operator/(const csm::EcefVector &vec, double scalar); -csm::EcefVector operator+(const csm::EcefVector &vec1, const csm::EcefVector &vec2); -csm::EcefVector operator-(const csm::EcefVector &vec1, const csm::EcefVector &vec2); +csm::EcefVector operator+(const csm::EcefVector &vec1, + const csm::EcefVector &vec2); +csm::EcefVector operator-(const csm::EcefVector &vec1, + const csm::EcefVector &vec2); double dot(const csm::EcefVector &vec1, const csm::EcefVector &vec2); csm::EcefVector cross(const csm::EcefVector &vec1, const csm::EcefVector &vec2); double norm(const csm::EcefVector &vec); csm::EcefVector normalized(const csm::EcefVector &vec); -// The projection of vec1 onto vec2. The component of vec1 that is parallel to vec2 -csm::EcefVector projection(const csm::EcefVector &vec1, const csm::EcefVector &vec2); -// The rejection of vec1 onto vec2. The component of vec1 that is orthogonal to vec2 -csm::EcefVector rejection(const csm::EcefVector &vec1, const csm::EcefVector &vec2); +// The projection of vec1 onto vec2. The component of vec1 that is parallel to +// vec2 +csm::EcefVector projection(const csm::EcefVector &vec1, + const csm::EcefVector &vec2); +// The rejection of vec1 onto vec2. The component of vec1 that is orthogonal to +// vec2 +csm::EcefVector rejection(const csm::EcefVector &vec1, + const csm::EcefVector &vec2); // Methods for checking/accessing the ISD -double metric_conversion(double val, std::string from, std::string to="m"); -std::string getSensorModelName(nlohmann::json isd, csm::WarningList *list=nullptr); -std::string getImageId(nlohmann::json isd, csm::WarningList *list=nullptr); -std::string getSensorName(nlohmann::json isd, csm::WarningList *list=nullptr); -std::string getPlatformName(nlohmann::json isd, csm::WarningList *list=nullptr); -std::string getLogFile(nlohmann::json isd, csm::WarningList *list=nullptr); -int getTotalLines(nlohmann::json isd, csm::WarningList *list=nullptr); -int getTotalSamples(nlohmann::json isd, csm::WarningList *list=nullptr); -double getStartingTime(nlohmann::json isd, csm::WarningList *list=nullptr); -double getCenterTime(nlohmann::json isd, csm::WarningList *list=nullptr); -double getEndingTime(nlohmann::json isd, csm::WarningList *list=nullptr); -std::vector getIntegrationStartLines(std::vector> lineScanRate, csm::WarningList *list=nullptr); -std::vector getIntegrationStartTimes(std::vector> lineScanRate, csm::WarningList *list=nullptr); -std::vector getIntegrationTimes(std::vector> lineScanRate, csm::WarningList *list=nullptr); -double getExposureDuration(nlohmann::json isd, csm::WarningList *list=nullptr); -double getScaledPixelWidth(nlohmann::json isd, csm::WarningList *list=nullptr); -std::string getLookDirection(nlohmann::json isd, csm::WarningList *list=nullptr); -std::vector getScaleConversionCoefficients(nlohmann::json isd, csm::WarningList *list=nullptr); -std::vector getScaleConversionTimes(nlohmann::json isd, csm::WarningList *list=nullptr); -int getSampleSumming(nlohmann::json isd, csm::WarningList *list=nullptr); -int getLineSumming(nlohmann::json isd, csm::WarningList *list=nullptr); -double getFocalLength(nlohmann::json isd, csm::WarningList *list=nullptr); -double getFocalLengthEpsilon(nlohmann::json isd, csm::WarningList *list=nullptr); -std::vector getFocal2PixelLines(nlohmann::json isd, csm::WarningList *list=nullptr); -std::vector getFocal2PixelSamples(nlohmann::json isd, csm::WarningList *list=nullptr); -double getDetectorCenterLine(nlohmann::json isd, csm::WarningList *list=nullptr); -double getDetectorCenterSample(nlohmann::json isd, csm::WarningList *list=nullptr); -double getDetectorStartingLine(nlohmann::json isd, csm::WarningList *list=nullptr); -double getDetectorStartingSample(nlohmann::json isd, csm::WarningList *list=nullptr); -double getMinHeight(nlohmann::json isd, csm::WarningList *list=nullptr); -double getMaxHeight(nlohmann::json isd, csm::WarningList *list=nullptr); -double getSemiMajorRadius(nlohmann::json isd, csm::WarningList *list=nullptr); -double getSemiMinorRadius(nlohmann::json isd, csm::WarningList *list=nullptr); -DistortionType getDistortionModel(nlohmann::json isd, csm::WarningList *list=nullptr); -DistortionType getDistortionModel(int aleDistortionModel, csm::WarningList *list=nullptr); -std::vector getDistortionCoeffs(nlohmann::json isd, csm::WarningList *list=nullptr); -std::vector getRadialDistortion(nlohmann::json isd, csm::WarningList *list=nullptr); -std::vector getSunPositions(nlohmann::json isd, csm::WarningList *list=nullptr); -std::vector getSunVelocities(nlohmann::json isd, csm::WarningList *list=nullptr); -std::vector getSensorPositions(nlohmann::json isd, csm::WarningList *list=nullptr); -std::vector getSensorVelocities(nlohmann::json isd, csm::WarningList *list=nullptr); -std::vector getSensorOrientations(nlohmann::json isd, csm::WarningList *list=nullptr); -double getWavelength(nlohmann::json isd, csm::WarningList *list=nullptr); +double metric_conversion(double val, std::string from, std::string to = "m"); +std::string getSensorModelName(nlohmann::json isd, + csm::WarningList *list = nullptr); +std::string getImageId(nlohmann::json isd, csm::WarningList *list = nullptr); +std::string getSensorName(nlohmann::json isd, csm::WarningList *list = nullptr); +std::string getPlatformName(nlohmann::json isd, + csm::WarningList *list = nullptr); +std::string getLogFile(nlohmann::json isd, csm::WarningList *list = nullptr); +int getTotalLines(nlohmann::json isd, csm::WarningList *list = nullptr); +int getTotalSamples(nlohmann::json isd, csm::WarningList *list = nullptr); +double getStartingTime(nlohmann::json isd, csm::WarningList *list = nullptr); +double getCenterTime(nlohmann::json isd, csm::WarningList *list = nullptr); +double getEndingTime(nlohmann::json isd, csm::WarningList *list = nullptr); +std::vector getIntegrationStartLines( + std::vector> lineScanRate, + csm::WarningList *list = nullptr); +std::vector getIntegrationStartTimes( + std::vector> lineScanRate, + csm::WarningList *list = nullptr); +std::vector getIntegrationTimes( + std::vector> lineScanRate, + csm::WarningList *list = nullptr); +double getExposureDuration(nlohmann::json isd, + csm::WarningList *list = nullptr); +double getScaledPixelWidth(nlohmann::json isd, + csm::WarningList *list = nullptr); +std::string getLookDirection(nlohmann::json isd, + csm::WarningList *list = nullptr); +std::vector getScaleConversionCoefficients( + nlohmann::json isd, csm::WarningList *list = nullptr); +std::vector getScaleConversionTimes(nlohmann::json isd, + csm::WarningList *list = nullptr); +int getSampleSumming(nlohmann::json isd, csm::WarningList *list = nullptr); +int getLineSumming(nlohmann::json isd, csm::WarningList *list = nullptr); +double getFocalLength(nlohmann::json isd, csm::WarningList *list = nullptr); +double getFocalLengthEpsilon(nlohmann::json isd, + csm::WarningList *list = nullptr); +std::vector getFocal2PixelLines(nlohmann::json isd, + csm::WarningList *list = nullptr); +std::vector getFocal2PixelSamples(nlohmann::json isd, + csm::WarningList *list = nullptr); +double getDetectorCenterLine(nlohmann::json isd, + csm::WarningList *list = nullptr); +double getDetectorCenterSample(nlohmann::json isd, + csm::WarningList *list = nullptr); +double getDetectorStartingLine(nlohmann::json isd, + csm::WarningList *list = nullptr); +double getDetectorStartingSample(nlohmann::json isd, + csm::WarningList *list = nullptr); +double getMinHeight(nlohmann::json isd, csm::WarningList *list = nullptr); +double getMaxHeight(nlohmann::json isd, csm::WarningList *list = nullptr); +double getSemiMajorRadius(nlohmann::json isd, csm::WarningList *list = nullptr); +double getSemiMinorRadius(nlohmann::json isd, csm::WarningList *list = nullptr); +DistortionType getDistortionModel(nlohmann::json isd, + csm::WarningList *list = nullptr); +DistortionType getDistortionModel(int aleDistortionModel, + csm::WarningList *list = nullptr); +std::vector getDistortionCoeffs(nlohmann::json isd, + csm::WarningList *list = nullptr); +std::vector getRadialDistortion(nlohmann::json isd, + csm::WarningList *list = nullptr); +std::vector getSunPositions(nlohmann::json isd, + csm::WarningList *list = nullptr); +std::vector getSunVelocities(nlohmann::json isd, + csm::WarningList *list = nullptr); +std::vector getSensorPositions(nlohmann::json isd, + csm::WarningList *list = nullptr); +std::vector getSensorVelocities(nlohmann::json isd, + csm::WarningList *list = nullptr); +std::vector getSensorOrientations(nlohmann::json isd, + csm::WarningList *list = nullptr); +double getWavelength(nlohmann::json isd, csm::WarningList *list = nullptr); #endif diff --git a/src/Distortion.cpp b/src/Distortion.cpp index 8f4f313c1..15803b672 100644 --- a/src/Distortion.cpp +++ b/src/Distortion.cpp @@ -5,7 +5,6 @@ void distortionJacobian(double x, double y, double *jacobian, const std::vector opticalDistCoeffs) { - double d_dx[10]; d_dx[0] = 0; d_dx[1] = 1; @@ -29,10 +28,10 @@ void distortionJacobian(double x, double y, double *jacobian, d_dy[8] = 2 * x * y; d_dy[9] = 3 * y * y; - jacobian[0] = 0; // xx - jacobian[1] = 0; // xy - jacobian[2] = 0; // yx - jacobian[3] = 0; // yy + jacobian[0] = 0; // xx + jacobian[1] = 0; // xy + jacobian[2] = 0; // yx + jacobian[3] = 0; // yy int xPointer = 0; int yPointer = opticalDistCoeffs.size() / 2; @@ -45,21 +44,20 @@ void distortionJacobian(double x, double y, double *jacobian, } } - /** - * @description Compute distorted focal plane (dx,dy) coordinate given an undistorted focal - * plane (ux,uy) coordinate. This uses the third order Taylor approximation to the - * distortion model. + * @description Compute distorted focal plane (dx,dy) coordinate given an + * undistorted focal plane (ux,uy) coordinate. This uses the third order Taylor + * approximation to the distortion model. * * @param ux Undistored x * @param uy Undistored y * @param opticalDistCoeffs For both X and Y coefficients * - * @returns distortedPoint Newly adjusted focal plane coordinates as an x, y tuple + * @returns distortedPoint Newly adjusted focal plane coordinates as an x, y + * tuple */ void computeTransverseDistortion(double ux, double uy, double &dx, double &dy, const std::vector opticalDistCoeffs) { - double f[10]; f[0] = 1; f[1] = ux; @@ -84,33 +82,31 @@ void computeTransverseDistortion(double ux, double uy, double &dx, double &dy, } } - void removeDistortion(double dx, double dy, double &ux, double &uy, const std::vector opticalDistCoeffs, - DistortionType distortionType, - const double tolerance) { + DistortionType distortionType, const double tolerance) { ux = dx; uy = dy; switch (distortionType) { - // Compute undistorted focal plane coordinate given a distorted // coordinate set and the distortion coefficients case RADIAL: { double rr = dx * dx + dy * dy; if (rr > tolerance) { - double dr = opticalDistCoeffs[0] + (rr * (opticalDistCoeffs[1] + rr * opticalDistCoeffs[2])); + double dr = opticalDistCoeffs[0] + + (rr * (opticalDistCoeffs[1] + rr * opticalDistCoeffs[2])); ux = dx * (1.0 - dr); uy = dy * (1.0 - dr); } - } - break; + } break; - // Computes undistorted focal plane (x,y) coordinates given a distorted focal plane (x,y) - // coordinate. The undistorted coordinates are solved for using the Newton-Raphson - // method for root-finding if the distortionFunction method is invoked. + // Computes undistorted focal plane (x,y) coordinates given a distorted + // focal plane (x,y) coordinate. The undistorted coordinates are solved for + // using the Newton-Raphson method for root-finding if the + // distortionFunction method is invoked. case TRANSVERSE: { // Solve the distortion equation using the Newton-Raphson method. // Set the error tolerance to about one millionth of a NAC pixel. @@ -129,8 +125,8 @@ void removeDistortion(double dx, double dy, double &ux, double &uy, computeTransverseDistortion(x, y, fx, fy, opticalDistCoeffs); - for (int count = 1; ((fabs(fx) +fabs(fy)) > tolerance) && (count < maxTries); count++) { - + for (int count = 1; + ((fabs(fx) + fabs(fy)) > tolerance) && (count < maxTries); count++) { computeTransverseDistortion(x, y, fx, fy, opticalDistCoeffs); fx = dx - fx; @@ -139,7 +135,8 @@ void removeDistortion(double dx, double dy, double &ux, double &uy, distortionJacobian(x, y, jacobian, opticalDistCoeffs); // Jxx * Jyy - Jxy * Jyx - double determinant = jacobian[0] * jacobian[3] - jacobian[1] * jacobian[2]; + double determinant = + jacobian[0] * jacobian[3] - jacobian[1] * jacobian[2]; if (fabs(determinant) < 1E-6) { ux = x; uy = y; @@ -163,8 +160,7 @@ void removeDistortion(double dx, double dy, double &ux, double &uy, } // Otherwise method did not converge to a root within the maximum // number of iterations - } - break; + } break; case KAGUYALISM: { // Apply distortion correction @@ -189,17 +185,22 @@ void removeDistortion(double dx, double dy, double &ux, double &uy, // Coeffs should be [boresightX,x0,x1,x2,x3,boresightY,y0,y1,y2,y3] if (opticalDistCoeffs.size() != 10) { csm::Error::ErrorType errorType = csm::Error::INDEX_OUT_OF_RANGE; - std::string message = "Distortion coefficients for Kaguya LISM must be of size 10, got: " + std::to_string(opticalDistCoeffs.size()); + std::string message = + "Distortion coefficients for Kaguya LISM must be of size 10, " + "got: " + + std::to_string(opticalDistCoeffs.size()); std::string function = "removeDistortion"; throw csm::Error(errorType, message, function); } double boresightX = opticalDistCoeffs[0]; - std::vector odkx(opticalDistCoeffs.begin()+1, opticalDistCoeffs.begin()+5); + std::vector odkx(opticalDistCoeffs.begin() + 1, + opticalDistCoeffs.begin() + 5); double boresightY = opticalDistCoeffs[5]; - std::vector odky(opticalDistCoeffs.begin()+6, opticalDistCoeffs.begin()+10); + std::vector odky(opticalDistCoeffs.begin() + 6, + opticalDistCoeffs.begin() + 10); - double r2 = dx*dx + dy*dy; + double r2 = dx * dx + dy * dy; double r = sqrt(r2); double r3 = r2 * r; @@ -211,29 +212,28 @@ void removeDistortion(double dx, double dy, double &ux, double &uy, ux = dx + dr_x + boresightX; uy = dy + dr_y + boresightY; - } - break; + } break; // The dawn distortion model is "reversed" from other distortion models so // the remove function iteratively computes undistorted coordinates based on - // the distorted coordinates, rather than iteratively computing distorted coordinates - // to undistorted coordinates. + // the distorted coordinates, rather than iteratively computing distorted + // coordinates to undistorted coordinates. case DAWNFC: { double r2; - int numAttempts = 1; - bool done; + int numAttempts = 1; + bool done; /**************************************************************************** - * Pre-loop intializations - ****************************************************************************/ + * Pre-loop intializations + ****************************************************************************/ r2 = dy * dy + dx * dx; double guess_dx, guess_dy; double guess_ux, guess_uy; /**************************************************************************** - * Loop ... - ****************************************************************************/ + * Loop ... + ****************************************************************************/ do { guess_ux = dx / (1.0 + opticalDistCoeffs[0] * r2); guess_uy = dy / (1.0 + opticalDistCoeffs[0] * r2); @@ -251,38 +251,43 @@ void removeDistortion(double dx, double dy, double &ux, double &uy, /* Not converging so bomb */ numAttempts++; - if(numAttempts > 20) { + if (numAttempts > 20) { std::cout << "Didn't converge" << std::endl; return; } - } - while(!done); + } while (!done); /**************************************************************************** - * Sucess ... - ****************************************************************************/ + * Sucess ... + ****************************************************************************/ ux = guess_ux; uy = guess_uy; - } - break; + } break; // LROLROCNAC case LROLROCNAC: { - if (opticalDistCoeffs.size() != 1) { csm::Error::ErrorType errorType = csm::Error::INDEX_OUT_OF_RANGE; - std::string message = "Distortion coefficients for LRO LROC NAC must be of size 1, current size: " + std::to_string(opticalDistCoeffs.size()); + std::string message = + "Distortion coefficients for LRO LROC NAC must be of size 1, " + "current size: " + + std::to_string(opticalDistCoeffs.size()); std::string function = "removeDistortion"; throw csm::Error(errorType, message, function); } double dk1 = opticalDistCoeffs[0]; - double den = 1 + dk1 * dy * dy; // r = dy*dy = distance from the focal plane center + double den = + 1 + + dk1 * dy * dy; // r = dy*dy = distance from the focal plane center if (den == 0.0) { csm::Error::ErrorType errorType = csm::Error::ALGORITHM; - std::string message = "Unable to remove distortion for LRO LROC NAC. Focal plane position " + std::to_string(dy); + std::string message = + "Unable to remove distortion for LRO LROC NAC. Focal plane " + "position " + + std::to_string(dy); std::string function = "removeDistortion"; throw csm::Error(errorType, message, function); } @@ -291,12 +296,10 @@ void removeDistortion(double dx, double dy, double &ux, double &uy, uy = dy / den; return; - } - break; + } break; } } - void applyDistortion(double ux, double uy, double &dx, double &dy, const std::vector opticalDistCoeffs, DistortionType distortionType, @@ -305,7 +308,6 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, dy = uy; switch (distortionType) { - // Compute undistorted focal plane coordinate given a distorted // focal plane coordinate. This case works by iteratively adding distortion // until the new distorted point, r, undistorts to within a tolerance of the @@ -316,8 +318,9 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, if (rp2 > tolerance) { double rp = sqrt(rp2); // Compute first fractional distortion using rp - double drOverR = opticalDistCoeffs[0] - + (rp2 * (opticalDistCoeffs[1] + (rp2 * opticalDistCoeffs[2]))); + double drOverR = + opticalDistCoeffs[0] + + (rp2 * (opticalDistCoeffs[1] + (rp2 * opticalDistCoeffs[2]))); // Compute first distorted point estimate, r double r = rp + (drOverR * rp); @@ -339,37 +342,40 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, r2_prev = r * r; // Compute new fractional distortion: - drOverR = opticalDistCoeffs[0] - + (r2_prev * (opticalDistCoeffs[1] + (r2_prev * opticalDistCoeffs[2]))); + drOverR = opticalDistCoeffs[0] + + (r2_prev * + (opticalDistCoeffs[1] + (r2_prev * opticalDistCoeffs[2]))); // Compute new estimate of r r = rp + (drOverR * r_prev); iteration++; - } - while (fabs(r - r_prev) > desiredPrecision); + } while (fabs(r - r_prev) > desiredPrecision); dx = ux / (1.0 - drOverR); dy = uy / (1.0 - drOverR); } - } - break; + } break; case TRANSVERSE: { computeTransverseDistortion(ux, uy, dx, dy, opticalDistCoeffs); - } - break; + } break; case KAGUYALISM: { if (opticalDistCoeffs.size() != 10) { - csm::Error::ErrorType errorType = csm::Error::INDEX_OUT_OF_RANGE; - std::string message = "Distortion coefficients for Kaguya LISM must be of size 10, got: " + std::to_string(opticalDistCoeffs.size()); - std::string function = "applyDistortion"; - throw csm::Error(errorType, message, function); + csm::Error::ErrorType errorType = csm::Error::INDEX_OUT_OF_RANGE; + std::string message = + "Distortion coefficients for Kaguya LISM must be of size 10, " + "got: " + + std::to_string(opticalDistCoeffs.size()); + std::string function = "applyDistortion"; + throw csm::Error(errorType, message, function); } double boresightX = opticalDistCoeffs[0]; - std::vector odkx(opticalDistCoeffs.begin()+1, opticalDistCoeffs.begin()+5); + std::vector odkx(opticalDistCoeffs.begin() + 1, + opticalDistCoeffs.begin() + 5); double boresightY = opticalDistCoeffs[5]; - std::vector odky(opticalDistCoeffs.begin()+6, opticalDistCoeffs.begin()+10); + std::vector odky(opticalDistCoeffs.begin() + 6, + opticalDistCoeffs.begin() + 10); double xt = ux - boresightX; double yt = uy - boresightY; @@ -413,7 +419,8 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, ydistorted = yt; // check for convergence - if ((fabs(xt - xprevious) < tolerance) && (fabs(yt - yprevious) < tolerance)) { + if ((fabs(xt - xprevious) < tolerance) && + (fabs(yt - yprevious) < tolerance)) { bConverged = true; break; } @@ -426,8 +433,7 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, dx = xdistorted; dy = ydistorted; } - } - break; + } break; // The dawn distortion model is "reversed" from other distortion models so // the apply function computes distorted coordinates as a @@ -439,14 +445,12 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, dx = ux * (1.0 + opticalDistCoeffs[0] * r2); dy = uy * (1.0 + opticalDistCoeffs[0] * r2); - } - break; + } break; // The LRO LROC NAC distortion model uses an iterative approach to go from // undistorted x,y to distorted x,y // Algorithum adapted from ISIS3 LRONarrowAngleDistortionMap.cpp case LROLROCNAC: { - double yt = uy; double rr, dr; @@ -458,19 +462,24 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, if (opticalDistCoeffs.size() != 1) { csm::Error::ErrorType errorType = csm::Error::INDEX_OUT_OF_RANGE; - std::string message = "Distortion coefficients for LRO LROC NAC must be of size 1, current size: " + std::to_string(opticalDistCoeffs.size()); + std::string message = + "Distortion coefficients for LRO LROC NAC must be of size 1, " + "current size: " + + std::to_string(opticalDistCoeffs.size()); std::string function = "applyDistortion"; throw csm::Error(errorType, message, function); } double dk1 = opticalDistCoeffs[0]; - // Owing to the odd distotion model employed in this senser if |y| is > 116.881145553046 - // then there is no root to find. Further, the greatest y that any measure on the sensor - // will acutally distort to is less than 20. Thus, if any distorted measure is greater - // that that skip the iterations. The points isn't in the cube, and exactly how far outside - // the cube is irrelevant. Just let the camera model know its not in the cube.... - if (fabs(uy) > 40) { //if the point is way off the image..... + // Owing to the odd distotion model employed in this senser if |y| is > + // 116.881145553046 then there is no root to find. Further, the greatest + // y that any measure on the sensor will acutally distort to is less + // than 20. Thus, if any distorted measure is greater that that skip the + // iterations. The points isn't in the cube, and exactly how far outside + // the cube is irrelevant. Just let the camera model know its not in the + // cube.... + if (fabs(uy) > 40) { // if the point is way off the image..... dx = ux; dy = uy; return; @@ -479,7 +488,7 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, // iterating to introduce distortion (in sample only)... // we stop when the difference between distorted coordinate // in successive iterations is at or below the given tolerance - for(int i = 0; i < 50; i++) { + for (int i = 0; i < 50; i++) { rr = yt * yt; // dr is the radial distortion contribution @@ -491,11 +500,11 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, // distorted sample ydistorted = yt; - if (yt < -1e121) //debug - break; //debug + if (yt < -1e121) // debug + break; // debug // check for convergence - if(fabs(yt - yprevious) <= tolerance) { + if (fabs(yt - yprevious) <= tolerance) { bConverged = true; break; } @@ -503,13 +512,12 @@ void applyDistortion(double ux, double uy, double &dx, double &dy, yprevious = yt; } - if(bConverged) { + if (bConverged) { dx = ux; dy = ydistorted; } return; - } - break; + } break; } } diff --git a/src/UsgsAstroFrameSensorModel.cpp b/src/UsgsAstroFrameSensorModel.cpp index ca49a6b7d..822987d97 100644 --- a/src/UsgsAstroFrameSensorModel.cpp +++ b/src/UsgsAstroFrameSensorModel.cpp @@ -11,23 +11,26 @@ #include "ale/Util.h" -#define MESSAGE_LOG(...) if (m_logger) { m_logger->info(__VA_ARGS__); } +#define MESSAGE_LOG(...) \ + if (m_logger) { \ + m_logger->info(__VA_ARGS__); \ + } using json = nlohmann::json; using namespace std; // Declaration of static variables -const std::string UsgsAstroFrameSensorModel::_SENSOR_MODEL_NAME - = "USGS_ASTRO_FRAME_SENSOR_MODEL"; +const std::string UsgsAstroFrameSensorModel::_SENSOR_MODEL_NAME = + "USGS_ASTRO_FRAME_SENSOR_MODEL"; const int UsgsAstroFrameSensorModel::NUM_PARAMETERS = 7; const std::string UsgsAstroFrameSensorModel::m_parameterName[] = { - "X Sensor Position (m)", // 0 - "Y Sensor Position (m)", // 1 - "Z Sensor Position (m)", // 2 - "w", // 3 - "v1", // 4 - "v2", // 5 - "v3" // 6 + "X Sensor Position (m)", // 0 + "Y Sensor Position (m)", // 1 + "Z Sensor Position (m)", // 2 + "w", // 3 + "v1", // 4 + "v2", // 5 + "v3" // 6 }; UsgsAstroFrameSensorModel::UsgsAstroFrameSensorModel() { @@ -37,53 +40,55 @@ UsgsAstroFrameSensorModel::UsgsAstroFrameSensorModel() { void UsgsAstroFrameSensorModel::reset() { MESSAGE_LOG("Resetting UsgsAstroFrameSensorModel"); - m_modelName = _SENSOR_MODEL_NAME; - m_platformName = ""; - m_sensorName = ""; - m_imageIdentifier = ""; - m_collectionIdentifier = ""; - m_majorAxis = 0.0; - m_minorAxis = 0.0; - m_focalLength = 0.0; - m_startingDetectorSample = 0.0; - m_startingDetectorLine = 0.0; - m_detectorSampleSumming = 1.0; - m_detectorLineSumming = 1.0; - m_targetName = ""; - m_ifov = 0; - m_instrumentID = ""; - m_focalLengthEpsilon = 0.0; - m_originalHalfLines = 0.0; - m_spacecraftName = ""; - m_pixelPitch = 0.0; - m_ephemerisTime = 0.0; - m_originalHalfSamples = 0.0; - m_nLines = 0; - m_nSamples = 0; - - m_currentParameterValue = std::vector(NUM_PARAMETERS, 0.0); - m_currentParameterCovariance = std::vector(NUM_PARAMETERS*NUM_PARAMETERS,0.0); - for (int i = 0; i < NUM_PARAMETERS*NUM_PARAMETERS; i += NUM_PARAMETERS+1) { - m_currentParameterCovariance[i] = 1; - } - m_noAdjustments = std::vector(NUM_PARAMETERS,0.0); - m_ccdCenter = std::vector(2, 0.0); - m_spacecraftVelocity = std::vector(3, 0.0); - m_sunPosition = std::vector(3, 0.0); - m_distortionType = DistortionType::TRANSVERSE; - m_opticalDistCoeffs = std::vector(20, 0.0); - m_transX = std::vector(3, 0.0); - m_transY = std::vector(3, 0.0); - m_iTransS = std::vector(3, 0.0); - m_iTransL = std::vector(3, 0.0); - m_boresight = std::vector(3, 0.0); - m_parameterType = std::vector(NUM_PARAMETERS, csm::param::REAL); - m_referencePointXyz.x = 0; - m_referencePointXyz.y = 0; - m_referencePointXyz.z = 0; + m_modelName = _SENSOR_MODEL_NAME; + m_platformName = ""; + m_sensorName = ""; + m_imageIdentifier = ""; + m_collectionIdentifier = ""; + m_majorAxis = 0.0; + m_minorAxis = 0.0; + m_focalLength = 0.0; + m_startingDetectorSample = 0.0; + m_startingDetectorLine = 0.0; + m_detectorSampleSumming = 1.0; + m_detectorLineSumming = 1.0; + m_targetName = ""; + m_ifov = 0; + m_instrumentID = ""; + m_focalLengthEpsilon = 0.0; + m_originalHalfLines = 0.0; + m_spacecraftName = ""; + m_pixelPitch = 0.0; + m_ephemerisTime = 0.0; + m_originalHalfSamples = 0.0; + m_nLines = 0; + m_nSamples = 0; + + m_currentParameterValue = std::vector(NUM_PARAMETERS, 0.0); + m_currentParameterCovariance = + std::vector(NUM_PARAMETERS * NUM_PARAMETERS, 0.0); + for (int i = 0; i < NUM_PARAMETERS * NUM_PARAMETERS; + i += NUM_PARAMETERS + 1) { + m_currentParameterCovariance[i] = 1; + } + m_noAdjustments = std::vector(NUM_PARAMETERS, 0.0); + m_ccdCenter = std::vector(2, 0.0); + m_spacecraftVelocity = std::vector(3, 0.0); + m_sunPosition = std::vector(3, 0.0); + m_distortionType = DistortionType::TRANSVERSE; + m_opticalDistCoeffs = std::vector(20, 0.0); + m_transX = std::vector(3, 0.0); + m_transY = std::vector(3, 0.0); + m_iTransS = std::vector(3, 0.0); + m_iTransL = std::vector(3, 0.0); + m_boresight = std::vector(3, 0.0); + m_parameterType = + std::vector(NUM_PARAMETERS, csm::param::REAL); + m_referencePointXyz.x = 0; + m_referencePointXyz.y = 0; + m_referencePointXyz.z = 0; } - UsgsAstroFrameSensorModel::~UsgsAstroFrameSensorModel() {} /** @@ -92,20 +97,21 @@ UsgsAstroFrameSensorModel::~UsgsAstroFrameSensorModel() {} * @param desiredPrecision * @param achievedPrecision * @param warnings - * @return Returns coordinate in the image corresponding to the ground point - * without bundle adjustment correction. + * @return Returns coordinate in the image corresponding to the + * ground point without bundle adjustment correction. */ -csm::ImageCoord UsgsAstroFrameSensorModel::groundToImage(const csm::EcefCoord &groundPt, - double desiredPrecision, - double *achievedPrecision, - csm::WarningList *warnings) const { - MESSAGE_LOG("Computing groundToImage(No adjustments) for {}, {}, {}, with desired precision {}", - groundPt.x, groundPt.y, groundPt.z, desiredPrecision); +csm::ImageCoord UsgsAstroFrameSensorModel::groundToImage( + const csm::EcefCoord &groundPt, double desiredPrecision, + double *achievedPrecision, csm::WarningList *warnings) const { + MESSAGE_LOG( + "Computing groundToImage(No adjustments) for {}, {}, {}, with desired " + "precision {}", + groundPt.x, groundPt.y, groundPt.z, desiredPrecision); - return groundToImage(groundPt, m_noAdjustments, desiredPrecision, achievedPrecision, warnings); + return groundToImage(groundPt, m_noAdjustments, desiredPrecision, + achievedPrecision, warnings); } - /** * @brief UsgsAstroFrameSensorModel::groundToImage * @param groundPt @@ -113,56 +119,50 @@ csm::ImageCoord UsgsAstroFrameSensorModel::groundToImage(const csm::EcefCoord &g * @param desired_precision * @param achieved_precision * @param warnings - * @return Returns coordinate in the image corresponding to the ground point. - * This function applies bundle adjustments to the final value. + * @return Returns coordinate in the image corresponding to the + * ground point. This function applies bundle adjustments to the final value. */ csm::ImageCoord UsgsAstroFrameSensorModel::groundToImage( - const csm::EcefCoord& groundPt, - const std::vector& adjustments, - double desired_precision, - double* achieved_precision, - csm::WarningList* warnings ) const { - - MESSAGE_LOG("Computing groundToImage for {}, {}, {}, with desired precision {}", - groundPt.x, groundPt.y, groundPt.z, desired_precision); + const csm::EcefCoord &groundPt, const std::vector &adjustments, + double desired_precision, double *achieved_precision, + csm::WarningList *warnings) const { + MESSAGE_LOG( + "Computing groundToImage for {}, {}, {}, with desired precision {}", + groundPt.x, groundPt.y, groundPt.z, desired_precision); double x = groundPt.x; double y = groundPt.y; double z = groundPt.z; - double xo = x - getValue(0,adjustments); - double yo = y - getValue(1,adjustments); - double zo = z - getValue(2,adjustments); + double xo = x - getValue(0, adjustments); + double yo = y - getValue(1, adjustments); + double zo = z - getValue(2, adjustments); double f; f = m_focalLength; // Camera rotation matrix double m[3][3]; - calcRotationMatrix(m,adjustments); + calcRotationMatrix(m, adjustments); // Sensor position double undistortedx, undistortedy, denom; denom = m[0][2] * xo + m[1][2] * yo + m[2][2] * zo; - undistortedx = (f * (m[0][0] * xo + m[1][0] * yo + m[2][0] * zo)/denom); - undistortedy = (f * (m[0][1] * xo + m[1][1] * yo + m[2][1] * zo)/denom); + undistortedx = (f * (m[0][0] * xo + m[1][0] * yo + m[2][0] * zo) / denom); + undistortedy = (f * (m[0][1] * xo + m[1][1] * yo + m[2][1] * zo) / denom); - // Apply the distortion to the line/sample location and then convert back to line/sample + // Apply the distortion to the line/sample location and then convert back to + // line/sample double distortedX, distortedY; applyDistortion(undistortedx, undistortedy, distortedX, distortedY, m_opticalDistCoeffs, m_distortionType); - // Convert distorted mm into line/sample double sample, line; - computePixel( - distortedX, distortedY, - m_ccdCenter[1], m_ccdCenter[0], - m_detectorSampleSumming, m_detectorLineSumming, - m_startingDetectorSample, m_startingDetectorLine, - &m_iTransS[0], &m_iTransL[0], - line, sample); - + computePixel(distortedX, distortedY, m_ccdCenter[1], m_ccdCenter[0], + m_detectorSampleSumming, m_detectorLineSumming, + m_startingDetectorSample, m_startingDetectorLine, &m_iTransS[0], + &m_iTransL[0], line, sample); MESSAGE_LOG("Computed groundToImage for {}, {}, {} as line, sample: {}, {}", groundPt.x, groundPt.y, groundPt.z, line, sample); @@ -170,38 +170,36 @@ csm::ImageCoord UsgsAstroFrameSensorModel::groundToImage( return csm::ImageCoord(line, sample); } - -csm::ImageCoordCovar UsgsAstroFrameSensorModel::groundToImage(const csm::EcefCoordCovar &groundPt, - double desiredPrecision, - double *achievedPrecision, - csm::WarningList *warnings) const { - MESSAGE_LOG("Computing groundToImage(Covar) for {}, {}, {}, with desired precision {}", - groundPt.x, groundPt.y, groundPt.z, desiredPrecision); - - csm::EcefCoord gp; - gp.x = groundPt.x; - gp.y = groundPt.y; - gp.z = groundPt.z; - - csm::ImageCoord ip = groundToImage( - gp, desiredPrecision, achievedPrecision, warnings); - csm::ImageCoordCovar result(ip.line, ip.samp); - // This is a partial, incorrect implementation to test if SocetGXP needs - // this method implemented in order to load the sensor. - MESSAGE_LOG("Computed groundToImage(Covar) for {}, {}, {}, as line, sample: {}, {}", - groundPt.x, groundPt.y, groundPt.z, ip.line, ip.samp); - return result; -} - - -csm::EcefCoord UsgsAstroFrameSensorModel::imageToGround(const csm::ImageCoord &imagePt, - double height, - double desiredPrecision, - double *achievedPrecision, - csm::WarningList *warnings) const { - - MESSAGE_LOG("Computing imageToGround for {}, {}, {}, with desired precision {}", - imagePt.line, imagePt.samp, height, desiredPrecision); +csm::ImageCoordCovar UsgsAstroFrameSensorModel::groundToImage( + const csm::EcefCoordCovar &groundPt, double desiredPrecision, + double *achievedPrecision, csm::WarningList *warnings) const { + MESSAGE_LOG( + "Computing groundToImage(Covar) for {}, {}, {}, with desired precision " + "{}", + groundPt.x, groundPt.y, groundPt.z, desiredPrecision); + + csm::EcefCoord gp; + gp.x = groundPt.x; + gp.y = groundPt.y; + gp.z = groundPt.z; + + csm::ImageCoord ip = + groundToImage(gp, desiredPrecision, achievedPrecision, warnings); + csm::ImageCoordCovar result(ip.line, ip.samp); + // This is a partial, incorrect implementation to test if SocetGXP needs + // this method implemented in order to load the sensor. + MESSAGE_LOG( + "Computed groundToImage(Covar) for {}, {}, {}, as line, sample: {}, {}", + groundPt.x, groundPt.y, groundPt.z, ip.line, ip.samp); + return result; +} + +csm::EcefCoord UsgsAstroFrameSensorModel::imageToGround( + const csm::ImageCoord &imagePt, double height, double desiredPrecision, + double *achievedPrecision, csm::WarningList *warnings) const { + MESSAGE_LOG( + "Computing imageToGround for {}, {}, {}, with desired precision {}", + imagePt.line, imagePt.samp, height, desiredPrecision); double sample = imagePt.samp; double line = imagePt.line; @@ -209,8 +207,10 @@ csm::EcefCoord UsgsAstroFrameSensorModel::imageToGround(const csm::ImageCoord &i // Here is where we should be able to apply an adjustment to opk double m[3][3]; calcRotationMatrix(m); - MESSAGE_LOG("Calculated rotation matrix [{}, {}, {}], [{}, {}, {}], [{}, {}, {}]", - 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]); + MESSAGE_LOG( + "Calculated rotation matrix [{}, {}, {}], [{}, {}, {}], [{}, {}, {}]", + 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; @@ -218,33 +218,32 @@ csm::EcefCoord UsgsAstroFrameSensorModel::imageToGround(const csm::ImageCoord &i // Convert from the pixel space into the metric space double x_camera, y_camera; computeDistortedFocalPlaneCoordinates( - line, sample, - m_ccdCenter[1], m_ccdCenter[0], - m_detectorSampleSumming, m_detectorLineSumming, - m_startingDetectorSample, m_startingDetectorLine, - &m_iTransS[0], &m_iTransL[0], - x_camera, y_camera); + line, sample, m_ccdCenter[1], m_ccdCenter[0], m_detectorSampleSumming, + m_detectorLineSumming, m_startingDetectorSample, m_startingDetectorLine, + &m_iTransS[0], &m_iTransL[0], x_camera, y_camera); // Apply the distortion model (remove distortion) double undistortedX, undistortedY; removeDistortion(x_camera, y_camera, undistortedX, undistortedY, - m_opticalDistCoeffs, - m_distortionType); - MESSAGE_LOG("Found undistortedX: {}, and undistortedY: {}", - undistortedX, undistortedY); + m_opticalDistCoeffs, m_distortionType); + MESSAGE_LOG("Found undistortedX: {}, and undistortedY: {}", undistortedX, + undistortedY); // Now back from distorted mm to pixels - xl = m[0][0] * undistortedX + m[0][1] * undistortedY - m[0][2] * - m_focalLength; - yl = m[1][0] * undistortedX + m[1][1] * undistortedY - m[1][2] * - m_focalLength; - zl = m[2][0] * undistortedX + m[2][1] * undistortedY - m[2][2] * - m_focalLength; + xl = m[0][0] * undistortedX + m[0][1] * undistortedY - + m[0][2] * -m_focalLength; + yl = m[1][0] * undistortedX + m[1][1] * undistortedY - + m[1][2] * -m_focalLength; + zl = m[2][0] * undistortedX + m[2][1] * undistortedY - + m[2][2] * -m_focalLength; MESSAGE_LOG("Compute xl, yl, zl as {}, {}, {}", xl, yl, zl); double xc, yc, zc; xc = m_currentParameterValue[0]; yc = m_currentParameterValue[1]; zc = m_currentParameterValue[2]; - MESSAGE_LOG("Set xc, yc, zc to {}, {}, {}", - m_currentParameterValue[0], m_currentParameterValue[1], m_currentParameterValue[2]); + MESSAGE_LOG("Set xc, yc, zc to {}, {}, {}", m_currentParameterValue[0], + m_currentParameterValue[1], m_currentParameterValue[2]); // Intersect with some height about the ellipsoid. double x, y, z; @@ -255,144 +254,136 @@ csm::EcefCoord UsgsAstroFrameSensorModel::imageToGround(const csm::ImageCoord &i return csm::EcefCoord(x, y, z); } +csm::EcefCoordCovar UsgsAstroFrameSensorModel::imageToGround( + const csm::ImageCoordCovar &imagePt, double height, double heightVariance, + double desiredPrecision, double *achievedPrecision, + csm::WarningList *warnings) const { + MESSAGE_LOG( + "Computing imageToGround(Covar) for {}, {}, {}, with desired precision " + "{}", + imagePt.line, imagePt.samp, height, desiredPrecision); + // This is an incomplete implementation to see if SocetGXP needs this method + // implemented. -csm::EcefCoordCovar UsgsAstroFrameSensorModel::imageToGround(const csm::ImageCoordCovar &imagePt, double height, - double heightVariance, double desiredPrecision, - double *achievedPrecision, - csm::WarningList *warnings) const { - - MESSAGE_LOG("Computing imageToGround(Covar) for {}, {}, {}, with desired precision {}", - imagePt.line, imagePt.samp, height, desiredPrecision); - // This is an incomplete implementation to see if SocetGXP needs this method implemented. - - MESSAGE_LOG("This is an incomplete implementation to see if SocetGXP needs this method implemented"); + MESSAGE_LOG( + "This is an incomplete implementation to see if SocetGXP needs this " + "method implemented"); - csm::EcefCoordCovar result; - return result; + csm::EcefCoordCovar result; + return result; } - -csm::EcefLocus UsgsAstroFrameSensorModel::imageToProximateImagingLocus(const csm::ImageCoord &imagePt, - const csm::EcefCoord &groundPt, - double desiredPrecision, - double *achievedPrecision, - csm::WarningList *warnings) const { +csm::EcefLocus UsgsAstroFrameSensorModel::imageToProximateImagingLocus( + const csm::ImageCoord &imagePt, const csm::EcefCoord &groundPt, + double desiredPrecision, double *achievedPrecision, + csm::WarningList *warnings) const { // Ignore the ground point? - MESSAGE_LOG("Computing imageToProximateImagingLocus(No ground) for point {}, {}, {}, with desired precision {}", - imagePt.line, imagePt.samp, desiredPrecision); + MESSAGE_LOG( + "Computing imageToProximateImagingLocus(No ground) for point {}, {}, {}, " + "with desired precision {}", + imagePt.line, imagePt.samp, desiredPrecision); return imageToRemoteImagingLocus(imagePt); } - -csm::EcefLocus UsgsAstroFrameSensorModel::imageToRemoteImagingLocus(const csm::ImageCoord &imagePt, - double desiredPrecision, - double *achievedPrecision, - csm::WarningList *warnings) const { - MESSAGE_LOG("Computing imageToProximateImagingLocus for {}, {}, {}, with desired precision {}", - imagePt.line, imagePt.samp, desiredPrecision); +csm::EcefLocus UsgsAstroFrameSensorModel::imageToRemoteImagingLocus( + const csm::ImageCoord &imagePt, double desiredPrecision, + double *achievedPrecision, csm::WarningList *warnings) const { + MESSAGE_LOG( + "Computing imageToProximateImagingLocus for {}, {}, {}, with desired " + "precision {}", + imagePt.line, imagePt.samp, desiredPrecision); // Find the line,sample on the focal plane (mm) double focalPlaneX, focalPlaneY; computeDistortedFocalPlaneCoordinates( - imagePt.line, imagePt.samp, - m_ccdCenter[1], m_ccdCenter[0], - m_detectorSampleSumming, m_detectorLineSumming, - m_startingDetectorSample, m_startingDetectorLine, - &m_iTransS[0], &m_iTransL[0], - focalPlaneY, focalPlaneY); + imagePt.line, imagePt.samp, m_ccdCenter[1], m_ccdCenter[0], + m_detectorSampleSumming, m_detectorLineSumming, m_startingDetectorSample, + m_startingDetectorLine, &m_iTransS[0], &m_iTransL[0], focalPlaneY, + focalPlaneY); // Distort double undistortedFocalPlaneX, undistortedFocalPlaneY; - removeDistortion(focalPlaneX, focalPlaneY, - undistortedFocalPlaneX, undistortedFocalPlaneY, - m_opticalDistCoeffs, + removeDistortion(focalPlaneX, focalPlaneY, undistortedFocalPlaneX, + undistortedFocalPlaneY, m_opticalDistCoeffs, m_distortionType); // Get rotation matrix and transform to a body-fixed frame double m[3][3]; calcRotationMatrix(m); - std::vector lookC { undistortedFocalPlaneX, undistortedFocalPlaneY, m_focalLength }; - std::vector lookB { - m[0][0] * lookC[0] + m[0][1] * lookC[1] + m[0][2] * lookC[2], - m[1][0] * lookC[0] + m[1][1] * lookC[1] + m[1][2] * lookC[2], - m[2][0] * lookC[0] + m[2][1] * lookC[1] + m[2][2] * lookC[2] - }; + std::vector lookC{undistortedFocalPlaneX, undistortedFocalPlaneY, + m_focalLength}; + std::vector lookB{ + m[0][0] * lookC[0] + m[0][1] * lookC[1] + m[0][2] * lookC[2], + m[1][0] * lookC[0] + m[1][1] * lookC[1] + m[1][2] * lookC[2], + m[2][0] * lookC[0] + m[2][1] * lookC[1] + m[2][2] * lookC[2]}; // Get unit vector - double mag = sqrt(lookB[0] * lookB[0] + lookB[1] * lookB[1] + lookB[2] * lookB[2]); - std::vector lookBUnit { - lookB[0] / mag, - lookB[1] / mag, - lookB[2] / mag - }; + double mag = + sqrt(lookB[0] * lookB[0] + lookB[1] * lookB[1] + lookB[2] * lookB[2]); + std::vector lookBUnit{lookB[0] / mag, lookB[1] / mag, lookB[2] / mag}; - return csm::EcefLocus(m_currentParameterValue[0], m_currentParameterValue[1], m_currentParameterValue[2], - lookBUnit[0], lookBUnit[1], lookBUnit[2]); + return csm::EcefLocus(m_currentParameterValue[0], m_currentParameterValue[1], + m_currentParameterValue[2], lookBUnit[0], lookBUnit[1], + lookBUnit[2]); } - csm::ImageCoord UsgsAstroFrameSensorModel::getImageStart() const { - MESSAGE_LOG("Accessing Image Start line: {}, sample: {}", - m_startingDetectorLine, m_startingDetectorSample); + m_startingDetectorLine, m_startingDetectorSample); csm::ImageCoord start; start.samp = m_startingDetectorSample; start.line = m_startingDetectorLine; return start; } - csm::ImageVector UsgsAstroFrameSensorModel::getImageSize() const { - - MESSAGE_LOG("Accessing Image Size line: {}, sample: {}", - m_nLines, m_nSamples); + MESSAGE_LOG("Accessing Image Size line: {}, sample: {}", m_nLines, + m_nSamples); csm::ImageVector size; size.line = m_nLines; size.samp = m_nSamples; return size; } - -std::pair UsgsAstroFrameSensorModel::getValidImageRange() const { - MESSAGE_LOG("Accessing Image Range"); - csm::ImageCoord min_pt(m_startingDetectorLine, m_startingDetectorSample); - csm::ImageCoord max_pt(m_nLines, m_nSamples); - MESSAGE_LOG("Valid image range: min {}, {} max: {}, {}", min_pt.samp, min_pt.line, - max_pt.samp, max_pt.line) - return std::pair(min_pt, max_pt); +std::pair +UsgsAstroFrameSensorModel::getValidImageRange() const { + MESSAGE_LOG("Accessing Image Range"); + csm::ImageCoord min_pt(m_startingDetectorLine, m_startingDetectorSample); + csm::ImageCoord max_pt(m_nLines, m_nSamples); + MESSAGE_LOG("Valid image range: min {}, {} max: {}, {}", min_pt.samp, + min_pt.line, max_pt.samp, max_pt.line) + return std::pair(min_pt, max_pt); } - -std::pair UsgsAstroFrameSensorModel::getValidHeightRange() const { - MESSAGE_LOG("Accessing Image Height min: {}, max: {}", - m_minElevation, m_maxElevation); - return std::pair(m_minElevation, m_maxElevation); +std::pair UsgsAstroFrameSensorModel::getValidHeightRange() + const { + MESSAGE_LOG("Accessing Image Height min: {}, max: {}", m_minElevation, + m_maxElevation); + return std::pair(m_minElevation, m_maxElevation); } - -csm::EcefVector UsgsAstroFrameSensorModel::getIlluminationDirection(const csm::EcefCoord &groundPt) const { +csm::EcefVector UsgsAstroFrameSensorModel::getIlluminationDirection( + const csm::EcefCoord &groundPt) const { // ground (body-fixed) - sun (body-fixed) gives us the illumination direction. MESSAGE_LOG("Accessing illumination direction for ground point {}, {}, {}", groundPt.x, groundPt.y, groundPt.z); - return csm::EcefVector { - groundPt.x - m_sunPosition[0], - groundPt.y - m_sunPosition[1], - groundPt.z - m_sunPosition[2] - }; + return csm::EcefVector{groundPt.x - m_sunPosition[0], + groundPt.y - m_sunPosition[1], + groundPt.z - m_sunPosition[2]}; } - -double UsgsAstroFrameSensorModel::getImageTime(const csm::ImageCoord &imagePt) const { - MESSAGE_LOG("Accessing image time for image point {}, {}", - imagePt.line, imagePt.samp); - // The entire image is aquired at once so image time for all pixels is the - // reference time - return 0.0; +double UsgsAstroFrameSensorModel::getImageTime( + const csm::ImageCoord &imagePt) const { + MESSAGE_LOG("Accessing image time for image point {}, {}", imagePt.line, + imagePt.samp); + // The entire image is aquired at once so image time for all pixels is the + // reference time + return 0.0; } - -csm::EcefCoord UsgsAstroFrameSensorModel::getSensorPosition(const csm::ImageCoord &imagePt) const { - MESSAGE_LOG("Accessing sensor position for image point {}, {}", - imagePt.line, imagePt.samp); +csm::EcefCoord UsgsAstroFrameSensorModel::getSensorPosition( + const csm::ImageCoord &imagePt) const { + MESSAGE_LOG("Accessing sensor position for image point {}, {}", imagePt.line, + imagePt.samp); // check if the image point is in range if (imagePt.samp >= m_startingDetectorSample && imagePt.samp <= (m_startingDetectorSample + m_nSamples) && @@ -404,88 +395,80 @@ csm::EcefCoord UsgsAstroFrameSensorModel::getSensorPosition(const csm::ImageCoor sensorPosition.z = m_currentParameterValue[2]; return sensorPosition; - } - else { - MESSAGE_LOG("ERROR: UsgsAstroFrameSensorModel::getSensorPosition: " - "Image Coordinate {},{} out of Bounds", - imagePt.line, imagePt.samp); - throw csm::Error(csm::Error::BOUNDS, - "Image Coordinate out of Bounds", + } else { + MESSAGE_LOG( + "ERROR: UsgsAstroFrameSensorModel::getSensorPosition: " + "Image Coordinate {},{} out of Bounds", + imagePt.line, imagePt.samp); + throw csm::Error(csm::Error::BOUNDS, "Image Coordinate out of Bounds", "UsgsAstroFrameSensorModel::getSensorPosition"); } } - csm::EcefCoord UsgsAstroFrameSensorModel::getSensorPosition(double time) const { - MESSAGE_LOG("Accessing sensor position for time {}", time); - if (time == 0.0){ - csm::EcefCoord sensorPosition; - sensorPosition.x = m_currentParameterValue[0]; - sensorPosition.y = m_currentParameterValue[1]; - sensorPosition.z = m_currentParameterValue[2]; - - return sensorPosition; - } else { - MESSAGE_LOG("ERROR: UsgsAstroFrameSensorModel::getSensorPosition: Valid image time is 0.0"); - std::string aMessage = "Valid image time is 0.0"; - throw csm::Error(csm::Error::BOUNDS, - aMessage, - "UsgsAstroFrameSensorModel::getSensorPosition"); - } -} + MESSAGE_LOG("Accessing sensor position for time {}", time); + if (time == 0.0) { + csm::EcefCoord sensorPosition; + sensorPosition.x = m_currentParameterValue[0]; + sensorPosition.y = m_currentParameterValue[1]; + sensorPosition.z = m_currentParameterValue[2]; + return sensorPosition; + } else { + MESSAGE_LOG( + "ERROR: UsgsAstroFrameSensorModel::getSensorPosition: Valid image time " + "is 0.0"); + std::string aMessage = "Valid image time is 0.0"; + throw csm::Error(csm::Error::BOUNDS, aMessage, + "UsgsAstroFrameSensorModel::getSensorPosition"); + } +} -csm::EcefVector UsgsAstroFrameSensorModel::getSensorVelocity(const csm::ImageCoord &imagePt) const { - MESSAGE_LOG("Accessing sensor velocity for image point {}, {}", - imagePt.line, imagePt.samp); +csm::EcefVector UsgsAstroFrameSensorModel::getSensorVelocity( + const csm::ImageCoord &imagePt) const { + MESSAGE_LOG("Accessing sensor velocity for image point {}, {}", imagePt.line, + imagePt.samp); // Make sure the passed coordinate is with the image dimensions. - if (imagePt.samp < 0.0 || imagePt.samp > m_nSamples || - imagePt.line < 0.0 || imagePt.line > m_nLines) { + if (imagePt.samp < 0.0 || imagePt.samp > m_nSamples || imagePt.line < 0.0 || + imagePt.line > m_nLines) { MESSAGE_LOG("ERROR: Image coordinate out of bounds.") throw csm::Error(csm::Error::BOUNDS, "Image coordinate out of bounds.", "UsgsAstroFrameSensorModel::getSensorVelocity"); } // Since this is a frame, just return the sensor velocity the ISD gave us. - return csm::EcefVector { - m_spacecraftVelocity[0], - m_spacecraftVelocity[1], - m_spacecraftVelocity[2] - }; -} - - -csm::EcefVector UsgsAstroFrameSensorModel::getSensorVelocity(double time) const { - MESSAGE_LOG("Accessing sensor position for time {}", time); - if (time == 0.0){ - return csm::EcefVector { - m_spacecraftVelocity[0], - m_spacecraftVelocity[1], - m_spacecraftVelocity[2] - }; - } else { - std::string aMessage = "Valid image time is 0.0"; - throw csm::Error(csm::Error::BOUNDS, - aMessage, - "UsgsAstroFrameSensorModel::getSensorVelocity"); - } + return csm::EcefVector{m_spacecraftVelocity[0], m_spacecraftVelocity[1], + m_spacecraftVelocity[2]}; +} + +csm::EcefVector UsgsAstroFrameSensorModel::getSensorVelocity( + double time) const { + MESSAGE_LOG("Accessing sensor position for time {}", time); + if (time == 0.0) { + return csm::EcefVector{m_spacecraftVelocity[0], m_spacecraftVelocity[1], + m_spacecraftVelocity[2]}; + } else { + std::string aMessage = "Valid image time is 0.0"; + throw csm::Error(csm::Error::BOUNDS, aMessage, + "UsgsAstroFrameSensorModel::getSensorVelocity"); + } } +csm::RasterGM::SensorPartials UsgsAstroFrameSensorModel::computeSensorPartials( + int index, const csm::EcefCoord &groundPt, double desiredPrecision, + double *achievedPrecision, csm::WarningList *warnings) const { + MESSAGE_LOG( + "Computing sensor partials image point from ground point {}, {}, {} \ + and desiredPrecision: {}", + groundPt.x, groundPt.y, groundPt.z, desiredPrecision); -csm::RasterGM::SensorPartials UsgsAstroFrameSensorModel::computeSensorPartials(int index, - const csm::EcefCoord &groundPt, - double desiredPrecision, - double *achievedPrecision, - csm::WarningList *warnings) const { - MESSAGE_LOG("Computing sensor partials image point from ground point {}, {}, {} \ - and desiredPrecision: {}", groundPt.x, groundPt.y, groundPt.z, desiredPrecision); + csm::ImageCoord img_pt = + groundToImage(groundPt, desiredPrecision, achievedPrecision); - csm::ImageCoord img_pt = groundToImage(groundPt, desiredPrecision, achievedPrecision); - - return computeSensorPartials(index, img_pt, groundPt, desiredPrecision, achievedPrecision); + return computeSensorPartials(index, img_pt, groundPt, desiredPrecision, + achievedPrecision); } - /** * @brief UsgsAstroFrameSensorModel::computeSensorPartials * @param index @@ -496,21 +479,20 @@ csm::RasterGM::SensorPartials UsgsAstroFrameSensorModel::computeSensorPartials(i * @param warnings * @return The partial derivatives in the line,sample directions. * - * Research: We should investigate using a central difference scheme to approximate - * the partials. It is more accurate, but it might be costlier calculation-wise. + * Research: We should investigate using a central difference scheme to + * approximate the partials. It is more accurate, but it might be costlier + * calculation-wise. * */ -csm::RasterGM::SensorPartials UsgsAstroFrameSensorModel::computeSensorPartials(int index, - const csm::ImageCoord &imagePt, - const csm::EcefCoord &groundPt, - double desiredPrecision, - double *achievedPrecision, - csm::WarningList *warnings) const { - - MESSAGE_LOG("Computing sensor partials for ground point {}, {}, {}\ +csm::RasterGM::SensorPartials UsgsAstroFrameSensorModel::computeSensorPartials( + int index, const csm::ImageCoord &imagePt, const csm::EcefCoord &groundPt, + double desiredPrecision, double *achievedPrecision, + csm::WarningList *warnings) const { + MESSAGE_LOG( + "Computing sensor partials for ground point {}, {}, {}\ with point: {}, {}, index: {}, and desiredPrecision: {}", - groundPt.x, groundPt.y, groundPt.z, imagePt.line, imagePt.samp, - index, desiredPrecision); + groundPt.x, groundPt.y, groundPt.z, imagePt.line, imagePt.samp, index, + desiredPrecision); double delta = 1.0; // The rotation parameters will usually be small (<1), // so a delta of 1.0 is too small. @@ -519,72 +501,72 @@ csm::RasterGM::SensorPartials UsgsAstroFrameSensorModel::computeSensorPartials(i delta = 0.01; } // Update the parameter - std::vectoradj(NUM_PARAMETERS, 0.0); + std::vector adj(NUM_PARAMETERS, 0.0); adj[index] = delta; - csm::ImageCoord imagePt1 = groundToImage(groundPt,adj,desiredPrecision,achievedPrecision); + csm::ImageCoord imagePt1 = + groundToImage(groundPt, adj, desiredPrecision, achievedPrecision); csm::RasterGM::SensorPartials partials; - partials.first = (imagePt1.line - imagePt.line)/delta; - partials.second = (imagePt1.samp - imagePt.samp)/delta; + partials.first = (imagePt1.line - imagePt.line) / delta; + partials.second = (imagePt1.samp - imagePt.samp) / delta; return partials; - } -std::vector UsgsAstroFrameSensorModel::computeAllSensorPartials( - const csm::ImageCoord& imagePt, - const csm::EcefCoord& groundPt, - csm::param::Set pset, - double desiredPrecision, - double *achievedPrecision, +std::vector +UsgsAstroFrameSensorModel::computeAllSensorPartials( + const csm::ImageCoord &imagePt, const csm::EcefCoord &groundPt, + csm::param::Set pset, double desiredPrecision, double *achievedPrecision, csm::WarningList *warnings) const { - MESSAGE_LOG("Computing all sensor partials for ground point {}, {}, {}\ + MESSAGE_LOG( + "Computing all sensor partials for ground point {}, {}, {}\ with point: {}, {}, pset: {}, and desiredPrecision: {}", - groundPt.x, groundPt.y, groundPt.z, imagePt.line, imagePt.samp, - pset, desiredPrecision); + groundPt.x, groundPt.y, groundPt.z, imagePt.line, imagePt.samp, pset, + desiredPrecision); std::vector indices = getParameterSetIndices(pset); size_t num = indices.size(); std::vector partials; - for (int index = 0;index < num;index++){ - partials.push_back(computeSensorPartials( - indices[index], - imagePt, groundPt, - desiredPrecision, achievedPrecision, warnings)); + for (int index = 0; index < num; index++) { + partials.push_back(computeSensorPartials(indices[index], imagePt, groundPt, + desiredPrecision, + achievedPrecision, warnings)); } return partials; } -std::vector UsgsAstroFrameSensorModel::computeAllSensorPartials( - const csm::EcefCoord& groundPt, - csm::param::Set pset, - double desiredPrecision, - double *achievedPrecision, +std::vector +UsgsAstroFrameSensorModel::computeAllSensorPartials( + const csm::EcefCoord &groundPt, csm::param::Set pset, + double desiredPrecision, double *achievedPrecision, csm::WarningList *warnings) const { - MESSAGE_LOG("Computing all sensor partials image point from ground point {}, {}, {} \ - and desiredPrecision: {}", groundPt.x, groundPt.y, groundPt.z, desiredPrecision); - csm::ImageCoord imagePt = groundToImage(groundPt, - desiredPrecision, achievedPrecision, warnings); - return computeAllSensorPartials(imagePt, groundPt, - pset, desiredPrecision, achievedPrecision, warnings); - } - -std::vector UsgsAstroFrameSensorModel::computeGroundPartials(const csm::EcefCoord - &groundPt) const { + MESSAGE_LOG( + "Computing all sensor partials image point from ground point {}, {}, {} \ + and desiredPrecision: {}", + groundPt.x, groundPt.y, groundPt.z, desiredPrecision); + csm::ImageCoord imagePt = + groundToImage(groundPt, desiredPrecision, achievedPrecision, warnings); + return computeAllSensorPartials(imagePt, groundPt, pset, desiredPrecision, + achievedPrecision, warnings); +} + +std::vector UsgsAstroFrameSensorModel::computeGroundPartials( + const csm::EcefCoord &groundPt) const { MESSAGE_LOG("Computing ground partials for ground point {}, {}, {}", - groundPt.x, groundPt.y, groundPt.z); + groundPt.x, groundPt.y, groundPt.z); // Partial of line, sample wrt X, Y, Z double x = groundPt.x; double y = groundPt.y; double z = groundPt.z; csm::ImageCoord ipB = groundToImage(groundPt); - csm::EcefCoord nextPoint = imageToGround(csm::ImageCoord(ipB.line + 1, ipB.samp + 1)); + csm::EcefCoord nextPoint = + imageToGround(csm::ImageCoord(ipB.line + 1, ipB.samp + 1)); double dx = nextPoint.x - x; double dy = nextPoint.y - y; double dz = nextPoint.z - z; - double pixelGroundSize = sqrt((dx*dx + dy*dy + dz*dz) / 2.0); + double pixelGroundSize = sqrt((dx * dx + dy * dy + dz * dz) / 2.0); // If the ground size is too small, try the opposite direction if (pixelGroundSize < 1e-10) { @@ -592,12 +574,15 @@ std::vector UsgsAstroFrameSensorModel::computeGroundPartials(const csm:: dx = nextPoint.x - x; dy = nextPoint.y - y; dz = nextPoint.z - z; - pixelGroundSize = sqrt((dx*dx + dy*dy + dz*dz) / 2.0); + pixelGroundSize = sqrt((dx * dx + dy * dy + dz * dz) / 2.0); } - csm::ImageCoord ipX = groundToImage(csm::EcefCoord(x + pixelGroundSize, y, z)); - csm::ImageCoord ipY = groundToImage(csm::EcefCoord(x, y + pixelGroundSize, z)); - csm::ImageCoord ipZ = groundToImage(csm::EcefCoord(x, y, z + pixelGroundSize)); + csm::ImageCoord ipX = + groundToImage(csm::EcefCoord(x + pixelGroundSize, y, z)); + csm::ImageCoord ipY = + groundToImage(csm::EcefCoord(x, y + pixelGroundSize, z)); + csm::ImageCoord ipZ = + groundToImage(csm::EcefCoord(x, y, z + pixelGroundSize)); std::vector partials(6, 0.0); partials[0] = (ipX.line - ipB.line) / pixelGroundSize; @@ -607,120 +592,109 @@ std::vector UsgsAstroFrameSensorModel::computeGroundPartials(const csm:: partials[2] = (ipZ.line - ipB.line) / pixelGroundSize; partials[5] = (ipZ.samp - ipB.samp) / pixelGroundSize; - MESSAGE_LOG("Computing ground partials results:\nLine: {}, {}, {}\nSample: {}, {}, {}", - partials[0], partials[1], partials[2], partials[3], partials[4], partials[5]); + MESSAGE_LOG( + "Computing ground partials results:\nLine: {}, {}, {}\nSample: {}, {}, " + "{}", + partials[0], partials[1], partials[2], partials[3], partials[4], + partials[5]); return partials; } - -const csm::CorrelationModel& UsgsAstroFrameSensorModel::getCorrelationModel() const { +const csm::CorrelationModel &UsgsAstroFrameSensorModel::getCorrelationModel() + const { MESSAGE_LOG("Accessing correlation model"); return _no_corr_model; } - -std::vector UsgsAstroFrameSensorModel::getUnmodeledCrossCovariance(const csm::ImageCoord &pt1, - const csm::ImageCoord &pt2) const { - - // No unmodeled error - MESSAGE_LOG("Accessing unmodeled cross covar with \ +std::vector UsgsAstroFrameSensorModel::getUnmodeledCrossCovariance( + const csm::ImageCoord &pt1, const csm::ImageCoord &pt2) const { + // No unmodeled error + MESSAGE_LOG( + "Accessing unmodeled cross covar with \ point1: {}, {} and point2: {}, {}", - pt1.line, pt1.samp, pt2.line, pt2.samp); - return std::vector(4, 0.0); + pt1.line, pt1.samp, pt2.line, pt2.samp); + return std::vector(4, 0.0); } - csm::Version UsgsAstroFrameSensorModel::getVersion() const { MESSAGE_LOG("Accessing CSM version"); - return csm::Version(0,1,0); + return csm::Version(0, 1, 0); } - std::string UsgsAstroFrameSensorModel::getModelName() const { MESSAGE_LOG("Accessing CSM name {}", _SENSOR_MODEL_NAME); return _SENSOR_MODEL_NAME; } - std::string UsgsAstroFrameSensorModel::getPedigree() const { MESSAGE_LOG("Accessing CSM pedigree"); return "USGS_FRAMER"; } - std::string UsgsAstroFrameSensorModel::getImageIdentifier() const { MESSAGE_LOG("Accessing image ID {}", m_imageIdentifier); return m_imageIdentifier; } - -void UsgsAstroFrameSensorModel::setImageIdentifier(const std::string& imageId, - csm::WarningList* warnings) { +void UsgsAstroFrameSensorModel::setImageIdentifier(const std::string &imageId, + csm::WarningList *warnings) { MESSAGE_LOG("Setting image ID to {}", imageId); m_imageIdentifier = imageId; } - std::string UsgsAstroFrameSensorModel::getSensorIdentifier() const { MESSAGE_LOG("Accessing sensor ID: {}", m_sensorName); return m_sensorName; } - std::string UsgsAstroFrameSensorModel::getPlatformIdentifier() const { MESSAGE_LOG("Accessing platform ID: {}", m_platformName); return m_platformName; } - std::string UsgsAstroFrameSensorModel::getCollectionIdentifier() const { MESSAGE_LOG("Accessing collection ID: {}", m_collectionIdentifier); return m_collectionIdentifier; } - std::string UsgsAstroFrameSensorModel::getTrajectoryIdentifier() const { MESSAGE_LOG("Accessing trajectory ID"); return ""; } - std::string UsgsAstroFrameSensorModel::getSensorType() const { - MESSAGE_LOG("Accessing sensor type"); - return CSM_SENSOR_TYPE_EO; + MESSAGE_LOG("Accessing sensor type"); + return CSM_SENSOR_TYPE_EO; } - std::string UsgsAstroFrameSensorModel::getSensorMode() const { - MESSAGE_LOG("Accessing sensor mode"); - return CSM_SENSOR_MODE_FRAME; + MESSAGE_LOG("Accessing sensor mode"); + return CSM_SENSOR_MODE_FRAME; } - std::string UsgsAstroFrameSensorModel::getReferenceDateAndTime() const { MESSAGE_LOG("Accessing reference data and time"); time_t ephemTime = m_ephemerisTime; struct tm t = {0}; // Initalize to all 0's - t.tm_year = 100; // This is year-1900, so 100 = 2000 + t.tm_year = 100; // This is year-1900, so 100 = 2000 t.tm_mday = 1; time_t timeSinceEpoch = mktime(&t); time_t finalTime = ephemTime + timeSinceEpoch; - char buffer [16]; + char buffer[16]; strftime(buffer, 16, "%Y%m%dT%H%M%S", localtime(&finalTime)); buffer[15] = '\0'; return buffer; } - std::string UsgsAstroFrameSensorModel::getModelState() const { - MESSAGE_LOG("Dumping model state"); - json state = { + MESSAGE_LOG("Dumping model state"); + json state = { {"m_modelName", _SENSOR_MODEL_NAME}, {"m_sensorName", m_sensorName}, {"m_platformName", m_platformName}, - {"m_focalLength" , m_focalLength}, + {"m_focalLength", m_focalLength}, {"m_iTransS", {m_iTransS[0], m_iTransS[1], m_iTransS[2]}}, {"m_iTransL", {m_iTransL[0], m_iTransL[1], m_iTransL[2]}}, {"m_boresight", {m_boresight[0], m_boresight[1], m_boresight[2]}}, @@ -730,7 +704,9 @@ std::string UsgsAstroFrameSensorModel::getModelState() const { {"m_iTransL", {m_iTransL[0], m_iTransL[1], m_iTransL[2]}}, {"m_majorAxis", m_majorAxis}, {"m_minorAxis", m_minorAxis}, - {"m_spacecraftVelocity", {m_spacecraftVelocity[0], m_spacecraftVelocity[1], m_spacecraftVelocity[2]}}, + {"m_spacecraftVelocity", + {m_spacecraftVelocity[0], m_spacecraftVelocity[1], + m_spacecraftVelocity[2]}}, {"m_sunPosition", {m_sunPosition[0], m_sunPosition[1], m_sunPosition[2]}}, {"m_startingDetectorSample", m_startingDetectorSample}, {"m_startingDetectorLine", m_startingDetectorLine}, @@ -755,40 +731,37 @@ std::string UsgsAstroFrameSensorModel::getModelState() const { {"m_currentParameterValue", m_currentParameterValue}, {"m_imageIdentifier", m_imageIdentifier}, {"m_collectionIdentifier", m_collectionIdentifier}, - {"m_referencePointXyz", {m_referencePointXyz.x, - m_referencePointXyz.y, - m_referencePointXyz.z}}, - {"m_currentParameterCovariance", m_currentParameterCovariance} - }; + {"m_referencePointXyz", + {m_referencePointXyz.x, m_referencePointXyz.y, m_referencePointXyz.z}}, + {"m_currentParameterCovariance", m_currentParameterCovariance}}; - return state.dump(); + return state.dump(); } -bool UsgsAstroFrameSensorModel::isValidModelState(const std::string& stringState, csm::WarningList *warnings) { +bool UsgsAstroFrameSensorModel::isValidModelState( + const std::string &stringState, csm::WarningList *warnings) { MESSAGE_LOG("Checking if model has valid state"); - std::vector requiredKeywords = { - "m_modelName", - "m_majorAxis", - "m_minorAxis", - "m_focalLength", - "m_startingDetectorSample", - "m_startingDetectorLine", - "m_detectorSampleSumming", - "m_detectorLineSumming", - "m_focalLengthEpsilon", - "m_nLines", - "m_nSamples", - "m_currentParameterValue", - "m_ccdCenter", - "m_spacecraftVelocity", - "m_sunPosition", - "m_distortionType", - "m_opticalDistCoeffs", - "m_transX", - "m_transY", - "m_iTransS", - "m_iTransL" - }; + std::vector requiredKeywords = {"m_modelName", + "m_majorAxis", + "m_minorAxis", + "m_focalLength", + "m_startingDetectorSample", + "m_startingDetectorLine", + "m_detectorSampleSumming", + "m_detectorLineSumming", + "m_focalLengthEpsilon", + "m_nLines", + "m_nSamples", + "m_currentParameterValue", + "m_ccdCenter", + "m_spacecraftVelocity", + "m_sunPosition", + "m_distortionType", + "m_opticalDistCoeffs", + "m_transX", + "m_transY", + "m_iTransS", + "m_iTransL"}; json jsonState = json::parse(stringState); std::vector missingKeywords; @@ -801,15 +774,15 @@ bool UsgsAstroFrameSensorModel::isValidModelState(const std::string& stringState if (!missingKeywords.empty() && warnings) { std::ostringstream oss; - std::copy(missingKeywords.begin(), missingKeywords.end(), std::ostream_iterator(oss, " ")); + std::copy(missingKeywords.begin(), missingKeywords.end(), + std::ostream_iterator(oss, " ")); - MESSAGE_LOG("State has missing keywords: {} ", oss.str()); + MESSAGE_LOG("State has missing keywords: {} ", oss.str()); - warnings->push_back(csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "State has missing keywords: " + oss.str(), - "UsgsAstroFrameSensorModel::isValidModelState" - )); + warnings->push_back( + csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "State has missing keywords: " + oss.str(), + "UsgsAstroFrameSensorModel::isValidModelState")); } std::string modelName = jsonState.value("m_modelName", ""); @@ -817,470 +790,461 @@ bool UsgsAstroFrameSensorModel::isValidModelState(const std::string& stringState if (modelName != _SENSOR_MODEL_NAME && warnings) { MESSAGE_LOG("Incorrect model name in state, expected {} but got {}", _SENSOR_MODEL_NAME, modelName); - - warnings->push_back(csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Incorrect model name in state, expected " + _SENSOR_MODEL_NAME + " but got " + modelName, - "UsgsAstroFrameSensorModel::isValidModelState" - )); + + warnings->push_back( + csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Incorrect model name in state, expected " + + _SENSOR_MODEL_NAME + " but got " + modelName, + "UsgsAstroFrameSensorModel::isValidModelState")); } return modelName == _SENSOR_MODEL_NAME && missingKeywords.empty(); } - -bool UsgsAstroFrameSensorModel::isValidIsd(const std::string& Isd, csm::WarningList *warnings) { +bool UsgsAstroFrameSensorModel::isValidIsd(const std::string &Isd, + csm::WarningList *warnings) { // no obvious clean way to truely validate ISD with it's nested structure, // or rather, it would be a pain to maintain, so just check if // we can get a valid state from ISD. Once ISD schema is 100% clear // we can change this. MESSAGE_LOG("Building isd to check model state"); - try { - std::string state = constructStateFromIsd(Isd, warnings); - return isValidModelState(state, warnings); - } - catch(...) { - return false; - } -} - - -void UsgsAstroFrameSensorModel::replaceModelState(const std::string& stringState) { - - json state = json::parse(stringState); - MESSAGE_LOG("Replacing model state"); - // The json library's .at() will except if key is missing - try { - m_modelName = state.at("m_modelName").get(); - m_majorAxis = state.at("m_majorAxis").get(); - m_minorAxis = state.at("m_minorAxis").get(); - m_focalLength = state.at("m_focalLength").get(); - m_startingDetectorSample = state.at("m_startingDetectorSample").get(); - m_startingDetectorLine = state.at("m_startingDetectorLine").get(); - m_detectorSampleSumming = state.at("m_detectorSampleSumming").get(); - m_detectorLineSumming = state.at("m_detectorLineSumming").get(); - m_focalLengthEpsilon = state.at("m_focalLengthEpsilon").get(); - m_nLines = state.at("m_nLines").get(); - m_nSamples = state.at("m_nSamples").get(); - m_ephemerisTime = state.at("m_ephemerisTime").get(); - m_currentParameterValue = state.at("m_currentParameterValue").get>(); - m_ccdCenter = state.at("m_ccdCenter").get>(); - m_spacecraftVelocity = state.at("m_spacecraftVelocity").get>(); - m_sunPosition = state.at("m_sunPosition").get>(); - m_distortionType = (DistortionType)state.at("m_distortionType").get(); - m_opticalDistCoeffs = state.at("m_opticalDistCoeffs").get>(); - m_transX = state.at("m_transX").get>(); - m_transY = state.at("m_transY").get>(); - m_iTransS = state.at("m_iTransS").get>(); - m_iTransL = state.at("m_iTransL").get>(); - m_imageIdentifier = state.at("m_imageIdentifier").get(); - m_platformName = state.at("m_platformName").get(); - m_sensorName = state.at("m_sensorName").get(); - m_collectionIdentifier = state.at("m_collectionIdentifier").get(); - // Set reference point to the center of the image - m_referencePointXyz = imageToGround(csm::ImageCoord(m_nLines/2.0, m_nSamples/2.0)); - m_currentParameterCovariance = state.at("m_currentParameterCovariance").get>(); - } - catch(std::out_of_range& e) { - MESSAGE_LOG("State keywords required to generate sensor model missing: " + std::string(e.what()) + "\nUsing model string: " + stringState + "UsgsAstroFrameSensorModel::replaceModelState"); - throw csm::Error(csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE, - "State keywords required to generate sensor model missing: " + std::string(e.what()) + "\nUsing model string: " + stringState, - "UsgsAstroFrameSensorModel::replaceModelState"); - } + try { + std::string state = constructStateFromIsd(Isd, warnings); + return isValidModelState(state, warnings); + } catch (...) { + return false; + } } +void UsgsAstroFrameSensorModel::replaceModelState( + const std::string &stringState) { + json state = json::parse(stringState); + MESSAGE_LOG("Replacing model state"); + // The json library's .at() will except if key is missing + try { + m_modelName = state.at("m_modelName").get(); + m_majorAxis = state.at("m_majorAxis").get(); + m_minorAxis = state.at("m_minorAxis").get(); + m_focalLength = state.at("m_focalLength").get(); + m_startingDetectorSample = + state.at("m_startingDetectorSample").get(); + m_startingDetectorLine = state.at("m_startingDetectorLine").get(); + m_detectorSampleSumming = state.at("m_detectorSampleSumming").get(); + m_detectorLineSumming = state.at("m_detectorLineSumming").get(); + m_focalLengthEpsilon = state.at("m_focalLengthEpsilon").get(); + m_nLines = state.at("m_nLines").get(); + m_nSamples = state.at("m_nSamples").get(); + m_ephemerisTime = state.at("m_ephemerisTime").get(); + m_currentParameterValue = + state.at("m_currentParameterValue").get>(); + m_ccdCenter = state.at("m_ccdCenter").get>(); + m_spacecraftVelocity = + state.at("m_spacecraftVelocity").get>(); + m_sunPosition = state.at("m_sunPosition").get>(); + m_distortionType = (DistortionType)state.at("m_distortionType").get(); + m_opticalDistCoeffs = + state.at("m_opticalDistCoeffs").get>(); + m_transX = state.at("m_transX").get>(); + m_transY = state.at("m_transY").get>(); + m_iTransS = state.at("m_iTransS").get>(); + m_iTransL = state.at("m_iTransL").get>(); + m_imageIdentifier = state.at("m_imageIdentifier").get(); + m_platformName = state.at("m_platformName").get(); + m_sensorName = state.at("m_sensorName").get(); + m_collectionIdentifier = + state.at("m_collectionIdentifier").get(); + // Set reference point to the center of the image + m_referencePointXyz = + imageToGround(csm::ImageCoord(m_nLines / 2.0, m_nSamples / 2.0)); + m_currentParameterCovariance = + state.at("m_currentParameterCovariance").get>(); + } catch (std::out_of_range &e) { + MESSAGE_LOG("State keywords required to generate sensor model missing: " + + std::string(e.what()) + "\nUsing model string: " + stringState + + "UsgsAstroFrameSensorModel::replaceModelState"); + throw csm::Error( + csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE, + "State keywords required to generate sensor model missing: " + + std::string(e.what()) + "\nUsing model string: " + stringState, + "UsgsAstroFrameSensorModel::replaceModelState"); + } +} -std::string UsgsAstroFrameSensorModel::constructStateFromIsd(const std::string& jsonIsd, csm::WarningList* warnings) { - MESSAGE_LOG("Constructing state from isd"); - json parsedIsd = json::parse(jsonIsd); - json state = {}; - - MESSAGE_LOG("Constructing state from isd"); - - csm::WarningList* parsingWarnings = new csm::WarningList; - - state["m_modelName"] = ale::getSensorModelName(parsedIsd); - state["m_imageIdentifier"] = ale::getImageId(parsedIsd); - state["m_sensorName"] = ale::getSensorName(parsedIsd); - state["m_platformName"] = ale::getPlatformName(parsedIsd); - - state["m_startingDetectorSample"] = ale::getDetectorStartingSample(parsedIsd); - state["m_startingDetectorLine"] = ale::getDetectorStartingLine(parsedIsd); - state["m_detectorSampleSumming"] = ale::getSampleSumming(parsedIsd); - state["m_detectorLineSumming"] = ale::getLineSumming(parsedIsd); - - state["m_focalLength"] = ale::getFocalLength(parsedIsd); - try { - state["m_focalLengthEpsilon"] = ale::getFocalLengthUncertainty(parsedIsd); - } - catch (std::runtime_error& e) { - state["m_focalLengthEpsilon"] = 0.0; - } - - - state["m_currentParameterValue"] = json(); - - ale::States inst_state = ale::getInstrumentPosition(parsedIsd); - std::vector ephemTime = inst_state.getTimes(); - std::vector instStates = inst_state.getStates(); - ale::Orientations j2000_to_target = ale::getBodyRotation(parsedIsd); - ale::State rotatedInstState; - std::vector positions = {}; - std::vector velocities = {}; - - for (int i = 0; i < ephemTime.size(); i++) { - rotatedInstState = j2000_to_target.rotateStateAt(ephemTime[i], instStates[i], ale::SLERP); - // ALE operates in Km and we want m - positions.push_back(rotatedInstState.position.x * 1000); - positions.push_back(rotatedInstState.position.y * 1000); - positions.push_back(rotatedInstState.position.z * 1000); - velocities.push_back(rotatedInstState.velocity.x * 1000); - velocities.push_back(rotatedInstState.velocity.y * 1000); - velocities.push_back(rotatedInstState.velocity.z * 1000); - } - - if (!positions.empty() && positions.size() != 3) { - MESSAGE_LOG("Sensor position does not have 3 values, " - "UsgsAstroFrameSensorModel::constructStateFromIsd()"); - parsingWarnings->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Sensor position does not have 3 values.", - "UsgsAstroFrameSensorModel::constructStateFromIsd()")); - state["m_currentParameterValue"][0] = 0; - state["m_currentParameterValue"][1] = 0; - state["m_currentParameterValue"][2] = 0; - } - else { - state["m_currentParameterValue"] = positions; - } - - // get sensor_velocity - if (!velocities.empty() && velocities.size() != 3) { - MESSAGE_LOG("Sensor velocity does not have 3 values, " - "UsgsAstroFrameSensorModel::constructStateFromIsd()"); - parsingWarnings->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Sensor velocity does not have 3 values.", - "UsgsAstroFrameSensorModel::constructStateFromIsd()")); - } - else { - state["m_spacecraftVelocity"] = velocities; - } +std::string UsgsAstroFrameSensorModel::constructStateFromIsd( + const std::string &jsonIsd, csm::WarningList *warnings) { + MESSAGE_LOG("Constructing state from isd"); + json parsedIsd = json::parse(jsonIsd); + json state = {}; + MESSAGE_LOG("Constructing state from isd"); - // get sun_position - // sun position is not strictly necessary, but is required for getIlluminationDirection. - ale::States sunState = ale::getSunPosition(parsedIsd); - std::vector sunStates = sunState.getStates(); - ephemTime = sunState.getTimes(); - ale::State rotatedSunState; - std::vector sunPositions = {}; - - for (int i = 0; i < ephemTime.size(); i++) { - rotatedSunState = j2000_to_target.rotateStateAt(ephemTime[i], sunStates[i]); - // ALE operates in Km and we want m - sunPositions.push_back(rotatedSunState.position.x * 1000); - sunPositions.push_back(rotatedSunState.position.y * 1000); - sunPositions.push_back(rotatedSunState.position.z * 1000); - } + csm::WarningList *parsingWarnings = new csm::WarningList; - state["m_sunPosition"] = sunPositions; - - // get sensor_orientation quaternion - ale::Orientations j2000_to_sensor = ale::getInstrumentPointing(parsedIsd); - ale::Orientations sensor_to_j2000 = j2000_to_sensor.inverse(); - ale::Orientations sensor_to_target = j2000_to_target * sensor_to_j2000; - ephemTime = sensor_to_target.getTimes(); - std::vector quaternion; - std::vector quaternions; - - for (int i = 0; i < ephemTime.size(); i++) { - ale::Rotation rotation = sensor_to_target.interpolate(ephemTime[i], ale::SLERP); - quaternion = rotation.toQuaternion(); - quaternions.push_back(quaternion[1]); - quaternions.push_back(quaternion[2]); - quaternions.push_back(quaternion[3]); - quaternions.push_back(quaternion[0]); - } + state["m_modelName"] = ale::getSensorModelName(parsedIsd); + state["m_imageIdentifier"] = ale::getImageId(parsedIsd); + state["m_sensorName"] = ale::getSensorName(parsedIsd); + state["m_platformName"] = ale::getPlatformName(parsedIsd); - if (quaternions.size() != 4) { - MESSAGE_LOG("Sensor quaternion does not have 4 values, " - "UsgsAstroFrameSensorModel::constructStateFromIsd()"); - parsingWarnings->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Sensor orientation quaternion does not have 4 values.", - "UsgsAstroFrameSensorModel::constructStateFromIsd()")); - } - else { - state["m_currentParameterValue"][3] = quaternions[0]; - state["m_currentParameterValue"][4] = quaternions[1]; - state["m_currentParameterValue"][5] = quaternions[2]; - state["m_currentParameterValue"][6] = quaternions[3]; - } + state["m_startingDetectorSample"] = ale::getDetectorStartingSample(parsedIsd); + state["m_startingDetectorLine"] = ale::getDetectorStartingLine(parsedIsd); + state["m_detectorSampleSumming"] = ale::getSampleSumming(parsedIsd); + state["m_detectorLineSumming"] = ale::getLineSumming(parsedIsd); - // get optical_distortion - state["m_distortionType"] = getDistortionModel(ale::getDistortionModel(parsedIsd)); - state["m_opticalDistCoeffs"] = ale::getDistortionCoeffs(parsedIsd); + state["m_focalLength"] = ale::getFocalLength(parsedIsd); + try { + state["m_focalLengthEpsilon"] = ale::getFocalLengthUncertainty(parsedIsd); + } catch (std::runtime_error &e) { + state["m_focalLengthEpsilon"] = 0.0; + } - // get detector_center - state["m_ccdCenter"][0] = ale::getDetectorCenterLine(parsedIsd); - state["m_ccdCenter"][1] = ale::getDetectorCenterSample(parsedIsd); + state["m_currentParameterValue"] = json(); + ale::States inst_state = ale::getInstrumentPosition(parsedIsd); + std::vector ephemTime = inst_state.getTimes(); + std::vector instStates = inst_state.getStates(); + ale::Orientations j2000_to_target = ale::getBodyRotation(parsedIsd); + ale::State rotatedInstState; + std::vector positions = {}; + std::vector velocities = {}; - // get radii + for (int i = 0; i < ephemTime.size(); i++) { + rotatedInstState = + j2000_to_target.rotateStateAt(ephemTime[i], instStates[i], ale::SLERP); // ALE operates in Km and we want m - state["m_minorAxis"] = ale::getSemiMinorRadius(parsedIsd) * 1000; - state["m_majorAxis"] = ale::getSemiMajorRadius(parsedIsd) * 1000; - - - // get reference_height - state["m_minElevation"] = ale::getMinHeight(parsedIsd); - state["m_maxElevation"] = ale::getMaxHeight(parsedIsd); + positions.push_back(rotatedInstState.position.x * 1000); + positions.push_back(rotatedInstState.position.y * 1000); + positions.push_back(rotatedInstState.position.z * 1000); + velocities.push_back(rotatedInstState.velocity.x * 1000); + velocities.push_back(rotatedInstState.velocity.y * 1000); + velocities.push_back(rotatedInstState.velocity.z * 1000); + } + if (!positions.empty() && positions.size() != 3) { + MESSAGE_LOG( + "Sensor position does not have 3 values, " + "UsgsAstroFrameSensorModel::constructStateFromIsd()"); + parsingWarnings->push_back( + csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Sensor position does not have 3 values.", + "UsgsAstroFrameSensorModel::constructStateFromIsd()")); + state["m_currentParameterValue"][0] = 0; + state["m_currentParameterValue"][1] = 0; + state["m_currentParameterValue"][2] = 0; + } else { + state["m_currentParameterValue"] = positions; + } - state["m_ephemerisTime"] = ale::getCenterTime(parsedIsd); - state["m_nLines"] = ale::getTotalLines(parsedIsd); - state["m_nSamples"] = ale::getTotalSamples(parsedIsd); + // get sensor_velocity + if (!velocities.empty() && velocities.size() != 3) { + MESSAGE_LOG( + "Sensor velocity does not have 3 values, " + "UsgsAstroFrameSensorModel::constructStateFromIsd()"); + parsingWarnings->push_back( + csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Sensor velocity does not have 3 values.", + "UsgsAstroFrameSensorModel::constructStateFromIsd()")); + } else { + state["m_spacecraftVelocity"] = velocities; + } - state["m_iTransL"] = ale::getFocal2PixelLines(parsedIsd); - state["m_iTransS"] = ale::getFocal2PixelSamples(parsedIsd); + // get sun_position + // sun position is not strictly necessary, but is required for + // getIlluminationDirection. + ale::States sunState = ale::getSunPosition(parsedIsd); + std::vector sunStates = sunState.getStates(); + ephemTime = sunState.getTimes(); + ale::State rotatedSunState; + std::vector sunPositions = {}; + + for (int i = 0; i < ephemTime.size(); i++) { + rotatedSunState = j2000_to_target.rotateStateAt(ephemTime[i], sunStates[i]); + // ALE operates in Km and we want m + sunPositions.push_back(rotatedSunState.position.x * 1000); + sunPositions.push_back(rotatedSunState.position.y * 1000); + sunPositions.push_back(rotatedSunState.position.z * 1000); + } - // We don't pass the pixel to focal plane transformation so invert the - // focal plane to pixel transformation - try { - double determinant = state["m_iTransL"][1].get() * state["m_iTransS"][2].get() - - state["m_iTransL"][2].get() * state["m_iTransS"][1].get(); + state["m_sunPosition"] = sunPositions; + + // get sensor_orientation quaternion + ale::Orientations j2000_to_sensor = ale::getInstrumentPointing(parsedIsd); + ale::Orientations sensor_to_j2000 = j2000_to_sensor.inverse(); + ale::Orientations sensor_to_target = j2000_to_target * sensor_to_j2000; + ephemTime = sensor_to_target.getTimes(); + std::vector quaternion; + std::vector quaternions; + + for (int i = 0; i < ephemTime.size(); i++) { + ale::Rotation rotation = + sensor_to_target.interpolate(ephemTime[i], ale::SLERP); + quaternion = rotation.toQuaternion(); + quaternions.push_back(quaternion[1]); + quaternions.push_back(quaternion[2]); + quaternions.push_back(quaternion[3]); + quaternions.push_back(quaternion[0]); + } - state["m_transX"][1] = state["m_iTransL"][1].get() / determinant; - state["m_transX"][2] = -state["m_iTransS"][1].get() / determinant; - state["m_transX"][0] = -(state["m_transX"][1].get() * state["m_iTransL"][0].get() + - state["m_transX"][2].get() * state["m_iTransS"][0].get()); + if (quaternions.size() != 4) { + MESSAGE_LOG( + "Sensor quaternion does not have 4 values, " + "UsgsAstroFrameSensorModel::constructStateFromIsd()"); + parsingWarnings->push_back( + csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Sensor orientation quaternion does not have 4 values.", + "UsgsAstroFrameSensorModel::constructStateFromIsd()")); + } else { + state["m_currentParameterValue"][3] = quaternions[0]; + state["m_currentParameterValue"][4] = quaternions[1]; + state["m_currentParameterValue"][5] = quaternions[2]; + state["m_currentParameterValue"][6] = quaternions[3]; + } - state["m_transY"][1] = -state["m_iTransL"][2].get() / determinant; - state["m_transY"][2] = state["m_iTransS"][2].get() / determinant; - state["m_transY"][0] = -(state["m_transY"][1].get() * state["m_iTransL"][0].get() + - state["m_transY"][2].get() * state["m_iTransS"][0].get()); - } - catch (...) { - MESSAGE_LOG("Could not compute detector pixel to focal plane coordinate transformation."); - parsingWarnings->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not compute detector pixel to focal plane coordinate transformation.", - "UsgsAstroFrameSensorModel::constructStateFromIsd()")); - } + // get optical_distortion + state["m_distortionType"] = + getDistortionModel(ale::getDistortionModel(parsedIsd)); + state["m_opticalDistCoeffs"] = ale::getDistortionCoeffs(parsedIsd); + + // get detector_center + state["m_ccdCenter"][0] = ale::getDetectorCenterLine(parsedIsd); + state["m_ccdCenter"][1] = ale::getDetectorCenterSample(parsedIsd); + + // get radii + // ALE operates in Km and we want m + state["m_minorAxis"] = ale::getSemiMinorRadius(parsedIsd) * 1000; + state["m_majorAxis"] = ale::getSemiMajorRadius(parsedIsd) * 1000; + + // get reference_height + state["m_minElevation"] = ale::getMinHeight(parsedIsd); + state["m_maxElevation"] = ale::getMaxHeight(parsedIsd); + + state["m_ephemerisTime"] = ale::getCenterTime(parsedIsd); + state["m_nLines"] = ale::getTotalLines(parsedIsd); + state["m_nSamples"] = ale::getTotalSamples(parsedIsd); + + state["m_iTransL"] = ale::getFocal2PixelLines(parsedIsd); + state["m_iTransS"] = ale::getFocal2PixelSamples(parsedIsd); + + // We don't pass the pixel to focal plane transformation so invert the + // focal plane to pixel transformation + try { + double determinant = state["m_iTransL"][1].get() * + state["m_iTransS"][2].get() - + state["m_iTransL"][2].get() * + state["m_iTransS"][1].get(); + + state["m_transX"][1] = state["m_iTransL"][1].get() / determinant; + state["m_transX"][2] = -state["m_iTransS"][1].get() / determinant; + state["m_transX"][0] = -(state["m_transX"][1].get() * + state["m_iTransL"][0].get() + + state["m_transX"][2].get() * + state["m_iTransS"][0].get()); + + state["m_transY"][1] = -state["m_iTransL"][2].get() / determinant; + state["m_transY"][2] = state["m_iTransS"][2].get() / determinant; + state["m_transY"][0] = -(state["m_transY"][1].get() * + state["m_iTransL"][0].get() + + state["m_transY"][2].get() * + state["m_iTransS"][0].get()); + } catch (...) { + MESSAGE_LOG( + "Could not compute detector pixel to focal plane coordinate " + "transformation."); + parsingWarnings->push_back( + csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not compute detector pixel to focal plane " + "coordinate transformation.", + "UsgsAstroFrameSensorModel::constructStateFromIsd()")); + } + state["m_referencePointXyz"] = std::vector(3, 0.0); + state["m_currentParameterCovariance"] = + std::vector(NUM_PARAMETERS * NUM_PARAMETERS, 0.0); + for (int i = 0; i < NUM_PARAMETERS * NUM_PARAMETERS; + i += NUM_PARAMETERS + 1) { + state["m_currentParameterCovariance"][i] = 1; + } + state["m_collectionIdentifier"] = ""; - state["m_referencePointXyz"] = std::vector(3, 0.0); - state["m_currentParameterCovariance"] = std::vector(NUM_PARAMETERS*NUM_PARAMETERS,0.0); - for (int i = 0; i < NUM_PARAMETERS*NUM_PARAMETERS; i += NUM_PARAMETERS+1) { - state["m_currentParameterCovariance"][i] = 1; - } - state["m_collectionIdentifier"] = ""; - - if (!parsingWarnings->empty()) { - if (warnings) { - warnings->insert(warnings->end(), parsingWarnings->begin(), parsingWarnings->end()); - } - delete parsingWarnings; - parsingWarnings = nullptr; - MESSAGE_LOG("ISD is invalid for creating the sensor model."); - - throw csm::Error(csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE, - "ISD is invalid for creating the sensor model.", - "UsgsAstroFrameSensorModel::constructStateFromIsd"); + if (!parsingWarnings->empty()) { + if (warnings) { + warnings->insert(warnings->end(), parsingWarnings->begin(), + parsingWarnings->end()); } - delete parsingWarnings; parsingWarnings = nullptr; + MESSAGE_LOG("ISD is invalid for creating the sensor model."); - return state.dump(); -} + throw csm::Error(csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE, + "ISD is invalid for creating the sensor model.", + "UsgsAstroFrameSensorModel::constructStateFromIsd"); + } + delete parsingWarnings; + parsingWarnings = nullptr; + return state.dump(); +} csm::EcefCoord UsgsAstroFrameSensorModel::getReferencePoint() const { MESSAGE_LOG("Accessing reference point x: {}, y: {}, z: {}", - m_referencePointXyz.x, m_referencePointXyz.y, m_referencePointXyz.z); + m_referencePointXyz.x, m_referencePointXyz.y, + m_referencePointXyz.z); return m_referencePointXyz; } - -void UsgsAstroFrameSensorModel::setReferencePoint(const csm::EcefCoord &groundPt) { - MESSAGE_LOG("Setting reference point to {}, {}, {}", - groundPt.x, groundPt.y, groundPt.z); +void UsgsAstroFrameSensorModel::setReferencePoint( + const csm::EcefCoord &groundPt) { + MESSAGE_LOG("Setting reference point to {}, {}, {}", groundPt.x, groundPt.y, + groundPt.z); m_referencePointXyz = groundPt; } - int UsgsAstroFrameSensorModel::getNumParameters() const { MESSAGE_LOG("Accessing num parameters: {}", NUM_PARAMETERS); return NUM_PARAMETERS; } - std::string UsgsAstroFrameSensorModel::getParameterName(int index) const { MESSAGE_LOG("Setting parameter name to {}", index); return m_parameterName[index]; } - std::string UsgsAstroFrameSensorModel::getParameterUnits(int index) const { MESSAGE_LOG("Accessing parameter units for {}", index); if (index < 3) { return "m"; - } - else { + } else { return "radians"; } } - bool UsgsAstroFrameSensorModel::hasShareableParameters() const { MESSAGE_LOG("Checking for shareable parameters"); return false; } - bool UsgsAstroFrameSensorModel::isParameterShareable(int index) const { MESSAGE_LOG("Checking is parameter: {} is shareable", index); return false; } - -csm::SharingCriteria UsgsAstroFrameSensorModel::getParameterSharingCriteria(int index) const { - MESSAGE_LOG("Checking sharing criteria for parameter {}. " - "Sharing is not supported, throwing exception", index); - // Parameter sharing is not supported for this sensor, - // all indices are out of range - throw csm::Error( - csm::Error::INDEX_OUT_OF_RANGE, - "Index out of range.", - "UsgsAstroLsSensorModel::getParameterSharingCriteria"); +csm::SharingCriteria UsgsAstroFrameSensorModel::getParameterSharingCriteria( + int index) const { + MESSAGE_LOG( + "Checking sharing criteria for parameter {}. " + "Sharing is not supported, throwing exception", + index); + // Parameter sharing is not supported for this sensor, + // all indices are out of range + throw csm::Error(csm::Error::INDEX_OUT_OF_RANGE, "Index out of range.", + "UsgsAstroLsSensorModel::getParameterSharingCriteria"); } - double UsgsAstroFrameSensorModel::getParameterValue(int index) const { - MESSAGE_LOG("Accessing parameter value {} at index: {}", m_currentParameterValue[index], index); + MESSAGE_LOG("Accessing parameter value {} at index: {}", + m_currentParameterValue[index], index); return m_currentParameterValue[index]; - } - void UsgsAstroFrameSensorModel::setParameterValue(int index, double value) { MESSAGE_LOG("Setting parameter value: {} at index: {}", value, index); m_currentParameterValue[index] = value; } - csm::param::Type UsgsAstroFrameSensorModel::getParameterType(int index) const { - MESSAGE_LOG("Accessing parameter type: {} at index: {}", m_parameterType[index], index); + MESSAGE_LOG("Accessing parameter type: {} at index: {}", + m_parameterType[index], index); return m_parameterType[index]; } - -void UsgsAstroFrameSensorModel::setParameterType(int index, csm::param::Type pType) { - MESSAGE_LOG("Setting parameter type: {} at index: {}", pType, index); - m_parameterType[index] = pType; +void UsgsAstroFrameSensorModel::setParameterType(int index, + csm::param::Type pType) { + MESSAGE_LOG("Setting parameter type: {} at index: {}", pType, index); + m_parameterType[index] = pType; } - -double UsgsAstroFrameSensorModel::getParameterCovariance(int index1, int index2) const { - int index = UsgsAstroFrameSensorModel::NUM_PARAMETERS * index1 + index2; - MESSAGE_LOG("Accessing parameter covar: {} between index1: {} and index2: {}", m_currentParameterCovariance[index], index1, index2); - return m_currentParameterCovariance[index]; +double UsgsAstroFrameSensorModel::getParameterCovariance(int index1, + int index2) const { + int index = UsgsAstroFrameSensorModel::NUM_PARAMETERS * index1 + index2; + MESSAGE_LOG("Accessing parameter covar: {} between index1: {} and index2: {}", + m_currentParameterCovariance[index], index1, index2); + return m_currentParameterCovariance[index]; } - -void UsgsAstroFrameSensorModel::setParameterCovariance(int index1, int index2, double covariance) { - MESSAGE_LOG("Setting parameter covar: {} between index1: {} and index2: {}", - covariance, index1, index2); - int index = UsgsAstroFrameSensorModel::NUM_PARAMETERS * index1 + index2; - m_currentParameterCovariance[index] = covariance; +void UsgsAstroFrameSensorModel::setParameterCovariance(int index1, int index2, + double covariance) { + MESSAGE_LOG("Setting parameter covar: {} between index1: {} and index2: {}", + covariance, index1, index2); + int index = UsgsAstroFrameSensorModel::NUM_PARAMETERS * index1 + index2; + m_currentParameterCovariance[index] = covariance; } - int UsgsAstroFrameSensorModel::getNumGeometricCorrectionSwitches() const { MESSAGE_LOG("Accessing num geom correction switches"); return 0; } - -std::string UsgsAstroFrameSensorModel::getGeometricCorrectionName(int index) const { - MESSAGE_LOG("Accessing name of geometric correction switch {}. " - "Geometric correction switches are not supported, throwing exception", - index); - // Since there are no geometric corrections, all indices are out of range - throw csm::Error( - csm::Error::INDEX_OUT_OF_RANGE, - "Index is out of range.", - "UsgsAstroLsSensorModel::getGeometricCorrectionName"); +std::string UsgsAstroFrameSensorModel::getGeometricCorrectionName( + int index) const { + MESSAGE_LOG( + "Accessing name of geometric correction switch {}. " + "Geometric correction switches are not supported, throwing exception", + index); + // Since there are no geometric corrections, all indices are out of range + throw csm::Error(csm::Error::INDEX_OUT_OF_RANGE, "Index is out of range.", + "UsgsAstroLsSensorModel::getGeometricCorrectionName"); } - -void UsgsAstroFrameSensorModel::setGeometricCorrectionSwitch(int index, - bool value, - csm::param::Type pType) { - MESSAGE_LOG("Setting geometric correction switch {} to {} " - "with parameter type {}. " - "Geometric correction switches are not supported, throwing exception", - index, value, pType); - // Since there are no geometric corrections, all indices are out of range - throw csm::Error( - csm::Error::INDEX_OUT_OF_RANGE, - "Index is out of range.", - "UsgsAstroLsSensorModel::setGeometricCorrectionSwitch"); +void UsgsAstroFrameSensorModel::setGeometricCorrectionSwitch( + int index, bool value, csm::param::Type pType) { + MESSAGE_LOG( + "Setting geometric correction switch {} to {} " + "with parameter type {}. " + "Geometric correction switches are not supported, throwing exception", + index, value, pType); + // Since there are no geometric corrections, all indices are out of range + throw csm::Error(csm::Error::INDEX_OUT_OF_RANGE, "Index is out of range.", + "UsgsAstroLsSensorModel::setGeometricCorrectionSwitch"); } - bool UsgsAstroFrameSensorModel::getGeometricCorrectionSwitch(int index) const { - MESSAGE_LOG("Accessing value of geometric correction switch {}. " - "Geometric correction switches are not supported, throwing exception", - index); - // Since there are no geometric corrections, all indices are out of range - throw csm::Error( - csm::Error::INDEX_OUT_OF_RANGE, - "Index is out of range.", - "UsgsAstroLsSensorModel::getGeometricCorrectionSwitch"); + MESSAGE_LOG( + "Accessing value of geometric correction switch {}. " + "Geometric correction switches are not supported, throwing exception", + index); + // Since there are no geometric corrections, all indices are out of range + throw csm::Error(csm::Error::INDEX_OUT_OF_RANGE, "Index is out of range.", + "UsgsAstroLsSensorModel::getGeometricCorrectionSwitch"); } - std::vector UsgsAstroFrameSensorModel::getCrossCovarianceMatrix( - const GeometricModel &comparisonModel, - csm::param::Set pSet, + const GeometricModel &comparisonModel, csm::param::Set pSet, const GeometricModelList &otherModels) const { - MESSAGE_LOG("Accessing cross covariance matrix"); + MESSAGE_LOG("Accessing cross covariance matrix"); - // No correlation between models. - const std::vector& indices = getParameterSetIndices(pSet); - size_t num_rows = indices.size(); - const std::vector& indices2 = comparisonModel.getParameterSetIndices(pSet); - size_t num_cols = indices.size(); + // No correlation between models. + const std::vector &indices = getParameterSetIndices(pSet); + size_t num_rows = indices.size(); + const std::vector &indices2 = + comparisonModel.getParameterSetIndices(pSet); + size_t num_cols = indices.size(); - return std::vector(num_rows * num_cols, 0.0); + return std::vector(num_rows * num_cols, 0.0); } - csm::Ellipsoid UsgsAstroFrameSensorModel::getEllipsoid() const { - MESSAGE_LOG("Accessing ellipsoid radii {} {}", - m_majorAxis, m_minorAxis); - return csm::Ellipsoid(m_majorAxis, m_minorAxis); + MESSAGE_LOG("Accessing ellipsoid radii {} {}", m_majorAxis, m_minorAxis); + return csm::Ellipsoid(m_majorAxis, m_minorAxis); } - void UsgsAstroFrameSensorModel::setEllipsoid(const csm::Ellipsoid &ellipsoid) { - MESSAGE_LOG("Setting ellipsoid radii {} {}", - ellipsoid.getSemiMajorRadius(), ellipsoid.getSemiMinorRadius()); - m_majorAxis = ellipsoid.getSemiMajorRadius(); - m_minorAxis = ellipsoid.getSemiMinorRadius(); + MESSAGE_LOG("Setting ellipsoid radii {} {}", ellipsoid.getSemiMajorRadius(), + ellipsoid.getSemiMinorRadius()); + m_majorAxis = ellipsoid.getSemiMajorRadius(); + m_minorAxis = ellipsoid.getSemiMinorRadius(); } - -void UsgsAstroFrameSensorModel::calcRotationMatrix( - double m[3][3]) const { +void UsgsAstroFrameSensorModel::calcRotationMatrix(double m[3][3]) const { MESSAGE_LOG("Calculating rotation matrix"); // Trigonometric functions for rotation matrix double x = m_currentParameterValue[3]; @@ -1294,20 +1258,19 @@ void UsgsAstroFrameSensorModel::calcRotationMatrix( w /= norm; z /= norm; - m[0][0] = w*w + x*x - y*y - z*z; - m[0][1] = 2 * (x*y - w*z); - m[0][2] = 2 * (w*y + x*z); - m[1][0] = 2 * (x*y + w*z); - m[1][1] = w*w - x*x + y*y - z*z; - m[1][2] = 2 * (y*z - w*x); - m[2][0] = 2 * (x*z - w*y); - m[2][1] = 2 * (w*x + y*z); - m[2][2] = w*w - x*x - y*y + z*z; + m[0][0] = w * w + x * x - y * y - z * z; + m[0][1] = 2 * (x * y - w * z); + m[0][2] = 2 * (w * y + x * z); + m[1][0] = 2 * (x * y + w * z); + m[1][1] = w * w - x * x + y * y - z * z; + m[1][2] = 2 * (y * z - w * x); + m[2][0] = 2 * (x * z - w * y); + m[2][1] = 2 * (w * x + y * z); + m[2][2] = w * w - x * x - y * y + z * z; } - void UsgsAstroFrameSensorModel::calcRotationMatrix( - double m[3][3], const std::vector &adjustments) const { + double m[3][3], const std::vector &adjustments) const { MESSAGE_LOG("Calculating rotation matrix with adjustments"); // Trigonometric functions for rotation matrix double x = getValue(3, adjustments); @@ -1315,83 +1278,70 @@ void UsgsAstroFrameSensorModel::calcRotationMatrix( double z = getValue(5, adjustments); double w = getValue(6, adjustments); - m[0][0] = w*w + x*x - y*y - z*z; - m[0][1] = 2 * (x*y - w*z); - m[0][2] = 2 * (w*y + x*z); - m[1][0] = 2 * (x*y + w*z); - m[1][1] = w*w - x*x + y*y - z*z; - m[1][2] = 2 * (y*z - w*x); - m[2][0] = 2 * (x*z - w*y); - m[2][1] = 2 * (w*x + y*z); - m[2][2] = w*w - x*x - y*y + z*z; + m[0][0] = w * w + x * x - y * y - z * z; + m[0][1] = 2 * (x * y - w * z); + m[0][2] = 2 * (w * y + x * z); + m[1][0] = 2 * (x * y + w * z); + m[1][1] = w * w - x * x + y * y - z * z; + m[1][2] = 2 * (y * z - w * x); + m[2][0] = 2 * (x * z - w * y); + m[2][1] = 2 * (w * x + y * z); + m[2][2] = w * w - x * x - y * y + z * z; } - void UsgsAstroFrameSensorModel::losEllipsoidIntersect( - const double height, - const double xc, - const double yc, - const double zc, - const double xl, - const double yl, - const double zl, - double& x, - double& y, - double& z ) const -{ - MESSAGE_LOG("Calculating losEllipsoidIntersect with height: {},\n\ + const double height, const double xc, const double yc, const double zc, + const double xl, const double yl, const double zl, double &x, double &y, + double &z) const { + MESSAGE_LOG( + "Calculating losEllipsoidIntersect with height: {},\n\ xc: {}, yc: {}, zc: {}\n\ - xl: {}, yl: {}, zl: {}", height, - xc, yc, zc, - xl, yl, zl); - // Helper function which computes the intersection of the image ray - // with an expanded ellipsoid. All vectors are in earth-centered-fixed - // coordinate system with origin at the center of the earth. - - double ap, bp, k; - ap = m_majorAxis + height; - bp = m_minorAxis + height; - k = ap * ap / (bp * bp); - - // Solve quadratic equation for scale factor - // applied to image ray to compute ground point - - double at, bt, ct, quadTerm; - at = xl * xl + yl * yl + k * zl * zl; - bt = 2.0 * (xl * xc + yl * yc + k * zl * zc); - ct = xc * xc + yc * yc + k * zc * zc - ap * ap; - quadTerm = bt * bt - 4.0 * at * ct; - - // If quadTerm is negative, the image ray does not - // intersect the ellipsoid. Setting the quadTerm to - // zero means solving for a point on the ray nearest - // the surface of the ellisoid. - - if ( 0.0 > quadTerm ) - { - quadTerm = 0.0; - } - double scale; - scale = (-bt - sqrt (quadTerm)) / (2.0 * at); - // Compute ground point vector + xl: {}, yl: {}, zl: {}", + height, xc, yc, zc, xl, yl, zl); + // Helper function which computes the intersection of the image ray + // with an expanded ellipsoid. All vectors are in earth-centered-fixed + // coordinate system with origin at the center of the earth. + + double ap, bp, k; + ap = m_majorAxis + height; + bp = m_minorAxis + height; + k = ap * ap / (bp * bp); + + // Solve quadratic equation for scale factor + // applied to image ray to compute ground point + + double at, bt, ct, quadTerm; + at = xl * xl + yl * yl + k * zl * zl; + bt = 2.0 * (xl * xc + yl * yc + k * zl * zc); + ct = xc * xc + yc * yc + k * zc * zc - ap * ap; + quadTerm = bt * bt - 4.0 * at * ct; + + // If quadTerm is negative, the image ray does not + // intersect the ellipsoid. Setting the quadTerm to + // zero means solving for a point on the ray nearest + // the surface of the ellisoid. + + if (0.0 > quadTerm) { + quadTerm = 0.0; + } + double scale; + scale = (-bt - sqrt(quadTerm)) / (2.0 * at); + // Compute ground point vector - x = xc + scale * xl; - y = yc + scale * yl; - z = zc + scale * zl; + x = xc + scale * xl; + y = yc + scale * yl; + z = zc + scale * zl; - MESSAGE_LOG("Calculated losEllipsoidIntersect at: {}, {}, {}", - x, y, z); + MESSAGE_LOG("Calculated losEllipsoidIntersect at: {}, {}, {}", x, y, z); } - /***** Helper Functions *****/ double UsgsAstroFrameSensorModel::getValue( - int index, - const std::vector &adjustments) const -{ - MESSAGE_LOG("Accessing value: {} at index: {}, with adjustments", m_currentParameterValue[index] + adjustments[index], index); - return m_currentParameterValue[index] + adjustments[index]; + int index, const std::vector &adjustments) const { + MESSAGE_LOG("Accessing value: {} at index: {}, with adjustments", + m_currentParameterValue[index] + adjustments[index], index); + return m_currentParameterValue[index] + adjustments[index]; } std::shared_ptr UsgsAstroFrameSensorModel::getLogger() { diff --git a/src/UsgsAstroLsSensorModel.cpp b/src/UsgsAstroLsSensorModel.cpp index e3ac56c5d..530413bf2 100644 --- a/src/UsgsAstroLsSensorModel.cpp +++ b/src/UsgsAstroLsSensorModel.cpp @@ -2,7 +2,8 @@ // // UNCLASSIFIED // -// Copyright © 1989-2017 BAE Systems Information and Electronic Systems Integration Inc. +// Copyright © 1989-2017 BAE Systems Information and Electronic Systems +// Integration Inc. // ALL RIGHTS RESERVED // Use of this software product is governed by the terms of a license // agreement. The license agreement is found in the installation directory. @@ -17,507 +18,492 @@ // 24-OCT-2017 BAE Systems Update for CSM 3.0.3 //----------------------------------------------------------------------------- #include "UsgsAstroLsSensorModel.h" -#include "Utilities.h" #include "Distortion.h" +#include "Utilities.h" +#include +#include #include #include #include -#include -#include -#include #include #include +#include #include "ale/Util.h" -#define MESSAGE_LOG(...) if (m_logger) { m_logger->info(__VA_ARGS__); } +#define MESSAGE_LOG(...) \ + if (m_logger) { \ + m_logger->info(__VA_ARGS__); \ + } using json = nlohmann::json; using namespace std; -const std::string UsgsAstroLsSensorModel::_SENSOR_MODEL_NAME - = "USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL"; -const int UsgsAstroLsSensorModel::NUM_PARAMETERS = 16; -const std::string UsgsAstroLsSensorModel::PARAMETER_NAME[] = -{ - "IT Pos. Bias ", // 0 - "CT Pos. Bias ", // 1 - "Rad Pos. Bias ", // 2 - "IT Vel. Bias ", // 3 - "CT Vel. Bias ", // 4 - "Rad Vel. Bias ", // 5 - "Omega Bias ", // 6 - "Phi Bias ", // 7 - "Kappa Bias ", // 8 - "Omega Rate ", // 9 - "Phi Rate ", // 10 - "Kappa Rate ", // 11 - "Omega Accl ", // 12 - "Phi Accl ", // 13 - "Kappa Accl ", // 14 - "Focal Bias " // 15 +const std::string UsgsAstroLsSensorModel::_SENSOR_MODEL_NAME = + "USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL"; +const int UsgsAstroLsSensorModel::NUM_PARAMETERS = 16; +const std::string UsgsAstroLsSensorModel::PARAMETER_NAME[] = { + "IT Pos. Bias ", // 0 + "CT Pos. Bias ", // 1 + "Rad Pos. Bias ", // 2 + "IT Vel. Bias ", // 3 + "CT Vel. Bias ", // 4 + "Rad Vel. Bias ", // 5 + "Omega Bias ", // 6 + "Phi Bias ", // 7 + "Kappa Bias ", // 8 + "Omega Rate ", // 9 + "Phi Rate ", // 10 + "Kappa Rate ", // 11 + "Omega Accl ", // 12 + "Phi Accl ", // 13 + "Kappa Accl ", // 14 + "Focal Bias " // 15 }; - -const std::string UsgsAstroLsSensorModel::_STATE_KEYWORD[] = -{ - "m_modelName", - "m_imageIdentifier", - "m_sensorName", - "m_nLines", - "m_nSamples", - "m_platformFlag", - "m_intTimeLines", - "m_intTimeStartTimes", - "m_intTimes", - "m_startingEphemerisTime", - "m_centerEphemerisTime", - "m_detectorSampleSumming", - "m_detectorSampleSumming", - "m_startingDetectorSample", - "m_startingDetectorLine", - "m_ikCode", - "m_focalLength", - "m_zDirection", - "m_distortionType", - "m_opticalDistCoeffs", - "m_iTransS", - "m_iTransL", - "m_detectorSampleOrigin", - "m_detectorLineOrigin", - "m_majorAxis", - "m_minorAxis", - "m_platformIdentifier", - "m_sensorIdentifier", - "m_minElevation", - "m_maxElevation", - "m_dtEphem", - "m_t0Ephem", - "m_dtQuat", - "m_t0Quat", - "m_numPositions", - "m_numQuaternions", - "m_positions", - "m_velocities", - "m_quaternions", - "m_currentParameterValue", - "m_parameterType", - "m_referencePointXyz", - "m_sunPosition", - "m_sunVelocity", - "m_gsd", - "m_flyingHeight", - "m_halfSwath", - "m_halfTime", - "m_covariance", +const std::string UsgsAstroLsSensorModel::_STATE_KEYWORD[] = { + "m_modelName", + "m_imageIdentifier", + "m_sensorName", + "m_nLines", + "m_nSamples", + "m_platformFlag", + "m_intTimeLines", + "m_intTimeStartTimes", + "m_intTimes", + "m_startingEphemerisTime", + "m_centerEphemerisTime", + "m_detectorSampleSumming", + "m_detectorSampleSumming", + "m_startingDetectorSample", + "m_startingDetectorLine", + "m_ikCode", + "m_focalLength", + "m_zDirection", + "m_distortionType", + "m_opticalDistCoeffs", + "m_iTransS", + "m_iTransL", + "m_detectorSampleOrigin", + "m_detectorLineOrigin", + "m_majorAxis", + "m_minorAxis", + "m_platformIdentifier", + "m_sensorIdentifier", + "m_minElevation", + "m_maxElevation", + "m_dtEphem", + "m_t0Ephem", + "m_dtQuat", + "m_t0Quat", + "m_numPositions", + "m_numQuaternions", + "m_positions", + "m_velocities", + "m_quaternions", + "m_currentParameterValue", + "m_parameterType", + "m_referencePointXyz", + "m_sunPosition", + "m_sunVelocity", + "m_gsd", + "m_flyingHeight", + "m_halfSwath", + "m_halfTime", + "m_covariance", }; const int UsgsAstroLsSensorModel::NUM_PARAM_TYPES = 4; -const std::string UsgsAstroLsSensorModel::PARAM_STRING_ALL[] = -{ - "NONE", - "FICTITIOUS", - "REAL", - "FIXED" -}; -const csm::param::Type - UsgsAstroLsSensorModel::PARAM_CHAR_ALL[] = -{ - csm::param::NONE, - csm::param::FICTITIOUS, - csm::param::REAL, - csm::param::FIXED -}; +const std::string UsgsAstroLsSensorModel::PARAM_STRING_ALL[] = { + "NONE", "FICTITIOUS", "REAL", "FIXED"}; +const csm::param::Type UsgsAstroLsSensorModel::PARAM_CHAR_ALL[] = { + csm::param::NONE, csm::param::FICTITIOUS, csm::param::REAL, + csm::param::FIXED}; //*************************************************************************** // UsgsAstroLineScannerSensorModel::replaceModelState //*************************************************************************** -void UsgsAstroLsSensorModel::replaceModelState(const std::string& stateString) -{ - MESSAGE_LOG("Replacing model state") - - reset(); - auto j = json::parse(stateString); - int num_params = NUM_PARAMETERS; - int num_paramsSq = num_params * num_params; - - m_imageIdentifier = j["m_imageIdentifier"].get(); - m_sensorName = j["m_sensorName"]; - m_nLines = j["m_nLines"]; - m_nSamples = j["m_nSamples"]; - m_platformFlag = j["m_platformFlag"]; - MESSAGE_LOG("m_imageIdentifier: {} " - "m_sensorName: {} " - "m_nLines: {} " - "m_nSamples: {} " - "m_platformFlag: {} ", - j["m_imageIdentifier"].dump(), - j["m_sensorName"].dump(), - j["m_nLines"].dump(), - j["m_nSamples"].dump(), - j["m_platformFlag"].dump()) - - m_intTimeLines = j["m_intTimeLines"].get>(); - m_intTimeStartTimes = j["m_intTimeStartTimes"].get>(); - m_intTimes = j["m_intTimes"].get>(); - - m_startingEphemerisTime = j["m_startingEphemerisTime"]; - m_centerEphemerisTime = j["m_centerEphemerisTime"]; - m_detectorSampleSumming = j["m_detectorSampleSumming"]; - m_detectorLineSumming = j["m_detectorLineSumming"]; - m_startingDetectorSample = j["m_startingDetectorSample"]; - m_startingDetectorLine = j["m_startingDetectorLine"]; - m_ikCode = j["m_ikCode"]; - MESSAGE_LOG("m_startingEphemerisTime: {} " - "m_centerEphemerisTime: {} " - "m_detectorSampleSumming: {} " - "m_detectorLineSumming: {} " - "m_startingDetectorSample: {} " - "m_ikCode: {} ", - j["m_startingEphemerisTime"].dump(), - j["m_centerEphemerisTime"].dump(), - j["m_detectorSampleSumming"].dump(), - j["m_detectorLineSumming"].dump(), - j["m_startingDetectorSample"].dump(), j["m_ikCode"].dump()) - - m_focalLength = j["m_focalLength"]; - m_zDirection = j["m_zDirection"]; - m_distortionType = (DistortionType)j["m_distortionType"].get(); - m_opticalDistCoeffs = j["m_opticalDistCoeffs"].get>(); - MESSAGE_LOG("m_focalLength: {} " - "m_zDirection: {} " - "m_distortionType: {} ", - j["m_focalLength"].dump(), j["m_zDirection"].dump(), - j["m_distortionType"].dump()) - - for (int i = 0; i < 3; i++) { - m_iTransS[i] = j["m_iTransS"][i]; - m_iTransL[i] = j["m_iTransL"][i]; - } - - m_detectorSampleOrigin = j["m_detectorSampleOrigin"]; - m_detectorLineOrigin = j["m_detectorLineOrigin"]; - m_majorAxis = j["m_majorAxis"]; - m_minorAxis = j["m_minorAxis"]; - MESSAGE_LOG("m_detectorSampleOrigin: {} " - "m_detectorLineOrigin: {} " - "m_majorAxis: {} " - "m_minorAxis: {} ", - j["m_detectorSampleOrigin"].dump(), - j["m_detectorLineOrigin"].dump(), - j["m_majorAxis"].dump(), j["m_minorAxis"].dump()) - - m_platformIdentifier = j["m_platformIdentifier"]; - m_sensorIdentifier = j["m_sensorIdentifier"]; - MESSAGE_LOG("m_platformIdentifier: {} " - "m_sensorIdentifier: {} ", - j["m_platformIdentifier"].dump(), - j["m_sensorIdentifier"].dump()) - - m_minElevation = j["m_minElevation"]; - m_maxElevation = j["m_maxElevation"]; - MESSAGE_LOG("m_minElevation: {} " - "m_maxElevation: {} ", - j["m_minElevation"].dump(), - j["m_maxElevation"].dump()) - - m_dtEphem = j["m_dtEphem"]; - m_t0Ephem = j["m_t0Ephem"]; - m_dtQuat = j["m_dtQuat"]; - m_t0Quat = j["m_t0Quat"]; - m_numPositions = j["m_numPositions"]; - MESSAGE_LOG("m_dtEphem: {} " - "m_t0Ephem: {} " - "m_dtQuat: {} " - "m_t0Quat: {} ", - j["m_dtEphem"].dump(), j["m_t0Ephem"].dump(), - j["m_dtQuat"].dump(), j["m_t0Quat"].dump()) - - m_numQuaternions = j["m_numQuaternions"]; - m_referencePointXyz.x = j["m_referencePointXyz"][0]; - m_referencePointXyz.y = j["m_referencePointXyz"][1]; - m_referencePointXyz.z = j["m_referencePointXyz"][2]; - MESSAGE_LOG("m_numQuaternions: {} " - "m_referencePointX: {} " - "m_referencePointY: {} " - "m_referencePointZ: {} ", - j["m_numQuaternions"].dump(), j["m_referencePointXyz"][0].dump(), - j["m_referencePointXyz"][1].dump(), - j["m_referencePointXyz"][2].dump()) - - m_gsd = j["m_gsd"]; - m_flyingHeight = j["m_flyingHeight"]; - m_halfSwath = j["m_halfSwath"]; - m_halfTime = j["m_halfTime"]; - MESSAGE_LOG("m_gsd: {} " - "m_flyingHeight: {} " - "m_halfSwath: {} " - "m_halfTime: {} ", - j["m_gsd"].dump(), j["m_flyingHeight"].dump(), - j["m_halfSwath"].dump(), j["m_halfTime"].dump()) - // Vector = is overloaded so explicit get with type required. - - m_positions = j["m_positions"].get>(); - m_velocities = j["m_velocities"].get>(); - m_quaternions = j["m_quaternions"].get>(); - m_currentParameterValue = j["m_currentParameterValue"].get>(); - m_covariance = j["m_covariance"].get>(); - m_sunPosition = j["m_sunPosition"].get>(); - m_sunVelocity = j["m_sunVelocity"].get>(); - - for (int i = 0; i < num_params; i++) { - for (int k = 0; k < NUM_PARAM_TYPES; k++) { - if (j["m_parameterType"][i] == PARAM_STRING_ALL[k]) { - m_parameterType[i] = PARAM_CHAR_ALL[k]; - break; - } +void UsgsAstroLsSensorModel::replaceModelState(const std::string& stateString) { + MESSAGE_LOG("Replacing model state") + + reset(); + auto j = json::parse(stateString); + int num_params = NUM_PARAMETERS; + int num_paramsSq = num_params * num_params; + + m_imageIdentifier = j["m_imageIdentifier"].get(); + m_sensorName = j["m_sensorName"]; + m_nLines = j["m_nLines"]; + m_nSamples = j["m_nSamples"]; + m_platformFlag = j["m_platformFlag"]; + MESSAGE_LOG( + "m_imageIdentifier: {} " + "m_sensorName: {} " + "m_nLines: {} " + "m_nSamples: {} " + "m_platformFlag: {} ", + j["m_imageIdentifier"].dump(), j["m_sensorName"].dump(), + j["m_nLines"].dump(), j["m_nSamples"].dump(), j["m_platformFlag"].dump()) + + m_intTimeLines = j["m_intTimeLines"].get>(); + m_intTimeStartTimes = j["m_intTimeStartTimes"].get>(); + m_intTimes = j["m_intTimes"].get>(); + + m_startingEphemerisTime = j["m_startingEphemerisTime"]; + m_centerEphemerisTime = j["m_centerEphemerisTime"]; + m_detectorSampleSumming = j["m_detectorSampleSumming"]; + m_detectorLineSumming = j["m_detectorLineSumming"]; + m_startingDetectorSample = j["m_startingDetectorSample"]; + m_startingDetectorLine = j["m_startingDetectorLine"]; + m_ikCode = j["m_ikCode"]; + MESSAGE_LOG( + "m_startingEphemerisTime: {} " + "m_centerEphemerisTime: {} " + "m_detectorSampleSumming: {} " + "m_detectorLineSumming: {} " + "m_startingDetectorSample: {} " + "m_ikCode: {} ", + j["m_startingEphemerisTime"].dump(), j["m_centerEphemerisTime"].dump(), + j["m_detectorSampleSumming"].dump(), j["m_detectorLineSumming"].dump(), + j["m_startingDetectorSample"].dump(), j["m_ikCode"].dump()) + + m_focalLength = j["m_focalLength"]; + m_zDirection = j["m_zDirection"]; + m_distortionType = (DistortionType)j["m_distortionType"].get(); + m_opticalDistCoeffs = j["m_opticalDistCoeffs"].get>(); + MESSAGE_LOG( + "m_focalLength: {} " + "m_zDirection: {} " + "m_distortionType: {} ", + j["m_focalLength"].dump(), j["m_zDirection"].dump(), + j["m_distortionType"].dump()) + + for (int i = 0; i < 3; i++) { + m_iTransS[i] = j["m_iTransS"][i]; + m_iTransL[i] = j["m_iTransL"][i]; + } + + m_detectorSampleOrigin = j["m_detectorSampleOrigin"]; + m_detectorLineOrigin = j["m_detectorLineOrigin"]; + m_majorAxis = j["m_majorAxis"]; + m_minorAxis = j["m_minorAxis"]; + MESSAGE_LOG( + "m_detectorSampleOrigin: {} " + "m_detectorLineOrigin: {} " + "m_majorAxis: {} " + "m_minorAxis: {} ", + j["m_detectorSampleOrigin"].dump(), j["m_detectorLineOrigin"].dump(), + j["m_majorAxis"].dump(), j["m_minorAxis"].dump()) + + m_platformIdentifier = j["m_platformIdentifier"]; + m_sensorIdentifier = j["m_sensorIdentifier"]; + MESSAGE_LOG( + "m_platformIdentifier: {} " + "m_sensorIdentifier: {} ", + j["m_platformIdentifier"].dump(), j["m_sensorIdentifier"].dump()) + + m_minElevation = j["m_minElevation"]; + m_maxElevation = j["m_maxElevation"]; + MESSAGE_LOG( + "m_minElevation: {} " + "m_maxElevation: {} ", + j["m_minElevation"].dump(), j["m_maxElevation"].dump()) + + m_dtEphem = j["m_dtEphem"]; + m_t0Ephem = j["m_t0Ephem"]; + m_dtQuat = j["m_dtQuat"]; + m_t0Quat = j["m_t0Quat"]; + m_numPositions = j["m_numPositions"]; + MESSAGE_LOG( + "m_dtEphem: {} " + "m_t0Ephem: {} " + "m_dtQuat: {} " + "m_t0Quat: {} ", + j["m_dtEphem"].dump(), j["m_t0Ephem"].dump(), j["m_dtQuat"].dump(), + j["m_t0Quat"].dump()) + + m_numQuaternions = j["m_numQuaternions"]; + m_referencePointXyz.x = j["m_referencePointXyz"][0]; + m_referencePointXyz.y = j["m_referencePointXyz"][1]; + m_referencePointXyz.z = j["m_referencePointXyz"][2]; + MESSAGE_LOG( + "m_numQuaternions: {} " + "m_referencePointX: {} " + "m_referencePointY: {} " + "m_referencePointZ: {} ", + j["m_numQuaternions"].dump(), j["m_referencePointXyz"][0].dump(), + j["m_referencePointXyz"][1].dump(), j["m_referencePointXyz"][2].dump()) + + m_gsd = j["m_gsd"]; + m_flyingHeight = j["m_flyingHeight"]; + m_halfSwath = j["m_halfSwath"]; + m_halfTime = j["m_halfTime"]; + MESSAGE_LOG( + "m_gsd: {} " + "m_flyingHeight: {} " + "m_halfSwath: {} " + "m_halfTime: {} ", + j["m_gsd"].dump(), j["m_flyingHeight"].dump(), j["m_halfSwath"].dump(), + j["m_halfTime"].dump()) + // Vector = is overloaded so explicit get with type required. + + m_positions = j["m_positions"].get>(); + m_velocities = j["m_velocities"].get>(); + m_quaternions = j["m_quaternions"].get>(); + m_currentParameterValue = + j["m_currentParameterValue"].get>(); + m_covariance = j["m_covariance"].get>(); + m_sunPosition = j["m_sunPosition"].get>(); + m_sunVelocity = j["m_sunVelocity"].get>(); + + for (int i = 0; i < num_params; i++) { + for (int k = 0; k < NUM_PARAM_TYPES; k++) { + if (j["m_parameterType"][i] == PARAM_STRING_ALL[k]) { + m_parameterType[i] = PARAM_CHAR_ALL[k]; + break; + } } - } + } - // If computed state values are still default, then compute them - if (m_gsd == 1.0 && m_flyingHeight == 1000.0) - { - updateState(); - } + // If computed state values are still default, then compute them + if (m_gsd == 1.0 && m_flyingHeight == 1000.0) { + updateState(); + } - try - { - setLinearApproximation(); - } - catch (...) - { - _linear = false; - } + try { + setLinearApproximation(); + } catch (...) { + _linear = false; + } } //*************************************************************************** // UsgsAstroLineScannerSensorModel::getModelNameFromModelState //*************************************************************************** std::string UsgsAstroLsSensorModel::getModelNameFromModelState( - const std::string& model_state) -{ - // Parse the string to JSON - auto j = json::parse(model_state); - // If model name cannot be determined, return a blank string - std::string model_name; - - if (j.find("m_modelName") != j.end()) { - model_name = j["m_modelName"]; - } else { - csm::Error::ErrorType aErrorType = csm::Error::INVALID_SENSOR_MODEL_STATE; - std::string aMessage = "No 'm_modelName' key in the model state object."; - std::string aFunction = "UsgsAstroLsPlugin::getModelNameFromModelState"; - csm::Error csmErr(aErrorType, aMessage, aFunction); - throw(csmErr); - } - if (model_name != _SENSOR_MODEL_NAME){ - csm::Error::ErrorType aErrorType = csm::Error::SENSOR_MODEL_NOT_SUPPORTED; - std::string aMessage = "Sensor model not supported."; - std::string aFunction = "UsgsAstroLsPlugin::getModelNameFromModelState()"; - csm::Error csmErr(aErrorType, aMessage, aFunction); - throw(csmErr); - } - return model_name; + const std::string& model_state) { + // Parse the string to JSON + auto j = json::parse(model_state); + // If model name cannot be determined, return a blank string + std::string model_name; + + if (j.find("m_modelName") != j.end()) { + model_name = j["m_modelName"]; + } else { + csm::Error::ErrorType aErrorType = csm::Error::INVALID_SENSOR_MODEL_STATE; + std::string aMessage = "No 'm_modelName' key in the model state object."; + std::string aFunction = "UsgsAstroLsPlugin::getModelNameFromModelState"; + csm::Error csmErr(aErrorType, aMessage, aFunction); + throw(csmErr); + } + if (model_name != _SENSOR_MODEL_NAME) { + csm::Error::ErrorType aErrorType = csm::Error::SENSOR_MODEL_NOT_SUPPORTED; + std::string aMessage = "Sensor model not supported."; + std::string aFunction = "UsgsAstroLsPlugin::getModelNameFromModelState()"; + csm::Error csmErr(aErrorType, aMessage, aFunction); + throw(csmErr); + } + return model_name; } //*************************************************************************** // UsgsAstroLineScannerSensorModel::getModelState //*************************************************************************** std::string UsgsAstroLsSensorModel::getModelState() const { - MESSAGE_LOG("Running getModelState") - - json state; - state["m_modelName"] = _SENSOR_MODEL_NAME; - state["m_startingDetectorSample"] = m_startingDetectorSample; - state["m_startingDetectorLine"] = m_startingDetectorLine; - state["m_imageIdentifier"] = m_imageIdentifier; - state["m_sensorName"] = m_sensorName; - state["m_nLines"] = m_nLines; - state["m_nSamples"] = m_nSamples; - state["m_platformFlag"] = m_platformFlag; - MESSAGE_LOG("m_imageIdentifier: {} " - "m_sensorName: {} " - "m_nLines: {} " - "m_nSamples: {} " - "m_platformFlag: {} ", - m_imageIdentifier, m_sensorName, - m_nLines, m_nSamples, m_platformFlag) - - state["m_intTimeLines"] = m_intTimeLines; - state["m_intTimeStartTimes"] = m_intTimeStartTimes; - state["m_intTimes"] = m_intTimes; - state["m_startingEphemerisTime"] = m_startingEphemerisTime; - state["m_centerEphemerisTime"] = m_centerEphemerisTime; - MESSAGE_LOG("m_startingEphemerisTime: {} " - "m_centerEphemerisTime: {} ", - m_startingEphemerisTime, - m_centerEphemerisTime) - - state["m_detectorSampleSumming"] = m_detectorSampleSumming; - state["m_detectorLineSumming"] = m_detectorLineSumming; - state["m_startingDetectorSample"] = m_startingDetectorSample; - state["m_ikCode"] = m_ikCode; - MESSAGE_LOG("m_detectorSampleSumming: {} " - "m_detectorLineSumming: {} " - "m_startingDetectorSample: {} " - "m_ikCode: {} ", - m_detectorSampleSumming, m_detectorLineSumming, - m_startingDetectorSample, - m_ikCode) - - state["m_focalLength"] = m_focalLength; - state["m_zDirection"] = m_zDirection; - state["m_distortionType"] = m_distortionType; - state["m_opticalDistCoeffs"] = m_opticalDistCoeffs; - MESSAGE_LOG("m_focalLength: {} " - "m_zDirection: {} " - "m_distortionType (0-Radial, 1-Transverse): {} ", - m_focalLength, m_zDirection, - m_distortionType) - - state["m_iTransS"] = std::vector(m_iTransS, m_iTransS+3); - state["m_iTransL"] = std::vector(m_iTransL, m_iTransL+3); - - state["m_detectorSampleOrigin"] = m_detectorSampleOrigin; - state["m_detectorLineOrigin"] = m_detectorLineOrigin; - state["m_majorAxis"] = m_majorAxis; - state["m_minorAxis"] = m_minorAxis; - MESSAGE_LOG("m_detectorSampleOrigin: {} " - "m_detectorLineOrigin: {} " - "m_majorAxis: {} " - "m_minorAxis: {} ", - m_detectorSampleOrigin, m_detectorLineOrigin, - m_majorAxis, m_minorAxis) - - state["m_platformIdentifier"] = m_platformIdentifier; - state["m_sensorIdentifier"] = m_sensorIdentifier; - state["m_minElevation"] = m_minElevation; - state["m_maxElevation"] = m_maxElevation; - MESSAGE_LOG("m_platformIdentifier: {} " - "m_sensorIdentifier: {} " - "m_minElevation: {} " - "m_maxElevation: {} ", - m_platformIdentifier, m_sensorIdentifier, - m_minElevation, m_maxElevation) - - state["m_dtEphem"] = m_dtEphem; - state["m_t0Ephem"] = m_t0Ephem; - state["m_dtQuat"] = m_dtQuat; - state["m_t0Quat"] = m_t0Quat; - MESSAGE_LOG("m_dtEphem: {} " - "m_t0Ephem: {} " - "m_dtQuat: {} " - "m_t0Quat: {} ", - m_dtEphem, m_t0Ephem, - m_dtQuat, m_t0Quat) - - state["m_numPositions"] = m_numPositions; - state["m_numQuaternions"] = m_numQuaternions; - state["m_positions"] = m_positions; - state["m_velocities"] = m_velocities; - state["m_quaternions"] = m_quaternions; - MESSAGE_LOG("m_numPositions: {} " - "m_numQuaternions: {} ", - m_numPositions, m_numQuaternions) - - state["m_currentParameterValue"] = m_currentParameterValue; - state["m_parameterType"] = m_parameterType; - - state["m_gsd"] = m_gsd; - state["m_flyingHeight"] = m_flyingHeight; - state["m_halfSwath"] = m_halfSwath; - state["m_halfTime"] = m_halfTime; - state["m_covariance"] = m_covariance; - MESSAGE_LOG("m_gsd: {} " - "m_flyingHeight: {} " - "m_halfSwath: {} " - "m_halfTime: {} ", - m_gsd, m_flyingHeight, - m_halfSwath, m_halfTime) - - state["m_referencePointXyz"] = json(); - state["m_referencePointXyz"][0] = m_referencePointXyz.x; - state["m_referencePointXyz"][1] = m_referencePointXyz.y; - state["m_referencePointXyz"][2] = m_referencePointXyz.z; - MESSAGE_LOG("m_referencePointXyz: {} " - "m_referencePointXyz: {} " - "m_referencePointXyz: {} ", - m_referencePointXyz.x, m_referencePointXyz.y, - m_referencePointXyz.z) - - state["m_sunPosition"] = m_sunPosition; - MESSAGE_LOG("num sun positions: {} ", m_sunPosition.size()) - - state["m_sunVelocity"] = m_sunVelocity; - MESSAGE_LOG("num sun velocities: {} ", m_sunVelocity.size()) - - return state.dump(); - } - - //*************************************************************************** - // UsgsAstroLineScannerSensorModel::reset - //*************************************************************************** -void UsgsAstroLsSensorModel::reset() -{ + MESSAGE_LOG("Running getModelState") + + json state; + state["m_modelName"] = _SENSOR_MODEL_NAME; + state["m_startingDetectorSample"] = m_startingDetectorSample; + state["m_startingDetectorLine"] = m_startingDetectorLine; + state["m_imageIdentifier"] = m_imageIdentifier; + state["m_sensorName"] = m_sensorName; + state["m_nLines"] = m_nLines; + state["m_nSamples"] = m_nSamples; + state["m_platformFlag"] = m_platformFlag; + MESSAGE_LOG( + "m_imageIdentifier: {} " + "m_sensorName: {} " + "m_nLines: {} " + "m_nSamples: {} " + "m_platformFlag: {} ", + m_imageIdentifier, m_sensorName, m_nLines, m_nSamples, m_platformFlag) + + state["m_intTimeLines"] = m_intTimeLines; + state["m_intTimeStartTimes"] = m_intTimeStartTimes; + state["m_intTimes"] = m_intTimes; + state["m_startingEphemerisTime"] = m_startingEphemerisTime; + state["m_centerEphemerisTime"] = m_centerEphemerisTime; + MESSAGE_LOG( + "m_startingEphemerisTime: {} " + "m_centerEphemerisTime: {} ", + m_startingEphemerisTime, m_centerEphemerisTime) + + state["m_detectorSampleSumming"] = m_detectorSampleSumming; + state["m_detectorLineSumming"] = m_detectorLineSumming; + state["m_startingDetectorSample"] = m_startingDetectorSample; + state["m_ikCode"] = m_ikCode; + MESSAGE_LOG( + "m_detectorSampleSumming: {} " + "m_detectorLineSumming: {} " + "m_startingDetectorSample: {} " + "m_ikCode: {} ", + m_detectorSampleSumming, m_detectorLineSumming, m_startingDetectorSample, + m_ikCode) + + state["m_focalLength"] = m_focalLength; + state["m_zDirection"] = m_zDirection; + state["m_distortionType"] = m_distortionType; + state["m_opticalDistCoeffs"] = m_opticalDistCoeffs; + MESSAGE_LOG( + "m_focalLength: {} " + "m_zDirection: {} " + "m_distortionType (0-Radial, 1-Transverse): {} ", + m_focalLength, m_zDirection, m_distortionType) + + state["m_iTransS"] = std::vector(m_iTransS, m_iTransS + 3); + state["m_iTransL"] = std::vector(m_iTransL, m_iTransL + 3); + + state["m_detectorSampleOrigin"] = m_detectorSampleOrigin; + state["m_detectorLineOrigin"] = m_detectorLineOrigin; + state["m_majorAxis"] = m_majorAxis; + state["m_minorAxis"] = m_minorAxis; + MESSAGE_LOG( + "m_detectorSampleOrigin: {} " + "m_detectorLineOrigin: {} " + "m_majorAxis: {} " + "m_minorAxis: {} ", + m_detectorSampleOrigin, m_detectorLineOrigin, m_majorAxis, m_minorAxis) + + state["m_platformIdentifier"] = m_platformIdentifier; + state["m_sensorIdentifier"] = m_sensorIdentifier; + state["m_minElevation"] = m_minElevation; + state["m_maxElevation"] = m_maxElevation; + MESSAGE_LOG( + "m_platformIdentifier: {} " + "m_sensorIdentifier: {} " + "m_minElevation: {} " + "m_maxElevation: {} ", + m_platformIdentifier, m_sensorIdentifier, m_minElevation, m_maxElevation) + + state["m_dtEphem"] = m_dtEphem; + state["m_t0Ephem"] = m_t0Ephem; + state["m_dtQuat"] = m_dtQuat; + state["m_t0Quat"] = m_t0Quat; + MESSAGE_LOG( + "m_dtEphem: {} " + "m_t0Ephem: {} " + "m_dtQuat: {} " + "m_t0Quat: {} ", + m_dtEphem, m_t0Ephem, m_dtQuat, m_t0Quat) + + state["m_numPositions"] = m_numPositions; + state["m_numQuaternions"] = m_numQuaternions; + state["m_positions"] = m_positions; + state["m_velocities"] = m_velocities; + state["m_quaternions"] = m_quaternions; + MESSAGE_LOG( + "m_numPositions: {} " + "m_numQuaternions: {} ", + m_numPositions, m_numQuaternions) + + state["m_currentParameterValue"] = m_currentParameterValue; + state["m_parameterType"] = m_parameterType; + + state["m_gsd"] = m_gsd; + state["m_flyingHeight"] = m_flyingHeight; + state["m_halfSwath"] = m_halfSwath; + state["m_halfTime"] = m_halfTime; + state["m_covariance"] = m_covariance; + MESSAGE_LOG( + "m_gsd: {} " + "m_flyingHeight: {} " + "m_halfSwath: {} " + "m_halfTime: {} ", + m_gsd, m_flyingHeight, m_halfSwath, m_halfTime) + + state["m_referencePointXyz"] = json(); + state["m_referencePointXyz"][0] = m_referencePointXyz.x; + state["m_referencePointXyz"][1] = m_referencePointXyz.y; + state["m_referencePointXyz"][2] = m_referencePointXyz.z; + MESSAGE_LOG( + "m_referencePointXyz: {} " + "m_referencePointXyz: {} " + "m_referencePointXyz: {} ", + m_referencePointXyz.x, m_referencePointXyz.y, m_referencePointXyz.z) + + state["m_sunPosition"] = m_sunPosition; + MESSAGE_LOG("num sun positions: {} ", m_sunPosition.size()) + + state["m_sunVelocity"] = m_sunVelocity; + MESSAGE_LOG("num sun velocities: {} ", m_sunVelocity.size()) + + return state.dump(); +} + +//*************************************************************************** +// UsgsAstroLineScannerSensorModel::reset +//*************************************************************************** +void UsgsAstroLsSensorModel::reset() { MESSAGE_LOG("Running reset()") - _linear = false; // default until a linear model is made - _u0 = 0.0; + _linear = false; // default until a linear model is made + _u0 = 0.0; _du_dx = 0.0; _du_dy = 0.0; _du_dz = 0.0; - _v0 = 0.0; + _v0 = 0.0; _dv_dx = 0.0; _dv_dy = 0.0; _dv_dz = 0.0; _no_adjustment.assign(UsgsAstroLsSensorModel::NUM_PARAMETERS, 0.0); - m_imageIdentifier = ""; // 1 - m_sensorName = "USGSAstroLineScanner"; // 2 - m_nLines = 0; // 3 - m_nSamples = 0; // 4 - m_platformFlag = 1; // 9 + m_imageIdentifier = ""; // 1 + m_sensorName = "USGSAstroLineScanner"; // 2 + m_nLines = 0; // 3 + m_nSamples = 0; // 4 + m_platformFlag = 1; // 9 m_intTimeLines.clear(); m_intTimeStartTimes.clear(); m_intTimes.clear(); - m_startingEphemerisTime = 0.0; // 13 - m_centerEphemerisTime = 0.0; // 14 - m_detectorSampleSumming = 1.0; // 15 + m_startingEphemerisTime = 0.0; // 13 + m_centerEphemerisTime = 0.0; // 14 + m_detectorSampleSumming = 1.0; // 15 m_detectorLineSumming = 1.0; - m_startingDetectorSample = 1.0; // 16 - m_ikCode = -85600; // 17 - m_focalLength = 350.0; // 18 - m_zDirection = 1.0; // 19 + m_startingDetectorSample = 1.0; // 16 + m_ikCode = -85600; // 17 + m_focalLength = 350.0; // 18 + m_zDirection = 1.0; // 19 m_distortionType = DistortionType::RADIAL; m_opticalDistCoeffs = std::vector(3, 0.0); - m_iTransS[0] = 0.0; // 21 - m_iTransS[1] = 0.0; // 21 - m_iTransS[2] = 150.0; // 21 - m_iTransL[0] = 0.0; // 22 - m_iTransL[1] = 150.0; // 22 - m_iTransL[2] = 0.0; // 22 - m_detectorSampleOrigin = 2500.0; // 23 - m_detectorLineOrigin = 0.0; // 24 - m_majorAxis = 3400000.0; // 27 - m_minorAxis = 3350000.0; // 28 - m_platformIdentifier = ""; // 31 - m_sensorIdentifier = ""; // 32 - m_minElevation = -8000.0; // 34 - m_maxElevation = 8000.0; // 35 - m_dtEphem = 2.0; // 36 - m_t0Ephem = -70.0; // 37 - m_dtQuat = 0.1; // 38 - m_t0Quat = -40.0; // 39 - m_numPositions = 0; // 40 - m_numQuaternions = 0; // 41 - m_positions.clear(); // 42 - m_velocities.clear(); // 43 - m_quaternions.clear(); // 44 - - m_currentParameterValue.assign(NUM_PARAMETERS,0.0); - m_parameterType.assign(NUM_PARAMETERS,csm::param::REAL); + m_iTransS[0] = 0.0; // 21 + m_iTransS[1] = 0.0; // 21 + m_iTransS[2] = 150.0; // 21 + m_iTransL[0] = 0.0; // 22 + m_iTransL[1] = 150.0; // 22 + m_iTransL[2] = 0.0; // 22 + m_detectorSampleOrigin = 2500.0; // 23 + m_detectorLineOrigin = 0.0; // 24 + m_majorAxis = 3400000.0; // 27 + m_minorAxis = 3350000.0; // 28 + m_platformIdentifier = ""; // 31 + m_sensorIdentifier = ""; // 32 + m_minElevation = -8000.0; // 34 + m_maxElevation = 8000.0; // 35 + m_dtEphem = 2.0; // 36 + m_t0Ephem = -70.0; // 37 + m_dtQuat = 0.1; // 38 + m_t0Quat = -40.0; // 39 + m_numPositions = 0; // 40 + m_numQuaternions = 0; // 41 + m_positions.clear(); // 42 + m_velocities.clear(); // 43 + m_quaternions.clear(); // 44 + + m_currentParameterValue.assign(NUM_PARAMETERS, 0.0); + m_parameterType.assign(NUM_PARAMETERS, csm::param::REAL); m_referencePointXyz.x = 0.0; m_referencePointXyz.y = 0.0; @@ -531,115 +517,110 @@ void UsgsAstroLsSensorModel::reset() m_halfSwath = 1000.0; m_halfTime = 10.0; - m_covariance = std::vector(NUM_PARAMETERS * NUM_PARAMETERS,0.0); // 52 - + m_covariance = + std::vector(NUM_PARAMETERS * NUM_PARAMETERS, 0.0); // 52 } //***************************************************************************** // UsgsAstroLsSensorModel Constructor //***************************************************************************** -UsgsAstroLsSensorModel::UsgsAstroLsSensorModel() -{ - _no_adjustment.assign(UsgsAstroLsSensorModel::NUM_PARAMETERS, 0.0); +UsgsAstroLsSensorModel::UsgsAstroLsSensorModel() { + _no_adjustment.assign(UsgsAstroLsSensorModel::NUM_PARAMETERS, 0.0); } - //***************************************************************************** // UsgsAstroLsSensorModel Destructor //***************************************************************************** -UsgsAstroLsSensorModel::~UsgsAstroLsSensorModel() -{ -} +UsgsAstroLsSensorModel::~UsgsAstroLsSensorModel() {} //***************************************************************************** // UsgsAstroLsSensorModel updateState //***************************************************************************** -void UsgsAstroLsSensorModel::updateState() -{ - // If sensor model is being created for the first time - // This routine will set some parameters not found in the ISD. - MESSAGE_LOG("Updating State") - // Reference point (image center) - double lineCtr = m_nLines / 2.0; - double sampCtr = m_nSamples / 2.0; - csm::ImageCoord ip(lineCtr, sampCtr); - MESSAGE_LOG("updateState: center image coordinate set to {} {}", - lineCtr, sampCtr) - - double refHeight = 0; - m_referencePointXyz = imageToGround(ip, refHeight); - MESSAGE_LOG("updateState: reference point (x, y, z) {} {} {}", - m_referencePointXyz.x, m_referencePointXyz.y, - m_referencePointXyz.z) - - // Compute ground sample distance - ip.line += 1; - ip.samp += 1; - csm::EcefCoord delta = imageToGround(ip, refHeight); - double dx = delta.x - m_referencePointXyz.x; - double dy = delta.y - m_referencePointXyz.y; - double dz = delta.z - m_referencePointXyz.z; - m_gsd = sqrt((dx*dx + dy*dy + dz*dz) / 2.0); - MESSAGE_LOG("updateState: ground sample distance set to {} " - "based on dx {} dy {} dz {}", - m_gsd, dx, dy, dz) - - // Compute flying height - csm::EcefCoord sensorPos = getSensorPosition(0.0); - dx = sensorPos.x - m_referencePointXyz.x; - dy = sensorPos.y - m_referencePointXyz.y; - dz = sensorPos.z - m_referencePointXyz.z; - m_flyingHeight = sqrt(dx*dx + dy*dy + dz*dz); - MESSAGE_LOG("updateState: flight height set to {}" - "based on dx {} dy {} dz {}", - m_flyingHeight, dx, dy, dz) - - // Compute half swath - m_halfSwath = m_gsd * m_nSamples / 2.0; - MESSAGE_LOG("updateState: half swath set to {}", - m_halfSwath) - - // Compute half time duration - double fullImageTime = m_intTimeStartTimes.back() - m_intTimeStartTimes.front() - + m_intTimes.back() * (m_nLines - m_intTimeLines.back()); - m_halfTime = fullImageTime / 2.0; - MESSAGE_LOG("updateState: half time duration set to {}", - m_halfTime) - - // Parameter covariance, hardcoded accuracy values - // hardcoded ~1 pixel accuracy values - int num_params = NUM_PARAMETERS; - double positionVariance = m_gsd * m_gsd; - // parameter time is scaled to [0, 2] - // so divide by 2 for velocities and 4 for accelerations - double velocityVariance = positionVariance / 2.0; - double accelerationVariance = positionVariance / 4.0; - m_covariance.assign(num_params * num_params, 0.0); - - // Set position variances - m_covariance[0] = positionVariance; - m_covariance[num_params + 1] = positionVariance; - m_covariance[2 * num_params + 2] = positionVariance; - m_covariance[3 * num_params + 3] = velocityVariance; - m_covariance[4 * num_params + 4] = velocityVariance; - m_covariance[5 * num_params + 5] = velocityVariance; - - // Set orientation variances - m_covariance[6 * num_params + 6] = positionVariance; - m_covariance[7 * num_params + 7] = positionVariance; - m_covariance[8 * num_params + 8] = positionVariance; - m_covariance[9 * num_params + 9] = velocityVariance; - m_covariance[10 * num_params + 10] = velocityVariance; - m_covariance[11 * num_params + 11] = velocityVariance; - m_covariance[12 * num_params + 12] = accelerationVariance; - m_covariance[13 * num_params + 13] = accelerationVariance; - m_covariance[14 * num_params + 14] = accelerationVariance; - - // Set focal length variance - m_covariance[15 * num_params + 15] = positionVariance; +void UsgsAstroLsSensorModel::updateState() { + // If sensor model is being created for the first time + // This routine will set some parameters not found in the ISD. + MESSAGE_LOG("Updating State") + // Reference point (image center) + double lineCtr = m_nLines / 2.0; + double sampCtr = m_nSamples / 2.0; + csm::ImageCoord ip(lineCtr, sampCtr); + MESSAGE_LOG("updateState: center image coordinate set to {} {}", lineCtr, + sampCtr) + + double refHeight = 0; + m_referencePointXyz = imageToGround(ip, refHeight); + MESSAGE_LOG("updateState: reference point (x, y, z) {} {} {}", + m_referencePointXyz.x, m_referencePointXyz.y, + m_referencePointXyz.z) + + // Compute ground sample distance + ip.line += 1; + ip.samp += 1; + csm::EcefCoord delta = imageToGround(ip, refHeight); + double dx = delta.x - m_referencePointXyz.x; + double dy = delta.y - m_referencePointXyz.y; + double dz = delta.z - m_referencePointXyz.z; + m_gsd = sqrt((dx * dx + dy * dy + dz * dz) / 2.0); + MESSAGE_LOG( + "updateState: ground sample distance set to {} " + "based on dx {} dy {} dz {}", + m_gsd, dx, dy, dz) + + // Compute flying height + csm::EcefCoord sensorPos = getSensorPosition(0.0); + dx = sensorPos.x - m_referencePointXyz.x; + dy = sensorPos.y - m_referencePointXyz.y; + dz = sensorPos.z - m_referencePointXyz.z; + m_flyingHeight = sqrt(dx * dx + dy * dy + dz * dz); + MESSAGE_LOG( + "updateState: flight height set to {}" + "based on dx {} dy {} dz {}", + m_flyingHeight, dx, dy, dz) + + // Compute half swath + m_halfSwath = m_gsd * m_nSamples / 2.0; + MESSAGE_LOG("updateState: half swath set to {}", m_halfSwath) + + // Compute half time duration + double fullImageTime = m_intTimeStartTimes.back() - + m_intTimeStartTimes.front() + + m_intTimes.back() * (m_nLines - m_intTimeLines.back()); + m_halfTime = fullImageTime / 2.0; + MESSAGE_LOG("updateState: half time duration set to {}", m_halfTime) + + // Parameter covariance, hardcoded accuracy values + // hardcoded ~1 pixel accuracy values + int num_params = NUM_PARAMETERS; + double positionVariance = m_gsd * m_gsd; + // parameter time is scaled to [0, 2] + // so divide by 2 for velocities and 4 for accelerations + double velocityVariance = positionVariance / 2.0; + double accelerationVariance = positionVariance / 4.0; + m_covariance.assign(num_params * num_params, 0.0); + + // Set position variances + m_covariance[0] = positionVariance; + m_covariance[num_params + 1] = positionVariance; + m_covariance[2 * num_params + 2] = positionVariance; + m_covariance[3 * num_params + 3] = velocityVariance; + m_covariance[4 * num_params + 4] = velocityVariance; + m_covariance[5 * num_params + 5] = velocityVariance; + + // Set orientation variances + m_covariance[6 * num_params + 6] = positionVariance; + m_covariance[7 * num_params + 7] = positionVariance; + m_covariance[8 * num_params + 8] = positionVariance; + m_covariance[9 * num_params + 9] = velocityVariance; + m_covariance[10 * num_params + 10] = velocityVariance; + m_covariance[11 * num_params + 11] = velocityVariance; + m_covariance[12 * num_params + 12] = accelerationVariance; + m_covariance[13 * num_params + 13] = accelerationVariance; + m_covariance[14 * num_params + 14] = accelerationVariance; + + // Set focal length variance + m_covariance[15 * num_params + 15] = positionVariance; } - //--------------------------------------------------------------------------- // Core Photogrammetry //--------------------------------------------------------------------------- @@ -648,429 +629,393 @@ void UsgsAstroLsSensorModel::updateState() // UsgsAstroLsSensorModel::groundToImage //*************************************************************************** csm::ImageCoord UsgsAstroLsSensorModel::groundToImage( - const csm::EcefCoord& ground_pt, - double desired_precision, - double* achieved_precision, - csm::WarningList* warnings) const -{ - - MESSAGE_LOG("Computing groundToImage(No adjustments) for {}, {}, {}, with desired precision {}", - ground_pt.x, ground_pt.y, ground_pt.z, desired_precision); + const csm::EcefCoord& ground_pt, double desired_precision, + double* achieved_precision, csm::WarningList* warnings) const { + MESSAGE_LOG( + "Computing groundToImage(No adjustments) for {}, {}, {}, with desired " + "precision {}", + ground_pt.x, ground_pt.y, ground_pt.z, desired_precision); - // The public interface invokes the private interface with no adjustments. - return groundToImage( - ground_pt, _no_adjustment, - desired_precision, achieved_precision, warnings); + // The public interface invokes the private interface with no adjustments. + return groundToImage(ground_pt, _no_adjustment, desired_precision, + achieved_precision, warnings); } //*************************************************************************** // UsgsAstroLsSensorModel::groundToImage (internal version) //*************************************************************************** csm::ImageCoord UsgsAstroLsSensorModel::groundToImage( - const csm::EcefCoord& groundPt, - const std::vector& 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 detectorView; - double detectorLine = m_nLines; - double detectorSample = 0; - double count = 0; - double timei; - - while(abs(detectorLine) > desiredPrecision && ++count < 15) { - timei = getImageTime(approxPt); - detectorView = computeDetectorView(timei, groundPt, adj); - - // 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; - - // Convert to image line - approxPt.line += detectorLine; - } - - timei = getImageTime(approxPt); - detectorView = computeDetectorView(timei, groundPt, adj); - - // Invert distortion - double distortedFocalX, distortedFocalY; - applyDistortion(detectorView[0], detectorView[1], distortedFocalX, distortedFocalY, - m_opticalDistCoeffs, m_distortionType, desiredPrecision); - - // Convert to detector line and sample - detectorLine = m_iTransL[0] - + m_iTransL[1] * distortedFocalX - + m_iTransL[2] * distortedFocalY; - detectorSample = m_iTransS[0] - + m_iTransS[1] * distortedFocalX - + m_iTransS[2] * distortedFocalY; - // Convert to image sample line - double finalUpdate = (detectorLine + m_detectorLineOrigin - m_startingDetectorLine) - / m_detectorLineSumming; - approxPt.line += finalUpdate; - approxPt.samp = (detectorSample + m_detectorSampleOrigin - m_startingDetectorSample) - / m_detectorSampleSumming; - - double precision = detectorLine + m_detectorLineOrigin - m_startingDetectorLine; - if (achievedPrecision) { - *achievedPrecision = finalUpdate; - } - - MESSAGE_LOG("groundToImage: image line sample {} {}", - approxPt.line, approxPt.samp) - - if (warnings && (desiredPrecision > 0.0) && (abs(finalUpdate) > desiredPrecision)) - { - warnings->push_back( - csm::Warning( - csm::Warning::PRECISION_NOT_MET, - "Desired precision not achieved.", - "UsgsAstroLsSensorModel::groundToImage()")); - } - - return approxPt; + const csm::EcefCoord& groundPt, const std::vector& 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 detectorView; + double detectorLine = m_nLines; + double detectorSample = 0; + double count = 0; + double timei; + + while (abs(detectorLine) > desiredPrecision && ++count < 15) { + timei = getImageTime(approxPt); + detectorView = computeDetectorView(timei, groundPt, adj); + + // 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; + + // Convert to image line + approxPt.line += detectorLine; + } + + timei = getImageTime(approxPt); + detectorView = computeDetectorView(timei, groundPt, adj); + + // Invert distortion + double distortedFocalX, distortedFocalY; + applyDistortion(detectorView[0], detectorView[1], distortedFocalX, + distortedFocalY, m_opticalDistCoeffs, m_distortionType, + desiredPrecision); + + // Convert to detector line and sample + detectorLine = m_iTransL[0] + m_iTransL[1] * distortedFocalX + + m_iTransL[2] * distortedFocalY; + detectorSample = m_iTransS[0] + m_iTransS[1] * distortedFocalX + + m_iTransS[2] * distortedFocalY; + // Convert to image sample line + double finalUpdate = + (detectorLine + m_detectorLineOrigin - m_startingDetectorLine) / + m_detectorLineSumming; + approxPt.line += finalUpdate; + approxPt.samp = + (detectorSample + m_detectorSampleOrigin - m_startingDetectorSample) / + m_detectorSampleSumming; + + double precision = + detectorLine + m_detectorLineOrigin - m_startingDetectorLine; + if (achievedPrecision) { + *achievedPrecision = finalUpdate; + } + + MESSAGE_LOG("groundToImage: image line sample {} {}", approxPt.line, + approxPt.samp) + + if (warnings && (desiredPrecision > 0.0) && + (abs(finalUpdate) > desiredPrecision)) { + warnings->push_back(csm::Warning( + csm::Warning::PRECISION_NOT_MET, "Desired precision not achieved.", + "UsgsAstroLsSensorModel::groundToImage()")); + } + + return approxPt; } //*************************************************************************** // UsgsAstroLsSensorModel::groundToImage //*************************************************************************** csm::ImageCoordCovar UsgsAstroLsSensorModel::groundToImage( - const csm::EcefCoordCovar& groundPt, - double desired_precision, - double* achieved_precision, - csm::WarningList* warnings) const -{ - MESSAGE_LOG("Computing groundToImage(Covar) for {}, {}, {}, with desired precision {}", - groundPt.x, groundPt.y, groundPt.z, desired_precision); - // Ground to image with error propagation - // Compute corresponding image point - csm::EcefCoord gp; - gp.x = groundPt.x; - gp.y = groundPt.y; - gp.z = groundPt.z; - - csm::ImageCoord ip = groundToImage( - gp, desired_precision, achieved_precision, warnings); - csm::ImageCoordCovar result(ip.line, ip.samp); - - // Compute partials ls wrt XYZ - std::vector prt(6, 0.0); - prt = computeGroundPartials(groundPt); - - // Error propagation - double ltx, lty, ltz; - double stx, sty, stz; - ltx = - prt[0] * groundPt.covariance[0] + - prt[1] * groundPt.covariance[3] + - prt[2] * groundPt.covariance[6]; - lty = - prt[0] * groundPt.covariance[1] + - prt[1] * groundPt.covariance[4] + - prt[2] * groundPt.covariance[7]; - ltz = - prt[0] * groundPt.covariance[2] + - prt[1] * groundPt.covariance[5] + - prt[2] * groundPt.covariance[8]; - stx = - prt[3] * groundPt.covariance[0] + - prt[4] * groundPt.covariance[3] + - prt[5] * groundPt.covariance[6]; - sty = - prt[3] * groundPt.covariance[1] + - prt[4] * groundPt.covariance[4] + - prt[5] * groundPt.covariance[7]; - stz = - prt[3] * groundPt.covariance[2] + - prt[4] * groundPt.covariance[5] + - prt[5] * groundPt.covariance[8]; - - double gp_cov[4]; // Input gp cov in image space - gp_cov[0] = ltx * prt[0] + lty * prt[1] + ltz * prt[2]; - gp_cov[1] = ltx * prt[3] + lty * prt[4] + ltz * prt[5]; - gp_cov[2] = stx * prt[0] + sty * prt[1] + stz * prt[2]; - gp_cov[3] = stx * prt[3] + sty * prt[4] + stz * prt[5]; - - std::vector unmodeled_cov = getUnmodeledError(ip); - double sensor_cov[4]; // sensor cov in image space - determineSensorCovarianceInImageSpace(gp, sensor_cov); - - result.covariance[0] = gp_cov[0] + unmodeled_cov[0] + sensor_cov[0]; - result.covariance[1] = gp_cov[1] + unmodeled_cov[1] + sensor_cov[1]; - result.covariance[2] = gp_cov[2] + unmodeled_cov[2] + sensor_cov[2]; - result.covariance[3] = gp_cov[3] + unmodeled_cov[3] + sensor_cov[3]; - - return result; + const csm::EcefCoordCovar& groundPt, double desired_precision, + double* achieved_precision, csm::WarningList* warnings) const { + MESSAGE_LOG( + "Computing groundToImage(Covar) for {}, {}, {}, with desired precision " + "{}", + groundPt.x, groundPt.y, groundPt.z, desired_precision); + // Ground to image with error propagation + // Compute corresponding image point + csm::EcefCoord gp; + gp.x = groundPt.x; + gp.y = groundPt.y; + gp.z = groundPt.z; + + csm::ImageCoord ip = + groundToImage(gp, desired_precision, achieved_precision, warnings); + csm::ImageCoordCovar result(ip.line, ip.samp); + + // Compute partials ls wrt XYZ + std::vector prt(6, 0.0); + prt = computeGroundPartials(groundPt); + + // Error propagation + double ltx, lty, ltz; + double stx, sty, stz; + ltx = prt[0] * groundPt.covariance[0] + prt[1] * groundPt.covariance[3] + + prt[2] * groundPt.covariance[6]; + lty = prt[0] * groundPt.covariance[1] + prt[1] * groundPt.covariance[4] + + prt[2] * groundPt.covariance[7]; + ltz = prt[0] * groundPt.covariance[2] + prt[1] * groundPt.covariance[5] + + prt[2] * groundPt.covariance[8]; + stx = prt[3] * groundPt.covariance[0] + prt[4] * groundPt.covariance[3] + + prt[5] * groundPt.covariance[6]; + sty = prt[3] * groundPt.covariance[1] + prt[4] * groundPt.covariance[4] + + prt[5] * groundPt.covariance[7]; + stz = prt[3] * groundPt.covariance[2] + prt[4] * groundPt.covariance[5] + + prt[5] * groundPt.covariance[8]; + + double gp_cov[4]; // Input gp cov in image space + gp_cov[0] = ltx * prt[0] + lty * prt[1] + ltz * prt[2]; + gp_cov[1] = ltx * prt[3] + lty * prt[4] + ltz * prt[5]; + gp_cov[2] = stx * prt[0] + sty * prt[1] + stz * prt[2]; + gp_cov[3] = stx * prt[3] + sty * prt[4] + stz * prt[5]; + + std::vector unmodeled_cov = getUnmodeledError(ip); + double sensor_cov[4]; // sensor cov in image space + determineSensorCovarianceInImageSpace(gp, sensor_cov); + + result.covariance[0] = gp_cov[0] + unmodeled_cov[0] + sensor_cov[0]; + result.covariance[1] = gp_cov[1] + unmodeled_cov[1] + sensor_cov[1]; + result.covariance[2] = gp_cov[2] + unmodeled_cov[2] + sensor_cov[2]; + result.covariance[3] = gp_cov[3] + unmodeled_cov[3] + sensor_cov[3]; + + return result; } //*************************************************************************** // UsgsAstroLsSensorModel::imageToGround //*************************************************************************** csm::EcefCoord UsgsAstroLsSensorModel::imageToGround( - const csm::ImageCoord& image_pt, - double height, - double desired_precision, - double* achieved_precision, - csm::WarningList* warnings) const -{ - MESSAGE_LOG("Computing imageToGround for {}, {}, {}, with desired precision {}", - image_pt.line, image_pt.samp, height, desired_precision); - double xc, yc, zc; - double vx, vy, vz; - double xl, yl, zl; - double dxl, dyl, dzl; - losToEcf( - image_pt.line, image_pt.samp, _no_adjustment, - xc, yc, zc, vx, vy, vz, xl, yl, zl); - - double aPrec; - double x, y, z; - losEllipsoidIntersect( - height, xc, yc, zc, xl, yl, zl, x, y, z, aPrec, desired_precision); - - if (achieved_precision) - *achieved_precision = aPrec; - - if (warnings && (desired_precision > 0.0) && (aPrec > desired_precision)) - { - warnings->push_back( - csm::Warning( - csm::Warning::PRECISION_NOT_MET, - "Desired precision not achieved.", - "UsgsAstroLsSensorModel::imageToGround()")); - } - -/* - MESSAGE_LOG("imageToGround for {} {} {} achieved precision {}", - image_pt.line, image_pt.samp, height, achieved_precision) -*/ - - return csm::EcefCoord(x, y, z); + const csm::ImageCoord& image_pt, double height, double desired_precision, + double* achieved_precision, csm::WarningList* warnings) const { + MESSAGE_LOG( + "Computing imageToGround for {}, {}, {}, with desired precision {}", + image_pt.line, image_pt.samp, height, desired_precision); + double xc, yc, zc; + double vx, vy, vz; + double xl, yl, zl; + double dxl, dyl, dzl; + losToEcf(image_pt.line, image_pt.samp, _no_adjustment, xc, yc, zc, vx, vy, vz, + xl, yl, zl); + + double aPrec; + double x, y, z; + losEllipsoidIntersect(height, xc, yc, zc, xl, yl, zl, x, y, z, aPrec, + desired_precision); + + if (achieved_precision) *achieved_precision = aPrec; + + if (warnings && (desired_precision > 0.0) && (aPrec > desired_precision)) { + warnings->push_back(csm::Warning( + csm::Warning::PRECISION_NOT_MET, "Desired precision not achieved.", + "UsgsAstroLsSensorModel::imageToGround()")); + } + + /* + MESSAGE_LOG("imageToGround for {} {} {} achieved precision {}", + image_pt.line, image_pt.samp, height, + achieved_precision) + */ + + return csm::EcefCoord(x, y, z); } //*************************************************************************** // UsgsAstroLineScannerSensorModel::determineSensorCovarianceInImageSpace //*************************************************************************** void UsgsAstroLsSensorModel::determineSensorCovarianceInImageSpace( - csm::EcefCoord &gp, - double sensor_cov[4] ) const -{ - MESSAGE_LOG("Calculating determineSensorCovarianceInImageSpace for {} {} {}", - gp.x, gp.y, gp.z) - - - int i, j, totalAdjParams; - totalAdjParams = getNumParameters(); - - std::vector sensor_partials = computeAllSensorPartials(gp); - - sensor_cov[0] = 0.0; - sensor_cov[1] = 0.0; - sensor_cov[2] = 0.0; - sensor_cov[3] = 0.0; - - for (i = 0; i < totalAdjParams; i++) - { - for (j = 0; j < totalAdjParams; j++) - { - sensor_cov[0] += sensor_partials[i].first * getParameterCovariance(i, j) * sensor_partials[j].first; - sensor_cov[1] += sensor_partials[i].second * getParameterCovariance(i, j) * sensor_partials[j].first; - sensor_cov[2] += sensor_partials[i].first * getParameterCovariance(i, j) * sensor_partials[j].second; - sensor_cov[3] += sensor_partials[i].second * getParameterCovariance(i, j) * sensor_partials[j].second; - } - } + csm::EcefCoord& gp, double sensor_cov[4]) const { + MESSAGE_LOG("Calculating determineSensorCovarianceInImageSpace for {} {} {}", + gp.x, gp.y, gp.z) + + int i, j, totalAdjParams; + totalAdjParams = getNumParameters(); + + std::vector sensor_partials = + computeAllSensorPartials(gp); + + sensor_cov[0] = 0.0; + sensor_cov[1] = 0.0; + sensor_cov[2] = 0.0; + sensor_cov[3] = 0.0; + + for (i = 0; i < totalAdjParams; i++) { + for (j = 0; j < totalAdjParams; j++) { + sensor_cov[0] += sensor_partials[i].first * getParameterCovariance(i, j) * + sensor_partials[j].first; + sensor_cov[1] += sensor_partials[i].second * + getParameterCovariance(i, j) * sensor_partials[j].first; + sensor_cov[2] += sensor_partials[i].first * getParameterCovariance(i, j) * + sensor_partials[j].second; + sensor_cov[3] += sensor_partials[i].second * + getParameterCovariance(i, j) * sensor_partials[j].second; + } + } } - //*************************************************************************** // UsgsAstroLsSensorModel::imageToGround //*************************************************************************** csm::EcefCoordCovar UsgsAstroLsSensorModel::imageToGround( - const csm::ImageCoordCovar& image_pt, - double height, - double heightVariance, - double desired_precision, - double* achieved_precision, - csm::WarningList* warnings) const -{ - MESSAGE_LOG("Calculating imageToGround (with error propagation) for {}, {}, {} with height varinace {} and desired precision {}", - image_pt.line, image_pt.samp, height, heightVariance, desired_precision) - // Image to ground with error propagation - // Use numerical partials - - const double DELTA_IMAGE = 1.0; - const double DELTA_GROUND = m_gsd; - csm::ImageCoord ip(image_pt.line, image_pt.samp); - - csm::EcefCoord gp = imageToGround( - ip, height, desired_precision, achieved_precision, warnings); - - // Compute numerical partials xyz wrt to lsh - ip.line = image_pt.line + DELTA_IMAGE; - ip.samp = image_pt.samp; - csm::EcefCoord gpl = imageToGround(ip, height, desired_precision); - double xpl = (gpl.x - gp.x) / DELTA_IMAGE; - double ypl = (gpl.y - gp.y) / DELTA_IMAGE; - double zpl = (gpl.z - gp.z) / DELTA_IMAGE; - - ip.line = image_pt.line; - ip.samp = image_pt.samp + DELTA_IMAGE; - csm::EcefCoord gps = imageToGround(ip, height, desired_precision); - double xps = (gps.x - gp.x) / DELTA_IMAGE; - double yps = (gps.y - gp.y) / DELTA_IMAGE; - double zps = (gps.z - gp.z) / DELTA_IMAGE; - - ip.line = image_pt.line; - ip.samp = image_pt.samp; // +DELTA_IMAGE; - csm::EcefCoord gph = imageToGround(ip, height + DELTA_GROUND, desired_precision); - double xph = (gph.x - gp.x) / DELTA_GROUND; - double yph = (gph.y - gp.y) / DELTA_GROUND; - double zph = (gph.z - gp.z) / DELTA_GROUND; - - // Convert sensor covariance to image space - double sCov[4]; - determineSensorCovarianceInImageSpace(gp, sCov); - - std::vector unmod = getUnmodeledError(image_pt); - - double iCov[4]; - iCov[0] = image_pt.covariance[0] + sCov[0] + unmod[0]; - iCov[1] = image_pt.covariance[1] + sCov[1] + unmod[1]; - iCov[2] = image_pt.covariance[2] + sCov[2] + unmod[2]; - iCov[3] = image_pt.covariance[3] + sCov[3] + unmod[3]; - - // Temporary matrix product - double t00, t01, t02, t10, t11, t12, t20, t21, t22; - t00 = xpl * iCov[0] + xps * iCov[2]; - t01 = xpl * iCov[1] + xps * iCov[3]; - t02 = xph * heightVariance; - t10 = ypl * iCov[0] + yps * iCov[2]; - t11 = ypl * iCov[1] + yps * iCov[3]; - t12 = yph * heightVariance; - t20 = zpl * iCov[0] + zps * iCov[2]; - t21 = zpl * iCov[1] + zps * iCov[3]; - t22 = zph * heightVariance; - - // Ground covariance - csm::EcefCoordCovar result; - result.x = gp.x; - result.y = gp.y; - result.z = gp.z; - - result.covariance[0] = t00 * xpl + t01 * xps + t02 * xph; - result.covariance[1] = t00 * ypl + t01 * yps + t02 * yph; - result.covariance[2] = t00 * zpl + t01 * zps + t02 * zph; - result.covariance[3] = t10 * xpl + t11 * xps + t12 * xph; - result.covariance[4] = t10 * ypl + t11 * yps + t12 * yph; - result.covariance[5] = t10 * zpl + t11 * zps + t12 * zph; - result.covariance[6] = t20 * xpl + t21 * xps + t22 * xph; - result.covariance[7] = t20 * ypl + t21 * yps + t22 * yph; - result.covariance[8] = t20 * zpl + t21 * zps + t22 * zph; - - return result; + const csm::ImageCoordCovar& image_pt, double height, double heightVariance, + double desired_precision, double* achieved_precision, + csm::WarningList* warnings) const { + MESSAGE_LOG( + "Calculating imageToGround (with error propagation) for {}, {}, {} with " + "height varinace {} and desired precision {}", + image_pt.line, image_pt.samp, height, heightVariance, desired_precision) + // Image to ground with error propagation + // Use numerical partials + + const double DELTA_IMAGE = 1.0; + const double DELTA_GROUND = m_gsd; + csm::ImageCoord ip(image_pt.line, image_pt.samp); + + csm::EcefCoord gp = imageToGround(ip, height, desired_precision, + achieved_precision, warnings); + + // Compute numerical partials xyz wrt to lsh + ip.line = image_pt.line + DELTA_IMAGE; + ip.samp = image_pt.samp; + csm::EcefCoord gpl = imageToGround(ip, height, desired_precision); + double xpl = (gpl.x - gp.x) / DELTA_IMAGE; + double ypl = (gpl.y - gp.y) / DELTA_IMAGE; + double zpl = (gpl.z - gp.z) / DELTA_IMAGE; + + ip.line = image_pt.line; + ip.samp = image_pt.samp + DELTA_IMAGE; + csm::EcefCoord gps = imageToGround(ip, height, desired_precision); + double xps = (gps.x - gp.x) / DELTA_IMAGE; + double yps = (gps.y - gp.y) / DELTA_IMAGE; + double zps = (gps.z - gp.z) / DELTA_IMAGE; + + ip.line = image_pt.line; + ip.samp = image_pt.samp; // +DELTA_IMAGE; + csm::EcefCoord gph = + imageToGround(ip, height + DELTA_GROUND, desired_precision); + double xph = (gph.x - gp.x) / DELTA_GROUND; + double yph = (gph.y - gp.y) / DELTA_GROUND; + double zph = (gph.z - gp.z) / DELTA_GROUND; + + // Convert sensor covariance to image space + double sCov[4]; + determineSensorCovarianceInImageSpace(gp, sCov); + + std::vector unmod = getUnmodeledError(image_pt); + + double iCov[4]; + iCov[0] = image_pt.covariance[0] + sCov[0] + unmod[0]; + iCov[1] = image_pt.covariance[1] + sCov[1] + unmod[1]; + iCov[2] = image_pt.covariance[2] + sCov[2] + unmod[2]; + iCov[3] = image_pt.covariance[3] + sCov[3] + unmod[3]; + + // Temporary matrix product + double t00, t01, t02, t10, t11, t12, t20, t21, t22; + t00 = xpl * iCov[0] + xps * iCov[2]; + t01 = xpl * iCov[1] + xps * iCov[3]; + t02 = xph * heightVariance; + t10 = ypl * iCov[0] + yps * iCov[2]; + t11 = ypl * iCov[1] + yps * iCov[3]; + t12 = yph * heightVariance; + t20 = zpl * iCov[0] + zps * iCov[2]; + t21 = zpl * iCov[1] + zps * iCov[3]; + t22 = zph * heightVariance; + + // Ground covariance + csm::EcefCoordCovar result; + result.x = gp.x; + result.y = gp.y; + result.z = gp.z; + + result.covariance[0] = t00 * xpl + t01 * xps + t02 * xph; + result.covariance[1] = t00 * ypl + t01 * yps + t02 * yph; + result.covariance[2] = t00 * zpl + t01 * zps + t02 * zph; + result.covariance[3] = t10 * xpl + t11 * xps + t12 * xph; + result.covariance[4] = t10 * ypl + t11 * yps + t12 * yph; + result.covariance[5] = t10 * zpl + t11 * zps + t12 * zph; + result.covariance[6] = t20 * xpl + t21 * xps + t22 * xph; + result.covariance[7] = t20 * ypl + t21 * yps + t22 * yph; + result.covariance[8] = t20 * zpl + t21 * zps + t22 * zph; + + return result; } //*************************************************************************** // UsgsAstroLsSensorModel::imageToProximateImagingLocus //*************************************************************************** csm::EcefLocus UsgsAstroLsSensorModel::imageToProximateImagingLocus( - const csm::ImageCoord& image_pt, - const csm::EcefCoord& ground_pt, - double desired_precision, - double* achieved_precision, - csm::WarningList* warnings) const -{ - - MESSAGE_LOG("Computing imageToProximateImagingLocus (ground {}, {}, {}) for image point {}, {} with desired precision {}", - ground_pt.x, ground_pt.y, ground_pt.z, image_pt.line, image_pt.samp, desired_precision); - - // Object ray unit direction near given ground location - const double DELTA_GROUND = m_gsd; - - double x = ground_pt.x; - double y = ground_pt.y; - double z = ground_pt.z; - - // Elevation at input ground point - double height = computeEllipsoidElevation( - x, y, z, - m_majorAxis, m_minorAxis, desired_precision); - - // Ground point on object ray with the same elevation - csm::EcefCoord gp1 = imageToGround( - image_pt, height, desired_precision, achieved_precision); - - // Vector between 2 ground points above - double dx1 = x - gp1.x; - double dy1 = y - gp1.y; - double dz1 = z - gp1.z; - - // Unit vector along object ray - csm::EcefCoord gp2 = imageToGround( - image_pt, height - DELTA_GROUND, desired_precision, achieved_precision); - double dx2 = gp2.x - gp1.x; - double dy2 = gp2.y - gp1.y; - double dz2 = gp2.z - gp1.z; - double mag2 = sqrt(dx2 * dx2 + dy2 * dy2 + dz2 * dz2); - dx2 /= mag2; - dy2 /= mag2; - dz2 /= mag2; - - // Point on object ray perpendicular to input point - - // Locus - csm::EcefLocus locus; - double scale = dx1 * dx2 + dy1 * dy2 + dz1 * dz2; - gp2.x = gp1.x + scale * dx2; - gp2.y = gp1.y + scale * dy2; - gp2.z = gp1.z + scale * dz2; + const csm::ImageCoord& image_pt, const csm::EcefCoord& ground_pt, + double desired_precision, double* achieved_precision, + csm::WarningList* warnings) const { + MESSAGE_LOG( + "Computing imageToProximateImagingLocus (ground {}, {}, {}) for image " + "point {}, {} with desired precision {}", + ground_pt.x, ground_pt.y, ground_pt.z, image_pt.line, image_pt.samp, + desired_precision); + + // Object ray unit direction near given ground location + const double DELTA_GROUND = m_gsd; + + double x = ground_pt.x; + double y = ground_pt.y; + double z = ground_pt.z; + + // Elevation at input ground point + double height = computeEllipsoidElevation(x, y, z, m_majorAxis, m_minorAxis, + desired_precision); + + // Ground point on object ray with the same elevation + csm::EcefCoord gp1 = + imageToGround(image_pt, height, desired_precision, achieved_precision); + + // Vector between 2 ground points above + double dx1 = x - gp1.x; + double dy1 = y - gp1.y; + double dz1 = z - gp1.z; + + // Unit vector along object ray + csm::EcefCoord gp2 = imageToGround(image_pt, height - DELTA_GROUND, + desired_precision, achieved_precision); + double dx2 = gp2.x - gp1.x; + double dy2 = gp2.y - gp1.y; + double dz2 = gp2.z - gp1.z; + double mag2 = sqrt(dx2 * dx2 + dy2 * dy2 + dz2 * dz2); + dx2 /= mag2; + dy2 /= mag2; + dz2 /= mag2; + + // Point on object ray perpendicular to input point + + // Locus + csm::EcefLocus locus; + double scale = dx1 * dx2 + dy1 * dy2 + dz1 * dz2; + gp2.x = gp1.x + scale * dx2; + gp2.y = gp1.y + scale * dy2; + gp2.z = gp1.z + scale * dz2; - double hLocus = computeEllipsoidElevation( - gp2.x, gp2.y, gp2.z, - m_majorAxis, m_minorAxis, desired_precision); - locus.point = imageToGround( - image_pt, hLocus, desired_precision, achieved_precision, warnings); + double hLocus = computeEllipsoidElevation(gp2.x, gp2.y, gp2.z, m_majorAxis, + m_minorAxis, desired_precision); + locus.point = imageToGround(image_pt, hLocus, desired_precision, + achieved_precision, warnings); - locus.direction.x = dx2; - locus.direction.y = dy2; - locus.direction.z = dz2; + locus.direction.x = dx2; + locus.direction.y = dy2; + locus.direction.z = dz2; - return locus; + return locus; } //*************************************************************************** // UsgsAstroLsSensorModel::imageToRemoteImagingLocus //*************************************************************************** csm::EcefLocus UsgsAstroLsSensorModel::imageToRemoteImagingLocus( - const csm::ImageCoord& image_pt, - double desired_precision, - double* achieved_precision, - csm::WarningList* warnings) const -{ - - MESSAGE_LOG("Calculating imageToRemoteImagingLocus for point {}, {} with desired precision {}", - image_pt.line, image_pt.samp, desired_precision) + const csm::ImageCoord& image_pt, double desired_precision, + double* achieved_precision, csm::WarningList* warnings) const { + MESSAGE_LOG( + "Calculating imageToRemoteImagingLocus for point {}, {} with desired " + "precision {}", + image_pt.line, image_pt.samp, desired_precision) double vx, vy, vz; csm::EcefLocus locus; - losToEcf( - image_pt.line, image_pt.samp, _no_adjustment, - locus.point.x, locus.point.y, locus.point.z, - vx, vy, vz, - locus.direction.x, locus.direction.y, locus.direction.z); + losToEcf(image_pt.line, image_pt.samp, _no_adjustment, locus.point.x, + locus.point.y, locus.point.z, vx, vy, vz, locus.direction.x, + locus.direction.y, locus.direction.z); // losToEcf computes the negative look vector, so negate it locus.direction.x = -locus.direction.x; locus.direction.y = -locus.direction.y; @@ -1086,86 +1031,78 @@ csm::EcefLocus UsgsAstroLsSensorModel::imageToRemoteImagingLocus( // UsgsAstroLsSensorModel::computeGroundPartials //*************************************************************************** std::vector UsgsAstroLsSensorModel::computeGroundPartials( - const csm::EcefCoord& ground_pt) const -{ + const csm::EcefCoord& ground_pt) const { + MESSAGE_LOG("Computing computeGroundPartials for point {}, {}, {}", + ground_pt.x, ground_pt.y, ground_pt.z) - MESSAGE_LOG("Computing computeGroundPartials for point {}, {}, {}", - ground_pt.x, ground_pt.y, ground_pt.z) + double GND_DELTA = m_gsd; + // Partial of line, sample wrt X, Y, Z + double x = ground_pt.x; + double y = ground_pt.y; + double z = ground_pt.z; - double GND_DELTA = m_gsd; - // Partial of line, sample wrt X, Y, Z - double x = ground_pt.x; - double y = ground_pt.y; - double z = ground_pt.z; + csm::ImageCoord ipB = groundToImage(ground_pt); + csm::ImageCoord ipX = groundToImage(csm::EcefCoord(x + GND_DELTA, y, z)); + csm::ImageCoord ipY = groundToImage(csm::EcefCoord(x, y + GND_DELTA, z)); + csm::ImageCoord ipZ = groundToImage(csm::EcefCoord(x, y, z + GND_DELTA)); - csm::ImageCoord ipB = groundToImage(ground_pt); - csm::ImageCoord ipX = groundToImage(csm::EcefCoord(x + GND_DELTA, y, z)); - csm::ImageCoord ipY = groundToImage(csm::EcefCoord(x, y + GND_DELTA, z)); - csm::ImageCoord ipZ = groundToImage(csm::EcefCoord(x, y, z + GND_DELTA)); + std::vector partials(6, 0.0); + partials[0] = (ipX.line - ipB.line) / GND_DELTA; + partials[3] = (ipX.samp - ipB.samp) / GND_DELTA; + partials[1] = (ipY.line - ipB.line) / GND_DELTA; + partials[4] = (ipY.samp - ipB.samp) / GND_DELTA; + partials[2] = (ipZ.line - ipB.line) / GND_DELTA; + partials[5] = (ipZ.samp - ipB.samp) / GND_DELTA; - std::vector partials(6, 0.0); - partials[0] = (ipX.line - ipB.line) / GND_DELTA; - partials[3] = (ipX.samp - ipB.samp) / GND_DELTA; - partials[1] = (ipY.line - ipB.line) / GND_DELTA; - partials[4] = (ipY.samp - ipB.samp) / GND_DELTA; - partials[2] = (ipZ.line - ipB.line) / GND_DELTA; - partials[5] = (ipZ.samp - ipB.samp) / GND_DELTA; - - return partials; + return partials; } - //*************************************************************************** // UsgsAstroLsSensorModel::computeSensorPartials //*************************************************************************** csm::RasterGM::SensorPartials UsgsAstroLsSensorModel::computeSensorPartials( - int index, - const csm::EcefCoord& ground_pt, - double desired_precision, - double* achieved_precision, - csm::WarningList* warnings) const -{ - - MESSAGE_LOG("Calculating computeSensorPartials for ground point {}, {}, {} with desired precision {}", - ground_pt.x, ground_pt.y, ground_pt.z, desired_precision) + int index, const csm::EcefCoord& ground_pt, double desired_precision, + double* achieved_precision, csm::WarningList* warnings) const { + MESSAGE_LOG( + "Calculating computeSensorPartials for ground point {}, {}, {} with " + "desired precision {}", + ground_pt.x, ground_pt.y, ground_pt.z, desired_precision) - // Compute image coordinate first - csm::ImageCoord img_pt = groundToImage( - ground_pt, desired_precision, achieved_precision); + // Compute image coordinate first + csm::ImageCoord img_pt = + groundToImage(ground_pt, desired_precision, achieved_precision); - // Call overloaded function - return computeSensorPartials( - index, img_pt, ground_pt, desired_precision, achieved_precision, warnings); + // Call overloaded function + return computeSensorPartials(index, img_pt, ground_pt, desired_precision, + achieved_precision, warnings); } //*************************************************************************** // UsgsAstroLsSensorModel::computeSensorPartials //*************************************************************************** csm::RasterGM::SensorPartials UsgsAstroLsSensorModel::computeSensorPartials( - int index, - const csm::ImageCoord& image_pt, - const csm::EcefCoord& ground_pt, - double desired_precision, - double* achieved_precision, - csm::WarningList* warnings) const -{ - - MESSAGE_LOG("Calculating computeSensorPartials (with image points {}, {}) for ground point {}, {}, {} with desired precision {}", - image_pt.line, image_pt.samp, ground_pt.x, ground_pt.y, ground_pt.z, desired_precision) + int index, const csm::ImageCoord& image_pt, const csm::EcefCoord& ground_pt, + double desired_precision, double* achieved_precision, + csm::WarningList* warnings) const { + MESSAGE_LOG( + "Calculating computeSensorPartials (with image points {}, {}) for ground " + "point {}, {}, {} with desired precision {}", + image_pt.line, image_pt.samp, ground_pt.x, ground_pt.y, ground_pt.z, + desired_precision) - // Compute numerical partials ls wrt specific parameter + // Compute numerical partials ls wrt specific parameter - const double DELTA = m_gsd; - std::vector adj(UsgsAstroLsSensorModel::NUM_PARAMETERS, 0.0); - adj[index] = DELTA; + const double DELTA = m_gsd; + std::vector adj(UsgsAstroLsSensorModel::NUM_PARAMETERS, 0.0); + adj[index] = DELTA; - csm::ImageCoord img1 = groundToImage( - ground_pt, adj, desired_precision, achieved_precision, warnings); + csm::ImageCoord img1 = groundToImage(ground_pt, adj, desired_precision, + achieved_precision, warnings); - double line_partial = (img1.line - image_pt.line) / DELTA; - double sample_partial = (img1.samp - image_pt.samp) / DELTA; + double line_partial = (img1.line - image_pt.line) / DELTA; + double sample_partial = (img1.samp - image_pt.samp) / DELTA; - return csm::RasterGM::SensorPartials(line_partial, sample_partial); + return csm::RasterGM::SensorPartials(line_partial, sample_partial); } //*************************************************************************** @@ -1173,19 +1110,18 @@ csm::RasterGM::SensorPartials UsgsAstroLsSensorModel::computeSensorPartials( //*************************************************************************** std::vector UsgsAstroLsSensorModel::computeAllSensorPartials( - const csm::EcefCoord& ground_pt, - csm::param::Set pSet, - double desired_precision, - double* achieved_precision, - csm::WarningList* warnings) const -{ - MESSAGE_LOG("Computing computeAllSensorPartials for ground point {}, {}, {} with desired precision {}", - ground_pt.x, ground_pt.y, ground_pt.z, desired_precision) - csm::ImageCoord image_pt = groundToImage( - ground_pt, desired_precision, achieved_precision, warnings); + const csm::EcefCoord& ground_pt, csm::param::Set pSet, + double desired_precision, double* achieved_precision, + csm::WarningList* warnings) const { + MESSAGE_LOG( + "Computing computeAllSensorPartials for ground point {}, {}, {} with " + "desired precision {}", + ground_pt.x, ground_pt.y, ground_pt.z, desired_precision) + csm::ImageCoord image_pt = + groundToImage(ground_pt, desired_precision, achieved_precision, warnings); - return computeAllSensorPartials( - image_pt, ground_pt, pSet, desired_precision, achieved_precision, warnings); + return computeAllSensorPartials(image_pt, ground_pt, pSet, desired_precision, + achieved_precision, warnings); } //*************************************************************************** @@ -1193,60 +1129,50 @@ UsgsAstroLsSensorModel::computeAllSensorPartials( //*************************************************************************** std::vector UsgsAstroLsSensorModel::computeAllSensorPartials( - const csm::ImageCoord& image_pt, - const csm::EcefCoord& ground_pt, - csm::param::Set pSet, - double desired_precision, - double* achieved_precision, - csm::WarningList* warnings) const -{ - - MESSAGE_LOG("Computing computeAllSensorPartials for image {} {} and ground {}, {}, {} with desired precision {}", - image_pt.line, image_pt.samp, ground_pt.x, ground_pt.y, ground_pt.z, desired_precision) - - std::vector indices = getParameterSetIndices(pSet); - size_t num = indices.size(); - std::vector partials; - for (int index = 0; index < num; index++) - { - partials.push_back( - computeSensorPartials( - indices[index], image_pt, ground_pt, - desired_precision, achieved_precision, warnings)); - } - return partials; + const csm::ImageCoord& image_pt, const csm::EcefCoord& ground_pt, + csm::param::Set pSet, double desired_precision, double* achieved_precision, + csm::WarningList* warnings) const { + MESSAGE_LOG( + "Computing computeAllSensorPartials for image {} {} and ground {}, {}, " + "{} with desired precision {}", + image_pt.line, image_pt.samp, ground_pt.x, ground_pt.y, ground_pt.z, + desired_precision) + + std::vector indices = getParameterSetIndices(pSet); + size_t num = indices.size(); + std::vector partials; + for (int index = 0; index < num; index++) { + partials.push_back(computeSensorPartials(indices[index], image_pt, + ground_pt, desired_precision, + achieved_precision, warnings)); + } + return partials; } //*************************************************************************** // UsgsAstroLsSensorModel::getParameterCovariance //*************************************************************************** -double UsgsAstroLsSensorModel::getParameterCovariance( - int index1, - int index2) const -{ - - int index = UsgsAstroLsSensorModel::NUM_PARAMETERS * index1 + index2; +double UsgsAstroLsSensorModel::getParameterCovariance(int index1, + int index2) const { + int index = UsgsAstroLsSensorModel::NUM_PARAMETERS * index1 + index2; - MESSAGE_LOG("getParameterCovariance for {} {} is {}", - index1, index2, m_covariance[index]) + MESSAGE_LOG("getParameterCovariance for {} {} is {}", index1, index2, + m_covariance[index]) - return m_covariance[index]; + return m_covariance[index]; } //*************************************************************************** // UsgsAstroLsSensorModel::setParameterCovariance //*************************************************************************** -void UsgsAstroLsSensorModel::setParameterCovariance( - int index1, - int index2, - double covariance) -{ - int index = UsgsAstroLsSensorModel::NUM_PARAMETERS * index1 + index2; +void UsgsAstroLsSensorModel::setParameterCovariance(int index1, int index2, + double covariance) { + int index = UsgsAstroLsSensorModel::NUM_PARAMETERS * index1 + index2; - MESSAGE_LOG("setParameterCovariance for {} {} is {}", - index1, index2, m_covariance[index]) + MESSAGE_LOG("setParameterCovariance for {} {} is {}", index1, index2, + m_covariance[index]) - m_covariance[index] = covariance; + m_covariance[index] = covariance; } //--------------------------------------------------------------------------- @@ -1256,68 +1182,66 @@ void UsgsAstroLsSensorModel::setParameterCovariance( //*************************************************************************** // UsgsAstroLsSensorModel::getTrajectoryIdentifier //*************************************************************************** -std::string UsgsAstroLsSensorModel::getTrajectoryIdentifier() const -{ - return "UNKNOWN"; +std::string UsgsAstroLsSensorModel::getTrajectoryIdentifier() const { + return "UNKNOWN"; } //*************************************************************************** // UsgsAstroLsSensorModel::getReferenceDateAndTime //*************************************************************************** -std::string UsgsAstroLsSensorModel::getReferenceDateAndTime() const -{ - csm::EcefCoord referencePointGround = UsgsAstroLsSensorModel::getReferencePoint(); - csm::ImageCoord referencePointImage = UsgsAstroLsSensorModel::groundToImage(referencePointGround); - double relativeTime = UsgsAstroLsSensorModel::getImageTime(referencePointImage); - time_t ephemTime = m_centerEphemerisTime + relativeTime; - struct tm t = {0}; // Initalize to all 0's - t.tm_year = 100; // This is year-1900, so 100 = 2000 - t.tm_mday = 1; - time_t timeSinceEpoch = mktime(&t); - time_t finalTime = ephemTime + timeSinceEpoch; - char buffer [16]; - strftime(buffer, 16, "%Y%m%dT%H%M%S", localtime(&finalTime)); - buffer[15] = '\0'; +std::string UsgsAstroLsSensorModel::getReferenceDateAndTime() const { + csm::EcefCoord referencePointGround = + UsgsAstroLsSensorModel::getReferencePoint(); + csm::ImageCoord referencePointImage = + UsgsAstroLsSensorModel::groundToImage(referencePointGround); + double relativeTime = + UsgsAstroLsSensorModel::getImageTime(referencePointImage); + time_t ephemTime = m_centerEphemerisTime + relativeTime; + struct tm t = {0}; // Initalize to all 0's + t.tm_year = 100; // This is year-1900, so 100 = 2000 + t.tm_mday = 1; + time_t timeSinceEpoch = mktime(&t); + time_t finalTime = ephemTime + timeSinceEpoch; + char buffer[16]; + strftime(buffer, 16, "%Y%m%dT%H%M%S", localtime(&finalTime)); + buffer[15] = '\0'; - return buffer; + return buffer; } //*************************************************************************** // UsgsAstroLsSensorModel::getImageTime //*************************************************************************** double UsgsAstroLsSensorModel::getImageTime( - const csm::ImageCoord& image_pt) const -{ - double lineFull = image_pt.line; + const csm::ImageCoord& image_pt) const { + double lineFull = image_pt.line; - auto referenceLineIt = std::upper_bound(m_intTimeLines.begin(), - m_intTimeLines.end(), - lineFull); - if (referenceLineIt != m_intTimeLines.begin()) { - --referenceLineIt; - } - size_t referenceIndex = std::distance(m_intTimeLines.begin(), referenceLineIt); + auto referenceLineIt = + std::upper_bound(m_intTimeLines.begin(), m_intTimeLines.end(), lineFull); + if (referenceLineIt != m_intTimeLines.begin()) { + --referenceLineIt; + } + size_t referenceIndex = + std::distance(m_intTimeLines.begin(), referenceLineIt); - // Adding 0.5 to the line results in center exposure time for a given line - double time = m_intTimeStartTimes[referenceIndex] - + m_intTimes[referenceIndex] * (lineFull - m_intTimeLines[referenceIndex] + 0.5); + // Adding 0.5 to the line results in center exposure time for a given line + double time = m_intTimeStartTimes[referenceIndex] + + m_intTimes[referenceIndex] * + (lineFull - m_intTimeLines[referenceIndex] + 0.5); - MESSAGE_LOG("getImageTime for image line {} is {}", - image_pt.line, time) + MESSAGE_LOG("getImageTime for image line {} is {}", image_pt.line, time) - return time; + return time; } //*************************************************************************** // UsgsAstroLsSensorModel::getSensorPosition //*************************************************************************** csm::EcefCoord UsgsAstroLsSensorModel::getSensorPosition( - const csm::ImageCoord& imagePt) const -{ - MESSAGE_LOG("getSensorPosition at line {}", - imagePt.line) + const csm::ImageCoord& imagePt) const { + MESSAGE_LOG("getSensorPosition at line {}", imagePt.line) - return getSensorPosition(getImageTime(imagePt)); + return getSensorPosition(getImageTime(imagePt)); } //*************************************************************************** @@ -1326,38 +1250,33 @@ csm::EcefCoord UsgsAstroLsSensorModel::getSensorPosition( csm::EcefCoord UsgsAstroLsSensorModel::getSensorPosition(double time) const { - double x, y, z, vx, vy, vz; - getAdjSensorPosVel(time, _no_adjustment, x, y, z, vx, vy, vz); + double x, y, z, vx, vy, vz; + getAdjSensorPosVel(time, _no_adjustment, x, y, z, vx, vy, vz); - MESSAGE_LOG("getSensorPosition at {}", - time) + MESSAGE_LOG("getSensorPosition at {}", time) - return csm::EcefCoord(x, y, z); + return csm::EcefCoord(x, y, z); } //*************************************************************************** // UsgsAstroLsSensorModel::getSensorVelocity //*************************************************************************** csm::EcefVector UsgsAstroLsSensorModel::getSensorVelocity( - const csm::ImageCoord& imagePt) const -{ - MESSAGE_LOG("getSensorVelocity at {}", - imagePt.line) - return getSensorVelocity(getImageTime(imagePt)); + const csm::ImageCoord& imagePt) const { + MESSAGE_LOG("getSensorVelocity at {}", imagePt.line) + return getSensorVelocity(getImageTime(imagePt)); } //*************************************************************************** // UsgsAstroLsSensorModel::getSensorVelocity //*************************************************************************** -csm::EcefVector UsgsAstroLsSensorModel::getSensorVelocity(double time) const -{ - double x, y, z, vx, vy, vz; - getAdjSensorPosVel(time, _no_adjustment, x, y, z, vx, vy, vz); +csm::EcefVector UsgsAstroLsSensorModel::getSensorVelocity(double time) const { + double x, y, z, vx, vy, vz; + getAdjSensorPosVel(time, _no_adjustment, x, y, z, vx, vy, vz); - MESSAGE_LOG("getSensorVelocity at {}", - time) + MESSAGE_LOG("getSensorVelocity at {}", time) - return csm::EcefVector(vx, vy, vz); + return csm::EcefVector(vx, vy, vz); } //--------------------------------------------------------------------------- @@ -1367,59 +1286,49 @@ csm::EcefVector UsgsAstroLsSensorModel::getSensorVelocity(double time) const //*************************************************************************** // UsgsAstroLsSensorModel::setParameterValue //*************************************************************************** -void UsgsAstroLsSensorModel::setParameterValue(int index, double value) -{ - m_currentParameterValue[index] = value; +void UsgsAstroLsSensorModel::setParameterValue(int index, double value) { + m_currentParameterValue[index] = value; } //*************************************************************************** // UsgsAstroLsSensorModel::getParameterValue //*************************************************************************** -double UsgsAstroLsSensorModel::getParameterValue(int index) const -{ - return m_currentParameterValue[index]; +double UsgsAstroLsSensorModel::getParameterValue(int index) const { + return m_currentParameterValue[index]; } //*************************************************************************** // UsgsAstroLsSensorModel::getParameterName //*************************************************************************** -std::string UsgsAstroLsSensorModel::getParameterName(int index) const -{ - return PARAMETER_NAME[index]; +std::string UsgsAstroLsSensorModel::getParameterName(int index) const { + return PARAMETER_NAME[index]; } -std::string UsgsAstroLsSensorModel::getParameterUnits(int index) const -{ - // All parameters are meters or scaled to meters - return "m"; +std::string UsgsAstroLsSensorModel::getParameterUnits(int index) const { + // All parameters are meters or scaled to meters + return "m"; } - //*************************************************************************** // UsgsAstroLsSensorModel::getNumParameters //*************************************************************************** -int UsgsAstroLsSensorModel::getNumParameters() const -{ - return UsgsAstroLsSensorModel::NUM_PARAMETERS; +int UsgsAstroLsSensorModel::getNumParameters() const { + return UsgsAstroLsSensorModel::NUM_PARAMETERS; } - //*************************************************************************** // UsgsAstroLsSensorModel::getParameterType //*************************************************************************** -csm::param::Type UsgsAstroLsSensorModel::getParameterType(int index) const -{ - return m_parameterType[index]; +csm::param::Type UsgsAstroLsSensorModel::getParameterType(int index) const { + return m_parameterType[index]; } - //*************************************************************************** // UsgsAstroLsSensorModel::setParameterType //*************************************************************************** -void UsgsAstroLsSensorModel::setParameterType( - int index, csm::param::Type pType) -{ - m_parameterType[index] = pType; +void UsgsAstroLsSensorModel::setParameterType(int index, + csm::param::Type pType) { + m_parameterType[index] = pType; } //--------------------------------------------------------------------------- @@ -1429,85 +1338,75 @@ void UsgsAstroLsSensorModel::setParameterType( //*************************************************************************** // UsgsAstroLsSensorModel::getPedigree //*************************************************************************** -std::string UsgsAstroLsSensorModel::getPedigree() const -{ - return "USGS_LINE_SCANNER"; +std::string UsgsAstroLsSensorModel::getPedigree() const { + return "USGS_LINE_SCANNER"; } //*************************************************************************** // UsgsAstroLsSensorModel::getImageIdentifier //*************************************************************************** -std::string UsgsAstroLsSensorModel::getImageIdentifier() const -{ - return m_imageIdentifier; +std::string UsgsAstroLsSensorModel::getImageIdentifier() const { + return m_imageIdentifier; } //*************************************************************************** // UsgsAstroLsSensorModel::setImageIdentifier //*************************************************************************** -void UsgsAstroLsSensorModel::setImageIdentifier( - const std::string& imageId, - csm::WarningList* warnings) -{ - // Image id should include the suffix without the path name - m_imageIdentifier = imageId; +void UsgsAstroLsSensorModel::setImageIdentifier(const std::string& imageId, + csm::WarningList* warnings) { + // Image id should include the suffix without the path name + m_imageIdentifier = imageId; } //*************************************************************************** // UsgsAstroLsSensorModel::getSensorIdentifier //*************************************************************************** -std::string UsgsAstroLsSensorModel::getSensorIdentifier() const -{ - return m_sensorIdentifier; +std::string UsgsAstroLsSensorModel::getSensorIdentifier() const { + return m_sensorIdentifier; } //*************************************************************************** // UsgsAstroLsSensorModel::getPlatformIdentifier //*************************************************************************** -std::string UsgsAstroLsSensorModel::getPlatformIdentifier() const -{ - return m_platformIdentifier; +std::string UsgsAstroLsSensorModel::getPlatformIdentifier() const { + return m_platformIdentifier; } //*************************************************************************** // UsgsAstroLsSensorModel::setReferencePoint //*************************************************************************** -void UsgsAstroLsSensorModel::setReferencePoint(const csm::EcefCoord& ground_pt) -{ - m_referencePointXyz = ground_pt; +void UsgsAstroLsSensorModel::setReferencePoint( + const csm::EcefCoord& ground_pt) { + m_referencePointXyz = ground_pt; } //*************************************************************************** // UsgsAstroLsSensorModel::getReferencePoint //*************************************************************************** -csm::EcefCoord UsgsAstroLsSensorModel::getReferencePoint() const -{ - // Return ground point at image center - return m_referencePointXyz; +csm::EcefCoord UsgsAstroLsSensorModel::getReferencePoint() const { + // Return ground point at image center + return m_referencePointXyz; } //*************************************************************************** // UsgsAstroLsSensorModel::getSensorModelName //*************************************************************************** -std::string UsgsAstroLsSensorModel::getModelName() const -{ - return UsgsAstroLsSensorModel::_SENSOR_MODEL_NAME; +std::string UsgsAstroLsSensorModel::getModelName() const { + return UsgsAstroLsSensorModel::_SENSOR_MODEL_NAME; } //*************************************************************************** // UsgsAstroLsSensorModel::getImageStart //*************************************************************************** -csm::ImageCoord UsgsAstroLsSensorModel::getImageStart() const -{ - return csm::ImageCoord(0.0, 0.0); +csm::ImageCoord UsgsAstroLsSensorModel::getImageStart() const { + return csm::ImageCoord(0.0, 0.0); } //*************************************************************************** // UsgsAstroLsSensorModel::getImageSize //*************************************************************************** -csm::ImageVector UsgsAstroLsSensorModel::getImageSize() const -{ - return csm::ImageVector(m_nLines, m_nSamples); +csm::ImageVector UsgsAstroLsSensorModel::getImageSize() const { + return csm::ImageVector(m_nLines, m_nSamples); } //--------------------------------------------------------------------------- @@ -1517,7 +1416,8 @@ csm::ImageVector UsgsAstroLsSensorModel::getImageSize() const // //*************************************************************************** // // UsgsAstroLsSensorModel::getSensorModelState // //*************************************************************************** -// std::string UsgsAstroLsSensorModel::setModelState(std::string stateString) const +// std::string UsgsAstroLsSensorModel::setModelState(std::string stateString) +// const // { // auto j = json::parse(stateString); // int num_params = NUM_PARAMETERS; @@ -1571,8 +1471,9 @@ csm::ImageVector UsgsAstroLsSensorModel::getImageSize() const // m_positions = j["m_positions"].get>(); // m_velocities = j["m_velocities"].get>(); // m_quaternions = j["m_quaternions"].get>(); -// m_currentParameterValue = j["m_currentParameterValue"].get>(); -// m_covariance = j["m_covariance"].get>(); +// m_currentParameterValue = +// j["m_currentParameterValue"].get>(); m_covariance = +// j["m_covariance"].get>(); // // for (int i = 0; i < num_params; i++) { // for (int k = 0; k < NUM_PARAM_TYPES; k++) { @@ -1592,36 +1493,37 @@ csm::ImageVector UsgsAstroLsSensorModel::getImageSize() const //*************************************************************************** // UsgsAstroLsSensorModel::getValidHeightRange //*************************************************************************** -std::pair UsgsAstroLsSensorModel::getValidHeightRange() const -{ - return std::pair(m_minElevation, m_maxElevation); +std::pair UsgsAstroLsSensorModel::getValidHeightRange() const { + return std::pair(m_minElevation, m_maxElevation); } //*************************************************************************** // UsgsAstroLsSensorModel::getValidImageRange //*************************************************************************** std::pair -UsgsAstroLsSensorModel::getValidImageRange() const -{ - return std::pair( +UsgsAstroLsSensorModel::getValidImageRange() const { + return std::pair( csm::ImageCoord(0.0, 0.0), - csm::ImageCoord(m_nLines, m_nSamples)); // Technically nl and ns are outside the image in a zero based system. + csm::ImageCoord(m_nLines, + m_nSamples)); // Technically nl and ns are outside the + // image in a zero based system. } //*************************************************************************** // UsgsAstroLsSensorModel::getIlluminationDirection //*************************************************************************** csm::EcefVector UsgsAstroLsSensorModel::getIlluminationDirection( - const csm::EcefCoord& groundPt) const -{ - MESSAGE_LOG("Accessing illumination direction of ground point" - "{} {} {}.", - groundPt.x, groundPt.y, groundPt.z); - - csm::EcefVector sunPosition = getSunPosition(getImageTime(groundToImage(groundPt))); - csm::EcefVector illuminationDirection = csm::EcefVector(groundPt.x - sunPosition.x, - groundPt.y - sunPosition.y, - groundPt.z - sunPosition.z); + const csm::EcefCoord& groundPt) const { + MESSAGE_LOG( + "Accessing illumination direction of ground point" + "{} {} {}.", + groundPt.x, groundPt.y, groundPt.z); + + csm::EcefVector sunPosition = + getSunPosition(getImageTime(groundToImage(groundPt))); + csm::EcefVector illuminationDirection = + csm::EcefVector(groundPt.x - sunPosition.x, groundPt.y - sunPosition.y, + groundPt.z - sunPosition.z); double scale = sqrt(illuminationDirection.x * illuminationDirection.x + illuminationDirection.y * illuminationDirection.y + @@ -1640,827 +1542,760 @@ csm::EcefVector UsgsAstroLsSensorModel::getIlluminationDirection( //*************************************************************************** // UsgsAstroLsSensorModel::getNumGeometricCorrectionSwitches //*************************************************************************** -int UsgsAstroLsSensorModel::getNumGeometricCorrectionSwitches() const -{ - return 0; +int UsgsAstroLsSensorModel::getNumGeometricCorrectionSwitches() const { + return 0; } //*************************************************************************** // UsgsAstroLsSensorModel::getGeometricCorrectionName //*************************************************************************** -std::string UsgsAstroLsSensorModel::getGeometricCorrectionName(int index) const -{ - MESSAGE_LOG("Accessing name of geometric correction switch {}. " - "Geometric correction switches are not supported, throwing exception", - index); - // Since there are no geometric corrections, all indices are out of range - throw csm::Error( - csm::Error::INDEX_OUT_OF_RANGE, - "Index is out of range.", - "UsgsAstroLsSensorModel::getGeometricCorrectionName"); +std::string UsgsAstroLsSensorModel::getGeometricCorrectionName( + int index) const { + MESSAGE_LOG( + "Accessing name of geometric correction switch {}. " + "Geometric correction switches are not supported, throwing exception", + index); + // Since there are no geometric corrections, all indices are out of range + throw csm::Error(csm::Error::INDEX_OUT_OF_RANGE, "Index is out of range.", + "UsgsAstroLsSensorModel::getGeometricCorrectionName"); } //*************************************************************************** // UsgsAstroLsSensorModel::setGeometricCorrectionSwitch //*************************************************************************** void UsgsAstroLsSensorModel::setGeometricCorrectionSwitch( - int index, - bool value, - csm::param::Type pType) + int index, bool value, csm::param::Type pType) { - MESSAGE_LOG("Setting geometric correction switch {} to {} " - "with parameter type {}. " - "Geometric correction switches are not supported, throwing exception", - index, value, pType); - // Since there are no geometric corrections, all indices are out of range - throw csm::Error( - csm::Error::INDEX_OUT_OF_RANGE, - "Index is out of range.", - "UsgsAstroLsSensorModel::setGeometricCorrectionSwitch"); + MESSAGE_LOG( + "Setting geometric correction switch {} to {} " + "with parameter type {}. " + "Geometric correction switches are not supported, throwing exception", + index, value, pType); + // Since there are no geometric corrections, all indices are out of range + throw csm::Error(csm::Error::INDEX_OUT_OF_RANGE, "Index is out of range.", + "UsgsAstroLsSensorModel::setGeometricCorrectionSwitch"); } //*************************************************************************** // UsgsAstroLsSensorModel::getGeometricCorrectionSwitch //*************************************************************************** -bool UsgsAstroLsSensorModel::getGeometricCorrectionSwitch(int index) const -{ - MESSAGE_LOG("Accessing value of geometric correction switch {}. " - "Geometric correction switches are not supported, throwing exception", - index); - // Since there are no geometric corrections, all indices are out of range - throw csm::Error( - csm::Error::INDEX_OUT_OF_RANGE, - "Index is out of range.", - "UsgsAstroLsSensorModel::getGeometricCorrectionSwitch"); +bool UsgsAstroLsSensorModel::getGeometricCorrectionSwitch(int index) const { + MESSAGE_LOG( + "Accessing value of geometric correction switch {}. " + "Geometric correction switches are not supported, throwing exception", + index); + // Since there are no geometric corrections, all indices are out of range + throw csm::Error(csm::Error::INDEX_OUT_OF_RANGE, "Index is out of range.", + "UsgsAstroLsSensorModel::getGeometricCorrectionSwitch"); } //*************************************************************************** // UsgsAstroLsSensorModel::getCrossCovarianceMatrix //*************************************************************************** std::vector UsgsAstroLsSensorModel::getCrossCovarianceMatrix( - const csm::GeometricModel& comparisonModel, - csm::param::Set pSet, - const csm::GeometricModel::GeometricModelList& otherModels) const -{ - // Return covariance matrix - if (&comparisonModel == this) { - std::vector paramIndices = getParameterSetIndices(pSet); - int numParams = paramIndices.size(); - std::vector covariances(numParams * numParams, 0.0); - for (int i = 0; i < numParams; i++) { - for (int j = 0; j < numParams; j++) { - covariances[i * numParams + j] = getParameterCovariance(paramIndices[i], paramIndices[j]); - } - } - return covariances; - } - // No correlation between models. - const std::vector& indices = getParameterSetIndices(pSet); - size_t num_rows = indices.size(); - const std::vector& indices2 = comparisonModel.getParameterSetIndices(pSet); - size_t num_cols = indices.size(); - - return std::vector(num_rows * num_cols, 0.0); + const csm::GeometricModel& comparisonModel, csm::param::Set pSet, + const csm::GeometricModel::GeometricModelList& otherModels) const { + // Return covariance matrix + if (&comparisonModel == this) { + std::vector paramIndices = getParameterSetIndices(pSet); + int numParams = paramIndices.size(); + std::vector covariances(numParams * numParams, 0.0); + for (int i = 0; i < numParams; i++) { + for (int j = 0; j < numParams; j++) { + covariances[i * numParams + j] = + getParameterCovariance(paramIndices[i], paramIndices[j]); + } + } + return covariances; + } + // No correlation between models. + const std::vector& indices = getParameterSetIndices(pSet); + size_t num_rows = indices.size(); + const std::vector& indices2 = + comparisonModel.getParameterSetIndices(pSet); + size_t num_cols = indices.size(); + + return std::vector(num_rows * num_cols, 0.0); } //*************************************************************************** // UsgsAstroLineScannerSensorModel::getCorrelationModel //*************************************************************************** -const csm::CorrelationModel& -UsgsAstroLsSensorModel::getCorrelationModel() const -{ - // All Line Scanner images are assumed uncorrelated - return _no_corr_model; +const csm::CorrelationModel& UsgsAstroLsSensorModel::getCorrelationModel() + const { + // All Line Scanner images are assumed uncorrelated + return _no_corr_model; } //*************************************************************************** // UsgsAstroLsSensorModel::getUnmodeledCrossCovariance //*************************************************************************** std::vector UsgsAstroLsSensorModel::getUnmodeledCrossCovariance( - const csm::ImageCoord& pt1, - const csm::ImageCoord& pt2) const -{ - // No unmodeled error - return std::vector(4, 0.0); + const csm::ImageCoord& pt1, const csm::ImageCoord& pt2) const { + // No unmodeled error + return std::vector(4, 0.0); } - //*************************************************************************** // UsgsAstroLsSensorModel::getCollectionIdentifier //*************************************************************************** -std::string UsgsAstroLsSensorModel::getCollectionIdentifier() const -{ - return "UNKNOWN"; +std::string UsgsAstroLsSensorModel::getCollectionIdentifier() const { + return "UNKNOWN"; } //*************************************************************************** // UsgsAstroLsSensorModel::hasShareableParameters //*************************************************************************** -bool UsgsAstroLsSensorModel::hasShareableParameters() const -{ - // Parameter sharing is not supported for this sensor - return false; +bool UsgsAstroLsSensorModel::hasShareableParameters() const { + // Parameter sharing is not supported for this sensor + return false; } //*************************************************************************** // UsgsAstroLsSensorModel::isParameterShareable //*************************************************************************** -bool UsgsAstroLsSensorModel::isParameterShareable(int index) const -{ - // Parameter sharing is not supported for this sensor - return false; +bool UsgsAstroLsSensorModel::isParameterShareable(int index) const { + // Parameter sharing is not supported for this sensor + return false; } //*************************************************************************** // UsgsAstroLsSensorModel::getParameterSharingCriteria //*************************************************************************** csm::SharingCriteria UsgsAstroLsSensorModel::getParameterSharingCriteria( - int index) const -{ - MESSAGE_LOG("Checking sharing criteria for parameter {}. " - "Sharing is not supported, throwing exception", index); - // Parameter sharing is not supported for this sensor, - // all indices are out of range - throw csm::Error( - csm::Error::INDEX_OUT_OF_RANGE, - "Index out of range.", - "UsgsAstroLsSensorModel::getParameterSharingCriteria"); + int index) const { + MESSAGE_LOG( + "Checking sharing criteria for parameter {}. " + "Sharing is not supported, throwing exception", + index); + // Parameter sharing is not supported for this sensor, + // all indices are out of range + throw csm::Error(csm::Error::INDEX_OUT_OF_RANGE, "Index out of range.", + "UsgsAstroLsSensorModel::getParameterSharingCriteria"); } //*************************************************************************** // UsgsAstroLsSensorModel::getSensorType //*************************************************************************** -std::string UsgsAstroLsSensorModel::getSensorType() const -{ - return CSM_SENSOR_TYPE_EO; +std::string UsgsAstroLsSensorModel::getSensorType() const { + return CSM_SENSOR_TYPE_EO; } //*************************************************************************** // UsgsAstroLsSensorModel::getSensorMode //*************************************************************************** -std::string UsgsAstroLsSensorModel::getSensorMode() const -{ - return CSM_SENSOR_MODE_PB; +std::string UsgsAstroLsSensorModel::getSensorMode() const { + return CSM_SENSOR_MODE_PB; } //*************************************************************************** // UsgsAstroLsSensorModel::getVersion //*************************************************************************** -csm::Version UsgsAstroLsSensorModel::getVersion() const -{ - return csm::Version(1, 0, 0); +csm::Version UsgsAstroLsSensorModel::getVersion() const { + return csm::Version(1, 0, 0); } //*************************************************************************** // UsgsAstroLineScannerSensorModel::getEllipsoid //*************************************************************************** -csm::Ellipsoid UsgsAstroLsSensorModel::getEllipsoid() const -{ - return csm::Ellipsoid(m_majorAxis, m_minorAxis); +csm::Ellipsoid UsgsAstroLsSensorModel::getEllipsoid() const { + return csm::Ellipsoid(m_majorAxis, m_minorAxis); } -void UsgsAstroLsSensorModel::setEllipsoid( - const csm::Ellipsoid &ellipsoid) -{ - m_majorAxis = ellipsoid.getSemiMajorRadius(); - m_minorAxis = ellipsoid.getSemiMinorRadius(); +void UsgsAstroLsSensorModel::setEllipsoid(const csm::Ellipsoid& ellipsoid) { + m_majorAxis = ellipsoid.getSemiMajorRadius(); + m_minorAxis = ellipsoid.getSemiMinorRadius(); } //*************************************************************************** // UsgsAstroLineScannerSensorModel::getValue //*************************************************************************** double UsgsAstroLsSensorModel::getValue( - int index, - const std::vector &adjustments) const -{ - return m_currentParameterValue[index] + adjustments[index]; + int index, const std::vector& adjustments) const { + return m_currentParameterValue[index] + adjustments[index]; } - //*************************************************************************** // Functions pulled out of losToEcf and computeViewingPixel // ************************************************************************** -void UsgsAstroLsSensorModel::getQuaternions(const double& time, double q[4]) const{ +void UsgsAstroLsSensorModel::getQuaternions(const double& time, + double q[4]) const { int nOrder = 8; - if (m_platformFlag == 0) - nOrder = 4; + if (m_platformFlag == 0) nOrder = 4; int nOrderQuat = nOrder; - if (m_numQuaternions < 6 && nOrder == 8) - nOrderQuat = 4; + if (m_numQuaternions < 6 && nOrder == 8) nOrderQuat = 4; - MESSAGE_LOG("Calculating getQuaternions for time {} with {}" - "order lagrange", - time, nOrder) - lagrangeInterp( - m_numQuaternions/4, &m_quaternions[0], m_t0Quat, m_dtQuat, time, 4, nOrderQuat, q); + MESSAGE_LOG( + "Calculating getQuaternions for time {} with {}" + "order lagrange", + time, nOrder) + lagrangeInterp(m_numQuaternions / 4, &m_quaternions[0], m_t0Quat, m_dtQuat, + time, 4, nOrderQuat, q); } //*************************************************************************** // UsgsAstroLineScannerSensorModel::calculateAttitudeCorrection //*************************************************************************** void UsgsAstroLsSensorModel::calculateAttitudeCorrection( - const double& time, - const std::vector& adj, - double attCorr[9]) const -{ - MESSAGE_LOG("Computing calculateAttitudeCorrection (with adjustment)" - "for time {}", time) + const double& time, const std::vector& adj, + double attCorr[9]) const { + MESSAGE_LOG( + "Computing calculateAttitudeCorrection (with adjustment)" + "for time {}", + time) double aTime = time - m_t0Quat; double euler[3]; double nTime = aTime / m_halfTime; double nTime2 = nTime * nTime; - euler[0] = - (getValue(6, adj) + getValue(9, adj)* nTime + getValue(12, adj)* nTime2) / m_flyingHeight; - euler[1] = - (getValue(7, adj) + getValue(10, adj)* nTime + getValue(13, adj)* nTime2) / m_flyingHeight; - euler[2] = - (getValue(8, adj) + getValue(11, adj)* nTime + getValue(14, adj)* nTime2) / m_halfSwath; - MESSAGE_LOG("calculateAttitudeCorrection: euler {} {} {}", - euler[0], euler[1], euler[2]) + euler[0] = (getValue(6, adj) + getValue(9, adj) * nTime + + getValue(12, adj) * nTime2) / + m_flyingHeight; + euler[1] = (getValue(7, adj) + getValue(10, adj) * nTime + + getValue(13, adj) * nTime2) / + m_flyingHeight; + euler[2] = (getValue(8, adj) + getValue(11, adj) * nTime + + getValue(14, adj) * nTime2) / + m_halfSwath; + MESSAGE_LOG("calculateAttitudeCorrection: euler {} {} {}", euler[0], euler[1], + euler[2]) calculateRotationMatrixFromEuler(euler, attCorr); } - //*************************************************************************** // UsgsAstroLsSensorModel::losToEcf //*************************************************************************** void UsgsAstroLsSensorModel::losToEcf( - const double& line, // CSM image convention - const double& sample, // UL pixel center == (0.5, 0.5) - const std::vector& adj, // Parameter Adjustments for partials - double& xc, // output sensor x coordinate - double& yc, // output sensor y coordinate - double& zc, // output sensor z coordinate - double& vx, // output sensor x velocity - double& vy, // output sensor y velocity - double& vz, // output sensor z velocity - double& bodyLookX, // output line-of-sight x coordinate - double& bodyLookY, // output line-of-sight y coordinate - double& bodyLookZ) const // output line-of-sight z coordinate -{ - //# private_func_description - // Computes image ray (look vector) in ecf coordinate system. - // Compute adjusted sensor position and velocity - MESSAGE_LOG("Computing losToEcf (with adjustments) for" - "line {} sample {}", - line, sample) - - double time = getImageTime(csm::ImageCoord(line, sample)); - getAdjSensorPosVel(time, adj, xc, yc, zc, vx, vy, vz); - // CSM image image convention: UL pixel center == (0.5, 0.5) - // USGS image convention: UL pixel center == (1.0, 1.0) - double sampleCSMFull = sample; - double sampleUSGSFull = sampleCSMFull; - - // Compute distorted image coordinates in mm (sample, line on image (pixels) -> focal plane - double distortedFocalPlaneX, distortedFocalPlaneY; - computeDistortedFocalPlaneCoordinates( - 0.0, sampleUSGSFull, - m_detectorSampleOrigin, m_detectorLineOrigin, - m_detectorSampleSumming, m_detectorLineSumming, - m_startingDetectorSample, m_startingDetectorLine, - m_iTransS, m_iTransL, - distortedFocalPlaneX, distortedFocalPlaneY); - MESSAGE_LOG("losToEcf: distorted focal plane coordinate {} {}", - distortedFocalPlaneX, distortedFocalPlaneY) - - // Remove lens - double undistortedFocalPlaneX, undistortedFocalPlaneY; - removeDistortion(distortedFocalPlaneX, distortedFocalPlaneY, - undistortedFocalPlaneX, undistortedFocalPlaneY, - m_opticalDistCoeffs, - m_distortionType); - MESSAGE_LOG("losToEcf: undistorted focal plane coordinate {} {}", - undistortedFocalPlaneX, undistortedFocalPlaneY) + const double& line, // CSM image convention + const double& sample, // UL pixel center == (0.5, 0.5) + const std::vector& adj, // Parameter Adjustments for partials + double& xc, // output sensor x coordinate + double& yc, // output sensor y coordinate + double& zc, // output sensor z coordinate + double& vx, // output sensor x velocity + double& vy, // output sensor y velocity + double& vz, // output sensor z velocity + double& bodyLookX, // output line-of-sight x coordinate + double& bodyLookY, // output line-of-sight y coordinate + double& bodyLookZ) const // output line-of-sight z coordinate +{ + //# private_func_description + // Computes image ray (look vector) in ecf coordinate system. + // Compute adjusted sensor position and velocity + MESSAGE_LOG( + "Computing losToEcf (with adjustments) for" + "line {} sample {}", + line, sample) + + double time = getImageTime(csm::ImageCoord(line, sample)); + getAdjSensorPosVel(time, adj, xc, yc, zc, vx, vy, vz); + // CSM image image convention: UL pixel center == (0.5, 0.5) + // USGS image convention: UL pixel center == (1.0, 1.0) + double sampleCSMFull = sample; + double sampleUSGSFull = sampleCSMFull; + + // Compute distorted image coordinates in mm (sample, line on image (pixels) + // -> focal plane + double distortedFocalPlaneX, distortedFocalPlaneY; + computeDistortedFocalPlaneCoordinates( + 0.0, sampleUSGSFull, m_detectorSampleOrigin, m_detectorLineOrigin, + m_detectorSampleSumming, m_detectorLineSumming, m_startingDetectorSample, + m_startingDetectorLine, m_iTransS, m_iTransL, distortedFocalPlaneX, + distortedFocalPlaneY); + MESSAGE_LOG("losToEcf: distorted focal plane coordinate {} {}", + distortedFocalPlaneX, distortedFocalPlaneY) + + // Remove lens + double undistortedFocalPlaneX, undistortedFocalPlaneY; + removeDistortion(distortedFocalPlaneX, distortedFocalPlaneY, + undistortedFocalPlaneX, undistortedFocalPlaneY, + m_opticalDistCoeffs, m_distortionType); + MESSAGE_LOG("losToEcf: undistorted focal plane coordinate {} {}", + undistortedFocalPlaneX, undistortedFocalPlaneY) // Define imaging ray (look vector) in camera space - double cameraLook[3]; - createCameraLookVector(undistortedFocalPlaneX, undistortedFocalPlaneY, - m_zDirection, m_focalLength * (1 - getValue(15, adj) / m_halfSwath), - cameraLook); - MESSAGE_LOG("losToEcf: uncorrected camera look vector {} {} {}", - cameraLook[0], cameraLook[1], cameraLook[2]) - - // Apply attitude correction - double attCorr[9]; - calculateAttitudeCorrection(time, adj, attCorr); - - double correctedCameraLook[3]; - correctedCameraLook[0] = attCorr[0] * cameraLook[0] - + attCorr[1] * cameraLook[1] - + attCorr[2] * cameraLook[2]; - correctedCameraLook[1] = attCorr[3] * cameraLook[0] - + attCorr[4] * cameraLook[1] - + attCorr[5] * cameraLook[2]; - correctedCameraLook[2] = attCorr[6] * cameraLook[0] - + attCorr[7] * cameraLook[1] - + attCorr[8] * cameraLook[2]; - MESSAGE_LOG("losToEcf: corrected camera look vector {} {} {}", - correctedCameraLook[0], correctedCameraLook[1], - correctedCameraLook[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(quaternions, cameraToBody); - - bodyLookX = cameraToBody[0] * correctedCameraLook[0] - + cameraToBody[1] * correctedCameraLook[1] - + cameraToBody[2] * correctedCameraLook[2]; - bodyLookY = cameraToBody[3] * correctedCameraLook[0] - + cameraToBody[4] * correctedCameraLook[1] - + cameraToBody[5] * correctedCameraLook[2]; - bodyLookZ = cameraToBody[6] * correctedCameraLook[0] - + cameraToBody[7] * correctedCameraLook[1] - + cameraToBody[8] * correctedCameraLook[2]; - MESSAGE_LOG("losToEcf: body look vector {} {} {}", - bodyLookX, bodyLookY, bodyLookZ) - + double cameraLook[3]; + createCameraLookVector( + undistortedFocalPlaneX, undistortedFocalPlaneY, m_zDirection, + m_focalLength * (1 - getValue(15, adj) / m_halfSwath), cameraLook); + MESSAGE_LOG("losToEcf: uncorrected camera look vector {} {} {}", + cameraLook[0], cameraLook[1], cameraLook[2]) + + // Apply attitude correction + double attCorr[9]; + calculateAttitudeCorrection(time, adj, attCorr); + + double correctedCameraLook[3]; + correctedCameraLook[0] = attCorr[0] * cameraLook[0] + + attCorr[1] * cameraLook[1] + + attCorr[2] * cameraLook[2]; + correctedCameraLook[1] = attCorr[3] * cameraLook[0] + + attCorr[4] * cameraLook[1] + + attCorr[5] * cameraLook[2]; + correctedCameraLook[2] = attCorr[6] * cameraLook[0] + + attCorr[7] * cameraLook[1] + + attCorr[8] * cameraLook[2]; + MESSAGE_LOG("losToEcf: corrected camera look vector {} {} {}", + correctedCameraLook[0], correctedCameraLook[1], + correctedCameraLook[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(quaternions, cameraToBody); + + bodyLookX = cameraToBody[0] * correctedCameraLook[0] + + cameraToBody[1] * correctedCameraLook[1] + + cameraToBody[2] * correctedCameraLook[2]; + bodyLookY = cameraToBody[3] * correctedCameraLook[0] + + cameraToBody[4] * correctedCameraLook[1] + + cameraToBody[5] * correctedCameraLook[2]; + bodyLookZ = cameraToBody[6] * correctedCameraLook[0] + + cameraToBody[7] * correctedCameraLook[1] + + cameraToBody[8] * correctedCameraLook[2]; + MESSAGE_LOG("losToEcf: body look vector {} {} {}", bodyLookX, bodyLookY, + bodyLookZ) } - //*************************************************************************** // UsgsAstroLsSensorModel::lightAberrationCorr //************************************************************************** void UsgsAstroLsSensorModel::lightAberrationCorr( - const double& vx, - const double& vy, - const double& vz, - const double& xl, - const double& yl, - const double& zl, - double& dxl, - double& dyl, - double& dzl) const -{ - MESSAGE_LOG("Computing lightAberrationCorr for camera velocity" - "{} {} {} and image ray {} {} {}", - vx, vy, vz, xl, yl, zl) - //# func_description - // Computes light aberration correction vector - - // Compute angle between the image ray and the velocity vector - - double dotP = xl * vx + yl * vy + zl * vz; - double losMag = sqrt(xl * xl + yl * yl + zl * zl); - double velocityMag = sqrt(vx * vx + vy * vy + vz * vz); - double cosThetap = dotP / (losMag * velocityMag); - double sinThetap = sqrt(1.0 - cosThetap * cosThetap); - - // Image ray is parallel to the velocity vector - - if (1.0 == fabs(cosThetap)) - { - dxl = 0.0; - dyl = 0.0; - dzl = 0.0; - MESSAGE_LOG("lightAberrationCorr: image ray is parallel" - "to velocity vector") - } + const double& vx, const double& vy, const double& vz, const double& xl, + const double& yl, const double& zl, double& dxl, double& dyl, + double& dzl) const { + MESSAGE_LOG( + "Computing lightAberrationCorr for camera velocity" + "{} {} {} and image ray {} {} {}", + vx, vy, vz, xl, yl, zl) + //# func_description + // Computes light aberration correction vector + + // Compute angle between the image ray and the velocity vector + + double dotP = xl * vx + yl * vy + zl * vz; + double losMag = sqrt(xl * xl + yl * yl + zl * zl); + double velocityMag = sqrt(vx * vx + vy * vy + vz * vz); + double cosThetap = dotP / (losMag * velocityMag); + double sinThetap = sqrt(1.0 - cosThetap * cosThetap); + + // Image ray is parallel to the velocity vector + + if (1.0 == fabs(cosThetap)) { + dxl = 0.0; + dyl = 0.0; + dzl = 0.0; + MESSAGE_LOG( + "lightAberrationCorr: image ray is parallel" + "to velocity vector") + } - // Compute the angle between the corrected image ray and spacecraft - // velocity. This key equation is derived using Lorentz transform. + // Compute the angle between the corrected image ray and spacecraft + // velocity. This key equation is derived using Lorentz transform. - double speedOfLight = 299792458.0; // meters per second - double beta = velocityMag / speedOfLight; - double cosTheta = (beta - cosThetap) / (beta * cosThetap - 1.0); - double sinTheta = sqrt(1.0 - cosTheta * cosTheta); + double speedOfLight = 299792458.0; // meters per second + double beta = velocityMag / speedOfLight; + double cosTheta = (beta - cosThetap) / (beta * cosThetap - 1.0); + double sinTheta = sqrt(1.0 - cosTheta * cosTheta); - // Compute line-of-sight correction + // Compute line-of-sight correction - double cfac = ((cosTheta * sinThetap - - sinTheta * cosThetap) * losMag) - / (sinTheta * velocityMag); - dxl = cfac * vx; - dyl = cfac * vy; - dzl = cfac * vz; - MESSAGE_LOG("lightAberrationCorr: light of sight correction" - "{} {} {}", dxl, dyl, dzl) + double cfac = ((cosTheta * sinThetap - sinTheta * cosThetap) * losMag) / + (sinTheta * velocityMag); + dxl = cfac * vx; + dyl = cfac * vy; + dzl = cfac * vz; + MESSAGE_LOG( + "lightAberrationCorr: light of sight correction" + "{} {} {}", + dxl, dyl, dzl) } //*************************************************************************** // UsgsAstroLsSensorModel::losEllipsoidIntersect //************************************************************************** void UsgsAstroLsSensorModel::losEllipsoidIntersect( - const double& height, - const double& xc, - const double& yc, - const double& zc, - const double& xl, - const double& yl, - const double& zl, - double& x, - double& y, - double& z, - double& achieved_precision, - const double& desired_precision) const -{ - MESSAGE_LOG("Computing losEllipsoidIntersect for camera position " - "{} {} {} looking {} {} {} with desired precision" - "{}", - xc, yc, zc, xl, yl, zl, desired_precision) - // Helper function which computes the intersection of the image ray - // with the ellipsoid. All vectors are in earth-centered-fixed - // coordinate system with origin at the center of the earth. - - const int MKTR = 10; - - double ap, bp, k; - ap = m_majorAxis + height; - bp = m_minorAxis + height; - k = ap * ap / (bp * bp); - - // Solve quadratic equation for scale factor - // applied to image ray to compute ground point - - double at, bt, ct, quadTerm; - at = xl * xl + yl * yl + k * zl * zl; - bt = 2.0 * (xl * xc + yl * yc + k * zl * zc); - ct = xc * xc + yc * yc + k * zc * zc - ap * ap; - quadTerm = bt * bt - 4.0 * at * ct; - - // If quadTerm is negative, the image ray does not - // intersect the ellipsoid. Setting the quadTerm to - // zero means solving for a point on the ray nearest - // the surface of the ellisoid. - - if (0.0 > quadTerm) - { - quadTerm = 0.0; - } - double scale, scale1, h, slope; - double sprev, hprev; - double sTerm; - int ktr = 0; - - // Compute ground point vector - - sTerm = sqrt(quadTerm); - scale = (-bt - sTerm); - scale1 = (-bt + sTerm); - if (fabs(scale1) < fabs(scale)) - scale = scale1; - scale /= (2.0 * at); - x = xc + scale * xl; - y = yc + scale * yl; - z = zc + scale * zl; - h = computeEllipsoidElevation(x, y, z, m_majorAxis, m_minorAxis, desired_precision); - slope = -1; - - achieved_precision = fabs(height - h); - MESSAGE_LOG("losEllipsoidIntersect: found intersect at {} {} {}" - "with achieved precision of {}", - x, y, z, achieved_precision) + const double& height, const double& xc, const double& yc, const double& zc, + const double& xl, const double& yl, const double& zl, double& x, double& y, + double& z, double& achieved_precision, + const double& desired_precision) const { + MESSAGE_LOG( + "Computing losEllipsoidIntersect for camera position " + "{} {} {} looking {} {} {} with desired precision" + "{}", + xc, yc, zc, xl, yl, zl, desired_precision) + // Helper function which computes the intersection of the image ray + // with the ellipsoid. All vectors are in earth-centered-fixed + // coordinate system with origin at the center of the earth. + + const int MKTR = 10; + + double ap, bp, k; + ap = m_majorAxis + height; + bp = m_minorAxis + height; + k = ap * ap / (bp * bp); + + // Solve quadratic equation for scale factor + // applied to image ray to compute ground point + + double at, bt, ct, quadTerm; + at = xl * xl + yl * yl + k * zl * zl; + bt = 2.0 * (xl * xc + yl * yc + k * zl * zc); + ct = xc * xc + yc * yc + k * zc * zc - ap * ap; + quadTerm = bt * bt - 4.0 * at * ct; + + // If quadTerm is negative, the image ray does not + // intersect the ellipsoid. Setting the quadTerm to + // zero means solving for a point on the ray nearest + // the surface of the ellisoid. + + if (0.0 > quadTerm) { + quadTerm = 0.0; + } + double scale, scale1, h, slope; + double sprev, hprev; + double sTerm; + int ktr = 0; + + // Compute ground point vector + + sTerm = sqrt(quadTerm); + scale = (-bt - sTerm); + scale1 = (-bt + sTerm); + if (fabs(scale1) < fabs(scale)) scale = scale1; + scale /= (2.0 * at); + x = xc + scale * xl; + y = yc + scale * yl; + z = zc + scale * zl; + h = computeEllipsoidElevation(x, y, z, m_majorAxis, m_minorAxis, + desired_precision); + slope = -1; + + achieved_precision = fabs(height - h); + MESSAGE_LOG( + "losEllipsoidIntersect: found intersect at {} {} {}" + "with achieved precision of {}", + x, y, z, achieved_precision) } //*************************************************************************** // UsgsAstroLsSensorModel::losPlaneIntersect //************************************************************************** void UsgsAstroLsSensorModel::losPlaneIntersect( - const double& xc, // input: camera x coordinate - const double& yc, // input: camera y coordinate - const double& zc, // input: camera z coordinate - const double& xl, // input: component x image ray - const double& yl, // input: component y image ray - const double& zl, // input: component z image ray - double& x, // input/output: ground x coordinate - double& y, // input/output: ground y coordinate - double& z, // input/output: ground z coordinate - int& mode) const // input: -1 fixed component to be computed - // 0(X), 1(Y), or 2(Z) fixed - // output: 0(X), 1(Y), or 2(Z) fixed -{ - MESSAGE_LOG("Calculating losPlaneIntersect for camera position" - "{} {} {} and image ray {} {} {}", - xc, yc, zc, xl, yl, zl) - //# func_description - // Computes 2 of the 3 coordinates of a ground point given the 3rd - // coordinate. The 3rd coordinate that is held fixed corresponds - // to the largest absolute component of the image ray. - - // Define fixed or largest component - - if (-1 == mode) - { - if (fabs(xl) > fabs(yl) && fabs(xl) > fabs(zl)) - { - mode = 0; - } - else if (fabs(yl) > fabs(xl) && fabs(yl) > fabs(zl)) - { - mode = 1; - } - else - { - mode = 2; - } - } - MESSAGE_LOG("losPlaneIntersect: largest/fixed image ray component" - "{} (1-x, 2-y, 3-z)", mode) - // X is the fixed or largest component - - if (0 == mode) - { - y = yc + (x - xc) * yl / xl; - z = zc + (x - xc) * zl / xl; - } + const double& xc, // input: camera x coordinate + const double& yc, // input: camera y coordinate + const double& zc, // input: camera z coordinate + const double& xl, // input: component x image ray + const double& yl, // input: component y image ray + const double& zl, // input: component z image ray + double& x, // input/output: ground x coordinate + double& y, // input/output: ground y coordinate + double& z, // input/output: ground z coordinate + int& mode) const // input: -1 fixed component to be computed + // 0(X), 1(Y), or 2(Z) fixed + // output: 0(X), 1(Y), or 2(Z) fixed +{ + MESSAGE_LOG( + "Calculating losPlaneIntersect for camera position" + "{} {} {} and image ray {} {} {}", + xc, yc, zc, xl, yl, zl) + //# func_description + // Computes 2 of the 3 coordinates of a ground point given the 3rd + // coordinate. The 3rd coordinate that is held fixed corresponds + // to the largest absolute component of the image ray. + + // Define fixed or largest component + + if (-1 == mode) { + if (fabs(xl) > fabs(yl) && fabs(xl) > fabs(zl)) { + mode = 0; + } else if (fabs(yl) > fabs(xl) && fabs(yl) > fabs(zl)) { + mode = 1; + } else { + mode = 2; + } + } + MESSAGE_LOG( + "losPlaneIntersect: largest/fixed image ray component" + "{} (1-x, 2-y, 3-z)", + mode) + // X is the fixed or largest component + + if (0 == mode) { + y = yc + (x - xc) * yl / xl; + z = zc + (x - xc) * zl / xl; + } - // Y is the fixed or largest component + // Y is the fixed or largest component - else if (1 == mode) - { - x = xc + (y - yc) * xl / yl; - z = zc + (y - yc) * zl / yl; - } + else if (1 == mode) { + x = xc + (y - yc) * xl / yl; + z = zc + (y - yc) * zl / yl; + } - // Z is the fixed or largest component + // Z is the fixed or largest component - else - { - x = xc + (z - zc) * xl / zl; - y = yc + (z - zc) * yl / zl; - } - MESSAGE_LOG("ground coordinate {} {} {}", x, y, z) + else { + x = xc + (z - zc) * xl / zl; + y = yc + (z - zc) * yl / zl; + } + MESSAGE_LOG("ground coordinate {} {} {}", x, y, z) } //*************************************************************************** // UsgsAstroLsSensorModel::imageToPlane //*************************************************************************** void UsgsAstroLsSensorModel::imageToPlane( - const double& line, // CSM Origin UL corner of UL pixel - const double& sample, // CSM Origin UL corner of UL pixel - const double& height, - const std::vector &adj, - double& x, - double& y, - double& z, - int& mode) const -{ + const double& line, // CSM Origin UL corner of UL pixel + const double& sample, // CSM Origin UL corner of UL pixel + const double& height, const std::vector& adj, double& x, double& y, + double& z, int& mode) const { MESSAGE_LOG("Computing imageToPlane") - //# func_description - // Computes ground coordinates by intersecting image ray with - // a plane perpendicular to the coordinate axis with the largest - // image ray component. This routine is primarily called by - // groundToImage(). + //# func_description + // Computes ground coordinates by intersecting image ray with + // a plane perpendicular to the coordinate axis with the largest + // image ray component. This routine is primarily called by + // groundToImage(). - // *** Computes camera position and image ray in ecf cs. + // *** Computes camera position and image ray in ecf cs. - double xc, yc, zc; - double vx, vy, vz; - double xl, yl, zl; - double dxl, dyl, dzl; + double xc, yc, zc; + double vx, vy, vz; + double xl, yl, zl; + double dxl, dyl, dzl; - losToEcf(line, sample, adj, xc, yc, zc, vx, vy, vz, xl, yl, zl); + losToEcf(line, sample, adj, xc, yc, zc, vx, vy, vz, xl, yl, zl); - losPlaneIntersect(xc, yc, zc, xl, yl, zl, x, y, z, mode); + losPlaneIntersect(xc, yc, zc, xl, yl, zl, x, y, z, mode); } //*************************************************************************** // UsgsAstroLineScannerSensorModel::getAdjSensorPosVel //*************************************************************************** -void UsgsAstroLsSensorModel::getAdjSensorPosVel( - const double& time, - const std::vector &adj, - double& xc, - double& yc, - double& zc, - double& vx, - double& vy, - double& vz) const -{ - MESSAGE_LOG("Calculating getAdjSensorPosVel at time {}", - time) - - // Sensor position and velocity (4th or 8th order Lagrange). - int nOrder = 8; - if (m_platformFlag == 0) - nOrder = 4; - double sensPosNom[3]; - lagrangeInterp(m_numPositions/3, &m_positions[0], m_t0Ephem, m_dtEphem, - time, 3, nOrder, sensPosNom); - double sensVelNom[3]; - lagrangeInterp(m_numPositions/3, &m_velocities[0], m_t0Ephem, m_dtEphem, - time, 3, nOrder, sensVelNom); - - MESSAGE_LOG("getAdjSensorPosVel: using {} order Lagrange", - nOrder) - - // Compute rotation matrix from ICR to ECF - double radialUnitVec[3]; - double radMag = sqrt(sensPosNom[0] * sensPosNom[0] + - sensPosNom[1] * sensPosNom[1] + - sensPosNom[2] * sensPosNom[2]); - for (int i = 0; i < 3; i++) - radialUnitVec[i] = sensPosNom[i] / radMag; - double crossTrackUnitVec[3]; - crossTrackUnitVec[0] = sensPosNom[1] * sensVelNom[2] - - sensPosNom[2] * sensVelNom[1]; - crossTrackUnitVec[1] = sensPosNom[2] * sensVelNom[0] - - sensPosNom[0] * sensVelNom[2]; - crossTrackUnitVec[2] = sensPosNom[0] * sensVelNom[1] - - sensPosNom[1] * sensVelNom[0]; - double crossMag = sqrt(crossTrackUnitVec[0] * crossTrackUnitVec[0] + - crossTrackUnitVec[1] * crossTrackUnitVec[1] + - crossTrackUnitVec[2] * crossTrackUnitVec[2]); - for (int i = 0; i < 3; i++) - crossTrackUnitVec[i] /= crossMag; - double inTrackUnitVec[3]; - inTrackUnitVec[0] = crossTrackUnitVec[1] * radialUnitVec[2] - - crossTrackUnitVec[2] * radialUnitVec[1]; - inTrackUnitVec[1] = crossTrackUnitVec[2] * radialUnitVec[0] - - crossTrackUnitVec[0] * radialUnitVec[2]; - inTrackUnitVec[2] = crossTrackUnitVec[0] * radialUnitVec[1] - - crossTrackUnitVec[1] * radialUnitVec[0]; - double ecfFromIcr[9]; - ecfFromIcr[0] = inTrackUnitVec[0]; - ecfFromIcr[1] = crossTrackUnitVec[0]; - ecfFromIcr[2] = radialUnitVec[0]; - ecfFromIcr[3] = inTrackUnitVec[1]; - ecfFromIcr[4] = crossTrackUnitVec[1]; - ecfFromIcr[5] = radialUnitVec[1]; - ecfFromIcr[6] = inTrackUnitVec[2]; - ecfFromIcr[7] = crossTrackUnitVec[2]; - ecfFromIcr[8] = radialUnitVec[2]; - - // Apply position and velocity corrections - double aTime = time - m_t0Ephem; - double dvi = getValue(3, adj) / m_halfTime; - double dvc = getValue(4, adj) / m_halfTime; - double dvr = getValue(5, adj) / m_halfTime; - vx = sensVelNom[0] - + ecfFromIcr[0] * dvi + ecfFromIcr[1] * dvc + ecfFromIcr[2] * dvr; - vy = sensVelNom[1] - + ecfFromIcr[3] * dvi + ecfFromIcr[4] * dvc + ecfFromIcr[5] * dvr; - vz = sensVelNom[2] - + ecfFromIcr[6] * dvi + ecfFromIcr[7] * dvc + ecfFromIcr[8] * dvr; - double di = getValue(0, adj) + dvi * aTime; - double dc = getValue(1, adj) + dvc * aTime; - double dr = getValue(2, adj) + dvr * aTime; - xc = sensPosNom[0] - + ecfFromIcr[0] * di + ecfFromIcr[1] * dc + ecfFromIcr[2] * dr; - yc = sensPosNom[1] - + ecfFromIcr[3] * di + ecfFromIcr[4] * dc + ecfFromIcr[5] * dr; - zc = sensPosNom[2] - + ecfFromIcr[6] * di + ecfFromIcr[7] * dc + ecfFromIcr[8] * dr; - - MESSAGE_LOG("getAdjSensorPosVel: postition {} {} {}" - "and velocity {} {} {}", - xc, yc, zc, vx, vy, vz) -} +void UsgsAstroLsSensorModel::getAdjSensorPosVel(const double& time, + const std::vector& adj, + double& xc, double& yc, + double& zc, double& vx, + double& vy, double& vz) const { + MESSAGE_LOG("Calculating getAdjSensorPosVel at time {}", time) + // Sensor position and velocity (4th or 8th order Lagrange). + int nOrder = 8; + if (m_platformFlag == 0) nOrder = 4; + double sensPosNom[3]; + lagrangeInterp(m_numPositions / 3, &m_positions[0], m_t0Ephem, m_dtEphem, + time, 3, nOrder, sensPosNom); + double sensVelNom[3]; + lagrangeInterp(m_numPositions / 3, &m_velocities[0], m_t0Ephem, m_dtEphem, + time, 3, nOrder, sensVelNom); + + MESSAGE_LOG("getAdjSensorPosVel: using {} order Lagrange", nOrder) + + // Compute rotation matrix from ICR to ECF + double radialUnitVec[3]; + double radMag = + sqrt(sensPosNom[0] * sensPosNom[0] + sensPosNom[1] * sensPosNom[1] + + sensPosNom[2] * sensPosNom[2]); + for (int i = 0; i < 3; i++) radialUnitVec[i] = sensPosNom[i] / radMag; + double crossTrackUnitVec[3]; + crossTrackUnitVec[0] = + sensPosNom[1] * sensVelNom[2] - sensPosNom[2] * sensVelNom[1]; + crossTrackUnitVec[1] = + sensPosNom[2] * sensVelNom[0] - sensPosNom[0] * sensVelNom[2]; + crossTrackUnitVec[2] = + sensPosNom[0] * sensVelNom[1] - sensPosNom[1] * sensVelNom[0]; + double crossMag = sqrt(crossTrackUnitVec[0] * crossTrackUnitVec[0] + + crossTrackUnitVec[1] * crossTrackUnitVec[1] + + crossTrackUnitVec[2] * crossTrackUnitVec[2]); + for (int i = 0; i < 3; i++) crossTrackUnitVec[i] /= crossMag; + double inTrackUnitVec[3]; + inTrackUnitVec[0] = crossTrackUnitVec[1] * radialUnitVec[2] - + crossTrackUnitVec[2] * radialUnitVec[1]; + inTrackUnitVec[1] = crossTrackUnitVec[2] * radialUnitVec[0] - + crossTrackUnitVec[0] * radialUnitVec[2]; + inTrackUnitVec[2] = crossTrackUnitVec[0] * radialUnitVec[1] - + crossTrackUnitVec[1] * radialUnitVec[0]; + double ecfFromIcr[9]; + ecfFromIcr[0] = inTrackUnitVec[0]; + ecfFromIcr[1] = crossTrackUnitVec[0]; + ecfFromIcr[2] = radialUnitVec[0]; + ecfFromIcr[3] = inTrackUnitVec[1]; + ecfFromIcr[4] = crossTrackUnitVec[1]; + ecfFromIcr[5] = radialUnitVec[1]; + ecfFromIcr[6] = inTrackUnitVec[2]; + ecfFromIcr[7] = crossTrackUnitVec[2]; + ecfFromIcr[8] = radialUnitVec[2]; + + // Apply position and velocity corrections + double aTime = time - m_t0Ephem; + double dvi = getValue(3, adj) / m_halfTime; + double dvc = getValue(4, adj) / m_halfTime; + double dvr = getValue(5, adj) / m_halfTime; + vx = sensVelNom[0] + ecfFromIcr[0] * dvi + ecfFromIcr[1] * dvc + + ecfFromIcr[2] * dvr; + vy = sensVelNom[1] + ecfFromIcr[3] * dvi + ecfFromIcr[4] * dvc + + ecfFromIcr[5] * dvr; + vz = sensVelNom[2] + ecfFromIcr[6] * dvi + ecfFromIcr[7] * dvc + + ecfFromIcr[8] * dvr; + double di = getValue(0, adj) + dvi * aTime; + double dc = getValue(1, adj) + dvc * aTime; + double dr = getValue(2, adj) + dvr * aTime; + xc = sensPosNom[0] + ecfFromIcr[0] * di + ecfFromIcr[1] * dc + + ecfFromIcr[2] * dr; + yc = sensPosNom[1] + ecfFromIcr[3] * di + ecfFromIcr[4] * dc + + ecfFromIcr[5] * dr; + zc = sensPosNom[2] + ecfFromIcr[6] * di + ecfFromIcr[7] * dc + + ecfFromIcr[8] * dr; + + MESSAGE_LOG( + "getAdjSensorPosVel: postition {} {} {}" + "and velocity {} {} {}", + xc, yc, zc, vx, vy, vz) +} //*************************************************************************** // UsgsAstroLineScannerSensorModel::computeDetectorView //*************************************************************************** std::vector UsgsAstroLsSensorModel::computeDetectorView( - const double& time, - const csm::EcefCoord& groundPoint, - const std::vector& adj) const -{ - MESSAGE_LOG("Computing computeDetectorView (with adjusments)" - "for ground point {} {} {} at time {} ", - groundPoint.x, groundPoint.y, groundPoint.z, time) - + const double& time, const csm::EcefCoord& groundPoint, + const std::vector& adj) const { + MESSAGE_LOG( + "Computing computeDetectorView (with adjusments)" + "for ground point {} {} {} at time {} ", + groundPoint.x, groundPoint.y, groundPoint.z, time) // Helper function to compute the CCD pixel that views a ground point based // on the exterior orientation at a given time. - // Get the exterior orientation - double xc, yc, zc, vx, vy, vz; - getAdjSensorPosVel(time, adj, xc, yc, zc, vx, vy, vz); - - // Compute the look vector - double bodyLookX = groundPoint.x - xc; - double bodyLookY = groundPoint.y - yc; - double bodyLookZ = groundPoint.z - zc; - MESSAGE_LOG("computeDetectorView: look vector {} {} {}", - bodyLookX, bodyLookY, bodyLookZ) - - // Rotate the look vector into the camera reference frame - double quaternions[4]; - getQuaternions(time, quaternions); - double bodyToCamera[9]; - calculateRotationMatrixFromQuaternions(quaternions, bodyToCamera); - - // Apply transpose of matrix to rotate body->camera - double cameraLookX = bodyToCamera[0] * bodyLookX - + bodyToCamera[3] * bodyLookY - + bodyToCamera[6] * bodyLookZ; - double cameraLookY = bodyToCamera[1] * bodyLookX - + bodyToCamera[4] * bodyLookY - + bodyToCamera[7] * bodyLookZ; - double cameraLookZ = bodyToCamera[2] * bodyLookX - + bodyToCamera[5] * bodyLookY - + bodyToCamera[8] * bodyLookZ; - MESSAGE_LOG("computeDetectorView: look vector (camrea ref frame)" - "{} {} {}", - cameraLookX, cameraLookY, cameraLookZ) - - // Invert the attitude correction - double attCorr[9]; - calculateAttitudeCorrection(time, adj, attCorr); - - // Apply transpose of matrix to invert the attidue correction - double adjustedLookX = attCorr[0] * cameraLookX - + attCorr[3] * cameraLookY - + attCorr[6] * cameraLookZ; - double adjustedLookY = attCorr[1] * cameraLookX - + attCorr[4] * cameraLookY - + attCorr[7] * cameraLookZ; - double adjustedLookZ = attCorr[2] * cameraLookX - + attCorr[5] * cameraLookY - + attCorr[8] * cameraLookZ; - MESSAGE_LOG("computeDetectorView: adjusted look vector" - "{} {} {}", - adjustedLookX, adjustedLookY, adjustedLookZ) - - // Convert to focal plane coordinate - double lookScale = (m_focalLength + getValue(15, adj)) / adjustedLookZ; - double focalX = adjustedLookX * lookScale; - double focalY = adjustedLookY * lookScale; - - MESSAGE_LOG("computeDetectorView: focal plane coordinates" - "x:{} y:{} scale:{}", - focalX, focalY, lookScale) - - return std::vector {focalX, focalY}; + // Get the exterior orientation + double xc, yc, zc, vx, vy, vz; + getAdjSensorPosVel(time, adj, xc, yc, zc, vx, vy, vz); + + // Compute the look vector + double bodyLookX = groundPoint.x - xc; + double bodyLookY = groundPoint.y - yc; + double bodyLookZ = groundPoint.z - zc; + MESSAGE_LOG("computeDetectorView: look vector {} {} {}", bodyLookX, bodyLookY, + bodyLookZ) + + // Rotate the look vector into the camera reference frame + double quaternions[4]; + getQuaternions(time, quaternions); + double bodyToCamera[9]; + calculateRotationMatrixFromQuaternions(quaternions, bodyToCamera); + + // Apply transpose of matrix to rotate body->camera + double cameraLookX = bodyToCamera[0] * bodyLookX + + bodyToCamera[3] * bodyLookY + + bodyToCamera[6] * bodyLookZ; + double cameraLookY = bodyToCamera[1] * bodyLookX + + bodyToCamera[4] * bodyLookY + + bodyToCamera[7] * bodyLookZ; + double cameraLookZ = bodyToCamera[2] * bodyLookX + + bodyToCamera[5] * bodyLookY + + bodyToCamera[8] * bodyLookZ; + MESSAGE_LOG( + "computeDetectorView: look vector (camrea ref frame)" + "{} {} {}", + cameraLookX, cameraLookY, cameraLookZ) + + // Invert the attitude correction + double attCorr[9]; + calculateAttitudeCorrection(time, adj, attCorr); + + // Apply transpose of matrix to invert the attidue correction + double adjustedLookX = attCorr[0] * cameraLookX + attCorr[3] * cameraLookY + + attCorr[6] * cameraLookZ; + double adjustedLookY = attCorr[1] * cameraLookX + attCorr[4] * cameraLookY + + attCorr[7] * cameraLookZ; + double adjustedLookZ = attCorr[2] * cameraLookX + attCorr[5] * cameraLookY + + attCorr[8] * cameraLookZ; + MESSAGE_LOG( + "computeDetectorView: adjusted look vector" + "{} {} {}", + adjustedLookX, adjustedLookY, adjustedLookZ) + + // Convert to focal plane coordinate + double lookScale = (m_focalLength + getValue(15, adj)) / adjustedLookZ; + double focalX = adjustedLookX * lookScale; + double focalY = adjustedLookY * lookScale; + + MESSAGE_LOG( + "computeDetectorView: focal plane coordinates" + "x:{} y:{} scale:{}", + focalX, focalY, lookScale) + + return std::vector{focalX, focalY}; } - //*************************************************************************** // UsgsAstroLineScannerSensorModel::computeLinearApproximation //*************************************************************************** void UsgsAstroLsSensorModel::computeLinearApproximation( - const csm::EcefCoord &gp, - csm::ImageCoord &ip) const -{ - if (_linear) - { - ip.line = _u0 + _du_dx * gp.x + _du_dy * gp.y + _du_dz * gp.z; - ip.samp = _v0 + _dv_dx * gp.x + _dv_dy * gp.y + _dv_dz * gp.z; - - // Since this is valid only over image, - // don't let result go beyond the image border. - double numRows = m_nLines; - double numCols = m_nSamples; - if (ip.line < 0.0) ip.line = 0.0; - if (ip.line > numRows) ip.line = numRows; - - if (ip.samp < 0.0) ip.samp = 0.0; - if (ip.samp > numCols) ip.samp = numCols; - MESSAGE_LOG("Computing computeLinearApproximation" - "with linear approximation") - } - else - { - ip.line = m_nLines / 2.0; - ip.samp = m_nSamples / 2.0; - MESSAGE_LOG("Computing computeLinearApproximation" - "nonlinear approx line/2 and sample/2") - } + const csm::EcefCoord& gp, csm::ImageCoord& ip) const { + if (_linear) { + ip.line = _u0 + _du_dx * gp.x + _du_dy * gp.y + _du_dz * gp.z; + ip.samp = _v0 + _dv_dx * gp.x + _dv_dy * gp.y + _dv_dz * gp.z; + + // Since this is valid only over image, + // don't let result go beyond the image border. + double numRows = m_nLines; + double numCols = m_nSamples; + if (ip.line < 0.0) ip.line = 0.0; + if (ip.line > numRows) ip.line = numRows; + + if (ip.samp < 0.0) ip.samp = 0.0; + if (ip.samp > numCols) ip.samp = numCols; + MESSAGE_LOG( + "Computing computeLinearApproximation" + "with linear approximation") + } else { + ip.line = m_nLines / 2.0; + ip.samp = m_nSamples / 2.0; + MESSAGE_LOG( + "Computing computeLinearApproximation" + "nonlinear approx line/2 and sample/2") + } } - //*************************************************************************** // UsgsAstroLineScannerSensorModel::setLinearApproximation //*************************************************************************** -void UsgsAstroLsSensorModel::setLinearApproximation() -{ +void UsgsAstroLsSensorModel::setLinearApproximation() { MESSAGE_LOG("Calculating setLinearApproximation") - double u_factors[4] = { 0.0, 0.0, 1.0, 1.0 }; - double v_factors[4] = { 0.0, 1.0, 0.0, 1.0 }; + double u_factors[4] = {0.0, 0.0, 1.0, 1.0}; + double v_factors[4] = {0.0, 1.0, 0.0, 1.0}; csm::EcefCoord refPt = getReferencePoint(); double desired_precision = 0.01; double height = computeEllipsoidElevation( - refPt.x, refPt.y, refPt.z, - m_majorAxis, m_minorAxis, desired_precision); - if (std::isnan(height)) - { - MESSAGE_LOG("setLinearApproximation: computeElevation of" - "reference point {} {} {} with desired precision" - "{} returned nan height; nonlinear", - refPt.x, refPt.y, refPt.z, desired_precision) + refPt.x, refPt.y, refPt.z, m_majorAxis, m_minorAxis, desired_precision); + if (std::isnan(height)) { + MESSAGE_LOG( + "setLinearApproximation: computeElevation of" + "reference point {} {} {} with desired precision" + "{} returned nan height; nonlinear", + refPt.x, refPt.y, refPt.z, desired_precision) _linear = false; return; } - MESSAGE_LOG("setLinearApproximation: computeElevation of" - "reference point {} {} {} with desired precision" - "{} returned {} height", - refPt.x, refPt.y, refPt.z, desired_precision, height) + MESSAGE_LOG( + "setLinearApproximation: computeElevation of" + "reference point {} {} {} with desired precision" + "{} returned {} height", + refPt.x, refPt.y, refPt.z, desired_precision, height) double numRows = m_nLines; double numCols = m_nSamples; csm::ImageCoord imagePt; - csm::EcefCoord gp[8]; + csm::EcefCoord gp[8]; int i; - for (i = 0; i < 4; i++) - { + for (i = 0; i < 4; i++) { imagePt.line = u_factors[i] * numRows; imagePt.samp = v_factors[i] * numCols; gp[i] = imageToGround(imagePt, height); @@ -2468,34 +2303,33 @@ void UsgsAstroLsSensorModel::setLinearApproximation() double delz = 100.0; height += delz; - for (i = 0; i < 4; i++) - { + for (i = 0; i < 4; i++) { imagePt.line = u_factors[i] * numRows; imagePt.samp = v_factors[i] * numCols; gp[i + 4] = imageToGround(imagePt, height); } csm::EcefCoord d_du; - d_du.x = ( - (gp[2].x + gp[3].x + gp[6].x + gp[7].x) - - (gp[0].x + gp[1].x + gp[4].x + gp[5].x)) / numRows / 4.0; - d_du.y = ( - (gp[2].y + gp[3].y + gp[6].y + gp[7].y) - - (gp[0].y + gp[1].y + gp[4].y + gp[5].y)) / numRows / 4.0; - d_du.z = ( - (gp[2].z + gp[3].z + gp[6].z + gp[7].z) - - (gp[0].z + gp[1].z + gp[4].z + gp[5].z)) / numRows / 4.0; + d_du.x = ((gp[2].x + gp[3].x + gp[6].x + gp[7].x) - + (gp[0].x + gp[1].x + gp[4].x + gp[5].x)) / + numRows / 4.0; + d_du.y = ((gp[2].y + gp[3].y + gp[6].y + gp[7].y) - + (gp[0].y + gp[1].y + gp[4].y + gp[5].y)) / + numRows / 4.0; + d_du.z = ((gp[2].z + gp[3].z + gp[6].z + gp[7].z) - + (gp[0].z + gp[1].z + gp[4].z + gp[5].z)) / + numRows / 4.0; csm::EcefCoord d_dv; - d_dv.x = ( - (gp[1].x + gp[3].x + gp[5].x + gp[7].x) - - (gp[0].x + gp[2].x + gp[4].x + gp[6].x)) / numCols / 4.0; - d_dv.y = ( - (gp[1].y + gp[3].y + gp[5].y + gp[7].y) - - (gp[0].y + gp[2].y + gp[4].y + gp[6].y)) / numCols / 4.0; - d_dv.z = ( - (gp[1].z + gp[3].z + gp[5].z + gp[7].z) - - (gp[0].z + gp[2].z + gp[4].z + gp[6].z)) / numCols / 4.0; + d_dv.x = ((gp[1].x + gp[3].x + gp[5].x + gp[7].x) - + (gp[0].x + gp[2].x + gp[4].x + gp[6].x)) / + numCols / 4.0; + d_dv.y = ((gp[1].y + gp[3].y + gp[5].y + gp[7].y) - + (gp[0].y + gp[2].y + gp[4].y + gp[6].y)) / + numCols / 4.0; + d_dv.z = ((gp[1].z + gp[3].z + gp[5].z + gp[7].z) - + (gp[0].z + gp[2].z + gp[4].z + gp[6].z)) / + numCols / 4.0; double mat3x3[9]; @@ -2511,11 +2345,12 @@ void UsgsAstroLsSensorModel::setLinearApproximation() double denom = determinant3x3(mat3x3); - if (fabs(denom) < 1.0e-8) // can not get derivatives this way + if (fabs(denom) < 1.0e-8) // can not get derivatives this way { - MESSAGE_LOG("setLinearApproximation: determinant3x3 of" - "matrix of partials is {}; nonlinear", - denom) + MESSAGE_LOG( + "setLinearApproximation: determinant3x3 of" + "matrix of partials is {}; nonlinear", + denom) _linear = false; return; } @@ -2564,20 +2399,17 @@ void UsgsAstroLsSensorModel::setLinearApproximation() //*************************************************************************** // UsgsAstroLineScannerSensorModel::determinant3x3 //*************************************************************************** -double UsgsAstroLsSensorModel::determinant3x3(double mat[9]) const -{ - return - mat[0] * (mat[4] * mat[8] - mat[7] * mat[5]) - - mat[1] * (mat[3] * mat[8] - mat[6] * mat[5]) + - mat[2] * (mat[3] * mat[7] - mat[6] * mat[4]); +double UsgsAstroLsSensorModel::determinant3x3(double mat[9]) const { + return mat[0] * (mat[4] * mat[8] - mat[7] * mat[5]) - + mat[1] * (mat[3] * mat[8] - mat[6] * mat[5]) + + mat[2] * (mat[3] * mat[7] - mat[6] * mat[4]); } - //*************************************************************************** // UsgsAstroLineScannerSensorModel::constructStateFromIsd //*************************************************************************** -std::string UsgsAstroLsSensorModel::constructStateFromIsd(const std::string imageSupportData, csm::WarningList *warnings) -{ +std::string UsgsAstroLsSensorModel::constructStateFromIsd( + const std::string imageSupportData, csm::WarningList* warnings) { json state = {}; MESSAGE_LOG("Constructing state from Isd") // Instantiate UsgsAstroLineScanner sensor model @@ -2590,50 +2422,52 @@ std::string UsgsAstroLsSensorModel::constructStateFromIsd(const std::string imag state["m_imageIdentifier"] = ale::getImageId(jsonIsd); state["m_sensorName"] = ale::getSensorName(jsonIsd); state["m_platformName"] = ale::getPlatformName(jsonIsd); - MESSAGE_LOG("m_modelName: {} " - "m_imageIdentifier: {} " - "m_sensorName: {} " - "m_platformName: {} ", - state["m_modelName"].dump(), - state["m_imageIdentifier"].dump(), - state["m_sensorName"].dump(), - state["m_platformName"].dump()) + MESSAGE_LOG( + "m_modelName: {} " + "m_imageIdentifier: {} " + "m_sensorName: {} " + "m_platformName: {} ", + state["m_modelName"].dump(), state["m_imageIdentifier"].dump(), + state["m_sensorName"].dump(), state["m_platformName"].dump()) state["m_focalLength"] = ale::getFocalLength(jsonIsd); MESSAGE_LOG("m_focalLength: {} ", state["m_focalLength"].dump()) state["m_nLines"] = ale::getTotalLines(jsonIsd); state["m_nSamples"] = ale::getTotalSamples(jsonIsd); - MESSAGE_LOG("m_nLines: {} " - "m_nSamples: {} ", - state["m_nLines"].dump(), state["m_nSamples"].dump()) + MESSAGE_LOG( + "m_nLines: {} " + "m_nSamples: {} ", + state["m_nLines"].dump(), state["m_nSamples"].dump()) state["m_iTransS"] = ale::getFocal2PixelSamples(jsonIsd); state["m_iTransL"] = ale::getFocal2PixelLines(jsonIsd); - MESSAGE_LOG("m_iTransS: {} " - "m_iTransL: {} ", - state["m_iTransS"].dump(), state["m_iTransL"].dump()) + MESSAGE_LOG( + "m_iTransS: {} " + "m_iTransL: {} ", + state["m_iTransS"].dump(), state["m_iTransL"].dump()) state["m_platformFlag"] = 1; state["m_ikCode"] = 0; state["m_zDirection"] = 1; - MESSAGE_LOG("m_platformFlag: {} " - "m_ikCode: {} " - "m_zDirection: {} ", - state["m_platformFlag"].dump(), state["m_ikCode"].dump(), - state["m_zDirection"].dump()) - - state["m_distortionType"] = getDistortionModel(ale::getDistortionModel(jsonIsd)); + MESSAGE_LOG( + "m_platformFlag: {} " + "m_ikCode: {} " + "m_zDirection: {} ", + state["m_platformFlag"].dump(), state["m_ikCode"].dump(), + state["m_zDirection"].dump()) + + state["m_distortionType"] = + getDistortionModel(ale::getDistortionModel(jsonIsd)); state["m_opticalDistCoeffs"] = ale::getDistortionCoeffs(jsonIsd); - MESSAGE_LOG("m_distortionType: {} " - "m_opticalDistCoeffs: {} ", - state["m_distortionType"].dump(), - state["m_opticalDistCoeffs"].dump()) + MESSAGE_LOG( + "m_distortionType: {} " + "m_opticalDistCoeffs: {} ", + state["m_distortionType"].dump(), state["m_opticalDistCoeffs"].dump()) // Zero computed state values state["m_referencePointXyz"] = std::vector(3, 0.0); - MESSAGE_LOG("m_referencePointXyz: {} ", - state["m_referencePointXyz"].dump()) + MESSAGE_LOG("m_referencePointXyz: {} ", state["m_referencePointXyz"].dump()) // Sun position and velocity are required for getIlluminationDirection ale::States sunState = ale::getSunPosition(jsonIsd); @@ -2663,30 +2497,34 @@ std::string UsgsAstroLsSensorModel::constructStateFromIsd(const std::string imag state["m_flyingHeight"] = 1000.0; state["m_halfSwath"] = 1000.0; state["m_halfTime"] = 10.0; - MESSAGE_LOG("m_gsd: {} " - "m_flyingHeight: {} " - "m_halfSwath: {} " - "m_halfTime: {} ", - state["m_gsd"].dump(), state["m_flyingHeight"].dump(), - state["m_halfSwath"].dump(), state["m_halfTime"].dump()) + MESSAGE_LOG( + "m_gsd: {} " + "m_flyingHeight: {} " + "m_halfSwath: {} " + "m_halfTime: {} ", + state["m_gsd"].dump(), state["m_flyingHeight"].dump(), + state["m_halfSwath"].dump(), state["m_halfTime"].dump()) state["m_centerEphemerisTime"] = ale::getCenterTime(jsonIsd); state["m_startingEphemerisTime"] = ale::getStartingTime(jsonIsd); - MESSAGE_LOG("m_centerEphemerisTime: {} " - "m_startingEphemerisTime: {} ", - state["m_centerEphemerisTime"].dump(), - state["m_startingEphemerisTime"].dump()) + MESSAGE_LOG( + "m_centerEphemerisTime: {} " + "m_startingEphemerisTime: {} ", + state["m_centerEphemerisTime"].dump(), + state["m_startingEphemerisTime"].dump()) std::vector> lineScanRate = ale::getLineScanRate(jsonIsd); - state["m_intTimeLines"] = getIntegrationStartLines(lineScanRate, parsingWarnings); - state["m_intTimeStartTimes"] = getIntegrationStartTimes(lineScanRate, parsingWarnings); + state["m_intTimeLines"] = + getIntegrationStartLines(lineScanRate, parsingWarnings); + state["m_intTimeStartTimes"] = + getIntegrationStartTimes(lineScanRate, parsingWarnings); state["m_intTimes"] = getIntegrationTimes(lineScanRate, parsingWarnings); - MESSAGE_LOG("m_intTimeLines: {} " - "m_intTimeStartTimes: {} " - "m_intTimes: {} ", - state["m_intTimeLines"].dump(), - state["m_intTimeStartTimes"].dump(), - state["m_intTimes"].dump()) + MESSAGE_LOG( + "m_intTimeLines: {} " + "m_intTimeStartTimes: {} " + "m_intTimes: {} ", + state["m_intTimeLines"].dump(), state["m_intTimeStartTimes"].dump(), + state["m_intTimes"].dump()) state["m_detectorSampleSumming"] = ale::getSampleSumming(jsonIsd); state["m_detectorLineSumming"] = ale::getLineSumming(jsonIsd); @@ -2694,31 +2532,30 @@ std::string UsgsAstroLsSensorModel::constructStateFromIsd(const std::string imag state["m_startingDetectorLine"] = ale::getDetectorStartingLine(jsonIsd); state["m_detectorSampleOrigin"] = ale::getDetectorCenterSample(jsonIsd); state["m_detectorLineOrigin"] = ale::getDetectorCenterLine(jsonIsd); - MESSAGE_LOG("m_detectorSampleSumming: {} " - "m_detectorLineSumming: {}" - "m_startingDetectorSample: {} " - "m_startingDetectorLine: {} " - "m_detectorSampleOrigin: {} " - "m_detectorLineOrigin: {} ", - state["m_detectorSampleSumming"].dump(), - state["m_detectorLineSumming"].dump(), - state["m_startingDetectorSample"].dump(), - state["m_startingDetectorLine"].dump(), - state["m_detectorSampleOrigin"].dump(), - state["m_detectorLineOrigin"].dump()) + MESSAGE_LOG( + "m_detectorSampleSumming: {} " + "m_detectorLineSumming: {}" + "m_startingDetectorSample: {} " + "m_startingDetectorLine: {} " + "m_detectorSampleOrigin: {} " + "m_detectorLineOrigin: {} ", + state["m_detectorSampleSumming"].dump(), + state["m_detectorLineSumming"].dump(), + state["m_startingDetectorSample"].dump(), + state["m_startingDetectorLine"].dump(), + 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); + 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", + } 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") } @@ -2726,12 +2563,9 @@ std::string UsgsAstroLsSensorModel::constructStateFromIsd(const std::string imag 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", + } 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") } @@ -2745,7 +2579,8 @@ std::string UsgsAstroLsSensorModel::constructStateFromIsd(const std::string imag std::vector velocities = {}; for (int i = 0; i < ephemTime.size(); i++) { - rotatedInstState = j2000_to_target.rotateStateAt(ephemTime[i], instStates[i], ale::SLERP); + rotatedInstState = + j2000_to_target.rotateStateAt(ephemTime[i], instStates[i], ale::SLERP); // ALE operates in Km and we want m positions.push_back(rotatedInstState.position.x * 1000); positions.push_back(rotatedInstState.position.y * 1000); @@ -2757,49 +2592,44 @@ std::string UsgsAstroLsSensorModel::constructStateFromIsd(const std::string imag state["m_positions"] = positions; state["m_numPositions"] = positions.size(); - MESSAGE_LOG("m_positions: {}" - "m_numPositions: {}", - state["m_positions"].dump(), - state["m_numPositions"].dump()) + MESSAGE_LOG( + "m_positions: {}" + "m_numPositions: {}", + state["m_positions"].dump(), state["m_numPositions"].dump()) state["m_velocities"] = velocities; - MESSAGE_LOG("m_velocities: {}", - state["m_velocities"].dump()) + MESSAGE_LOG("m_velocities: {}", state["m_velocities"].dump()) ale::Orientations sensor_to_j2000 = j2000_to_sensor.inverse(); ale::Orientations sensor_to_target = j2000_to_target * sensor_to_j2000; ephemTime = sensor_to_target.getTimes(); - double quatStep = (ephemTime.back() - ephemTime.front()) / (ephemTime.size() - 1); - try{ - state["m_dtQuat"] = quatStep; + double quatStep = + (ephemTime.back() - ephemTime.front()) / (ephemTime.size() - 1); + try { + state["m_dtQuat"] = quatStep; MESSAGE_LOG("dt_quaternion: {}", state["m_dtQuat"].dump()) - } - catch(...) { - parsingWarnings->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "dt_quaternion not in ISD", + } catch (...) { + parsingWarnings->push_back(csm::Warning( + csm::Warning::DATA_NOT_AVAILABLE, "dt_quaternion not in ISD", "UsgsAstroFrameSensorModel::constructStateFromIsd()")); MESSAGE_LOG("dt_quaternion not in ISD") } - try{ - state["m_t0Quat"] = ephemTime[0] - ale::getCenterTime(jsonIsd); + try { + state["m_t0Quat"] = ephemTime[0] - ale::getCenterTime(jsonIsd); MESSAGE_LOG("m_t0Quat: {}", state["m_t0Quat"].dump()) - } - catch(...) { - parsingWarnings->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "t0_quaternion not in ISD", + } catch (...) { + parsingWarnings->push_back(csm::Warning( + csm::Warning::DATA_NOT_AVAILABLE, "t0_quaternion not in ISD", "UsgsAstroFrameSensorModel::constructStateFromIsd()")); MESSAGE_LOG("t0_quaternion not in ISD") } std::vector quaternion; std::vector quaternions; - for (size_t i = 0 ; i < ephemTime.size(); i++) { - ale::Rotation rotation = sensor_to_target.interpolate(ephemTime.front() + quatStep * i, ale::SLERP); + for (size_t i = 0; i < ephemTime.size(); i++) { + ale::Rotation rotation = sensor_to_target.interpolate( + ephemTime.front() + quatStep * i, ale::SLERP); quaternion = rotation.toQuaternion(); quaternions.push_back(quaternion[1]); quaternions.push_back(quaternion[2]); @@ -2809,10 +2639,10 @@ std::string UsgsAstroLsSensorModel::constructStateFromIsd(const std::string imag state["m_quaternions"] = quaternions; state["m_numQuaternions"] = quaternions.size(); - MESSAGE_LOG("m_quaternions: {}" - "m_numQuaternions: {}", - state["m_quaternions"].dump(), - state["m_numQuaternions"].dump()) + MESSAGE_LOG( + "m_quaternions: {}" + "m_numQuaternions: {}", + state["m_quaternions"].dump(), state["m_numQuaternions"].dump()) state["m_currentParameterValue"] = std::vector(NUM_PARAMETERS, 0.0); MESSAGE_LOG("m_currentParameterValue: {}", @@ -2822,36 +2652,38 @@ std::string UsgsAstroLsSensorModel::constructStateFromIsd(const std::string imag // ALE operates in Km and we want m state["m_minorAxis"] = ale::getSemiMinorRadius(jsonIsd) * 1000; state["m_majorAxis"] = ale::getSemiMajorRadius(jsonIsd) * 1000; - MESSAGE_LOG("m_minorAxis: {}" - "m_majorAxis: {}", - state["m_minorAxis"].dump(), state["m_majorAxis"].dump()) + MESSAGE_LOG( + "m_minorAxis: {}" + "m_majorAxis: {}", + state["m_minorAxis"].dump(), state["m_majorAxis"].dump()) // set identifiers state["m_platformIdentifier"] = ale::getPlatformName(jsonIsd); state["m_sensorIdentifier"] = ale::getSensorName(jsonIsd); - MESSAGE_LOG("m_platformIdentifier: {}" - "m_sensorIdentifier: {}", - state["m_platformIdentifier"].dump(), - state["m_sensorIdentifier"].dump()) + MESSAGE_LOG( + "m_platformIdentifier: {}" + "m_sensorIdentifier: {}", + state["m_platformIdentifier"].dump(), state["m_sensorIdentifier"].dump()) // get reference_height state["m_minElevation"] = ale::getMinHeight(jsonIsd); state["m_maxElevation"] = ale::getMaxHeight(jsonIsd); - MESSAGE_LOG("m_minElevation: {}" - "m_maxElevation: {}", - state["m_minElevation"].dump(), - state["m_maxElevation"].dump()) + MESSAGE_LOG( + "m_minElevation: {}" + "m_maxElevation: {}", + state["m_minElevation"].dump(), state["m_maxElevation"].dump()) // Default to identity covariance state["m_covariance"] = - std::vector(NUM_PARAMETERS * NUM_PARAMETERS, 0.0); + std::vector(NUM_PARAMETERS * NUM_PARAMETERS, 0.0); for (int i = 0; i < NUM_PARAMETERS; i++) { - state["m_covariance"][i * NUM_PARAMETERS + i] = 1.0; + state["m_covariance"][i * NUM_PARAMETERS + i] = 1.0; } if (!parsingWarnings->empty()) { if (warnings) { - warnings->insert(warnings->end(), parsingWarnings->begin(), parsingWarnings->end()); + warnings->insert(warnings->end(), parsingWarnings->begin(), + parsingWarnings->end()); } delete parsingWarnings; parsingWarnings = nullptr; @@ -2868,7 +2700,6 @@ std::string UsgsAstroLsSensorModel::constructStateFromIsd(const std::string imag return state.dump(); } - //*************************************************************************** // UsgsAstroLineScannerSensorModel::getLogger //*************************************************************************** @@ -2882,39 +2713,34 @@ void UsgsAstroLsSensorModel::setLogger(std::string logName) { m_logger = spdlog::get(logName); } - csm::EcefVector UsgsAstroLsSensorModel::getSunPosition( - const double imageTime) const -{ - + const double imageTime) const { int numSunPositions = m_sunPosition.size(); int numSunVelocities = m_sunVelocity.size(); csm::EcefVector sunPosition = csm::EcefVector(); // If there are multiple positions, use Lagrange interpolation - if ((numSunPositions/3) > 1) { + 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); + 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); sunPosition.x = sunPos[0]; sunPosition.y = sunPos[1]; sunPosition.z = sunPos[2]; - } - else if ((numSunVelocities/3) >= 1){ + } else if ((numSunVelocities / 3) >= 1) { // If there is one position triple with at least one velocity triple // then the illumination direction is calculated via linear extrapolation. - sunPosition.x = (imageTime * m_sunVelocity[0] + m_sunPosition[0]); - sunPosition.y = (imageTime * m_sunVelocity[1] + m_sunPosition[1]); - sunPosition.z = (imageTime * m_sunVelocity[2] + m_sunPosition[2]); - } - else { + sunPosition.x = (imageTime * m_sunVelocity[0] + m_sunPosition[0]); + sunPosition.y = (imageTime * m_sunVelocity[1] + m_sunPosition[1]); + sunPosition.z = (imageTime * m_sunVelocity[2] + m_sunPosition[2]); + } else { // If there is one position triple with no velocity triple, then the // illumination direction is the difference of the original vectors. - sunPosition.x = m_sunPosition[0]; - sunPosition.y = m_sunPosition[1]; - sunPosition.z = m_sunPosition[2]; + sunPosition.x = m_sunPosition[0]; + sunPosition.y = m_sunPosition[1]; + sunPosition.z = m_sunPosition[2]; } return sunPosition; } diff --git a/src/UsgsAstroPlugin.cpp b/src/UsgsAstroPlugin.cpp index 7b6b290cd..822ce9216 100644 --- a/src/UsgsAstroPlugin.cpp +++ b/src/UsgsAstroPlugin.cpp @@ -6,40 +6,42 @@ #include #include -#include #include +#include -#include -#include #include #include -#include #include +#include +#include +#include #include using json = nlohmann::json; #ifdef _WIN32 -# define DIR_DELIMITER_STR "\\" +#define DIR_DELIMITER_STR "\\" #else -# define DIR_DELIMITER_STR "/" +#define DIR_DELIMITER_STR "/" #endif -#define MESSAGE_LOG(...) if (m_logger) { m_logger->info(__VA_ARGS__); } +#define MESSAGE_LOG(...) \ + if (m_logger) { \ + m_logger->info(__VA_ARGS__); \ + } // Declaration of static variables const std::string UsgsAstroPlugin::_PLUGIN_NAME = "UsgsAstroPluginCSM"; const std::string UsgsAstroPlugin::_MANUFACTURER_NAME = "UsgsAstrogeology"; const std::string UsgsAstroPlugin::_RELEASE_DATE = "20190222"; -const int UsgsAstroPlugin::_N_SENSOR_MODELS = 3; +const int UsgsAstroPlugin::_N_SENSOR_MODELS = 3; // Static Instance of itself const UsgsAstroPlugin UsgsAstroPlugin::m_registeredPlugin; UsgsAstroPlugin::UsgsAstroPlugin() { - // Build and register the USGSCSM logger on plugin creation - char * logFilePtr = getenv("ALE_LOG_FILE"); + char *logFilePtr = getenv("ALE_LOG_FILE"); if (logFilePtr != NULL) { std::string logFile(logFilePtr); @@ -48,154 +50,132 @@ UsgsAstroPlugin::UsgsAstroPlugin() { std::shared_ptr m_logger = spdlog::get("usgscsm_logger"); if (!m_logger) { - std::shared_ptr m_logger = spdlog::basic_logger_mt("usgscsm_logger", logFile); + std::shared_ptr m_logger = + spdlog::basic_logger_mt("usgscsm_logger", logFile); } } } } - -UsgsAstroPlugin::~UsgsAstroPlugin() { -} - +UsgsAstroPlugin::~UsgsAstroPlugin() {} std::string UsgsAstroPlugin::getPluginName() const { MESSAGE_LOG("Get Plugin Name: {}", _PLUGIN_NAME); return _PLUGIN_NAME; } - std::string UsgsAstroPlugin::getManufacturer() const { MESSAGE_LOG("Get Manufacturer Name: {}", _MANUFACTURER_NAME); return _MANUFACTURER_NAME; } - std::string UsgsAstroPlugin::getReleaseDate() const { MESSAGE_LOG("Get Release Date: {}", _RELEASE_DATE); return _RELEASE_DATE; } - csm::Version UsgsAstroPlugin::getCsmVersion() const { MESSAGE_LOG("Get Current CSM Version"); return CURRENT_CSM_VERSION; } - size_t UsgsAstroPlugin::getNumModels() const { MESSAGE_LOG("Get Number of Sensor Models: {}", _N_SENSOR_MODELS); return _N_SENSOR_MODELS; } - std::string UsgsAstroPlugin::getModelName(size_t modelIndex) const { std::vector supportedModelNames = { - UsgsAstroFrameSensorModel::_SENSOR_MODEL_NAME, - UsgsAstroLsSensorModel::_SENSOR_MODEL_NAME, - UsgsAstroSarSensorModel::_SENSOR_MODEL_NAME - }; - MESSAGE_LOG("Get Model Name: {}. Used index: {}", supportedModelNames[modelIndex], modelIndex); + UsgsAstroFrameSensorModel::_SENSOR_MODEL_NAME, + UsgsAstroLsSensorModel::_SENSOR_MODEL_NAME, + UsgsAstroSarSensorModel::_SENSOR_MODEL_NAME}; + MESSAGE_LOG("Get Model Name: {}. Used index: {}", + supportedModelNames[modelIndex], modelIndex); return supportedModelNames[modelIndex]; } - std::string UsgsAstroPlugin::getModelFamily(size_t modelIndex) const { - MESSAGE_LOG("Get Model Familey: {}", CSM_RASTER_FAMILY); + MESSAGE_LOG("Get Model Familey: {}", CSM_RASTER_FAMILY); return CSM_RASTER_FAMILY; } - -csm::Version UsgsAstroPlugin::getModelVersion(const std::string &modelName) const { +csm::Version UsgsAstroPlugin::getModelVersion( + const std::string &modelName) const { MESSAGE_LOG("Get Model Version"); return csm::Version(1, 0, 0); } - -bool UsgsAstroPlugin::canModelBeConstructedFromState(const std::string &modelName, - const std::string &modelState, - csm::WarningList *warnings) const { +bool UsgsAstroPlugin::canModelBeConstructedFromState( + const std::string &modelName, const std::string &modelState, + csm::WarningList *warnings) const { try { - csm::Model* model = constructModelFromState(modelState, warnings); + csm::Model *model = constructModelFromState(modelState, warnings); return (bool)model; - } - catch(std::exception& e) { + } catch (std::exception &e) { std::string msg = "Could not create model ["; msg += modelName; msg += "] with error ["; msg += e.what(); msg += "]"; MESSAGE_LOG(msg); - if(warnings) { - warnings->push_back( - csm::Warning( - csm::Warning::UNKNOWN_WARNING, - msg, + if (warnings) { + warnings->push_back(csm::Warning( + csm::Warning::UNKNOWN_WARNING, msg, "UsgsAstroFrameSensorModel::canModelBeConstructedFromState()")); } return false; - } - catch(...) { + } catch (...) { std::string msg = "Could not create model ["; msg += modelName; msg += "] with an unknown error."; MESSAGE_LOG(msg); - if(warnings) { - warnings->push_back( - csm::Warning( - csm::Warning::UNKNOWN_WARNING, - msg, + if (warnings) { + warnings->push_back(csm::Warning( + csm::Warning::UNKNOWN_WARNING, msg, "UsgsAstroFrameSensorModel::canModelBeConstructedFromState()")); } } return false; } - -bool UsgsAstroPlugin::canModelBeConstructedFromISD(const csm::Isd &imageSupportData, - const std::string &modelName, - csm::WarningList *warnings) const { +bool UsgsAstroPlugin::canModelBeConstructedFromISD( + const csm::Isd &imageSupportData, const std::string &modelName, + csm::WarningList *warnings) const { try { - csm::Model* model = constructModelFromISD(imageSupportData, modelName, warnings); + csm::Model *model = + constructModelFromISD(imageSupportData, modelName, warnings); return (bool)model; - } - catch(std::exception& e) { - if(warnings) { + } catch (std::exception &e) { + if (warnings) { std::string msg = "Could not create model ["; msg += modelName; msg += "] with error ["; msg += e.what(); msg += "]"; MESSAGE_LOG(msg); - warnings->push_back( - csm::Warning( - csm::Warning::UNKNOWN_WARNING, - msg, + warnings->push_back(csm::Warning( + csm::Warning::UNKNOWN_WARNING, msg, "UsgsAstroFrameSensorModel::canModelBeConstructedFromISD()")); } - } - catch(...) { - if(warnings) { + } catch (...) { + if (warnings) { std::string msg = "Could not create model ["; msg += modelName; msg += "] with an unknown error."; MESSAGE_LOG(msg); - warnings->push_back( - csm::Warning( - csm::Warning::UNKNOWN_WARNING, - msg, + warnings->push_back(csm::Warning( + csm::Warning::UNKNOWN_WARNING, msg, "UsgsAstroFrameSensorModel::canModelBeConstructedFromISD()")); } } return false; } - -// This function takes a csm::Isd which only has the image filename set. It uses this filename to -// find a metadata json file located alongside the image file and returns a json -// encoded string. -std::string UsgsAstroPlugin::loadImageSupportData(const csm::Isd &imageSupportDataOriginal) const { - +// This function takes a csm::Isd which only has the image filename set. It uses +// this filename to find a metadata json file located alongside the image file +// and returns a json encoded string. +std::string UsgsAstroPlugin::loadImageSupportData( + const csm::Isd &imageSupportDataOriginal) const { // Get image location from the input csm::Isd: std::string imageFilename = imageSupportDataOriginal.filename(); size_t lastIndex = imageFilename.find_last_of("."); @@ -203,7 +183,7 @@ std::string UsgsAstroPlugin::loadImageSupportData(const csm::Isd &imageSupportDa lastIndex = baseName.find_last_of(DIR_DELIMITER_STR); std::string filename = baseName.substr(lastIndex + 1); std::string isdFilename = baseName.append(".json"); - MESSAGE_LOG("Load Image Support Data using: {}, {}, {}, {}, {}", + MESSAGE_LOG("Load Image Support Data using: {}, {}, {}, {}, {}", imageFilename, lastIndex, baseName, filename, isdFilename); try { std::ifstream isd_sidecar(isdFilename); @@ -212,9 +192,9 @@ std::string UsgsAstroPlugin::loadImageSupportData(const csm::Isd &imageSupportDa jsonisd["image_identifier"] = filename; return jsonisd.dump(); - } - catch (std::exception& e) { - std::string errorMessage = "Could not read metadata file associated with image ["; + } catch (std::exception &e) { + std::string errorMessage = + "Could not read metadata file associated with image ["; errorMessage += isdFilename; errorMessage += "] with error ["; errorMessage += e.what(); @@ -225,45 +205,41 @@ std::string UsgsAstroPlugin::loadImageSupportData(const csm::Isd &imageSupportDa } } - -std::string UsgsAstroPlugin::getModelNameFromModelState(const std::string &modelState, - csm::WarningList *warnings) const { +std::string UsgsAstroPlugin::getModelNameFromModelState( + const std::string &modelState, csm::WarningList *warnings) const { auto state = json::parse(modelState); std::string name = state.value("name_model", ""); - MESSAGE_LOG("Get model name from model state. State: {}, Name: {}", modelState, name); + MESSAGE_LOG("Get model name from model state. State: {}, Name: {}", + modelState, name); if (name == "") { - csm::Error::ErrorType aErrorType = csm::Error::INVALID_SENSOR_MODEL_STATE; - std::string aMessage = "No 'name_model' key in the model state object."; - std::string aFunction = "UsgsAstroPlugin::getModelNameFromModelState"; - MESSAGE_LOG(aMessage); - csm::Error csmErr(aErrorType, aMessage, aFunction); - throw(csmErr); + csm::Error::ErrorType aErrorType = csm::Error::INVALID_SENSOR_MODEL_STATE; + std::string aMessage = "No 'name_model' key in the model state object."; + std::string aFunction = "UsgsAstroPlugin::getModelNameFromModelState"; + MESSAGE_LOG(aMessage); + csm::Error csmErr(aErrorType, aMessage, aFunction); + throw(csmErr); } return name; } - -bool UsgsAstroPlugin::canISDBeConvertedToModelState(const csm::Isd &imageSupportData, - const std::string &modelName, - csm::WarningList *warnings) const { +bool UsgsAstroPlugin::canISDBeConvertedToModelState( + const csm::Isd &imageSupportData, const std::string &modelName, + csm::WarningList *warnings) const { MESSAGE_LOG("Running canISDBeConvertedToModelState"); try { - convertISDToModelState(imageSupportData, modelName, warnings); - } - catch(std::exception& e) { - if(warnings) { + convertISDToModelState(imageSupportData, modelName, warnings); + } catch (std::exception &e) { + if (warnings) { std::string msg = "Could not create model ["; msg += modelName; msg += "] state with error ["; msg += e.what(); msg += "]"; MESSAGE_LOG(msg); - warnings->push_back( - csm::Warning( - csm::Warning::UNKNOWN_WARNING, - msg, + warnings->push_back(csm::Warning( + csm::Warning::UNKNOWN_WARNING, msg, "UsgsAstroFrameSensorModel::canISDBeConvertedToModelState()")); } return false; @@ -271,7 +247,6 @@ bool UsgsAstroPlugin::canISDBeConvertedToModelState(const csm::Isd &imageSupport return true; } - std::string UsgsAstroPlugin::getStateFromISD(csm::Isd imageSupportData) const { MESSAGE_LOG("Running getStateFromISD"); std::string stringIsd = loadImageSupportData(imageSupportData); @@ -280,32 +255,32 @@ std::string UsgsAstroPlugin::getStateFromISD(csm::Isd imageSupportData) const { return convertISDToModelState(imageSupportData, jsonIsd.at("name_model")); } - -std::string UsgsAstroPlugin::convertISDToModelState(const csm::Isd &imageSupportData, - const std::string &modelName, - csm::WarningList *warnings) const { +std::string UsgsAstroPlugin::convertISDToModelState( + const csm::Isd &imageSupportData, const std::string &modelName, + csm::WarningList *warnings) const { MESSAGE_LOG("Running convertISDToModelState"); - csm::Model* sensor_model = constructModelFromISD(imageSupportData, modelName, warnings); + csm::Model *sensor_model = + constructModelFromISD(imageSupportData, modelName, warnings); return sensor_model->getModelState(); } - -csm::Model *UsgsAstroPlugin::constructModelFromISD(const csm::Isd &imageSupportDataOriginal, - const std::string &modelName, - csm::WarningList *warnings) const { +csm::Model *UsgsAstroPlugin::constructModelFromISD( + const csm::Isd &imageSupportDataOriginal, const std::string &modelName, + csm::WarningList *warnings) const { MESSAGE_LOG("Running constructModelFromISD"); std::string stringIsd = loadImageSupportData(imageSupportDataOriginal); - + MESSAGE_LOG("ISD String: {}", stringIsd); if (modelName == UsgsAstroFrameSensorModel::_SENSOR_MODEL_NAME) { - UsgsAstroFrameSensorModel *model = new UsgsAstroFrameSensorModel(); + UsgsAstroFrameSensorModel *model = new UsgsAstroFrameSensorModel(); try { MESSAGE_LOG("Trying to construct a UsgsAstroFrameSensorModel"); - model->replaceModelState(model->constructStateFromIsd(stringIsd, warnings)); + model->replaceModelState( + model->constructStateFromIsd(stringIsd, warnings)); MESSAGE_LOG("Constructed model: {}", modelName); - } - catch (std::exception& e) { - csm::Error::ErrorType aErrorType = csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE; + } catch (std::exception &e) { + csm::Error::ErrorType aErrorType = + csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE; std::string aMessage = "Could not construct model ["; aMessage += modelName; aMessage += "] with error ["; @@ -316,15 +291,15 @@ csm::Model *UsgsAstroPlugin::constructModelFromISD(const csm::Isd &imageSupportD throw csm::Error(aErrorType, aMessage, aFunction); } return model; - } - else if (modelName == UsgsAstroLsSensorModel::_SENSOR_MODEL_NAME) { - UsgsAstroLsSensorModel *model = new UsgsAstroLsSensorModel(); + } else if (modelName == UsgsAstroLsSensorModel::_SENSOR_MODEL_NAME) { + UsgsAstroLsSensorModel *model = new UsgsAstroLsSensorModel(); try { MESSAGE_LOG("Trying to construct a UsgsAstroLsSensorModel"); - model->replaceModelState(model->constructStateFromIsd(stringIsd, warnings)); - } - catch (std::exception& e) { - csm::Error::ErrorType aErrorType = csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE; + model->replaceModelState( + model->constructStateFromIsd(stringIsd, warnings)); + } catch (std::exception &e) { + csm::Error::ErrorType aErrorType = + csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE; std::string aMessage = "Could not construct model ["; aMessage += modelName; aMessage += "] with error ["; @@ -335,15 +310,15 @@ csm::Model *UsgsAstroPlugin::constructModelFromISD(const csm::Isd &imageSupportD throw csm::Error(aErrorType, aMessage, aFunction); } return model; - } - else if (modelName == UsgsAstroSarSensorModel::_SENSOR_MODEL_NAME) { - UsgsAstroSarSensorModel *model = new UsgsAstroSarSensorModel(); + } else if (modelName == UsgsAstroSarSensorModel::_SENSOR_MODEL_NAME) { + UsgsAstroSarSensorModel *model = new UsgsAstroSarSensorModel(); MESSAGE_LOG("Trying to construct a UsgsAstroSarSensorModel"); try { - model->replaceModelState(model->constructStateFromIsd(stringIsd, warnings)); - } - catch (std::exception& e) { - csm::Error::ErrorType aErrorType = csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE; + model->replaceModelState( + model->constructStateFromIsd(stringIsd, warnings)); + } catch (std::exception &e) { + csm::Error::ErrorType aErrorType = + csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE; std::string aMessage = "Could not construct model ["; aMessage += modelName; aMessage += "] with error ["; @@ -354,8 +329,7 @@ csm::Model *UsgsAstroPlugin::constructModelFromISD(const csm::Isd &imageSupportD throw csm::Error(aErrorType, aMessage, aFunction); } return model; - } - else { + } else { csm::Error::ErrorType aErrorType = csm::Error::SENSOR_MODEL_NOT_SUPPORTED; std::string aMessage = "Model [" + modelName + "] not supported: "; std::string aFunction = "UsgsAstroPlugin::constructModelFromISD()"; @@ -364,9 +338,8 @@ csm::Model *UsgsAstroPlugin::constructModelFromISD(const csm::Isd &imageSupportD } } - -csm::Model *UsgsAstroPlugin::constructModelFromState(const std::string& modelState, - csm::WarningList *warnings) const { +csm::Model *UsgsAstroPlugin::constructModelFromState( + const std::string &modelState, csm::WarningList *warnings) const { MESSAGE_LOG("Runing constructModelFromState with modelState: {}", modelState); json state = json::parse(modelState); std::string modelName = state["m_modelName"]; @@ -374,23 +347,20 @@ csm::Model *UsgsAstroPlugin::constructModelFromState(const std::string& modelSta if (modelName == UsgsAstroFrameSensorModel::_SENSOR_MODEL_NAME) { MESSAGE_LOG("Constructing a UsgsAstroFrameSensorModel"); - UsgsAstroFrameSensorModel* model = new UsgsAstroFrameSensorModel(); + UsgsAstroFrameSensorModel *model = new UsgsAstroFrameSensorModel(); model->replaceModelState(modelState); return model; - } - else if (modelName == UsgsAstroLsSensorModel::_SENSOR_MODEL_NAME) { + } else if (modelName == UsgsAstroLsSensorModel::_SENSOR_MODEL_NAME) { MESSAGE_LOG("Constructing a UsgsAstroLsSensorModel"); - UsgsAstroLsSensorModel* model = new UsgsAstroLsSensorModel(); + UsgsAstroLsSensorModel *model = new UsgsAstroLsSensorModel(); model->replaceModelState(modelState); return model; - } - else if (modelName == UsgsAstroSarSensorModel::_SENSOR_MODEL_NAME) { + } else if (modelName == UsgsAstroSarSensorModel::_SENSOR_MODEL_NAME) { MESSAGE_LOG("Constructing a UsgsAstroSarSensorModel"); - UsgsAstroSarSensorModel* model = new UsgsAstroSarSensorModel(); + UsgsAstroSarSensorModel *model = new UsgsAstroSarSensorModel(); model->replaceModelState(modelState); return model; - } - else { + } else { csm::Error::ErrorType aErrorType = csm::Error::ISD_NOT_SUPPORTED; std::string aMessage = "Model" + modelName + " not supported: "; std::string aFunction = "UsgsAstroPlugin::constructModelFromState()"; diff --git a/src/UsgsAstroSarSensorModel.cpp b/src/UsgsAstroSarSensorModel.cpp index 0cb53e4b6..331dce552 100644 --- a/src/UsgsAstroSarSensorModel.cpp +++ b/src/UsgsAstroSarSensorModel.cpp @@ -1,52 +1,44 @@ #include "UsgsAstroSarSensorModel.h" #include "Utilities.h" -#include -#include #include #include +#include +#include #include using json = nlohmann::json; using namespace std; -#define MESSAGE_LOG(...) if (m_logger) { m_logger->info(__VA_ARGS__); } +#define MESSAGE_LOG(...) \ + if (m_logger) { \ + m_logger->info(__VA_ARGS__); \ + } -const string UsgsAstroSarSensorModel::_SENSOR_MODEL_NAME = "USGS_ASTRO_SAR_SENSOR_MODEL"; +const string UsgsAstroSarSensorModel::_SENSOR_MODEL_NAME = + "USGS_ASTRO_SAR_SENSOR_MODEL"; const int UsgsAstroSarSensorModel::NUM_PARAMETERS = 6; -const string UsgsAstroSarSensorModel::PARAMETER_NAME[] = -{ - "X Pos. Bias ", // 0 - "Y Pos. Bias ", // 1 - "Z Pos. Bias ", // 2 - "X Vel. Bias ", // 3 - "Y Vel. Bias ", // 4 - "Z Vel. Bias " // 5 +const string UsgsAstroSarSensorModel::PARAMETER_NAME[] = { + "X Pos. Bias ", // 0 + "Y Pos. Bias ", // 1 + "Z Pos. Bias ", // 2 + "X Vel. Bias ", // 3 + "Y Vel. Bias ", // 4 + "Z Vel. Bias " // 5 }; const int UsgsAstroSarSensorModel::NUM_PARAM_TYPES = 4; -const string UsgsAstroSarSensorModel::PARAM_STRING_ALL[] = -{ - "NONE", - "FICTITIOUS", - "REAL", - "FIXED" -}; -const csm::param::Type - UsgsAstroSarSensorModel::PARAM_CHAR_ALL[] = -{ - csm::param::NONE, - csm::param::FICTITIOUS, - csm::param::REAL, - csm::param::FIXED -}; +const string UsgsAstroSarSensorModel::PARAM_STRING_ALL[] = { + "NONE", "FICTITIOUS", "REAL", "FIXED"}; +const csm::param::Type UsgsAstroSarSensorModel::PARAM_CHAR_ALL[] = { + csm::param::NONE, csm::param::FICTITIOUS, csm::param::REAL, + csm::param::FIXED}; string UsgsAstroSarSensorModel::constructStateFromIsd( - const string imageSupportData, - csm::WarningList *warnings){ - - MESSAGE_LOG("UsgsAstroSarSensorModel constructing state from ISD, with {}", imageSupportData); + const string imageSupportData, csm::WarningList* warnings) { + MESSAGE_LOG("UsgsAstroSarSensorModel constructing state from ISD, with {}", + imageSupportData); json isd = json::parse(imageSupportData); json state = {}; @@ -75,28 +67,22 @@ string UsgsAstroSarSensorModel::constructStateFromIsd( try { state["m_dtEphem"] = isd.at("dt_ephemeris"); - } - catch(...) { - std::string msg = "dt_ephemeris not in ISD"; + } catch (...) { + std::string msg = "dt_ephemeris not in ISD"; MESSAGE_LOG(msg); parsingWarnings->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - msg, - "UsgsAstroSarSensorModel::constructStateFromIsd()")); + csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, msg, + "UsgsAstroSarSensorModel::constructStateFromIsd()")); } try { state["m_t0Ephem"] = isd.at("t0_ephemeris"); - } - catch(...) { + } catch (...) { std::string msg = "t0_ephemeris not in ISD"; MESSAGE_LOG(msg); parsingWarnings->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - msg, - "UsgsAstroSarSensorModel::constructStateFromIsd()")); + csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, msg, + "UsgsAstroSarSensorModel::constructStateFromIsd()")); } state["m_positions"] = getSensorPositions(isd, parsingWarnings); @@ -118,31 +104,33 @@ string UsgsAstroSarSensorModel::constructStateFromIsd( // SAR specific values state["m_scaledPixelWidth"] = getScaledPixelWidth(isd, parsingWarnings); - state["m_scaleConversionCoefficients"] = getScaleConversionCoefficients(isd, parsingWarnings); - state["m_scaleConversionTimes"] = getScaleConversionTimes(isd, parsingWarnings); + state["m_scaleConversionCoefficients"] = + getScaleConversionCoefficients(isd, parsingWarnings); + state["m_scaleConversionTimes"] = + getScaleConversionTimes(isd, parsingWarnings); state["m_wavelength"] = getWavelength(isd, parsingWarnings); state["m_lookDirection"] = getLookDirection(isd, parsingWarnings); // Default to identity covariance - state["m_covariance"] = - vector(NUM_PARAMETERS * NUM_PARAMETERS, 0.0); + state["m_covariance"] = vector(NUM_PARAMETERS * NUM_PARAMETERS, 0.0); for (int i = 0; i < NUM_PARAMETERS; i++) { - state["m_covariance"][i * NUM_PARAMETERS + i] = 1.0; + state["m_covariance"][i * NUM_PARAMETERS + i] = 1.0; } if (!parsingWarnings->empty()) { if (warnings) { - warnings->insert(warnings->end(), parsingWarnings->begin(), parsingWarnings->end()); + warnings->insert(warnings->end(), parsingWarnings->begin(), + parsingWarnings->end()); } - std::string message = "ISD is invalid for creating the sensor model with error ["; + std::string message = + "ISD is invalid for creating the sensor model with error ["; csm::Warning warn = parsingWarnings->front(); message += warn.getMessage(); message += "]"; parsingWarnings = nullptr; delete parsingWarnings; MESSAGE_LOG(message); - throw csm::Error(csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE, - message, + throw csm::Error(csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE, message, "UsgsAstroSarSensorModel::constructStateFromIsd"); } @@ -154,7 +142,8 @@ string UsgsAstroSarSensorModel::constructStateFromIsd( return state.dump(); } -string UsgsAstroSarSensorModel::getModelNameFromModelState(const string& model_state) { +string UsgsAstroSarSensorModel::getModelNameFromModelState( + const string& model_state) { MESSAGE_LOG("Getting model name from model state: {}", model_state); // Parse the string to JSON auto j = json::parse(model_state); @@ -171,7 +160,7 @@ string UsgsAstroSarSensorModel::getModelNameFromModelState(const string& model_s csm::Error csmErr(aErrorType, aMessage, aFunction); throw(csmErr); } - if (model_name != _SENSOR_MODEL_NAME){ + if (model_name != _SENSOR_MODEL_NAME) { csm::Error::ErrorType aErrorType = csm::Error::SENSOR_MODEL_NOT_SUPPORTED; string aMessage = "Sensor model not supported."; string aFunction = "UsgsAstroSarSensorModel::getModelNameFromModelState()"; @@ -219,14 +208,14 @@ void UsgsAstroSarSensorModel::reset() { m_referencePointXyz.x = 0.0; m_referencePointXyz.y = 0.0; m_referencePointXyz.z = 0.0; - m_covariance = vector(NUM_PARAMETERS * NUM_PARAMETERS,0.0); + m_covariance = vector(NUM_PARAMETERS * NUM_PARAMETERS, 0.0); m_sunPosition.clear(); m_sunVelocity.clear(); m_wavelength = 0; - m_noAdjustments = std::vector(NUM_PARAMETERS,0.0); + m_noAdjustments = std::vector(NUM_PARAMETERS, 0.0); } -void UsgsAstroSarSensorModel::replaceModelState(const string& argState){ +void UsgsAstroSarSensorModel::replaceModelState(const string& argState) { reset(); MESSAGE_LOG("Replacing model state with: {}", argState); @@ -240,17 +229,14 @@ void UsgsAstroSarSensorModel::replaceModelState(const string& argState){ m_exposureDuration = stateJson["m_exposureDuration"]; m_scaledPixelWidth = stateJson["m_scaledPixelWidth"]; std::string lookStr = stateJson["m_lookDirection"]; - if (lookStr.compare("right") == 0 ) { + if (lookStr.compare("right") == 0) { m_lookDirection = UsgsAstroSarSensorModel::RIGHT; - } - else if (lookStr.compare("left") == 0) { + } else if (lookStr.compare("left") == 0) { m_lookDirection = UsgsAstroSarSensorModel::LEFT; - } - else { + } else { std::string message = "Could not determine look direction from state"; MESSAGE_LOG(message); - throw csm::Error(csm::Error::INVALID_SENSOR_MODEL_STATE, - message, + throw csm::Error(csm::Error::INVALID_SENSOR_MODEL_STATE, message, "UsgsAstroSarSensorModel::replaceModelState"); } m_wavelength = stateJson["m_wavelength"]; @@ -265,11 +251,14 @@ void UsgsAstroSarSensorModel::replaceModelState(const string& argState){ m_maxElevation = stateJson["m_maxElevation"]; m_dtEphem = stateJson["m_dtEphem"]; m_t0Ephem = stateJson["m_t0Ephem"]; - m_scaleConversionCoefficients = stateJson["m_scaleConversionCoefficients"].get>(); - m_scaleConversionTimes = stateJson["m_scaleConversionTimes"].get>(); + m_scaleConversionCoefficients = + stateJson["m_scaleConversionCoefficients"].get>(); + m_scaleConversionTimes = + stateJson["m_scaleConversionTimes"].get>(); m_positions = stateJson["m_positions"].get>(); m_velocities = stateJson["m_velocities"].get>(); - m_currentParameterValue = stateJson["m_currentParameterValue"].get>(); + m_currentParameterValue = + stateJson["m_currentParameterValue"].get>(); m_referencePointXyz.x = stateJson["m_referencePointXyz"][0]; m_referencePointXyz.y = stateJson["m_referencePointXyz"][1]; m_referencePointXyz.z = stateJson["m_referencePointXyz"][2]; @@ -277,16 +266,17 @@ void UsgsAstroSarSensorModel::replaceModelState(const string& argState){ m_sunPosition = stateJson["m_sunPosition"].get>(); m_sunVelocity = stateJson["m_sunVelocity"].get>(); - - // If sensor model is being created for the first time, this routine will set the reference point - if (m_referencePointXyz.x == 0 && m_referencePointXyz.y == 0 && m_referencePointXyz.z == 0) { + // If sensor model is being created for the first time, this routine will set + // the reference point + if (m_referencePointXyz.x == 0 && m_referencePointXyz.y == 0 && + m_referencePointXyz.z == 0) { MESSAGE_LOG("Updating State") double lineCtr = m_nLines / 2.0; double sampCtr = m_nSamples / 2.0; csm::ImageCoord ip(lineCtr, sampCtr); - MESSAGE_LOG("updateState: center image coordinate set to {} {}", - lineCtr, sampCtr) + MESSAGE_LOG("updateState: center image coordinate set to {} {}", lineCtr, + sampCtr) double refHeight = 0; m_referencePointXyz = imageToGround(ip, refHeight); @@ -305,11 +295,8 @@ string UsgsAstroSarSensorModel::getModelState() const { state["m_platformName"] = m_platformName; state["m_nLines"] = m_nLines; state["m_nSamples"] = m_nSamples; - state["m_referencePointXyz"] = { - m_referencePointXyz.x, - m_referencePointXyz.y, - m_referencePointXyz.z - }; + state["m_referencePointXyz"] = {m_referencePointXyz.x, m_referencePointXyz.y, + m_referencePointXyz.z}; state["m_sunPosition"] = m_sunPosition; state["m_sunVelocity"] = m_sunVelocity; state["m_centerEphemerisTime"] = m_centerEphemerisTime; @@ -330,15 +317,12 @@ string UsgsAstroSarSensorModel::getModelState() const { state["m_scaledPixelWidth"] = m_scaledPixelWidth; if (m_lookDirection == 0) { state["m_lookDirection"] = "left"; - } - else if (m_lookDirection == 1) { + } else if (m_lookDirection == 1) { state["m_lookDirection"] = "right"; - } - else { + } else { std::string message = "Could not parse look direction from json state."; MESSAGE_LOG(message); - throw csm::Error(csm::Error::INVALID_SENSOR_MODEL_STATE, - message, + throw csm::Error(csm::Error::INVALID_SENSOR_MODEL_STATE, message, "UsgsAstroSarSensorModel::getModelState"); } state["m_wavelength"] = m_wavelength; @@ -349,50 +333,49 @@ string UsgsAstroSarSensorModel::getModelState() const { return state.dump(); } - csm::ImageCoord UsgsAstroSarSensorModel::groundToImage( - const csm::EcefCoord& groundPt, - double desiredPrecision, - double* achievedPrecision, - csm::WarningList* warnings) const { - - MESSAGE_LOG("Computing groundToImage(ImageCoord) for {}, {}, {}, with desired precision {}" - "No adjustments.", - groundPt.x, groundPt.y, groundPt.z, desiredPrecision); + const csm::EcefCoord& groundPt, double desiredPrecision, + double* achievedPrecision, csm::WarningList* warnings) const { + MESSAGE_LOG( + "Computing groundToImage(ImageCoord) for {}, {}, {}, with desired " + "precision {}" + "No adjustments.", + groundPt.x, groundPt.y, groundPt.z, desiredPrecision); - csm::ImageCoord imagePt = groundToImage(groundPt, m_noAdjustments, desiredPrecision, achievedPrecision, warnings); - return imagePt; + csm::ImageCoord imagePt = groundToImage( + groundPt, m_noAdjustments, desiredPrecision, achievedPrecision, warnings); + return imagePt; } csm::ImageCoord UsgsAstroSarSensorModel::groundToImage( - const csm::EcefCoord& groundPt, - const std::vector adj, - double desiredPrecision, - double* achievedPrecision, + const csm::EcefCoord& groundPt, const std::vector adj, + double desiredPrecision, double* achievedPrecision, csm::WarningList* warnings) const { - - MESSAGE_LOG("Computing groundToImage(ImageCoord) for {}, {}, {}, with desired precision {}, and " - "adjustments.", - groundPt.x, groundPt.y, groundPt.z, desiredPrecision); - - // Find time of closest approach to groundPt and the corresponding slant range by finding - // the root of the doppler shift frequency + MESSAGE_LOG( + "Computing groundToImage(ImageCoord) for {}, {}, {}, with desired " + "precision {}, and " + "adjustments.", + groundPt.x, groundPt.y, groundPt.z, desiredPrecision); + + // Find time of closest approach to groundPt and the corresponding slant range + // by finding the root of the doppler shift frequency try { double timeTolerance = m_exposureDuration * desiredPrecision / 2.0; double time = dopplerShift(groundPt, timeTolerance, adj); double slantRangeValue = slantRange(groundPt, time, adj); - // Find the ground range, based on the ground-range-to-slant-range polynomial defined by the - // range coefficient set, with a time closest to the calculated time of closest approach + // Find the ground range, based on the ground-range-to-slant-range + // polynomial defined by the range coefficient set, with a time closest to + // the calculated time of closest approach double groundTolerance = m_scaledPixelWidth * desiredPrecision / 2.0; - double groundRange = slantRangeToGroundRange(groundPt, time, slantRangeValue, groundTolerance); + double groundRange = slantRangeToGroundRange( + groundPt, time, slantRangeValue, groundTolerance); double line = (time - m_startingEphemerisTime) / m_exposureDuration + 0.5; double sample = groundRange / m_scaledPixelWidth; return csm::ImageCoord(line, sample); - } - catch (std::exception& error) { + } catch (std::exception& error) { std::string message = "Could not calculate groundToImage, with error ["; message += error.what(); message += "]"; @@ -401,50 +384,50 @@ csm::ImageCoord UsgsAstroSarSensorModel::groundToImage( } } - // Calculate the root double UsgsAstroSarSensorModel::dopplerShift( - csm::EcefCoord groundPt, - double tolerance, + csm::EcefCoord groundPt, double tolerance, const std::vector adj) const { - MESSAGE_LOG("Calculating doppler shift with: {}, {}, {}, and tolerance {}.", - groundPt.x, groundPt.y, groundPt.z, tolerance); - csm::EcefVector groundVec(groundPt.x ,groundPt.y, groundPt.z); - std::function dopplerShiftFunction = [this, groundVec, adj](double time) { - csm::EcefVector spacecraftPosition = getAdjustedSpacecraftPosition(time, adj); + MESSAGE_LOG("Calculating doppler shift with: {}, {}, {}, and tolerance {}.", + groundPt.x, groundPt.y, groundPt.z, tolerance); + csm::EcefVector groundVec(groundPt.x, groundPt.y, groundPt.z); + std::function dopplerShiftFunction = [this, groundVec, + adj](double time) { + csm::EcefVector spacecraftPosition = + getAdjustedSpacecraftPosition(time, adj); csm::EcefVector spacecraftVelocity = getAdjustedSensorVelocity(time, adj); csm::EcefVector lookVector = spacecraftPosition - groundVec; double slantRange = norm(lookVector); - double dopplerShift = -2.0 * dot(lookVector, spacecraftVelocity) / (slantRange * m_wavelength); + double dopplerShift = -2.0 * dot(lookVector, spacecraftVelocity) / + (slantRange * m_wavelength); return dopplerShift; - }; + }; // Do root-finding for "dopplerShift" double timePadding = m_exposureDuration * m_nLines * 0.25; - return brentRoot(m_startingEphemerisTime - timePadding, m_endingEphemerisTime + timePadding, - dopplerShiftFunction, tolerance); + return brentRoot(m_startingEphemerisTime - timePadding, + m_endingEphemerisTime + timePadding, dopplerShiftFunction, + tolerance); } - -double UsgsAstroSarSensorModel::slantRange(csm::EcefCoord surfPt, - double time, std::vector adj) const { +double UsgsAstroSarSensorModel::slantRange(csm::EcefCoord surfPt, double time, + std::vector adj) const { MESSAGE_LOG("Calculating slant range with: {}, {}, {}, and time {}.", surfPt.x, surfPt.y, surfPt.z, time); - csm::EcefVector surfVec(surfPt.x ,surfPt.y, surfPt.z); + csm::EcefVector surfVec(surfPt.x, surfPt.y, surfPt.z); csm::EcefVector spacecraftPosition = getAdjustedSpacecraftPosition(time, adj); return norm(spacecraftPosition - surfVec); } double UsgsAstroSarSensorModel::slantRangeToGroundRange( - const csm::EcefCoord& groundPt, - double time, - double slantRange, + const csm::EcefCoord& groundPt, double time, double slantRange, double tolerance) const { - MESSAGE_LOG("Calculating slant range to ground range with: {}, {}, {}, {}, {}, {}", - groundPt.x, groundPt.y, groundPt.z, time, slantRange, tolerance); + MESSAGE_LOG( + "Calculating slant range to ground range with: {}, {}, {}, {}, {}, {}", + groundPt.x, groundPt.y, groundPt.z, time, slantRange, tolerance); std::vector coeffs = getRangeCoefficients(time); @@ -457,23 +440,22 @@ double UsgsAstroSarSensorModel::slantRangeToGroundRange( return polynomialRoot(coeffs, guess, tolerance); } -double UsgsAstroSarSensorModel::groundRangeToSlantRange(double groundRange, const std::vector &coeffs) const { +double UsgsAstroSarSensorModel::groundRangeToSlantRange( + double groundRange, const std::vector& coeffs) const { return evaluatePolynomial(coeffs, groundRange); } - csm::ImageCoordCovar UsgsAstroSarSensorModel::groundToImage( - const csm::EcefCoordCovar& groundPt, - double desiredPrecision, - double* achievedPrecision, - csm::WarningList* warnings) const { - MESSAGE_LOG("Calculating groundToImage with: {}, {}, {}, {}", groundPt.x, groundPt.y, groundPt.z, - desiredPrecision); + const csm::EcefCoordCovar& groundPt, double desiredPrecision, + double* achievedPrecision, csm::WarningList* warnings) const { + MESSAGE_LOG("Calculating groundToImage with: {}, {}, {}, {}", groundPt.x, + groundPt.y, groundPt.z, desiredPrecision); // Ground to image with error propagation // Compute corresponding image point csm::EcefCoord gp(groundPt); - csm::ImageCoord ip = groundToImage(gp, desiredPrecision, achievedPrecision, warnings); + csm::ImageCoord ip = + groundToImage(gp, desiredPrecision, achievedPrecision, warnings); csm::ImageCoordCovar result(ip.line, ip.samp); // Compute partials ls wrt XYZ @@ -483,39 +465,27 @@ csm::ImageCoordCovar UsgsAstroSarSensorModel::groundToImage( // Error propagation double ltx, lty, ltz; double stx, sty, stz; - ltx = - prt[0] * groundPt.covariance[0] + - prt[1] * groundPt.covariance[3] + - prt[2] * groundPt.covariance[6]; - lty = - prt[0] * groundPt.covariance[1] + - prt[1] * groundPt.covariance[4] + - prt[2] * groundPt.covariance[7]; - ltz = - prt[0] * groundPt.covariance[2] + - prt[1] * groundPt.covariance[5] + - prt[2] * groundPt.covariance[8]; - stx = - prt[3] * groundPt.covariance[0] + - prt[4] * groundPt.covariance[3] + - prt[5] * groundPt.covariance[6]; - sty = - prt[3] * groundPt.covariance[1] + - prt[4] * groundPt.covariance[4] + - prt[5] * groundPt.covariance[7]; - stz = - prt[3] * groundPt.covariance[2] + - prt[4] * groundPt.covariance[5] + - prt[5] * groundPt.covariance[8]; - - double gp_cov[4]; // Input gp cov in image space + ltx = prt[0] * groundPt.covariance[0] + prt[1] * groundPt.covariance[3] + + prt[2] * groundPt.covariance[6]; + lty = prt[0] * groundPt.covariance[1] + prt[1] * groundPt.covariance[4] + + prt[2] * groundPt.covariance[7]; + ltz = prt[0] * groundPt.covariance[2] + prt[1] * groundPt.covariance[5] + + prt[2] * groundPt.covariance[8]; + stx = prt[3] * groundPt.covariance[0] + prt[4] * groundPt.covariance[3] + + prt[5] * groundPt.covariance[6]; + sty = prt[3] * groundPt.covariance[1] + prt[4] * groundPt.covariance[4] + + prt[5] * groundPt.covariance[7]; + stz = prt[3] * groundPt.covariance[2] + prt[4] * groundPt.covariance[5] + + prt[5] * groundPt.covariance[8]; + + double gp_cov[4]; // Input gp cov in image space gp_cov[0] = ltx * prt[0] + lty * prt[1] + ltz * prt[2]; gp_cov[1] = ltx * prt[3] + lty * prt[4] + ltz * prt[5]; gp_cov[2] = stx * prt[0] + sty * prt[1] + stz * prt[2]; gp_cov[3] = stx * prt[3] + sty * prt[4] + stz * prt[5]; std::vector unmodeled_cov = getUnmodeledError(ip); - double sensor_cov[4]; // sensor cov in image space + double sensor_cov[4]; // sensor cov in image space determineSensorCovarianceInImageSpace(gp, sensor_cov); result.covariance[0] = gp_cov[0] + unmodeled_cov[0] + sensor_cov[0]; @@ -527,14 +497,12 @@ csm::ImageCoordCovar UsgsAstroSarSensorModel::groundToImage( } csm::EcefCoord UsgsAstroSarSensorModel::imageToGround( - const csm::ImageCoord& imagePt, - double height, - double desiredPrecision, - double* achievedPrecision, - csm::WarningList* warnings) const { - MESSAGE_LOG("Calculating imageToGround with: {}, {}, {}, {}", imagePt.samp, imagePt.line, height, - desiredPrecision); - double time = m_startingEphemerisTime + (imagePt.line - 0.5) * m_exposureDuration; + const csm::ImageCoord& imagePt, double height, double desiredPrecision, + double* achievedPrecision, csm::WarningList* warnings) const { + MESSAGE_LOG("Calculating imageToGround with: {}, {}, {}, {}", imagePt.samp, + imagePt.line, height, desiredPrecision); + double time = + m_startingEphemerisTime + (imagePt.line - 0.5) * m_exposureDuration; double groundRange = imagePt.samp * m_scaledPixelWidth; std::vector coeffs = getRangeCoefficients(time); double slantRange = groundRangeToSlantRange(groundRange, coeffs); @@ -562,18 +530,18 @@ csm::EcefCoord UsgsAstroSarSensorModel::imageToGround( csm::EcefVector groundVec; do { radiusSqr = pointRadius * pointRadius; - double alpha = (radiusSqr - slantRange * slantRange - positionMag * positionMag) / (2 * nadirComp); + double alpha = + (radiusSqr - slantRange * slantRange - positionMag * positionMag) / + (2 * nadirComp); double beta = sqrt(slantRange * slantRange - alpha * alpha); if (m_lookDirection == LEFT) { beta *= -1; } groundVec = alpha * tHat + beta * uHat + spacecraftPosition; pointHeight = computeEllipsoidElevation( - groundVec.x, groundVec.y, groundVec.z, - m_majorAxis, m_minorAxis); + groundVec.x, groundVec.y, groundVec.z, m_majorAxis, m_minorAxis); pointRadius -= (pointHeight - height); - } while(fabs(pointHeight - height) > desiredPrecision); - + } while (fabs(pointHeight - height) > desiredPrecision); csm::EcefCoord groundPt(groundVec.x, groundVec.y, groundVec.z); @@ -581,21 +549,19 @@ csm::EcefCoord UsgsAstroSarSensorModel::imageToGround( } csm::EcefCoordCovar UsgsAstroSarSensorModel::imageToGround( - const csm::ImageCoordCovar& imagePt, - double height, - double heightVariance, - double desiredPrecision, - double* achievedPrecision, + const csm::ImageCoordCovar& imagePt, double height, double heightVariance, + double desiredPrecision, double* achievedPrecision, csm::WarningList* warnings) const { - MESSAGE_LOG("Calculating imageToGroundWith: {}, {}, {}, {}, {}", imagePt.samp, imagePt.line, - height, heightVariance, desiredPrecision); + MESSAGE_LOG("Calculating imageToGroundWith: {}, {}, {}, {}, {}", imagePt.samp, + imagePt.line, height, heightVariance, desiredPrecision); // Image to ground with error propagation // Use numerical partials const double DELTA_IMAGE = 1.0; const double DELTA_GROUND = m_scaledPixelWidth; csm::ImageCoord ip(imagePt.line, imagePt.samp); - csm::EcefCoord gp = imageToGround(ip, height, desiredPrecision, achievedPrecision, warnings); + csm::EcefCoord gp = + imageToGround(ip, height, desiredPrecision, achievedPrecision, warnings); // Compute numerical partials xyz wrt to lsh ip.line = imagePt.line + DELTA_IMAGE; @@ -613,8 +579,9 @@ csm::EcefCoordCovar UsgsAstroSarSensorModel::imageToGround( double zps = (gps.z - gp.z) / DELTA_IMAGE; ip.line = imagePt.line; - ip.samp = imagePt.samp; // +DELTA_IMAGE; - csm::EcefCoord gph = imageToGround(ip, height + DELTA_GROUND, desiredPrecision); + ip.samp = imagePt.samp; // +DELTA_IMAGE; + csm::EcefCoord gph = + imageToGround(ip, height + DELTA_GROUND, desiredPrecision); double xph = (gph.x - gp.x) / DELTA_GROUND; double yph = (gph.y - gp.y) / DELTA_GROUND; double zph = (gph.z - gp.z) / DELTA_GROUND; @@ -663,13 +630,12 @@ csm::EcefCoordCovar UsgsAstroSarSensorModel::imageToGround( } csm::EcefLocus UsgsAstroSarSensorModel::imageToProximateImagingLocus( - const csm::ImageCoord& imagePt, - const csm::EcefCoord& groundPt, - double desiredPrecision, - double* achievedPrecision, + const csm::ImageCoord& imagePt, const csm::EcefCoord& groundPt, + double desiredPrecision, double* achievedPrecision, csm::WarningList* warnings) const { // Compute the slant range - double time = m_startingEphemerisTime + (imagePt.line - 0.5) * m_exposureDuration; + double time = + m_startingEphemerisTime + (imagePt.line - 0.5) * m_exposureDuration; double groundRange = imagePt.samp * m_scaledPixelWidth; std::vector coeffs = getRangeCoefficients(time); double slantRange = groundRangeToSlantRange(groundRange, coeffs); @@ -679,32 +645,29 @@ csm::EcefLocus UsgsAstroSarSensorModel::imageToProximateImagingLocus( csm::EcefVector spacecraftPosition = getSpacecraftPosition(time); csm::EcefVector spacecraftVelocity = getSensorVelocity(time); csm::EcefVector groundVec(groundPt.x, groundPt.y, groundPt.z); - csm::EcefVector lookVec = normalized(rejection(groundVec - spacecraftPosition, spacecraftVelocity)); + csm::EcefVector lookVec = + normalized(rejection(groundVec - spacecraftPosition, spacecraftVelocity)); csm::EcefVector closestVec = spacecraftPosition + slantRange * lookVec; - // Compute the tangent at the closest point csm::EcefVector tangent; if (m_lookDirection == LEFT) { tangent = cross(spacecraftVelocity, lookVec); - } - else { + } else { tangent = cross(lookVec, spacecraftVelocity); } tangent = normalized(tangent); - return csm::EcefLocus(closestVec.x, closestVec.y, closestVec.z, - tangent.x, tangent.y, tangent.z); + return csm::EcefLocus(closestVec.x, closestVec.y, closestVec.z, tangent.x, + tangent.y, tangent.z); } csm::EcefLocus UsgsAstroSarSensorModel::imageToRemoteImagingLocus( - const csm::ImageCoord& imagePt, - double desiredPrecision, - double* achievedPrecision, - csm::WarningList* warnings) const -{ + const csm::ImageCoord& imagePt, double desiredPrecision, + double* achievedPrecision, csm::WarningList* warnings) const { // Compute the slant range - double time = m_startingEphemerisTime + (imagePt.line - 0.5) * m_exposureDuration; + double time = + m_startingEphemerisTime + (imagePt.line - 0.5) * m_exposureDuration; double groundRange = imagePt.samp * m_scaledPixelWidth; std::vector coeffs = getRangeCoefficients(time); double slantRange = groundRangeToSlantRange(groundRange, coeffs); @@ -713,145 +676,140 @@ csm::EcefLocus UsgsAstroSarSensorModel::imageToRemoteImagingLocus( // then compute the closest point at the slant range to that csm::EcefVector spacecraftPosition = getSpacecraftPosition(time); csm::EcefVector spacecraftVelocity = getSensorVelocity(time); - csm::EcefVector lookVec = normalized(rejection(-1 * spacecraftPosition, spacecraftVelocity)); + csm::EcefVector lookVec = + normalized(rejection(-1 * spacecraftPosition, spacecraftVelocity)); csm::EcefVector closestVec = spacecraftPosition + slantRange * lookVec; - // Compute the tangent at the closest point csm::EcefVector tangent; if (m_lookDirection == LEFT) { tangent = cross(spacecraftVelocity, lookVec); - } - else { + } else { tangent = cross(lookVec, spacecraftVelocity); } tangent = normalized(tangent); - return csm::EcefLocus(closestVec.x, closestVec.y, closestVec.z, - tangent.x, tangent.y, tangent.z); + return csm::EcefLocus(closestVec.x, closestVec.y, closestVec.z, tangent.x, + tangent.y, tangent.z); } -csm::ImageCoord UsgsAstroSarSensorModel::getImageStart() const -{ +csm::ImageCoord UsgsAstroSarSensorModel::getImageStart() const { return csm::ImageCoord(0.0, 0.0); } -csm::ImageVector UsgsAstroSarSensorModel::getImageSize() const -{ +csm::ImageVector UsgsAstroSarSensorModel::getImageSize() const { return csm::ImageVector(m_nLines, m_nSamples); } -pair UsgsAstroSarSensorModel::getValidImageRange() const -{ +pair +UsgsAstroSarSensorModel::getValidImageRange() const { csm::ImageCoord start = getImageStart(); csm::ImageVector size = getImageSize(); - return make_pair(start, csm::ImageCoord(start.line + size.line, start.samp + size.samp)); + return make_pair( + start, csm::ImageCoord(start.line + size.line, start.samp + size.samp)); } -pair UsgsAstroSarSensorModel::getValidHeightRange() const -{ +pair UsgsAstroSarSensorModel::getValidHeightRange() const { return make_pair(m_minElevation, m_maxElevation); } -csm::EcefVector UsgsAstroSarSensorModel::getIlluminationDirection(const csm::EcefCoord& groundPt) const -{ +csm::EcefVector UsgsAstroSarSensorModel::getIlluminationDirection( + const csm::EcefCoord& groundPt) const { csm::EcefVector groundVec(groundPt.x, groundPt.y, groundPt.z); - csm::EcefVector sunPosition = getSunPosition(getImageTime(groundToImage(groundPt))); + csm::EcefVector sunPosition = + getSunPosition(getImageTime(groundToImage(groundPt))); csm::EcefVector illuminationDirection = normalized(groundVec - sunPosition); return illuminationDirection; } -double UsgsAstroSarSensorModel::getImageTime(const csm::ImageCoord& imagePt) const -{ +double UsgsAstroSarSensorModel::getImageTime( + const csm::ImageCoord& imagePt) const { return m_startingEphemerisTime + (imagePt.line - 0.5) * m_exposureDuration; } -csm::EcefVector UsgsAstroSarSensorModel::getSpacecraftPosition(double time) const { +csm::EcefVector UsgsAstroSarSensorModel::getSpacecraftPosition( + double time) const { MESSAGE_LOG("getSpacecraftPosition at {} without adjustments", time) csm::EcefCoord spacecraftPosition = getSensorPosition(time); - return csm::EcefVector(spacecraftPosition.x, spacecraftPosition.y, spacecraftPosition.z); + return csm::EcefVector(spacecraftPosition.x, spacecraftPosition.y, + spacecraftPosition.z); } -csm::EcefVector UsgsAstroSarSensorModel::getAdjustedSpacecraftPosition(double time, std::vector adj) -const { +csm::EcefVector UsgsAstroSarSensorModel::getAdjustedSpacecraftPosition( + double time, std::vector adj) const { MESSAGE_LOG("getSpacecraftPosition at {} with adjustments", time) csm::EcefCoord spacecraftPosition = getAdjustedSensorPosition(time, adj); - return csm::EcefVector(spacecraftPosition.x, spacecraftPosition.y, spacecraftPosition.z); + return csm::EcefVector(spacecraftPosition.x, spacecraftPosition.y, + spacecraftPosition.z); } -csm::EcefCoord UsgsAstroSarSensorModel::getSensorPosition(double time) const -{ +csm::EcefCoord UsgsAstroSarSensorModel::getSensorPosition(double time) const { MESSAGE_LOG("getSensorPosition at {}.", time) - csm::EcefCoord sensorPosition = getAdjustedSensorPosition(time, m_noAdjustments); + csm::EcefCoord sensorPosition = + getAdjustedSensorPosition(time, m_noAdjustments); return sensorPosition; } - -csm::EcefCoord UsgsAstroSarSensorModel::getSensorPosition(const csm::ImageCoord& imagePt) const -{ +csm::EcefCoord UsgsAstroSarSensorModel::getSensorPosition( + const csm::ImageCoord& imagePt) const { MESSAGE_LOG("getSensorPosition at {}, {}.", imagePt.samp, imagePt.line); double time = getImageTime(imagePt); return getSensorPosition(time); } - -csm::EcefCoord UsgsAstroSarSensorModel::getAdjustedSensorPosition(double time, - std::vector adj) const -{ +csm::EcefCoord UsgsAstroSarSensorModel::getAdjustedSensorPosition( + double time, std::vector adj) const { MESSAGE_LOG("getSensorPosition at {}. With adjustments: {}.", time); int numPositions = m_positions.size(); csm::EcefVector spacecraftPosition = csm::EcefVector(); // If there are multiple positions, use Lagrange interpolation - if ((numPositions/3) > 1) { + if ((numPositions / 3) > 1) { double position[3]; - lagrangeInterp(numPositions/3, &m_positions[0], m_t0Ephem, m_dtEphem, + lagrangeInterp(numPositions / 3, &m_positions[0], m_t0Ephem, m_dtEphem, time, 3, 8, position); spacecraftPosition.x = position[0] + getValue(0, adj); spacecraftPosition.y = position[1] + getValue(1, adj); spacecraftPosition.z = position[2] + getValue(2, adj); - } - else { + } else { spacecraftPosition.x = m_positions[0] + getValue(0, adj); spacecraftPosition.y = m_positions[1] + getValue(1, adj); spacecraftPosition.z = m_positions[2] + getValue(2, adj); } - return csm::EcefCoord(spacecraftPosition.x, spacecraftPosition.y, spacecraftPosition.z); + return csm::EcefCoord(spacecraftPosition.x, spacecraftPosition.y, + spacecraftPosition.z); } - -csm::EcefVector UsgsAstroSarSensorModel::getSensorVelocity(const csm::ImageCoord& imagePt) const -{ - MESSAGE_LOG("getSensorVelocity at {}, {}. No adjustments.", imagePt.samp, imagePt.line); +csm::EcefVector UsgsAstroSarSensorModel::getSensorVelocity( + const csm::ImageCoord& imagePt) const { + MESSAGE_LOG("getSensorVelocity at {}, {}. No adjustments.", imagePt.samp, + imagePt.line); double time = getImageTime(imagePt); return getSensorVelocity(time); } -csm::EcefVector UsgsAstroSarSensorModel::getSensorVelocity(double time) const -{ +csm::EcefVector UsgsAstroSarSensorModel::getSensorVelocity(double time) const { MESSAGE_LOG("getSensorVelocity at {}. Without adjustments.", time); - csm::EcefVector spacecraftVelocity = getAdjustedSensorVelocity(time, m_noAdjustments); + csm::EcefVector spacecraftVelocity = + getAdjustedSensorVelocity(time, m_noAdjustments); return spacecraftVelocity; } -csm::EcefVector UsgsAstroSarSensorModel::getAdjustedSensorVelocity(double time, - std::vector adj) const -{ - +csm::EcefVector UsgsAstroSarSensorModel::getAdjustedSensorVelocity( + double time, std::vector adj) const { MESSAGE_LOG("getSensorVelocity at {}. With adjustments.", time); int numVelocities = m_velocities.size(); csm::EcefVector spacecraftVelocity = csm::EcefVector(); // If there are multiple positions, use Lagrange interpolation - if ((numVelocities/3) > 1) { + if ((numVelocities / 3) > 1) { double velocity[3]; - lagrangeInterp(numVelocities/3, &m_velocities[0], m_t0Ephem, m_dtEphem, + lagrangeInterp(numVelocities / 3, &m_velocities[0], m_t0Ephem, m_dtEphem, time, 3, 8, velocity); spacecraftVelocity.x = velocity[0] + getValue(3, adj); spacecraftVelocity.y = velocity[1] + getValue(4, adj); spacecraftVelocity.z = velocity[2] + getValue(5, adj); - } - else { + } else { spacecraftVelocity.x = m_velocities[0] + getValue(3, adj); spacecraftVelocity.y = m_velocities[1] + getValue(4, adj); spacecraftVelocity.z = m_velocities[2] + getValue(5, adj); @@ -860,55 +818,50 @@ csm::EcefVector UsgsAstroSarSensorModel::getAdjustedSensorVelocity(double time, } csm::RasterGM::SensorPartials UsgsAstroSarSensorModel::computeSensorPartials( - int index, - const csm::EcefCoord& groundPt, - double desiredPrecision, - double* achievedPrecision, - csm::WarningList* warnings) const -{ + int index, const csm::EcefCoord& groundPt, double desiredPrecision, + double* achievedPrecision, csm::WarningList* warnings) const { + MESSAGE_LOG( + "Calculating computeSensorPartials for ground point {}, {}, {} with " + "desired precision {}", + groundPt.x, groundPt.y, groundPt.z, desiredPrecision) - MESSAGE_LOG("Calculating computeSensorPartials for ground point {}, {}, {} with desired precision {}", - groundPt.x, groundPt.y, groundPt.z, desiredPrecision) + // Compute image coordinate first + csm::ImageCoord imgPt = + groundToImage(groundPt, desiredPrecision, achievedPrecision); - // Compute image coordinate first - csm::ImageCoord imgPt = groundToImage( - groundPt, desiredPrecision, achievedPrecision); - - // Call overloaded function - return computeSensorPartials( - index, imgPt, groundPt, desiredPrecision, achievedPrecision, warnings); + // Call overloaded function + return computeSensorPartials(index, imgPt, groundPt, desiredPrecision, + achievedPrecision, warnings); } csm::RasterGM::SensorPartials UsgsAstroSarSensorModel::computeSensorPartials( - int index, - const csm::ImageCoord& imagePt, - const csm::EcefCoord& groundPt, - double desiredPrecision, - double* achievedPrecision, - csm::WarningList* warnings) const -{ - MESSAGE_LOG("Calculating computeSensorPartials (with image points {}, {}) for ground point {}, {}, " - "{} with desired precision {}", - imagePt.line, imagePt.samp, groundPt.x, groundPt.y, groundPt.z, desiredPrecision) + int index, const csm::ImageCoord& imagePt, const csm::EcefCoord& groundPt, + double desiredPrecision, double* achievedPrecision, + csm::WarningList* warnings) const { + MESSAGE_LOG( + "Calculating computeSensorPartials (with image points {}, {}) for ground " + "point {}, {}, " + "{} with desired precision {}", + imagePt.line, imagePt.samp, groundPt.x, groundPt.y, groundPt.z, + desiredPrecision) - // Compute numerical partials wrt specific parameter + // Compute numerical partials wrt specific parameter - const double DELTA = m_scaledPixelWidth; - std::vector adj(UsgsAstroSarSensorModel::NUM_PARAMETERS, 0.0); - adj[index] = DELTA; + const double DELTA = m_scaledPixelWidth; + std::vector adj(UsgsAstroSarSensorModel::NUM_PARAMETERS, 0.0); + adj[index] = DELTA; - csm::ImageCoord adjImgPt = groundToImage( - groundPt, adj, desiredPrecision, achievedPrecision, warnings); + csm::ImageCoord adjImgPt = groundToImage(groundPt, adj, desiredPrecision, + achievedPrecision, warnings); - double linePartial = (adjImgPt.line - imagePt.line) / DELTA; - double samplePartial = (adjImgPt.samp - imagePt.samp) / DELTA; + double linePartial = (adjImgPt.line - imagePt.line) / DELTA; + double samplePartial = (adjImgPt.samp - imagePt.samp) / DELTA; - return csm::RasterGM::SensorPartials(linePartial, samplePartial); + return csm::RasterGM::SensorPartials(linePartial, samplePartial); } - -vector UsgsAstroSarSensorModel::computeGroundPartials(const csm::EcefCoord& groundPt) const -{ +vector UsgsAstroSarSensorModel::computeGroundPartials( + const csm::EcefCoord& groundPt) const { double GND_DELTA = m_scaledPixelWidth; // Partial of line, sample wrt X, Y, Z double x = groundPt.x; @@ -931,131 +884,98 @@ vector UsgsAstroSarSensorModel::computeGroundPartials(const csm::EcefCoo return partials; } -const csm::CorrelationModel& UsgsAstroSarSensorModel::getCorrelationModel() const -{ +const csm::CorrelationModel& UsgsAstroSarSensorModel::getCorrelationModel() + const { return _NO_CORR_MODEL; } vector UsgsAstroSarSensorModel::getUnmodeledCrossCovariance( - const csm::ImageCoord& pt1, - const csm::ImageCoord& pt2) const -{ + const csm::ImageCoord& pt1, const csm::ImageCoord& pt2) const { return vector(4, 0.0); } -csm::EcefCoord UsgsAstroSarSensorModel::getReferencePoint() const -{ +csm::EcefCoord UsgsAstroSarSensorModel::getReferencePoint() const { return m_referencePointXyz; } -void UsgsAstroSarSensorModel::setReferencePoint(const csm::EcefCoord& groundPt) -{ +void UsgsAstroSarSensorModel::setReferencePoint( + const csm::EcefCoord& groundPt) { m_referencePointXyz = groundPt; } -int UsgsAstroSarSensorModel::getNumParameters() const -{ - return NUM_PARAMETERS; -} +int UsgsAstroSarSensorModel::getNumParameters() const { return NUM_PARAMETERS; } -string UsgsAstroSarSensorModel::getParameterName(int index) const -{ +string UsgsAstroSarSensorModel::getParameterName(int index) const { return PARAMETER_NAME[index]; } -string UsgsAstroSarSensorModel::getParameterUnits(int index) const -{ +string UsgsAstroSarSensorModel::getParameterUnits(int index) const { return "m"; } -bool UsgsAstroSarSensorModel::hasShareableParameters() const -{ - return false; -} +bool UsgsAstroSarSensorModel::hasShareableParameters() const { return false; } -bool UsgsAstroSarSensorModel::isParameterShareable(int index) const -{ +bool UsgsAstroSarSensorModel::isParameterShareable(int index) const { return false; } -csm::SharingCriteria UsgsAstroSarSensorModel::getParameterSharingCriteria(int index) const -{ +csm::SharingCriteria UsgsAstroSarSensorModel::getParameterSharingCriteria( + int index) const { return csm::SharingCriteria(); } -double UsgsAstroSarSensorModel::getParameterValue(int index) const -{ +double UsgsAstroSarSensorModel::getParameterValue(int index) const { return m_currentParameterValue[index]; } -void UsgsAstroSarSensorModel::setParameterValue(int index, double value) -{ +void UsgsAstroSarSensorModel::setParameterValue(int index, double value) { m_currentParameterValue[index] = value; } -csm::param::Type UsgsAstroSarSensorModel::getParameterType(int index) const -{ +csm::param::Type UsgsAstroSarSensorModel::getParameterType(int index) const { return m_parameterType[index]; } -void UsgsAstroSarSensorModel::setParameterType(int index, csm::param::Type pType) -{ +void UsgsAstroSarSensorModel::setParameterType(int index, + csm::param::Type pType) { m_parameterType[index] = pType; } -double UsgsAstroSarSensorModel::getParameterCovariance( - int index1, - int index2) const -{ +double UsgsAstroSarSensorModel::getParameterCovariance(int index1, + int index2) const { return m_covariance[index1 * NUM_PARAMETERS + index2]; } - -void UsgsAstroSarSensorModel::setParameterCovariance( - int index1, - int index2, - double covariance) +void UsgsAstroSarSensorModel::setParameterCovariance(int index1, int index2, + double covariance) { m_covariance[index1 * NUM_PARAMETERS + index2] = covariance; } -int UsgsAstroSarSensorModel::getNumGeometricCorrectionSwitches() const -{ +int UsgsAstroSarSensorModel::getNumGeometricCorrectionSwitches() const { return 0; } -string UsgsAstroSarSensorModel::getGeometricCorrectionName(int index) const -{ - throw csm::Error( - csm::Error::INDEX_OUT_OF_RANGE, - "Index is out of range.", - "UsgsAstroSarSensorModel::getGeometricCorrectionName"); +string UsgsAstroSarSensorModel::getGeometricCorrectionName(int index) const { + throw csm::Error(csm::Error::INDEX_OUT_OF_RANGE, "Index is out of range.", + "UsgsAstroSarSensorModel::getGeometricCorrectionName"); } -void UsgsAstroSarSensorModel::setGeometricCorrectionSwitch(int index, - bool value, - csm::param::Type pType) -{ - throw csm::Error( - csm::Error::INDEX_OUT_OF_RANGE, - "Index is out of range.", - "UsgsAstroSarSensorModel::setGeometricCorrectionSwitch"); +void UsgsAstroSarSensorModel::setGeometricCorrectionSwitch( + int index, bool value, csm::param::Type pType) { + throw csm::Error(csm::Error::INDEX_OUT_OF_RANGE, "Index is out of range.", + "UsgsAstroSarSensorModel::setGeometricCorrectionSwitch"); } -bool UsgsAstroSarSensorModel::getGeometricCorrectionSwitch(int index) const -{ - throw csm::Error( - csm::Error::INDEX_OUT_OF_RANGE, - "Index is out of range.", - "UsgsAstroSarSensorModel::getGeometricCorrectionSwitch"); +bool UsgsAstroSarSensorModel::getGeometricCorrectionSwitch(int index) const { + throw csm::Error(csm::Error::INDEX_OUT_OF_RANGE, "Index is out of range.", + "UsgsAstroSarSensorModel::getGeometricCorrectionSwitch"); } vector UsgsAstroSarSensorModel::getCrossCovarianceMatrix( - const csm::GeometricModel& comparisonModel, - csm::param::Set pSet, - const csm::GeometricModel::GeometricModelList& otherModels) const -{ + const csm::GeometricModel& comparisonModel, csm::param::Set pSet, + const csm::GeometricModel::GeometricModelList& otherModels) const { // Return covariance matrix if (&comparisonModel == this) { vector paramIndices = getParameterSetIndices(pSet); @@ -1063,7 +983,8 @@ vector UsgsAstroSarSensorModel::getCrossCovarianceMatrix( vector covariances(numParams * numParams, 0.0); for (int i = 0; i < numParams; i++) { for (int j = 0; j < numParams; j++) { - covariances[i * numParams + j] = getParameterCovariance(paramIndices[i], paramIndices[j]); + covariances[i * numParams + j] = + getParameterCovariance(paramIndices[i], paramIndices[j]); } } return covariances; @@ -1077,134 +998,115 @@ vector UsgsAstroSarSensorModel::getCrossCovarianceMatrix( return vector(num_rows * num_cols, 0.0); } -csm::Version UsgsAstroSarSensorModel::getVersion() const -{ +csm::Version UsgsAstroSarSensorModel::getVersion() const { return csm::Version(1, 0, 0); } -string UsgsAstroSarSensorModel::getModelName() const -{ +string UsgsAstroSarSensorModel::getModelName() const { return _SENSOR_MODEL_NAME; } -string UsgsAstroSarSensorModel::getPedigree() const -{ - return "USGS_SAR"; -} +string UsgsAstroSarSensorModel::getPedigree() const { return "USGS_SAR"; } -string UsgsAstroSarSensorModel::getImageIdentifier() const -{ +string UsgsAstroSarSensorModel::getImageIdentifier() const { return m_imageIdentifier; } -void UsgsAstroSarSensorModel::setImageIdentifier( - const string& imageId, - csm::WarningList* warnings) -{ +void UsgsAstroSarSensorModel::setImageIdentifier(const string& imageId, + csm::WarningList* warnings) { m_imageIdentifier = imageId; } -string UsgsAstroSarSensorModel::getSensorIdentifier() const -{ +string UsgsAstroSarSensorModel::getSensorIdentifier() const { return m_sensorIdentifier; } -string UsgsAstroSarSensorModel::getPlatformIdentifier() const -{ +string UsgsAstroSarSensorModel::getPlatformIdentifier() const { return m_platformIdentifier; } -string UsgsAstroSarSensorModel::getCollectionIdentifier() const -{ +string UsgsAstroSarSensorModel::getCollectionIdentifier() const { return m_collectionIdentifier; } -string UsgsAstroSarSensorModel::getTrajectoryIdentifier() const -{ +string UsgsAstroSarSensorModel::getTrajectoryIdentifier() const { return m_trajectoryIdentifier; } -string UsgsAstroSarSensorModel::getSensorType() const -{ - return "SAR"; -} +string UsgsAstroSarSensorModel::getSensorType() const { return "SAR"; } -string UsgsAstroSarSensorModel::getSensorMode() const -{ - return "STRIP"; -} +string UsgsAstroSarSensorModel::getSensorMode() const { return "STRIP"; } -string UsgsAstroSarSensorModel::getReferenceDateAndTime() const -{ +string UsgsAstroSarSensorModel::getReferenceDateAndTime() const { csm::ImageCoord referencePointImage = groundToImage(m_referencePointXyz); double relativeTime = getImageTime(referencePointImage); time_t ephemTime = m_centerEphemerisTime + relativeTime; struct tm t = {0}; // Initalize to all 0's - t.tm_year = 100; // This is year-1900, so 100 = 2000 + t.tm_year = 100; // This is year-1900, so 100 = 2000 t.tm_mday = 1; time_t timeSinceEpoch = mktime(&t); time_t finalTime = ephemTime + timeSinceEpoch; - char buffer [16]; + char buffer[16]; strftime(buffer, 16, "%Y%m%dT%H%M%S", localtime(&finalTime)); buffer[15] = '\0'; return buffer; } -csm::Ellipsoid UsgsAstroSarSensorModel::getEllipsoid() const -{ - return csm::Ellipsoid(m_majorAxis, m_minorAxis); +csm::Ellipsoid UsgsAstroSarSensorModel::getEllipsoid() const { + return csm::Ellipsoid(m_majorAxis, m_minorAxis); } -void UsgsAstroSarSensorModel::setEllipsoid(const csm::Ellipsoid &ellipsoid) -{ - m_majorAxis = ellipsoid.getSemiMajorRadius(); - m_minorAxis = ellipsoid.getSemiMinorRadius(); +void UsgsAstroSarSensorModel::setEllipsoid(const csm::Ellipsoid& ellipsoid) { + m_majorAxis = ellipsoid.getSemiMajorRadius(); + m_minorAxis = ellipsoid.getSemiMinorRadius(); } void UsgsAstroSarSensorModel::determineSensorCovarianceInImageSpace( - csm::EcefCoord &gp, - double sensor_cov[4] ) const -{ + csm::EcefCoord& gp, double sensor_cov[4]) const { int i, j, totalAdjParams; totalAdjParams = getNumParameters(); - std::vector sensor_partials = computeAllSensorPartials(gp); + std::vector sensor_partials = + computeAllSensorPartials(gp); sensor_cov[0] = 0.0; sensor_cov[1] = 0.0; sensor_cov[2] = 0.0; sensor_cov[3] = 0.0; - for (i = 0; i < totalAdjParams; i++) - { - for (j = 0; j < totalAdjParams; j++) - { - sensor_cov[0] += sensor_partials[i].first * getParameterCovariance(i, j) * sensor_partials[j].first; - sensor_cov[1] += sensor_partials[i].second * getParameterCovariance(i, j) * sensor_partials[j].first; - sensor_cov[2] += sensor_partials[i].first * getParameterCovariance(i, j) * sensor_partials[j].second; - sensor_cov[3] += sensor_partials[i].second * getParameterCovariance(i, j) * sensor_partials[j].second; + for (i = 0; i < totalAdjParams; i++) { + for (j = 0; j < totalAdjParams; j++) { + sensor_cov[0] += sensor_partials[i].first * getParameterCovariance(i, j) * + sensor_partials[j].first; + sensor_cov[1] += sensor_partials[i].second * + getParameterCovariance(i, j) * sensor_partials[j].first; + sensor_cov[2] += sensor_partials[i].first * getParameterCovariance(i, j) * + sensor_partials[j].second; + sensor_cov[3] += sensor_partials[i].second * + getParameterCovariance(i, j) * sensor_partials[j].second; } } } - -std::vector UsgsAstroSarSensorModel::getRangeCoefficients(double time) const { +std::vector UsgsAstroSarSensorModel::getRangeCoefficients( + double time) const { int numCoeffs = m_scaleConversionCoefficients.size(); std::vector interpCoeffs; double endTime = m_scaleConversionTimes.back(); - if ((numCoeffs/4) > 1) { + if ((numCoeffs / 4) > 1) { double coefficients[4]; - double dtEphem = (endTime - m_scaleConversionTimes[0]) / (m_scaleConversionCoefficients.size()/4); - lagrangeInterp(m_scaleConversionCoefficients.size()/4, &m_scaleConversionCoefficients[0], m_scaleConversionTimes[0], dtEphem, - time, 4, 8, coefficients); + double dtEphem = (endTime - m_scaleConversionTimes[0]) / + (m_scaleConversionCoefficients.size() / 4); + lagrangeInterp(m_scaleConversionCoefficients.size() / 4, + &m_scaleConversionCoefficients[0], m_scaleConversionTimes[0], + dtEphem, time, 4, 8, coefficients); interpCoeffs.push_back(coefficients[0]); interpCoeffs.push_back(coefficients[1]); interpCoeffs.push_back(coefficients[2]); interpCoeffs.push_back(coefficients[3]); - } - else { + } else { interpCoeffs.push_back(m_scaleConversionCoefficients[0]); interpCoeffs.push_back(m_scaleConversionCoefficients[1]); interpCoeffs.push_back(m_scaleConversionCoefficients[2]); @@ -1213,40 +1115,39 @@ std::vector UsgsAstroSarSensorModel::getRangeCoefficients(double time) c return interpCoeffs; } -csm::EcefVector UsgsAstroSarSensorModel::getSunPosition(const double imageTime) const -{ +csm::EcefVector UsgsAstroSarSensorModel::getSunPosition( + const double imageTime) const { int numSunPositions = m_sunPosition.size(); int numSunVelocities = m_sunVelocity.size(); csm::EcefVector sunPosition = csm::EcefVector(); // If there are multiple positions, use Lagrange interpolation - if ((numSunPositions/3) > 1) { + if ((numSunPositions / 3) > 1) { double sunPos[3]; double endTime = m_t0Ephem + m_nLines * m_exposureDuration; - double sun_dtEphem = (endTime - m_t0Ephem) / (numSunPositions/3); - lagrangeInterp(numSunPositions/3, &m_sunPosition[0], m_t0Ephem, sun_dtEphem, - imageTime, 3, 8, sunPos); + double sun_dtEphem = (endTime - m_t0Ephem) / (numSunPositions / 3); + lagrangeInterp(numSunPositions / 3, &m_sunPosition[0], m_t0Ephem, + sun_dtEphem, imageTime, 3, 8, sunPos); sunPosition.x = sunPos[0]; sunPosition.y = sunPos[1]; sunPosition.z = sunPos[2]; - } - else if ((numSunVelocities/3) >= 1){ + } else if ((numSunVelocities / 3) >= 1) { // If there is one position triple with at least one velocity triple // then the illumination direction is calculated via linear extrapolation. - sunPosition.x = (imageTime * m_sunVelocity[0] + m_sunPosition[0]); - sunPosition.y = (imageTime * m_sunVelocity[1] + m_sunPosition[1]); - sunPosition.z = (imageTime * m_sunVelocity[2] + m_sunPosition[2]); - } - else { + sunPosition.x = (imageTime * m_sunVelocity[0] + m_sunPosition[0]); + sunPosition.y = (imageTime * m_sunVelocity[1] + m_sunPosition[1]); + sunPosition.z = (imageTime * m_sunVelocity[2] + m_sunPosition[2]); + } else { // If there is one position triple with no velocity triple, then the // illumination direction is the difference of the original vectors. - sunPosition.x = m_sunPosition[0]; - sunPosition.y = m_sunPosition[1]; - sunPosition.z = m_sunPosition[2]; + sunPosition.x = m_sunPosition[0]; + sunPosition.y = m_sunPosition[1]; + sunPosition.z = m_sunPosition[2]; } return sunPosition; } -double UsgsAstroSarSensorModel::getValue(int index, const std::vector &adjustments) const { +double UsgsAstroSarSensorModel::getValue( + int index, const std::vector& adjustments) const { return m_currentParameterValue[index] + adjustments[index]; } diff --git a/src/Utilities.cpp b/src/Utilities.cpp index 36e2632b3..57ce5ef8f 100644 --- a/src/Utilities.cpp +++ b/src/Utilities.cpp @@ -1,10 +1,10 @@ #include "Utilities.h" -#include #include +#include #include -#include #include +#include #include "ale/Distortion.h" @@ -13,10 +13,7 @@ using json = nlohmann::json; // Calculates a rotation matrix from Euler angles // in - euler[3] // out - rotationMatrix[9] -void calculateRotationMatrixFromEuler( - double euler[], - double rotationMatrix[]) -{ +void calculateRotationMatrixFromEuler(double euler[], double rotationMatrix[]) { double cos_a = cos(euler[0]); double sin_a = sin(euler[0]); double cos_b = cos(euler[1]); @@ -35,14 +32,11 @@ void calculateRotationMatrixFromEuler( rotationMatrix[8] = cos_a * cos_b; } - // uses a quaternion to calclate a rotation matrix. // in - q[4] // out - rotationMatrix[9] -void calculateRotationMatrixFromQuaternions( - double q[4], - double rotationMatrix[9]) -{ +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; @@ -63,28 +57,18 @@ void calculateRotationMatrixFromQuaternions( // Compue the distorted focal plane coordinate for a given image pixel // in - line // in - sample -// in - sampleOrigin - the origin of the ccd coordinate system relative to the top left of the ccd -// in - lineOrigin - the origin of the ccd coordinate system relative to the top left of the ccd -// in - sampleSumming -// in - startingSample - first ccd sample for the image -// in - iTransS[3] - the transformation from focal plane to ccd samples -// in - iTransL[3] - the transformation from focal plane to ccd lines -// out - distortedX -// out - distortedY +// in - sampleOrigin - the origin of the ccd coordinate system relative to the +// top left of the ccd in - lineOrigin - the origin of the ccd coordinate system +// relative to the top left of the ccd in - sampleSumming in - startingSample - +// first ccd sample for the image in - iTransS[3] - the transformation from +// focal plane to ccd samples in - iTransL[3] - the transformation from focal +// plane to ccd lines out - distortedX out - distortedY void computeDistortedFocalPlaneCoordinates( - const double& line, - const double& sample, - const double& sampleOrigin, - const double& lineOrigin, - const double& sampleSumming, - const double& lineSumming, - const double& startingSample, - const double& startingLine, - const double iTransS[], - const double iTransL[], - double &distortedX, - double &distortedY) -{ + const double &line, const double &sample, const double &sampleOrigin, + const double &lineOrigin, const double &sampleSumming, + const double &lineSumming, const double &startingSample, + const double &startingLine, const double iTransS[], const double iTransL[], + double &distortedX, double &distortedY) { double detSample = sample * sampleSumming + startingSample; double detLine = line * lineSumming + startingLine; double m11 = iTransL[1]; @@ -106,91 +90,72 @@ void computeDistortedFocalPlaneCoordinates( // Compue the image pixel for a distorted focal plane coordinate // in - line // in - sample -// in - sampleOrigin - the origin of the ccd coordinate system relative to the top left of the ccd -// in - lineOrigin - the origin of the ccd coordinate system relative to the top left of the ccd -// in - sampleSumming -// in - startingSample - first ccd sample for the image -// in - iTransS[3] - the transformation from focal plane to ccd samples -// in - iTransL[3] - the transformation from focal plane to ccd lines -// out - natFocalPlane -void computePixel( - const double& distortedX, - const double& distortedY, - const double& sampleOrigin, - const double& lineOrigin, - const double& sampleSumming, - const double& lineSumming, - const double& startingSample, - const double& startingLine, - const double iTransS[], - const double iTransL[], - double &line, - double &sample) -{ - double centeredSample = iTransS[0] + iTransS[1] * distortedX + iTransS[2] * distortedY; - double centeredLine = iTransL[0] + iTransL[1] * distortedX + iTransL[2] * distortedY; +// in - sampleOrigin - the origin of the ccd coordinate system relative to the +// top left of the ccd in - lineOrigin - the origin of the ccd coordinate system +// relative to the top left of the ccd in - sampleSumming in - startingSample - +// first ccd sample for the image in - iTransS[3] - the transformation from +// focal plane to ccd samples in - iTransL[3] - the transformation from focal +// plane to ccd lines out - natFocalPlane +void computePixel(const double &distortedX, const double &distortedY, + const double &sampleOrigin, const double &lineOrigin, + const double &sampleSumming, const double &lineSumming, + const double &startingSample, const double &startingLine, + const double iTransS[], const double iTransL[], double &line, + double &sample) { + double centeredSample = + iTransS[0] + iTransS[1] * distortedX + iTransS[2] * distortedY; + double centeredLine = + iTransL[0] + iTransL[1] * distortedX + iTransL[2] * distortedY; double detSample = centeredSample + sampleOrigin; double detLine = centeredLine + lineOrigin; sample = (detSample - startingSample) / sampleSumming; line = (detLine - startingLine) / lineSumming; }; -// Define imaging ray in image space (In other words, create a look vector in camera space) -// in - undistortedFocalPlaneX -// in - undistortedFocalPlaneY -// in - zDirection - Either 1 or -1. The direction of the boresight -// in - focalLength +// Define imaging ray in image space (In other words, create a look vector in +// camera space) in - undistortedFocalPlaneX in - undistortedFocalPlaneY in - +// zDirection - Either 1 or -1. The direction of the boresight in - focalLength // out - cameraLook[3] -void createCameraLookVector( - const double& undistortedFocalPlaneX, - const double& undistortedFocalPlaneY, - const double& zDirection, - const double& focalLength, - double cameraLook[]) -{ +void createCameraLookVector(const double &undistortedFocalPlaneX, + const double &undistortedFocalPlaneY, + const double &zDirection, const double &focalLength, + double cameraLook[]) { cameraLook[0] = -undistortedFocalPlaneX * zDirection; cameraLook[1] = -undistortedFocalPlaneY * zDirection; cameraLook[2] = -focalLength; - double magnitude = sqrt(cameraLook[0] * cameraLook[0] - + cameraLook[1] * cameraLook[1] - + cameraLook[2] * cameraLook[2]); + double magnitude = + sqrt(cameraLook[0] * cameraLook[0] + cameraLook[1] * cameraLook[1] + + cameraLook[2] * cameraLook[2]); cameraLook[0] /= magnitude; cameraLook[1] /= magnitude; cameraLook[2] /= magnitude; } // Lagrange Interpolation for equally spaced data -void lagrangeInterp( - const int& numTime, - const double* valueArray, - const double& startTime, - const double& delTime, - const double& time, - const int& vectorLength, - const int& i_order, - double* valueVector) { +void lagrangeInterp(const int &numTime, const double *valueArray, + const double &startTime, const double &delTime, + const double &time, const int &vectorLength, + const int &i_order, double *valueVector) { // Lagrange interpolation for uniform post interval. // Largest order possible is 8th. Points far away from // data center are handled gracefully to avoid failure. if (numTime < 2) { throw csm::Error( - csm::Error::INDEX_OUT_OF_RANGE, - "At least 2 points are required to perform Lagrange interpolation.", - "lagrangeInterp"); + csm::Error::INDEX_OUT_OF_RANGE, + "At least 2 points are required to perform Lagrange interpolation.", + "lagrangeInterp"); } // Compute index double fndex = (time - startTime) / delTime; - int index = int(fndex); + int index = int(fndex); - if (index < 0) - { + if (index < 0) { index = 0; } - if (index > numTime - 2) - { + if (index > numTime - 2) { index = numTime - 2; } @@ -199,14 +164,11 @@ void lagrangeInterp( int order; if (index >= 3 && index < numTime - 4) { order = 8; - } - else if (index >= 2 && index < numTime - 3) { + } else if (index >= 2 && index < numTime - 3) { order = 6; - } - else if (index >= 1 && index < numTime - 2) { + } else if (index >= 1 && index < numTime - 2) { order = 4; - } - else { + } else { order = 2; } if (order > i_order) { @@ -220,30 +182,27 @@ void lagrangeInterp( tm1 = tau - 1; d[0] = -tm1; d[1] = tau; - } - else if (order == 4) { + } else if (order == 4) { tp1 = tau + 1; tm1 = tau - 1; tm2 = tau - 2; d[0] = -tau * tm1 * tm2 / 6.0; - d[1] = tp1 * tm1 * tm2 / 2.0; - d[2] = -tp1 * tau * tm2 / 2.0; + d[1] = tp1 * tm1 * tm2 / 2.0; + d[2] = -tp1 * tau * tm2 / 2.0; d[3] = tp1 * tau * tm1 / 6.0; - } - else if (order == 6) { + } else if (order == 6) { tp2 = tau + 2; tp1 = tau + 1; tm1 = tau - 1; tm2 = tau - 2; tm3 = tau - 3; d[0] = -tp1 * tau * tm1 * tm2 * tm3 / 120.0; - d[1] = tp2 * tau * tm1 * tm2 * tm3 / 24.0; - d[2] = -tp2 * tp1 * tm1 * tm2 * tm3 / 12.0; - d[3] = tp2 * tp1 * tau * tm2 * tm3 / 12.0; - d[4] = -tp2 * tp1 * tau * tm1 * tm3 / 24.0; + d[1] = tp2 * tau * tm1 * tm2 * tm3 / 24.0; + d[2] = -tp2 * tp1 * tm1 * tm2 * tm3 / 12.0; + d[3] = tp2 * tp1 * tau * tm2 * tm3 / 12.0; + d[4] = -tp2 * tp1 * tau * tm1 * tm3 / 24.0; d[5] = tp2 * tp1 * tau * tm1 * tm2 / 120.0; - } - else if (order == 8) { + } else if (order == 8) { tp3 = tau + 3; tp2 = tau + 2; tp1 = tau + 1; @@ -253,42 +212,38 @@ void lagrangeInterp( tm4 = tau - 4; // Why are the denominators hard coded, as it should be x[0] - x[i] d[0] = -tp2 * tp1 * tau * tm1 * tm2 * tm3 * tm4 / 5040.0; - d[1] = tp3 * tp1 * tau * tm1 * tm2 * tm3 * tm4 / 720.0; - d[2] = -tp3 * tp2 * tau * tm1 * tm2 * tm3 * tm4 / 240.0; - d[3] = tp3 * tp2 * tp1 * tm1 * tm2 * tm3 * tm4 / 144.0; - d[4] = -tp3 * tp2 * tp1 * tau * tm2 * tm3 * tm4 / 144.0; - d[5] = tp3 * tp2 * tp1 * tau * tm1 * tm3 * tm4 / 240.0; - d[6] = -tp3 * tp2 * tp1 * tau * tm1 * tm2 * tm4 / 720.0; + d[1] = tp3 * tp1 * tau * tm1 * tm2 * tm3 * tm4 / 720.0; + d[2] = -tp3 * tp2 * tau * tm1 * tm2 * tm3 * tm4 / 240.0; + d[3] = tp3 * tp2 * tp1 * tm1 * tm2 * tm3 * tm4 / 144.0; + d[4] = -tp3 * tp2 * tp1 * tau * tm2 * tm3 * tm4 / 144.0; + d[5] = tp3 * tp2 * tp1 * tau * tm1 * tm3 * tm4 / 240.0; + d[6] = -tp3 * tp2 * tp1 * tau * tm1 * tm2 * tm4 / 720.0; d[7] = tp3 * tp2 * tp1 * tau * tm1 * tm2 * tm3 / 5040.0; } // Compute interpolated point - int indx0 = index - order / 2 + 1; - for (int i = 0; i < vectorLength; i++) - { + int indx0 = index - order / 2 + 1; + for (int i = 0; i < vectorLength; i++) { valueVector[i] = 0.0; } - for (int i = 0; i < order; i++) - { + for (int i = 0; i < order; i++) { int jndex = vectorLength * (indx0 + i); - for (int j = 0; j < vectorLength; j++) - { - valueVector[j] += d[i] * valueArray[jndex + j]; + for (int j = 0; j < vectorLength; j++) { + valueVector[j] += d[i] * valueArray[jndex + j]; } } } -double brentRoot(double lowerBound, - double upperBound, - std::function func, - double epsilon) { +double brentRoot(double lowerBound, double upperBound, + std::function func, double epsilon) { double counterPoint = lowerBound; double currentPoint = upperBound; double counterFunc = func(counterPoint); double currentFunc = func(currentPoint); if (counterFunc * currentFunc > 0.0) { - throw std::invalid_argument("Function values at the boundaries have the same sign [brentRoot]."); + throw std::invalid_argument( + "Function values at the boundaries have the same sign [brentRoot]."); } if (fabs(counterFunc) < fabs(currentFunc)) { std::swap(counterPoint, currentPoint); @@ -305,26 +260,35 @@ double brentRoot(double lowerBound, do { // Inverse quadratic interpolation - if (counterFunc != previousFunc && counterFunc != currentFunc && currentFunc != previousFunc) { - nextPoint = (counterPoint * currentFunc * previousFunc) / ((counterFunc - currentFunc) * (counterFunc - previousFunc)); - nextPoint += (currentPoint * counterFunc * previousFunc) / ((currentFunc - counterFunc) * (currentFunc - previousFunc)); - nextPoint += (previousPoint * currentFunc * counterFunc) / ((previousFunc - counterFunc) * (previousFunc - currentFunc)); + if (counterFunc != previousFunc && counterFunc != currentFunc && + currentFunc != previousFunc) { + nextPoint = (counterPoint * currentFunc * previousFunc) / + ((counterFunc - currentFunc) * (counterFunc - previousFunc)); + nextPoint += (currentPoint * counterFunc * previousFunc) / + ((currentFunc - counterFunc) * (currentFunc - previousFunc)); + nextPoint += + (previousPoint * currentFunc * counterFunc) / + ((previousFunc - counterFunc) * (previousFunc - currentFunc)); } // Secant method else { - nextPoint = currentPoint - currentFunc * (currentPoint - counterPoint) / (currentFunc - counterFunc); + nextPoint = currentPoint - currentFunc * (currentPoint - counterPoint) / + (currentFunc - counterFunc); } // Bisection method - if (((currentPoint - nextPoint) * (nextPoint - (3 * counterPoint + currentPoint) / 4) < 0) || - (bisected && fabs(nextPoint - currentPoint) >= fabs(currentPoint - previousPoint) / 2) || - (!bisected && fabs(nextPoint - currentPoint) >= fabs(previousPoint - evenOlderPoint) / 2) || + if (((currentPoint - nextPoint) * + (nextPoint - (3 * counterPoint + currentPoint) / 4) < + 0) || + (bisected && fabs(nextPoint - currentPoint) >= + fabs(currentPoint - previousPoint) / 2) || + (!bisected && fabs(nextPoint - currentPoint) >= + fabs(previousPoint - evenOlderPoint) / 2) || (bisected && fabs(currentPoint - previousPoint) < epsilon) || (!bisected && fabs(previousPoint - evenOlderPoint) < epsilon)) { nextPoint = (currentPoint + counterPoint) / 2; bisected = true; - } - else { + } else { bisected = false; } @@ -336,8 +300,7 @@ double brentRoot(double lowerBound, if (counterFunc * nextFunc < 0) { currentPoint = nextPoint; currentFunc = nextFunc; - } - else { + } else { counterPoint = nextPoint; counterFunc = nextFunc; } @@ -346,8 +309,7 @@ double brentRoot(double lowerBound, return nextPoint; } -double evaluatePolynomial(const std::vector &coeffs, - double x) { +double evaluatePolynomial(const std::vector &coeffs, double x) { if (coeffs.empty()) { throw std::invalid_argument("Polynomial coeffs must be non-empty."); } @@ -376,21 +338,20 @@ double evaluatePolynomialDerivative(const std::vector &coeffs, return value; } -double polynomialRoot(const std::vector &coeffs, - double guess, - double threshold, - int maxIterations) { +double polynomialRoot(const std::vector &coeffs, double guess, + double threshold, int maxIterations) { double root = guess; double polyValue = evaluatePolynomial(coeffs, root); double polyDeriv = 0.0; - for (int iteration = 0; iteration < maxIterations+1; iteration++) { + for (int iteration = 0; iteration < maxIterations + 1; iteration++) { if (fabs(polyValue) < threshold) { return root; } polyDeriv = evaluatePolynomialDerivative(coeffs, root); if (fabs(polyDeriv) < 1e-15) { throw std::invalid_argument("Derivative at guess (" + - std::to_string(guess) + ") is too close to 0."); + std::to_string(guess) + + ") is too close to 0."); } root -= polyValue / polyDeriv; polyValue = evaluatePolynomial(coeffs, root); @@ -399,15 +360,9 @@ double polynomialRoot(const std::vector &coeffs, std::to_string(maxIterations) + " iterations"); } -double computeEllipsoidElevation( - double x, - double y, - double z, - double semiMajor, - double semiMinor, - double desired_precision, - double* achieved_precision) -{ +double computeEllipsoidElevation(double x, double y, double z, double semiMajor, + double semiMinor, double desired_precision, + double *achieved_precision) { // Compute elevation given xyz // Requires semi-major-axis and eccentricity-square const int MKTR = 10; @@ -420,39 +375,35 @@ double computeEllipsoidElevation( double hPrev, r; // Suited for points near equator - if (d >= z) - { - double tt, zz, n; - double tanPhi = z / d; - do - { - hPrev = h; - tt = tanPhi * tanPhi; - r = semiMajor / sqrt(1.0 + ep2 * tt); - zz = z + r * ecc_sqr * tanPhi; - n = r * sqrt(1.0 + tt); - h = sqrt(d2 + zz * zz) - n; - tanPhi = zz / d; - ktr++; - } while (MKTR > ktr && fabs(h - hPrev) > desired_precision); + if (d >= z) { + double tt, zz, n; + double tanPhi = z / d; + do { + hPrev = h; + tt = tanPhi * tanPhi; + r = semiMajor / sqrt(1.0 + ep2 * tt); + zz = z + r * ecc_sqr * tanPhi; + n = r * sqrt(1.0 + tt); + h = sqrt(d2 + zz * zz) - n; + tanPhi = zz / d; + ktr++; + } while (MKTR > ktr && fabs(h - hPrev) > desired_precision); } // Suited for points near the poles - else - { - double cc, dd, nn; - double cotPhi = d / z; - do - { - hPrev = h; - cc = cotPhi * cotPhi; - r = semiMajor / sqrt(ep2 + cc); - dd = d - r * ecc_sqr * cotPhi; - nn = r * sqrt(1.0 + cc) * ep2; - h = sqrt(dd * dd + z * z) - nn; - cotPhi = dd / z; - ktr++; - } while (MKTR > ktr && fabs(h - hPrev) > desired_precision); + else { + double cc, dd, nn; + double cotPhi = d / z; + do { + hPrev = h; + cc = cotPhi * cotPhi; + r = semiMajor / sqrt(ep2 + cc); + dd = d - r * ecc_sqr * cotPhi; + nn = r * sqrt(1.0 + cc) * ep2; + h = sqrt(dd * dd + z * z) - nn; + cotPhi = dd / z; + ktr++; + } while (MKTR > ktr && fabs(h - hPrev) > desired_precision); } if (achieved_precision) { @@ -462,99 +413,76 @@ double computeEllipsoidElevation( return h; } -csm::EcefVector operator*(double scalar, const csm::EcefVector &vec) -{ - return csm::EcefVector( - scalar * vec.x, - scalar * vec.y, - scalar * vec.z); +csm::EcefVector operator*(double scalar, const csm::EcefVector &vec) { + return csm::EcefVector(scalar * vec.x, scalar * vec.y, scalar * vec.z); } -csm::EcefVector operator*(const csm::EcefVector &vec, double scalar) -{ +csm::EcefVector operator*(const csm::EcefVector &vec, double scalar) { return scalar * vec; } -csm::EcefVector operator/(const csm::EcefVector &vec, double scalar) -{ +csm::EcefVector operator/(const csm::EcefVector &vec, double scalar) { return 1.0 / scalar * vec; } -csm::EcefVector operator+(const csm::EcefVector &vec1, const csm::EcefVector &vec2) -{ - return csm::EcefVector( - vec1.x + vec2.x, - vec1.y + vec2.y, - vec1.z + vec2.z); +csm::EcefVector operator+(const csm::EcefVector &vec1, + const csm::EcefVector &vec2) { + return csm::EcefVector(vec1.x + vec2.x, vec1.y + vec2.y, vec1.z + vec2.z); } -csm::EcefVector operator-(const csm::EcefVector &vec1, const csm::EcefVector &vec2) -{ - return csm::EcefVector( - vec1.x - vec2.x, - vec1.y - vec2.y, - vec1.z - vec2.z); +csm::EcefVector operator-(const csm::EcefVector &vec1, + const csm::EcefVector &vec2) { + return csm::EcefVector(vec1.x - vec2.x, vec1.y - vec2.y, vec1.z - vec2.z); } -double dot(const csm::EcefVector &vec1, const csm::EcefVector &vec2) -{ +double dot(const csm::EcefVector &vec1, const csm::EcefVector &vec2) { return vec1.x * vec2.x + vec1.y * vec2.y + vec1.z * vec2.z; } -csm::EcefVector cross(const csm::EcefVector &vec1, const csm::EcefVector &vec2) -{ - return csm::EcefVector( - vec1.y * vec2.z - vec1.z * vec2.y, - vec1.z * vec2.x - vec1.x * vec2.z, - vec1.x * vec2.y - vec1.y * vec2.x); +csm::EcefVector cross(const csm::EcefVector &vec1, + const csm::EcefVector &vec2) { + return csm::EcefVector(vec1.y * vec2.z - vec1.z * vec2.y, + vec1.z * vec2.x - vec1.x * vec2.z, + vec1.x * vec2.y - vec1.y * vec2.x); } -double norm(const csm::EcefVector &vec) -{ +double norm(const csm::EcefVector &vec) { return sqrt(vec.x * vec.x + vec.y * vec.y + vec.z * vec.z); } -csm::EcefVector normalized(const csm::EcefVector &vec) -{ +csm::EcefVector normalized(const csm::EcefVector &vec) { return vec / norm(vec); } -csm::EcefVector projection(const csm::EcefVector &vec1, const csm::EcefVector &vec2) -{ +csm::EcefVector projection(const csm::EcefVector &vec1, + const csm::EcefVector &vec2) { return dot(vec1, vec2) / dot(vec2, vec2) * vec2; } -csm::EcefVector rejection(const csm::EcefVector &vec1, const csm::EcefVector &vec2) -{ +csm::EcefVector rejection(const csm::EcefVector &vec1, + const csm::EcefVector &vec2) { return vec1 - projection(vec1, vec2); } - // convert a measurement double metric_conversion(double val, std::string from, std::string to) { - json typemap = { - {"m", 0}, - {"km", 3} - }; - - // everything to lowercase - std::transform(from.begin(), from.end(), from.begin(), ::tolower); - std::transform(to.begin(), to.end(), to.begin(), ::tolower); - return val*pow(10, typemap[from].get() - typemap[to].get()); + json typemap = {{"m", 0}, {"km", 3}}; + + // everything to lowercase + std::transform(from.begin(), from.end(), from.begin(), ::tolower); + std::transform(to.begin(), to.end(), to.begin(), ::tolower); + return val * pow(10, typemap[from].get() - typemap[to].get()); } std::string getSensorModelName(json isd, csm::WarningList *list) { std::string name = ""; try { name = isd.at("name_model"); - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the sensor model name.", - "Utilities::getSensorModelName()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the sensor model name.", + "Utilities::getSensorModelName()")); } } return name; @@ -564,14 +492,11 @@ std::string getImageId(json isd, csm::WarningList *list) { std::string id = ""; try { id = isd.at("image_identifier"); - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the image identifier.", - "Utilities::getImageId()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the image identifier.", + "Utilities::getImageId()")); } } return id; @@ -581,14 +506,11 @@ std::string getSensorName(json isd, csm::WarningList *list) { std::string name = ""; try { name = isd.at("name_sensor"); - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the sensor name.", - "Utilities::getSensorName()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the sensor name.", + "Utilities::getSensorName()")); } } return name; @@ -598,32 +520,25 @@ std::string getPlatformName(json isd, csm::WarningList *list) { std::string name = ""; try { name = isd.at("name_platform"); - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the platform name.", - "Utilities::getPlatformName()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the platform name.", + "Utilities::getPlatformName()")); } } return name; } - std::string getLogFile(nlohmann::json isd, csm::WarningList *list) { std::string file = ""; try { file = isd.at("log_file"); - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the log filename.", - "Utilities::getLogFile()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the log filename.", + "Utilities::getLogFile()")); } } return file; @@ -633,14 +548,12 @@ int getTotalLines(json isd, csm::WarningList *list) { int lines = 0; try { lines = isd.at("image_lines"); - } - catch (...) { + } catch (...) { if (list) { list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the number of lines in the image.", - "Utilities::getTotalLines()")); + csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the number of lines in the image.", + "Utilities::getTotalLines()")); } } return lines; @@ -650,48 +563,40 @@ int getTotalSamples(json isd, csm::WarningList *list) { int samples = 0; try { samples = isd.at("image_samples"); - } - catch (...) { + } catch (...) { if (list) { list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the number of samples in the image.", - "Utilities::getTotalSamples()")); + csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the number of samples in the image.", + "Utilities::getTotalSamples()")); } } return samples; } double getStartingTime(json isd, csm::WarningList *list) { - double time = 0.0; - try { - time = isd.at("starting_ephemeris_time"); - } - catch (...) { - if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the image start time.", - "Utilities::getStartingTime()")); - } + double time = 0.0; + try { + time = isd.at("starting_ephemeris_time"); + } catch (...) { + if (list) { + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the image start time.", + "Utilities::getStartingTime()")); } - return time; + } + return time; } double getCenterTime(json isd, csm::WarningList *list) { double time = 0.0; try { time = isd.at("center_ephemeris_time"); - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the center image time.", - "Utilities::getCenterTime()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the center image time.", + "Utilities::getCenterTime()")); } } return time; @@ -701,89 +606,86 @@ double getEndingTime(json isd, csm::WarningList *list) { double time = 0.0; try { time = isd.at("ending_ephemeris_time"); - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the ending image time.", - "Utilities::getEndingTime()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the ending image time.", + "Utilities::getEndingTime()")); } } return time; } -std::vector getIntegrationStartLines(std::vector> lineScanRate, csm::WarningList *list) { +std::vector getIntegrationStartLines( + std::vector> lineScanRate, csm::WarningList *list) { std::vector lines; try { - for (auto& scanRate : lineScanRate) { + for (auto &scanRate : lineScanRate) { if (scanRate.size() != 3) { - throw csm::Error(csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE, - "Unable to parse integration start lines from line " - "scan rate due to malformed vector. Expected vector size of 3.", - "Utilities::getIntegrationStartLines()"); + throw csm::Error( + csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE, + "Unable to parse integration start lines from line " + "scan rate due to malformed vector. Expected vector size of 3.", + "Utilities::getIntegrationStartLines()"); } lines.push_back(scanRate[0]); } - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the integration start lines in the integration time table.", - "Utilities::getIntegrationStartLines()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the integration start " + "lines in the integration time table.", + "Utilities::getIntegrationStartLines()")); } } return lines; } -std::vector getIntegrationStartTimes(std::vector> lineScanRate, csm::WarningList *list) { +std::vector getIntegrationStartTimes( + std::vector> lineScanRate, csm::WarningList *list) { std::vector times; try { - for (auto& scanRate : lineScanRate) { + for (auto &scanRate : lineScanRate) { if (scanRate.size() != 3) { - throw csm::Error(csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE, - "Unable to parse integration start times from line " - "scan rate due to malformed vector. Expected vector size of 3.", - "Utilities::getIntegrationStartTimes()"); + throw csm::Error( + csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE, + "Unable to parse integration start times from line " + "scan rate due to malformed vector. Expected vector size of 3.", + "Utilities::getIntegrationStartTimes()"); } times.push_back(scanRate[1]); } - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the integration start times in the integration time table.", - "Utilities::getIntegrationStartTimes()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the integration start " + "times in the integration time table.", + "Utilities::getIntegrationStartTimes()")); } } return times; } -std::vector getIntegrationTimes(std::vector> lineScanRate, csm::WarningList *list) { +std::vector getIntegrationTimes( + std::vector> lineScanRate, csm::WarningList *list) { std::vector times; try { - for (auto& scanRate : lineScanRate) { + for (auto &scanRate : lineScanRate) { if (scanRate.size() != 3) { - throw csm::Error(csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE, - "Unable to parse integration times from line " - "scan rate due to malformed vector. Expected vector size of 3.", - "Utilities::getIntegrationTimes()"); + throw csm::Error( + csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE, + "Unable to parse integration times from line " + "scan rate due to malformed vector. Expected vector size of 3.", + "Utilities::getIntegrationTimes()"); } times.push_back(scanRate[2]); } - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the integration times in the integration time table.", - "Utilities::getIntegrationTimes()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the integration times in " + "the integration time table.", + "Utilities::getIntegrationTimes()")); } } return times; @@ -793,11 +695,9 @@ int getSampleSumming(json isd, csm::WarningList *list) { int summing = 0; try { summing = isd.at("detector_sample_summing"); - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( + list->push_back(csm::Warning( csm::Warning::DATA_NOT_AVAILABLE, "Could not parse the sample direction detector pixel summing.", "Utilities::getSampleSumming()")); @@ -810,11 +710,9 @@ int getLineSumming(json isd, csm::WarningList *list) { int summing = 0; try { summing = isd.at("detector_line_summing"); - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( + list->push_back(csm::Warning( csm::Warning::DATA_NOT_AVAILABLE, "Could not parse the line direction detector pixel summing.", "Utilities::getLineSumming()")); @@ -827,14 +725,11 @@ double getFocalLength(json isd, csm::WarningList *list) { double length = 0.0; try { length = isd.at("focal_length_model").at("focal_length"); - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the focal length.", - "Utilities::getFocalLength()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the focal length.", + "Utilities::getFocalLength()")); } } return length; @@ -844,14 +739,12 @@ double getFocalLengthEpsilon(json isd, csm::WarningList *list) { double epsilon = 0.0; try { epsilon = isd.at("focal_length_model").at("focal_epsilon"); - } - catch (...) { + } catch (...) { if (list) { list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the focal length uncertainty.", - "Utilities::getFocalLengthEpsilon()")); + csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the focal length uncertainty.", + "Utilities::getFocalLengthEpsilon()")); } } return epsilon; @@ -861,14 +754,12 @@ std::vector getFocal2PixelLines(json isd, csm::WarningList *list) { std::vector transformation; try { transformation = isd.at("focal2pixel_lines").get>(); - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the focal plane coordinate to detector lines transformation.", - "Utilities::getFocal2PixelLines()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the focal plane coordinate " + "to detector lines transformation.", + "Utilities::getFocal2PixelLines()")); } } return transformation; @@ -878,14 +769,12 @@ std::vector getFocal2PixelSamples(json isd, csm::WarningList *list) { std::vector transformation; try { transformation = isd.at("focal2pixel_samples").get>(); - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the focal plane coordinate to detector samples transformation.", - "Utilities::getFocal2PixelSamples()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the focal plane coordinate " + "to detector samples transformation.", + "Utilities::getFocal2PixelSamples()")); } } return transformation; @@ -895,14 +784,11 @@ double getDetectorCenterLine(json isd, csm::WarningList *list) { double line; try { line = isd.at("detector_center").at("line"); - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the detector center line.", - "Utilities::getDetectorCenterLine()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the detector center line.", + "Utilities::getDetectorCenterLine()")); } } return line; @@ -912,32 +798,27 @@ double getDetectorCenterSample(json isd, csm::WarningList *list) { double sample; try { sample = isd.at("detector_center").at("sample"); - } - catch (...) { + } catch (...) { if (list) { list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the detector center sample.", - "Utilities::getDetectorCenterSample()")); + csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the detector center sample.", + "Utilities::getDetectorCenterSample()")); } } return sample; } - double getDetectorStartingLine(json isd, csm::WarningList *list) { double line; try { line = isd.at("starting_detector_line"); - } - catch (...) { + } catch (...) { if (list) { list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the detector starting line.", - "Utilities::getDetectorStartingLine()")); + csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the detector starting line.", + "Utilities::getDetectorStartingLine()")); } } return line; @@ -947,14 +828,12 @@ double getDetectorStartingSample(json isd, csm::WarningList *list) { double sample; try { sample = isd.at("starting_detector_sample"); - } - catch (...) { + } catch (...) { if (list) { list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the detector starting sample.", - "Utilities::getDetectorStartingSample()")); + csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the detector starting sample.", + "Utilities::getDetectorStartingSample()")); } } return sample; @@ -966,12 +845,11 @@ double getMinHeight(json isd, csm::WarningList *list) { json referenceHeight = isd.at("reference_height"); json minHeight = referenceHeight.at("minheight"); json unit = referenceHeight.at("unit"); - height = metric_conversion(minHeight.get(), unit.get()); - } - catch (...) { + height = + metric_conversion(minHeight.get(), unit.get()); + } catch (...) { if (list) { - list->push_back( - csm::Warning( + list->push_back(csm::Warning( csm::Warning::DATA_NOT_AVAILABLE, "Could not parse the minimum height above the reference ellipsoid.", "Utilities::getMinHeight()")); @@ -986,12 +864,11 @@ double getMaxHeight(json isd, csm::WarningList *list) { json referenceHeight = isd.at("reference_height"); json maxHeight = referenceHeight.at("maxheight"); json unit = referenceHeight.at("unit"); - height = metric_conversion(maxHeight.get(), unit.get()); - } - catch (...) { + height = + metric_conversion(maxHeight.get(), unit.get()); + } catch (...) { if (list) { - list->push_back( - csm::Warning( + list->push_back(csm::Warning( csm::Warning::DATA_NOT_AVAILABLE, "Could not parse the maximum height above the reference ellipsoid.", "Utilities::getMaxHeight()")); @@ -1006,12 +883,11 @@ double getSemiMajorRadius(json isd, csm::WarningList *list) { json radii = isd.at("radii"); json semiMajor = radii.at("semimajor"); json unit = radii.at("unit"); - radius = metric_conversion(semiMajor.get(), unit.get()); - } - catch (...) { + radius = + metric_conversion(semiMajor.get(), unit.get()); + } catch (...) { if (list) { - list->push_back( - csm::Warning( + list->push_back(csm::Warning( csm::Warning::DATA_NOT_AVAILABLE, "Could not parse the reference ellipsoid semimajor radius.", "Utilities::getSemiMajorRadius()")); @@ -1020,19 +896,17 @@ double getSemiMajorRadius(json isd, csm::WarningList *list) { return radius; } - double getSemiMinorRadius(json isd, csm::WarningList *list) { double radius = 0.0; try { json radii = isd.at("radii"); json semiMinor = radii.at("semiminor"); json unit = radii.at("unit"); - radius = metric_conversion(semiMinor.get(), unit.get()); - } - catch (...) { + radius = + metric_conversion(semiMinor.get(), unit.get()); + } catch (...) { if (list) { - list->push_back( - csm::Warning( + list->push_back(csm::Warning( csm::Warning::DATA_NOT_AVAILABLE, "Could not parse the reference ellipsoid semiminor radius.", "Utilities::getSemiMinorRadius()")); @@ -1041,7 +915,6 @@ double getSemiMinorRadius(json isd, csm::WarningList *list) { return radius; } - // Converts the distortion model name from the ISD (string) to the enumeration // type. Defaults to transverse DistortionType getDistortionModel(json isd, csm::WarningList *list) { @@ -1054,65 +927,50 @@ DistortionType getDistortionModel(json isd, csm::WarningList *list) { if (distortion.compare("transverse") == 0) { return DistortionType::TRANSVERSE; - } - else if (distortion.compare("radial") == 0) { + } else if (distortion.compare("radial") == 0) { return DistortionType::RADIAL; - } - else if (distortion.compare("kaguyalism") == 0) { + } else if (distortion.compare("kaguyalism") == 0) { return DistortionType::KAGUYALISM; - } - else if (distortion.compare("dawnfc") == 0) { + } else if (distortion.compare("dawnfc") == 0) { return DistortionType::DAWNFC; - } - else if (distortion.compare("lrolrocnac") == 0) { + } else if (distortion.compare("lrolrocnac") == 0) { return DistortionType::LROLROCNAC; } - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the distortion model.", - "Utilities::getDistortionModel()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the distortion model.", + "Utilities::getDistortionModel()")); } } return DistortionType::TRANSVERSE; - } -DistortionType getDistortionModel(int aleDistortionModel, csm::WarningList *list) { +DistortionType getDistortionModel(int aleDistortionModel, + csm::WarningList *list) { try { ale::DistortionType aleDistortionType; - aleDistortionType = (ale::DistortionType) aleDistortionModel; + aleDistortionType = (ale::DistortionType)aleDistortionModel; if (aleDistortionType == ale::DistortionType::TRANSVERSE) { return DistortionType::TRANSVERSE; - } - else if (aleDistortionType == ale::DistortionType::RADIAL) { + } else if (aleDistortionType == ale::DistortionType::RADIAL) { return DistortionType::RADIAL; - } - else if (aleDistortionType == ale::DistortionType::KAGUYALISM) { + } else if (aleDistortionType == ale::DistortionType::KAGUYALISM) { return DistortionType::KAGUYALISM; - } - else if (aleDistortionType == ale::DistortionType::DAWNFC) { + } else if (aleDistortionType == ale::DistortionType::DAWNFC) { return DistortionType::DAWNFC; - } - else if (aleDistortionType == ale::DistortionType::LROLROCNAC) { + } else if (aleDistortionType == ale::DistortionType::LROLROCNAC) { return DistortionType::LROLROCNAC; } - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the distortion model.", - "Utilities::getDistortionModel()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the distortion model.", + "Utilities::getDistortionModel()")); } } return DistortionType::TRANSVERSE; - } std::vector getDistortionCoeffs(json isd, csm::WarningList *list) { @@ -1125,119 +983,131 @@ std::vector getDistortionCoeffs(json isd, csm::WarningList *list) { try { std::vector coefficientsX, coefficientsY; - coefficientsX = isd.at("optical_distortion").at("transverse").at("x").get>(); + coefficientsX = isd.at("optical_distortion") + .at("transverse") + .at("x") + .get>(); coefficientsX.resize(10, 0.0); - coefficientsY = isd.at("optical_distortion").at("transverse").at("y").get>(); + coefficientsY = isd.at("optical_distortion") + .at("transverse") + .at("y") + .get>(); coefficientsY.resize(10, 0.0); coefficients = coefficientsX; - coefficients.insert(coefficients.end(), coefficientsY.begin(), coefficientsY.end()); + coefficients.insert(coefficients.end(), coefficientsY.begin(), + coefficientsY.end()); return coefficients; - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse a set of transverse distortion model coefficients.", - "Utilities::getDistortion()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse a set of transverse " + "distortion model coefficients.", + "Utilities::getDistortion()")); } coefficients = std::vector(20, 0.0); coefficients[1] = 1.0; coefficients[12] = 1.0; } - } - break; + } break; case DistortionType::RADIAL: { try { - coefficients = isd.at("optical_distortion").at("radial").at("coefficients").get>(); + coefficients = isd.at("optical_distortion") + .at("radial") + .at("coefficients") + .get>(); return coefficients; - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( + list->push_back(csm::Warning( csm::Warning::DATA_NOT_AVAILABLE, "Could not parse the radial distortion model coefficients.", "Utilities::getDistortion()")); } coefficients = std::vector(3, 0.0); } - } - break; + } break; case DistortionType::KAGUYALISM: { try { - - std::vector coefficientsX = isd.at("optical_distortion").at("kaguyalism").at("x").get>(); - std::vector coefficientsY = isd.at("optical_distortion").at("kaguyalism").at("y").get>(); - double boresightX = isd.at("optical_distortion").at("kaguyalism").at("boresight_x").get(); - double boresightY = isd.at("optical_distortion").at("kaguyalism").at("boresight_y").get(); + std::vector coefficientsX = isd.at("optical_distortion") + .at("kaguyalism") + .at("x") + .get>(); + std::vector coefficientsY = isd.at("optical_distortion") + .at("kaguyalism") + .at("y") + .get>(); + double boresightX = isd.at("optical_distortion") + .at("kaguyalism") + .at("boresight_x") + .get(); + double boresightY = isd.at("optical_distortion") + .at("kaguyalism") + .at("boresight_y") + .get(); coefficientsX.resize(4, 0.0); coefficientsY.resize(4, 0.0); coefficientsX.insert(coefficientsX.begin(), boresightX); coefficientsY.insert(coefficientsY.begin(), boresightY); - coefficientsX.insert(coefficientsX.end(), coefficientsY.begin(), coefficientsY.end()); + coefficientsX.insert(coefficientsX.end(), coefficientsY.begin(), + coefficientsY.end()); return coefficientsX; - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse a set of Kaguya LISM distortion model coefficients.", - "Utilities::getDistortion()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse a set of Kaguya LISM " + "distortion model coefficients.", + "Utilities::getDistortion()")); } coefficients = std::vector(8, 0.0); } - } - break; + } break; case DistortionType::DAWNFC: { try { - coefficients = isd.at("optical_distortion").at("dawnfc").at("coefficients").get>(); + coefficients = isd.at("optical_distortion") + .at("dawnfc") + .at("coefficients") + .get>(); return coefficients; - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( + list->push_back(csm::Warning( csm::Warning::DATA_NOT_AVAILABLE, "Could not parse the dawn radial distortion model coefficients.", "Utilities::getDistortion()")); } coefficients = std::vector(1, 0.0); } - } - break; + } break; case DistortionType::LROLROCNAC: { try { - coefficients = isd.at("optical_distortion").at("lrolrocnac").at("coefficients").get>(); + coefficients = isd.at("optical_distortion") + .at("lrolrocnac") + .at("coefficients") + .get>(); return coefficients; - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( + list->push_back(csm::Warning( csm::Warning::DATA_NOT_AVAILABLE, "Could not parse the lrolrocnac distortion model coefficients.", "Utilities::getDistortion()")); } coefficients = std::vector(1, 0.0); } - } - break; + } break; } if (list) { list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the distortion model coefficients.", - "Utilities::getDistortion()")); + csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the distortion model coefficients.", + "Utilities::getDistortion()")); } return coefficients; @@ -1248,19 +1118,19 @@ std::vector getSunPositions(json isd, csm::WarningList *list) { try { json jayson = isd.at("sun_position"); json unit = jayson.at("unit"); - for (auto& location : jayson.at("positions")) { - positions.push_back(metric_conversion(location[0].get(), unit.get())); - positions.push_back(metric_conversion(location[1].get(), unit.get())); - positions.push_back(metric_conversion(location[2].get(), unit.get())); - } - } - catch (...) { + for (auto &location : jayson.at("positions")) { + positions.push_back(metric_conversion(location[0].get(), + unit.get())); + positions.push_back(metric_conversion(location[1].get(), + unit.get())); + positions.push_back(metric_conversion(location[2].get(), + unit.get())); + } + } catch (...) { if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the sun positions.", - "Utilities::getSunPositions()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the sun positions.", + "Utilities::getSunPositions()")); } } return positions; @@ -1271,20 +1141,20 @@ std::vector getSunVelocities(json isd, csm::WarningList *list) { try { json jayson = isd.at("sun_position"); json unit = jayson.at("unit"); - for (auto& location : jayson.at("velocities")) { - velocities.push_back(metric_conversion(location[0].get(), unit.get())); - velocities.push_back(metric_conversion(location[1].get(), unit.get())); - velocities.push_back(metric_conversion(location[2].get(), unit.get())); - } - } - catch (...) { - std::cout<<"Could not parse sun velocity"<(), + unit.get())); + velocities.push_back(metric_conversion(location[1].get(), + unit.get())); + velocities.push_back(metric_conversion(location[2].get(), + unit.get())); + } + } catch (...) { + std::cout << "Could not parse sun velocity" << std::endl; if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the sun velocities.", - "Utilities::getSunVelocities()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the sun velocities.", + "Utilities::getSunVelocities()")); } } return velocities; @@ -1295,19 +1165,19 @@ std::vector getSensorPositions(json isd, csm::WarningList *list) { try { json jayson = isd.at("sensor_position"); json unit = jayson.at("unit"); - for (auto& location : jayson.at("positions")) { - positions.push_back(metric_conversion(location[0].get(), unit.get())); - positions.push_back(metric_conversion(location[1].get(), unit.get())); - positions.push_back(metric_conversion(location[2].get(), unit.get())); - } - } - catch (...) { + for (auto &location : jayson.at("positions")) { + positions.push_back(metric_conversion(location[0].get(), + unit.get())); + positions.push_back(metric_conversion(location[1].get(), + unit.get())); + positions.push_back(metric_conversion(location[2].get(), + unit.get())); + } + } catch (...) { if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the sensor positions.", - "Utilities::getSensorPositions()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the sensor positions.", + "Utilities::getSensorPositions()")); } } return positions; @@ -1318,19 +1188,19 @@ std::vector getSensorVelocities(json isd, csm::WarningList *list) { try { json jayson = isd.at("sensor_position"); json unit = jayson.at("unit"); - for (auto& velocity : jayson.at("velocities")) { - velocities.push_back(metric_conversion(velocity[0].get(), unit.get())); - velocities.push_back(metric_conversion(velocity[1].get(), unit.get())); - velocities.push_back(metric_conversion(velocity[2].get(), unit.get())); - } - } - catch (...) { + for (auto &velocity : jayson.at("velocities")) { + velocities.push_back(metric_conversion(velocity[0].get(), + unit.get())); + velocities.push_back(metric_conversion(velocity[1].get(), + unit.get())); + velocities.push_back(metric_conversion(velocity[2].get(), + unit.get())); + } + } catch (...) { if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the sensor velocities.", - "Utilities::getSensorVelocities()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the sensor velocities.", + "Utilities::getSensorVelocities()")); } } return velocities; @@ -1339,20 +1209,17 @@ std::vector getSensorVelocities(json isd, csm::WarningList *list) { std::vector getSensorOrientations(json isd, csm::WarningList *list) { std::vector quaternions; try { - for (auto& quaternion : isd.at("sensor_orientation").at("quaternions")) { + for (auto &quaternion : isd.at("sensor_orientation").at("quaternions")) { quaternions.push_back(quaternion[0]); quaternions.push_back(quaternion[1]); quaternions.push_back(quaternion[2]); quaternions.push_back(quaternion[3]); - } - } - catch (...) { + } + } catch (...) { if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the sensor orientations.", - "Utilities::getSensorOrientations()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the sensor orientations.", + "Utilities::getSensorOrientations()")); } } return quaternions; @@ -1362,14 +1229,12 @@ double getExposureDuration(nlohmann::json isd, csm::WarningList *list) { double duration; try { duration = isd.at("line_exposure_duration"); - } - catch (...) { + } catch (...) { if (list) { list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the line exposure duration.", - "Utilities::getExposureDuration()")); + csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the line exposure duration.", + "Utilities::getExposureDuration()")); } } return duration; @@ -1379,14 +1244,11 @@ double getScaledPixelWidth(nlohmann::json isd, csm::WarningList *list) { double width; try { width = isd.at("scaled_pixel_width"); - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the scaled pixel width.", - "Utilities::getScaledPixelWidth()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the scaled pixel width.", + "Utilities::getScaledPixelWidth()")); } } return width; @@ -1396,53 +1258,48 @@ std::string getLookDirection(nlohmann::json isd, csm::WarningList *list) { std::string lookDirection = ""; try { lookDirection = isd.at("look_direction"); - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the scaled pixel width.", - "Utilities::getScaledPixelWidth()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the scaled pixel width.", + "Utilities::getScaledPixelWidth()")); } } return lookDirection; } -std::vector getScaleConversionTimes(nlohmann::json isd, csm::WarningList *list) { +std::vector getScaleConversionTimes(nlohmann::json isd, + csm::WarningList *list) { std::vector time; try { time = isd.at("range_conversion_times").get>(); - } - catch (...) { + } catch (...) { if (list) { list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the range conversion times.", - "Utilities::getScaleConversionTimes()")); + csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the range conversion times.", + "Utilities::getScaleConversionTimes()")); } } return time; } -std::vector getScaleConversionCoefficients(nlohmann::json isd, csm::WarningList *list) { +std::vector getScaleConversionCoefficients(nlohmann::json isd, + csm::WarningList *list) { std::vector coefficients; try { - for (auto& location : isd.at("range_conversion_coefficients")){ - coefficients.push_back(location[0]); - coefficients.push_back(location[1]); - coefficients.push_back(location[2]); - coefficients.push_back(location[3]); + for (auto &location : isd.at("range_conversion_coefficients")) { + coefficients.push_back(location[0]); + coefficients.push_back(location[1]); + coefficients.push_back(location[2]); + coefficients.push_back(location[3]); } - } - catch (...) { + } catch (...) { if (list) { list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the range conversion coefficients.", - "Utilities::getScaleConversionCoefficients()")); + csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the range conversion coefficients.", + "Utilities::getScaleConversionCoefficients()")); } } return coefficients; @@ -1452,14 +1309,11 @@ double getWavelength(json isd, csm::WarningList *list) { double wavelength = 0.0; try { wavelength = isd.at("wavelength"); - } - catch (...) { + } catch (...) { if (list) { - list->push_back( - csm::Warning( - csm::Warning::DATA_NOT_AVAILABLE, - "Could not parse the wavelength.", - "Utilities::getWavelength()")); + list->push_back(csm::Warning(csm::Warning::DATA_NOT_AVAILABLE, + "Could not parse the wavelength.", + "Utilities::getWavelength()")); } } return wavelength; diff --git a/tests/DistortionTests.cpp b/tests/DistortionTests.cpp index e7cb92991..7c003584f 100644 --- a/tests/DistortionTests.cpp +++ b/tests/DistortionTests.cpp @@ -6,75 +6,87 @@ // NOTE: The imagePt format is (Lines,Samples) -INSTANTIATE_TEST_SUITE_P(JacobianTest,ImageCoordParameterizedTest, - ::testing::Values(csm::ImageCoord(2.5,2.5),csm::ImageCoord(7.5,7.5))); +INSTANTIATE_TEST_SUITE_P(JacobianTest, ImageCoordParameterizedTest, + ::testing::Values(csm::ImageCoord(2.5, 2.5), + csm::ImageCoord(7.5, 7.5))); TEST_P(ImageCoordParameterizedTest, JacobianTest) { - std::vector transverseDistortionCoeffs = {1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, - 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0}; + std::vector transverseDistortionCoeffs = { + 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, + 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0}; - double Jxx,Jxy,Jyx,Jyy; + double Jxx, Jxy, Jyx, Jyy; - csm::ImageCoord imagePt = GetParam(); - double jacobian[4]; - distortionJacobian(imagePt.samp, imagePt.line, jacobian, transverseDistortionCoeffs); + csm::ImageCoord imagePt = GetParam(); + double jacobian[4]; + distortionJacobian(imagePt.samp, imagePt.line, jacobian, + transverseDistortionCoeffs); - // Jxx * Jyy - Jxy * Jyx - double determinant = fabs(jacobian[0] * jacobian[3] - jacobian[1] * jacobian[2]); - EXPECT_GT(determinant,1e-3); + // Jxx * Jyy - Jxy * Jyx + double determinant = + fabs(jacobian[0] * jacobian[3] - jacobian[1] * jacobian[2]); + EXPECT_GT(determinant, 1e-3); } TEST(Transverse, Jacobian1) { csm::ImageCoord imagePt(7.5, 7.5); - std::vector transverseDistortionCoeffs = {0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0, - 0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,1.0}; + std::vector transverseDistortionCoeffs = { + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 1.0}; double jacobian[4]; - distortionJacobian(imagePt.samp, imagePt.line, jacobian, transverseDistortionCoeffs); + distortionJacobian(imagePt.samp, imagePt.line, jacobian, + transverseDistortionCoeffs); - EXPECT_NEAR(jacobian[0],56.25,1e-8 ); - EXPECT_NEAR(jacobian[1],112.5,1e-8); - EXPECT_NEAR(jacobian[2],56.25,1e-8); - EXPECT_NEAR(jacobian[3],281.25,1e-8); + EXPECT_NEAR(jacobian[0], 56.25, 1e-8); + EXPECT_NEAR(jacobian[1], 112.5, 1e-8); + EXPECT_NEAR(jacobian[2], 56.25, 1e-8); + EXPECT_NEAR(jacobian[3], 281.25, 1e-8); } TEST(Transverse, distortMe_AlternatingOnes) { csm::ImageCoord imagePt(7.5, 7.5); - std::vector transverseDistortionCoeffs = {1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0, - 0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0}; + std::vector transverseDistortionCoeffs = { + 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, + 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0}; double dx, dy; - computeTransverseDistortion(imagePt.samp, imagePt.line, dx, dy, transverseDistortionCoeffs); + computeTransverseDistortion(imagePt.samp, imagePt.line, dx, dy, + transverseDistortionCoeffs); - EXPECT_NEAR(dx,908.5,1e-8 ); - EXPECT_NEAR(dy,963.75,1e-8); + EXPECT_NEAR(dx, 908.5, 1e-8); + EXPECT_NEAR(dy, 963.75, 1e-8); } -TEST(Transverse, distortMe_AllCoefficientsOne) { +TEST(Transverse, distortMe_AllCoefficientsOne) { csm::ImageCoord imagePt(7.5, 7.5); - std::vector transverseDistortionCoeffs = {1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0, - 1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0}; + std::vector transverseDistortionCoeffs = { + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; double dx, dy; - computeTransverseDistortion(imagePt.samp, imagePt.line, dx, dy, transverseDistortionCoeffs); + computeTransverseDistortion(imagePt.samp, imagePt.line, dx, dy, + transverseDistortionCoeffs); - EXPECT_NEAR(dx,1872.25,1e-8 ); - EXPECT_NEAR(dy,1872.25,1e-8); + EXPECT_NEAR(dx, 1872.25, 1e-8); + EXPECT_NEAR(dy, 1872.25, 1e-8); } TEST(transverse, removeDistortion1) { csm::ImageCoord imagePt(7.5, 7.5); double ux, uy; - std::vector transverseDistortionCoeffs = {0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0, - 0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0}; + std::vector transverseDistortionCoeffs = { + 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - removeDistortion(imagePt.samp, imagePt.line, ux, uy, transverseDistortionCoeffs, DistortionType::TRANSVERSE); + removeDistortion(imagePt.samp, imagePt.line, ux, uy, + transverseDistortionCoeffs, DistortionType::TRANSVERSE); - EXPECT_NEAR(imagePt.samp, 7.5, 1e-8 ); + EXPECT_NEAR(imagePt.samp, 7.5, 1e-8); EXPECT_NEAR(imagePt.line, 7.5, 1e-8); EXPECT_NEAR(imagePt.line, ux, 1e-8); EXPECT_NEAR(imagePt.samp, uy, 1e-8); @@ -84,27 +96,31 @@ TEST(transverse, removeDistortion_AllCoefficientsOne) { csm::ImageCoord imagePt(1872.25, 1872.25); double ux, uy; - std::vector transverseDistortionCoeffs = {1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0, - 1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0}; + std::vector transverseDistortionCoeffs = { + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, + 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0}; - removeDistortion(imagePt.samp, imagePt.line, ux, uy, transverseDistortionCoeffs, DistortionType::TRANSVERSE); + removeDistortion(imagePt.samp, imagePt.line, ux, uy, + transverseDistortionCoeffs, DistortionType::TRANSVERSE); - // The Jacobian is singular, so the setFocalPlane should break out of it's iteration and - // returns the same distorted coordinates that were passed in. - EXPECT_NEAR(ux, imagePt.samp,1e-8 ); - EXPECT_NEAR(uy, imagePt.line,1e-8); + // The Jacobian is singular, so the setFocalPlane should break out of it's + // iteration and returns the same distorted coordinates that were passed in. + EXPECT_NEAR(ux, imagePt.samp, 1e-8); + EXPECT_NEAR(uy, imagePt.line, 1e-8); } TEST(transverse, removeDistortion_AlternatingOnes) { csm::ImageCoord imagePt(963.75, 908.5); double ux, uy; - std::vector transverseDistortionCoeffs = {1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0, - 0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0,0.0,1.0}; + std::vector transverseDistortionCoeffs = { + 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, + 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 1.0}; - removeDistortion(imagePt.samp, imagePt.line, ux, uy, transverseDistortionCoeffs, DistortionType::TRANSVERSE); + removeDistortion(imagePt.samp, imagePt.line, ux, uy, + transverseDistortionCoeffs, DistortionType::TRANSVERSE); - EXPECT_NEAR(ux, 7.5, 1e-8 ); + EXPECT_NEAR(ux, 7.5, 1e-8); EXPECT_NEAR(uy, 7.5, 1e-8); } @@ -114,7 +130,8 @@ TEST(Radial, testRemoveDistortion) { double ux, uy; std::vector coeffs = {0, 0, 0}; - removeDistortion(imagePt.samp, imagePt.line, ux, uy, coeffs, DistortionType::RADIAL); + removeDistortion(imagePt.samp, imagePt.line, ux, uy, coeffs, + DistortionType::RADIAL); EXPECT_NEAR(ux, 4, 1e-8); EXPECT_NEAR(uy, 0, 1e-8); @@ -122,7 +139,7 @@ TEST(Radial, testRemoveDistortion) { // If coeffs are 0 then this will have the same result as removeDistortion // with 0 distortion coefficients -TEST(Radial, testInverseDistortion){ +TEST(Radial, testInverseDistortion) { csm::ImageCoord imagePt(0.0, 4.0); double dx, dy; @@ -130,13 +147,13 @@ TEST(Radial, testInverseDistortion){ std::vector coeffs = {0, 0, 0}; applyDistortion(imagePt.samp, imagePt.line, dx, dy, coeffs, - DistortionType::RADIAL, desiredPrecision); + DistortionType::RADIAL, desiredPrecision); - EXPECT_NEAR(dx,4,1e-8); - EXPECT_NEAR(dy,0,1e-8); + EXPECT_NEAR(dx, 4, 1e-8); + EXPECT_NEAR(dy, 0, 1e-8); } -TEST(Radial, testInverseOnesCoeffs){ +TEST(Radial, testInverseOnesCoeffs) { csm::ImageCoord imagePt(0.0, 4.0); double dx, dy; @@ -144,10 +161,10 @@ TEST(Radial, testInverseOnesCoeffs){ std::vector coeffs = {1, 1, 1}; applyDistortion(imagePt.samp, imagePt.line, dx, dy, coeffs, - DistortionType::RADIAL, desiredPrecision); + DistortionType::RADIAL, desiredPrecision); - EXPECT_NEAR(dx,4,1e-8); - EXPECT_NEAR(dy,0,1e-8); + EXPECT_NEAR(dx, 4, 1e-8); + EXPECT_NEAR(dy, 0, 1e-8); } TEST(DawnFc, testApply) { @@ -173,7 +190,7 @@ TEST(DawnFc, testRemove) { std::vector coeffs = {8.4e-06}; removeDistortion(imagePt.samp, imagePt.line, ux, uy, coeffs, - DistortionType::DAWNFC, desiredPrecision); + DistortionType::DAWNFC, desiredPrecision); EXPECT_NEAR(ux, 10.0, 1e-8); EXPECT_NEAR(uy, 10.0, 1e-8); @@ -189,9 +206,8 @@ TEST(DawnFc, testZeroCoeffs) { applyDistortion(imagePt.samp, imagePt.line, dx, dy, coeffs, DistortionType::DAWNFC, desiredPrecision); - removeDistortion(dx, dy, ux, uy, coeffs, - DistortionType::DAWNFC, desiredPrecision); - + removeDistortion(dx, dy, ux, uy, coeffs, DistortionType::DAWNFC, + desiredPrecision); ASSERT_DOUBLE_EQ(dx, 10.0); ASSERT_DOUBLE_EQ(dy, 10.0); @@ -204,32 +220,31 @@ TEST(KaguyaLism, testRemoveCoeffs) { double ux, uy; double desiredPrecision = 0.0000001; - std::vector distortionCoeffs = {0.5, 1, 2, 3, 4, - 0.5, 1, 2, 3, 4}; + std::vector distortionCoeffs = {0.5, 1, 2, 3, 4, 0.5, 1, 2, 3, 4}; removeDistortion(imagePt.samp, imagePt.line, ux, uy, distortionCoeffs, - DistortionType::KAGUYALISM, desiredPrecision); + DistortionType::KAGUYALISM, desiredPrecision); EXPECT_NEAR(ux, 1 + 1 + 2.828427125 + 6 + 11.313708499 + 0.5, 1e-8); EXPECT_NEAR(uy, 1 + 1 + 2.828427125 + 6 + 11.313708499 + 0.5, 1e-8); } - TEST(KaguyaLism, testCoeffs) { csm::ImageCoord imagePt(1.0, 1.0); double ux, uy, dx, dy; double desiredPrecision = 0.0000001; // Coeffs obtained from file TC1W2B0_01_05211N095E3380.img - std::vector coeffs = {-0.0725, -0.0009649900000000001, 0.00098441, 8.5773e-06, -3.7438e-06, - 0.0214, -0.0013796, 1.3502e-05, 2.7251e-06, -6.193800000000001e-06}; + std::vector coeffs = {-0.0725, -0.0009649900000000001, + 0.00098441, 8.5773e-06, + -3.7438e-06, 0.0214, + -0.0013796, 1.3502e-05, + 2.7251e-06, -6.193800000000001e-06}; removeDistortion(imagePt.samp, imagePt.line, ux, uy, coeffs, - DistortionType::KAGUYALISM, desiredPrecision); - applyDistortion(ux, uy, dx, dy, coeffs, - DistortionType::KAGUYALISM, desiredPrecision); - - + DistortionType::KAGUYALISM, desiredPrecision); + applyDistortion(ux, uy, dx, dy, coeffs, DistortionType::KAGUYALISM, + desiredPrecision); EXPECT_NEAR(ux, 0.9279337415074662, 1e-6); EXPECT_NEAR(uy, 1.0200274261995939, 1e-5); @@ -237,20 +252,18 @@ TEST(KaguyaLism, testCoeffs) { EXPECT_NEAR(dy, 1.0, 1e-8); } - TEST(KaguyaLism, testZeroCoeffs) { csm::ImageCoord imagePt(1.0, 1.0); double ux, uy, dx, dy; double desiredPrecision = 0.0000001; - std::vector coeffs = {0, 0, 0, 0, 0, - 0, 0, 0, 0, 0}; + std::vector coeffs = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; applyDistortion(imagePt.samp, imagePt.line, dx, dy, coeffs, DistortionType::KAGUYALISM, desiredPrecision); - removeDistortion(dx, dy, ux, uy, coeffs, - DistortionType::KAGUYALISM, desiredPrecision); + removeDistortion(dx, dy, ux, uy, coeffs, DistortionType::KAGUYALISM, + desiredPrecision); ASSERT_DOUBLE_EQ(dx, 1.0); ASSERT_DOUBLE_EQ(dy, 1.0); @@ -258,7 +271,6 @@ TEST(KaguyaLism, testZeroCoeffs) { ASSERT_DOUBLE_EQ(uy, 1.0); } - // Test for LRO LROC NAC TEST(LroLrocNac, testLastDetectorSample) { double ux, uy, dx, dy; @@ -267,10 +279,10 @@ TEST(LroLrocNac, testLastDetectorSample) { std::vector coeffs = {1.81E-5}; removeDistortion(0.0, 5064.0 / 2.0 * 0.007, ux, uy, coeffs, - DistortionType::LROLROCNAC, desiredPrecision); + DistortionType::LROLROCNAC, desiredPrecision); - applyDistortion(ux, uy, dx, dy, coeffs, - DistortionType::LROLROCNAC, desiredPrecision); + applyDistortion(ux, uy, dx, dy, coeffs, DistortionType::LROLROCNAC, + desiredPrecision); EXPECT_NEAR(dx, 0.0, 1e-8); EXPECT_NEAR(dy, 17.724, 1e-8); @@ -278,18 +290,17 @@ TEST(LroLrocNac, testLastDetectorSample) { EXPECT_NEAR(uy, 17.6237922244, 1e-8); } - TEST(LroLrocNac, testCoeffs) { double ux, uy, dx, dy; double desiredPrecision = 0.0000001; // Coeff obtained from file: lro_lroc_v18.ti std::vector coeffs = {1.81E-5}; - applyDistortion(0.0, 0.0, dx, dy, coeffs, - DistortionType::LROLROCNAC, desiredPrecision); + applyDistortion(0.0, 0.0, dx, dy, coeffs, DistortionType::LROLROCNAC, + desiredPrecision); - removeDistortion(dx, dy, ux, uy, coeffs, - DistortionType::LROLROCNAC, desiredPrecision); + removeDistortion(dx, dy, ux, uy, coeffs, DistortionType::LROLROCNAC, + desiredPrecision); EXPECT_NEAR(dx, 0.0, 1e-8); EXPECT_NEAR(dy, 0.0, 1e-8); @@ -297,18 +308,16 @@ TEST(LroLrocNac, testCoeffs) { EXPECT_NEAR(uy, 0.0, 1e-8); } - TEST(LroLrocNac, testZeroCoeffs) { - double ux, uy, dx, dy; double desiredPrecision = 0.0000001; std::vector coeffs = {0}; - applyDistortion(0.0, 0.0, dx, dy, coeffs, - DistortionType::LROLROCNAC, desiredPrecision); + applyDistortion(0.0, 0.0, dx, dy, coeffs, DistortionType::LROLROCNAC, + desiredPrecision); - removeDistortion(dx, dy, ux, uy, coeffs, - DistortionType::LROLROCNAC, desiredPrecision); + removeDistortion(dx, dy, ux, uy, coeffs, DistortionType::LROLROCNAC, + desiredPrecision); ASSERT_DOUBLE_EQ(dx, 0.0); ASSERT_DOUBLE_EQ(dy, 0.0); diff --git a/tests/Fixtures.h b/tests/Fixtures.h index feb7add1c..6b8060c83 100644 --- a/tests/Fixtures.h +++ b/tests/Fixtures.h @@ -1,181 +1,175 @@ #ifndef Fixtures_h #define Fixtures_h -#include "UsgsAstroPlugin.h" #include "UsgsAstroFrameSensorModel.h" #include "UsgsAstroLsSensorModel.h" +#include "UsgsAstroPlugin.h" #include "UsgsAstroSarSensorModel.h" #include +#include #include #include #include -#include #include #include // Should this be positioned somewhere in the public API? -inline void jsonToIsd(nlohmann::json &object, csm::Isd &isd, std::string prefix="") { +inline void jsonToIsd(nlohmann::json &object, csm::Isd &isd, + std::string prefix = "") { for (nlohmann::json::iterator it = object.begin(); it != object.end(); ++it) { - nlohmann::json jsonValue = it.value(); - if (jsonValue.is_array()) { - for (int i = 0; i < jsonValue.size(); i++) { - isd.addParam(prefix+it.key(), jsonValue[i].dump()); - } - } - else if (jsonValue.is_string()) { - isd.addParam(prefix+it.key(), jsonValue.get()); - } - else { - isd.addParam(prefix+it.key(), jsonValue.dump()); - } + nlohmann::json jsonValue = it.value(); + if (jsonValue.is_array()) { + for (int i = 0; i < jsonValue.size(); i++) { + isd.addParam(prefix + it.key(), jsonValue[i].dump()); + } + } else if (jsonValue.is_string()) { + isd.addParam(prefix + it.key(), jsonValue.get()); + } else { + isd.addParam(prefix + it.key(), jsonValue.dump()); + } } } //////////Framing Camera Sensor Model Fixtures////////// class FrameSensorModel : public ::testing::Test { - protected: - csm::Isd isd; - UsgsAstroFrameSensorModel *sensorModel; + protected: + csm::Isd isd; + UsgsAstroFrameSensorModel *sensorModel; - void SetUp() override { - sensorModel = NULL; + void SetUp() override { + sensorModel = NULL; - isd.setFilename("data/simpleFramerISD.img"); - UsgsAstroPlugin frameCameraPlugin; + isd.setFilename("data/simpleFramerISD.img"); + UsgsAstroPlugin frameCameraPlugin; - csm::Model *model = frameCameraPlugin.constructModelFromISD( - isd, - "USGS_ASTRO_FRAME_SENSOR_MODEL"); - sensorModel = dynamic_cast(model); + csm::Model *model = frameCameraPlugin.constructModelFromISD( + isd, "USGS_ASTRO_FRAME_SENSOR_MODEL"); + sensorModel = dynamic_cast(model); - ASSERT_NE(sensorModel, nullptr); - } + ASSERT_NE(sensorModel, nullptr); + } - void TearDown() override { - if (sensorModel) { - delete sensorModel; - sensorModel = NULL; - } - } + void TearDown() override { + if (sensorModel) { + delete sensorModel; + sensorModel = NULL; + } + } }; class FrameSensorModelLogging : public ::testing::Test { - protected: - csm::Isd isd; - UsgsAstroFrameSensorModel *sensorModel; - std::ostringstream oss; + protected: + csm::Isd isd; + UsgsAstroFrameSensorModel *sensorModel; + std::ostringstream oss; - void SetUp() override { - sensorModel = NULL; + void SetUp() override { + sensorModel = NULL; - isd.setFilename("data/simpleFramerISD.img"); - UsgsAstroPlugin frameCameraPlugin; + isd.setFilename("data/simpleFramerISD.img"); + UsgsAstroPlugin frameCameraPlugin; - csm::Model *model = frameCameraPlugin.constructModelFromISD( - isd, - "USGS_ASTRO_FRAME_SENSOR_MODEL"); - sensorModel = dynamic_cast(model); + csm::Model *model = frameCameraPlugin.constructModelFromISD( + isd, "USGS_ASTRO_FRAME_SENSOR_MODEL"); + sensorModel = dynamic_cast(model); - ASSERT_NE(sensorModel, nullptr); + ASSERT_NE(sensorModel, nullptr); - auto ostream_sink = std::make_shared (oss); - // We need a unique ID for the sensor model so that we don't have - // logger name collisions. Use the sensor model's memory addresss. - std::uintptr_t sensorId = reinterpret_cast(sensorModel); - auto logger = std::make_shared(std::to_string(sensorId), ostream_sink); - spdlog::register_logger(logger); + auto ostream_sink = std::make_shared(oss); + // We need a unique ID for the sensor model so that we don't have + // logger name collisions. Use the sensor model's memory addresss. + std::uintptr_t sensorId = reinterpret_cast(sensorModel); + auto logger = std::make_shared(std::to_string(sensorId), + ostream_sink); + spdlog::register_logger(logger); - sensorModel->setLogger(std::to_string(sensorId)); - } + sensorModel->setLogger(std::to_string(sensorId)); + } - void TearDown() override { - if (sensorModel) { - // Remove the logger from the registry for other tests - std::uintptr_t sensorId = reinterpret_cast(sensorModel); - spdlog::drop(std::to_string(sensorId)); + void TearDown() override { + if (sensorModel) { + // Remove the logger from the registry for other tests + std::uintptr_t sensorId = reinterpret_cast(sensorModel); + spdlog::drop(std::to_string(sensorId)); - delete sensorModel; - sensorModel = NULL; - } + delete sensorModel; + sensorModel = NULL; + } - EXPECT_FALSE(oss.str().empty()); - } + EXPECT_FALSE(oss.str().empty()); + } }; class OrbitalFrameSensorModel : public ::testing::Test { - protected: - csm::Isd isd; - UsgsAstroFrameSensorModel *sensorModel; + protected: + csm::Isd isd; + UsgsAstroFrameSensorModel *sensorModel; - void SetUp() override { - sensorModel = NULL; + void SetUp() override { + sensorModel = NULL; - isd.setFilename("data/orbitalFramer.img"); - UsgsAstroPlugin frameCameraPlugin; + isd.setFilename("data/orbitalFramer.img"); + UsgsAstroPlugin frameCameraPlugin; - csm::Model *model = frameCameraPlugin.constructModelFromISD( - isd, - "USGS_ASTRO_FRAME_SENSOR_MODEL"); - sensorModel = dynamic_cast(model); + csm::Model *model = frameCameraPlugin.constructModelFromISD( + isd, "USGS_ASTRO_FRAME_SENSOR_MODEL"); + sensorModel = dynamic_cast(model); - ASSERT_NE(sensorModel, nullptr); - } + ASSERT_NE(sensorModel, nullptr); + } - void TearDown() override { - if (sensorModel) { - delete sensorModel; - sensorModel = NULL; - } - } + void TearDown() override { + if (sensorModel) { + delete sensorModel; + sensorModel = NULL; + } + } }; class FrameIsdTest : public ::testing::Test { - protected: - csm::Isd isd; + protected: + csm::Isd isd; - virtual void SetUp() { - isd.setFilename("data/simpleFramerISD.img"); - } + virtual void SetUp() { isd.setFilename("data/simpleFramerISD.img"); } }; class ConstVelLineScanIsdTest : public ::testing::Test { - protected: - csm::Isd isd; + protected: + csm::Isd isd; - virtual void SetUp() { - isd.setFilename("data/constVelocityLineScan.img"); - } + virtual void SetUp() { isd.setFilename("data/constVelocityLineScan.img"); } }; -class ImageCoordParameterizedTest : public ::testing::TestWithParam {}; +class ImageCoordParameterizedTest + : public ::testing::TestWithParam {}; -class FramerParameterizedTest : public ::testing::TestWithParam { - -protected: +class FramerParameterizedTest + : public ::testing::TestWithParam { + protected: csm::Isd isd; std::string printIsd(csm::Isd &localIsd) { std::string str; - std::multimap isdmap= localIsd.parameters(); - for (auto it = isdmap.begin(); it != isdmap.end();++it){ + std::multimap isdmap = localIsd.parameters(); + for (auto it = isdmap.begin(); it != isdmap.end(); ++it) { str.append(it->first); str.append(":"); str.append(it->second); } return str; } - UsgsAstroFrameSensorModel* createModel(csm::Isd &modifiedIsd) { - + UsgsAstroFrameSensorModel *createModel(csm::Isd &modifiedIsd) { UsgsAstroPlugin frameCameraPlugin; csm::Model *model = frameCameraPlugin.constructModelFromISD( - modifiedIsd,"USGS_ASTRO_FRAME_SENSOR_MODEL"); + modifiedIsd, "USGS_ASTRO_FRAME_SENSOR_MODEL"); - UsgsAstroFrameSensorModel* sensorModel = dynamic_cast(model); + UsgsAstroFrameSensorModel *sensorModel = + dynamic_cast(model); if (sensorModel) return sensorModel; @@ -183,131 +177,124 @@ class FramerParameterizedTest : public ::testing::TestWithParam return nullptr; } - - virtual void SetUp() { - isd.setFilename("data/simpleFramerISD.img"); - }; + virtual void SetUp() { isd.setFilename("data/simpleFramerISD.img"); }; }; class FrameStateTest : public ::testing::Test { - protected: - csm::Isd isd; - UsgsAstroFrameSensorModel* createModifiedStateSensorModel(std::string key, double newValue) { - UsgsAstroPlugin cameraPlugin; - csm::Model *model = cameraPlugin.constructModelFromISD(isd,"USGS_ASTRO_FRAME_SENSOR_MODEL"); - - UsgsAstroFrameSensorModel* sensorModel = dynamic_cast(model); - if (sensorModel) { - sensorModel->getModelState(); - std::string modelState = sensorModel->getModelState(); - auto state = nlohmann::json::parse(modelState); - state[key] = newValue; - sensorModel->replaceModelState(state.dump()); - - return sensorModel; - } - else { - return nullptr; - } - } + protected: + csm::Isd isd; + UsgsAstroFrameSensorModel *createModifiedStateSensorModel(std::string key, + double newValue) { + UsgsAstroPlugin cameraPlugin; + csm::Model *model = cameraPlugin.constructModelFromISD( + isd, "USGS_ASTRO_FRAME_SENSOR_MODEL"); + + UsgsAstroFrameSensorModel *sensorModel = + dynamic_cast(model); + if (sensorModel) { + sensorModel->getModelState(); + std::string modelState = sensorModel->getModelState(); + auto state = nlohmann::json::parse(modelState); + state[key] = newValue; + sensorModel->replaceModelState(state.dump()); - void SetUp() override { - isd.setFilename("data/simpleFramerISD.img"); + return sensorModel; + } else { + return nullptr; } + } + + void SetUp() override { isd.setFilename("data/simpleFramerISD.img"); } }; //////////Line Scan Camera Sensor Model Fixtures////////// class ConstVelocityLineScanSensorModel : public ::testing::Test { - protected: - csm::Isd isd; - UsgsAstroLsSensorModel *sensorModel; + protected: + csm::Isd isd; + UsgsAstroLsSensorModel *sensorModel; - void SetUp() override { - sensorModel = NULL; + void SetUp() override { + sensorModel = NULL; - isd.setFilename("data/constVelocityLineScan.img"); - UsgsAstroPlugin cameraPlugin; + isd.setFilename("data/constVelocityLineScan.img"); + UsgsAstroPlugin cameraPlugin; - csm::Model *model = cameraPlugin.constructModelFromISD( - isd, - "USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL"); - sensorModel = dynamic_cast(model); + csm::Model *model = cameraPlugin.constructModelFromISD( + isd, "USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL"); + sensorModel = dynamic_cast(model); - ASSERT_NE(sensorModel, nullptr); - } + ASSERT_NE(sensorModel, nullptr); + } - void TearDown() override { - if (sensorModel) { - delete sensorModel; - sensorModel = NULL; - } - } + void TearDown() override { + if (sensorModel) { + delete sensorModel; + sensorModel = NULL; + } + } }; class OrbitalLineScanSensorModel : public ::testing::Test { - protected: - csm::Isd isd; - UsgsAstroLsSensorModel *sensorModel; + protected: + csm::Isd isd; + UsgsAstroLsSensorModel *sensorModel; - void SetUp() override { - sensorModel = NULL; + void SetUp() override { + sensorModel = NULL; - isd.setFilename("data/orbitalLineScan.img"); - UsgsAstroPlugin cameraPlugin; + isd.setFilename("data/orbitalLineScan.img"); + UsgsAstroPlugin cameraPlugin; - csm::Model *model = cameraPlugin.constructModelFromISD( - isd, - "USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL"); - sensorModel = dynamic_cast(model); + csm::Model *model = cameraPlugin.constructModelFromISD( + isd, "USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL"); + sensorModel = dynamic_cast(model); - ASSERT_NE(sensorModel, nullptr); - } + ASSERT_NE(sensorModel, nullptr); + } - void TearDown() override { - if (sensorModel) { - delete sensorModel; - sensorModel = NULL; - } - } + void TearDown() override { + if (sensorModel) { + delete sensorModel; + sensorModel = NULL; + } + } }; class TwoLineScanSensorModels : public ::testing::Test { - protected: - csm::Isd isd; - UsgsAstroLsSensorModel *sensorModel1; - UsgsAstroLsSensorModel *sensorModel2; - - void SetUp() override { - sensorModel1 = nullptr; - sensorModel2 = nullptr; - - isd.setFilename("data/orbitalLineScan.img"); - UsgsAstroPlugin cameraPlugin; - - csm::Model *model1 = cameraPlugin.constructModelFromISD( - isd, - "USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL"); - sensorModel1 = dynamic_cast(model1); - csm::Model *model2 = cameraPlugin.constructModelFromISD( - isd, - "USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL"); - sensorModel2 = dynamic_cast(model2); - - ASSERT_NE(sensorModel1, nullptr); - ASSERT_NE(sensorModel2, nullptr); - } + protected: + csm::Isd isd; + UsgsAstroLsSensorModel *sensorModel1; + UsgsAstroLsSensorModel *sensorModel2; - void TearDown() override { - if (sensorModel1) { - delete sensorModel1; - sensorModel1 = nullptr; - } - if (sensorModel2) { - delete sensorModel2; - sensorModel2 = nullptr; - } - } + void SetUp() override { + sensorModel1 = nullptr; + sensorModel2 = nullptr; + + isd.setFilename("data/orbitalLineScan.img"); + UsgsAstroPlugin cameraPlugin; + + csm::Model *model1 = cameraPlugin.constructModelFromISD( + isd, "USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL"); + sensorModel1 = dynamic_cast(model1); + csm::Model *model2 = cameraPlugin.constructModelFromISD( + isd, "USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL"); + sensorModel2 = dynamic_cast(model2); + + ASSERT_NE(sensorModel1, nullptr); + ASSERT_NE(sensorModel2, nullptr); + } + + void TearDown() override { + if (sensorModel1) { + delete sensorModel1; + sensorModel1 = nullptr; + } + if (sensorModel2) { + delete sensorModel2; + sensorModel2 = nullptr; + } + } }; ////////////////// @@ -315,38 +302,35 @@ class TwoLineScanSensorModels : public ::testing::Test { ////////////////// class SarIsdTest : public ::testing::Test { - protected: - csm::Isd isd; + protected: + csm::Isd isd; - virtual void SetUp() { - isd.setFilename("data/orbitalSar.img"); - } + virtual void SetUp() { isd.setFilename("data/orbitalSar.img"); } }; class SarSensorModel : public ::testing::Test { - protected: - csm::Isd isd; - UsgsAstroSarSensorModel *sensorModel; + protected: + csm::Isd isd; + UsgsAstroSarSensorModel *sensorModel; - void SetUp() override { - sensorModel = NULL; + void SetUp() override { + sensorModel = NULL; - isd.setFilename("data/orbitalSar.img"); - UsgsAstroPlugin sarCameraPlugin; + isd.setFilename("data/orbitalSar.img"); + UsgsAstroPlugin sarCameraPlugin; - csm::Model *model = sarCameraPlugin.constructModelFromISD( - isd, - "USGS_ASTRO_SAR_SENSOR_MODEL"); - sensorModel = dynamic_cast(model); - ASSERT_NE(sensorModel, nullptr); - } + csm::Model *model = sarCameraPlugin.constructModelFromISD( + isd, "USGS_ASTRO_SAR_SENSOR_MODEL"); + sensorModel = dynamic_cast(model); + ASSERT_NE(sensorModel, nullptr); + } - void TearDown() override { - if (sensorModel) { - delete sensorModel; - sensorModel = NULL; - } - } + void TearDown() override { + if (sensorModel) { + delete sensorModel; + sensorModel = NULL; + } + } }; #endif diff --git a/tests/FrameCameraTests.cpp b/tests/FrameCameraTests.cpp index 51d718d27..daf7324aa 100644 --- a/tests/FrameCameraTests.cpp +++ b/tests/FrameCameraTests.cpp @@ -1,8 +1,8 @@ -#include "UsgsAstroPlugin.h" #include "UsgsAstroFrameSensorModel.h" +#include "UsgsAstroPlugin.h" -#include #include +#include #include "Fixtures.h" @@ -14,52 +14,50 @@ using json = nlohmann::json; // **************************************************************************** TEST_F(FrameSensorModel, State) { - std::string modelState = sensorModel->getModelState(); - EXPECT_NO_THROW( - sensorModel->replaceModelState(modelState) - ); - EXPECT_EQ(sensorModel->getModelState(), modelState); + std::string modelState = sensorModel->getModelState(); + EXPECT_NO_THROW(sensorModel->replaceModelState(modelState)); + EXPECT_EQ(sensorModel->getModelState(), modelState); } // centered and slightly off-center: TEST_F(FrameSensorModel, Center) { - csm::ImageCoord imagePt(7.5, 7.5); - csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); - EXPECT_NEAR(groundPt.x, 10.0, 1e-8); - EXPECT_NEAR(groundPt.y, 0, 1e-8); - EXPECT_NEAR(groundPt.z, 0, 1e-8); + csm::ImageCoord imagePt(7.5, 7.5); + csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); + EXPECT_NEAR(groundPt.x, 10.0, 1e-8); + EXPECT_NEAR(groundPt.y, 0, 1e-8); + EXPECT_NEAR(groundPt.z, 0, 1e-8); } TEST_F(FrameSensorModel, SlightlyOffCenter) { - csm::ImageCoord imagePt(7.5, 6.5); - csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); - EXPECT_NEAR(groundPt.x, 9.80194018, 1e-8); - EXPECT_NEAR(groundPt.y, 0, 1e-8); - EXPECT_NEAR(groundPt.z, 1.98039612, 1e-8); + csm::ImageCoord imagePt(7.5, 6.5); + csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); + EXPECT_NEAR(groundPt.x, 9.80194018, 1e-8); + EXPECT_NEAR(groundPt.y, 0, 1e-8); + EXPECT_NEAR(groundPt.z, 1.98039612, 1e-8); } // Test all four corners: TEST_F(FrameSensorModel, OffBody1) { - csm::ImageCoord imagePt(15.0, 0.0); - csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); - EXPECT_NEAR(groundPt.x, 0.44979759, 1e-8); - EXPECT_NEAR(groundPt.y, -14.99325304, 1e-8); - EXPECT_NEAR(groundPt.z, 14.99325304, 1e-8); + csm::ImageCoord imagePt(15.0, 0.0); + csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); + EXPECT_NEAR(groundPt.x, 0.44979759, 1e-8); + EXPECT_NEAR(groundPt.y, -14.99325304, 1e-8); + EXPECT_NEAR(groundPt.z, 14.99325304, 1e-8); } TEST_F(FrameSensorModel, OffBody2) { - csm::ImageCoord imagePt(0.0, 15.0); - csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); - EXPECT_NEAR(groundPt.x, 0.44979759, 1e-8); - EXPECT_NEAR(groundPt.y, 14.99325304, 1e-8); - EXPECT_NEAR(groundPt.z, -14.99325304, 1e-8); + csm::ImageCoord imagePt(0.0, 15.0); + csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); + EXPECT_NEAR(groundPt.x, 0.44979759, 1e-8); + EXPECT_NEAR(groundPt.y, 14.99325304, 1e-8); + EXPECT_NEAR(groundPt.z, -14.99325304, 1e-8); } TEST_F(FrameSensorModel, OffBody3) { - csm::ImageCoord imagePt(0.0, 0.0); - csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); - EXPECT_NEAR(groundPt.x, 0.44979759, 1e-8); - EXPECT_NEAR(groundPt.y, 14.99325304, 1e-8); - EXPECT_NEAR(groundPt.z, 14.99325304, 1e-8); + csm::ImageCoord imagePt(0.0, 0.0); + csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); + EXPECT_NEAR(groundPt.x, 0.44979759, 1e-8); + EXPECT_NEAR(groundPt.y, 14.99325304, 1e-8); + EXPECT_NEAR(groundPt.z, 14.99325304, 1e-8); } TEST_F(FrameSensorModel, getReferencePoint) { @@ -72,11 +70,11 @@ TEST_F(FrameSensorModel, getReferencePoint) { } TEST_F(FrameSensorModel, OffBody4) { - csm::ImageCoord imagePt(15.0, 15.0); - csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); - EXPECT_NEAR(groundPt.x, 0.44979759, 1e-8); - EXPECT_NEAR(groundPt.y, -14.99325304, 1e-8); - EXPECT_NEAR(groundPt.z, -14.99325304, 1e-8); + csm::ImageCoord imagePt(15.0, 15.0); + csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); + EXPECT_NEAR(groundPt.x, 0.44979759, 1e-8); + EXPECT_NEAR(groundPt.y, -14.99325304, 1e-8); + EXPECT_NEAR(groundPt.z, -14.99325304, 1e-8); } TEST_F(FrameSensorModel, getImageIdentifier) { @@ -84,11 +82,11 @@ TEST_F(FrameSensorModel, getImageIdentifier) { } TEST_F(FrameSensorModel, Inversion) { - csm::ImageCoord imagePt1(9.0, 9.0); - csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt1, 0.0); - csm::ImageCoord imagePt2 = sensorModel->groundToImage(groundPt); - EXPECT_DOUBLE_EQ(imagePt1.line, imagePt2.line); - EXPECT_DOUBLE_EQ(imagePt1.samp, imagePt2.samp); + csm::ImageCoord imagePt1(9.0, 9.0); + csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt1, 0.0); + csm::ImageCoord imagePt2 = sensorModel->groundToImage(groundPt); + EXPECT_DOUBLE_EQ(imagePt1.line, imagePt2.line); + EXPECT_DOUBLE_EQ(imagePt1.samp, imagePt2.samp); } // **************************************************************************** @@ -96,40 +94,40 @@ TEST_F(FrameSensorModel, Inversion) { // **************************************************************************** TEST_F(OrbitalFrameSensorModel, Center) { - csm::ImageCoord imagePt(8.0, 8.0); - csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); - EXPECT_DOUBLE_EQ(groundPt.x, 1000000.0); - EXPECT_DOUBLE_EQ(groundPt.y, 0); - EXPECT_DOUBLE_EQ(groundPt.z, 0); + csm::ImageCoord imagePt(8.0, 8.0); + csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); + EXPECT_DOUBLE_EQ(groundPt.x, 1000000.0); + EXPECT_DOUBLE_EQ(groundPt.y, 0); + EXPECT_DOUBLE_EQ(groundPt.z, 0); } TEST_F(FrameSensorModel, Radii) { - csm::Ellipsoid ellipsoid = sensorModel->getEllipsoid(); - EXPECT_DOUBLE_EQ(ellipsoid.getSemiMajorRadius(), 10); - EXPECT_DOUBLE_EQ(ellipsoid.getSemiMinorRadius(), 10); + csm::Ellipsoid ellipsoid = sensorModel->getEllipsoid(); + EXPECT_DOUBLE_EQ(ellipsoid.getSemiMajorRadius(), 10); + EXPECT_DOUBLE_EQ(ellipsoid.getSemiMinorRadius(), 10); } TEST_F(FrameSensorModel, SetRadii) { - csm::Ellipsoid ellipsoid1(1000, 1500); - sensorModel->setEllipsoid(ellipsoid1); - csm::Ellipsoid ellipsoid2 = sensorModel->getEllipsoid(); - EXPECT_DOUBLE_EQ(ellipsoid2.getSemiMajorRadius(), 1000); - EXPECT_DOUBLE_EQ(ellipsoid2.getSemiMinorRadius(), 1500); + csm::Ellipsoid ellipsoid1(1000, 1500); + sensorModel->setEllipsoid(ellipsoid1); + csm::Ellipsoid ellipsoid2 = sensorModel->getEllipsoid(); + EXPECT_DOUBLE_EQ(ellipsoid2.getSemiMajorRadius(), 1000); + EXPECT_DOUBLE_EQ(ellipsoid2.getSemiMinorRadius(), 1500); } TEST_F(OrbitalFrameSensorModel, GroundPartials) { - csm::EcefCoord groundPt(1000000.0, 0.0, 0.0); - std::vector partials = sensorModel->computeGroundPartials(groundPt); - // Pixels are 100m - // lines are increasing z and samples are increasing y in body fixed - // lines partials should be 0 except for the z partial which should be 1/100 - // sample partials should be 0 except for the y partial which should be 1/100 - EXPECT_DOUBLE_EQ(partials[0], 0.0); - EXPECT_DOUBLE_EQ(partials[1], 0.0); - EXPECT_DOUBLE_EQ(partials[2], 1 / 100.0); - EXPECT_DOUBLE_EQ(partials[3], 0.0); - EXPECT_DOUBLE_EQ(partials[4], 1 / 100.0); - EXPECT_DOUBLE_EQ(partials[5], 0.0); + csm::EcefCoord groundPt(1000000.0, 0.0, 0.0); + std::vector partials = sensorModel->computeGroundPartials(groundPt); + // Pixels are 100m + // lines are increasing z and samples are increasing y in body fixed + // lines partials should be 0 except for the z partial which should be 1/100 + // sample partials should be 0 except for the y partial which should be 1/100 + EXPECT_DOUBLE_EQ(partials[0], 0.0); + EXPECT_DOUBLE_EQ(partials[1], 0.0); + EXPECT_DOUBLE_EQ(partials[2], 1 / 100.0); + EXPECT_DOUBLE_EQ(partials[3], 0.0); + EXPECT_DOUBLE_EQ(partials[4], 1 / 100.0); + EXPECT_DOUBLE_EQ(partials[5], 0.0); } // **************************************************************************** @@ -140,7 +138,8 @@ TEST_F(OrbitalFrameSensorModel, GroundPartials) { TEST_F(FrameStateTest, FL500_OffBody4) { std::string key = "m_focalLength"; double newValue = 500.0; - UsgsAstroFrameSensorModel* sensorModel = createModifiedStateSensorModel(key, newValue); + UsgsAstroFrameSensorModel* sensorModel = + createModifiedStateSensorModel(key, newValue); ASSERT_NE(sensorModel, nullptr); csm::ImageCoord imagePt(15.0, 15.0); @@ -156,7 +155,8 @@ TEST_F(FrameStateTest, FL500_OffBody4) { TEST_F(FrameStateTest, FL500_OffBody3) { std::string key = "m_focalLength"; double newValue = 500.0; - UsgsAstroFrameSensorModel* sensorModel = createModifiedStateSensorModel(key, newValue); + UsgsAstroFrameSensorModel* sensorModel = + createModifiedStateSensorModel(key, newValue); ASSERT_NE(sensorModel, nullptr); csm::ImageCoord imagePt(0.0, 0.0); @@ -172,7 +172,8 @@ TEST_F(FrameStateTest, FL500_OffBody3) { TEST_F(FrameStateTest, FL500_Center) { std::string key = "m_focalLength"; double newValue = 500.0; - UsgsAstroFrameSensorModel* sensorModel = createModifiedStateSensorModel(key, newValue); + UsgsAstroFrameSensorModel* sensorModel = + createModifiedStateSensorModel(key, newValue); ASSERT_NE(sensorModel, nullptr); csm::ImageCoord imagePt(7.5, 7.5); @@ -188,7 +189,8 @@ TEST_F(FrameStateTest, FL500_Center) { TEST_F(FrameStateTest, FL500_SlightlyOffCenter) { std::string key = "m_focalLength"; double newValue = 500.0; - UsgsAstroFrameSensorModel* sensorModel = createModifiedStateSensorModel(key, newValue); + UsgsAstroFrameSensorModel* sensorModel = + createModifiedStateSensorModel(key, newValue); ASSERT_NE(sensorModel, nullptr); csm::ImageCoord imagePt(7.5, 6.5); @@ -206,7 +208,8 @@ TEST_F(FrameStateTest, SemiMajorAxis100x_Center) { std::string key = "m_majorAxis"; double newValue = 1000.0; - UsgsAstroFrameSensorModel* sensorModel = createModifiedStateSensorModel(key, newValue); + UsgsAstroFrameSensorModel* sensorModel = + createModifiedStateSensorModel(key, newValue); ASSERT_NE(sensorModel, nullptr); csm::ImageCoord imagePt(7.5, 7.5); @@ -222,12 +225,14 @@ TEST_F(FrameStateTest, SemiMajorAxis100x_Center) { TEST_F(FrameStateTest, SemiMajorAxis10x_SlightlyOffCenter) { std::string key = "m_majorAxis"; double newValue = 100.0; - UsgsAstroFrameSensorModel* sensorModel = createModifiedStateSensorModel(key, newValue); + UsgsAstroFrameSensorModel* sensorModel = + createModifiedStateSensorModel(key, newValue); ASSERT_NE(sensorModel, nullptr); csm::ImageCoord imagePt(7.5, 6.5); csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); - // Note: In the following, the tolerance was increased due to the combination of an offset image point and + // Note: In the following, the tolerance was increased due to the combination + // of an offset image point and // a very large deviation from sphericity. EXPECT_NEAR(groundPt.x, 9.83606557e+01, 1e-7); EXPECT_NEAR(groundPt.y, 0.0, 1e-7); @@ -237,12 +242,13 @@ TEST_F(FrameStateTest, SemiMajorAxis10x_SlightlyOffCenter) { sensorModel = NULL; } -// The following test is for the scenario where the semi_minor_axis is actually larger -// than the semi_major_axis: +// The following test is for the scenario where the semi_minor_axis is actually +// larger than the semi_major_axis: TEST_F(FrameStateTest, SemiMinorAxis10x_SlightlyOffCenter) { std::string key = "m_minorAxis"; double newValue = 100.0; - UsgsAstroFrameSensorModel* sensorModel = createModifiedStateSensorModel(key, newValue); + UsgsAstroFrameSensorModel* sensorModel = + createModifiedStateSensorModel(key, newValue); ASSERT_NE(sensorModel, nullptr); csm::ImageCoord imagePt(7.5, 6.5); @@ -258,14 +264,15 @@ TEST_F(FrameStateTest, SemiMinorAxis10x_SlightlyOffCenter) { TEST_F(FrameStateTest, SampleSumming) { std::string key = "m_detectorSampleSumming"; double newValue = 2.0; - UsgsAstroFrameSensorModel* sensorModel = createModifiedStateSensorModel(key, newValue); + UsgsAstroFrameSensorModel* sensorModel = + createModifiedStateSensorModel(key, newValue); ASSERT_NE(sensorModel, nullptr); - csm::ImageCoord imagePt(7.5, 3.75); - csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); - EXPECT_NEAR(groundPt.x, 10.0, 1e-8); - EXPECT_NEAR(groundPt.y, 0, 1e-8); - EXPECT_NEAR(groundPt.z, 0, 1e-8); + csm::ImageCoord imagePt(7.5, 3.75); + csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); + EXPECT_NEAR(groundPt.x, 10.0, 1e-8); + EXPECT_NEAR(groundPt.y, 0, 1e-8); + EXPECT_NEAR(groundPt.z, 0, 1e-8); delete sensorModel; sensorModel = NULL; @@ -274,14 +281,15 @@ TEST_F(FrameStateTest, SampleSumming) { TEST_F(FrameStateTest, LineSumming) { std::string key = "m_detectorLineSumming"; double newValue = 2.0; - UsgsAstroFrameSensorModel* sensorModel = createModifiedStateSensorModel(key, newValue); + UsgsAstroFrameSensorModel* sensorModel = + createModifiedStateSensorModel(key, newValue); ASSERT_NE(sensorModel, nullptr); - csm::ImageCoord imagePt(3.75, 7.5); - csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); - EXPECT_NEAR(groundPt.x, 10.0, 1e-8); - EXPECT_NEAR(groundPt.y, 0, 1e-8); - EXPECT_NEAR(groundPt.z, 0, 1e-8); + csm::ImageCoord imagePt(3.75, 7.5); + csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); + EXPECT_NEAR(groundPt.x, 10.0, 1e-8); + EXPECT_NEAR(groundPt.y, 0, 1e-8); + EXPECT_NEAR(groundPt.z, 0, 1e-8); delete sensorModel; sensorModel = NULL; @@ -290,14 +298,15 @@ TEST_F(FrameStateTest, LineSumming) { TEST_F(FrameStateTest, StartSample) { std::string key = "m_startingDetectorSample"; double newValue = 5.0; - UsgsAstroFrameSensorModel* sensorModel = createModifiedStateSensorModel(key, newValue); + UsgsAstroFrameSensorModel* sensorModel = + createModifiedStateSensorModel(key, newValue); ASSERT_NE(sensorModel, nullptr); - csm::ImageCoord imagePt(7.5, 2.5); - csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); - EXPECT_NEAR(groundPt.x, 10.0, 1e-8); - EXPECT_NEAR(groundPt.y, 0, 1e-8); - EXPECT_NEAR(groundPt.z, 0, 1e-8); + csm::ImageCoord imagePt(7.5, 2.5); + csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); + EXPECT_NEAR(groundPt.x, 10.0, 1e-8); + EXPECT_NEAR(groundPt.y, 0, 1e-8); + EXPECT_NEAR(groundPt.z, 0, 1e-8); delete sensorModel; sensorModel = NULL; @@ -306,14 +315,15 @@ TEST_F(FrameStateTest, StartSample) { TEST_F(FrameStateTest, StartLine) { std::string key = "m_startingDetectorLine"; double newValue = 5.0; - UsgsAstroFrameSensorModel* sensorModel = createModifiedStateSensorModel(key, newValue); + UsgsAstroFrameSensorModel* sensorModel = + createModifiedStateSensorModel(key, newValue); ASSERT_NE(sensorModel, nullptr); - csm::ImageCoord imagePt(2.5, 7.5); - csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); - EXPECT_NEAR(groundPt.x, 10.0, 1e-8); - EXPECT_NEAR(groundPt.y, 0, 1e-8); - EXPECT_NEAR(groundPt.z, 0, 1e-8); + csm::ImageCoord imagePt(2.5, 7.5); + csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); + EXPECT_NEAR(groundPt.x, 10.0, 1e-8); + EXPECT_NEAR(groundPt.y, 0, 1e-8); + EXPECT_NEAR(groundPt.z, 0, 1e-8); delete sensorModel; sensorModel = NULL; @@ -325,57 +335,58 @@ TEST_F(FrameStateTest, StartLine) { // Observer x position: TEST_F(FrameSensorModel, X10_SlightlyOffCenter) { - double newValue = 10.0; - sensorModel->setParameterValue(0, newValue); // X + double newValue = 10.0; + sensorModel->setParameterValue(0, newValue); // X - ASSERT_NE(sensorModel, nullptr); - csm::ImageCoord imagePt(7.5, 6.5); - csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); - EXPECT_NEAR(groundPt.x, 10.0, 1e-8); - EXPECT_NEAR(groundPt.y, 0.0, 1e-8); - EXPECT_NEAR(groundPt.z, 0.0, 1e-8); + ASSERT_NE(sensorModel, nullptr); + csm::ImageCoord imagePt(7.5, 6.5); + csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); + EXPECT_NEAR(groundPt.x, 10.0, 1e-8); + EXPECT_NEAR(groundPt.y, 0.0, 1e-8); + EXPECT_NEAR(groundPt.z, 0.0, 1e-8); - delete sensorModel; - sensorModel = NULL; + delete sensorModel; + sensorModel = NULL; } TEST_F(FrameSensorModel, X1e9_SlightlyOffCenter) { - double newValue = 1000000000.0; - sensorModel->setParameterValue(0, newValue); // X + double newValue = 1000000000.0; + sensorModel->setParameterValue(0, newValue); // X - ASSERT_NE(sensorModel, nullptr); - csm::ImageCoord imagePt(7.5, 6.5); - csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); - // Note: In the following, the tolerance was increased due to the very large distance being tested (~6.68 AU). - EXPECT_NEAR(groundPt.x, 3.99998400e+03, 1e-4); - EXPECT_NEAR(groundPt.y, 0.0, 1e-4); - EXPECT_NEAR(groundPt.z, 1.99999200e+06, 1e-4); + ASSERT_NE(sensorModel, nullptr); + csm::ImageCoord imagePt(7.5, 6.5); + csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); + // Note: In the following, the tolerance was increased due to the very large + // distance being tested (~6.68 AU). + EXPECT_NEAR(groundPt.x, 3.99998400e+03, 1e-4); + EXPECT_NEAR(groundPt.y, 0.0, 1e-4); + EXPECT_NEAR(groundPt.z, 1.99999200e+06, 1e-4); - delete sensorModel; - sensorModel = NULL; + delete sensorModel; + sensorModel = NULL; } // Angle rotations: TEST_F(FrameSensorModel, Rotation_omegaPi_Center) { - sensorModel->setParameterValue(3, 1.0); - sensorModel->setParameterValue(4, 1.0); - sensorModel->setParameterValue(5, 1.0); + sensorModel->setParameterValue(3, 1.0); + sensorModel->setParameterValue(4, 1.0); + sensorModel->setParameterValue(5, 1.0); - sensorModel->setParameterValue(6, 1.0); + sensorModel->setParameterValue(6, 1.0); - sensorModel->setParameterValue(0, 1000.0); // X - sensorModel->setParameterValue(1, 0.0); // Y - sensorModel->setParameterValue(2, 0.0); // Z + sensorModel->setParameterValue(0, 1000.0); // X + sensorModel->setParameterValue(1, 0.0); // Y + sensorModel->setParameterValue(2, 0.0); // Z - ASSERT_NE(sensorModel, nullptr); - csm::ImageCoord imagePt(7.5, 7.5); - csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); - EXPECT_NEAR(groundPt.x, -10.0, 1e-8); - EXPECT_NEAR(groundPt.y, 0.0, 1e-8); - EXPECT_NEAR(groundPt.z, 0.0, 1e-8); + ASSERT_NE(sensorModel, nullptr); + csm::ImageCoord imagePt(7.5, 7.5); + csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); + EXPECT_NEAR(groundPt.x, -10.0, 1e-8); + EXPECT_NEAR(groundPt.y, 0.0, 1e-8); + EXPECT_NEAR(groundPt.z, 0.0, 1e-8); - delete sensorModel; - sensorModel = NULL; + delete sensorModel; + sensorModel = NULL; } TEST_F(FrameSensorModel, Rotation_NPole_Center) { @@ -385,9 +396,9 @@ TEST_F(FrameSensorModel, Rotation_NPole_Center) { sensorModel->setParameterValue(6, 0.0); - sensorModel->setParameterValue(0, 0.0); // X - sensorModel->setParameterValue(1, 0.0); // Y - sensorModel->setParameterValue(2, 1000.0); // Z + sensorModel->setParameterValue(0, 0.0); // X + sensorModel->setParameterValue(1, 0.0); // Y + sensorModel->setParameterValue(2, 1000.0); // Z ASSERT_NE(sensorModel, nullptr); csm::ImageCoord imagePt(7.5, 7.5); @@ -401,20 +412,20 @@ TEST_F(FrameSensorModel, Rotation_NPole_Center) { } TEST_F(FrameSensorModel, Rotation_SPole_Center) { - sensorModel->setParameterValue(3, 0.0); // phi - sensorModel->setParameterValue(0, 0.0); // X - sensorModel->setParameterValue(1, 0.0); // Y - sensorModel->setParameterValue(2, -1000.0); // Z + sensorModel->setParameterValue(3, 0.0); // phi + sensorModel->setParameterValue(0, 0.0); // X + sensorModel->setParameterValue(1, 0.0); // Y + sensorModel->setParameterValue(2, -1000.0); // Z - ASSERT_NE(sensorModel, nullptr); - csm::ImageCoord imagePt(7.5, 7.5); - csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); - EXPECT_NEAR(groundPt.x, 0.0, 1e-8); - EXPECT_NEAR(groundPt.y, 0.0, 1e-8); - EXPECT_NEAR(groundPt.z, -10.0, 1e-8); + ASSERT_NE(sensorModel, nullptr); + csm::ImageCoord imagePt(7.5, 7.5); + csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); + EXPECT_NEAR(groundPt.x, 0.0, 1e-8); + EXPECT_NEAR(groundPt.y, 0.0, 1e-8); + EXPECT_NEAR(groundPt.z, -10.0, 1e-8); - delete sensorModel; - sensorModel = NULL; + delete sensorModel; + sensorModel = NULL; } // **************************************************************************** @@ -433,10 +444,7 @@ TEST_F(FrameSensorModelLogging, GroundToImageAdjustments) { } TEST_F(FrameSensorModelLogging, GroundToImageCovariance) { - csm::EcefCoordCovar groundPt(10.0, 0.0, 0.0, - 1.0, 0.0, 0.0, - 1.0, 0.0, - 1.0); + csm::EcefCoordCovar groundPt(10.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.0, 0.0, 1.0); sensorModel->groundToImage(groundPt); } @@ -446,9 +454,7 @@ TEST_F(FrameSensorModelLogging, ImageToGround) { } TEST_F(FrameSensorModelLogging, ImageToGroundCovariance) { - csm::ImageCoordCovar imagePt(7.5, 7.5, - 1.0, 0.0, - 1.0); + csm::ImageCoordCovar imagePt(7.5, 7.5, 1.0, 0.0, 1.0); sensorModel->imageToGround(imagePt, 0.0); } @@ -463,13 +469,9 @@ TEST_F(FrameSensorModelLogging, ImageToRemoteImagingLocus) { sensorModel->imageToRemoteImagingLocus(imagePt); } -TEST_F(FrameSensorModelLogging, GetImageStart) { - sensorModel->getImageStart(); -} +TEST_F(FrameSensorModelLogging, GetImageStart) { sensorModel->getImageStart(); } -TEST_F(FrameSensorModelLogging, GetImageSize) { - sensorModel->getImageSize(); -} +TEST_F(FrameSensorModelLogging, GetImageSize) { sensorModel->getImageSize(); } TEST_F(FrameSensorModelLogging, GetValidImageRange) { sensorModel->getValidImageRange(); @@ -544,17 +546,11 @@ TEST_F(FrameSensorModelLogging, GetUnmodeledCrossCovariance) { sensorModel->getUnmodeledCrossCovariance(imagePt1, imagePt2); } -TEST_F(FrameSensorModelLogging, GetVersion) { - sensorModel->getVersion(); -} +TEST_F(FrameSensorModelLogging, GetVersion) { sensorModel->getVersion(); } -TEST_F(FrameSensorModelLogging, GetModelName) { - sensorModel->getModelName(); -} +TEST_F(FrameSensorModelLogging, GetModelName) { sensorModel->getModelName(); } -TEST_F(FrameSensorModelLogging, GetPedigree) { - sensorModel->getPedigree(); -} +TEST_F(FrameSensorModelLogging, GetPedigree) { sensorModel->getPedigree(); } TEST_F(FrameSensorModelLogging, GetImageIdentifier) { sensorModel->getImageIdentifier(); @@ -580,21 +576,15 @@ TEST_F(FrameSensorModelLogging, GetTrajectoryIdentifier) { sensorModel->getTrajectoryIdentifier(); } -TEST_F(FrameSensorModelLogging, GetSensorType) { - sensorModel->getSensorType(); -} +TEST_F(FrameSensorModelLogging, GetSensorType) { sensorModel->getSensorType(); } -TEST_F(FrameSensorModelLogging, GetSensorMode) { - sensorModel->getSensorMode(); -} +TEST_F(FrameSensorModelLogging, GetSensorMode) { sensorModel->getSensorMode(); } TEST_F(FrameSensorModelLogging, GetReferenceDateAndTime) { sensorModel->getReferenceDateAndTime(); } -TEST_F(FrameSensorModelLogging, GetModelState) { - sensorModel->getModelState(); -} +TEST_F(FrameSensorModelLogging, GetModelState) { sensorModel->getModelState(); } TEST_F(FrameSensorModelLogging, replaceModelState) { std::string state = sensorModel->getModelState(); @@ -636,8 +626,7 @@ TEST_F(FrameSensorModelLogging, IsParameterShareable) { TEST_F(FrameSensorModelLogging, GetParameterSharingCriteria) { try { sensorModel->getParameterSharingCriteria(0); - } - catch (...) { + } catch (...) { // Just testing logging, so do nothing, it should still log } } @@ -673,8 +662,7 @@ TEST_F(FrameSensorModelLogging, GetNumGeometricCorrectionSwitches) { TEST_F(FrameSensorModelLogging, GetGeometricCorrectionName) { try { sensorModel->getGeometricCorrectionName(0); - } - catch (...) { + } catch (...) { // Just testing logging, so do nothing, it should still log } } @@ -682,8 +670,7 @@ TEST_F(FrameSensorModelLogging, GetGeometricCorrectionName) { TEST_F(FrameSensorModelLogging, SetGeometricCorrectionSwitch) { try { sensorModel->setGeometricCorrectionSwitch(0, true, csm::param::REAL); - } - catch (...) { + } catch (...) { // Just testing logging, so do nothing, it should still log } } @@ -691,8 +678,7 @@ TEST_F(FrameSensorModelLogging, SetGeometricCorrectionSwitch) { TEST_F(FrameSensorModelLogging, GetGeometricCorrectionSwitch) { try { sensorModel->getGeometricCorrectionSwitch(0); - } - catch (...) { + } catch (...) { // Just testing logging, so do nothing, it should still log } } @@ -717,9 +703,9 @@ TEST_F(FrameSensorModelLogging, IsValidIsd) { TEST_F(FrameSensorModelLogging, ConstructStateFromIsd) { try { - sensorModel->constructStateFromIsd("{\"test_key\" : \"test_string\"}", nullptr); - } - catch (...) { + sensorModel->constructStateFromIsd("{\"test_key\" : \"test_string\"}", + nullptr); + } catch (...) { // Just testing logging, so do nothing, it should still log } } @@ -749,11 +735,7 @@ TEST_F(FrameSensorModelLogging, losEllipsoidIntersect) { xl = -1.0; yl = 0.0; zl = 0.0; - sensorModel->losEllipsoidIntersect( - height, - xc, yc, zc, - xl, yl, zl, - x, y, z); + sensorModel->losEllipsoidIntersect(height, xc, yc, zc, xl, yl, zl, x, y, z); } TEST_F(OrbitalFrameSensorModel, ReferenceDateTime) { diff --git a/tests/ISDParsingTests.cpp b/tests/ISDParsingTests.cpp index 773451db9..f2ab92e36 100644 --- a/tests/ISDParsingTests.cpp +++ b/tests/ISDParsingTests.cpp @@ -7,303 +7,199 @@ using json = nlohmann::json; TEST(MetricConversion, DistanceConversion) { - EXPECT_EQ(1, metric_conversion(1000, "m", "km")); - EXPECT_EQ(1000, metric_conversion(1000, "m", "m")); - EXPECT_EQ(1000, metric_conversion(1, "km", "m")); - EXPECT_EQ(1, metric_conversion(1, "km", "km")); - EXPECT_EQ(0, metric_conversion(0, "m", "km")); - EXPECT_EQ(0, metric_conversion(0, "m", "m")); - EXPECT_EQ(0, metric_conversion(0, "km", "m")); - EXPECT_EQ(0, metric_conversion(0, "km", "km")); - EXPECT_EQ(-1, metric_conversion(-1000, "m", "km")); + EXPECT_EQ(1, metric_conversion(1000, "m", "km")); + EXPECT_EQ(1000, metric_conversion(1000, "m", "m")); + EXPECT_EQ(1000, metric_conversion(1, "km", "m")); + EXPECT_EQ(1, metric_conversion(1, "km", "km")); + EXPECT_EQ(0, metric_conversion(0, "m", "km")); + EXPECT_EQ(0, metric_conversion(0, "m", "m")); + EXPECT_EQ(0, metric_conversion(0, "km", "m")); + EXPECT_EQ(0, metric_conversion(0, "km", "km")); + EXPECT_EQ(-1, metric_conversion(-1000, "m", "km")); EXPECT_EQ(-1000, metric_conversion(-1000, "m", "m")); EXPECT_EQ(-1000, metric_conversion(-1, "km", "m")); - EXPECT_EQ(-1, metric_conversion(-1, "km", "km")); + EXPECT_EQ(-1, metric_conversion(-1, "km", "km")); } TEST(ISDParsing, ModelName) { - json isd = { - {"name_model", "Test"} - }; + json isd = {{"name_model", "Test"}}; EXPECT_EQ("Test", getSensorModelName(isd)); } TEST(ISDParsing, ImageIdentifier) { - json isd = { - {"image_identifier", "Test"} - }; + json isd = {{"image_identifier", "Test"}}; EXPECT_EQ("Test", getImageId(isd)); } TEST(ISDParsing, SensorName) { - json isd = { - {"name_sensor", "Test"} - }; + json isd = {{"name_sensor", "Test"}}; EXPECT_EQ("Test", getSensorName(isd)); } TEST(ISDParsing, PlatformName) { - json isd = { - {"name_platform", "Test"} - }; + json isd = {{"name_platform", "Test"}}; EXPECT_EQ("Test", getPlatformName(isd)); } TEST(ISDParsing, TotalLines) { - json isd = { - {"image_lines", 16} - }; + json isd = {{"image_lines", 16}}; EXPECT_EQ(16, getTotalLines(isd)); } TEST(ISDParsing, TotalSamples) { - json isd = { - {"image_samples", 16} - }; + json isd = {{"image_samples", 16}}; EXPECT_EQ(16, getTotalSamples(isd)); } TEST(ISDParsing, StartTime) { - json isd = { - {"starting_ephemeris_time", -10} - }; + json isd = {{"starting_ephemeris_time", -10}}; EXPECT_EQ(-10, getStartingTime(isd)); } TEST(ISDParsing, CenterTime) { - json isd = { - {"center_ephemeris_time", -10} - }; + json isd = {{"center_ephemeris_time", -10}}; EXPECT_EQ(-10, getCenterTime(isd)); } TEST(ISDParsing, IntegrationStartLines) { std::vector> lineScanRate = { - {0, 1, 2}, - {3, 4, 5}, - {6, 7, 8} - }; + {0, 1, 2}, {3, 4, 5}, {6, 7, 8}}; std::vector startLines = {0, 3, 6}; EXPECT_EQ(startLines, getIntegrationStartLines(lineScanRate)); } TEST(ISDParsing, IntegrationStartTimes) { std::vector> lineScanRate = { - {0, 1, 2}, - {3, 4, 5}, - {6, 7, 8} - }; + {0, 1, 2}, {3, 4, 5}, {6, 7, 8}}; std::vector startTimes = {1, 4, 7}; EXPECT_EQ(startTimes, getIntegrationStartTimes(lineScanRate)); } TEST(ISDParsing, IntegrationTimes) { std::vector> lineScanRate = { - {0, 1, 2}, - {3, 4, 5}, - {6, 7, 8} - }; + {0, 1, 2}, {3, 4, 5}, {6, 7, 8}}; std::vector times = {2, 5, 8}; EXPECT_EQ(times, getIntegrationTimes(lineScanRate)); } TEST(ISDParsing, SampleSumming) { - json isd = { - {"detector_sample_summing", 4} - }; + json isd = {{"detector_sample_summing", 4}}; EXPECT_EQ(4, getSampleSumming(isd)); } TEST(ISDParsing, LineSumming) { - json isd = { - {"detector_line_summing", 4} - }; + json isd = {{"detector_line_summing", 4}}; EXPECT_EQ(4, getLineSumming(isd)); } TEST(ISDParsing, FocalLength) { - json isd = { - {"focal_length_model", { - {"focal_length", 2}} - } - }; + json isd = {{"focal_length_model", {{"focal_length", 2}}}}; EXPECT_EQ(2, getFocalLength(isd)); } TEST(ISDParsing, FocalLengthEpsilon) { - json isd = { - {"focal_length_model", { - {"focal_epsilon", 0.1}} - } - }; + json isd = {{"focal_length_model", {{"focal_epsilon", 0.1}}}}; EXPECT_EQ(0.1, getFocalLengthEpsilon(isd)); } TEST(ISDParsing, Focal2PixelLines) { - json isd = { - {"focal2pixel_lines", {0, 1, 2}} - }; + json isd = {{"focal2pixel_lines", {0, 1, 2}}}; std::vector coefficients = {0, 1, 2}; EXPECT_EQ(coefficients, getFocal2PixelLines(isd)); } TEST(ISDParsing, Focal2PixelSamples) { - json isd = { - {"focal2pixel_samples", {0, 1, 2}} - }; + json isd = {{"focal2pixel_samples", {0, 1, 2}}}; std::vector coefficients = {0, 1, 2}; EXPECT_EQ(coefficients, getFocal2PixelSamples(isd)); } TEST(ISDParsing, DetectorCenterLine) { - json isd = { - {"detector_center", { - {"line", 2}} - } - }; + json isd = {{"detector_center", {{"line", 2}}}}; EXPECT_EQ(2, getDetectorCenterLine(isd)); } TEST(ISDParsing, DetectorCenterSample) { - json isd = { - {"detector_center", { - {"sample", 3}} - } - }; + json isd = {{"detector_center", {{"sample", 3}}}}; EXPECT_EQ(3, getDetectorCenterSample(isd)); } TEST(ISDParsing, DetectorStartingLine) { - json isd = { - {"starting_detector_line", 1} - }; + json isd = {{"starting_detector_line", 1}}; EXPECT_EQ(1, getDetectorStartingLine(isd)); } TEST(ISDParsing, DetectorStartingSample) { - json isd = { - {"starting_detector_sample", 2} - }; + json isd = {{"starting_detector_sample", 2}}; EXPECT_EQ(2, getDetectorStartingSample(isd)); } TEST(ISDParsing, MinHeight) { - json isd = { - {"reference_height", { - {"minheight", -1}, - {"unit", "km"}} - } - }; + json isd = {{"reference_height", {{"minheight", -1}, {"unit", "km"}}}}; EXPECT_EQ(-1000, getMinHeight(isd)); } TEST(ISDParsing, MaxHeight) { - json isd = { - {"reference_height", { - {"maxheight", 10}, - {"unit", "km"}} - } - }; + json isd = {{"reference_height", {{"maxheight", 10}, {"unit", "km"}}}}; EXPECT_EQ(10000, getMaxHeight(isd)); } TEST(ISDParsing, SemiMajor) { - json isd = { - {"radii", { - {"semimajor", 2}, - {"unit", "km"}} - } - }; + json isd = {{"radii", {{"semimajor", 2}, {"unit", "km"}}}}; EXPECT_EQ(2000, getSemiMajorRadius(isd)); } TEST(ISDParsing, SemiMinor) { - json isd = { - {"radii", { - {"semiminor", 1}, - {"unit", "km"}} - } - }; + json isd = {{"radii", {{"semiminor", 1}, {"unit", "km"}}}}; EXPECT_EQ(1000, getSemiMinorRadius(isd)); } TEST(ISDParsing, TransverseDistortion) { - json isd = { - {"optical_distortion", { - {"transverse", { - {"y", {-11, 21, 24}}, - {"x", {-1, 2, 4}}}} - } - } - }; - std::vector coefficients = {-1, 2, 4, 0, 0, 0, 0, 0, 0, 0, - -11, 21, 24, 0, 0, 0, 0, 0, 0, 0}; + json isd = {{"optical_distortion", + {{"transverse", {{"y", {-11, 21, 24}}, {"x", {-1, 2, 4}}}}}}}; + std::vector coefficients = {-1, 2, 4, 0, 0, 0, 0, 0, 0, 0, + -11, 21, 24, 0, 0, 0, 0, 0, 0, 0}; EXPECT_EQ(coefficients, getDistortionCoeffs(isd)); } TEST(ISDParsing, Radial) { json isd = { - {"optical_distortion", { - {"radial", { - {"coefficients", {0, 1, 2}}}} - } - } - }; + {"optical_distortion", {{"radial", {{"coefficients", {0, 1, 2}}}}}}}; std::vector coefficients = {0, 1, 2}; EXPECT_EQ(coefficients, getDistortionCoeffs(isd)); } TEST(ISDParsing, SunPosition) { json isd = { - {"sun_position", { - {"positions", { - {0, 1, 2}, - {3, 4, 5}, - {6, 7, 8}}}, - {"unit", "km"}} - } - }; - std::vector positions = {0, 1000, 2000, 3000, 4000, 5000, 6000, 7000, 8000}; + {"sun_position", + {{"positions", {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}}}, {"unit", "km"}}}}; + std::vector positions = {0, 1000, 2000, 3000, 4000, + 5000, 6000, 7000, 8000}; EXPECT_EQ(positions, getSunPositions(isd)); } TEST(ISDParsing, SensorPosition) { json isd = { - {"sensor_position", { - {"positions", { - {0, 1, 2}, - {3, 4, 5}, - {6, 7, 8}}}, - {"unit", "km"}} - } - }; - std::vector positions = {0, 1000, 2000, 3000, 4000, 5000, 6000, 7000, 8000}; + {"sensor_position", + {{"positions", {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}}}, {"unit", "km"}}}}; + std::vector positions = {0, 1000, 2000, 3000, 4000, + 5000, 6000, 7000, 8000}; EXPECT_EQ(positions, getSensorPositions(isd)); } TEST(ISDParsing, SensorVelocities) { json isd = { - {"sensor_position", { - {"velocities", { - {0, 1, 2}, - {3, 4, 5}, - {6, 7, 8}}}, - {"unit", "km"}} - } - }; - std::vector velocity = {0, 1000, 2000, 3000, 4000, 5000, 6000, 7000, 8000}; + {"sensor_position", + {{"velocities", {{0, 1, 2}, {3, 4, 5}, {6, 7, 8}}}, {"unit", "km"}}}}; + std::vector velocity = {0, 1000, 2000, 3000, 4000, + 5000, 6000, 7000, 8000}; EXPECT_EQ(velocity, getSensorVelocities(isd)); } TEST(ISDParsing, SensorOrientations) { json isd = { - {"sensor_orientation", { - {"quaternions", { - {0, 1, 2, 3}, - {4, 5, 6, 7}, - {8, 9, 10, 11}}}} - } - }; + {"sensor_orientation", + {{"quaternions", {{0, 1, 2, 3}, {4, 5, 6, 7}, {8, 9, 10, 11}}}}}}; std::vector rotations = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11}; EXPECT_EQ(rotations, getSensorOrientations(isd)); } @@ -319,10 +215,9 @@ TEST(ISDParsing, getScaledPixelWidth) { } TEST(ISDParsing, getScaleConversionCoefficients) { - json isd = {{"range_conversion_coefficients", - {{300, 1, 0.1, 0.01}, - {400, 2, 0.2, 0.02}, - {500, 3, 0.3, 0.03}}}}; + json isd = { + {"range_conversion_coefficients", + {{300, 1, 0.1, 0.01}, {400, 2, 0.2, 0.02}, {500, 3, 0.3, 0.03}}}}; std::vector coefficients = getScaleConversionCoefficients(isd); ASSERT_EQ(coefficients.size(), 12); EXPECT_EQ(coefficients[0], 300); @@ -340,8 +235,7 @@ TEST(ISDParsing, getScaleConversionCoefficients) { } TEST(ISDParsing, getScaleConversionTimes) { - json isd = {{"range_conversion_times", - {100, 200, 300, -400}}}; + json isd = {{"range_conversion_times", {100, 200, 300, -400}}}; std::vector times = getScaleConversionTimes(isd); ASSERT_EQ(times.size(), 4); EXPECT_EQ(times[0], 100); diff --git a/tests/LineScanCameraTests.cpp b/tests/LineScanCameraTests.cpp index 4053aceb1..a4e43529f 100644 --- a/tests/LineScanCameraTests.cpp +++ b/tests/LineScanCameraTests.cpp @@ -1,43 +1,43 @@ #define _USE_MATH_DEFINES #include "Fixtures.h" -#include "UsgsAstroPlugin.h" #include "UsgsAstroLsSensorModel.h" +#include "UsgsAstroPlugin.h" -#include #include +#include #include #include using json = nlohmann::json; - TEST_F(ConstVelocityLineScanSensorModel, State) { - std::string modelState = sensorModel->getModelState(); - sensorModel->replaceModelState(modelState); + std::string modelState = sensorModel->getModelState(); + sensorModel->replaceModelState(modelState); - // When this is different, the output is very hard to parse - // TODO implement JSON diff for gtest + // When this is different, the output is very hard to parse + // TODO implement JSON diff for gtest - EXPECT_EQ(sensorModel->getModelState(), modelState); + EXPECT_EQ(sensorModel->getModelState(), modelState); } // Fly by tests TEST_F(ConstVelocityLineScanSensorModel, Center) { - csm::ImageCoord imagePt(8.0, 8.0); - csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); - EXPECT_NEAR(groundPt.x, 9.99999500000, 1e-10); - EXPECT_NEAR(groundPt.y, 0.0, 1e-10); - EXPECT_NEAR(groundPt.z, 0.00999999500, 1e-10); + csm::ImageCoord imagePt(8.0, 8.0); + csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); + EXPECT_NEAR(groundPt.x, 9.99999500000, 1e-10); + EXPECT_NEAR(groundPt.y, 0.0, 1e-10); + EXPECT_NEAR(groundPt.z, 0.00999999500, 1e-10); } TEST_F(ConstVelocityLineScanSensorModel, Inversion) { double achievedPrecision; csm::ImageCoord imagePt(8.5, 8); csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); - csm::ImageCoord imageReprojPt = sensorModel->groundToImage(groundPt, 0.001, &achievedPrecision); + csm::ImageCoord imageReprojPt = + sensorModel->groundToImage(groundPt, 0.001, &achievedPrecision); EXPECT_LT(achievedPrecision, 0.001); EXPECT_NEAR(imagePt.line, imageReprojPt.line, 1e-3); @@ -45,59 +45,62 @@ TEST_F(ConstVelocityLineScanSensorModel, Inversion) { } TEST_F(ConstVelocityLineScanSensorModel, OffBody) { - csm::ImageCoord imagePt(0.0, 4.0); - csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); - EXPECT_NEAR(groundPt.x, 0.04799688020, 1e-10); - EXPECT_NEAR(groundPt.y, -7.99961602495, 1e-10); - EXPECT_NEAR(groundPt.z, 16.00004799688, 1e-10); + csm::ImageCoord imagePt(0.0, 4.0); + csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); + EXPECT_NEAR(groundPt.x, 0.04799688020, 1e-10); + EXPECT_NEAR(groundPt.y, -7.99961602495, 1e-10); + EXPECT_NEAR(groundPt.z, 16.00004799688, 1e-10); } TEST_F(ConstVelocityLineScanSensorModel, ProximateImageLocus) { - csm::ImageCoord imagePt(8.0, 8.0); - csm::EcefCoord groundPt(10, 2, 1); - csm::EcefLocus remoteLocus = sensorModel->imageToRemoteImagingLocus(imagePt); - csm::EcefLocus locus = sensorModel->imageToProximateImagingLocus(imagePt, groundPt); - double locusToGroundX = locus.point.x - groundPt.x; - double locusToGroundY = locus.point.y - groundPt.y; - double locusToGroundZ = locus.point.z - groundPt.z; - EXPECT_NEAR(locus.direction.x, remoteLocus.direction.x, 1e-10); - EXPECT_NEAR(locus.direction.y, remoteLocus.direction.y, 1e-10); - EXPECT_NEAR(locus.direction.z, remoteLocus.direction.z, 1e-10); - EXPECT_NEAR(locusToGroundX * locus.direction.x + - locusToGroundY * locus.direction.y + - locusToGroundZ * locus.direction.z, 0.0, 1e-10); + csm::ImageCoord imagePt(8.0, 8.0); + csm::EcefCoord groundPt(10, 2, 1); + csm::EcefLocus remoteLocus = sensorModel->imageToRemoteImagingLocus(imagePt); + csm::EcefLocus locus = + sensorModel->imageToProximateImagingLocus(imagePt, groundPt); + double locusToGroundX = locus.point.x - groundPt.x; + double locusToGroundY = locus.point.y - groundPt.y; + double locusToGroundZ = locus.point.z - groundPt.z; + EXPECT_NEAR(locus.direction.x, remoteLocus.direction.x, 1e-10); + EXPECT_NEAR(locus.direction.y, remoteLocus.direction.y, 1e-10); + EXPECT_NEAR(locus.direction.z, remoteLocus.direction.z, 1e-10); + EXPECT_NEAR(locusToGroundX * locus.direction.x + + locusToGroundY * locus.direction.y + + locusToGroundZ * locus.direction.z, + 0.0, 1e-10); } TEST_F(ConstVelocityLineScanSensorModel, RemoteImageLocus) { - csm::ImageCoord imagePt(8.5, 8.0); - csm::EcefLocus locus = sensorModel->imageToRemoteImagingLocus(imagePt); - csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); - double lookX = groundPt.x - locus.point.x; - double lookY = groundPt.y - locus.point.y; - double lookZ = groundPt.z - locus.point.z; - double lookMag = sqrt(lookX * lookX + lookY * lookY + lookZ * lookZ); - lookX /= lookMag; - lookY /= lookMag; - lookZ /= lookMag; - EXPECT_NEAR(locus.direction.x, lookX, 1e-10); - EXPECT_NEAR(locus.direction.y, lookY, 1e-10); - EXPECT_NEAR(locus.direction.z, lookZ, 1e-10); - EXPECT_NEAR(locus.point.x, 1000.0, 1e-10); - EXPECT_NEAR(locus.point.y, 0.0, 1e-10); - EXPECT_NEAR(locus.point.z, 0.0, 1e-10); + csm::ImageCoord imagePt(8.5, 8.0); + csm::EcefLocus locus = sensorModel->imageToRemoteImagingLocus(imagePt); + csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); + double lookX = groundPt.x - locus.point.x; + double lookY = groundPt.y - locus.point.y; + double lookZ = groundPt.z - locus.point.z; + double lookMag = sqrt(lookX * lookX + lookY * lookY + lookZ * lookZ); + lookX /= lookMag; + lookY /= lookMag; + lookZ /= lookMag; + EXPECT_NEAR(locus.direction.x, lookX, 1e-10); + EXPECT_NEAR(locus.direction.y, lookY, 1e-10); + EXPECT_NEAR(locus.direction.z, lookZ, 1e-10); + EXPECT_NEAR(locus.point.x, 1000.0, 1e-10); + EXPECT_NEAR(locus.point.y, 0.0, 1e-10); + EXPECT_NEAR(locus.point.z, 0.0, 1e-10); } TEST_F(ConstVelocityLineScanSensorModel, calculateAttitudeCorrection) { std::vector adj; double attCorr[9]; adj.resize(15, 0); - // Pi/2 with simply compensating for member variable m_flyingHeight in UsgsAstroLsSensorModel + // Pi/2 with simply compensating for member variable m_flyingHeight in + // UsgsAstroLsSensorModel adj[7] = (M_PI / 2) * sensorModel->m_flyingHeight; sensorModel->calculateAttitudeCorrection(999.5, adj, attCorr); - // EXPECT_NEARs are used here instead of EXPECT_DOUBLE_EQs because index 0 and 8 of the matrix - // are evaluating to 6.12...e-17. This is too small to be worried about here, but - // EXPECT_DOUBLE_EQ is too sensitive. + // EXPECT_NEARs are used here instead of EXPECT_DOUBLE_EQs because index 0 and + // 8 of the matrix are evaluating to 6.12...e-17. This is too small to be + // worried about here, but EXPECT_DOUBLE_EQ is too sensitive. EXPECT_NEAR(attCorr[0], 0, 1e-8); EXPECT_NEAR(attCorr[1], 0, 1e-8); EXPECT_NEAR(attCorr[2], 1, 1e-8); @@ -109,17 +112,16 @@ TEST_F(ConstVelocityLineScanSensorModel, calculateAttitudeCorrection) { EXPECT_NEAR(attCorr[8], 0, 1e-8); } - TEST_F(OrbitalLineScanSensorModel, getIlluminationDirectionStationary) { // Get state information, replace sun position / velocity to hit third case: // One position, no velocity. std::string state = sensorModel->getModelState(); json jState = json::parse(state); - jState["m_sunPosition"] = std::vector{100.0,100.0,100.0}; + jState["m_sunPosition"] = std::vector{100.0, 100.0, 100.0}; jState["m_sunVelocity"] = std::vector{}; sensorModel->replaceModelState(jState.dump()); - csm::ImageCoord imagePt(8.5,8); + csm::ImageCoord imagePt(8.5, 8); csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); csm::EcefVector direction = sensorModel->getIlluminationDirection(groundPt); @@ -129,8 +131,9 @@ TEST_F(OrbitalLineScanSensorModel, getIlluminationDirectionStationary) { double expected_y = groundPt.y - sensorModel->m_sunPosition[1]; double expected_z = groundPt.z - sensorModel->m_sunPosition[2]; - //normalize - double scale = sqrt((expected_x * expected_x) + (expected_y * expected_y) + (expected_z * expected_z)); + // normalize + double scale = sqrt((expected_x * expected_x) + (expected_y * expected_y) + + (expected_z * expected_z)); expected_x /= scale; expected_y /= scale; expected_z /= scale; @@ -140,19 +143,19 @@ TEST_F(OrbitalLineScanSensorModel, getIlluminationDirectionStationary) { EXPECT_DOUBLE_EQ(direction.z, expected_z); } -TEST_F(OrbitalLineScanSensorModel, getSunPositionLagrange){ +TEST_F(OrbitalLineScanSensorModel, getSunPositionLagrange) { csm::EcefVector sunPosition = sensorModel->getSunPosition(-.6); EXPECT_NEAR(sunPosition.x, 125, 1e-11); EXPECT_NEAR(sunPosition.y, 125, 1e-11); EXPECT_NEAR(sunPosition.z, 125, 1e-11); } -TEST_F(OrbitalLineScanSensorModel, getSunPositionLinear){ +TEST_F(OrbitalLineScanSensorModel, getSunPositionLinear) { // Get state information, replace sun position / velocity to hit third case: // One position, no velocity. std::string state = sensorModel->getModelState(); json jState = json::parse(state); - jState["m_sunPosition"] = std::vector{100.0,100.0,100.0}; + jState["m_sunPosition"] = std::vector{100.0, 100.0, 100.0}; jState["m_sunVelocity"] = std::vector{50.0, 50.0, 50.0}; sensorModel->replaceModelState(jState.dump()); @@ -162,12 +165,12 @@ TEST_F(OrbitalLineScanSensorModel, getSunPositionLinear){ EXPECT_DOUBLE_EQ(sunPosition.z, 125); } -TEST_F(OrbitalLineScanSensorModel, getSunPositionStationary){ +TEST_F(OrbitalLineScanSensorModel, getSunPositionStationary) { // Get state information, replace sun position / velocity to hit third case: // One position, no velocity. std::string state = sensorModel->getModelState(); json jState = json::parse(state); - jState["m_sunPosition"] = std::vector{100.0,100.0,100.0}; + jState["m_sunPosition"] = std::vector{100.0, 100.0, 100.0}; jState["m_sunVelocity"] = std::vector{}; sensorModel->replaceModelState(jState.dump()); @@ -190,7 +193,8 @@ TEST_F(OrbitalLineScanSensorModel, Inversion) { for (double line = 0.5; line < 16; line++) { csm::ImageCoord imagePt(line, 8); csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 0.0); - csm::ImageCoord imageReprojPt = sensorModel->groundToImage(groundPt, 0.001, &achievedPrecision); + csm::ImageCoord imageReprojPt = + sensorModel->groundToImage(groundPt, 0.001, &achievedPrecision); // groundToImage has a default precision of 0.001m and each pixel is 100m // so we should be within 0.1 pixels @@ -203,9 +207,8 @@ TEST_F(OrbitalLineScanSensorModel, Inversion) { TEST_F(OrbitalLineScanSensorModel, ImageToGroundHeight) { csm::ImageCoord imagePt(8.5, 8); csm::EcefCoord groundPt = sensorModel->imageToGround(imagePt, 100.0); - double height = sqrt(groundPt.x*groundPt.x + - groundPt.y*groundPt.y + - groundPt.z*groundPt.z); + double height = sqrt(groundPt.x * groundPt.x + groundPt.y * groundPt.y + + groundPt.z * groundPt.z); EXPECT_DOUBLE_EQ(height, 1000100); } @@ -242,29 +245,34 @@ TEST_F(OrbitalLineScanSensorModel, ReferenceDateTime) { } TEST_F(TwoLineScanSensorModels, CrossCovariance) { - std::vector crossCovars = sensorModel1->getCrossCovarianceMatrix(*sensorModel2); - ASSERT_EQ(crossCovars.size(), sensorModel1->getNumParameters() * sensorModel2->getNumParameters()); + std::vector crossCovars = + sensorModel1->getCrossCovarianceMatrix(*sensorModel2); + ASSERT_EQ(crossCovars.size(), sensorModel1->getNumParameters() * + sensorModel2->getNumParameters()); for (int i = 0; i < sensorModel1->getNumParameters(); i++) { for (int j = 0; j < sensorModel2->getNumParameters(); j++) { EXPECT_EQ(crossCovars[i * sensorModel2->getNumParameters() + j], 0.0) - << "Value at row " << i << " column " << j; + << "Value at row " << i << " column " << j; } } - std::vector covars = sensorModel1->getCrossCovarianceMatrix(*sensorModel1); - ASSERT_EQ(covars.size(), 16*16); + std::vector covars = + sensorModel1->getCrossCovarianceMatrix(*sensorModel1); + ASSERT_EQ(covars.size(), 16 * 16); for (int i = 0; i < 16; i++) { for (int j = 0; j < 16; j++) { if (i == j) { - EXPECT_GT(covars[i * 16 + j], 0.0) << "Value at row " << i << " column " << j; - } - else { - EXPECT_EQ(covars[i * 16 + j], 0.0) << "Value at row " << i << " column " << j; + EXPECT_GT(covars[i * 16 + j], 0.0) + << "Value at row " << i << " column " << j; + } else { + EXPECT_EQ(covars[i * 16 + j], 0.0) + << "Value at row " << i << " column " << j; } } } - std::vector fixedCovars = sensorModel1->getCrossCovarianceMatrix(*sensorModel1, csm::param::NON_ADJUSTABLE); + std::vector fixedCovars = sensorModel1->getCrossCovarianceMatrix( + *sensorModel1, csm::param::NON_ADJUSTABLE); EXPECT_EQ(fixedCovars.size(), 0); } diff --git a/tests/PluginTests.cpp b/tests/PluginTests.cpp index 5f5f9fe84..6fccfddd3 100644 --- a/tests/PluginTests.cpp +++ b/tests/PluginTests.cpp @@ -1,285 +1,247 @@ -#include "UsgsAstroPlugin.h" #include "UsgsAstroFrameSensorModel.h" #include "UsgsAstroLsSensorModel.h" +#include "UsgsAstroPlugin.h" -#include #include +#include #include #include "Fixtures.h" TEST(PluginTests, PluginName) { - UsgsAstroPlugin testPlugin; - EXPECT_EQ("UsgsAstroPluginCSM", testPlugin.getPluginName()); + UsgsAstroPlugin testPlugin; + EXPECT_EQ("UsgsAstroPluginCSM", testPlugin.getPluginName()); } TEST(PluginTests, ManufacturerName) { - UsgsAstroPlugin testPlugin; - EXPECT_EQ("UsgsAstrogeology", testPlugin.getManufacturer()); + UsgsAstroPlugin testPlugin; + EXPECT_EQ("UsgsAstrogeology", testPlugin.getManufacturer()); } TEST(PluginTests, ReleaseDate) { - UsgsAstroPlugin testPlugin; - EXPECT_EQ("20190222", testPlugin.getReleaseDate()); + UsgsAstroPlugin testPlugin; + EXPECT_EQ("20190222", testPlugin.getReleaseDate()); } TEST(PluginTests, NumModels) { - UsgsAstroPlugin testPlugin; - EXPECT_EQ(3, testPlugin.getNumModels()); + UsgsAstroPlugin testPlugin; + EXPECT_EQ(3, testPlugin.getNumModels()); } TEST(PluginTests, BadISDFile) { - UsgsAstroPlugin testPlugin; - csm::Isd badIsd("Not a file"); - EXPECT_FALSE(testPlugin.canModelBeConstructedFromISD( - badIsd, - "USGS_ASTRO_FRAME_SENSOR_MODEL")); - EXPECT_FALSE(testPlugin.canModelBeConstructedFromISD( - badIsd, - "USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL")); + UsgsAstroPlugin testPlugin; + csm::Isd badIsd("Not a file"); + EXPECT_FALSE(testPlugin.canModelBeConstructedFromISD( + badIsd, "USGS_ASTRO_FRAME_SENSOR_MODEL")); + EXPECT_FALSE(testPlugin.canModelBeConstructedFromISD( + badIsd, "USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL")); } TEST(FrameStateTests, NoStateName) { - UsgsAstroPlugin testPlugin; - std::string badState = "{\"not_a_name\":\"bad_name\"}"; - EXPECT_FALSE(testPlugin.canModelBeConstructedFromState( - "USGS_ASTRO_FRAME_SENSOR_MODEL", - badState)); + UsgsAstroPlugin testPlugin; + std::string badState = "{\"not_a_name\":\"bad_name\"}"; + EXPECT_FALSE(testPlugin.canModelBeConstructedFromState( + "USGS_ASTRO_FRAME_SENSOR_MODEL", badState)); } TEST(FrameStateTests, BadStateName) { - UsgsAstroPlugin testPlugin; - std::string badState = "{\"m_model_name\":\"bad_name\"}"; - EXPECT_FALSE(testPlugin.canModelBeConstructedFromState( - "USGS_ASTRO_FRAME_SENSOR_MODEL", - badState)); + UsgsAstroPlugin testPlugin; + std::string badState = "{\"m_model_name\":\"bad_name\"}"; + EXPECT_FALSE(testPlugin.canModelBeConstructedFromState( + "USGS_ASTRO_FRAME_SENSOR_MODEL", badState)); } TEST(FrameStateTests, BadStateValue) { - UsgsAstroPlugin testPlugin; - std::string badState = "{" - "\"m_model_name\":\"USGS_ASTRO_FRAME_SENSOR_MODEL\"," - "\"bad_param\":\"bad_value\"}"; - EXPECT_FALSE(testPlugin.canModelBeConstructedFromState( - "USGS_ASTRO_FRAME_SENSOR_MODEL", - badState)); + UsgsAstroPlugin testPlugin; + std::string badState = + "{" + "\"m_model_name\":\"USGS_ASTRO_FRAME_SENSOR_MODEL\"," + "\"bad_param\":\"bad_value\"}"; + EXPECT_FALSE(testPlugin.canModelBeConstructedFromState( + "USGS_ASTRO_FRAME_SENSOR_MODEL", badState)); } TEST(FrameStateTests, MissingStateValue) { - UsgsAstroPlugin testPlugin; - std::string badState = "{" - "\"m_model_name\":\"USGS_ASTRO_FRAME_SENSOR_MODEL\"}"; - EXPECT_FALSE(testPlugin.canModelBeConstructedFromState( - "USGS_ASTRO_FRAME_SENSOR_MODEL", - badState)); + UsgsAstroPlugin testPlugin; + std::string badState = + "{" + "\"m_model_name\":\"USGS_ASTRO_FRAME_SENSOR_MODEL\"}"; + EXPECT_FALSE(testPlugin.canModelBeConstructedFromState( + "USGS_ASTRO_FRAME_SENSOR_MODEL", badState)); } TEST_F(FrameIsdTest, Constructible) { - UsgsAstroPlugin testPlugin; - EXPECT_TRUE(testPlugin.canModelBeConstructedFromISD( - isd, - "USGS_ASTRO_FRAME_SENSOR_MODEL")); + UsgsAstroPlugin testPlugin; + EXPECT_TRUE(testPlugin.canModelBeConstructedFromISD( + isd, "USGS_ASTRO_FRAME_SENSOR_MODEL")); } TEST_F(FrameIsdTest, ConstructibleFromState) { - UsgsAstroPlugin testPlugin; - std::string modelState = testPlugin.getStateFromISD(isd); - EXPECT_TRUE(testPlugin.canModelBeConstructedFromState( - "USGS_ASTRO_FRAME_SENSOR_MODEL", - modelState)); + UsgsAstroPlugin testPlugin; + std::string modelState = testPlugin.getStateFromISD(isd); + EXPECT_TRUE(testPlugin.canModelBeConstructedFromState( + "USGS_ASTRO_FRAME_SENSOR_MODEL", modelState)); } TEST_F(FrameIsdTest, NotConstructible) { - UsgsAstroPlugin testPlugin; - isd.setFilename("data/constVelocityLineScan.img"); - EXPECT_FALSE(testPlugin.canModelBeConstructedFromISD( - isd, - "USGS_ASTRO_FRAME_SENSOR_MODEL")); + UsgsAstroPlugin testPlugin; + isd.setFilename("data/constVelocityLineScan.img"); + EXPECT_FALSE(testPlugin.canModelBeConstructedFromISD( + isd, "USGS_ASTRO_FRAME_SENSOR_MODEL")); } - TEST_F(FrameIsdTest, ConstructValidCamera) { - UsgsAstroPlugin testPlugin; - csm::Model *cameraModel = NULL; - EXPECT_NO_THROW( - cameraModel = testPlugin.constructModelFromISD( - isd, - "USGS_ASTRO_FRAME_SENSOR_MODEL", - NULL) - ); - UsgsAstroFrameSensorModel *frameModel = dynamic_cast(cameraModel); - EXPECT_NE(frameModel, nullptr); - if (cameraModel) { - delete cameraModel; - } + UsgsAstroPlugin testPlugin; + csm::Model *cameraModel = NULL; + EXPECT_NO_THROW(cameraModel = testPlugin.constructModelFromISD( + isd, "USGS_ASTRO_FRAME_SENSOR_MODEL", NULL)); + UsgsAstroFrameSensorModel *frameModel = + dynamic_cast(cameraModel); + EXPECT_NE(frameModel, nullptr); + if (cameraModel) { + delete cameraModel; + } } - TEST_F(FrameIsdTest, ConstructInValidCamera) { - UsgsAstroPlugin testPlugin; - isd.setFilename("data/empty.img"); - csm::Model *cameraModel = NULL; - try { - testPlugin.constructModelFromISD( - isd, - "USGS_ASTRO_FRAME_SENSOR_MODEL", - nullptr); - FAIL() << "Expected an error"; - } - catch(csm::Error &e) { - EXPECT_EQ(e.getError(), csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE); - } - catch(...) { - FAIL() << "Expected csm SENSOR_MODEL_NOT_CONSTRUCTIBLE error"; - } - if (cameraModel) { - delete cameraModel; - } + UsgsAstroPlugin testPlugin; + isd.setFilename("data/empty.img"); + csm::Model *cameraModel = NULL; + try { + testPlugin.constructModelFromISD(isd, "USGS_ASTRO_FRAME_SENSOR_MODEL", + nullptr); + FAIL() << "Expected an error"; + } catch (csm::Error &e) { + EXPECT_EQ(e.getError(), csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE); + } catch (...) { + FAIL() << "Expected csm SENSOR_MODEL_NOT_CONSTRUCTIBLE error"; + } + if (cameraModel) { + delete cameraModel; + } } TEST_F(ConstVelLineScanIsdTest, Constructible) { - UsgsAstroPlugin testPlugin; - EXPECT_TRUE(testPlugin.canModelBeConstructedFromISD( - isd, - "USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL")); + UsgsAstroPlugin testPlugin; + EXPECT_TRUE(testPlugin.canModelBeConstructedFromISD( + isd, "USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL")); } TEST_F(ConstVelLineScanIsdTest, ConstructibleFromState) { - UsgsAstroPlugin testPlugin; - std::string modelState = testPlugin.getStateFromISD(isd); - EXPECT_TRUE(testPlugin.canModelBeConstructedFromState( - "USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL", - modelState)); + UsgsAstroPlugin testPlugin; + std::string modelState = testPlugin.getStateFromISD(isd); + EXPECT_TRUE(testPlugin.canModelBeConstructedFromState( + "USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL", modelState)); } TEST_F(ConstVelLineScanIsdTest, NotConstructible) { - UsgsAstroPlugin testPlugin; - isd.setFilename("data/simpleFramerISD.img"); - EXPECT_FALSE(testPlugin.canModelBeConstructedFromISD( - isd, - "USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL")); + UsgsAstroPlugin testPlugin; + isd.setFilename("data/simpleFramerISD.img"); + EXPECT_FALSE(testPlugin.canModelBeConstructedFromISD( + isd, "USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL")); } TEST_F(ConstVelLineScanIsdTest, ConstructValidCamera) { - UsgsAstroPlugin testPlugin; - csm::Model *cameraModel = NULL; - EXPECT_NO_THROW( - cameraModel = testPlugin.constructModelFromISD( - isd, - "USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL", - NULL) - ); - UsgsAstroLsSensorModel *frameModel = dynamic_cast(cameraModel); - EXPECT_NE(frameModel, nullptr); - if (cameraModel) { - delete cameraModel; - } + UsgsAstroPlugin testPlugin; + csm::Model *cameraModel = NULL; + EXPECT_NO_THROW(cameraModel = testPlugin.constructModelFromISD( + isd, "USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL", NULL)); + UsgsAstroLsSensorModel *frameModel = + dynamic_cast(cameraModel); + EXPECT_NE(frameModel, nullptr); + if (cameraModel) { + delete cameraModel; + } } TEST_F(ConstVelLineScanIsdTest, ConstructInValidCamera) { - UsgsAstroPlugin testPlugin; - isd.setFilename("data/empty.img"); - csm::Model *cameraModel = NULL; - try { - testPlugin.constructModelFromISD( - isd, - "USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL", - nullptr); - FAIL() << "Expected an error"; - - } - catch(csm::Error &e) { - EXPECT_EQ(e.getError(), csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE); - } - catch(...) { - FAIL() << "Expected csm SENSOR_MODEL_NOT_CONSTRUCTIBLE error"; - } - if (cameraModel) { - delete cameraModel; - } + UsgsAstroPlugin testPlugin; + isd.setFilename("data/empty.img"); + csm::Model *cameraModel = NULL; + try { + testPlugin.constructModelFromISD( + isd, "USGS_ASTRO_LINE_SCANNER_SENSOR_MODEL", nullptr); + FAIL() << "Expected an error"; + + } catch (csm::Error &e) { + EXPECT_EQ(e.getError(), csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE); + } catch (...) { + FAIL() << "Expected csm SENSOR_MODEL_NOT_CONSTRUCTIBLE error"; + } + if (cameraModel) { + delete cameraModel; + } } TEST_F(SarIsdTest, Constructible) { UsgsAstroPlugin testPlugin; csm::WarningList warnings; EXPECT_TRUE(testPlugin.canModelBeConstructedFromISD( - isd, - "USGS_ASTRO_SAR_SENSOR_MODEL", - &warnings)); - for (auto &warn: warnings) { + isd, "USGS_ASTRO_SAR_SENSOR_MODEL", &warnings)); + for (auto &warn : warnings) { std::cerr << "Warning in " << warn.getFunction() << std::endl; std::cerr << " " << warn.getMessage() << std::endl; } } TEST_F(SarIsdTest, ConstructibleFromState) { - UsgsAstroPlugin testPlugin; - csm::WarningList warnings; - std::string modelState = testPlugin.getStateFromISD(isd); - EXPECT_TRUE(testPlugin.canModelBeConstructedFromState( - "USGS_ASTRO_SAR_SENSOR_MODEL", - modelState, - &warnings)); - for (auto &warn: warnings) { + UsgsAstroPlugin testPlugin; + csm::WarningList warnings; + std::string modelState = testPlugin.getStateFromISD(isd); + EXPECT_TRUE(testPlugin.canModelBeConstructedFromState( + "USGS_ASTRO_SAR_SENSOR_MODEL", modelState, &warnings)); + for (auto &warn : warnings) { std::cerr << "Warning in " << warn.getFunction() << std::endl; std::cerr << " " << warn.getMessage() << std::endl; } } TEST_F(SarIsdTest, NotConstructible) { - UsgsAstroPlugin testPlugin; - isd.setFilename("data/simpleFramerISD.img"); - EXPECT_FALSE(testPlugin.canModelBeConstructedFromISD( - isd, - "USGS_ASTRO_SAR_SENSOR_MODEL")); + UsgsAstroPlugin testPlugin; + isd.setFilename("data/simpleFramerISD.img"); + EXPECT_FALSE(testPlugin.canModelBeConstructedFromISD( + isd, "USGS_ASTRO_SAR_SENSOR_MODEL")); } TEST_F(SarIsdTest, ConstructValidCamera) { - UsgsAstroPlugin testPlugin; - csm::WarningList warnings; - csm::Model *cameraModel = NULL; - EXPECT_NO_THROW( - cameraModel = testPlugin.constructModelFromISD( - isd, - "USGS_ASTRO_SAR_SENSOR_MODEL", - &warnings) - ); - for (auto &warn: warnings) { - std::cerr << "Warning in " << warn.getFunction() << std::endl; - std::cerr << " " << warn.getMessage() << std::endl; - } - UsgsAstroSarSensorModel *sarModel = dynamic_cast(cameraModel); - EXPECT_NE(sarModel, nullptr); - if (cameraModel) { - delete cameraModel; - } + UsgsAstroPlugin testPlugin; + csm::WarningList warnings; + csm::Model *cameraModel = NULL; + EXPECT_NO_THROW(cameraModel = testPlugin.constructModelFromISD( + isd, "USGS_ASTRO_SAR_SENSOR_MODEL", &warnings)); + for (auto &warn : warnings) { + std::cerr << "Warning in " << warn.getFunction() << std::endl; + std::cerr << " " << warn.getMessage() << std::endl; + } + UsgsAstroSarSensorModel *sarModel = + dynamic_cast(cameraModel); + EXPECT_NE(sarModel, nullptr); + if (cameraModel) { + delete cameraModel; + } } TEST_F(SarIsdTest, ConstructInValidCamera) { - UsgsAstroPlugin testPlugin; - isd.setFilename("data/empty.img"); - csm::Model *cameraModel = NULL; - try { - testPlugin.constructModelFromISD( - isd, - "USGS_ASTRO_SAR_SENSOR_MODEL", - nullptr); - FAIL() << "Expected an error"; - - } - catch(csm::Error &e) { - EXPECT_EQ(e.getError(), csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE); - } - catch(...) { - FAIL() << "Expected csm SENSOR_MODEL_NOT_CONSTRUCTIBLE error"; - } - if (cameraModel) { - delete cameraModel; - } + UsgsAstroPlugin testPlugin; + isd.setFilename("data/empty.img"); + csm::Model *cameraModel = NULL; + try { + testPlugin.constructModelFromISD(isd, "USGS_ASTRO_SAR_SENSOR_MODEL", + nullptr); + FAIL() << "Expected an error"; + + } catch (csm::Error &e) { + EXPECT_EQ(e.getError(), csm::Error::SENSOR_MODEL_NOT_CONSTRUCTIBLE); + } catch (...) { + FAIL() << "Expected csm SENSOR_MODEL_NOT_CONSTRUCTIBLE error"; + } + if (cameraModel) { + delete cameraModel; + } } int main(int argc, char **argv) { - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } diff --git a/tests/SarTests.cpp b/tests/SarTests.cpp index 020920261..5e843bbd3 100644 --- a/tests/SarTests.cpp +++ b/tests/SarTests.cpp @@ -2,16 +2,16 @@ #include "UsgsAstroSarSensorModel.h" -#include "Warning.h" #include "Fixtures.h" +#include "Warning.h" -#include #include +#include #include -#include #include #include +#include using json = nlohmann::json; @@ -24,9 +24,8 @@ TEST_F(SarSensorModel, stateFromIsd) { std::string stateString; try { stateString = sensorModel->constructStateFromIsd(isdString, &warnings); - } - catch(...) { - for (auto &warn: warnings) { + } catch (...) { + for (auto &warn : warnings) { std::cerr << "Warning in " << warn.getFunction() << std::endl; std::cerr << " " << warn.getMessage() << std::endl; } @@ -37,9 +36,7 @@ TEST_F(SarSensorModel, stateFromIsd) { TEST_F(SarSensorModel, State) { std::string modelState = sensorModel->getModelState(); - EXPECT_NO_THROW( - sensorModel->replaceModelState(modelState) - ); + EXPECT_NO_THROW(sensorModel->replaceModelState(modelState)); EXPECT_EQ(sensorModel->getModelState(), modelState); } @@ -52,7 +49,8 @@ TEST_F(SarSensorModel, Center) { } TEST_F(SarSensorModel, GroundToImage) { - csm::EcefCoord groundPt(1737387.8590770673, -5303.280537826621, -3749.9796183814506); + csm::EcefCoord groundPt(1737387.8590770673, -5303.280537826621, + -3749.9796183814506); csm::ImageCoord imagePt = sensorModel->groundToImage(groundPt, 0.001); EXPECT_NEAR(imagePt.line, 500.0, 1e-3); EXPECT_NEAR(imagePt.samp, 500.0, 1e-3); @@ -95,7 +93,8 @@ TEST_F(SarSensorModel, computeGroundPartials) { TEST_F(SarSensorModel, imageToProximateImagingLocus) { csm::EcefLocus locus = sensorModel->imageToProximateImagingLocus( csm::ImageCoord(500.0, 500.0), - csm::EcefCoord(1737287.8590770673, -5403.280537826621, -3849.9796183814506)); + csm::EcefCoord(1737287.8590770673, -5403.280537826621, + -3849.9796183814506)); EXPECT_NEAR(locus.point.x, 1737388.1260092105, 1e-2); EXPECT_NEAR(locus.point.y, -5403.0102509726485, 1e-2); EXPECT_NEAR(locus.point.z, -3749.9801945280433, 1e-2); @@ -105,23 +104,26 @@ TEST_F(SarSensorModel, imageToProximateImagingLocus) { } TEST_F(SarSensorModel, imageToRemoteImagingLocus) { - csm::EcefLocus locus = sensorModel->imageToRemoteImagingLocus( - csm::ImageCoord(500.0, 500.0)); - EXPECT_NEAR(locus.point.x, 1737380.8279381434, 1e-3); - EXPECT_NEAR(locus.point.y, 0.0, 1e-3); - EXPECT_NEAR(locus.point.z, -3749.964442364465, 1e-3); - EXPECT_NEAR(locus.direction.x, 0.0, 1e-8); - EXPECT_NEAR(locus.direction.y, -1.0, 1e-8); - EXPECT_NEAR(locus.direction.z, 0.0, 1e-8); + csm::EcefLocus locus = + sensorModel->imageToRemoteImagingLocus(csm::ImageCoord(500.0, 500.0)); + EXPECT_NEAR(locus.point.x, 1737380.8279381434, 1e-3); + EXPECT_NEAR(locus.point.y, 0.0, 1e-3); + EXPECT_NEAR(locus.point.z, -3749.964442364465, 1e-3); + EXPECT_NEAR(locus.direction.x, 0.0, 1e-8); + EXPECT_NEAR(locus.direction.y, -1.0, 1e-8); + EXPECT_NEAR(locus.direction.z, 0.0, 1e-8); } TEST_F(SarSensorModel, adjustedPositionVelocity) { - std::vector adjustments = {1000000.0, 0.2, -10.0, -20.0, 0.5, 2000000.0}; + std::vector adjustments = {1000000.0, 0.2, -10.0, + -20.0, 0.5, 2000000.0}; csm::EcefCoord sensorPosition = sensorModel->getSensorPosition(0.0); csm::EcefVector sensorVelocity = sensorModel->getSensorVelocity(0.0); - csm::EcefCoord adjPosition = sensorModel->getAdjustedSensorPosition(0.0, adjustments); - csm::EcefVector adjVelocity = sensorModel->getAdjustedSensorVelocity(0.0, adjustments); - + csm::EcefCoord adjPosition = + sensorModel->getAdjustedSensorPosition(0.0, adjustments); + csm::EcefVector adjVelocity = + sensorModel->getAdjustedSensorVelocity(0.0, adjustments); + EXPECT_NEAR(adjPosition.x, sensorPosition.x + adjustments[0], 1e-2); EXPECT_NEAR(adjPosition.y, sensorPosition.y + adjustments[1], 1e-2); EXPECT_NEAR(adjPosition.z, sensorPosition.z + adjustments[2], 1e-2); diff --git a/tests/TestMain.cpp b/tests/TestMain.cpp index b29705146..35932dc5c 100644 --- a/tests/TestMain.cpp +++ b/tests/TestMain.cpp @@ -1,6 +1,5 @@ #include "gtest/gtest.h" -int main(int argc, char **argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); +int main(int argc, char **argv) { + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); } \ No newline at end of file diff --git a/tests/UtilitiesTests.cpp b/tests/UtilitiesTests.cpp index f228c63c4..21fe02d99 100644 --- a/tests/UtilitiesTests.cpp +++ b/tests/UtilitiesTests.cpp @@ -4,173 +4,136 @@ #include "UsgsAstroPlugin.h" #include "Utilities.h" -#include #include +#include #include -#include #include +#include using json = nlohmann::json; TEST(UtilitiesTests, calculateRotationMatrixFromEuler) { - double euler[3], rotationMatrix[9]; - euler[0] = 0; - euler[1] = M_PI/2; - euler[2] = 0; - calculateRotationMatrixFromEuler(euler, rotationMatrix); - - // EXPECT_NEARs are used here instead of EXPECT_DOUBLE_EQs because index 0 and 8 of the matrix - // are evaluating to 6.12...e-17. This is too small to be worried about here, but - // EXPECT_DOUBLE_EQ is too sensitive. - EXPECT_NEAR(rotationMatrix[0], 0, 1e-8); - EXPECT_NEAR(rotationMatrix[1], 0, 1e-8); - EXPECT_NEAR(rotationMatrix[2], 1, 1e-8); - EXPECT_NEAR(rotationMatrix[3], 0, 1e-8); - EXPECT_NEAR(rotationMatrix[4], 1, 1e-8); - EXPECT_NEAR(rotationMatrix[5], 0, 1e-8); - EXPECT_NEAR(rotationMatrix[6], -1, 1e-8); - EXPECT_NEAR(rotationMatrix[7], 0, 1e-8); - EXPECT_NEAR(rotationMatrix[8], 0, 1e-8); + double euler[3], rotationMatrix[9]; + euler[0] = 0; + euler[1] = M_PI / 2; + euler[2] = 0; + calculateRotationMatrixFromEuler(euler, rotationMatrix); + + // EXPECT_NEARs are used here instead of EXPECT_DOUBLE_EQs because index 0 and + // 8 of the matrix are evaluating to 6.12...e-17. This is too small to be + // worried about here, but EXPECT_DOUBLE_EQ is too sensitive. + EXPECT_NEAR(rotationMatrix[0], 0, 1e-8); + EXPECT_NEAR(rotationMatrix[1], 0, 1e-8); + EXPECT_NEAR(rotationMatrix[2], 1, 1e-8); + EXPECT_NEAR(rotationMatrix[3], 0, 1e-8); + EXPECT_NEAR(rotationMatrix[4], 1, 1e-8); + EXPECT_NEAR(rotationMatrix[5], 0, 1e-8); + EXPECT_NEAR(rotationMatrix[6], -1, 1e-8); + EXPECT_NEAR(rotationMatrix[7], 0, 1e-8); + EXPECT_NEAR(rotationMatrix[8], 0, 1e-8); } TEST(UtilitiesTests, calculateRotationMatrixFromQuaternions) { - double q[4], rotationMatrix[9]; - q[0] = 0; - q[1] = -1/sqrt(2); - q[2] = 0; - q[3] = 1/sqrt(2); - calculateRotationMatrixFromQuaternions(q, rotationMatrix); - EXPECT_DOUBLE_EQ(rotationMatrix[0], 0); - EXPECT_DOUBLE_EQ(rotationMatrix[1], 0); - EXPECT_DOUBLE_EQ(rotationMatrix[2], -1); - EXPECT_DOUBLE_EQ(rotationMatrix[3], 0); - EXPECT_DOUBLE_EQ(rotationMatrix[4], 1); - EXPECT_DOUBLE_EQ(rotationMatrix[5], 0); - EXPECT_DOUBLE_EQ(rotationMatrix[6], 1); - EXPECT_DOUBLE_EQ(rotationMatrix[7], 0); - EXPECT_DOUBLE_EQ(rotationMatrix[8], 0); + double q[4], rotationMatrix[9]; + q[0] = 0; + q[1] = -1 / sqrt(2); + q[2] = 0; + q[3] = 1 / sqrt(2); + calculateRotationMatrixFromQuaternions(q, rotationMatrix); + EXPECT_DOUBLE_EQ(rotationMatrix[0], 0); + EXPECT_DOUBLE_EQ(rotationMatrix[1], 0); + EXPECT_DOUBLE_EQ(rotationMatrix[2], -1); + EXPECT_DOUBLE_EQ(rotationMatrix[3], 0); + EXPECT_DOUBLE_EQ(rotationMatrix[4], 1); + EXPECT_DOUBLE_EQ(rotationMatrix[5], 0); + EXPECT_DOUBLE_EQ(rotationMatrix[6], 1); + EXPECT_DOUBLE_EQ(rotationMatrix[7], 0); + EXPECT_DOUBLE_EQ(rotationMatrix[8], 0); } TEST(UtilitiesTests, computeDistortedFocalPlaneCoordinates) { - double iTransS[] = {0.0, 0.0, 10.0}; - double iTransL[] = {0.0, 10.0, 0.0}; - double undistortedFocalPlaneX, undistortedFocalPlaneY; - computeDistortedFocalPlaneCoordinates( - 0.5, 4.0, - 8.0, 0.5, - 1.0, 1.0, - 0.0, 0.0, - iTransS, iTransL, - undistortedFocalPlaneX, undistortedFocalPlaneY); - EXPECT_DOUBLE_EQ(undistortedFocalPlaneX, 0); - EXPECT_DOUBLE_EQ(undistortedFocalPlaneY, -0.4); + double iTransS[] = {0.0, 0.0, 10.0}; + double iTransL[] = {0.0, 10.0, 0.0}; + double undistortedFocalPlaneX, undistortedFocalPlaneY; + computeDistortedFocalPlaneCoordinates( + 0.5, 4.0, 8.0, 0.5, 1.0, 1.0, 0.0, 0.0, iTransS, iTransL, + undistortedFocalPlaneX, undistortedFocalPlaneY); + EXPECT_DOUBLE_EQ(undistortedFocalPlaneX, 0); + EXPECT_DOUBLE_EQ(undistortedFocalPlaneY, -0.4); } TEST(UtilitiesTests, computeDistortedFocalPlaneCoordinatesSumming) { - double iTransS[] = {0.0, 0.0, 10.0}; - double iTransL[] = {0.0, 10.0, 0.0}; - double undistortedFocalPlaneX, undistortedFocalPlaneY; - computeDistortedFocalPlaneCoordinates( - 2.0, 4.0, - 8.0, 8.0, - 2.0, 2.0, - 0.0, 0.0, - iTransS, iTransL, - undistortedFocalPlaneX, undistortedFocalPlaneY); - EXPECT_DOUBLE_EQ(undistortedFocalPlaneX, -0.4); - EXPECT_DOUBLE_EQ(undistortedFocalPlaneY, 0); + double iTransS[] = {0.0, 0.0, 10.0}; + double iTransL[] = {0.0, 10.0, 0.0}; + double undistortedFocalPlaneX, undistortedFocalPlaneY; + computeDistortedFocalPlaneCoordinates( + 2.0, 4.0, 8.0, 8.0, 2.0, 2.0, 0.0, 0.0, iTransS, iTransL, + undistortedFocalPlaneX, undistortedFocalPlaneY); + EXPECT_DOUBLE_EQ(undistortedFocalPlaneX, -0.4); + EXPECT_DOUBLE_EQ(undistortedFocalPlaneY, 0); } TEST(UtilitiesTests, computeDistortedFocalPlaneCoordinatesAffine) { - double iTransS[] = {-10.0, 0.0, 0.1}; - double iTransL[] = {10.0, -0.1, 0.0}; - double undistortedFocalPlaneX, undistortedFocalPlaneY; - computeDistortedFocalPlaneCoordinates( - 11.0, -9.0, - 0.0, 0.0, - 1.0, 1.0, - 0.0, 0.0, - iTransS, iTransL, - undistortedFocalPlaneX, undistortedFocalPlaneY); - EXPECT_NEAR(undistortedFocalPlaneX, -10.0, 1e-12); - EXPECT_NEAR(undistortedFocalPlaneY, 10.0, 1e-12); + double iTransS[] = {-10.0, 0.0, 0.1}; + double iTransL[] = {10.0, -0.1, 0.0}; + double undistortedFocalPlaneX, undistortedFocalPlaneY; + computeDistortedFocalPlaneCoordinates( + 11.0, -9.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0, iTransS, iTransL, + undistortedFocalPlaneX, undistortedFocalPlaneY); + EXPECT_NEAR(undistortedFocalPlaneX, -10.0, 1e-12); + EXPECT_NEAR(undistortedFocalPlaneY, 10.0, 1e-12); } - TEST(UtilitiesTests, computeDistortedFocalPlaneCoordinatesStart) { - double iTransS[] = {0.0, 0.0, 10.0}; - double iTransL[] = {0.0, 10.0, 0.0}; - double undistortedFocalPlaneX, undistortedFocalPlaneY; - computeDistortedFocalPlaneCoordinates( - 2.0, 4.0, - 8.0, 8.0, - 1.0, 1.0, - 2.0, 1.0, - iTransS, iTransL, - undistortedFocalPlaneX, undistortedFocalPlaneY); - EXPECT_DOUBLE_EQ(undistortedFocalPlaneX, -0.5); - EXPECT_DOUBLE_EQ(undistortedFocalPlaneY, -0.2); + double iTransS[] = {0.0, 0.0, 10.0}; + double iTransL[] = {0.0, 10.0, 0.0}; + double undistortedFocalPlaneX, undistortedFocalPlaneY; + computeDistortedFocalPlaneCoordinates( + 2.0, 4.0, 8.0, 8.0, 1.0, 1.0, 2.0, 1.0, iTransS, iTransL, + undistortedFocalPlaneX, undistortedFocalPlaneY); + EXPECT_DOUBLE_EQ(undistortedFocalPlaneX, -0.5); + EXPECT_DOUBLE_EQ(undistortedFocalPlaneY, -0.2); } TEST(UtilitiesTests, computePixel) { - double iTransS[] = {0.0, 0.0, 10.0}; - double iTransL[] = {0.0, 10.0, 0.0}; - double line, sample; - computePixel( - 0.0, -0.4, - 8.0, 0.5, - 1.0, 1.0, - 0.0, 0.0, - iTransS, iTransL, - line, sample); - EXPECT_DOUBLE_EQ(line, 0.5); - EXPECT_DOUBLE_EQ(sample, 4.0); + double iTransS[] = {0.0, 0.0, 10.0}; + double iTransL[] = {0.0, 10.0, 0.0}; + double line, sample; + computePixel(0.0, -0.4, 8.0, 0.5, 1.0, 1.0, 0.0, 0.0, iTransS, iTransL, line, + sample); + EXPECT_DOUBLE_EQ(line, 0.5); + EXPECT_DOUBLE_EQ(sample, 4.0); } TEST(UtilitiesTests, computePixelSumming) { - double iTransS[] = {0.0, 0.0, 10.0}; - double iTransL[] = {0.0, 10.0, 0.0}; - double line, sample; - computePixel( - -0.4, 0.0, - 8.0, 8.0, - 2.0, 2.0, - 0.0, 0.0, - iTransS, iTransL, - line, sample); - EXPECT_DOUBLE_EQ(line, 2.0); - EXPECT_DOUBLE_EQ(sample, 4.0); + double iTransS[] = {0.0, 0.0, 10.0}; + double iTransL[] = {0.0, 10.0, 0.0}; + double line, sample; + computePixel(-0.4, 0.0, 8.0, 8.0, 2.0, 2.0, 0.0, 0.0, iTransS, iTransL, line, + sample); + EXPECT_DOUBLE_EQ(line, 2.0); + EXPECT_DOUBLE_EQ(sample, 4.0); } TEST(UtilitiesTests, computePixelStart) { - double iTransS[] = {0.0, 0.0, 10.0}; - double iTransL[] = {0.0, 10.0, 0.0}; - double line, sample; - computePixel( - -0.5, -0.2, - 8.0, 8.0, - 1.0, 1.0, - 2.0, 1.0, - iTransS, iTransL, - line, sample); - EXPECT_DOUBLE_EQ(line, 2.0); - EXPECT_DOUBLE_EQ(sample, 4.0); + double iTransS[] = {0.0, 0.0, 10.0}; + double iTransL[] = {0.0, 10.0, 0.0}; + double line, sample; + computePixel(-0.5, -0.2, 8.0, 8.0, 1.0, 1.0, 2.0, 1.0, iTransS, iTransL, line, + sample); + EXPECT_DOUBLE_EQ(line, 2.0); + EXPECT_DOUBLE_EQ(sample, 4.0); } TEST(UtilitiesTests, computePixelStartSumming) { - double iTransS[] = {0.0, 0.0, 10.0}; - double iTransL[] = {0.0, 10.0, 0.0}; - double line, sample; - computePixel( - -0.5, -0.2, - 8.0, 8.0, - 2.0, 4.0, - 2.0, 1.0, - iTransS, iTransL, - line, sample); - EXPECT_DOUBLE_EQ(line, 0.5); - EXPECT_DOUBLE_EQ(sample, 2.0); + double iTransS[] = {0.0, 0.0, 10.0}; + double iTransL[] = {0.0, 10.0, 0.0}; + double line, sample; + computePixel(-0.5, -0.2, 8.0, 8.0, 2.0, 4.0, 2.0, 1.0, iTransS, iTransL, line, + sample); + EXPECT_DOUBLE_EQ(line, 0.5); + EXPECT_DOUBLE_EQ(sample, 2.0); } TEST(UtilitiesTests, createCameraLookVector) { @@ -192,15 +155,13 @@ TEST(UtilitiesTests, lagrangeInterp1Point) { int order = 8; try { - lagrangeInterp(numTime, &singlePoint[0], startTime, delTime, - time, vectorLength, order, &interpPoint[0]); - FAIL() << "Expected an error"; - } - catch(csm::Error &e) { - EXPECT_EQ(e.getError(), csm::Error::INDEX_OUT_OF_RANGE); - } - catch(...) { - FAIL() << "Expected csm INDEX_OUT_OF_RANGE error"; + lagrangeInterp(numTime, &singlePoint[0], startTime, delTime, time, + vectorLength, order, &interpPoint[0]); + FAIL() << "Expected an error"; + } catch (csm::Error &e) { + EXPECT_EQ(e.getError(), csm::Error::INDEX_OUT_OF_RANGE); + } catch (...) { + FAIL() << "Expected csm INDEX_OUT_OF_RANGE error"; } } @@ -214,8 +175,8 @@ TEST(UtilitiesTests, lagrangeInterp2ndOrder) { int vectorLength = 1; int order = 2; - lagrangeInterp(numTime, &interpValues[0], startTime, delTime, - time, vectorLength, order, &outputValue[0]); + lagrangeInterp(numTime, &interpValues[0], startTime, delTime, time, + vectorLength, order, &outputValue[0]); EXPECT_DOUBLE_EQ(outputValue[0], 24.0 / 2.0); } @@ -229,8 +190,8 @@ TEST(UtilitiesTests, lagrangeInterp4thOrder) { int vectorLength = 1; int order = 4; - lagrangeInterp(numTime, &interpValues[0], startTime, delTime, - time, vectorLength, order, &outputValue[0]); + lagrangeInterp(numTime, &interpValues[0], startTime, delTime, time, + vectorLength, order, &outputValue[0]); EXPECT_DOUBLE_EQ(outputValue[0], 180.0 / 16.0); } @@ -244,8 +205,8 @@ TEST(UtilitiesTests, lagrangeInterp6thOrder) { int vectorLength = 1; int order = 6; - lagrangeInterp(numTime, &interpValues[0], startTime, delTime, - time, vectorLength, order, &outputValue[0]); + lagrangeInterp(numTime, &interpValues[0], startTime, delTime, time, + vectorLength, order, &outputValue[0]); EXPECT_DOUBLE_EQ(outputValue[0], 2898.0 / 256.0); } @@ -259,8 +220,8 @@ TEST(UtilitiesTests, lagrangeInterp8thOrder) { int vectorLength = 1; int order = 8; - lagrangeInterp(numTime, &interpValues[0], startTime, delTime, - time, vectorLength, order, &outputValue[0]); + lagrangeInterp(numTime, &interpValues[0], startTime, delTime, time, + vectorLength, order, &outputValue[0]); EXPECT_DOUBLE_EQ(outputValue[0], 23169.0 / 2048.0); } @@ -274,13 +235,13 @@ TEST(UtilitiesTests, lagrangeInterpReduced2ndOrder) { int vectorLength = 1; int order = 8; - lagrangeInterp(numTime, &interpValues[0], startTime, delTime, - time, vectorLength, order, &outputValue[0]); + lagrangeInterp(numTime, &interpValues[0], startTime, delTime, time, + vectorLength, order, &outputValue[0]); EXPECT_DOUBLE_EQ(outputValue[0], 3.0 / 2.0); time = 6.5; - lagrangeInterp(numTime, &interpValues[0], startTime, delTime, - time, vectorLength, order, &outputValue[0]); + lagrangeInterp(numTime, &interpValues[0], startTime, delTime, time, + vectorLength, order, &outputValue[0]); EXPECT_DOUBLE_EQ(outputValue[0], 192.0 / 2.0); } @@ -294,13 +255,13 @@ TEST(UtilitiesTests, lagrangeInterpReduced4thOrder) { int vectorLength = 1; int order = 8; - lagrangeInterp(numTime, &interpValues[0], startTime, delTime, - time, vectorLength, order, &outputValue[0]); + lagrangeInterp(numTime, &interpValues[0], startTime, delTime, time, + vectorLength, order, &outputValue[0]); EXPECT_DOUBLE_EQ(outputValue[0], 45.0 / 16.0); time = 5.5; - lagrangeInterp(numTime, &interpValues[0], startTime, delTime, - time, vectorLength, order, &outputValue[0]); + lagrangeInterp(numTime, &interpValues[0], startTime, delTime, time, + vectorLength, order, &outputValue[0]); EXPECT_DOUBLE_EQ(outputValue[0], 720.0 / 16.0); } @@ -314,13 +275,13 @@ TEST(UtilitiesTests, lagrangeInterpReduced6thOrder) { int vectorLength = 1; int order = 8; - lagrangeInterp(numTime, &interpValues[0], startTime, delTime, - time, vectorLength, order, &outputValue[0]); + lagrangeInterp(numTime, &interpValues[0], startTime, delTime, time, + vectorLength, order, &outputValue[0]); EXPECT_DOUBLE_EQ(outputValue[0], 1449.0 / 256.0); time = 4.5; - lagrangeInterp(numTime, &interpValues[0], startTime, delTime, - time, vectorLength, order, &outputValue[0]); + lagrangeInterp(numTime, &interpValues[0], startTime, delTime, time, + vectorLength, order, &outputValue[0]); EXPECT_DOUBLE_EQ(outputValue[0], 5796.0 / 256.0); } @@ -334,8 +295,8 @@ TEST(UtilitiesTests, lagrangeInterp2D) { int vectorLength = 2; int order = 2; - lagrangeInterp(numTime, &interpValues[0], startTime, delTime, - time, vectorLength, order, &outputValue[0]); + lagrangeInterp(numTime, &interpValues[0], startTime, delTime, time, + vectorLength, order, &outputValue[0]); EXPECT_DOUBLE_EQ(outputValue[0], 0.5); EXPECT_DOUBLE_EQ(outputValue[1], 1.5); } @@ -355,18 +316,21 @@ TEST(UtilitiesTests, polynomialEval) { EXPECT_DOUBLE_EQ(evaluatePolynomial(coeffs, -1.0), -20.0); EXPECT_DOUBLE_EQ(evaluatePolynomialDerivative(coeffs, -1.0), 13.0); EXPECT_DOUBLE_EQ(evaluatePolynomial(coeffs, 0.0), -12.0); - EXPECT_DOUBLE_EQ(evaluatePolynomialDerivative(coeffs, 0.0), 4.0); + EXPECT_DOUBLE_EQ(evaluatePolynomialDerivative(coeffs, 0.0), 4.0); EXPECT_DOUBLE_EQ(evaluatePolynomial(coeffs, 2.0), -8.0); - EXPECT_DOUBLE_EQ(evaluatePolynomialDerivative(coeffs, 2.0), 4.0); + EXPECT_DOUBLE_EQ(evaluatePolynomialDerivative(coeffs, 2.0), 4.0); EXPECT_DOUBLE_EQ(evaluatePolynomial(coeffs, 3.5), 8.125); EXPECT_DOUBLE_EQ(evaluatePolynomialDerivative(coeffs, 3.5), 19.75); - EXPECT_THROW(evaluatePolynomial(std::vector(), 0.0), std::invalid_argument); - EXPECT_THROW(evaluatePolynomialDerivative(std::vector(), 0.0), std::invalid_argument); + EXPECT_THROW(evaluatePolynomial(std::vector(), 0.0), + std::invalid_argument); + EXPECT_THROW(evaluatePolynomialDerivative(std::vector(), 0.0), + std::invalid_argument); } TEST(UtilitiesTests, polynomialRoot) { - std::vector oneRootCoeffs = {-12.0, 4.0, -3.0, 1.0}; // roots are 3, +-2i - std::vector noRootCoeffs = {4.0, 0.0, 1.0}; // roots are +-2i + std::vector oneRootCoeffs = {-12.0, 4.0, -3.0, + 1.0}; // roots are 3, +-2i + std::vector noRootCoeffs = {4.0, 0.0, 1.0}; // roots are +-2i EXPECT_NEAR(polynomialRoot(oneRootCoeffs, 0.0), 3.0, 1e-10); EXPECT_THROW(polynomialRoot(noRootCoeffs, 0.0), std::invalid_argument); } @@ -429,7 +393,6 @@ TEST(UtilitiesTests, vectorCross) { EXPECT_DOUBLE_EQ(unitXY.y, 0.0); EXPECT_DOUBLE_EQ(unitXY.z, 1.0); - csm::EcefVector unitYX = cross(unitY, unitX); EXPECT_DOUBLE_EQ(unitYX.x, 0.0); EXPECT_DOUBLE_EQ(unitYX.y, 0.0); @@ -440,7 +403,6 @@ TEST(UtilitiesTests, vectorCross) { EXPECT_DOUBLE_EQ(unitXZ.y, -1.0); EXPECT_DOUBLE_EQ(unitXZ.z, 0.0); - csm::EcefVector unitZX = cross(unitZ, unitX); EXPECT_DOUBLE_EQ(unitZX.x, 0.0); EXPECT_DOUBLE_EQ(unitZX.y, 1.0); @@ -451,7 +413,6 @@ TEST(UtilitiesTests, vectorCross) { EXPECT_DOUBLE_EQ(unitYZ.y, 0.0); EXPECT_DOUBLE_EQ(unitYZ.z, 0.0); - csm::EcefVector unitZY = cross(unitZ, unitY); EXPECT_DOUBLE_EQ(unitZY.x, -1.0); EXPECT_DOUBLE_EQ(unitZY.y, 0.0);