quadMap.h

Go to the documentation of this file.

#ifndef DLR_COMPUTERVISION_QUADMAP_H
#define DLR_COMPUTERVISION_QUADMAP_H

#include <functional>
#include <vector>


namespace dlr {

  namespace computerVision {

    template <class Type>
    class QuadMapComparator
      : public std::binary_function<Type, Type, bool>
    {
    public:
      
      QuadMapComparator() {}
      
      
      virtual
      ~QuadMapComparator() {}
      

      unsigned int
      chooseChild(Type const& element,
                  double border0,
                  double border1) const {
#if 1
        unsigned int quadrantIndex = 0;
        quadrantIndex |= (element[0] < border0) ? 0x00 : 0x01;
        quadrantIndex |= (element[1] < border1) ? 0x00 : 0x02;
        return quadrantIndex;
#else
        if(element[1] < border1) {
          if
            return 0;
          }
          return 1;
        }
        if(element[0] < border0) {
          return 2;
        }
        return 3;
#endif
      }

      
      double
      computeDistanceSquared(Type const& arg0, Type const& arg1) const {
        double term0 = arg1[0] - arg0[0];
        double term1 = arg1[1] - arg0[1];
        return term0 * term0 + term1 * term1;
      }


      void
      getLowerBoundSquaredDistances(Type const& element,
                                    double boundary0,
                                    double boundary1,
                                    double& minDistance0,
                                    double& minDistance1,
                                    double& minDistance01) const {
        double term0 = element[0] - boundary0;
        double term1 = element[1] - boundary1;
        minDistance0 = term0 * term0;
        minDistance1 = term1 * term1;
        minDistance01 = minDistance0 + minDistance1;
      }


      bool isEqual(Type const& arg0, Type const& arg1) const {
        return arg0 == arg1;
      }
      
    };


    
    template <class Type>
    class QuadMap {
    public:

      QuadMap(double minCoord0, double minCoord1,
              double maxCoord0, double maxCoord1,
              unsigned int maxDepth);


      virtual
      ~QuadMap();


      Type&
      addElement(Type const& element);


      template <class Iter>
      void
      addElements(Iter beginIter, Iter endIter);


      bool
      empty() {return this->isEmpty();}

      
      bool
      find(Type const& element) const;


      Type const&
      findNearest(Type const& element, double& distance) const;


      bool
      isEmpty() {return m_localStore.empty() && (m_childrenPtr == 0);}

      
    protected:

      QuadMap();


      Type&
      addElement(Type* elementPtr);

      
      void
      findNearestIterative(Type const& element,
                           Type const*& bestElementPtr,
                           double& bestDistanceSquared) const;

      
      void
      findNearestRecursive(Type const& element,
                           Type const*& bestElementPtr,
                           double& bestDistanceSquared) const;
      

      void
      setBounds(double minCoord0, double minCoord1,
                double maxCoord0, double maxCoord1,
                unsigned int maxDepth);



      double m_border0;
      double m_border1;
      QuadMap* m_childrenPtr;
      QuadMapComparator<Type> m_comparator;

      // Grr.  Temporarily trapped into using vector here, even though
      // it makes our data structure very clunky.  Will fix this when
      // time permits.
      std::vector<Type*> m_localStore;
      
      double m_maxCoord0;
      double m_maxCoord1;
      unsigned int m_maxDepth;
      double m_minCoord0;
      double m_minCoord1;
    };

  } // namespace computerVision
  
} // namespace dlr


/* ============ Definitions of inline & template functions ============ */

#include <algorithm>
#include <cmath>
#include <limits>
#include <stack>
#include <dlrCommon/exception.h>

namespace dlr {

  namespace computerVision {

    template <class Type>
    QuadMap<Type>::
    QuadMap(double minCoord0, double minCoord1,
            double maxCoord0, double maxCoord1,
            unsigned int maxDepth)
      : m_border0((minCoord0 + maxCoord0) / 2.0),
        m_border1((minCoord1 + maxCoord1) / 2.0),
        m_childrenPtr(0),
        m_comparator(),
        m_localStore(),
        m_maxCoord0(maxCoord0),
        m_maxCoord1(maxCoord1),
        m_maxDepth(maxDepth),
        m_minCoord0(minCoord0),
        m_minCoord1(minCoord1)
    {}


    // The destructor cleans up any system resources during destruction.
    template <class Type>
    QuadMap<Type>::
    ~QuadMap() {
      for(unsigned int ii = 0; ii < m_localStore.size(); ++ii) {
        delete m_localStore[ii];
      }
      if(m_childrenPtr != 0) {
        delete[] m_childrenPtr;
      }
    }


    template <class Type>
    Type&
    QuadMap<Type>::
    addElement(Type const& element)
    {
      Type* elementPtr = new Type(element);
      this->addElement(elementPtr);
      return *elementPtr;
    }
  

