Select Git revision
UsgsAstroFrameSensorModel.cpp
-
Jesse Mapel authored
* Normalized quaternion in framer * Made quaternion delta betta * Clarified comment about parameter partial delta
Jesse Mapel authored* Normalized quaternion in framer * Made quaternion delta betta * Clarified comment about parameter partial delta
UsgsAstroFrameSensorModel.cpp 50.04 KiB
#include "UsgsAstroFrameSensorModel.h"
#include <iomanip>
#include <iostream>
#include <sstream>
#include <json/json.hpp>
#include <Error.h>
#include <Version.h>
#define MESSAGE_LOG(logger, ...) if (logger) { 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 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
};
UsgsAstroFrameSensorModel::UsgsAstroFrameSensorModel() {
reset();
}
void UsgsAstroFrameSensorModel::reset() {
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<double>(NUM_PARAMETERS, 0.0);
m_currentParameterCovariance = std::vector<double>(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<double>(NUM_PARAMETERS,0.0);
m_ccdCenter = std::vector<double>(2, 0.0);
m_spacecraftVelocity = std::vector<double>(3, 0.0);
m_sunPosition = std::vector<double>(3, 0.0);
m_distortionType = DistortionType::TRANSVERSE;
m_opticalDistCoeffs = std::vector<double>(20, 0.0); m_transX = std::vector<double>(3, 0.0);
m_transY = std::vector<double>(3, 0.0);
m_iTransS = std::vector<double>(3, 0.0);
m_iTransL = std::vector<double>(3, 0.0);
m_boresight = std::vector<double>(3, 0.0);
m_parameterType = std::vector<csm::param::Type>(NUM_PARAMETERS, csm::param::REAL);
m_referencePointXyz.x = 0;
m_referencePointXyz.y = 0;
m_referencePointXyz.z = 0;
m_logFile = "";
m_logger.reset();
}
UsgsAstroFrameSensorModel::~UsgsAstroFrameSensorModel() {}
/**
* @brief UsgsAstroFrameSensorModel::groundToImage
* @param groundPt
* @param desiredPrecision
* @param achievedPrecision
* @param warnings
* @return Returns <line, sample> 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(this->m_logger, "Computing groundToImage(No adjustments) for {}, {}, {}, with desired precision {}",
groundPt.x, groundPt.y, groundPt.z, desiredPrecision);
return groundToImage(groundPt, m_noAdjustments, desiredPrecision, achievedPrecision, warnings);
}
/**
* @brief UsgsAstroFrameSensorModel::groundToImage
* @param groundPt
* @param adjustments
* @param desired_precision
* @param achieved_precision
* @param warnings
* @return Returns <line,sample> 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<double>& adjustments,
double desired_precision,
double* achieved_precision,
csm::WarningList* warnings ) const {
MESSAGE_LOG(this->m_logger, "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 f;
f = m_focalLength;
// Camera rotation matrix
double m[3][3];
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);
// 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);
return csm::ImageCoord(line, sample);
}
csm::ImageCoordCovar UsgsAstroFrameSensorModel::groundToImage(const csm::EcefCoordCovar &groundPt,
double desiredPrecision,
double *achievedPrecision,
csm::WarningList *warnings) const {
MESSAGE_LOG(this->m_logger, "Computeing 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.
return result;
}
csm::EcefCoord UsgsAstroFrameSensorModel::imageToGround(const csm::ImageCoord &imagePt,
double height,
double desiredPrecision,
double *achievedPrecision,
csm::WarningList *warnings) const {
MESSAGE_LOG(this->m_logger, "Computing imageToGround for {}, {}, {}, with desired precision {}",
imagePt.line, imagePt.samp, height, desiredPrecision);
double sample = imagePt.samp;
double line = imagePt.line;
// Here is where we should be able to apply an adjustment to opk
double m[3][3];
calcRotationMatrix(m);
MESSAGE_LOG(this->m_logger, "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;
// 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);
// Apply the distortion model (remove distortion)
double undistortedX, undistortedY;
removeDistortion(x_camera, y_camera, undistortedX, undistortedY,
m_opticalDistCoeffs,
m_distortionType);
MESSAGE_LOG(this->m_logger, "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;
MESSAGE_LOG(this->m_logger, "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(this->m_logger, "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;
losEllipsoidIntersect(height, xc, yc, zc, xl, yl, zl, x, y, z);
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(this->m_logger, "Computeing 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 result;
return result;
}
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(this->m_logger, "Computeing 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(this->m_logger, "Computeing 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);
// Distort
double undistortedFocalPlaneX, undistortedFocalPlaneY;
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<double> lookC { undistortedFocalPlaneX, undistortedFocalPlaneY, m_focalLength };
std::vector<double> 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<double> 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]);
}
csm::ImageCoord UsgsAstroFrameSensorModel::getImageStart() const {
MESSAGE_LOG(this->m_logger, "Accessing Image Start line: {}, sample: {}",
m_startingDetectorLine, m_startingDetectorSample);
csm::ImageCoord start;
start.samp = m_startingDetectorSample;
start.line = m_startingDetectorLine;
return start;
}
csm::ImageVector UsgsAstroFrameSensorModel::getImageSize() const {
MESSAGE_LOG(this->m_logger, "Accessing Image Size line: {}, sample: {}",
m_nLines, m_nSamples);
csm::ImageVector size;
size.line = m_nLines;
size.samp = m_nSamples;
return size;
}
std::pair<csm::ImageCoord, csm::ImageCoord> UsgsAstroFrameSensorModel::getValidImageRange() const {
MESSAGE_LOG(this->m_logger, "Accessing Image Range");
csm::ImageCoord min_pt(m_startingDetectorLine, m_startingDetectorSample);
csm::ImageCoord max_pt(m_nLines, m_nSamples);
return std::pair<csm::ImageCoord, csm::ImageCoord>(min_pt, max_pt);
}
std::pair<double, double> UsgsAstroFrameSensorModel::getValidHeightRange() const { MESSAGE_LOG(this->m_logger, "Accessing Image Height min: {}, max: {}",
m_minElevation, m_maxElevation);
return std::pair<double, double>(m_minElevation, m_maxElevation);
}
csm::EcefVector UsgsAstroFrameSensorModel::getIlluminationDirection(const csm::EcefCoord &groundPt) const {
// ground (body-fixed) - sun (body-fixed) gives us the illumination direction.
MESSAGE_LOG(this->m_logger, "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]
};
}
double UsgsAstroFrameSensorModel::getImageTime(const csm::ImageCoord &imagePt) const {
MESSAGE_LOG(this->m_logger, "Accessing image time 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) &&
imagePt.line >= m_startingDetectorSample &&
imagePt.line <= (m_startingDetectorLine + m_nLines)) {
return m_ephemerisTime;
}
else {
throw csm::Error(csm::Error::BOUNDS,
"Image Coordinate out of Bounds",
"UsgsAstroFrameSensorModel::getImageTime");
}
}
csm::EcefCoord UsgsAstroFrameSensorModel::getSensorPosition(const csm::ImageCoord &imagePt) const {
MESSAGE_LOG(this->m_logger, "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) &&
imagePt.line >= m_startingDetectorSample &&
imagePt.line <= (m_startingDetectorLine + m_nLines)) {
csm::EcefCoord sensorPosition;
sensorPosition.x = m_currentParameterValue[0];
sensorPosition.y = m_currentParameterValue[1];
sensorPosition.z = m_currentParameterValue[2];
return sensorPosition;
}
else {
throw csm::Error(csm::Error::BOUNDS,
"Image Coordinate out of Bounds",
"UsgsAstroFrameSensorModel::getSensorPosition");
}
}
csm::EcefCoord UsgsAstroFrameSensorModel::getSensorPosition(double time) const {
MESSAGE_LOG(this->m_logger, "Accessing sensor position for time {}", time);
if (time == m_ephemerisTime){
csm::EcefCoord sensorPosition;
sensorPosition.x = m_currentParameterValue[0];
sensorPosition.y = m_currentParameterValue[1];
sensorPosition.z = m_currentParameterValue[2];
return sensorPosition;
} else {
std::string aMessage = "Valid image time is %d", m_ephemerisTime; throw csm::Error(csm::Error::BOUNDS,
aMessage,
"UsgsAstroFrameSensorModel::getSensorPosition");
}
}
csm::EcefVector UsgsAstroFrameSensorModel::getSensorVelocity(const csm::ImageCoord &imagePt) const {
MESSAGE_LOG(this->m_logger, "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) {
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(this->m_logger, "Accessing sensor position for time {}", time);
if (time == m_ephemerisTime){
return csm::EcefVector {
m_spacecraftVelocity[0],
m_spacecraftVelocity[1],
m_spacecraftVelocity[2]
};
} else {
std::string aMessage = "Valid image time is %d", m_ephemerisTime;
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(this->m_logger, "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);
return computeSensorPartials(index, img_pt, groundPt, desiredPrecision, achievedPrecision);
}
/**
* @brief UsgsAstroFrameSensorModel::computeSensorPartials
* @param index
* @param imagePt
* @param groundPt
* @param desiredPrecision
* @param achievedPrecision
* @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.
* */
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(this->m_logger, "Computing sensor partials for ground point {}, {}, {}\
with point: {}, {}, index: {}, and 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.
// Rotation parameters are indices 3-6
if (index > 2 && index < 7) {
delta = 0.01;
}
// Update the parameter
std::vector<double>adj(NUM_PARAMETERS, 0.0);
adj[index] = delta;
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;
return partials;
}
std::vector<csm::RasterGM::SensorPartials> UsgsAstroFrameSensorModel::computeAllSensorPartials(
const csm::ImageCoord& imagePt,
const csm::EcefCoord& groundPt,
csm::param::Set pset,
double desiredPrecision,
double *achievedPrecision,
csm::WarningList *warnings) const {
MESSAGE_LOG(this->m_logger, "Computing all sensor partials for ground point {}, {}, {}\
with point: {}, {}, pset: {}, and desiredPrecision: {}",
groundPt.x, groundPt.y, groundPt.z, imagePt.line, imagePt.samp,
pset, desiredPrecision);
std::vector<int> indices = getParameterSetIndices(pset);
size_t num = indices.size();
std::vector<csm::RasterGM::SensorPartials> partials;
for (int index = 0;index < num;index++){
partials.push_back(computeSensorPartials(
indices[index],
imagePt, groundPt,
desiredPrecision, achievedPrecision, warnings));
}
return partials;
}
std::vector<csm::RasterGM::SensorPartials> UsgsAstroFrameSensorModel::computeAllSensorPartials(
const csm::EcefCoord& groundPt,
csm::param::Set pset,
double desiredPrecision,
double *achievedPrecision,
csm::WarningList *warnings) const {
MESSAGE_LOG(this->m_logger, "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<double> UsgsAstroFrameSensorModel::computeGroundPartials(const csm::EcefCoord
&groundPt) const {
MESSAGE_LOG(this->m_logger, "Computing ground partials for ground point {}, {}, {}",
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));
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);
// If the ground size is too small, try the opposite direction
if (pixelGroundSize < 1e-10) {
nextPoint = imageToGround(csm::ImageCoord(ipB.line - 1, ipB.samp - 1));
dx = nextPoint.x - x;
dy = nextPoint.y - y;
dz = nextPoint.z - z;
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));
std::vector<double> partials(6, 0.0);
partials[0] = (ipX.line - ipB.line) / pixelGroundSize;
partials[3] = (ipX.samp - ipB.samp) / pixelGroundSize;
partials[1] = (ipY.line - ipB.line) / pixelGroundSize;
partials[4] = (ipY.samp - ipB.samp) / pixelGroundSize;
partials[2] = (ipZ.line - ipB.line) / pixelGroundSize;
partials[5] = (ipZ.samp - ipB.samp) / pixelGroundSize;
MESSAGE_LOG(this->m_logger, "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 {
MESSAGE_LOG(this->m_logger, "Accessing correlation model");
return _no_corr_model;
}
std::vector<double> UsgsAstroFrameSensorModel::getUnmodeledCrossCovariance(const csm::ImageCoord &pt1,
const csm::ImageCoord &pt2) const {
// No unmodeled error
MESSAGE_LOG(this->m_logger, "Accessing unmodeled cross covar with \
point1: {}, {} and point2: {}, {}",
pt1.line, pt1.samp, pt2.line, pt2.samp);
return std::vector<double>(4, 0.0);
}
csm::Version UsgsAstroFrameSensorModel::getVersion() const {
MESSAGE_LOG(this->m_logger, "Accessing CSM version");
return csm::Version(0,1,0);
}
std::string UsgsAstroFrameSensorModel::getModelName() const {
MESSAGE_LOG(this->m_logger, "Accessing CSM name {}", _SENSOR_MODEL_NAME); return _SENSOR_MODEL_NAME;
}
std::string UsgsAstroFrameSensorModel::getPedigree() const {
MESSAGE_LOG(this->m_logger, "Accessing CSM pedigree");
return "USGS_FRAMER";
}
std::string UsgsAstroFrameSensorModel::getImageIdentifier() const {
MESSAGE_LOG(this->m_logger, "Accessing image ID {}", m_imageIdentifier);
return m_imageIdentifier;
}
void UsgsAstroFrameSensorModel::setImageIdentifier(const std::string& imageId,
csm::WarningList* warnings) {
MESSAGE_LOG(this->m_logger, "Setting image ID to {}", imageId);
m_imageIdentifier = imageId;
}
std::string UsgsAstroFrameSensorModel::getSensorIdentifier() const {
MESSAGE_LOG(this->m_logger, "Accessing sensor ID");
return m_sensorName;
}
std::string UsgsAstroFrameSensorModel::getPlatformIdentifier() const {
MESSAGE_LOG(this->m_logger, "Accessing platform ID");
return m_platformName;
}
std::string UsgsAstroFrameSensorModel::getCollectionIdentifier() const {
MESSAGE_LOG(this->m_logger, "Accessing collection ID");
return m_collectionIdentifier;
}
std::string UsgsAstroFrameSensorModel::getTrajectoryIdentifier() const {
MESSAGE_LOG(this->m_logger, "Accessing trajectory ID");
return "";
}
std::string UsgsAstroFrameSensorModel::getSensorType() const {
MESSAGE_LOG(this->m_logger, "Accessing sensor type");
return CSM_SENSOR_TYPE_EO;
}
std::string UsgsAstroFrameSensorModel::getSensorMode() const {
MESSAGE_LOG(this->m_logger, "Accessing sensor mode");
return CSM_SENSOR_MODE_FRAME;
}
std::string UsgsAstroFrameSensorModel::getReferenceDateAndTime() const {
MESSAGE_LOG(this->m_logger, "Accessing reference data and time");
return "";
}
std::string UsgsAstroFrameSensorModel::getModelState() const {
MESSAGE_LOG(this->m_logger, "Dumping model state");
json state = {
{"m_modelName", _SENSOR_MODEL_NAME},
{"m_sensorName", m_sensorName}, {"m_platformName", m_platformName},
{"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]}},
{"m_transX", {m_transX[0], m_transX[1], m_transX[2]}},
{"m_transY", {m_transY[0], m_transY[1], m_transY[2]}},
{"m_iTransS", {m_iTransS[0], m_iTransS[1], m_iTransS[2]}},
{"m_iTransL", {m_iTransL[0], m_iTransL[1], m_iTransL[2]}},
{"m_majorAxis", m_majorAxis},
{"m_minorAxis", m_minorAxis},
{"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},
{"m_detectorSampleSumming", m_detectorSampleSumming},
{"m_detectorLineSumming", m_detectorLineSumming},
{"m_targetName", m_targetName},
{"m_ifov", m_ifov},
{"m_instrumentID", m_instrumentID},
{"m_focalLengthEpsilon", m_focalLengthEpsilon},
{"m_ccdCenter", {m_ccdCenter[0], m_ccdCenter[1]}},
{"m_minElevation", m_minElevation},
{"m_maxElevation", m_maxElevation},
{"m_distortionType", m_distortionType},
{"m_opticalDistCoeffs", m_opticalDistCoeffs},
{"m_originalHalfLines", m_originalHalfLines},
{"m_originalHalfSamples", m_originalHalfSamples},
{"m_spacecraftName", m_spacecraftName},
{"m_pixelPitch", m_pixelPitch},
{"m_ephemerisTime", m_ephemerisTime},
{"m_nLines", m_nLines},
{"m_nSamples", m_nSamples},
{"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_logFile", m_logFile}
};
return state.dump();
}
bool UsgsAstroFrameSensorModel::isValidModelState(const std::string& stringState, csm::WarningList *warnings) {
MESSAGE_LOG(this->m_logger, "Checking if model has valid state");
std::vector<std::string> 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<std::string> missingKeywords;
for (auto &key : requiredKeywords) {
if (jsonState.find(key) == jsonState.end()) {
missingKeywords.push_back(key);
}
}
if (!missingKeywords.empty() && warnings) {
std::ostringstream oss;
std::copy(missingKeywords.begin(), missingKeywords.end(), std::ostream_iterator<std::string>(oss, " "));
warnings->push_back(csm::Warning(
csm::Warning::DATA_NOT_AVAILABLE,
"State has missing keywrods: " + oss.str(),
"UsgsAstroFrameSensorModel::isValidModelState"
));
}
std::string modelName = jsonState.value<std::string>("m_modelName", "");
if (modelName != _SENSOR_MODEL_NAME && warnings) {
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) {
// 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(this->m_logger, "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(this->m_logger, "Replaceing model state");
// The json library's .at() will except if key is missing
try {
m_modelName = state.at("m_modelName").get<std::string>();
m_majorAxis = state.at("m_majorAxis").get<double>();
m_minorAxis = state.at("m_minorAxis").get<double>();
m_focalLength = state.at("m_focalLength").get<double>();
m_startingDetectorSample = state.at("m_startingDetectorSample").get<double>();
m_startingDetectorLine = state.at("m_startingDetectorLine").get<double>();
m_detectorSampleSumming = state.at("m_detectorSampleSumming").get<double>();
m_detectorLineSumming = state.at("m_detectorLineSumming").get<double>();
m_focalLengthEpsilon = state.at("m_focalLengthEpsilon").get<double>();
m_nLines = state.at("m_nLines").get<int>();
m_nSamples = state.at("m_nSamples").get<int>();
m_currentParameterValue = state.at("m_currentParameterValue").get<std::vector<double>>();
m_ccdCenter = state.at("m_ccdCenter").get<std::vector<double>>(); m_spacecraftVelocity = state.at("m_spacecraftVelocity").get<std::vector<double>>();
m_sunPosition = state.at("m_sunPosition").get<std::vector<double>>();
m_distortionType = (DistortionType)state.at("m_distortionType").get<int>();
m_opticalDistCoeffs = state.at("m_opticalDistCoeffs").get<std::vector<double>>();
m_transX = state.at("m_transX").get<std::vector<double>>();
m_transY = state.at("m_transY").get<std::vector<double>>();
m_iTransS = state.at("m_iTransS").get<std::vector<double>>();
m_iTransL = state.at("m_iTransL").get<std::vector<double>>();
m_imageIdentifier = state.at("m_imageIdentifier").get<std::string>();
m_platformName = state.at("m_platformName").get<std::string>();
m_sensorName = state.at("m_sensorName").get<std::string>();
m_collectionIdentifier = state.at("m_collectionIdentifier").get<std::string>();
// 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<std::vector<double>>();
m_logFile = state.at("m_logFile").get<std::string>();
if (m_logFile.empty()) {
m_logger.reset();
}
else {
m_logger = spdlog::get(m_logFile);
if (!m_logger) {
m_logger = spdlog::basic_logger_mt(m_logFile, m_logFile);
}
}
}
catch(std::out_of_range& e) {
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(this->m_logger, "Constructing state from isd");
json isd = json::parse(jsonIsd);
json state = {};
csm::WarningList* parsingWarnings = new csm::WarningList;
state["m_modelName"] = getSensorModelName(isd, parsingWarnings);
state["m_imageIdentifier"] = getImageId(isd, parsingWarnings);
state["m_sensorName"] = getSensorName(isd, parsingWarnings);
state["m_platformName"] = getPlatformName(isd, parsingWarnings);
state["m_startingDetectorSample"] = getDetectorStartingSample(isd, parsingWarnings);
state["m_startingDetectorLine"] = getDetectorStartingLine(isd, parsingWarnings);
state["m_detectorSampleSumming"] = getSampleSumming(isd, parsingWarnings);
state["m_detectorLineSumming"] = getLineSumming(isd, parsingWarnings);
// get focal length
state["m_focalLength"] = getFocalLength(isd, parsingWarnings);
state["m_focalLengthEpsilon"] = getFocalLengthEpsilon(isd);
state["m_currentParameterValue"] = json();
// get sensor_position
std::vector<double> position = getSensorPositions(isd, parsingWarnings);
if (!position.empty() && position.size() != 3) {
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"] = position;
}
// get sensor_velocity
std::vector<double> velocity = getSensorVelocities(isd, parsingWarnings);
if (!velocity.empty() && velocity.size() != 3) {
parsingWarnings->push_back(
csm::Warning(
csm::Warning::DATA_NOT_AVAILABLE,
"Sensor velocity does not have 3 values.",
"UsgsAstroFrameSensorModel::constructStateFromIsd()"));
}
else {
state["m_spacecraftVelocity"] = velocity;
}
// get sun_position
// sun position is not strictly necessary, but is required for getIlluminationDirection.
state["m_sunPosition"] = getSunPositions(isd);
// get sensor_orientation quaternion
std::vector<double> quaternion = getSensorOrientations(isd, parsingWarnings);
if (quaternion.size() != 4) {
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] = quaternion[0];
state["m_currentParameterValue"][4] = quaternion[1];
state["m_currentParameterValue"][5] = quaternion[2];
state["m_currentParameterValue"][6] = quaternion[3];
}
// get optical_distortion
state["m_distortionType"] = getDistortionModel(isd, warnings);
state["m_opticalDistCoeffs"] = getDistortionCoeffs(isd, warnings);
// get detector_center
state["m_ccdCenter"][0] = getDetectorCenterLine(isd, parsingWarnings);
state["m_ccdCenter"][1] = getDetectorCenterSample(isd, parsingWarnings);
// get radii
state["m_minorAxis"] = getSemiMinorRadius(isd, parsingWarnings);
state["m_majorAxis"] = getSemiMajorRadius(isd, parsingWarnings);
// get reference_height
state["m_minElevation"] = getMinHeight(isd, parsingWarnings);
state["m_maxElevation"] = getMaxHeight(isd, parsingWarnings);
state["m_ephemerisTime"] = getCenterTime(isd, parsingWarnings);
state["m_nLines"] = getTotalLines(isd, parsingWarnings);
state["m_nSamples"] = getTotalSamples(isd, parsingWarnings);
state["m_iTransL"] = getFocal2PixelLines(isd, parsingWarnings);
state["m_iTransS"] = getFocal2PixelSamples(isd, parsingWarnings);
// 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<double>() * state["m_iTransS"][2].get<double>() -
state["m_iTransL"][2].get<double>() * state["m_iTransS"][1].get<double>();
state["m_transX"][1] = state["m_iTransL"][1].get<double>() / determinant;
state["m_transX"][2] = -state["m_iTransS"][1].get<double>() / determinant;
state["m_transX"][0] = -(state["m_transX"][1].get<double>() * state["m_iTransL"][0].get<double>() +
state["m_transX"][2].get<double>() * state["m_iTransS"][0].get<double>());
state["m_transY"][1] = -state["m_iTransL"][2].get<double>() / determinant;
state["m_transY"][2] = state["m_iTransS"][2].get<double>() / determinant;
state["m_transY"][0] = -(state["m_transY"][1].get<double>() * state["m_iTransL"][0].get<double>() +
state["m_transY"][2].get<double>() * state["m_iTransS"][0].get<double>());
}
catch (...) {
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<double>(3, 0.0);
state["m_currentParameterCovariance"] = std::vector<double>(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"] = "";
// Get the optional logging file
state["m_logFile"] = getLogFile(isd);
if (!parsingWarnings->empty()) {
if (warnings) {
warnings->insert(warnings->end(), parsingWarnings->begin(), parsingWarnings->end());
}
delete parsingWarnings;
parsingWarnings = nullptr;
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(this->m_logger, "Accessing reference point x: {}, y: {}, z: {}",
m_referencePointXyz.x, m_referencePointXyz.y, m_referencePointXyz.z);
return m_referencePointXyz;
}
void UsgsAstroFrameSensorModel::setReferencePoint(const csm::EcefCoord &groundPt) {
MESSAGE_LOG(this->m_logger, "Setting reference point to {}, {}, {}",
groundPt.x, groundPt.y, groundPt.z);
m_referencePointXyz = groundPt;
}
int UsgsAstroFrameSensorModel::getNumParameters() const {
MESSAGE_LOG(this->m_logger, "Accessing num parameters: {}", NUM_PARAMETERS);
return NUM_PARAMETERS;
}
std::string UsgsAstroFrameSensorModel::getParameterName(int index) const { MESSAGE_LOG(this->m_logger, "Setting parameter name to {}", index);
return m_parameterName[index];
}
std::string UsgsAstroFrameSensorModel::getParameterUnits(int index) const {
MESSAGE_LOG(this->m_logger, "Accessing parameter units for {}", index);
if (index < 3) {
return "m";
}
else {
return "radians";
}
}
bool UsgsAstroFrameSensorModel::hasShareableParameters() const {
MESSAGE_LOG(this->m_logger, "Checking for shareable parameters");
return false;
}
bool UsgsAstroFrameSensorModel::isParameterShareable(int index) const {
MESSAGE_LOG(this->m_logger, "Checking is parameter: {} is shareable", index);
return false;
}
csm::SharingCriteria UsgsAstroFrameSensorModel::getParameterSharingCriteria(int index) const {
MESSAGE_LOG(this->m_logger, "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(this->m_logger, "Accessing parameter value {} at index: {}", m_currentParameterValue[index], index);
return m_currentParameterValue[index];
}
void UsgsAstroFrameSensorModel::setParameterValue(int index, double value) {
MESSAGE_LOG(this->m_logger, "Setting parameter value: {} at index: {}", value, index);
m_currentParameterValue[index] = value;
}
csm::param::Type UsgsAstroFrameSensorModel::getParameterType(int index) const {
MESSAGE_LOG(this->m_logger, "Accessing parameter type: {} at index: {}", m_parameterType[index], index);
return m_parameterType[index];
}
void UsgsAstroFrameSensorModel::setParameterType(int index, csm::param::Type pType) {
MESSAGE_LOG(this->m_logger, "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(this->m_logger, "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(this->m_logger, "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(this->m_logger, "Accessing num geom correction switches");
return 0;
}
std::string UsgsAstroFrameSensorModel::getGeometricCorrectionName(int index) const {
MESSAGE_LOG(this->m_logger, "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(this->m_logger, "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(this->m_logger, "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<double> UsgsAstroFrameSensorModel::getCrossCovarianceMatrix(
const GeometricModel &comparisonModel,
csm::param::Set pSet,
const GeometricModelList &otherModels) const {
MESSAGE_LOG(this->m_logger, "Accessing cross covariance matrix");
// No correlation between models.
const std::vector<int>& indices = getParameterSetIndices(pSet);
size_t num_rows = indices.size();
const std::vector<int>& indices2 = comparisonModel.getParameterSetIndices(pSet);
size_t num_cols = indices.size();
return std::vector<double>(num_rows * num_cols, 0.0);
}
csm::Ellipsoid UsgsAstroFrameSensorModel::getEllipsoid() const {
MESSAGE_LOG(this->m_logger, "Accessing ellipsoid radii {} {}",
m_majorAxis, m_minorAxis);
return csm::Ellipsoid(m_majorAxis, m_minorAxis);
}
void UsgsAstroFrameSensorModel::setEllipsoid(const csm::Ellipsoid &ellipsoid) {
MESSAGE_LOG(this->m_logger, "Setting ellipsoid radii {} {}",
ellipsoid.getSemiMajorRadius(), ellipsoid.getSemiMinorRadius());
m_majorAxis = ellipsoid.getSemiMajorRadius();
m_minorAxis = ellipsoid.getSemiMinorRadius();
}
void UsgsAstroFrameSensorModel::calcRotationMatrix(
double m[3][3]) const {
MESSAGE_LOG(this->m_logger, "Calculating rotation matrix");
// Trigonometric functions for rotation matrix
double x = m_currentParameterValue[3];
double y = m_currentParameterValue[4];
double z = m_currentParameterValue[5];
double w = m_currentParameterValue[6];
double norm = sqrt(x * x + y * y + z * z + w * w);
x /= norm;
y /= norm;
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;
}
void UsgsAstroFrameSensorModel::calcRotationMatrix(
double m[3][3], const std::vector<double> &adjustments) const {
MESSAGE_LOG(this->m_logger, "Calculating rotation matrix with adjustments");
// Trigonometric functions for rotation matrix
double x = getValue(3, adjustments);
double y = getValue(4, adjustments);
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;
}
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(this->m_logger, "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
x = xc + scale * xl;
y = yc + scale * yl;
z = zc + scale * zl;
MESSAGE_LOG(this->m_logger, "Calculated losEllipsoidIntersect at: {}, {}, {}",
x, y, z);
}
/***** Helper Functions *****/
double UsgsAstroFrameSensorModel::getValue(
int index,
const std::vector<double> &adjustments) const
{
MESSAGE_LOG(this->m_logger, "Accessing value: {} at index: {}, with adjustments", m_currentParameterValue[index] + adjustments[index], index);
return m_currentParameterValue[index] + adjustments[index];
}
std::shared_ptr<spdlog::logger> UsgsAstroFrameSensorModel::getLogger() {
return m_logger;
}
void UsgsAstroFrameSensorModel::setLogger(std::shared_ptr<spdlog::logger> logger) {
m_logger = logger;
}