BundleControlPoint.cpp

#include "BundleControlPoint.h"

#include <QDebug>

#include "ControlMeasure.h"
#include "Latitude.h"
#include "Longitude.h"
#include "SpecialPixel.h"

namespace Isis {


  BundleControlPoint::BundleControlPoint(ControlPoint *controlPoint) {
    m_controlPoint = controlPoint;

    // setup vector of BundleMeasures for this control point
    int numMeasures = controlPoint->GetNumMeasures();
    for (int i = 0; i < numMeasures; i++) {
      ControlMeasure *controlMeasure = controlPoint->GetMeasure(i);
      if (controlMeasure->IsIgnored()) {
        continue;
      }

      addMeasure(controlMeasure);      
    }

    // initialize to 0.0
    m_corrections.clear();
    m_weights.clear();
    m_nicVector.clear();

    // initialize to Null for consistency with other bundle classes...
    m_aprioriSigmas.clear();
    m_aprioriSigmas[0] = Isis::Null;
    m_aprioriSigmas[1] = Isis::Null;
    m_aprioriSigmas[2] = Isis::Null;
    m_adjustedSigmas.clear();
    m_adjustedSigmas[0] = Isis::Null;
    m_adjustedSigmas[1] = Isis::Null;
    m_adjustedSigmas[2] = Isis::Null;
  }


  BundleControlPoint::BundleControlPoint(const BundleControlPoint &src) {
    copy(src);
  }


  BundleControlPoint::~BundleControlPoint() {
  }


  void BundleControlPoint::copy(const BundleControlPoint &src) {

    // sanity check
    clear();

    m_controlPoint = src.m_controlPoint;

    int numMeasures = src.size();

    for (int i = 0; i < numMeasures; i++)
      append(BundleMeasureQsp( new BundleMeasure(*(src.at(i))) ));

    m_corrections = src.m_corrections;
    m_aprioriSigmas = src.m_aprioriSigmas;
    m_adjustedSigmas = src.m_adjustedSigmas;
    m_weights = src.m_weights;
    m_nicVector = src.m_nicVector;
  }


  BundleMeasureQsp BundleControlPoint::addMeasure(ControlMeasure *controlMeasure) {

    BundleMeasureQsp bundleMeasure = BundleMeasureQsp( new BundleMeasure(controlMeasure, this) );

    append(bundleMeasure);

    return bundleMeasure;
  }


  void BundleControlPoint::computeResiduals() {
    m_controlPoint->ComputeResiduals();
  }


  void BundleControlPoint::setAdjustedSurfacePoint(SurfacePoint surfacePoint) {
    m_controlPoint->SetAdjustedSurfacePoint(surfacePoint);
  }


  void BundleControlPoint::setNumberOfRejectedMeasures(int numRejected) {
    m_controlPoint->SetNumberOfRejectedMeasures(numRejected);
  }


  void BundleControlPoint::setRejected(bool reject) {
    m_controlPoint->SetRejected(reject);
  }