    template <class Type>
    template <class Iter>
    void
    QuadMap<Type>::
    addElements(Iter beginIter, Iter endIter)
    {
#if 0
      size_t numberOfElements = endIter - beginIter;
      if(numberOfElements == 0) {
        return;
      }
      if(numberOfElements == 1) {
        this->addElement(*beginIter);
        return;
      }

      // If adding more than one element, dispatch to the recursive,
      // vector-argument addElements method.
      std::vector<Type> elementVector(numberOfElements);
      std::copy(beginIter, endIter, elementVector.begin());
      this->addElements(elementVector, 0, numberOfElements);
#else
      while(beginIter != endIter) {
        this->addElement(*beginIter);
        ++beginIter;
      }
#endif
    }
      
    
    template <class Type>
    bool
    QuadMap<Type>::
    find(Type const& element) const
    {
      // If we have a local element, just check to see if it matches.
      if(!m_localStore.empty()) {
        for(unsigned int ii = 0; ii < m_localStore.size(); ++ii) {
          if(m_comparator.isEqual(*(m_localStore[ii]), element)) {
            return true;
          }
        }
        return false;
      }

      if(m_childrenPtr != 0) {
        unsigned int childIndex = m_comparator.chooseChild(
          element, m_border0, m_border1);
        return m_childrenPtr[childIndex].find(element);
      }

      return false;
    }
    

    template <class Type>
    Type&
    QuadMap<Type>::
    addElement(Type* elementPtr)
    {
      // If we're in a leaf node... a cell of the map that has no
      // children or can't have children, just remember this point.
      if((m_localStore.empty() && m_childrenPtr == 0) || m_maxDepth == 0) {
        m_localStore.push_back(elementPtr);
        return *elementPtr;
      }

      // We're going to pass this element on to one of the children of
      // *this.  If no children exist, create them now.
      if(m_childrenPtr == 0) {
        m_childrenPtr = new QuadMap<Type>[4];
        m_childrenPtr[0].setBounds(
          m_minCoord0, m_minCoord1, m_border0, m_border1, m_maxDepth - 1);
        m_childrenPtr[1].setBounds(
          m_border0, m_minCoord1, m_maxCoord0, m_border1, m_maxDepth - 1);
        m_childrenPtr[2].setBounds(
          m_minCoord0, m_border1, m_border0, m_maxCoord1, m_maxDepth - 1);
        m_childrenPtr[3].setBounds(
          m_border0, m_border1, m_maxCoord0, m_maxCoord1, m_maxDepth - 1);
        for(unsigned int ii = 0; ii < m_localStore.size(); ++ii) {
          unsigned int childIndex = m_comparator.chooseChild(
            *(m_localStore[ii]), m_border0, m_border1);
          m_childrenPtr[childIndex].addElement(m_localStore[ii]);
        }
        m_localStore.clear();
      }

      // Pass the new element to on of the child nodes.
      unsigned int childIndex = m_comparator.chooseChild(
        *elementPtr, m_border0, m_border1);
      return m_childrenPtr[childIndex].addElement(elementPtr);
    }
    
    
    template <class Type>
    Type const&
    QuadMap<Type>::
    findNearest(Type const& element, double& distanceSquared) const
    {
      distanceSquared = std::numeric_limits<double>::max();
      Type const* bestElementPtr = 0;
      // this->findNearestRecursive(element, bestElementPtr, distanceSquared);
      this->findNearestIterative(element, bestElementPtr, distanceSquared);
      if(bestElementPtr == 0) {
        DLR_THROW(common::StateException, "QuadMap::findNearest()",
                  "Can't find nearest element in an empty map.");
      }
      return *bestElementPtr;
    }


    /* ================ Protected ================= */

    template <class Type>
    QuadMap<Type>::
    QuadMap()
      : m_border0(0.0),
        m_border1(0.0),
        m_childrenPtr(0),
        m_comparator(),
        m_localStore(),
        m_maxCoord0(0.0),
        m_maxCoord1(0.0),
        m_minCoord0(0.0),
        m_minCoord1(0.0)
    {}


