Moved fit check methods into SpicePosition and SpiceRotation. Increased the...

#include "Cube.h"

#include "FileList.h"

#include "FileName.h"

#include "Histogram.h"

#include "IException.h"

#include "PiecewisePolynomial.h"

#include "Progress.h"

  // Fit the position

  instPosition->SetPolynomialDegree(positionDegree);

  instPosition->setPolynomialSegments(positionSegments);

  PiecewisePolynomial positionPoly = instPosition->testFit();

  PiecewisePolynomial positionPoly = instPosition->fitPolynomial(positionDegree, positionSegments);

  // Fit the rotation

  instRotation->SetPolynomialDegree(pointingDegree);

  instRotation->setPolynomialSegments(pointingSegments);

  PiecewisePolynomial rotationPoly = instRotation->testFit();

  PiecewisePolynomial rotationPoly = instRotation->fitPolynomial(pointingDegree, pointingSegments);

  // Compute the position RMS

  double sumSquaredPositionError = 0;

  double positionBaseTime = instPosition->GetBaseTime();

  double positionTimeScale = instPosition->GetTimeScale();

  std::vector<double> positionSampleTimes = instPosition->timeCache();

  int positionSampleCount = positionSampleTimes.size();

  for (int i = 0; i < positionSampleCount; i++) {

    double error = 0;

    double scaledTime = (positionSampleTimes[i] - positionBaseTime) / positionTimeScale;

    std::vector<double> measuredCoord = instPosition->SetEphemerisTime(positionSampleTimes[i]);

    std::vector<double> estimatedCoord = positionPoly.evaluate(scaledTime);

    error += (measuredCoord[0] - estimatedCoord[0]) * (measuredCoord[0] - estimatedCoord[0]);

    error += (measuredCoord[1] - estimatedCoord[1]) * (measuredCoord[1] - estimatedCoord[1]);

    error += (measuredCoord[2] - estimatedCoord[2]) * (measuredCoord[2] - estimatedCoord[2]);

    sumSquaredPositionError += sqrt(error);

  }

  double positionRMS = sqrt(sumSquaredPositionError / positionSampleCount);

  Histogram positionHist = instPosition->computeError(positionPoly);

  double positionRMS = positionHist.Rms();

  // Compute the rotation RMS

  double sumSquaredRotationError = 0;

  double rotationBaseTime = instRotation->GetBaseTime();

  double rotationTimeScale = instRotation->GetTimeScale();

  std::vector<double> rotationSampleTimes = instRotation->timeCache();

  int rotationSampleCount = rotationSampleTimes.size();

  double start1 = 0.; // value of 1st angle1 in cache

  double start3 = 0.; // value of 1st angle3 in cache

  for (int i = 0; i < rotationSampleCount; i++) {

    double error = 0;

    double scaledTime = (rotationSampleTimes[i] - rotationBaseTime) / rotationTimeScale;

    instRotation->SetEphemerisTime(rotationSampleTimes[i]);

    std::vector<double> measuredAngles = instRotation->Angles(3, 1, 3);

    // Fix the angles crossing the domain bound

    if (i == 0) {

      start1 = measuredAngles[0];

      start3 = measuredAngles[2];

    }

    else {

      measuredAngles[0] = instRotation->WrapAngle(start1, measuredAngles[0]);

      measuredAngles[2] = instRotation->WrapAngle(start3, measuredAngles[2]);

    }

    std::vector<double> estimatedAngles = rotationPoly.evaluate(scaledTime);

    error += (measuredAngles[0] - estimatedAngles[0]) * (measuredAngles[0] - estimatedAngles[0]);

    error += (measuredAngles[1] - estimatedAngles[1]) * (measuredAngles[1] - estimatedAngles[1]);

    error += (measuredAngles[2] - estimatedAngles[2]) * (measuredAngles[2] - estimatedAngles[2]);

    sumSquaredRotationError += sqrt(error);

  }

  double rotationRMS = sqrt(sumSquaredRotationError / rotationSampleCount);

  Histogram rotationHist = instRotation->computeError(rotationPoly);

  double rotationRMS = rotationHist.Rms();

  return QPair<double, double>(positionRMS, rotationRMS);

}

    This program takes a list of cubes and tests the quality of the polynomial

    fit over instrument position and pointing values. This emulates how the

    jigsaw app converts exterior orientation into polynomials prior to

    adjustment. Poor quality fits can result in jigsaw failing.

    adjustment. This app can be used to help determine jigsaw parameters that

    will work well with a data set.

  </description>

  <category>

  <seeAlso>

    <applications>

      <item>rotate</item>

      <item>flip</item>

      <item>jigsaw</item>

    </applications>

  </seeAlso>

    <change name="Jesse Mapel" date="2017-07-17">

      Original version

    </change>

  </history>

  <groups>

          List of input cubes to check

        </brief>

        <description>

          The list of input cubes that a test polynomial fit will be done for.

          The list of input cubes that test polynomial fits will be done for.

          Each cube will have polnomials fit over its instrument position and

          instrument pointing.

        </description>

        <filter>

          *.lis

          Output file that contains the quality of fits

        </brief>

        <description>

          This text file will contain the quality of fit for each input cube

          sorted from best fit to worst fit. The first column is the cube name,

          the second column is the quality of the instrument position fit, and

          the third column is the quality of the instrument pointing fit.

          This text file will contain the quality of fit for each input cube.

          The first column is the cube name, the second column is the RMS error

          of the instrument position fit in kilometers, and the third column is

          the RMS error of the instrument pointing fit in radians. Any cubes

          for which the process fails will not be included in this output file.

        </description>

        <filter>

          *.txt, *.csv

      <parameter name="SPKDEGREE">

        <type>integer</type>

        <brief>

          The degree of the instrument position polynomial fit.

          The degree of the instrument position polynomial fits.

        </brief>

        <description>

          The degree of the instrument position polynomial fit.

          The degree of the instrument position polynomial fits. This

          corresponds to the SPKSOLVEDEGREE parameter in the jigsaw

          application. It is recommended that a fit no higher than 2nd degree

          is used. High degree polynomial fits often result in overfitting with

          minimal gain. Increasing the number of segments with lower degree

          polynomial fits is suggested when working with highly volatile data.

        </description>

        <minimum inclusive="yes">0</minimum>

        <default>

          <item>2</item>

        </default>

      </parameter>

      <parameter name="SPKSEGMENTS">

        <type>integer</type>

        <brief>

          The number of polynomial segments used in the instrument position fit.

          The number of polynomial segments used in the instrument position

          fits.

        </brief>

        <description>

          The number of polynomial segments used in the instrument position fit.

          The number of polynomial segments used in the instrument position

          fits. Increasing the number of segments with lower degree polynomial

          fits is suggested when working with highly volatile data.

        </description>

        <minimum inclusive="yes">1</minimum>

        <default>

          <item>1</item>

        </default>

      </parameter>

      <parameter name="CKDEGREE">

          The degree of the instrument pointing polynomial fit.

        </brief>

        <description>

          The degree of the instrument pointing polynomial fit.

          The degree of the instrument pointing polynomial fit. This

          corresponds to the CKSOLVEDEGREE parameter in the jigsaw

          application. It is recommended that a fit no higher than 2nd degree

          is used. High degree polynomial fits often result in overfitting with

          minimal gain. Increasing the number of segments with lower degree

          polynomial fits is suggested when working with highly volatile data.

        </description>

        <minimum inclusive="yes">0</minimum>

        <default>

          <item>2</item>

        </default>

      </parameter>

      <parameter name="CKSEGMENTS">

        <type>integer</type>

        <brief>

          The number of polynomial segments used in the instrument pointing fit.

          The number of polynomial segments used in the instrument pointing

          fit.

        </brief>

        <description>

          The number of polynomial segments used in the instrument pointing fit.

          The number of polynomial segments used in the instrument pointing

          fit. Increasing the number of segments with lower degree polynomial

          fits is suggested when working with highly volatile data.

        </description>

        <minimum inclusive="yes">1</minimum>

        <default>

          <item>1</item>

        </default>

      </parameter>

    </group>

  </groups>

  /**

   * Returns the time cache values.

   * Returns the time cache values. If the cache is reduced, this will return

   * the original unreduced time cache.

   */

  std::vector<double> SpicePosition::timeCache() const {

    return (p_cacheTime);

    if ((int)p_fullCacheSize != (int)p_cacheTime.size()) {

      std::vector<double> fullTimeCache;

      fullTimeCache.resize(p_fullCacheSize);

      double sampleRate = (p_fullCacheEndTime - p_fullCacheStartTime) / p_fullCacheSize;

      for (int i = 0; i < (int)p_fullCacheSize; i++) {

        fullTimeCache[i] = p_fullCacheStartTime + sampleRate * i;

      }

      return fullTimeCache;

    }

    return p_cacheTime;

  }

      throw IException(IException::Programmer, msg, _FILEINFO_);

    }

    // Adjust the degree of the polynomial to the available data

    if ( p_degree > (int)p_cache.size() - 1 ) {

      p_degree = p_cache.size() - 1;

      // Flag if velocity is available

      p_hasVelocity = ( p_degree > 0 || !p_cacheVelocity.empty() );

    }

    // Recompute the time scaling

    ComputeBaseTime();

    // If fitting a polynomial over hermite data, then do not fit the polynomial.

    // Create a zero polynomial, set the knots and move on.

    if ( type == PolyFunctionOverHermiteConstant ) {

      // Compute first and last times in scaled time.

      //   If there is only one cached time, use the full extents.

      //   Otherwise scale the first and last times.

      // Initialize the zero polynomial.

      m_polynomial = PiecewisePolynomial(scaledFirstTime, scaledLastTime, p_degree, 3);

    // If fitting a polynomial over hermite data, then do not fit the polynomial,

    // just set the knots and move on.

    if ( type == PolyFunctionOverHermiteConstant ) {

      if ( m_segments > 1 ) {

        std::vector<double> knots;

        double knotStep = (scaledLastTime - scaledFirstTime) / m_segments;

        m_polynomial.setKnots(knots);

      }

    }

    // Otherwise, fit the polynomial over the coordinate evaluated at the

    // values in the time cache.

    else {

      // Collect data to fit the polynomial over.

      std::vector<double> scaledTimes;

      std::vector< std::vector<double> > coordinates;

      // Compute how many points are required to fit the data

      //   This over estimates because the continuity conditions reduce the

      //   number of samples required.

      int requiredSamples = m_segments * ( p_degree + 1 ) * 3;

      // Double it to ensure that each segment is sufficiently supported

      requiredSamples *= 2;

      // If there are not enough samples, generate more

      if ( (int)p_cacheTime.size() < requiredSamples ) {

        double sampleRate = ( p_cacheTime.back() - p_cacheTime.front() ) / requiredSamples;

        for (int i = 0; i < requiredSamples; i++) {

          double sampleTime = p_cacheTime.front() + sampleRate * i;

          SetEphemerisTime( sampleTime );

          scaledTimes.push_back( (sampleTime - p_baseTime) / p_timeScale);

          coordinates.push_back( Coordinate() );

        }

      }

      else {

        for (int i = 0; i < (int)p_cacheTime.size(); i++) {

          SetEphemerisTime( p_cacheTime[i] );

          scaledTimes.push_back( (p_cacheTime[i] - p_baseTime) / p_timeScale);

          coordinates.push_back( Coordinate() );

        }

      }

      // Fit the polynomial.

      m_polynomial.fitPolynomials(scaledTimes, coordinates, m_segments);

    // Otherwise, fit the polynomial.

    else {

      m_polynomial = fitPolynomial(p_degree, m_segments);

    }

    // Set the flag indicating p_degree has been applied to the spacecraft

  /**

   * Create a test polynomial fit over the cached position data.

   * Create a polynomial fit over the cached position data.

   * The polynomial works in scaled time, so all inputs must be adjusted

   * by subtracting the base time and then dividing by the time scale.

   * 

   * @param segmentCount The number of segments for the piecewise polynomial

   *                     fit. Defaults to 1.

   * @param degree The degree of the piecewise polynomial fit.

   * 

   * @return @b PiecewisePolynomial A piecewise polynomial fit over the cached

   *                                position data based on the segment count

   *                                and polynomial degree.

   */

  PiecewisePolynomial SpicePosition::testFit() {

  PiecewisePolynomial SpicePosition::fitPolynomial(const int degree,

                                                   const int segmentCount) {

    // Check to see if the position is already a Polynomial Function

    if (p_source == PolyFunction)

    if (p_source == PolyFunction

        && p_degree == degree

        && m_segments == segmentCount) {

      return m_polynomial;

    }

    // Check that there is data available to fit a polynomial to

    if ( p_cache.empty() ) {

      throw IException(IException::Programmer, msg, _FILEINFO_);

    }

    // Adjust the degree of the polynomial to the available data

    int fitDegree = p_degree;

    if ( fitDegree > (int)p_cache.size() - 1 ) {

      fitDegree = p_cache.size() - 1;

    }

    // Recompute the time scaling

    ComputeBaseTime();

    }

    // Initialize the zero polynomial.

    PiecewisePolynomial testPoly(scaledFirstTime, scaledLastTime, fitDegree, 3);

    PiecewisePolynomial positionPoly(scaledFirstTime, scaledLastTime, degree, 3);

    // Collect data to fit the polynomial over.

    std::vector<double> scaledTimes;

    // Compute how many points are required

    //   This over estimates because the continuity conditions reduce the

    //   number of samples required.

    int requiredSamples = m_segments * ( p_degree + 1 ) * 3;

    // Double it to ensure that each segment is sufficiently supported

    requiredSamples *= 2;

    int requiredSamples = segmentCount * ( degree + 1 ) * 3;

    // Triple the number of samples just to be sure there are enough in each segments

    requiredSamples *= 3;

    // If there are not enough samples, generate more

    if ( (int)p_cacheTime.size() < requiredSamples ) {

      double sampleRate = ( p_cacheTime.back() - p_cacheTime.front() ) / requiredSamples;

    }

    // Fit the polynomial.

    testPoly.fitPolynomials(scaledTimes, coordinates, m_segments);

    try {

      positionPoly.fitPolynomials(scaledTimes, coordinates, segmentCount);

    }

    catch (IException &e) {

      QString msg = "Failed fitting polynomial over cached position data.";

      throw IException(e, IException::Unknown, msg, _FILEINFO_);

    }

    return testPoly;

    return positionPoly;

  }

   */

  void SpicePosition::setPolynomialSegments(int segments) {

    m_segments = segments;

    if ( !m_polynomial.isZero() ) {

    if ( p_degreeApplied ) {

      m_polynomial.refitPolynomials(segments);

    }

  }

  }

  /**

   * Compute the error between a polynomial and the stored position data.

   * 

   * @param poly The polynomial to compare with. It is assumed that the

   *             polynomial uses the same time scaling as this.

   * 

   * @return @b Histogram A histogram object containg the distance error in km.

   */

  Histogram SpicePosition::computeError(PiecewisePolynomial poly) {

    // TODO check the input poly

    // Compute the errors

    double error;

    std::vector<double> sampleTimes, sampleErrors, measuredCoord, polyCoord;

    sampleTimes = timeCache();

    for (int i = 0; i < (int)sampleTimes.size(); i++) {

      error = 0;

      measuredCoord = SetEphemerisTime(sampleTimes[i]);

      polyCoord = poly.evaluate( (sampleTimes[i] - p_baseTime) / p_timeScale );

      error += (measuredCoord[0] - polyCoord[0]) * (measuredCoord[0] - polyCoord[0]);

      error += (measuredCoord[1] - polyCoord[1]) * (measuredCoord[1] - polyCoord[1]);

      error += (measuredCoord[2] - polyCoord[2]) * (measuredCoord[2] - polyCoord[2]);

      sampleErrors.push_back(sqrt(error));

    }

    // Create the output histogram

    double minError = *std::min_element(sampleErrors.begin(), sampleErrors.end());

    double maxError = *std::max_element(sampleErrors.begin(), sampleErrors.end());

    Histogram errorHist(minError, maxError);

    errorHist.AddData(&sampleErrors[0], sampleErrors.size());

    return errorHist;

  }

  /**

   * Cache J2000 position over existing cached time range using

   * table

#include <SpiceZfc.h>

#include <SpiceZmc.h>

#include "Histogram.h"

#include "Table.h"

#include "PiecewisePolynomial.h"

#include "PolynomialUnivariate.h"

      void SetPolynomial(const Source type = PolyFunction);

      PiecewisePolynomial testFit();

      PiecewisePolynomial fitPolynomial(const int degree,

                                        const int segmentCount = 1);

      void SetPolynomial(const std::vector<double>& XC,

                         const std::vector<double>& YC,

      std::vector<double> polynomialKnots() const;

      int polySegmentIndex(double et) const;

      Histogram computeError(PiecewisePolynomial poly);

      //! Return the source of the position

      Source GetSource() {

        return p_source;

Testing with polynomial functions...

Time           = -69382819

Spacecraft (J) = -1730.4646 -1562.2157 2691.2806

Velocity (J) = -4.1416737 1.1953728 -1.9583892

Spacecraft (J) = -1730.4475 -1562.1986 2691.2512

Velocity (J) = -4.1218191 1.2118396 -1.9868448

Time           = -69382785

Spacecraft (J) = -1870.766 -1520.585 2623.0012

Velocity (J) = -4.0837166 1.245281 -2.0445834

Spacecraft (J) = -1870.5609 -1520.4151 2622.7076

Velocity (J) = -4.0850785 1.2438672 -2.0421458

Time           = -69382751

Spacecraft (J) = -2009.0152 -1477.2767 2551.8225

Velocity (J) = -4.0213941 1.2937034 -2.1283234

Spacecraft (J) = -2008.8516 -1477.1563 2551.614

Velocity (J) = -4.0215117 1.2930795 -2.1272575

Time           = -69382717

Spacecraft (J) = -2145.0681 -1432.3446 2477.8337

Velocity (J) = -3.9549945 1.3404603 -2.2093002

Spacecraft (J) = -2144.8786 -1432.2194 2477.6165

Velocity (J) = -3.9538877 1.3409582 -2.2101721

Time           = -69382683

Spacecraft (J) = -2278.7907 -1385.8484 2401.1337

Velocity (J) = -3.8848114 1.3853831 -2.287244

Spacecraft (J) = -2278.5785 -1385.7165 2400.9046

Velocity (J) = -3.8847094 1.3851462 -2.2868448

Time           = -69382648

Spacecraft (J) = -2410.0592 -1337.8533 2321.8302

Velocity (J) = -3.8111662 1.4283185 -2.3619019

Spacecraft (J) = -2409.8535 -1337.7387 2321.6307

Velocity (J) = -3.8113714 1.4277528 -2.3609357

Time           = -69382614

Spacecraft (J) = -2538.7616 -1288.4295 2240.0396

Velocity (J) = -3.7344202 1.4691342 -2.4330333

Spacecraft (J) = -2538.5495 -1288.3241 2239.8556

Velocity (J) = -3.7338252 1.4692066 -2.4331681

Time           = -69382580

Spacecraft (J) = -2664.799 -1237.651 2155.8858

Velocity (J) = -3.6549756 1.5077253 -2.5004246

Spacecraft (J) = -2664.5674 -1237.5411 2155.6938

Velocity (J) = -3.6547493 1.5076553 -2.5003078

Time           = -69382546

Spacecraft (J) = -2788.0864 -1185.5952 2069.4994

Velocity (J) = -3.5732667 1.5440209 -2.5639219

Spacecraft (J) = -2787.8525 -1185.4981 2069.3292

Velocity (J) = -3.5722258 1.5439137 -2.5637586

Time           = -69382512

Spacecraft (J) = -2908.5545 -1132.3409 1981.0142

Velocity (J) = -3.48974 1.5779929 -2.6234829

Spacecraft (J) = -2908.0063 -1132.0887 1980.5734

Velocity (J) = -3.4649233 1.5919584 -2.6477786

Testing line cache...

Time           = -69382819

Spacecraft (J) = -1730.4646 -1562.2157 2691.2806

Velocity (J) = -4.1416737 1.1953728 -1.9583892

Spacecraft (J) = -1730.4475 -1562.1986 2691.2512

Velocity (J) = -4.1218191 1.2118396 -1.9868448

Time           = -69382785

Spacecraft (J) = -1870.766 -1520.585 2623.0012

Velocity (J) = -4.0837166 1.245281 -2.0445834

Spacecraft (J) = -1870.5609 -1520.4151 2622.7076

Velocity (J) = -4.0850785 1.2438672 -2.0421458

Time           = -69382751

Spacecraft (J) = -2009.0152 -1477.2767 2551.8225

Velocity (J) = -4.0213941 1.2937034 -2.1283234

Spacecraft (J) = -2008.8516 -1477.1563 2551.614

Velocity (J) = -4.0215117 1.2930795 -2.1272575

Time           = -69382717

Spacecraft (J) = -2145.0681 -1432.3446 2477.8337

Velocity (J) = -3.9549945 1.3404603 -2.2093002

Spacecraft (J) = -2144.8786 -1432.2194 2477.6165

Velocity (J) = -3.9538877 1.3409582 -2.2101721

Time           = -69382683

Spacecraft (J) = -2278.7907 -1385.8484 2401.1337

Velocity (J) = -3.8848114 1.3853831 -2.287244

Spacecraft (J) = -2278.5785 -1385.7165 2400.9046

Velocity (J) = -3.8847094 1.3851462 -2.2868448

Time           = -69382648

Spacecraft (J) = -2410.0592 -1337.8533 2321.8302

Velocity (J) = -3.8111662 1.4283185 -2.3619019

Spacecraft (J) = -2409.8535 -1337.7387 2321.6307

Velocity (J) = -3.8113714 1.4277528 -2.3609357

Time           = -69382614

Spacecraft (J) = -2538.7616 -1288.4295 2240.0396

Velocity (J) = -3.7344202 1.4691342 -2.4330333

Spacecraft (J) = -2538.5495 -1288.3241 2239.8556

Velocity (J) = -3.7338252 1.4692066 -2.4331681

Time           = -69382580

Spacecraft (J) = -2664.799 -1237.651 2155.8858

Velocity (J) = -3.6549756 1.5077253 -2.5004246

Spacecraft (J) = -2664.5674 -1237.5411 2155.6938

Velocity (J) = -3.6547493 1.5076553 -2.5003078

Time           = -69382546

Spacecraft (J) = -2788.0864 -1185.5952 2069.4994

Velocity (J) = -3.5732667 1.5440209 -2.5639219

Spacecraft (J) = -2787.8525 -1185.4981 2069.3292

Velocity (J) = -3.5722258 1.5439137 -2.5637586

Time           = -69382512

Spacecraft (J) = -2908.5545 -1132.3409 1981.0142

Velocity (J) = -3.48974 1.5779929 -2.6234829

Spacecraft (J) = -2908.0063 -1132.0887 1980.5734

Velocity (J) = -3.4649233 1.5919584 -2.6477786

Testing extrapolation...

Time           = -69382512

Spacecraft (J) = -2908.5545 -1132.3409 1981.0142

Spacecraft (J) = -2908.0063 -1132.0887 1980.5734

Time           = -69382512

Spacecraft (J) = -2908.5548 -1132.3408 1981.0139

Spacecraft (J) = -2908.0066 -1132.0885 1980.5731

Testing Hermite function input ...

Source = 2