  void BundleControlPoint::setWeights(const BundleSettingsQsp settings, double metersToRadians) {

    double d;

    double globalLatitudeAprioriSigma = settings->globalLatitudeAprioriSigma();
    double globalLongitudeAprioriSigma = settings->globalLongitudeAprioriSigma();
    double globalRadiusAprioriSigma = settings->globalRadiusAprioriSigma();

    if (m_controlPoint->GetType() == ControlPoint::Fixed) {
      m_weights[0] = 1.0e+50;
      m_weights[1] = 1.0e+50;
      m_weights[2] = 1.0e+50;
      // m_aprioriSigmas = Isis::Null by default
    }
    if (m_controlPoint->GetType() == ControlPoint::Free) {
      if (!IsSpecial(globalLatitudeAprioriSigma)) {
        m_aprioriSigmas[0] = globalLatitudeAprioriSigma;
        d = globalLatitudeAprioriSigma*metersToRadians;
        m_weights[0] = 1.0/(d*d);
      } // else m_aprioriSigma = Isis::Null
        // m_weights = 0.0
      if (!IsSpecial(globalLongitudeAprioriSigma)) {
        m_aprioriSigmas[1] = globalLongitudeAprioriSigma;
        d = globalLongitudeAprioriSigma*metersToRadians;
        m_weights[1] = 1.0/(d*d);
      } // else m_aprioriSigma = Isis::Null
        // m_weights = 0.0
      if (!settings->solveRadius()) {
        m_weights[2] = 1.0e+50;
      }
      else {
        if (!IsSpecial(globalRadiusAprioriSigma)) {
          m_aprioriSigmas[2] = globalRadiusAprioriSigma;
          d = globalRadiusAprioriSigma*0.001;
          m_weights[2] = 1.0/(d*d);
        }
      }
    }
    if (m_controlPoint->GetType() == ControlPoint::Constrained) {
      if ( m_controlPoint->IsLatitudeConstrained() ) {
        m_aprioriSigmas[0] = m_controlPoint->GetAprioriSurfacePoint().GetLatSigmaDistance().meters();
        m_weights[0] = m_controlPoint->GetAprioriSurfacePoint().GetLatWeight();
      }
      else if (!IsSpecial(globalLatitudeAprioriSigma)) {
        m_aprioriSigmas[0] = globalLatitudeAprioriSigma;
        d = globalLatitudeAprioriSigma*metersToRadians;
        m_weights[0] = 1.0/(d*d);
      } // else not constrained and global sigma is Null, then  m_aprioriSigmas = Isis::Null
        // m_weights = 0.0

      if ( m_controlPoint->IsLongitudeConstrained() ) {
        m_aprioriSigmas[1] = m_controlPoint->GetAprioriSurfacePoint().GetLonSigmaDistance().meters();
        m_weights[1] = m_controlPoint->GetAprioriSurfacePoint().GetLonWeight();
      }
      else if (!IsSpecial(globalLongitudeAprioriSigma)) {
        m_aprioriSigmas[1] = globalLongitudeAprioriSigma;
        d = globalLongitudeAprioriSigma*metersToRadians;
        m_weights[1] = 1.0/(d*d);
      } // else not constrained and global sigma is Null, then  m_aprioriSigmas = Isis::Null
        // m_weights = 0.0

      if (!settings->solveRadius()) {
        m_weights[2] = 1.0e+50;
      }
      else {
        if ( m_controlPoint->IsRadiusConstrained() ) {
          m_aprioriSigmas[2] = m_controlPoint->GetAprioriSurfacePoint().GetLocalRadiusSigma().meters();
          m_weights[2] = m_controlPoint->GetAprioriSurfacePoint().GetLocalRadiusWeight();
        }
        else if (!IsSpecial(globalRadiusAprioriSigma)) {
          m_aprioriSigmas[2] = globalRadiusAprioriSigma;
          d = globalRadiusAprioriSigma*0.001;
          m_weights[2] = 1.0/(d*d);
        } // else not constrained and global sigma is Null, then  m_aprioriSigmas = Isis::Null
          // m_weights = 0.0
      }
    }
  }


   void BundleControlPoint::zeroNumberOfRejectedMeasures() {
     m_controlPoint->ZeroNumberOfRejectedMeasures();
   }


  ControlPoint *BundleControlPoint::rawControlPoint() const {
    return m_controlPoint;
  }


  bool BundleControlPoint::isRejected() const {
    return m_controlPoint->IsRejected();
  }


  int BundleControlPoint::numberOfMeasures() const {
    return this->size();
  }


  int BundleControlPoint::numberOfRejectedMeasures() const {
    return m_controlPoint->GetNumberOfRejectedMeasures();
  }


  double BundleControlPoint::residualRms() const {
    return m_controlPoint->GetResidualRms();
  }


  SurfacePoint BundleControlPoint::adjustedSurfacePoint() const {
    return m_controlPoint->GetAdjustedSurfacePoint();
  }


  QString BundleControlPoint::id() const {
    return m_controlPoint->GetId();
  }


  ControlPoint::PointType BundleControlPoint::type() const{
    return m_controlPoint->GetType();
  }


  // ??? why bounded vector ??? can we use linear algebra vector ??? 
  boost::numeric::ublas::bounded_vector< double, 3 > &BundleControlPoint::corrections() {
    return m_corrections;
  }


  boost::numeric::ublas::bounded_vector< double, 3 > &BundleControlPoint::aprioriSigmas() {
    return m_aprioriSigmas;

  }


  boost::numeric::ublas::bounded_vector< double, 3 > &BundleControlPoint::adjustedSigmas() {
    return m_adjustedSigmas;
  }


  boost::numeric::ublas::bounded_vector< double, 3 > &BundleControlPoint::weights() {
    return m_weights;
  }


  boost::numeric::ublas::bounded_vector<double, 3> &BundleControlPoint::nicVector() {
    return m_nicVector;
  }


  SparseBlockRowMatrix &BundleControlPoint::cholmodQMatrix() {
    return m_cholmodQMatrix;
  }