    template <class Type>
    void
    QuadMap<Type>::
    findNearestIterative(Type const& element,
                         Type const*& bestElementPtr,
                         double& bestDistance) const
    {
      // Contents of this stack are std::pairs in which the first
      // element points to an un-searched map, and the second element
      // is a lower bound on the squared distance from argument
      // element to the elements contained in the un-searched map.
      std::stack< std::pair< QuadMap<Type> const*, double> >
        quadMapStack;
      quadMapStack.push(std::make_pair(this, 0.0));

      while(!(quadMapStack.empty())) {
        QuadMap<Type> const* currentMap = quadMapStack.top().first;
        double bound = quadMapStack.top().second;
        quadMapStack.pop();

        if(bestDistance < bound) {
          continue;
        }

        if(!(currentMap->m_localStore).empty()) {
          for(unsigned int ii = 0; ii < (currentMap->m_localStore).size();
              ++ii) {
            double myDistance = currentMap->m_comparator.computeDistanceSquared(
              element, *(currentMap->m_localStore[ii]));

            // Sanity check.
            if(myDistance < bound) {
              DLR_THROW(LogicException, "QuadMap<Type>::findNearestIterative()",
                        "Bounds calculation is erronious.");
            }
            
            if(myDistance < bestDistance) {
              bestDistance = myDistance;
              bestElementPtr = currentMap->m_localStore[ii];
            }
          }
          continue;
        }

        if(currentMap->m_childrenPtr != 0) {
          // Figure out which child the element belongs in.
          unsigned int nearChildIndex = currentMap->m_comparator.chooseChild(
            element, currentMap->m_border0, currentMap->m_border1);

          // We want to push the farthest child first and the nearest
          // last, so that near child will be popped first.  By
          // putting the appropriate indices in an array, we can do
          // the pushing in a for loop.
          unsigned int childIndices[4];
          childIndices[0] = nearChildIndex ^ 0x03;
          childIndices[1] = nearChildIndex ^ 0x02;
          childIndices[2] = nearChildIndex ^ 0x01;
          childIndices[3] = nearChildIndex;

          // We only want to push a child if it's plausible that it
          // contains a closer element than our best so far.  This
          // means we need to compute min distances to each of the
          // children.  We'll arrange the results in an array
          // corresponding to the index array above.
          double minDistances[4];
          minDistances[3] = bound;
          currentMap->m_comparator.getLowerBoundSquaredDistances(
            element, currentMap->m_border0, currentMap->m_border1,
            minDistances[2], minDistances[1], minDistances[0]);

          // At this point, childIndices[0] is the farthest quadrant,
          // and childIndices[3] is the nearest, but it's just luck as
          // to which of childIndices[1] and childIndices[2] has the
          // smaller minDistance.
          if(minDistances[1] < minDistances[2]) {
            std::swap(childIndices[1], childIndices[2]);
            std::swap(minDistances[1], minDistances[2]);
          }

          // At last we push the children onto the stack so that
          // they'll be searched in-turn.
          for(unsigned int ii = 0; ii < 4; ++ii) {
            if(minDistances[ii] < bestDistance) {
              quadMapStack.push(
                std::make_pair(&(currentMap->m_childrenPtr[childIndices[ii]]),
                               std::max(bound, minDistances[ii])));
            }
          }
        }
      }
    }

      
    template <class Type>
    void
    QuadMap<Type>::
    findNearestRecursive(Type const& element,
                         Type const*& bestElementPtr,
                         double& bestDistance) const
    {
      // Note(xxx): The lower-bound-on-distance-to-element calculation
      // in this function could be more aggressive if we added a
      // "bound" argument, allowing the same kind of check as is done
      // in findNearestIterative().

      if(!m_localStore.empty()) {
        for(unsigned int ii = 0; ii < m_localStore.size(); ++ii) {
          double myDistance = m_comparator.computeDistanceSquared(
            element, *(m_localStore[ii]));
          if(myDistance < bestDistance) {
            bestDistance = myDistance;
            bestElementPtr = m_localStore[ii];
          }
        }
      }

      if(m_childrenPtr) {
        // Figure out which child the element belongs in.
        unsigned int nearChildIndex = m_comparator.chooseChild(
          element, m_border0, m_border1);

        // We want to search the nearest child first and the others
        // later.  By putting the appropriate indices in an array, we
        // can do the searching in a for loop.
        unsigned int childIndices[4];
        childIndices[0] = nearChildIndex;
        childIndices[1] = nearChildIndex ^ 0x01;
        childIndices[2] = nearChildIndex ^ 0x02;
        childIndices[3] = nearChildIndex ^ 0x03;

        // We only want to search a child if it's plausible that it
        // contains a closer element than our best so far.  This
        // means we need to compute min distances to each of the
        // children.  We'll arrange the results in an array
        // corresponding to the index array above.
        double minDistances[4];
        minDistances[0] = 0.0;
        m_comparator.getLowerBoundSquaredDistances(
          element, m_border0, m_border1,
          minDistances[1], minDistances[2], minDistances[3]);
          
        // At this point, childIndices[0] is the farthest quadrant,
        // and childIndices[3] is the nearest, but it's just luck as
        // to which of childIndices[1] and childIndices[2] has the
        // smaller minDistance.
        if(minDistances[1] < minDistances[2]) {
          std::swap(childIndices[1], childIndices[2]);
          std::swap(minDistances[1], minDistances[2]);
        }

        for(unsigned int ii = 0; ii < 4; ++ii) {
          if(minDistances[ii] < bestDistance) {
            m_childrenPtr[childIndices[ii]].findNearestRecursive(
              element, bestElementPtr, bestDistance);
          }
        }
      }
    }


    template <class Type>
    void
    QuadMap<Type>::
    setBounds(double minCoord0, double minCoord1,
              double maxCoord0, double maxCoord1,
              unsigned int maxDepth)
    {
      m_border0 = (minCoord0 + maxCoord0) / 2.0;
      m_border1 = (minCoord1 + maxCoord1) / 2.0;
      m_minCoord0 = minCoord0;
      m_minCoord1 = minCoord1;
      m_maxCoord0 = maxCoord0;
      m_maxCoord1 = maxCoord1;
      m_maxDepth = maxDepth;
    }
    
  } // namespace computerVision
  
} // namespace dlr

#endif /* #ifndef DLR_COMPUTERVISION_QUADMAP_H */