SmtkMatcher.cpp

 
/* SPDX-License-Identifier: CC0-1.0 */
#include <cmath>
#include <cstdlib>
#include <ctime>
#include <sstream>
#include <iostream>
#include <iomanip>
 
#include <QSharedPointer>
 
#include "Camera.h"
#include "Projection.h"
#include "SpecialPixel.h"
#include "SmtkMatcher.h"
 
 
namespace Isis {
 
  SmtkMatcher::SmtkMatcher() : m_lhCube(0), m_rhCube(0), m_gruen(),
                               m_offImage(0), m_spiceErr(0),
                               m_useAutoReg(true) {
    randomNumberSetup();
  }
 
  SmtkMatcher::SmtkMatcher(const QString &regdef) : m_lhCube(0),
                                                        m_rhCube(0),
                                                        m_gruen(0),
                                                        m_offImage(0),
                                                        m_spiceErr(0),
                                                        m_useAutoReg(true) {
    setGruenDef(regdef);
    randomNumberSetup();
  }
 
  SmtkMatcher::SmtkMatcher(const QString &regdef, Cube *lhCube,
                           Cube *rhCube) : m_lhCube(lhCube),
                                           m_rhCube(rhCube),
                                           m_gruen(0), m_offImage(0),
                                           m_spiceErr(0),
                                           m_useAutoReg(true) {
    setGruenDef(regdef);
    randomNumberSetup();
  }
 
  SmtkMatcher::~SmtkMatcher() {
    gsl_rng_free(r);
  }
 
 
  void SmtkMatcher::setImages(Cube *lhCube, Cube *rhCube) {
    m_lhCube = lhCube;
    m_rhCube = rhCube;
    return;
  }
 
  void SmtkMatcher::setGruenDef(const QString &regdef) {
    Pvl reg(regdef);
    m_gruen = QSharedPointer<Gruen> (new Gruen(reg));  // Deallocation automatic
    return;
  }
 
  bool SmtkMatcher::isValid(const Coordinate &pnt) {
    validate();  // cubs and cameras
    Coordinate pnt2 = getLatLon(lhCamera(), pnt);
    if (pnt2.isValid()) {
      pnt2 = getLineSample(rhCamera(), pnt2);
    }
    return (pnt2.isValid());
  }
 
  void SmtkMatcher::setWriteSubsearchChipPattern(const QString &fileptrn) {
    validate();
    m_gruen->WriteSubsearchChips(fileptrn);
  }
 
  SmtkQStackIter SmtkMatcher::FindSmallestEV(SmtkQStack &stack) {
    SmtkQStackIter best(stack.begin()), current(stack.begin());
    while ( current != stack.end() ) {
      if (current.value().GoodnessOfFit() < best.value().GoodnessOfFit() ) {
        best = current;
      }
      ++current;
    }
    return (best);
  }
 
 
  SmtkQStackIter SmtkMatcher::FindExpDistEV(SmtkQStack &stack,
                                            const double &seedsample,
                                            const double &minEV,
                                            const double &maxEV) {
 
    if (stack.isEmpty()) return (stack.end());
    SmtkQStackIter best(stack.begin()), current(stack.begin());
 
    // Random number generator must scale between 0 and 1
    double randNum = gsl_rng_uniform (r);
    double t1 = -std::log(1.0 - randNum *
                          (1.0 - std::exp(-seedsample)))/seedsample;
    double pt = minEV + t1*(maxEV-minEV);
    double best_ev(DBL_MAX);
    while ( current != stack.end() ) {
      double test_ev(std::fabs(current.value().GoodnessOfFit()-pt));
      if ( test_ev  < best_ev) {
        best = current;
        best_ev = test_ev; // differs from ISIS2 which saved current eigenvalue
      }
      ++current;
    }
    return (best);
  }
 
  bool SmtkMatcher::isValid(const SmtkPoint &spnt) {
    bool valid(true);
 
    if (!spnt.isValid()) {
      std::cout << "Point status is invalid!\n";
      valid = false;
    }
 
    // Is valid in left image?
    if (!inCube(lhCamera(), spnt.getLeft())) {
      std::cout << "Left point (" << spnt.getLeft().getLine() << ","
           << spnt.getLeft().getSample() << ") is invalid!\n";
      valid = false;
    }
 
    // Is valud in right image?
    if (!inCube(rhCamera(), spnt.getRight())) {
      std::cout << "Right point (" << spnt.getRight().getLine() << ","
           << spnt.getRight().getSample() << ") is invalid!\n";
      valid = false;
    }
 
    return (valid);
  }
 