  QString BundleControlPoint::formatBundleOutputSummaryString(bool errorPropagation) const {

    int numRays        = numberOfMeasures(); // should this depend on the raw point, as written, or this->size()???
    int numGoodRays    = numRays - numberOfRejectedMeasures();
    double ResidualRms = residualRms();
    double lat         = m_controlPoint->GetAdjustedSurfacePoint().GetLatitude().degrees();
    double lon         = m_controlPoint->GetAdjustedSurfacePoint().GetLongitude().degrees();
    double rad         = m_controlPoint->GetAdjustedSurfacePoint().GetLocalRadius().kilometers();

    QString pointType = ControlPoint::PointTypeToString(type()).toUpper();

    QString output = QString("%1%2%3 of %4%5%6%7%8%9%10%11\n")
                             .arg(id(), 16)
                             .arg(pointType, 15)
                             .arg(numGoodRays, 5)
                             .arg(numRays)
                             .arg(formatValue(ResidualRms, 6, 2))
                             .arg(formatValue(lat, 16, 8)) // deg
                             .arg(formatValue(lon, 16, 8)) // deg
                             .arg(formatValue(rad, 16, 8)) // km
                             .arg(formatLatitudeAdjustedSigmaString(16,8,errorPropagation))  // m
                             .arg(formatLongitudeAdjustedSigmaString(16,8,errorPropagation)) // m
                             .arg(formatRadiusAdjustedSigmaString(16,8,errorPropagation));   // m

    return output;
  }


  QString BundleControlPoint::formatBundleOutputDetailString(bool errorPropagation,
                                                             double RTM,
                                                             bool solveRadius) const {

    int numRays     = numberOfMeasures();
    int numGoodRays = numRays - numberOfRejectedMeasures();
    double lat      = m_controlPoint->GetAdjustedSurfacePoint().GetLatitude().degrees();
    double lon      = m_controlPoint->GetAdjustedSurfacePoint().GetLongitude().degrees();
    double rad      = m_controlPoint->GetAdjustedSurfacePoint().GetLocalRadius().kilometers();

    // ??? corrections is always zero ??? never set in this class ???
    // point corrections and initial sigmas
    double cor_lat_dd = m_corrections(0) * RAD2DEG;                // lat correction, decimal degs
    double cor_lon_dd = m_corrections(1) * RAD2DEG;                // lon correction, decimal degs
    double cor_rad_m  = m_corrections(2) * 1000.0;                 // radius correction, meters


    double cor_lat_m = m_corrections(0) * RTM;                     // lat correction, meters
    double cor_lon_m = m_corrections(1) * RTM * cos(lat*DEG2RAD);  // lon correction, meters

    double latInit = Isis::Null;
    if (!IsSpecial(lat)) {
      latInit = lat - cor_lat_dd;
    }


    double lonInit = Isis::Null;
    if (!IsSpecial(lon)) {
      lonInit = lon - cor_lon_dd;
    }

    double radInit = Isis::Null;
    if (!IsSpecial(rad)) {
      radInit = rad - m_corrections(2); // km
    }

    QString pointType = ControlPoint::PointTypeToString(type()).toUpper();

    QString output;

    output = QString(" Label: %1\nStatus: %2\n  Rays: %3 of %4\n")
                        .arg(id())
                        .arg(pointType)
                        .arg(numGoodRays)
                        .arg(numRays);

    QString labels = "\n     Point         Initial               Total               Total        "
                     "      Final             Initial             Final\n"
                     "Coordinate          Value             Correction          Correction        "
                     "    Value             Accuracy          Accuracy\n"
                     "                 (dd/dd/km)           (dd/dd/km)           (Meters)         "
                     "  (dd/dd/km)          (Meters)          (Meters)\n";
    output += labels;

    output += QString("  LATITUDE%1%2%3%4%5%6\n")
                      .arg(formatValue(latInit, 17, 8))                               // deg
                      .arg(formatValue(cor_lat_dd, 21, 8))                            // deg
                      .arg(formatValue(cor_lat_m, 20, 8))                             // m 
                      .arg(formatValue(lat, 20, 8))                                   // deg
                      .arg(formatLatitudeAprioriSigmaString(18,8))                    // m
                      .arg(formatLatitudeAdjustedSigmaString(18,8,errorPropagation)); // m 

    output += QString(" LONGITUDE%1%2%3%4%5%6\n")
                      .arg(formatValue(lonInit, 17, 8))                                // deg
                      .arg(formatValue(cor_lon_dd, 21, 8))                             // deg
                      .arg(formatValue(cor_lon_m, 20, 8))                              // m
                      .arg(formatValue(lon, 20, 8))                                    // deg
                      .arg(formatLongitudeAprioriSigmaString(18,8))                    // m
                      .arg(formatLongitudeAdjustedSigmaString(18,8,errorPropagation)); // m

    output += QString("    RADIUS%1%2%3%4%5%6\n\n")
                      .arg(formatValue(radInit, 17, 8))                             // km
                      .arg(formatValue(m_corrections(2), 21, 8))                    // km
                      .arg(formatValue(cor_rad_m, 20, 8))                           // m
                      .arg(formatValue(rad, 20, 8))                                 // km
                      .arg(formatRadiusAprioriSigmaString(18,8,solveRadius))        // m
                      .arg(formatRadiusAdjustedSigmaString(18,8,errorPropagation)); // m

    return output;
  }


