Unverified Commit 5c850955 authored by dcookastro's avatar dcookastro Committed by GitHub
Browse files

Merge pull request #301 from kledmundson/LroLidar_Infrastructure 

Lro lidar infrastructure
parents 4bbcb474 463a5501

isis/src/control/objs/BundleAdjust/BundleAdjust.cpp

+32 −60
Original line number Diff line number Diff line
@@ -553,13 +553,14 @@ namespace Isis { 


        measure->setParentObservation(observation);

        measure->setParentImage(image);

        measure->setMeasureSigma(1.4);

      }



      point->ComputeApriori();

    }



    // add BundleLidarControlPoints

    int numLidarPoints;

    int numLidarPoints = 0;

    if (m_lidarDataSet) {

      numLidarPoints = m_lidarDataSet->points().size();

    }
@@ -586,6 +587,7 @@ namespace Isis { 


        measure->setParentObservation(observation);

        measure->setParentImage(image);

        measure->setMeasureSigma(30.0*1.4);



        lidarPoint->ComputeApriori();

      }
@@ -1262,10 +1264,6 @@ bool BundleAdjust::formNormalEquations() { 
      for (int j = 0; j < numMeasures; j++) {

        BundleMeasureQsp measure = point->at(j);



        if (measure->parentControlPoint()->id() == "Lidar3768") {

          int fred=1;

        }



        // flagged as "JigsawFail" implies this measure has been rejected

        // TODO  IsRejected is obsolete -- replace code or add to ControlMeasure

        if (measure->isRejected()) {
@@ -1554,9 +1552,6 @@ bool BundleAdjust::formNormalEquations() { 
                                      vector<double> &nj,

                                      BundleControlPointQsp &bundleControlPoint) {



    if (bundleControlPoint->id().contains("Lidar3304"))

        int fred=1;



    boost::numeric::ublas::bounded_vector<double, 3> &NIC = bundleControlPoint->nicVector();

    SparseBlockRowMatrix &Q = bundleControlPoint->qMatrix();
@@ -2388,8 +2383,6 @@ bool BundleAdjust::formNormalEquations() { 


    double measuredX, computedX, measuredY, computedY;

    double deltaX, deltaY;

    double observationSigma;

    double observationWeight;



    measureCamera = measure->camera();
@@ -2557,15 +2550,8 @@ bool BundleAdjust::formNormalEquations() { 
    obsValue = deltaY / measureCamera->PixelPitch();

    m_bundleResults.addResidualsProbabilityDistributionObservation(obsValue);



    // TODO: what if camera has been subsampled, is pixel pitch computation still valid?

    observationSigma = 1.4 * measureCamera->PixelPitch();



    // TODO: lidar kluge

    if (measure->parentControlPoint()->id().contains("Lidar")) {

      observationSigma *=30.0;

    }



    observationWeight = 1.0 / observationSigma;

    double observationSigma = measure->measureSigma();

    double observationWeightSqrt = measure->measureWeightSqrt();



    if (m_bundleResults.numberMaximumLikelihoodModels()

          > m_bundleResults.maximumLikelihoodModelIndex()) {
@@ -2575,18 +2561,18 @@ bool BundleAdjust::formNormalEquations() { 
      //dynamically build the cumulative probability distribution of the R^2 residual Z Scores

      m_bundleResults.addProbabilityDistributionObservation(residualR2ZScore);

      int currentModelIndex = m_bundleResults.maximumLikelihoodModelIndex();

      observationWeight *= m_bundleResults.maximumLikelihoodModelWFunc(currentModelIndex)

      observationWeightSqrt *= m_bundleResults.maximumLikelihoodModelWFunc(currentModelIndex)

                            .sqrtWeightScaler(residualR2ZScore);

    }



    // multiply coefficients by observation weight

    coeffImagePosition *= observationWeight;

    coeffImagePointing *= observationWeight;

    coeffPoint3D *= observationWeight;

    coeffRHS *= observationWeight;

    coeffImagePosition *= observationWeightSqrt;

    coeffImagePointing *= observationWeightSqrt;

    coeffPoint3D *= observationWeightSqrt;

    coeffRHS *= observationWeightSqrt;



    if (m_bundleSettings->solveTargetBody()) {

      coeffTarget *= observationWeight;

      coeffTarget *= observationWeightSqrt;

    }



    return true;
@@ -2598,8 +2584,6 @@ bool BundleAdjust::formNormalEquations() { 
   */

  void BundleAdjust::applyParameterCorrections() {



//    qDebug() << m_imageSolution;



    int t = 0;



    // TODO - update target body parameters if in solution
@@ -2630,12 +2614,6 @@ bool BundleAdjust::formNormalEquations() { 
    // Apply parameter corrections for control points

    // TODO: can we do this faster by threading with QtConcurrent::run?

    m_bundleControlPoints.applyParameterCorrections(m_sparseNormals, m_imageSolution);



//    QFuture<void> f1 =

//        QtConcurrent::run(&m_bundleControlPoints,

//                          &BundleControlPointVector::applyParameterCorrections, m_sparseNormals,

//                                                                                m_imageSolution);

//    f1.waitForFinished();

  }
@@ -2652,7 +2630,6 @@ bool BundleAdjust::formNormalEquations() { 
    double vtpvRange = 0.0;

    double weight;

    double vx, vy;

    double rmsx, rmsy, rmsxy;



    // x, y, and xy residual stats vectors

    Statistics xResiduals;
@@ -2662,9 +2639,6 @@ bool BundleAdjust::formNormalEquations() { 
    // vtpv for image coordinates

    int numObjectPoints = m_bundleControlPoints.size();



//    double vtpvTest = m_bundleControlPoints.vtpvMeasureContribution();

//    m_bundleControlPoints.rmsResiduals(rmsx, rmsy, rmsxy);



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



      BundleControlPointQsp point = m_bundleControlPoints.at(i);
@@ -2673,41 +2647,39 @@ bool BundleAdjust::formNormalEquations() { 
      for (int j = 0; j < numMeasures; j++) {

        const BundleMeasureQsp measure = point->at(j);



        weight = 1.4 * (measure->camera())->PixelPitch();



        // TODO: lidar kluge

        if (measure->parentControlPoint()->id().contains("Lidar")) {

          weight *=30.0;

        }



        weight = 1.0 / weight;

        weight = measure->measureWeightSqrt();

        weight *= weight;



        vx = measure->focalPlaneMeasuredX() - measure->focalPlaneComputedX();

        vy = measure->focalPlaneMeasuredY() - measure->focalPlaneComputedY();



        // TODO: Testing - correct lidar simultaneous measure by it's residual

//        if ( point->id().contains("Lidar", Qt::CaseInsensitive) ) {



        if ( point->id().contains("Lidar", Qt::CaseInsensitive) ) {

//          double sample = measure->sample();

//          double line = measure->line();



//          // in undistorted space

//          double sampleResidual = measure->sampleResidual();

//          double lineResidual = measure->lineResidual();



//          // are measure->sample, measure->line in distorted or undistorted space?

//          double newsample = measure->sample() - measure->sampleResidual();

//          double newline   = measure->line() - measure->lineResidual();

          //testing re-backprojection of point



          // get a priori surface point

          SurfacePoint surfacepoint = point->adjustedSurfacePoint();



//          measure->rawMeasure()->SetCoordinate(newsample, newline);

//          Camera* camera = measure->camera();

          double newsamp = 0.0;

          double newline = 0.0;



//          camera->SetImage(newsample,newline);

//          double newx = camera->DistortionMap()->UndistortedFocalPlaneX();

//          double newy = camera->DistortionMap()->UndistortedFocalPlaneY();

//          measure->rawMeasure()->SetFocalPlaneMeasured(newx,newy);

//        }

          if (measure->camera()->SetGround(surfacepoint)) {

            newsamp = measure->camera()->Sample();

            newline = measure->camera()->Line();

          }



          measure->rawMeasure()->SetCoordinate(newsamp, newline);

          Camera* camera = measure->camera();

          camera->SetImage(newsamp,newline);

          double newx = camera->DistortionMap()->UndistortedFocalPlaneX();

          double newy = camera->DistortionMap()->UndistortedFocalPlaneY();

          measure->rawMeasure()->SetFocalPlaneMeasured(newx,newy);

        }



        // if rejected, don't include in statistics

        if (measure->isRejected()) {

isis/src/control/objs/BundleAdjust/BundleAdjust.h

+3 −0
Original line number Diff line number Diff line
@@ -315,6 +315,9 @@ namespace Isis { 
   *                           constructor in the BundleSolutionInfo class because it is derived

   *                           from QObject. Note that we ultimately want to return a QSharedPointer

   *                           instead of a raw pointer.

   *   @history 2018-06-27 Ken Edmundson - Now setting measure sigmas in BundleMeasure in init()

   *                           method; retrieving sigma and sqrt of weight in computePartials and

   *                           computeVtpv methods.

   */

  class BundleAdjust : public QObject {

      Q_OBJECT

isis/src/control/objs/BundleConstraint/BundleLidarRangeConstraint.cpp

+1 −9
Original line number Diff line number Diff line
@@ -8,10 +8,8 @@ 
#include "CameraGroundMap.h"

#include "SpicePosition.h"



// boost lib

#include <boost/numeric/ublas/matrix_sparse.hpp>

// Boost Library

#include <boost/numeric/ublas/vector_proxy.hpp>

#include <boost/numeric/ublas/matrix_proxy.hpp>



using namespace boost::numeric::ublas;
@@ -135,7 +133,6 @@ namespace Isis { 
    // vtpv is the weight sum of squares of the residuals

    double t = m_rangeObserved - m_rangeComputed;

    m_vtpv = t * t * m_rangeObservedWeightSqrt * m_rangeObservedWeightSqrt;

    int fred=1;

  }
@@ -258,14 +255,9 @@ namespace Isis { 
    // form N22

    N22 += prod(trans(m_coeff_range_point3D), m_coeff_range_point3D);



//  std::cout << "N22" << std::endl << N22 << std::endl;

//  std::cout << "n2" << std::endl << n2 << std::endl;



    // contribution to n2 vector

    n2 += prod(trans(m_coeff_range_point3D), m_coeff_range_RHS);



//  std::cout << "n2" << std::endl << n2 << std::endl;



    return true;

  }

isis/src/control/objs/BundleConstraint/BundleLidarRangeConstraint.h

+2 −1
Original line number Diff line number Diff line
@@ -42,7 +42,8 @@ namespace Isis { 
   * @author 2018-04-13 Ken Edmundson

   *

   * @internal

   *   @history 2017-04-13 Ken Edmundson - Original version.

   *   @history 2018-04-13 Ken Edmundson - Original version.

   *   @history 2018-06-27 Ken Edmundson - Code clean up.

   */

  class BundleLidarRangeConstraint : public BundleConstraint {

    public:

isis/src/control/objs/BundleSolutionInfo/BundleSolutionInfo.cpp

+1 −6
Original line number Diff line number Diff line
@@ -589,12 +589,7 @@ namespace Isis { 
    int numValidPoints = m_statisticsResults->outputControlNet()->GetNumValidPoints();

    int numInnerConstraints = 0;

    int numDistanceConstraints = 0;

    int numDegreesOfFreedom = m_statisticsResults->numberObservations()

                            + m_statisticsResults->numberConstrainedPointParameters()

                            + m_statisticsResults->numberConstrainedImageParameters()

                            + m_statisticsResults->numberConstrainedTargetParameters()

                            + m_statisticsResults->numberContinuityConstraintEquations()

                            - m_statisticsResults->numberUnknownParameters();

    int numDegreesOfFreedom = m_statisticsResults->degreesOfFreedom();



    int convergenceCriteria = 1;