  SmtkPoint SmtkMatcher::Register(const Coordinate &lpnt,
                                  const AffineRadio &affrad) {
    return (Register(PointGeometry(lpnt), PointGeometry(), affrad));
  }
 
  SmtkPoint SmtkMatcher::Register(const PointPair &pnts,
                                  const AffineRadio &affrad) {
    return (Register(PointGeometry(pnts.getLeft()),
                     PointGeometry(pnts.getRight()), affrad));
  }
 
 
 
  SmtkPoint SmtkMatcher::Register(const SmtkPoint &spnt,
                                  const AffineRadio &affrad) {
 
    //  If point is already registered don't do it again.
    if (spnt.isRegistered()) { return (spnt); }
 
    PointGeometry left = PointGeometry(spnt.getLeft());
    PointGeometry right = PointGeometry(spnt.getRight());
 
    return (Register(left, right, affrad));
  }
 
 
  SmtkPoint SmtkMatcher::Register(const PointGeometry &lpg,
                                  const PointGeometry &rpg,
                                  const AffineRadio &affrad) {
 
    // Validate object state
    validate();
 
    // Test if the left point is defined. This will throw an error if this
    // situation occurs
    if (!lpg.getPoint().isValid()) {
      QString mess = "Left point is not defined which is required";
      throw IException(IException::Programmer, mess, _FILEINFO_);
    }
 
   // First we need a lat,lon from the left image to find the same place in
    // the right image.
    Coordinate lpnt = lpg.getPoint();
    Coordinate lgeom = lpg.getGeometry();
    if (!lgeom.isValid()) {
      lgeom = getLatLon(lhCamera(), lpnt);
    }
 
    // Construct geometry and check validity
    PointGeometry left = PointGeometry(lpnt, lgeom);
    if (!left.isValid()) {
      m_offImage++;
      return ( SmtkPoint(PointPair(lpnt), PointPair(lgeom)) );
    }
 
    // Validate the right point
    Coordinate rpnt = rpg.getPoint();
    Coordinate rgeom = rpg.getGeometry();
    if ( !rpnt.isValid() ) {
      if (rgeom.isValid()) {
        rpnt = getLineSample(rhCamera(), rgeom);
      }
      else {
        rpnt = getLineSample(rhCamera(), lgeom);
        rgeom = lgeom;
      }
    }
    else if (!rgeom.isValid() ){
      rgeom = getLatLon(rhCamera(), rpnt);
    }
 
    //  Construct and for good right geometry
    PointGeometry right = PointGeometry(rpnt, rgeom);
    if (!right.isValid()) {
      m_spiceErr++;
      return (SmtkPoint(PointPair(lpnt, rpnt), PointPair(lgeom, rgeom)));
    }
 
    try {
      // These calls are computationally expensive... can we fix it?
      m_gruen->PatternChip()->TackCube(lpnt.getSample(), lpnt.getLine());
      m_gruen->PatternChip()->Load(*m_lhCube);
      m_gruen->SearchChip()->TackCube(rpnt.getSample(), rpnt.getLine());
      m_gruen->SearchChip()->Load(*m_rhCube, *m_gruen->PatternChip(),
                                  *m_lhCube);
    }
    catch (IException &) {
      m_offImage++;  // Failure to load is assumed an offimage error
      return (SmtkPoint(PointPair(lpnt,rpnt), PointPair(lgeom,rgeom)));
    }
 
    // Register the points with incoming affine/radiometric parameters
    m_gruen->setAffineRadio(affrad);
    return (makeRegisteredPoint(left, right, m_gruen.data()));
  }
 
  SmtkPoint SmtkMatcher::Create(const Coordinate &left,
                                const Coordinate &right) {
 
    // Check out left image point
    Coordinate lgeom = getLatLon(lhCamera(), left);
    if (!lgeom.isValid() ) {
      m_offImage ++;
      return (SmtkPoint(PointPair(left,right), PointPair(lgeom)));
    }
 
    // Check out right image point
    Coordinate rgeom = getLatLon(rhCamera(), right);
    if (!rgeom.isValid() ) {
      m_offImage ++;
      return (SmtkPoint(PointPair(left,right), PointPair(lgeom,rgeom)));
    }
 
    //  Make the point
    SmtkPoint spnt = SmtkPoint(PointPair(left,right), PointPair(lgeom,rgeom));
    spnt.m_matchpt.m_analysis.setZeroState();
    spnt.m_isValid = true;
    return (spnt);
  }
 
 
  SmtkPoint SmtkMatcher::Clone(const SmtkPoint &point,
                               const Coordinate &left) {
 
    //  Computes local chip location (chipLoc) with Affine.  This gives offset
    //   in right pixel space.  Simply add result to right point to get the
    //   new cloned right point.
    Coordinate offset = left - point.getLeft();
    Coordinate right = point.getRight() + point.getAffine().getPoint(offset);
    PointPair cpoint = PointPair(left, right);
 
    MatchPoint mpt = point.m_matchpt;
    mpt.m_point = cpoint;
 
    //  Currently will not have any valid geometry
    SmtkPoint spnt = SmtkPoint(mpt, PointGeometry(right), PointPair());
    spnt.m_registered = false;
    spnt.m_isValid = inCube(lhCamera(), left) && inCube(rhCamera(), right);
    return (spnt);
  }
 
