BundleLidarControlPoint.cpp

#include "BundleLidarControlPoint.h"
 
// qt lib
#include <QDebug>
 
// std lib
#include <vector>
 
// boost lib
#include <boost/lexical_cast.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
 
// Isis lib
#include "BundleLidarControlPoint.h"
#include "BundleLidarRangeConstraint.h"
#include "Camera.h"
#include "ControlMeasure.h"
#include "Latitude.h"
#include "LidarControlPoint.h"
#include "Longitude.h"
#include "SpecialPixel.h"
 
using namespace boost::numeric::ublas;
 
namespace Isis {
 
  BundleLidarControlPoint::BundleLidarControlPoint(const BundleSettingsQsp bundleSettings,
                                                   LidarControlPointQsp lidarControlPoint)
    : BundleControlPoint(bundleSettings, (ControlPoint*)  lidarControlPoint.data()) {
 
    m_lidarControlPoint = lidarControlPoint;
  }
 
 
  void BundleLidarControlPoint::initializeRangeConstraints() {
 
    QStringList simultaneousSerialNumbers = m_lidarControlPoint->snSimultaneous();
    for (int i= 0; i < size(); i++) {
      BundleMeasureQsp measure = at(i);
      if (!simultaneousSerialNumbers.contains(measure->cubeSerialNumber())) {
          continue;
      }
 
      m_rangeConstraints.append(BundleLidarRangeConstraintQsp
                                (new BundleLidarRangeConstraint(m_lidarControlPoint, measure)));
    }
  }
 
 
//  BundleLidarControlPoint::BundleLidarControlPoint(const BundleLidarControlPoint &src) {
//    : BundleControlPoint(((ControlPoint) src) {
//    copy(src);
//  }
 
 
  BundleLidarControlPoint::~BundleLidarControlPoint() {
  }
 
 
  void BundleLidarControlPoint::copy(const BundleLidarControlPoint &src) {
 
    BundleControlPoint::copy((BundleControlPoint) src);
  }
 
 
  void BundleLidarControlPoint::computeResiduals() {
    m_lidarControlPoint->ComputeResiduals();
 
    // compute and store focal plane residuals in millimeters
    for (int i = 0; i < size(); i++) {
      at(i)->setFocalPlaneResidualsMillimeters();
    }
  }
 
 
  void BundleLidarControlPoint::applyParameterCorrections(
        LinearAlgebra::Vector imageSolution,
        SparseBlockMatrix &sparseNormals,
        const BundleTargetBodyQsp target) {
    // Call parent class method to apply point corrections
    BundleControlPoint::applyParameterCorrections(imageSolution, sparseNormals, target);
 
    for (int i = 0; i < m_rangeConstraints.size(); i++) {
      m_lidarControlPoint->setRange(m_rangeConstraints[i]->rangeComputed());
      // The updated range sigma cannot be computed because the bundle does not
      // solve for the range, it simply re-computes it each iteration
      // m_lidarControlPoint->setRangeSigma(m_rangeConstraints[i]->rangeAdjustedSigma());
    }
  }
 
 
  int BundleLidarControlPoint::applyLidarRangeConstraints(SparseBlockMatrix &normalsMatrix,
                                                         LinearAlgebra::MatrixUpperTriangular& N22,
                                                         SparseBlockColumnMatrix& N12,
                                                         LinearAlgebra::VectorCompressed& n1,
                                                         LinearAlgebra::Vector& n2) {
    int constraintsApplied = 0;
 
    // loop over range constraints
    for (int i = 0; i < m_rangeConstraints.size(); i++) {
      if (m_rangeConstraints.at(i)->applyConstraint(normalsMatrix, N22, N12, n1, n2)) {
        constraintsApplied++;
      }
    }
 
    return constraintsApplied;
  }
 
 
  double BundleLidarControlPoint::vtpvRangeContribution() {
    double vtpv = 0.0;
    for (int i = 0; i < m_rangeConstraints.size(); i++) {
      vtpv += m_rangeConstraints.at(i)->vtpv();
    }
 
    return vtpv;
  }
 
 
  int BundleLidarControlPoint::numberRangeConstraints() {
    return m_rangeConstraints.size();
  }
 
 
  BundleLidarRangeConstraintQsp BundleLidarControlPoint::rangeConstraint(int n) {
    return m_rangeConstraints.at(n);
  }
 
 
  double BundleLidarControlPoint::range() {
    return m_lidarControlPoint->range();
  }
 
 
  double BundleLidarControlPoint::sigmaRange() {
    return m_lidarControlPoint->sigmaRange();
  }
}