Fix findfeatures matrix inversion issues and improve FastGeom performance

isis/src/control/apps/findfeatures/FastGeom.h

@@ -19,6 +19,7 @@ find files of those names at the top level of this repository. **/
 #include "GenericTransform.h"
 #include "MatchImage.h"
 #include "PvlFlatMap.h"
+#include "QDebugLogger.h"
 
 namespace Isis {
 
@@ -52,19 +53,37 @@ class ImageTransform;
  * @internal
  *   @history 2015-10-01 Kris Becker - Original Version
  *   @history 2016-04-06 Kris Becker Created .cpp file and completed documentation
+ *   @history 2021-10-29 Kris Becker Added const qualifier to all compute() and
+ *                                     apply() methods
+ *   @history 2022-02-02 Kris Becker Added Grid implementation resulting in
+ *                                     consolidation/abstraction of Radial
+ *                                     algorithm code
  */
 
 class FastGeom {
   public:
     typedef GenericTransform::RectArea   RectArea;
+    typedef cv::Point2d                  FGPoint;
+    typedef cv::Rect2d                   FGFov;
+
     FastGeom();
     FastGeom(const PvlFlatMap &parameters);
     FastGeom(const int maxpts, const double tolerance, const bool crop = false,
              const bool preserve = false, const double &maxarea = 3.0);
     virtual ~FastGeom();
 
-    ImageTransform *compute(MatchImage &query, MatchImage &train);
-    void apply(MatchImage &query, MatchImage &train);
+    // ImageTransform *compute(MatchImage &query, MatchImage &train) const;
+    ImageTransform *compute(MatchImage &query, MatchImage &train,
+                            QLogger logger = QLogger() ) const;
+    void apply(MatchImage &query, MatchImage &train, QLogger logger = QLogger() ) const;
+
+    static cv::Mat getTransformMatrix(const std::vector<FGPoint> &querypts,
+                                      const std::vector<FGPoint> &trainpts,
+                                      std::vector<uchar>         &inliers,
+                                      const double tolerance,
+                                      QLogger logger = QLogger() );
+
+    PvlFlatMap getParameters() const;
 
   private:
     int        m_fastpts;    //!< Number of points to use for geom
@@ -75,6 +94,21 @@ class FastGeom {
     PvlFlatMap m_parameters; //!< Parameters of transform
 
     void validate( const QString &geomtype ) const;
+
+    /** Determine if point is in defined FOV for x,y */
+    inline bool in_fov(const FGPoint &point,
+                       const double xmin, const double xmax,
+                       const double ymin, const double ymax)
+                       const {
+
+      // Test if in FOV consistent with ISIS image coordinates
+      if ( (point.x >= xmin ) && (point.x < xmax ) ) {
+        if ( ( point.y >= ymin ) && (point.y < ymax ) ) {
+          return (true);
+        }
+      }
+      return (false);
+    }
 };
 
 }  // namespace Isis

isis/src/control/apps/findfeatures/GenericTransform.cpp

@@ -6,10 +6,12 @@ find files of those names at the top level of this repository. **/
 
 /* SPDX-License-Identifier: CC0-1.0 */
 
+#include <QtGlobal>
 #include <QString>
 #include <QSharedPointer>
 #include <opencv2/opencv.hpp>
 #include "GenericTransform.h"
+#include "IException.h"
 
 namespace Isis {
 /** Generic constructor is simply an identity transform */
@@ -62,7 +64,7 @@ cv::Mat GenericTransform::getMatrix() const {
 
 /** Return inverse transform matrix */
 cv::Mat GenericTransform::getInverse() const {
-  return ( m_inverse);
+  return ( m_inverse );
 }
 
 /** Return the resulting size of the transformed image */
@@ -120,6 +122,35 @@ cv::Point2f GenericTransform::inverse(const cv::Point2f &point) const {
   return ( dst[0] );
 }
 
+/**
+ * @brief Compute inverse with validation
+ *
+ * OpenCV may silenty return an empty matrix using the inv() method. There is a
+ * safer method that is applied here that will test if the matrix is invertable.
+ * If its not invertable and verify == true, then an exception is thrown.
+ *
+ * @author 2021-10-03 Kris J. Becker
+ *
+ * @param matrix Square matrix to invert
+ * @param verify If true will throw an exception if not invertable
+ *
+ * @return cv::Mat Inverted matrix
+ */
+cv::Mat GenericTransform::computeInverse(const cv::Mat &matrix,
+                                         const bool verify) {
+  cv::Mat inverse;
+  double result = cv::invert(matrix, inverse);
+  if ( verify == true ) {
+    if ( qFuzzyCompare(result+1.0, 0.0+1.0) ) {
+      QString msg = "cv::Mat inverse matrix is not invertable";
+      throw IException(IException::Programmer, msg, _FILEINFO_);
+    }
+  }
+
+  return ( inverse );
+}
+
+
 /** Set the forward matrix */
 void GenericTransform::setMatrix(const cv::Mat &matrix) {
   m_matrix = matrix;
@@ -132,14 +163,21 @@ void GenericTransform::setInverse(const cv::Mat &matrix) {
   return;
 }
 
-/** Calculate the inverse transform from the forward matrix */
-cv::Mat GenericTransform::calculateInverse(const cv::Mat &matrix) {
-  if (matrix.empty()) {  // inverting an empty matrix causes a segfault
-    QString msg = "Can't invert empty matrix.";
-    throw IException(IException::Programmer, msg, _FILEINFO_);
-  }
-
-  return ( matrix.inv() );
+/**
+ * @brief Calculate the inverse transform from the forward matrix
+ *
+ * This method will compute the inverse of the matrix and return the result. If
+ * the matrix is not invertable an exception is thrown unless verify == false.
+ * If verify == false, it is up to the caller to test the returned matrix (which
+ * may be filled with 0's).
+ *
+ * @param matrix Matrix to invert
+ *
+ * @return cv:Mat Inverted matrix
+ */
+cv::Mat GenericTransform::calculateInverse(const cv::Mat &matrix,
+                                           const bool verify) {
+  return ( computeInverse(matrix, verify) );
 }
 
 /** Set the size of the transformed image */

isis/src/control/apps/findfeatures/GenericTransform.h

@@ -31,6 +31,10 @@ namespace Isis {
  *   @history 2014-07-01 Kris Becker - Original Version
  *   @history 2016-03-08 Kris Becker Created .cpp from header and completed
  *                           documentation
+ *   @history 2021-10-30 Kris J. Becker Added verify parameter to
+ *                           calculateInverse() method; if verify==true, the
+ *                           inverse matrix is tested if it is indeed
+ *                           invertable.
  */
 class GenericTransform : public ImageTransform {
   public:
@@ -55,11 +59,17 @@ class GenericTransform : public ImageTransform {
     virtual cv::Point2f forward(const cv::Point2f &point) const;
     virtual cv::Point2f inverse(const cv::Point2f &point) const;
 
+    static cv::Mat computeInverse(const cv::Mat &matrix,
+                                  const bool verify = true);
+
   protected:
     void setMatrix(const cv::Mat &matrix);
     void setInverse(const cv::Mat &matrix);
 
-    virtual cv::Mat calculateInverse(const cv::Mat &matrix);
+
+    // Use in deriving clases for differnt behavior
+    virtual cv::Mat calculateInverse(const cv::Mat &matrix,
+                                     const bool verify = true);
 
     void setSize(const cv::Size &mSize);
 

isis/src/control/apps/findfeatures/ImageSource.cpp

@@ -92,7 +92,7 @@ bool ImageSource::hasCamera() const {
 QString ImageSource::getTargetName() const {
   if ( hasProjection() ) {
     PvlGroup mapping = m_data->m_projection->Mapping();
-    return (mapping["Target"][0]);
+    return (mapping["TargetName"][0]);
   }
   else if ( hasCamera() ) {
     return ( m_data->m_camera->targetName() );
@@ -311,7 +311,7 @@ cv::Mat ImageSource::getGeometryMapping(ImageSource &match,
     train.clear();
 
   #if 0
-    std::cout << "SSamp, NSamps: " << ssamp << ", " << nsamps << "\n";
+    std::cout << "\nSSamp, NSamps: " << ssamp << ", " << nsamps << "\n";
     std::cout << "SLine, NLines: " << sline << ", " << nlines << "\n";
     std::cout << "SINC, LINC:    " << sSpacing << ", " << lSpacing << "\n";
     std::cout << "Increment:     " << increment << "\n";

isis/src/control/apps/findfeatures/ImageSource.h

@@ -39,6 +39,9 @@ namespace Isis {
  *   @history 2014-07-01 Kris Becker - Original Version
  *   @history 2016-02-08 Kris Becker - Changed call to ISIS Histogram class for
  *                           recent changes
+ *   @history 2019-11-19 Kris Becker - Correctly return of target name from the
+ *                           Mapping group of projected images. Incorrect
+ *                           keyword Target used instead of TargetName
  */
 
 class ImageSource {

isis/src/control/apps/findfeatures/MatchMaker.cpp

@@ -32,6 +32,7 @@ find files of those names at the top level of this repository. **/
 #include "QDebugLogger.h"
 #include "RobustMatcher.h"
 #include "Statistics.h"
+#include "SurfacePoint.h"
 
 namespace Isis {
 
@@ -104,6 +105,7 @@ MatchImage MatchMaker::getGeometrySource() const {
   }
 
   if ( Query == m_geomFlag )  return ( query() );
+  if ( Both  == m_geomFlag )  return ( query() );
   if ( Train == m_geomFlag )  return ( train() );
   return ( MatchImage() );  // Got none
 }
@@ -145,7 +147,8 @@ MatcherSolutionList MatchMaker::match(const RobustMatcherList &matchers) {
 }
 
 
- PvlGroup MatchMaker::network(ControlNet &cnet, const MatcherSolution &solution,
+ PvlGroup MatchMaker::network(ControlNet &cnet, 
+                              const MatcherSolution &solution, 
                               ID &pointMaker) const {
 
 
@@ -179,14 +182,26 @@ MatcherSolutionList MatchMaker::match(const RobustMatcherList &matchers) {
     logger().flush();
   }
 
-  // Create control network
+  // Create control network. This will transfer all points to the network
+  // and any ones that don't make will be deleted.
   int nPoints = 0;
-  int nBad = 0;
+  int nBadPoints = 0;
+  int nBadMeasures = 0;
+  bool preserve_ignored = toBool(m_parameters.get("PreserveIgnoredControl", "False"));
   Statistics pointStats;
   for (int i = 0 ; i < points.size() ; i++) {
-    if ( points[i] != 0 ) {
-      if ( points[i]->GetNumValidMeasures() > 1 ) {
-        pointStats.AddData((double)points[i]->GetNumValidMeasures());
+    if ( (points[i] != 0)  ) {
+      bool isValid = ( !points[i]->IsIgnored() ) && (points[i]->GetNumValidMeasures() > 1);
+      nBadMeasures += (points[i]->GetNumMeasures() - points[i]->GetNumValidMeasures());
+      if ( preserve_ignored || isValid ) {
+        if ( isValid ) {
+          pointStats.AddData((double)points[i]->GetNumValidMeasures());
+        }
+        else {
+          // Ensure the point is ignored
+          points[i]->SetIgnored( true );
+          nBadPoints++;
+        }
         points[i]->SetId(pointMaker.Next());
         cnet.AddPoint(points[i]);
         points[i] = 0;
@@ -197,31 +212,48 @@ MatcherSolutionList MatchMaker::match(const RobustMatcherList &matchers) {
         // other place to do it.
         delete points[i];
         points[i] = 0;
-        nBad++;
+        nBadPoints++;
       }
     }
   }
 
   cnetinfo += PvlKeyword("ImagesMatched", toString(nImages) );
   cnetinfo += PvlKeyword("ControlPoints", toString(nPoints) );
   cnetinfo += PvlKeyword("ControlMeasures", toString(nMeasures) );
-  cnetinfo += PvlKeyword("SingleMeasurePoints", toString(nBad) );
+  cnetinfo += PvlKeyword("InvalidIgnoredPoints", toString(nBadPoints) );
+  cnetinfo += PvlKeyword("InvalidIgnoredMeasures", toString(nBadMeasures) );
+  cnetinfo += PvlKeyword("PreserveIgnoredControl", toString(preserve_ignored) );
   if ( isDebug() ) {
     logger() << "  Images Matched:                 " << nImages << "\n";
     logger() << "  ControlPoints created:          " << nPoints << "\n";
     logger() << "  ControlMeasures created:        " << nMeasures << "\n";
-    logger() << "  Excluded single measure points: " << nBad << "\n";
+    logger() << "  InvalidIgnoredPoints:           " << nBadPoints << "\n";
+    logger() << "  InvalidIgnoredMeasures:         " << nBadMeasures << "\n";
+    logger() << "  PreserveIgnoredControl          " << toString(preserve_ignored) << "\n";
     logger().flush();
   }
 
+  // Report measure statistics
+  PvlKeyword mkey = PvlKeyword("ValidPoints", toString(pointStats.ValidPixels()) );
+  mkey.addComment(" -- Valid Point/Measure Statistics ---");
+  cnetinfo += mkey;
+  if ( isDebug() ) {
+      logger() << "\n  -- Valid Point/Measure Statistics -- \n";
+      logger() << "  ValidPoints            " << pointStats.ValidPixels() << "\n";
+  }
+
   if ( pointStats.ValidPixels() > 0 ) {
     cnetinfo += PvlKeyword("MinimumMeasures", toString(pointStats.Minimum()) );
     cnetinfo += PvlKeyword("MaximumMeasures", toString(pointStats.Maximum()) );
+    cnetinfo += PvlKeyword("AverageMeasures", toString(pointStats.Average()) );
     cnetinfo += PvlKeyword("StdDevMeasures", toString(pointStats.StandardDeviation()) );
+    cnetinfo += PvlKeyword("TotalMeasures", toString((int) pointStats.Sum()) );
     if ( isDebug() ) {
       logger() << "  MinimumMeasures:       " << pointStats.Minimum() << "\n";
       logger() << "  MaximumMeasures:       " << pointStats.Maximum() << "\n";
+      logger() << "  AverageMeasures:       " << pointStats.Average() << "\n";
       logger() << "  StdDevMeasures:        " << pointStats.StandardDeviation() << "\n";
+      logger() << "  TotalMeasures:         " << (int) pointStats.Sum() << "\n";
       logger().flush();
     }
   }
@@ -256,9 +288,11 @@ int MatchMaker::addMeasure(ControlPoint **cpt, const MatchPair &mpair,
     (*cpt)->SetRefMeasure(reference);
 
     // Set lat/lon if requested for Query image
-    if ( Query == m_geomFlag ) {
-      // What to do when it fails??
-      (void) setAprioriLatLon(*(*cpt), *reference, mpair.query());
+    if ( (Query == m_geomFlag) || (Both == m_geomFlag) ) {
+      // We'll set the reference to ignore if this fails
+      if ( !setAprioriLatLon(*(*cpt), *reference, mpair.query() ) ) {
+        reference->SetIgnored( true );
+      }
     }
     nMade++;
   }
@@ -298,8 +332,19 @@ int MatchMaker::addMeasure(ControlPoint **cpt, const MatchPair &mpair,
 
     // Set lat/lon if requested for train image
     if ( Train == m_geomFlag ) {
-      // What to do when it fails??
-      (void) setAprioriLatLon(*(*cpt), *trainpt, mpair.train() );
+      // If it fails, ignore the point
+      if ( !setAprioriLatLon(*(*cpt), *trainpt, mpair.train() ) ) {
+        (*cpt)->SetIgnored( true );
+      }
+    }
+    else if ( Both == m_geomFlag ) {
+      // Check for valid ground mapping.
+      SurfacePoint latlon = getSurfacePoint( *trainpt, mpair.train() );
+
+      // If it fails, ignore the measure
+      if ( !latlon.Valid() ) {
+        trainpt->SetIgnored( true );
+      }
     }
 
     nMade++;
@@ -325,20 +370,26 @@ ControlMeasure *MatchMaker::makeMeasure(const MatchImage &image,
   return ( v_measure.take() );
 }
 
-bool MatchMaker::setAprioriLatLon(ControlPoint &point,
-                                  const ControlMeasure &measure,
-                                  const MatchImage &image) const {
+SurfacePoint MatchMaker::getSurfacePoint( const ControlMeasure &measure, 
+                                         const MatchImage &image) const {
   // Check if the source has geometry
-  if ( !image.source().hasGeometry() ) { return (false); }
+  if ( !image.source().hasGeometry() ) { return (SurfacePoint()); }
 
   // Compute lat/lon
   double samp = measure.GetSample();
   double line = measure.GetLine();
   SurfacePoint latlon = image.source().getLatLon(line,samp);
 
-  // std::cout << "  Lat/Lon Coordinate: " << latlon.GetLatitude().degrees() << ", "
-  //           << latlon.GetLongitude().degrees() << "\n";
-  point.SetAprioriSurfacePoint(latlon);
+  return (latlon);
+}
+
+bool MatchMaker::setAprioriLatLon(ControlPoint &point, 
+                                  const ControlMeasure &measure, 
+                                  const MatchImage &image) const {
+  SurfacePoint latlon = getSurfacePoint(measure, image);
+  if ( latlon.Valid() )  { // Only set if its valid
+    point.SetAprioriSurfacePoint(latlon);
+  }
   return ( latlon.Valid() );
 }