/** Set ephemeris time for the high pass filtered rotation
* from the instrument frame to the true (corrected) instrument
* frame. [TC] = [angle2 - Pangle2(t)] [angle1 - Pangle1(t)]
* 2 1
* where t = (time - p_baseTime) / p_timeScale, and n = p_degree.
*
* @param [in] et Ephemeris time
*
*/
std::vector<double> PixelOffset::SetEphemerisTimeHPF ( const double et ) {
Isis::PolynomialUnivariate function1( p_degree );
Isis::PolynomialUnivariate function2( p_degree );
//If outside the range of the offsets just return the identity matrix
if (et < p_times[0] || et > p_times[p_times.size()-1]) {
return (p_I);
}
// Load the functions with the coefficients
function1.SetCoefficients ( p_ang1Coefficients );
function2.SetCoefficients ( p_ang2Coefficients );
// Compute polynomial approximations to angles, pangle1 and pangle2
std::vector<double> rtime;
rtime.push_back((et - p_baseTime) / p_timeScale);
double pangle1 = p_sampOff + p_sampScale*function1.Evaluate (rtime);
double pangle2 = p_lineOff + p_lineScale*function2.Evaluate (rtime);
// Compute p_angles for this time
SetEphemerisTime(et);
double angle1 = p_angleScale*(p_angle1 - pangle1);
double angle2 = p_angleScale*(p_angle2 - pangle2);
// std::cout<<" "<<p_angle1*p_angleScale<<" "<<pangle1*p_angleScale<<" "<<angle1<<" "<<et<<std::endl;
// std::cout<<" "<<p_angle2*p_angleScale<<" "<<pangle2*p_angleScale<<" "<<angle2<<" "<<et<<std::endl;
std::vector<double> TC(9);
eul2m_c ( (SpiceDouble) 0., (SpiceDouble) angle2, (SpiceDouble) angle1,
3, 2, 1,
(SpiceDouble(*) [3]) &TC[0]);
return (TC);
}
/** Cache J2000 rotation over existing cached time range using polynomials
*
* This method will reload an internal cache with matrices
* formed from rotation angles fit to polynomials over a time
* range.
*
* @param function1 The first polynomial function used to
* find the rotation angles
* @param function2 The second polynomial function used to
* find the rotation angles
* @param function3 The third polynomial function used to
* find the rotation angles
*/
void LineScanCameraRotation::ReloadCache() {
NaifStatus::CheckErrors();
// Make sure caches are already loaded
if(!p_cachesLoaded) {
QString msg = "A LineScanCameraRotation cache has not been loaded yet";
throw IException(IException::Programmer, msg, _FILEINFO_);
}
// Clear existing matrices from cache
p_cache.clear();
// Create polynomials fit to angles & use to reload cache
Isis::PolynomialUnivariate function1(p_degree);
Isis::PolynomialUnivariate function2(p_degree);
Isis::PolynomialUnivariate function3(p_degree);
// Get the coefficients of the polynomials already fit to the angles of rotation defining [CI]
std::vector<double> coeffAng1;
std::vector<double> coeffAng2;
std::vector<double> coeffAng3;
GetPolynomial(coeffAng1, coeffAng2, coeffAng3);
// Reset linear term to center around zero -- what works best is either roll-avg & pitchavg+ or pitchavg+ & yawavg-
// coeffAng1[1] -= 0.0000158661225;
// coeffAng2[1] = 0.0000308433;
// coeffAng3[0] = -0.001517547;
if(p_pitchRate) coeffAng2[1] = p_pitchRate;
if(p_yaw) coeffAng3[0] = p_yaw;
// Load the functions with the coefficients
function1.SetCoefficients(coeffAng1);
function2.SetCoefficients(coeffAng2);
function3.SetCoefficients(coeffAng3);
double CI[3][3];
double IJ[3][3];
std::vector<double> rtime;
SpiceRotation *prot = p_spi->bodyRotation();
std::vector<double> CJ;
CJ.resize(9);
for(std::vector<double>::size_type pos = 0; pos < p_cacheTime.size(); pos++) {
double et = p_cacheTime.at(pos);
rtime.push_back((et - GetBaseTime()) / GetTimeScale());
double angle1 = function1.Evaluate(rtime);
double angle2 = function2.Evaluate(rtime);
double angle3 = function3.Evaluate(rtime);
rtime.clear();
// Get the first angle back into the range Naif expects [180.,180.]
if(angle1 < -1 * pi_c()) {
angle1 += twopi_c();
}
else if(angle1 > pi_c()) {
angle1 -= twopi_c();
}
eul2m_c((SpiceDouble) angle3, (SpiceDouble) angle2, (SpiceDouble) angle1,
p_axis3, p_axis2, p_axis1,
CI);
mxm_c((SpiceDouble( *)[3]) & (p_jitter->SetEphemerisTimeHPF(et))[0], CI, CI);
prot->SetEphemerisTime(et);
mxm_c((SpiceDouble( *)[3]) & (p_cacheIB.at(pos))[0], (SpiceDouble( *)[3]) & (prot->Matrix())[0], IJ);
mxm_c(CI, IJ, (SpiceDouble( *)[3]) &CJ[0]);
p_cache.push_back(CJ); // J2000 to constant frame
}
// Set source to cache to get updated values
SetSource(SpiceRotation::Memcache);
// Make sure SetEphemerisTime updates the matrix by resetting it twice (in case the first one
// matches the current et. p_et is private and not available from the child class
NaifStatus::CheckErrors();
SetEphemerisTime(p_cacheTime[0]);
SetEphemerisTime(p_cacheTime[1]);
NaifStatus::CheckErrors();
}
