ControlPoint.cpp

 
/* SPDX-License-Identifier: CC0-1.0 */
 
#include "ControlPoint.h"
 
#include <boost/numeric/ublas/symmetric.hpp>
#include <boost/numeric/ublas/io.hpp>
 
#include <QDebug>
#include <QHash>
#include <QString>
#include <QStringList>
 
#include "Application.h"
#include "CameraDetectorMap.h"
#include "CameraDistortionMap.h"
#include "CameraFocalPlaneMap.h"
#include "CameraGroundMap.h"
#include "ControlMeasure.h"
#include "ControlMeasureLogData.h"
#include "ControlNet.h"
#include "Cube.h"
#include "IString.h"
#include "Latitude.h"
#include "Longitude.h"
#include "PvlObject.h"
#include "SerialNumberList.h"
#include "SpecialPixel.h"
#include "Statistics.h"
 
using boost::numeric::ublas::symmetric_matrix;
using boost::numeric::ublas::upper;
using namespace std;
 
namespace Isis {
  ControlPoint::ControlPoint() : invalid(false) {
    measures = NULL;
    cubeSerials = NULL;
 
    measures = new QHash< QString, ControlMeasure * >;
    cubeSerials = new QStringList;
 
    type = Free;
    dateTime = "";
    editLock = false;
    ignore = false;
    jigsawRejected = false;
    referenceExplicitlySet = false;
    aprioriSurfacePointSource = SurfacePointSource::None;
    aprioriRadiusSource = RadiusSource::None;
    parentNetwork = NULL;
    referenceMeasure = NULL;
    numberOfRejectedMeasures = 0;
    constraintStatus.reset();
  }
 
 
  ControlPoint::ControlPoint(const ControlPoint &other) {
    measures = NULL;
    cubeSerials = NULL;
    referenceMeasure = NULL;
    parentNetwork = NULL;
 
    editLock = false;
 
    measures = new QHash< QString, ControlMeasure * >;
    cubeSerials = new QStringList;
 
    QListIterator<QString> i(*other.cubeSerials);
    while (i.hasNext()) {
      QString sn = i.next();
 
      const ControlMeasure *otherCm = other.GetMeasure(sn);
      ControlMeasure *newMeasure = new ControlMeasure(*otherCm);
      AddMeasure(newMeasure);
 
      if (other.referenceMeasure == otherCm) {
        SetRefMeasure(newMeasure);
      }
    }
 
    id = other.id;
    chooserName = other.chooserName;
    dateTime = other.dateTime;
    type = other.type;
    invalid = other.invalid;
    editLock = other.editLock;
    jigsawRejected = other.jigsawRejected;
    referenceExplicitlySet = other.referenceExplicitlySet;
    ignore = other.ignore;
    aprioriSurfacePointSource = other.aprioriSurfacePointSource;
    aprioriSurfacePointSourceFile = other.aprioriSurfacePointSourceFile;
    aprioriRadiusSource = other.aprioriRadiusSource;
    aprioriRadiusSourceFile = other.aprioriRadiusSourceFile;
    aprioriSurfacePoint = other.aprioriSurfacePoint;
    adjustedSurfacePoint = other.adjustedSurfacePoint;
    numberOfRejectedMeasures = other.numberOfRejectedMeasures;
    constraintStatus = other.constraintStatus;
  }
 
 
  ControlPoint::ControlPoint(const QString &newId) : invalid(false) {
    parentNetwork = NULL;
    measures = NULL;
    referenceMeasure = NULL;
    numberOfRejectedMeasures = 0;
    measures = new QHash< QString, ControlMeasure * >;
    cubeSerials = new QStringList;
 
    id = newId;
    type = Free;
    editLock = false;
    jigsawRejected = false;
    referenceExplicitlySet = false;
    ignore = false;
    aprioriSurfacePointSource = SurfacePointSource::None;
    aprioriRadiusSource = RadiusSource::None;
    constraintStatus.reset();
  }
 
  ControlPoint::~ControlPoint() {
    if (measures != NULL) {
      QList< QString > keys = measures->keys();
      for (int i = 0; i < keys.size(); i++) {
        delete(*measures)[keys[i]];
        (*measures)[keys[i]] = NULL;
      }
 
      delete measures;
      measures = NULL;
    }
 
    if (cubeSerials) {
      delete cubeSerials;
      cubeSerials = NULL;
    }
 
    referenceMeasure = NULL;
  }
 