  void SmtkMatcher::randomNumberSetup() {
    (void) GSL::GSLUtility::getInstance();
    gsl_rng_env_setup();
    T = gsl_rng_default;
    r = gsl_rng_alloc (T);
    return;
  }
  bool SmtkMatcher::validate(const bool &throwError) const {
    bool isGood(true);
    if (!m_lhCube) isGood = false;
    if (!m_rhCube) isGood = false;
    if (!m_gruen.data()) isGood = false;
    if ((!isGood) && throwError) {
      QString mess = "Images/match algorithm not initialized!";
      throw IException(IException::Programmer, mess, _FILEINFO_);
    }
    return (isGood);
  }
 
  bool SmtkMatcher::inCube(const Camera &camera, const Coordinate &pnt) const {
    if (!pnt.isValid()) return (false);
    if ( (pnt.getSample() < 0.5) || (pnt.getLine() < 0.5) ) return false;
    if ( (pnt.getSample() > (camera.Samples() + 0.5)) ||
         (pnt.getLine()   > (camera.Lines()   + 0.5)) ) {
      return false;
    }
 
    return true;
  }
 
 
  Coordinate SmtkMatcher::getLatLon(Camera &camera, const Coordinate &pnt) {
    // Check if pixel coordinate is in the left image
    Coordinate geom;
    if (pnt.isValid()) {
      if (inCube(camera, pnt)) {
        if ( camera.SetImage(pnt.getSample(), pnt.getLine()) ) {
          double latitude = camera.UniversalLatitude();
          double longitude = camera.UniversalLongitude();
          geom.setLatLon(latitude, longitude);
        }
      }
    }
    return (geom);
  }
 
  Coordinate SmtkMatcher::getLineSample(Camera &camera,
                                        const Coordinate &geom) {
    // Check if pixel coordinate is in the left image
    Coordinate pnt;
    if (geom.isValid()) {
      if ( camera.SetUniversalGround(geom.getLatitude(), geom.getLongitude()) ) {
        if ( camera.InCube() ) {
          pnt.setLineSamp(camera.Line(), camera.Sample());
        }
      }
    }
    return (pnt);
  }
 
  SmtkPoint SmtkMatcher::makeRegisteredPoint(const PointGeometry &left,
                                             const PointGeometry &right,
                                             Gruen *gruen) {
 
    if (gruen->Register() != AutoReg::SuccessSubPixel) {
      return (SmtkPoint(PointPair(left.getPoint(),right.getPoint()),
                         PointPair(left.getGeometry(), right.getGeometry())));
    }
 
    // Registration point data
    MatchPoint match = gruen->getLastMatch();
 
    // Compute new right coordinate data
    Coordinate rcorr = Coordinate(gruen->CubeLine(), gruen->CubeSample());
    Coordinate rgeom = getLatLon(rhCamera(), rcorr);
    PointGeometry rpoint = PointGeometry(rcorr, rgeom);
 
    //  Get left and original right geometry
    PointPair pgeom = PointPair(left.getGeometry(), right.getGeometry());
 
    // Create SMTK point and determine status/validity
    SmtkPoint spnt = SmtkPoint(match, rpoint, pgeom);
 
    //  Check for valid mapping
    if ( !rpoint.isValid() ) {
      m_offImage++;
      spnt.m_isValid = false;
    }
    else {
     // Check for distance error
      double dist = (rcorr - right.getPoint()).getDistance();
      if (dist > gruen->getSpiceConstraint()) {
        m_spiceErr++;
        spnt.m_isValid = false;
      }
      else {
        spnt.m_isValid = match.isValid();
      }
    }
 
    return (spnt);
  }
 
}