LidarControlPoint.cpp

#include "LidarControlPoint.h"
 
#include <QString>
#include <QVector>
 
#include "Camera.h"
#include "CameraDistortionMap.h"
#include "CameraFocalPlaneMap.h"
#include "CameraGroundMap.h"
#include "ControlMeasure.h"
#include "ControlPoint.h"
#include "Cube.h"
#include "IException.h"
#include "iTime.h"
 
using namespace std;
 
namespace Isis {
  
 
  LidarControlPoint::LidarControlPoint() {
    m_time = iTime();
    m_range = -1.0;
    m_sigmaRange = -1.0;
    m_snSimultaneous = NULL;
 
    m_snSimultaneous = new QStringList;
  }
  
  
  LidarControlPoint::~LidarControlPoint() {
    if (m_snSimultaneous) {
      delete m_snSimultaneous;
      m_snSimultaneous = NULL;
    }
    
  }
  
  
  ControlPoint::Status LidarControlPoint::setTime(iTime time) {
    if (IsEditLocked()) {
      return ControlPoint::Status::PointLocked;
    }
    m_time = time;
    return ControlPoint::Status::Success;
  }
 
  
  ControlPoint::Status LidarControlPoint::setRange(double range) {
    if (IsEditLocked()) {
      return ControlPoint::Status::PointLocked;
    }
    m_range = range;
    return ControlPoint::Status::Success;
  }
  
  
  ControlPoint::Status LidarControlPoint::setSigmaRange(double sigmaRange) {
    if (IsEditLocked()) {
      return ControlPoint::Status::PointLocked;
    }
    m_sigmaRange = sigmaRange;
    return ControlPoint::Status::Success;
  }
  
  
  ControlPoint::Status LidarControlPoint::addSimultaneous(QString newSerial) {
    m_snSimultaneous->append(newSerial);
    return ControlPoint::Status::Success;
  }
  
  
  double LidarControlPoint::range() {
    return m_range;
  }
  
  
  iTime LidarControlPoint::time() {
    return m_time;
  }
  
  
  double LidarControlPoint::sigmaRange() {
    return m_sigmaRange;
  }
  
  
  QStringList LidarControlPoint::snSimultaneous() const{
    return *m_snSimultaneous;
  }
 
 
  bool LidarControlPoint::isSimultaneous(QString serialNumber) {
 
    if (m_snSimultaneous->contains(serialNumber)) {
      return true;
    }
 
    return false;
  }
 
 
  ControlPoint::Status LidarControlPoint::ComputeResiduals() {
    if (IsIgnored()) {
      return Failure;
    }
 
    ControlPoint::PointModified();
 
    double newSampleDistorted = 0.0;
    double newLineDistorted = 0.0;
 
    // Loop for each measure to compute the error
    QList<QString> keys = measures->keys();
 
    for (int j = 0; j < keys.size(); j++) {
      ControlMeasure *measure = (*measures)[keys[j]];
      if (measure->IsIgnored()) {
        continue;
      }
 
      // back project the adjusted surface point with SPICE updated from current bundle iteration
      Camera* camera = measure->Camera();
      if (camera->SetGround(GetAdjustedSurfacePoint())) {
        newSampleDistorted = camera->Sample();
        newLineDistorted = camera->Line();
        measure->SetRejected(false);
      }
      // what happens if SetGround fails (e.g. if back-projection is off the image?
      else {
        measure->SetRejected(true);
        measure->SetResidual(0.0, 0.0);
        continue;
      }
 
      // set measures sample, line to the back-projected location
      measure->SetCoordinate(newSampleDistorted, newLineDistorted);
 
      double measuredUndistortedFPx = camera->DistortionMap()->UndistortedFocalPlaneX();
      double measuredUndistortedFPy = camera->DistortionMap()->UndistortedFocalPlaneY();
      measure->SetFocalPlaneMeasured(measuredUndistortedFPx,measuredUndistortedFPy);
      measure->SetFocalPlaneComputed(measuredUndistortedFPx, measuredUndistortedFPy);
 
      measure->SetResidual(0.0, 0.0);
    }
 
    return Success;
  }
}