  void ControlPoint::Load(PvlObject &p) {
 
  }
 
 
  void ControlPoint::Add(ControlMeasure *measure) {
    PointModified();
    AddMeasure(measure);
  }
 
 
  void ControlPoint::AddMeasure(ControlMeasure *measure) {
    // Make sure measure is unique
    foreach(ControlMeasure * m, measures->values()) {
      if (m->GetCubeSerialNumber() == measure->GetCubeSerialNumber()) {
        QString msg = "The SerialNumber is not unique. A measure with "
            "serial number [" + measure->GetCubeSerialNumber() + "] already "
            "exists for ControlPoint [" + GetId() + "]";
        throw IException(IException::Programmer, msg, _FILEINFO_);
      }
    }
 
    if (!measures->size()) {
      referenceMeasure = measure;
    }
    else if (referenceMeasure->IsIgnored() && !measure->IsIgnored() &&
        !IsReferenceExplicit() && !IsEditLocked()) {
      // The current "implicit" reference is ignored, but this new measure
      // isn't, and the point is not edit locked, so make this measure the new
      // reference
      referenceMeasure = measure;
    }
 
    measure->parentPoint = this;
    QString newSerial = measure->GetCubeSerialNumber();
    measures->insert(newSerial, measure);
    cubeSerials->append(newSerial);
 
    // notify parent network if we have one
    if (parentNetwork) {
      parentNetwork->measureAdded(measure);
      parentNetwork->emitNetworkStructureModified();
    }
  }
 
 
  void ControlPoint::ValidateMeasure(QString serialNumber) const {
    if (!measures->contains(serialNumber)) {
      QString msg = "No measure with serial number [" + serialNumber +
          "] is owned by this point";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
  }
 
 
  int ControlPoint::Delete(QString serialNumber) {
    ValidateMeasure(serialNumber);
    ControlMeasure *cm = (*measures)[serialNumber];
 
    // notify parent network of the change
    if (parentNetwork) {
      parentNetwork->measureDeleted(cm);
 
      if (!IsIgnored() && !cm->IsIgnored()) {
        parentNetwork->emitNetworkStructureModified();
      }
    }
 
    if (cm->IsEditLocked()) {
      return ControlMeasure::MeasureLocked;
    }
 
    // remove measure from the point's data structures
    measures->remove(serialNumber);
    cubeSerials->removeAt(cubeSerials->indexOf(serialNumber));
 
    // update the reference measure
    if (cubeSerials->size()) {
      if (referenceMeasure == cm) {
        referenceMeasure = (*measures)[cubeSerials->at(0)];
        referenceExplicitlySet = false;
      }
    }
    else {
      referenceMeasure = NULL;
    }
 
    delete cm;
    cm = NULL;
 
    PointModified();
 
    return ControlMeasure::Success;
  }
 
 
  void ControlPoint::emitMeasureModified(ControlMeasure *measure, ControlMeasure::ModType modType, QVariant oldValue, QVariant newValue) {
    if (parentNetwork) {
      parentNetwork->emitMeasureModified(measure, modType, oldValue, newValue);
    }
  }
 
 
  int ControlPoint::Delete(ControlMeasure *measure) {
    return Delete(measure->GetCubeSerialNumber());
  }
 
 
  int ControlPoint::Delete(int index) {
    if (index < 0 || index >= cubeSerials->size()) {
      QString msg = "index [" + QString(index) + "] out of bounds";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    return Delete(cubeSerials->at(index));
  }
 
 
  ControlPoint::Status ControlPoint::ResetApriori() {
    if (IsEditLocked()) {
      return PointLocked;
    }
 
    aprioriSurfacePointSource = SurfacePointSource::None;
    aprioriSurfacePointSourceFile    = "";
    aprioriRadiusSource     = RadiusSource::None;
    aprioriRadiusSourceFile = "";
 
    aprioriSurfacePoint = SurfacePoint();
    constraintStatus.reset();
 
    return Success;
  }
 
 
  ControlMeasure *ControlPoint::GetMeasure(QString serialNumber) {
    ValidateMeasure(serialNumber);
    return (*measures)[serialNumber];
  }
 
 
  const ControlMeasure *ControlPoint::GetMeasure(QString serialNumber) const {
    ValidateMeasure(serialNumber);
    return measures->value(serialNumber);
  }
 
 
  const ControlMeasure *ControlPoint::GetMeasure(int index) const {
    if (index < 0 || index >= cubeSerials->size()) {
      QString msg = "Index [" + toString(index) + "] out of range";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    return GetMeasure(cubeSerials->at(index));
  }
 
 
  ControlMeasure *ControlPoint::GetMeasure(int index) {
    if (index < 0 || index >= cubeSerials->size()) {
      QString msg = "Index [" + toString(index) + "] out of range";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    return GetMeasure(cubeSerials->at(index));
  }
 
 
  bool ControlPoint::HasRefMeasure() const {
    return !(referenceMeasure == NULL);
  }
 
 
  const ControlMeasure *ControlPoint::GetRefMeasure() const {
    if (!HasRefMeasure()) {
      QString msg = "Control point [" + GetId() + "] has no reference measure!";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    return referenceMeasure;
  }
 
 
  ControlMeasure *ControlPoint::GetRefMeasure() {
    if (!HasRefMeasure()) {
      QString msg = "Control point [" + GetId() + "] has no reference measure!";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    return referenceMeasure;
  }
 
 
  ControlPoint::Status ControlPoint::SetChooserName(QString name) {
    if (editLock) {
      return PointLocked;
    }
    chooserName = name;
    return Success;
  }
 
 
  ControlPoint::Status ControlPoint::SetDateTime(QString newDateTime) {
    if (editLock) {
      return PointLocked;
    }
    dateTime = newDateTime;
    return Success;
  }
 
 
  ControlPoint::Status ControlPoint::SetEditLock(bool lock) {
    if (parentNetwork) {
      parentNetwork->emitPointModified(this, ControlPoint::EditLockModified, editLock, lock);
    }
    editLock = lock;
    return Success;
  }
 
 
  ControlPoint::Status ControlPoint::SetRejected(bool reject) {
    jigsawRejected = reject;
    return Success;
  }
 
 
  ControlPoint::Status ControlPoint::SetId(QString newId) {
    if (editLock) {
      return PointLocked;
    }
    QString oldId = id;
    id = newId;
    if (parentNetwork) {
      parentNetwork->UpdatePointReference(this, oldId);
    }
    return Success;
  }
 
 
  ControlPoint::Status ControlPoint::SetRefMeasure(ControlMeasure *cm) {
    if (editLock) {
      return PointLocked;
    }
 
    SetExplicitReference(cm);
    return Success;
  }
 
 
  ControlPoint::Status ControlPoint::SetRefMeasure(int index) {
    if (editLock) {
      return PointLocked;
    }
 
    if (index < 0 || index >= cubeSerials->size()) {
      QString msg = "Index [";
      msg += toString(index) + "] out of range";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    SetExplicitReference((*measures)[cubeSerials->at(index)]);
    return Success;
  }
 
 
  ControlPoint::Status ControlPoint::SetRefMeasure(QString sn) {
    if (editLock) {
      return PointLocked;
    }
 
    if (!cubeSerials->contains(sn)) {
      QString msg = "Point [" + id + "] has no measure with serial number [" +
          sn + "]";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    SetExplicitReference((*measures)[sn]);
    return Success;
  }
 
 
  void ControlPoint::SetExplicitReference(ControlMeasure *measure) {
    referenceExplicitlySet = true;
    referenceMeasure = measure;
  }
 
 
  ControlPoint::Status ControlPoint::SetIgnored(bool newIgnoreStatus) {
    if (editLock) {
      return PointLocked;
    }
 
    bool oldStatus = ignore;
    ignore = newIgnoreStatus;
 
    // only update if there was a change in status
    if (oldStatus != ignore) {
      PointModified();
      if (parentNetwork) {
        if (ignore) {
          parentNetwork->pointIgnored(this);
        }
        else {
          parentNetwork->pointUnIgnored(this);
        }
        parentNetwork->emitPointModified(this, ControlPoint::IgnoredModified, oldStatus, ignore);
      }
    }
 
    return Success;
  }
 
 
  ControlPoint::Status ControlPoint::SetAdjustedSurfacePoint(
    SurfacePoint newSurfacePoint) {
 
    PointModified();
    adjustedSurfacePoint = newSurfacePoint;
    return Success;
  }
 
 
  ControlPoint::Status ControlPoint::SetType(PointType newType) {
    if (type != Fixed && type != Free && type != Constrained) {
      QString msg = "Invalid Point Enumeration, [" + QString(type) + "], for "
          "Control Point [" + GetId() + "]";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    if (editLock) {
      return PointLocked;
    }
    if (parentNetwork) {
      parentNetwork->emitPointModified(this, ControlPoint::TypeModified, type, newType);
    }
 
    PointModified();
    type = newType;
    return Success;
  }
 
 
  ControlPoint::Status ControlPoint::SetAprioriRadiusSource(
    RadiusSource::Source source) {
    if (editLock) {
      return PointLocked;
    }
    PointModified();
    aprioriRadiusSource = source;
    return Success;
  }
 
 
  ControlPoint::Status ControlPoint::SetAprioriRadiusSourceFile(
    QString sourceFile) {
    if (editLock) {
      return PointLocked;
    }
    PointModified();
    aprioriRadiusSourceFile = sourceFile;
    return Success;
  }
 
 
  ControlPoint::Status ControlPoint::SetAprioriSurfacePoint(
    SurfacePoint aprioriSP) {
    SurfacePoint::CoordinateType coordType = SurfacePoint::Latitudinal;
    if (parentNetwork) {
      coordType = parentNetwork->GetCoordType();
    }
    if (editLock) {
      return PointLocked;
    }
      // ***TBD*** Does it make sense to try to do a generic check here? The
      // data types are different (angles vs distance) so for now do a switch.
    switch (coordType) {
      case SurfacePoint::Latitudinal:
        if (aprioriSP.GetLatSigma().isValid())
          constraintStatus.set(Coord1Constrained);
        if (aprioriSP.GetLonSigma().isValid())
          constraintStatus.set(Coord2Constrained);
        if (aprioriSP.GetLocalRadiusSigma().isValid())
          constraintStatus.set(Coord3Constrained);
        break;
      case SurfacePoint::Rectangular:
        if (aprioriSP.GetXSigma().isValid())
          constraintStatus.set(Coord1Constrained);
        if (aprioriSP.GetYSigma().isValid())
          constraintStatus.set(Coord2Constrained);
        if (aprioriSP.GetZSigma().isValid())
          constraintStatus.set(Coord3Constrained);
      }
 
    PointModified();
    aprioriSurfacePoint = aprioriSP;
    return Success;
  }
 
 
  ControlPoint::Status ControlPoint::SetAprioriSurfacePointSource(
      SurfacePointSource::Source source) {
    if (editLock) {
      return PointLocked;
    }
    PointModified();
    aprioriSurfacePointSource = source;
    return Success;
  }
 
 
  ControlPoint::Status ControlPoint::SetAprioriSurfacePointSourceFile(
      QString sourceFile) {
    if (editLock) {
      return PointLocked;
    }
    PointModified();
    aprioriSurfacePointSourceFile = sourceFile;
    return Success;
  }
 
 
  ControlPoint::Status ControlPoint::ComputeApriori() {
    // TODO (KLE): where should call this go? Also, what's the point? The method has no description.
    PointModified();
 
    // if point is fixed or constrained, ensure valid a priori point coordinates exist
    if ( (IsFixed() || IsConstrained()) &&  !aprioriSurfacePoint.Valid() ) {
      QString msg = "In method ControlPoint::ComputeApriori(). ControlPoint [" + GetId() + "] is ";
      msg += "fixed or constrained and requires a priori coordinates";
      throw IException(IException::User, msg, _FILEINFO_);
    }
 
    double xB = 0.0;  // body-fixed x
    double yB = 0.0;  // body-fixed y
    double zB = 0.0;  // body-fixed z
    double r2B = 0.0; // radius squared in body-fixed
    int goodMeasures = 0;
    double pB[3];
 
    // loop over measures to ...
    // 1) set focal plane x,y coordinates for all unignored measures;
    // 2) sum latitude, longitude, and radius coordinates in preparation for computing a priori
    //    coordinates by averaging.
    for (int i = 0; i < cubeSerials->size(); i++) {
      ControlMeasure *m = GetMeasure(i);
      if (m->IsIgnored()) {
        continue;
      }
 
      Camera *cam = m->Camera();
      if (cam == NULL) {
        QString cubeSN = m->GetCubeSerialNumber();
        QString msg = "in method ControlPoint::ComputeApriori(). Camera has not been set in ";
        msg += "measure for cube serial number [" + cubeSN + "], Control Point id ";
        msg += "[" + GetId() + "]. Camera must be set prior to calculating a priori coordinates";
        throw IException(IException::Programmer, msg, _FILEINFO_);
      }
 
      bool setImageSuccess = cam->SetImage(m->GetSample(), m->GetLine());
      // CSM cameras do not have focal planes so use sample and line instead
      if (cam->GetCameraType() == Camera::Csm) {
        m->SetFocalPlaneMeasured(m->GetSample(),
                                 m->GetLine());
      }
      else {
        m->SetFocalPlaneMeasured(cam->DistortionMap()->UndistortedFocalPlaneX(),
                                 cam->DistortionMap()->UndistortedFocalPlaneY());
      }
 
      // TODO: Seems like we should be able to skip this computation if point is fixed or
      // constrained in any coordinate. Currently we are always summing coordinates here. We could
      // save time by not doing this for fixed or constrained points.
      if (setImageSuccess) {
        goodMeasures++;
        cam->Coordinate(pB);
        xB += pB[0];
        yB += pB[1];
        zB += pB[2];
        r2B += pB[0]*pB[0] + pB[1]*pB[1] + pB[2]*pB[2];
      }
    }
 
    // if point is Fixed or Constrained in any number of coordinates, initialize adjusted surface
    // point to a priori coordinates (set in e.g. qnet or cneteditor) and exit
    if( IsFixed() || IsConstrained() || id.contains("Lidar")) {
      adjustedSurfacePoint = aprioriSurfacePoint;
      return Success;
    }
 
    // if point is Free, we continue to compute a priori coordinates
 
    // if no good measures, we're done
    // TODO: is the message true/meaningful?
    if (goodMeasures == 0) {
      QString msg = "in method ControlPoint::ComputeApriori(). ControlPoint [" + GetId() + "] has ";
      msg += "no measures which project to the body";
      throw IException(IException::User, msg, _FILEINFO_);
    }
 
    // Compute the averages if all coordinates are free
    // TODO: confirm if this "if" statement is necessary
    if (GetType() == Free || NumberOfConstrainedCoordinates() == 0) {
      double avgX = xB / goodMeasures;
      double avgY = yB / goodMeasures;
      double avgZ = zB / goodMeasures;
      double avgR2 = r2B / goodMeasures;
      double scale = sqrt(avgR2/(avgX*avgX+avgY*avgY+avgZ*avgZ));
 
      aprioriSurfacePoint.SetRectangular(
        Displacement((avgX*scale), Displacement::Kilometers),
        Displacement((avgY*scale), Displacement::Kilometers),
        Displacement((avgZ*scale), Displacement::Kilometers));
    }
 
    adjustedSurfacePoint = aprioriSurfacePoint;
    SetAprioriSurfacePointSource(SurfacePointSource::AverageOfMeasures);
    SetAprioriRadiusSource(RadiusSource::AverageOfMeasures);
 
    return Success;
  }
 
 
  ControlPoint::Status ControlPoint::ComputeResiduals() {
    if (IsIgnored()) {
      return Failure;
    }
 
    PointModified();
 
    // Loop for each measure to compute the error
    QList<QString> keys = measures->keys();
 
    for (int j = 0; j < keys.size(); j++) {
      ControlMeasure *m = (*measures)[keys[j]];
      if (m->IsIgnored()) {
        continue;
      }
 
      Camera *cam = m->Camera();
 
      double cuSamp;
      double cuLine;
      CameraFocalPlaneMap *fpmap = m->Camera()->FocalPlaneMap();
 
      // Map the coordinates of the control point through the Spice of the
      // measurement sample/line to get the computed sample/line.  This must be
      // done manually because the camera will compute a new time for line scanners,
      // instead of using the measured time.
      ComputeResiduals_Millimeters();
 
      // Convert the residuals in millimeters to undistorted pixels
      if (cam->GetCameraType()  ==  Isis::Camera::Radar) {
        // For radar line is calculated from time in the camera.  Use the
        // closest line to scale the focal plane y (doppler shift) to image line
        // for computing the line residual.  Get a local ratio
        //     measureLine    =   adjacentLine
        //     ------------       --------------  in both cases, doppler shift
        //     dopplerMLine       dopplerAdjLine  is calculated using SPICE
        //                                        at the time of the measurement
        //
        // 1.  Get the surface point mapped to by an adjacent pixel above (if
        //     doppler is < 0) or below (if doppler is > 0) the measured pixel
        // 2.  Set image to the measured sample/line to load the SPICE for the
        //     time of the measurement.
        // 3.  Map the surface point from the adjacent pixel through the SPICE
        //     into the image plane to get a scale for mapping from doppler
        //     shift to line.  Apply the scale to get the line residual
        double sample = m->GetSample();
        double computedY = m->GetFocalPlaneComputedY();
        double computedX = m->GetFocalPlaneComputedX();
        double adjLine;
 
        // Step 1. What happens if measured line is 1???  TODO
        if (computedY < 0) {
          adjLine = m->GetLine() - 1.;
        }
        else {
          adjLine = m->GetLine() + 1.;
        }
 
        cam->SetImage(sample, adjLine);
        SurfacePoint sp = cam->GetSurfacePoint();
 
        // Step 2.
        cam->SetImage(sample, m->GetLine());
        double focalplaneX;
        double scalingY;
 
        // Step 3.
        // The default bool value will come from CameraGroundMap instead of
        // RadarGroundMap so be explicit to turn off back-of-planet test.
        cam->GroundMap()->GetXY(sp, &focalplaneX, &scalingY, false);
        double deltaLine;
 
        if (computedY < 0) {
          deltaLine = -computedY/scalingY;
        }
        else {
          deltaLine = computedY/scalingY;
        }
 
        // Now map through the camera steps to take X from slant range to ground
        // range to pixels.  Y just tracks through as 0.
        if (cam->DistortionMap()->SetUndistortedFocalPlane(computedX,
                                                           computedY)){
          double focalPlaneX = cam->DistortionMap()->FocalPlaneX();
          double focalPlaneY = cam->DistortionMap()->FocalPlaneY();
          fpmap->SetFocalPlane(focalPlaneX,focalPlaneY);
        }
        cuSamp = fpmap->DetectorSample();
        cuLine = m->GetLine() + deltaLine;
      }
      else if (cam->GetCameraType()  ==  Isis::Camera::Csm) {
        //
        cuSamp = m->GetFocalPlaneComputedX();
        cuLine = m->GetFocalPlaneComputedY();
      }
      else {
        // Now things get tricky.  We want to produce errors in pixels not mm
        // but some of the camera maps could fail.  One that won't is the
        // FocalPlaneMap which takes x/y to detector s/l.  We will bypass the
        // distortion map and have residuals in undistorted pixels.
        if (!fpmap->SetFocalPlane(m->GetFocalPlaneComputedX(), m->GetFocalPlaneComputedY())) {
          QString msg = "Sanity check #1 for ControlPoint [" + GetId() +
              "], ControlMeasure [" + m->GetCubeSerialNumber() + "]";
          throw IException(IException::Programmer, msg, _FILEINFO_);
          // This error shouldn't happen but check anyways
        }
 
        cuSamp = fpmap->DetectorSample();
        cuLine = fpmap->DetectorLine();
      }
 
      // Compute the measures sample and line
 
      double muSamp;
      double muLine;
 
      if (cam->GetCameraType()  ==  Isis::Camera::Radar ||
          cam->GetCameraType()  ==  Isis::Camera::Csm) {
        // For CSM and Radar we use distorted pixels
        muSamp = m->GetSample();
        muLine = m->GetLine();
      }
      else {
        // For other sensors conver to undistorted pixels
        // Again we will bypass the distortion map and have residuals in undistorted pixels.
        if (!fpmap->SetFocalPlane(m->GetFocalPlaneMeasuredX(), m->GetFocalPlaneMeasuredY())) {
          QString msg = "Sanity check #2 for ControlPoint [" + GetId() +
              "], ControlMeasure [" + m->GetCubeSerialNumber() + "]";
          throw IException(IException::Programmer, msg, _FILEINFO_);
          // This error shouldn't happen but check anyways
        }
        muSamp = fpmap->DetectorSample();
        muLine = fpmap->DetectorLine();
      }
 
      // The units are in detector sample/lines.  We will apply the instrument
      // summing mode to get close to real pixels.  Note however we are in
      // undistorted pixels except for radar and CSM instruments.
      double sampResidual = muSamp - cuSamp;
      double lineResidual = muLine - cuLine;
      m->SetResidual(sampResidual, lineResidual);
    }
 
    return Success;
  }
 
  ControlPoint::Status ControlPoint::ComputeResiduals_Millimeters() {
    if (IsIgnored()) {
      return Failure;
    }
 
    PointModified();
 
    // Loop for each measure to compute the error
    QList<QString> keys = measures->keys();
 
    for (int j = 0; j < keys.size(); j++) {
      ControlMeasure *m = (*measures)[keys[j]];
      if (m->IsIgnored()) {
        continue;
      }
 
      Camera *cam = m->Camera();
      double cudx, cudy;
 
      // Map the coordinates of the control point through the Spice of the
      // measurement sample/line to get the computed undistorted focal plane
      // coordinates (mm if not radar).
      // This works for radar too because in the undistorted focal plane,
      // y has not been set to 0 (set to 0 when going to distorted focal plane
      // or ground range in this case), so we can hold the Spice to calculate
      // residuals in undistorted focal plane coordinates.
      // This does not work with CSM as it does not have a focal plane so
      // just use the sample and line
      if (cam->GetCameraType() == Camera::Csm) {
        cam->SetGround(GetAdjustedSurfacePoint());
        cudx = cam->Sample();
        cudy = cam->Line();
        // Reset to measure
        cam->SetImage(m->GetSample(), m->GetLine());
      }
      else {
        // no need to call setimage for framing camera
        if (cam->GetCameraType() != 0) {
          cam->SetImage(m->GetSample(), m->GetLine());
        }
        // The default bool value is true.  Turn back-of-planet test off for bundle adjustment.
        cam->GroundMap()->GetXY(GetAdjustedSurfacePoint(), &cudx, &cudy, false);
      }
 
      m->SetFocalPlaneComputed(cudx, cudy);
    }
 
    return Success;
  }
 
  QString ControlPoint::GetChooserName() const {
    if (chooserName != "") {
      return chooserName;
    }
    else {
      return FileName(Application::Name()).name();
    }
  }
 
  bool ControlPoint::HasChooserName() const {
    return !chooserName.isEmpty();
  }
 
  bool ControlPoint::HasDateTime() const {
    return !dateTime.isEmpty();
  }
 
 
  QString ControlPoint::GetDateTime() const {
    if (dateTime != "") {
      return dateTime;
    }
    else {
      return Application::DateTime();
    }
  }
 
 
  bool ControlPoint::IsEditLocked() const {
    return editLock;
  }
 
 
  bool ControlPoint::IsRejected() const {
    return jigsawRejected;
  }
 
 
  SurfacePoint ControlPoint::GetAdjustedSurfacePoint() const {
    return adjustedSurfacePoint;
  }
 
 
  SurfacePoint ControlPoint::GetBestSurfacePoint() const {
    if (adjustedSurfacePoint.Valid()) {
      return adjustedSurfacePoint;
    }
    else {
      return aprioriSurfacePoint;
    }
  }
 
 
  QString ControlPoint::GetId() const {
    return id;
  }
 
 
  bool ControlPoint::IsIgnored() const {
    return ignore;
  }
 
 
  bool ControlPoint::IsValid() const {
    return !invalid;
  }
 
 
  bool ControlPoint::IsInvalid() const {
    return invalid;
  }
 
 
  QString ControlPoint::PointTypeToString(PointType pointType) {
    QString str;
 
    switch (pointType) {
      case Fixed:
        str = "Fixed";
        break;
      case Constrained:
        str = "Constrained";
        break;
      case Free:
        str = "Free";
        break;
    }
 
    return str;
  }
 
 
  ControlPoint::PointType ControlPoint::StringToPointType(
      QString pointTypeString) {
 
    //  On failure assume Free
    ControlPoint::PointType type = ControlPoint::Free;
 
    QString errMsg  = "There is no PointType that has a string representation"
                      " of \"";
            errMsg += pointTypeString;
            errMsg += "\".";
 
    if (pointTypeString == "Fixed") {
      type = ControlPoint::Fixed;
    }
    else if (pointTypeString == "Constrained") {
      type = ControlPoint::Constrained;
    }
    else if (pointTypeString == "Free") {
      type = ControlPoint::Free;
    }
    else {
      throw IException(IException::Programmer, errMsg, _FILEINFO_);
    }
 
    return type;
  }
 
 
  QString ControlPoint::GetPointTypeString() const {
    return PointTypeToString(GetType());
  }
 
 
  ControlPoint::PointType ControlPoint::GetType() const {
    return type;
  }
 
 
  QString ControlPoint::RadiusSourceToString(RadiusSource::Source source) {
    QString str;
 
    switch (source) {
      case RadiusSource::None:
        str = "None";
        break;
      case RadiusSource::User:
        str = "User";
        break;
      case RadiusSource::AverageOfMeasures:
        str = "AverageOfMeasures";
        break;
      case RadiusSource::Ellipsoid:
        str = "Ellipsoid";
        break;
      case RadiusSource::DEM:
        str = "DEM";
        break;
      case RadiusSource::BundleSolution:
        str = "BundleSolution";
        break;
    }
 
    return str;
  }
 
 
  ControlPoint::RadiusSource::Source ControlPoint::StringToRadiusSource(
    QString str) {
 
    str = str.toLower();
    RadiusSource::Source source = RadiusSource::None;
 
    if (str == "user") {
      source = RadiusSource::User;
    }
    else if (str == "averageofmeasures") {
      source = RadiusSource::AverageOfMeasures;
    }
    else if (str == "ellipsoid") {
      source = RadiusSource::Ellipsoid;
    }
    else if (str == "dem") {
      source = RadiusSource::DEM;
    }
    else if (str == "bundlesolution") {
      source = RadiusSource::BundleSolution;
    }
 
    return source;
  }
 
 
 
  QString ControlPoint::GetRadiusSourceString() const {
    return RadiusSourceToString(aprioriRadiusSource);
  }
 
 
  QString ControlPoint::SurfacePointSourceToString(
    SurfacePointSource::Source source) {
 
    QString str;
 
    switch (source) {
      case SurfacePointSource::None:
        str = "None";
        break;
      case SurfacePointSource::User:
        str = "User";
        break;
      case SurfacePointSource::AverageOfMeasures:
        str = "AverageOfMeasures";
        break;
      case SurfacePointSource::Reference:
        str = "Reference";
        break;
      case SurfacePointSource::Basemap:
        str = "Basemap";
        break;
      case SurfacePointSource::BundleSolution:
        str = "BundleSolution";
        break;
    }
 
    return str;
  }
 
 
  ControlPoint::SurfacePointSource::Source
  ControlPoint::StringToSurfacePointSource(
    QString str) {
 
    str = str.toLower();
    SurfacePointSource::Source source = SurfacePointSource::None;
 
    if (str == "user") {
      source = SurfacePointSource::User;
    }
    else if (str == "averageofmeasures") {
      source = SurfacePointSource::AverageOfMeasures;
    }
    else if (str == "reference") {
      source = SurfacePointSource::Reference;
    }
    else if (str == "basemap") {
      source = SurfacePointSource::Basemap;
    }
    else if (str == "bundlesolution") {
      source = SurfacePointSource::BundleSolution;
    }
 
    return source;
  }
 
 
  QString ControlPoint::GetSurfacePointSourceString() const {
    return SurfacePointSourceToString(aprioriSurfacePointSource);
  }
 
 
  SurfacePoint ControlPoint::GetAprioriSurfacePoint() const {
    return aprioriSurfacePoint;
  }
 
 
   ControlPoint::RadiusSource::Source ControlPoint::GetAprioriRadiusSource()
       const {
     return aprioriRadiusSource;
   }
 
 
   bool ControlPoint::HasAprioriCoordinates() {
     if (aprioriSurfacePoint.GetX().isValid() &&
         aprioriSurfacePoint.GetY().isValid() &&
         aprioriSurfacePoint.GetZ().isValid()) {
       return true;
     }
 
     return false;
     // return aprioriSurfacePoint.Valid(); ???
   }
 
 
   bool ControlPoint::IsFree() const {
     return (type != Fixed && type != Constrained);
   }
 
 
   bool ControlPoint::IsFixed() const {
     return (type == Fixed);
   }
 
 
  bool ControlPoint::IsConstrained() {
    // if point type is Free, we ignore any a priori sigmas on the coordinates
    if (type == Free) {
      return false;
    }
 
    return constraintStatus.any();
  }
 
 
  bool ControlPoint::IsCoord1Constrained() {
    return constraintStatus[Coord1Constrained];
  }
 
 
  bool ControlPoint::IsCoord2Constrained() {
    return constraintStatus[Coord2Constrained];
  }
 
  bool ControlPoint::IsCoord3Constrained() {
    return constraintStatus[Coord3Constrained];
  }
 
 
  int ControlPoint::NumberOfConstrainedCoordinates() {
    return constraintStatus.count();
  }
 
 
  bool ControlPoint::HasAprioriRadiusSourceFile() const {
    return !( aprioriRadiusSourceFile.isEmpty() || aprioriRadiusSourceFile.isNull() );
  }
 
 
  QString ControlPoint::GetAprioriRadiusSourceFile() const {
    return aprioriRadiusSourceFile;
  }
 
 
  ControlPoint::SurfacePointSource::Source
  ControlPoint::GetAprioriSurfacePointSource() const {
    return aprioriSurfacePointSource;
  }
 
 
  bool ControlPoint::HasAprioriSurfacePointSourceFile() const {
    return !( aprioriSurfacePointSourceFile.isEmpty() || aprioriSurfacePointSourceFile.isNull() );
  }
 
 
  QString ControlPoint::GetAprioriSurfacePointSourceFile() const {
    return aprioriSurfacePointSourceFile;
  }
 
 
  int ControlPoint::GetNumMeasures() const {
    return measures->size();
  }
 
 
  int ControlPoint::GetNumValidMeasures() const {
    int size = 0;
    QList<QString> keys = measures->keys();
    for (int cm = 0; cm < keys.size(); cm++) {
      if (!(*measures)[keys[cm]]->IsIgnored()) {
        size++;
      }
    }
    return size;
  }
 
 
  int ControlPoint::GetNumLockedMeasures() const {
    int size = 0;
    QList<QString> keys = measures->keys();
    for (int cm = 0; cm < keys.size(); cm++) {
      if ((*measures)[keys[cm]]->IsEditLocked()) {
        size++;
      }
    }
    return size;
  }
 
 
  bool ControlPoint::HasSerialNumber(QString serialNumber) const {
    return cubeSerials->contains(serialNumber);
  }
 
 
  bool ControlPoint::IsReferenceExplicit() const {
    return referenceExplicitlySet;
  }
 
 
  QString ControlPoint::GetReferenceSN() const {
    if (!HasRefMeasure()) {
      QString msg = "There is no reference measure set in the ControlPoint [" +
          GetId() + "]";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    return referenceMeasure->GetCubeSerialNumber();
  }
 
 
  int ControlPoint::IndexOf(ControlMeasure *cm, bool throws) const {
    return IndexOf(cm->GetCubeSerialNumber(), throws);
  }
 
 
  int ControlPoint::IndexOf(QString sn, bool throws) const {
    int index = cubeSerials->indexOf(sn);
 
    if (throws && index == -1) {
      QString msg = "ControlMeasure [" + sn + "] does not exist in point [" +
          id + "]";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    return index;
  }
 
 
  int ControlPoint::IndexOfRefMeasure() const {
    if (!HasRefMeasure()) {
      QString msg = "There is no reference measure for point [" + id + "]."
          "  This also means of course that the point is empty!";
      throw IException(IException::Programmer, msg, _FILEINFO_);
    }
 
    int index = cubeSerials->indexOf(referenceMeasure->GetCubeSerialNumber());
 
    return index;
  }
 
 
  Statistics ControlPoint::GetStatistic(
    double(ControlMeasure::*statFunc)() const) const {
    Statistics stats;
    foreach(ControlMeasure * cm, *measures) {
      if (!cm->IsIgnored()) {
        stats.AddData((cm->*statFunc)());
      }
    }
 
    return stats;
  }
 
 
  Statistics ControlPoint::GetStatistic(long dataType) const {
    Statistics stats;
    foreach(ControlMeasure * cm, *measures) {
      if (!cm->IsIgnored()) {
        stats.AddData(cm->GetLogData(dataType).GetNumericalValue());
      }
    }
 
    return stats;
  }
 
 
  QList< ControlMeasure * > ControlPoint::getMeasures(
    bool excludeIgnored) const {
    QList< ControlMeasure * > orderedMeasures;
    for (int i = 0; i < cubeSerials->size(); i++) {
      ControlMeasure *measure = measures->value((*cubeSerials)[i]);
      if (!excludeIgnored || !measure->IsIgnored()) {
        orderedMeasures.append(measures->value((*cubeSerials)[i]));
      }
    }
    return orderedMeasures;
  }
 
 
  QList< QString > ControlPoint::getCubeSerialNumbers() const {
    return *cubeSerials;
  }
 
 
  const ControlMeasure *ControlPoint::operator[](QString serialNumber) const {
    return GetMeasure(serialNumber);
  }
 
 
  ControlMeasure *ControlPoint::operator[](QString serialNumber) {
    return GetMeasure(serialNumber);
  }
 
 
  const ControlMeasure *ControlPoint::operator[](int index) const {
    return GetMeasure(index);
  }
 
 
  ControlMeasure *ControlPoint::operator[](int index) {
    return GetMeasure(index);
  }
 
 
  bool ControlPoint::operator!=(const ControlPoint &other) const {
    return !(*this == other);
  }
 
 
  bool ControlPoint::operator==(const ControlPoint &other) const {
    return other.GetNumMeasures() == GetNumMeasures() &&
        other.id == id &&
        other.type == type &&
        other.chooserName == chooserName &&
        other.editLock == editLock &&
        other.ignore == ignore &&
        other.aprioriSurfacePointSource  == aprioriSurfacePointSource &&
        other.aprioriSurfacePointSourceFile == aprioriSurfacePointSourceFile &&
        other.aprioriRadiusSource  == aprioriRadiusSource &&
        other.aprioriRadiusSourceFile  == aprioriRadiusSourceFile &&
        other.aprioriSurfacePoint == aprioriSurfacePoint &&
        other.adjustedSurfacePoint == adjustedSurfacePoint &&
        other.invalid == invalid &&
        other.measures == measures &&
        other.dateTime == dateTime &&
        other.jigsawRejected == jigsawRejected &&
        other.constraintStatus == constraintStatus &&
        other.referenceExplicitlySet == referenceExplicitlySet &&
        other.numberOfRejectedMeasures == numberOfRejectedMeasures &&
        other.cubeSerials == cubeSerials &&
        other.referenceMeasure == referenceMeasure;
  }
 
 
  const ControlPoint &ControlPoint::operator=(const ControlPoint &other) {
 
    if (this != &other) {
      bool oldLock = editLock;
      editLock = false;
      for (int i = cubeSerials->size() - 1; i >= 0; i--) {
        (*measures)[cubeSerials->at(i)]->SetEditLock(false);
        Delete(cubeSerials->at(i));
      }
 
      //measures->clear(); = new QHash< QString, ControlMeasure * >;
 
      QHashIterator< QString, ControlMeasure * > i(*other.measures);
      while (i.hasNext()) {
        i.next();
        ControlMeasure *newMeasure = new ControlMeasure;
        *newMeasure = *i.value();
        AddMeasure(newMeasure);
        if (other.referenceMeasure == i.value()) {
          SetRefMeasure(newMeasure);
        }
      }
 
      invalid = other.invalid;
      referenceExplicitlySet   = other.referenceExplicitlySet;
      numberOfRejectedMeasures = other.numberOfRejectedMeasures;
      constraintStatus         = other.constraintStatus;
 
      SetId(other.id);
      SetChooserName(other.chooserName);
      SetDateTime(other.dateTime);
      SetType(other.type);
      SetRejected(other.jigsawRejected);
      SetIgnored(other.ignore);
      SetAprioriSurfacePointSource(other.aprioriSurfacePointSource);
      SetAprioriSurfacePointSourceFile(other.aprioriSurfacePointSourceFile);
      SetAprioriRadiusSource(other.aprioriRadiusSource);
      SetAprioriRadiusSourceFile(other.aprioriRadiusSourceFile);
      SetAprioriSurfacePoint(other.aprioriSurfacePoint);
      SetAdjustedSurfacePoint(other.adjustedSurfacePoint);
 
      // Set edit lock last so the it doesn't interfere with copying the other fields over.
      editLock = oldLock;
      SetEditLock(other.editLock);
    }
 
    return *this;
  }
 
 
  void ControlPoint::PointModified() {
    dateTime = "";
  }
 
 
  void ControlPoint::ZeroNumberOfRejectedMeasures()
 
  {
    numberOfRejectedMeasures = 0;
  }
 
 
  void ControlPoint::SetNumberOfRejectedMeasures(int numRejected) {
    numberOfRejectedMeasures = numRejected;
  }
 
 
  int ControlPoint::GetNumberOfRejectedMeasures() const {
    return numberOfRejectedMeasures;
  }
 
  double ControlPoint::GetSampleResidualRms() const {
      int nmeasures = measures->size();
      if ( nmeasures <= 0 ) {
          return 0.0;
      }
 
      Statistics stats;
 
      for( int i = 0; i < nmeasures; i++) {
          const ControlMeasure* m = GetMeasure(i);
          if ( !m ) {
              continue;
          }
 
          if ( !m->IsIgnored() || m->IsRejected() ) {
              continue;
          }
 
          stats.AddData(m->GetSampleResidual());
      }
 
      return stats.Rms();
  }
 
 
  double ControlPoint::GetLineResidualRms() const {
      int nmeasures = measures->size();
      if ( nmeasures <= 0 ) {
          return 0.0;
      }
 
      Statistics stats;
 
      for( int i = 0; i < nmeasures; i++) {
          const ControlMeasure* m = GetMeasure(i);
          if ( !m ) {
              continue;
          }
 
          if ( !m->IsIgnored() || m->IsRejected() ) {
              continue;
          }
 
          stats.AddData(m->GetLineResidual());
      }
 
      return stats.Rms();
  }
 
 
  double ControlPoint::GetResidualRms() const {
      int nmeasures = measures->size();
      if ( nmeasures <= 0 ) {
          return 0.0;
      }
 
      Statistics stats;
 
      for( int i = 0; i < nmeasures; i++) {
          const ControlMeasure* m = GetMeasure(i);
          if ( !m ) {
              continue;
          }
 
          if ( m->IsIgnored() || m->IsRejected() ) {
              continue;
          }
 
          stats.AddData(m->GetSampleResidual());
          stats.AddData(m->GetLineResidual());
      }
 
      return stats.Rms();
  }
 
  void ControlPoint::ClearJigsawRejected() {
    int nmeasures = measures->size();
    if ( nmeasures <= 0 ) {
        return;
    }
 
    for( int i = 0; i < nmeasures; i++) {
      ControlMeasure* m = GetMeasure(i);
      if ( !m ) {
        continue;
      }
 
      m->SetRejected(false);
    }
 
    SetRejected(false);
  }
 
}