  QString BundleControlPoint::formatValue(double value, int fieldWidth, int precision) const {
    QString output;
    IsSpecial(value) ? 
      output = QString("%1").arg("Null", fieldWidth) :
      output = QString("%1").arg(value, fieldWidth, 'f', precision);
    return output;
  }


  QString BundleControlPoint::formatAprioriSigmaString(int version, int fieldWidth,
                                                       int precision, bool solveRadius) const {
    QString aprioriSigmaStr;
    QString pointType = ControlPoint::PointTypeToString(type()).toUpper();
    if (pointType == "CONSTRAINED"||!solveRadius) {
        pointType = "N/A";
    }
    double sigma = m_aprioriSigmas[version];
    if (IsSpecial(sigma)) { // if globalAprioriSigma <= 0 (including Isis::NUll), then m_aprioriSigmas = Null
      aprioriSigmaStr = QString("%1").arg(pointType, fieldWidth);
    }
    else {
      aprioriSigmaStr = QString("%1").arg(sigma, fieldWidth, 'f', precision);
    }
    return aprioriSigmaStr;
  }


  QString BundleControlPoint::formatLatitudeAprioriSigmaString(int fieldWidth, 
                                                               int precision) const {
    return formatAprioriSigmaString(0, fieldWidth, precision, true);
  }


  QString BundleControlPoint::formatLongitudeAprioriSigmaString(int fieldWidth, 
                                                                int precision) const {
    return formatAprioriSigmaString(1, fieldWidth, precision, true);
  }


  QString BundleControlPoint::formatRadiusAprioriSigmaString(int fieldWidth,
                                                             int precision,
                                                             bool solveRadius) const {
    return formatAprioriSigmaString(2, fieldWidth, precision, solveRadius);
  }


  QString BundleControlPoint::formatAdjustedSigmaString(int type, int fieldWidth, int precision,
                                                        bool errorPropagation) const {
    QString adjustedSigmaStr;

    if (!errorPropagation) {
      adjustedSigmaStr = QString("%1").arg("N/A",fieldWidth);
    }
    else {
      double sigma = Isis::Null;
      if (type == 0) {
        sigma = m_controlPoint->GetAdjustedSurfacePoint().GetLatSigmaDistance().meters();
      }
      if (type == 1) {
        sigma = m_controlPoint->GetAdjustedSurfacePoint().GetLonSigmaDistance().meters();
      }
      if (type == 2) {
        sigma = m_controlPoint->GetAdjustedSurfacePoint().GetLocalRadiusSigma().meters();
      }
      if (IsSpecial(sigma)) {
        adjustedSigmaStr = QString("%1").arg("N/A",fieldWidth);
      }
      else {
        adjustedSigmaStr = QString("%1").arg(sigma, fieldWidth, 'f', precision);
      }
    }

    return adjustedSigmaStr;
  }


  QString BundleControlPoint::formatLatitudeAdjustedSigmaString(int fieldWidth, int precision,
                                                                bool errorPropagation) const {
    return formatAdjustedSigmaString(0, fieldWidth, precision, errorPropagation);
  }


  QString BundleControlPoint::formatLongitudeAdjustedSigmaString(int fieldWidth, int precision,
                                                                 bool errorPropagation) const {
    return formatAdjustedSigmaString(1, fieldWidth, precision, errorPropagation);
  }


  QString BundleControlPoint::formatRadiusAdjustedSigmaString(int fieldWidth, int precision,
                                                              bool errorPropagation) const {
    return formatAdjustedSigmaString(2, fieldWidth, precision, errorPropagation);
  }
}