AtpTileSource.cpp

Go to the documentation of this file.

//**************************************************************************************************
//
//     OSSIM Open Source Geospatial Data Processing Library
//     See top level LICENSE.txt file for license information
//
//**************************************************************************************************

#include "AtpTileSource.h"
#include "../AtpCommon.h"
#include "AtpGenerator.h"
#include <ossim/imaging/ossimImageDataFactory.h>
#include <ossim/base/ossimVisitor.h>
#include <ossim/imaging/ossimImageRenderer.h>
#include <ossim/elevation/ossimElevManager.h>

using namespace std;

namespace ATP
{
double AtpTileSource::s_minVectorResDiff = 0;
double AtpTileSource::s_maxDiffRatio = 1.0;
double AtpTileSource::s_cosMaxAngleDiff = 0;
double AtpTileSource::s_maxPaxDev = 1.0;
double AtpTileSource::s_maxPaxPix = 2.0;
unsigned int AtpTileSource::s_minNumConsistent = 3;
unsigned int AtpTileSource::s_percentConsistent = 50;
unsigned int AtpTileSource::s_numTpsPerTile = 2;
unsigned int AtpTileSource::s_numFilterIterations = 0;
bool   AtpTileSource::s_useRasterMode = false;
bool   AtpTileSource::s_initialized = false;

AtpTileSource::AtpTileSource()
:  m_considerParallax (true)
{
}

AtpTileSource::AtpTileSource(ossimConnectableObject::ConnectableObjectList& inputSources)
:  ossimImageCombiner(inputSources),
   m_considerParallax (true)

{
}

AtpTileSource::AtpTileSource(AtpGenerator* generator)
:  m_generator(generator),
   m_considerParallax (true)
{
   theComputeFullResBoundsFlag = true;
   if (m_generator)
   {
      connectMyInputTo(0, m_generator->getRefChain().get());
      connectMyInputTo(1, m_generator->getCmpChain().get());
   }
   addListener((ossimConnectableObjectListener*)this);
   initialize();
}

void AtpTileSource::initialize()
{
   const char* MODULE = "AtpTileSource::initialize() -- ";
   if (!s_initialized)
      initializeStaticMembers();

   ossimImageCombiner::initialize();
   m_tiePoints.clear();
   if ( getNumberOfInputs() < 2)
      return;
}

void AtpTileSource::allocate()
{
   // The AtpTileSources cannot be relied on to return valid pixel data, since their main product
   // are the tie points, not pixels. However, the tile is used to communicate scalar type, rect,
   // and other tile metadata, so is still needed:
   m_tile = NULL;
   if ( getNumberOfInputs() > 1)
   {
      ossimImageSource* firstSource = dynamic_cast<ossimImageSource*>(theInputObjectList[0].get());
      m_tile = ossimImageDataFactory::instance()->create(this, firstSource);
      m_tile->initialize();
   }
}

void AtpTileSource::filterPoints()
{
   const char* MODULE = "AtpTileSource::filterPoints() -- ";
   AtpConfig& config = AtpConfig::instance();

   // First remove points with no matches:
   auto tiePoint = m_tiePoints.begin();
   while (tiePoint != m_tiePoints.end())
   {
      if ((*tiePoint) && (*tiePoint)->hasValidMatch())
         ++tiePoint;
      else
         tiePoint = m_tiePoints.erase(tiePoint);
   }

   // No filtering done for raster mode:
   if (s_useRasterMode)
      return;

   if (config.diagnosticLevel(3))
   {
      // Annotate red before filtering:
      m_generator->m_annotatedRefImage->annotateResiduals(m_tiePoints, 255, 0, 0);
      m_generator->m_annotatedCmpImage->annotateCorrelations(m_tiePoints, 255, 0, 0);
   }

   // Check for consistency check override
   if (s_minNumConsistent == 0)
      return;

   // Don't accept any if there are an insufficient number of neighbors:
   if (m_tiePoints.size() < s_minNumConsistent)
   {
      m_tiePoints.clear();
      return;
   }

   if (config.paramExists("considerParallax"))
      m_considerParallax = config.getParameter("considerParallax").asBool();
   if (m_considerParallax)
   {
      // If the perspective differences are small, fall back to non-parallax check:
      computeParallax();
      if ((fabs(m_paxInfo.dx_dH) < 1.0) && (fabs(m_paxInfo.dy_dH) < 1.0))
      {
         if (config.diagnosticLevel(5))
            CINFO << MODULE << "Insignificant parallax, falling back to non-parallax filtering."
                  << endl;
         filterWithoutParallax();
      }
      else
      {
         s_numFilterIterations = 0;
         filterWithParallax();
      }
   }
   else
      filterWithoutParallax();

   if (config.diagnosticLevel(3))
   {
      // Annotate yellow before pruning:
      m_generator->m_annotatedRefImage->annotateResiduals(m_tiePoints, 255, 255, 0);
      m_generator->m_annotatedCmpImage->annotateCorrelations(m_tiePoints, 255, 255, 0);
   }

   pruneList();

   if (config.diagnosticLevel(2))
   {
      CINFO<<MODULE<<"After filtering & pruning: num TPs in tile = "<<m_tiePoints.size()<<endl;
      if (config.diagnosticLevel(3))
      {
         // Annotate Green after accepting TP:
         m_generator->m_annotatedRefImage->annotateResiduals(m_tiePoints, 0, 255, 0);
         m_generator->m_annotatedCmpImage->annotateCorrelations(m_tiePoints, 0, 255, 0);
         //m_annotatedRefImage->annotateTPIDs(m_tiePoints, 180, 180, 0);
      }
   }
}

void AtpTileSource::filterWithParallax()
{
   // May be called recursively, limited by s_numFilterIterations < 5

   const char* MODULE = "AtpTileSource::filterWithParallax()  ";
   AtpConfig& config = AtpConfig::instance();
   if (m_tiePoints.size() < 2)
   {
      m_tiePoints.clear();
      return;
   }

   computeParallaxStatistics();

   // This is where recursion is stopped:
   if ((s_numFilterIterations == 5) && (m_paxInfo.maxDistance > s_maxPaxPix))
   {
      m_tiePoints.clear();
      if (config.diagnosticLevel(5))
      {
         CINFO << MODULE << "Could not establish good parallax statistics for this set. "
               << "Removing all points for this tile." << endl;
      }
      return;
   }

   // Loop to eliminate bad peaks and inconsistent ATPs in tile:
   ossimDpt residual;
   bool statsChanged = false;
   auto tiePoint = m_tiePoints.begin();
   while (tiePoint != m_tiePoints.end())
   {
      // Check point's active peak against the set of residuals along parallax:
      auto entry = m_paxInfo.distanceMap.find((*tiePoint)->getTiePointId());
      if (entry == m_paxInfo.distanceMap.end())
      {  // Should never happen
         tiePoint = m_tiePoints.erase(tiePoint);
         continue;
      }
      double distance = entry->second;

      // Loop to find a match in this ATP that is consistent.
      while (distance > m_paxInfo.maxDistance)
      {
         // This residual is outside the acceptable limit, try the next match in the tiepoint or
         // remove it if it has no more peaks.
         statsChanged = true;
         if (!(*tiePoint)->bumpActiveMatch())
         {
            if (config.diagnosticLevel(5))
            {
               CINFO << MODULE << "Removing TP " << (*tiePoint)->getTiePointId() << endl;
               CINFO << "   distance: " << distance << ",  maxDistance: " << m_paxInfo.maxDistance << endl;
            }
            tiePoint = m_tiePoints.erase(tiePoint);
            break;
         }
         else if (config.diagnosticLevel(5))
         {
            CINFO << MODULE << "Bumping active match for TP " << (*tiePoint)->getTiePointId()
                  << endl;
            CINFO << "   distance: " << distance << ",  maxDistance: " << m_paxInfo.maxDistance << endl;
         }

         // A new peak is being used, will need to recompute statistics later.
         (*tiePoint)->getVectorResidual(residual);
         if (m_paxInfo.dx_dH == 0.0)
            distance = residual.x;
         else if (m_paxInfo.dx_dH == 0.0)
            distance = residual.y;
         else
         {
            distance = fabs(m_paxInfo.parallaxOffset + m_paxInfo.parallaxSlope * residual.x -
                            residual.y) / m_paxInfo.denom;
         }
      }
      if (distance <= m_paxInfo.maxDistance)
      {
         // The match was deemed consistent. proceed to next point:
         ++tiePoint;
      }
   }

   if (statsChanged)
   {
      // Recursive call to recompute statistics and eliminate suspect points
      s_numFilterIterations++;
      if (config.diagnosticLevel(5))
      {
         CINFO << MODULE << "Iterating filterWithParallax after ATP list change (N = "
               << s_numFilterIterations << ")."<< endl;
      }
      filterWithParallax();
   }
}

void AtpTileSource::computeParallax()
{
   // Use the point at tile center to establish reference ground point at two different elevations:
   static const double DELTA_HEIGHT = 500.0;
   ossimDpt vpt, ipt0, ipt1;
   ossimGpt gpt0, gpt1;

   // Possible debug mode will not have a tile assigned:
   if (!m_tile || (m_tile->getDataObjectStatus() == OSSIM_STATUS_UNKNOWN))
       m_tiePoints[0]->getRefViewPoint(vpt);
   else
      m_tile->getImageRectangle().getCenter(vpt);
   ossimImageGeometry *geom = m_generator->m_refIVT->getViewGeometry();
   geom->localToWorld(vpt, gpt0);
   if (gpt0.isHgtNan())
      gpt0.hgt = ossimElevManager::instance()->getHeightAboveEllipsoid(gpt0);
   gpt1 = gpt0;
   gpt1.hgt += DELTA_HEIGHT;

   // Determine dx/dh and dy/dh for ref image:
   geom = m_generator->m_refIVT->getImageGeometry();
   geom->worldToLocal(gpt0, ipt0);
   geom->worldToLocal(gpt1, ipt1);
   double dx_ref = (ipt1.x - ipt0.x);
   double dy_ref = (ipt1.y - ipt0.y);

   // Determine dx/dh and dy/dh for cmp image:
   geom = m_generator->m_cmpIVT->getImageGeometry();
   geom->worldToLocal(gpt0, ipt0);
   geom->worldToLocal(gpt1, ipt1);
   double dx_cmp = (ipt1.x - ipt0.x);
   double dy_cmp = (ipt1.y - ipt0.y);

   // Compute parallax direction REF-->CMP
   m_paxInfo.dx_dH = dx_cmp - dx_ref;
   m_paxInfo.dy_dH = dy_cmp - dy_ref;
   if (m_paxInfo.dx_dH != 0)
   {
      m_paxInfo.parallaxSlope = m_paxInfo.dy_dH / m_paxInfo.dx_dH;
      m_paxInfo.denom = sqrt(m_paxInfo.parallaxSlope * m_paxInfo.parallaxSlope + 1.0);
   }
   else
   {
      m_paxInfo.parallaxSlope == ossim::nan();
      m_paxInfo.denom = ossim::nan();
   }
   //CINFO << "### dx_dH: " << m_paxInfo.dx_dH << endl; // TODO REMOVE
   //CINFO << "### dy_dH: " << m_paxInfo.dy_dH << endl; // TODO REMOVE
   //CINFO << "### parallaxSlope: " << m_paxInfo.parallaxSlope << endl; // TODO REMOVE
}


void AtpTileSource::computeParallaxStatistics()
{
   // Determining the parallax direction (slope) is easy. It is the offset (y-intercept) that
   // depends on the collection of residuals.
   // Use the point at tile center to establish dx/dH and dy/dH in view-space:
   // Compute parallax slope and intercept:
   ossimDpt residual;
   if (m_paxInfo.dx_dH != 0.0)
   {
      // Compute parallax intercept using collection of all primary matches:
      double sum_c_i = 0;
      for (auto &tiePoint : m_tiePoints)
      {
         tiePoint->getVectorResidual(residual);
         sum_c_i += residual.y - m_paxInfo.parallaxSlope * residual.x;
      }
      m_paxInfo.parallaxOffset = sum_c_i/m_tiePoints.size();
   }

   // Compute statistics of residual vector distribution, saving individual distance samples:
   m_paxInfo.distanceMap.clear();
   double distance, sumDistance2=0.0;
   for (auto &tiePoint : m_tiePoints)
   {
      tiePoint->getVectorResidual(residual);
      if (m_paxInfo.dx_dH == 0)
         distance = residual.x;
      else if (m_paxInfo.dy_dH == 0)
         distance = residual.y;
      else
      {
         distance = fabs(m_paxInfo.parallaxOffset + m_paxInfo.parallaxSlope*residual.x - residual.y)/m_paxInfo.denom;
      }
      sumDistance2 += distance*distance;
      m_paxInfo.distanceMap.emplace(tiePoint->getTiePointId(), distance);
      //CINFO << "### TP: "<<tiePoint->getTiePointId()<<"  distance: " << distance << endl; // TODO REMOVE

   }
   double sigmaDistance = sqrt(sumDistance2/(m_tiePoints.size()-1));
   m_paxInfo.maxDistance = s_maxPaxDev*sigmaDistance;
   if (m_paxInfo.maxDistance < s_maxPaxPix)
      m_paxInfo.maxDistance = s_maxPaxPix;

   //CINFO << "### sigmaDistance: " << sigmaDistance << endl; // TODO REMOVE
   //CINFO << "### maxDistance: " << m_paxInfo.maxDistance << endl; // TODO REMOVE
}

void AtpTileSource::filterWithoutParallax()
{
   const char* MODULE = "AtpTileSource::filterWithoutParallax()  ";
   AtpConfig& config = AtpConfig::instance();

   // Loop to eliminate bad peaks and inconsistent ATPs in tile:
   auto tiePoint = m_tiePoints.begin();
   while (tiePoint != m_tiePoints.end())
   {
      // The "tiepoint" may not have any matches -- it may be just a feature:
      ossimDpt r_atp, r_nbr;
      (*tiePoint)->getVectorResidual(r_atp);

      if (config.diagnosticLevel(5))
      {
         ossimDpt refPt;
         (*tiePoint)->getRefImagePoint(refPt);
         CINFO<<"AutoTiePoint::checkConsistency() -- Processing TP "<<(*tiePoint)->getTiePointId()
              <<" at REF image pt: ("<<refPt.x<<", "<<refPt.y<<")"<<endl;
      }

      // Loop over all of the TPs neighbors to count the number that are consistent:
      unsigned int num_consistent_neighbors = 0;
      for(auto &neighbor : m_tiePoints)
      {
         if ((*tiePoint)->getTiePointId() == neighbor->getTiePointId())
            continue;

         // Check for simple minimum vector difference:
         neighbor->getVectorResidual(r_nbr);
         double diff = (r_nbr - r_atp).length();
         if (diff < s_minVectorResDiff)
         {
            num_consistent_neighbors++;;
            continue;
         }

         // Compute angle and magnitude ratio between residuals. Note no check for valid peak:
         double cos_theta=1.0, mag_ratio=0;
         double r0 = r_atp.length();
         double r1 = r_nbr.length();
         mag_ratio = 2.0*fabs(r1-r0)/(r0+r1);
         if ((r1 > 0.0) && (r0 > 0.0))
            cos_theta = (r_atp.x*r_nbr.x + r_atp.y*r_nbr.y)/(r0*r1);

         // Test for consistency with this neighbor's peak:
         if ((mag_ratio < s_maxDiffRatio) && (cos_theta > s_cosMaxAngleDiff))
            num_consistent_neighbors++;;
      }

      // Decide if this peak was good or out there:
      int minConsistent = (int) ceil(s_percentConsistent*(m_tiePoints.size()-1)/100);
      if (minConsistent < (int)s_minNumConsistent)
         minConsistent = s_minNumConsistent;
      if (num_consistent_neighbors < minConsistent)
      {
         // Peak was bad, fetch next strongest match or exit loop if no more available:
         if (!(*tiePoint)->bumpActiveMatch())
         {
            if (config.diagnosticLevel(5))
               CINFO << MODULE << "Removing TP " << (*tiePoint)->getTiePointId() << endl;
            tiePoint = m_tiePoints.erase(tiePoint);
            if (m_tiePoints.empty())
               break;
         }
      }
      else
      {
         // Match is consistent, keep the tiepoint in the list an advance to the next tiepoint
         tiePoint++;
      }
   }
}


void AtpTileSource::pruneList()
{
   const char* MODULE = "AtpTileSource::pruneList()  ";
   AtpConfig &config =  AtpConfig::instance();

   // Use a map to sort by confidence measure:
   multimap<double, shared_ptr<AutoTiePoint> > atpMap;
   auto atp = m_tiePoints.begin();
   double confidence;
   while (atp != m_tiePoints.end())
   {
      if (!(*atp)) // Should never happen
      {
         CWARN<<MODULE<<"WARNING: Null AutoTiePoint encountred in list. Skipping point."<<endl;
         ++atp;
         continue;
      }

      // The "tiepoint" may not have any matches -- it may be just a feature:
      bool hasValidMatch = (*atp)->getConfidenceMeasure(confidence);
      if (hasValidMatch)
         atpMap.emplace(1.0/confidence, *atp);

      atp++;
   }

   // Skim off the top best:
   m_tiePoints.clear();
   multimap<double, shared_ptr<AutoTiePoint> >::iterator tp = atpMap.begin();
   while (tp != atpMap.end())
   {
      m_tiePoints.push_back(tp->second);
      if (m_tiePoints.size() == s_numTpsPerTile)
         break;
      tp++;
   }
}

void AtpTileSource::initializeStaticMembers()
{
   static const char* MODULE = "AtpTileSource::initializeStaticMembers() -- ";

   AtpConfig& config = AtpConfig::instance();
   s_maxDiffRatio = config.getParameter("maxResMagDiffRatio").asFloat();
   if (ossim::isnan(s_maxDiffRatio) || (s_maxDiffRatio == 0))
   {
      CWARN<<MODULE<<"Bad or missing parameter in config: "
              "maxResMagDiffRatio = "<<s_maxDiffRatio<<". Defaulting to 0.1."<<endl;
      s_maxDiffRatio = 0.1;
   }

   double theta = config.getParameter("maxResAngleDiff").asFloat();
   if (ossim::isnan(theta) || (theta <= 0))
   {
      CWARN<<MODULE<<"Bad or missing parameter in config: "
              "maxAngleDiff = "<<theta<<". Defaulting to 10 deg."<<endl;
      theta = 10.0;
   }
   s_cosMaxAngleDiff = ossim::cosd(theta);

   s_minVectorResDiff = config.getParameter("minVectorResDiff").asFloat();
   if (ossim::isnan(s_minVectorResDiff) || (s_minVectorResDiff < 1.0))
   {
      CWARN<<MODULE<<"Bad or missing parameter in config: "
              "minVectorResDiff = "<<s_minVectorResDiff<<". Defaulting to 1.0."<<endl;
      s_minVectorResDiff = 1.0;
   }

   s_minNumConsistent = config.getParameter("minNumConsistentNeighbors").asUint();
   if (s_minNumConsistent == 0)
   {
      CWARN<<MODULE<<"Bad or missing parameter in config: "
              "minNumConsistentNeighbors = "<<s_minNumConsistent<<". Defaulting to 3."<<endl;
      s_minNumConsistent = 3.0;
   }

   s_percentConsistent = config.getParameter("percentConsistentThreshold").asUint();
   if (s_percentConsistent == 0)
   {
      CWARN<<MODULE<<"Bad or missing parameter in config: "
              "percentConsistentThreshold = "<<s_percentConsistent<<". Defaulting to 50."<<endl;
      s_percentConsistent = 50;
   }

   s_numTpsPerTile = config.getParameter("numTiePointsPerTile").asUint();
   if (s_numTpsPerTile == 0)
   {
      CWARN<<MODULE<<"Bad or missing parameter in config: "
              "numTiePointsPerTile = "<<s_numTpsPerTile<<". Defaulting to 2."<<endl;
      s_numTpsPerTile = 2;
   }

   s_maxPaxDev = config.getParameter("maxResParallaxDev").asFloat();
   if (ossim::isnan(s_maxPaxDev) || (s_maxPaxDev <= 0))
   {
      CWARN<<MODULE<<"Bad or missing parameter in config: "
              "maxResParallaxDev = "<<s_maxPaxDev<<". Defaulting to 1.0."<<endl;
      s_maxPaxDev = 1.0;
   }

   s_maxPaxPix = (double) config.getParameter("maxResParallaxPix").asUint();
   if (ossim::isnan(s_maxPaxPix) || (s_maxPaxPix <= 0))
   {
      CWARN<<MODULE<<"Bad or missing parameter in config: "
              "maxResParallaxPix = "<<s_maxPaxPix<<". Defaulting to 1.0."<<endl;
      s_maxPaxPix = 2.0;
   }

   if (config.paramExists("useRasterMode"))
      s_useRasterMode = config.getParameter("useRasterMode").asBool();

   s_initialized = true;
}

void AtpTileSource::setTiePoints(ossim::TiePointList& tpList)
{
   // Engineering code for manually specifying TPs to test consistency checking
   string id;
   ossimDpt rip, cip;
   NEWMAT::SymmetricMatrix cov;
   m_tiePoints.clear();
   for (auto &tp : tpList)
   {
      shared_ptr<AutoTiePoint> atp (new AutoTiePoint(m_generator, tp->getTiePointId()));
      tp->getImagePoint(0,id,rip,cov);
      tp->getImagePoint(1,id,cip,cov);
      atp->setRefImagePt(rip);
      atp->addImageMatch(cip);
      m_tiePoints.emplace_back(atp);
   }
}

}