GroupsMatcher.hpp

/* $Id$ */

#ifndef GFM_GROUPSMATCHER_HPP_
#define GFM_GROUPSMATCHER_HPP_

#include <boost/noncopyable.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/utility.hpp>
#include <boost/serialization/version.hpp>

#include "geom/Rectangle.hpp"
#include "kernel/jafarMacro.hpp"
#include "jmath/jblas.hpp"
#include "fdetect/Descriptor.hpp"
#include "gfm/Matcher.hpp"
#include "gfm/MatchingResult.hpp"
#include "gfm/MatchFocus.hpp"

namespace jafar {
  namespace gfm {
    class GroupsMatcher : public Matcher {
        class Group;
        struct InterestFeatureInfo {
          inline InterestFeatureInfo(fdetect::InterestFeature* iP) : group(0), interestPoint(iP), matchPoint(0) {}
          Group* group;
          fdetect::InterestFeature* interestPoint;
          InterestFeatureInfo* matchPoint;
          //          friend std::ostream& operator <<(std::ostream& s, const GroupsMatcher::InterestFeatureInfo& p_);
        };
        typedef std::vector<InterestFeatureInfo*> vInterestFeatureInfos;
        typedef vInterestFeatureInfos::iterator vInterestFeatureInfos_it;
        typedef vInterestFeatureInfos::const_iterator vInterestFeatureInfos_cit;
        typedef std::map<double, InterestFeatureInfo*> double2interestPointInfo;
        typedef std::pair<InterestFeatureInfo*, InterestFeatureInfo*> pIpInfoIpInfo;
        typedef std::vector<pIpInfoIpInfo> vpIpInfoIpInfos;
        class Group {
          public:
            struct GroupMember {
              double distance, angle;
              InterestFeatureInfo* interestPointInfo;
              //friend std::ostream& operator <<(std::ostream& s, const GroupsMatcher::Group::GroupMember& p_);
            };
            typedef std::vector<GroupMember> vGroupMembers;
          public:
            Group(double2interestPointInfo::iterator itbegin, double2interestPointInfo::iterator itend )
            {
              // Insert interrestpoints in the group
              for(double2interestPointInfo::iterator it = itbegin; it != itend; ++it)
              {
                GroupMember gm;
                gm.interestPointInfo = it->second;
                m_members.push_back(gm);
              }
              // Compute the distance and the angle
              jblas::vec2 origin = at(0).interestPointInfo->interestPoint->point();
              at(0).angle = 0.;
              at(0).distance = 0.;
              for(vGroupMembers::iterator it = begin() + 1; it != end(); ++it)
              {
                jblas::vec2 v = it->interestPointInfo->interestPoint->point() - origin;
                it->distance = ublas::norm_2(v);
                it->angle = atan2( v(0), v(1));
              }
            }
            inline GroupMember pivot() { return m_members[0]; }
            inline GroupMember& at(int index) { return m_members[index]; }
            inline vGroupMembers::iterator begin() { return m_members.begin(); }
            inline vGroupMembers::iterator end() { return m_members.end(); }
            inline unsigned int size() { return m_members.size(); }
  friend std::ostream& operator <<(std::ostream& s, const Group& p_);
          private:
            vGroupMembers m_members;
        };
        typedef std::vector<Group*> vGroups;
        typedef vGroups::iterator vGroups_it;
        typedef std::pair<Group::GroupMember, Group::GroupMember> pGrpMembGrMemb;
        typedef std::vector< pGrpMembGrMemb > vpGrpMembGrMembs;
        typedef vpGrpMembGrMembs::iterator vpGrpMembGrMembs_it;
        struct GroupMatchHypothesis {
          inline GroupMatchHypothesis()  {}
          double matchingStrength;
          double sumRotation;
          double sumScale;
          double sumTranslationU;
          double sumTranslationV;
          double angleErr;
          int count;
          geom::RectangleDouble rect;
          GroupsMatcher::Group* groupref;
          GroupsMatcher::Group* groupmatch;
          vpGrpMembGrMembs individualmatches;
        };
        class GroupsMatchSourceInfo : public MatchSourceInfo, public boost::noncopyable {
          public:
            GroupsMatchSourceInfo( jafar::fdetect::DetectionResult points, GroupsMatcher* gm);
            ~GroupsMatchSourceInfo();
            virtual void reinit();
            inline vGroups groups() { return m_groups; }
            inline const vInterestFeatureInfos& pointsInfo() { return m_pointsInfo; }
  friend std::ostream& operator <<(std::ostream& s, const GroupsMatchSourceInfo& p_);
          private:
            vGroups m_groups;
            vInterestFeatureInfos m_pointsInfo;
        };
      public:
        GroupsMatcher(MatchFocus* matchFocus = 0, 
                      unsigned int groupsize = 12, 
                      unsigned int groupsizeseedsearch = 12, 
                      unsigned int minimalgroupsize = 2, 
                      double distance = 20.0, 
                      double closeness = 0.6, 
                      double weakcloseness = 0.6, 
                      double distinguability = 0.1, 
                      double rotationTolerance = 0.2, 
                      double scaleTolerance = 10000.0, 
                      double distanceTolerance = 10.0, 
                      double rotationDifferenceWithMedian = 0.4, 
                      double thresholdofenoughtgroup = 0. ) :
          m_matchFocus(matchFocus),
          m_groupSize(groupsize),
          m_groupSizeSeedSearch(groupsizeseedsearch),
          m_minimalGroupSize(minimalgroupsize),
          m_distance(distance),
          m_closeness(closeness),
          m_weakCloseness(weakcloseness),
          m_distinguability(distinguability),
          m_rotationTolerance(rotationTolerance),
          m_scaleTolerance(scaleTolerance),
          m_distanceTolerance(distanceTolerance),
          m_rotationDifferenceWithMedian(rotationDifferenceWithMedian),
          m_thresholdOfEnoughtGroup(thresholdofenoughtgroup),
          m_rotationTolerancePropagation(rotationTolerance)
        { }
        GroupsMatcher(GroupsMatcher const& gm);
        GroupsMatcher& operator=(GroupsMatcher const& m) { if (&m != this) *this = *(new GroupsMatcher(m)); return *this; }
        virtual ~GroupsMatcher();
      public:
        virtual jafar::gfm::MatchingResult computeMatch( MatchSourceInfo* refsourceinfo, MatchSourceInfo* matchsourceinfo);
      virtual MatchSourceInfo* initMatchSourceInfo( const jafar::fdetect::DetectionResult& pointsource );
      public:
        void setMatchFocus(MatchFocus* matchFocus);
      private:
        GroupMatchHypothesis* searchBestMatchFor( Group* groupref, vGroups& groupsmatch );
        void propageMatching( vInterestFeatureInfos& matches, InterestFeatureInfo* startingpoint , GroupMatchHypothesis* hypothesis);
        void initiatePropagation(vInterestFeatureInfos& matches, GroupMatchHypothesis* match, vpGrpMembGrMembs individualmatches);
#ifndef SWIG
        JFR_DEFINE_PARAM_RO(MatchFocus*, matchFocus)
        JFR_DEFINE_PARAM_RW(unsigned int, groupSize, setGroupSize)
        JFR_DEFINE_PARAM_RW(unsigned int, groupSizeSeedSearch, setGroupSizeSeedSearch)
        JFR_DEFINE_PARAM_RW(unsigned int, minimalGroupSize, setMinimalGroupSize)
        JFR_DEFINE_PARAM_RW(double, distance, setDistance)
        JFR_DEFINE_PARAM_RW(double, closeness, setCloseness)
        JFR_DEFINE_PARAM_RW(double, weakCloseness, setWeakCloseness)
        JFR_DEFINE_PARAM_RW(double, distinguability, setDistinguability)
        JFR_DEFINE_PARAM_RW(double, rotationTolerance, setRotationTolerance)
        JFR_DEFINE_PARAM_RW(double, scaleTolerance, setScaleTolerance)
        JFR_DEFINE_PARAM_RW(double, distanceTolerance, setDistanceTolerance)
        JFR_DEFINE_PARAM_RW(double, rotationDifferenceWithMedian, setRotationDifferenceWithMedian)
        JFR_DEFINE_PARAM_RW(double, thresholdOfEnoughtGroup, setThresholdOfEnoughtGroup)
        JFR_DEFINE_PARAM_RW(double, rotationTolerancePropagation, setRotationTolerancePropagation)
#endif
      private:
        // Variable use for debugging and mark as to be removed in the final version
        int md_currentnumberofmatch;

  friend std::ostream& operator <<(std::ostream& s, const GroupsMatcher::InterestFeatureInfo& p_);
  friend std::ostream& operator <<(std::ostream& s, const GroupsMatcher::Group::GroupMember& p_);
  friend std::ostream& operator <<(std::ostream& s, const GroupsMatcher::Group& p_);
  friend std::ostream& operator <<(std::ostream& s, const GroupsMatcher::GroupsMatchSourceInfo& p_);
      friend class boost::serialization::access;
      template<class Archive>
      void serialize(Archive &ar, const unsigned int version)
      {
        ar & BOOST_SERIALIZATION_NVP(m_matchFocus);
        ar & BOOST_SERIALIZATION_NVP(m_groupSize);
        ar & BOOST_SERIALIZATION_NVP(m_groupSizeSeedSearch);
        ar & BOOST_SERIALIZATION_NVP(m_minimalGroupSize);
        ar & BOOST_SERIALIZATION_NVP(m_distance);
        ar & BOOST_SERIALIZATION_NVP(m_closeness);
        ar & BOOST_SERIALIZATION_NVP(m_weakCloseness);
        ar & BOOST_SERIALIZATION_NVP(m_distinguability);
        ar & BOOST_SERIALIZATION_NVP(m_rotationTolerance);
        ar & BOOST_SERIALIZATION_NVP(m_scaleTolerance);
        ar & BOOST_SERIALIZATION_NVP(m_distanceTolerance);
        ar & BOOST_SERIALIZATION_NVP(m_rotationDifferenceWithMedian);
        ar & BOOST_SERIALIZATION_NVP(m_thresholdOfEnoughtGroup);
        ar & BOOST_SERIALIZATION_NVP(m_rotationTolerancePropagation);
      }
    };
  }

  namespace gfm_v2 {
    template<typename InterestFeatureT>
    class GroupsMatcher : public Matcher<InterestFeatureT> {
      class Group;
      //struct InterestFeatureInfo
      struct InterestFeatureInfo {
        inline InterestFeatureInfo(InterestFeatureT* iP) : group(0), interestPoint(iP), matchPoint(0) {}
        Group* group;
        InterestFeatureT* interestPoint;
        InterestFeatureInfo* matchPoint;
        //        friend std::ostream& operator <<(std::ostream& s, 
        //                               const InterestFeatureInfo& p_);
      };
      //typedefs
      typedef std::vector< InterestFeatureInfo* > vInterestFeatureInfos;
      typedef typename vInterestFeatureInfos::iterator vInterestFeatureInfos_it;
      typedef typename vInterestFeatureInfos::const_iterator vInterestFeatureInfos_cit;
      typedef std::map<double, InterestFeatureInfo* > double2interestPointInfo;
      typedef typename double2interestPointInfo::iterator double2interestPointInfo_it;
      typedef std::pair<InterestFeatureInfo*, InterestFeatureInfo*> pIpInfoIpInfo;
      typedef std::vector<pIpInfoIpInfo> vpIpInfoIpInfos;
      typedef MatchSourceInfo<InterestFeatureT> MatchSourceInfoType;
      typedef jafar::fdetect_v2::DetectionResult< InterestFeatureT > DetectionResult;
      typedef jafar::gfm_v2::MatchingResult<InterestFeatureT> MatchingResult;
      
      class Group {
      public:
        struct GroupMember {
          double distance, angle;
          InterestFeatureInfo* interestPointInfo;
          //          friend std::ostream& operator <<(std::ostream& s, const GroupMember& p_);
        };
        typedef std::vector< GroupMember > vGroupMembers;
      public:
        Group(double2interestPointInfo_it itbegin, double2interestPointInfo_it itend )
        {
          // Insert interrestpoints in the group
          for(double2interestPointInfo_it it = itbegin; it != itend; ++it)
          {
            GroupMember gm;
            gm.interestPointInfo = it->second;
            m_members.push_back(gm);
          }
          // Compute the distance and the angle
          jblas::vec2 origin = at(0).interestPointInfo->interestPoint->point();
          at(0).angle = 0.;
          at(0).distance = 0.;
          for(typename vGroupMembers::iterator it = begin() + 1; it != end(); ++it)
          {
            jblas::vec2 v = it->interestPointInfo->interestPoint->point() - origin;
            it->distance = ublas::norm_2(v);
            it->angle = atan2( v(0), v(1));
          }
        }
        inline GroupMember pivot() { return m_members[0]; }
        inline GroupMember& at(int index) { return m_members[index]; }
        inline typename vGroupMembers::iterator begin() { return m_members.begin(); }
        inline typename vGroupMembers::iterator end() { return m_members.end(); }
        inline unsigned int size() { return m_members.size(); }
        //friend std::ostream& operator <<(std::ostream& s, const Group& p_);
      private:
        vGroupMembers m_members;
      };
      typedef std::vector<Group*> vGroups;
      typedef typename vGroups::iterator vGroups_it;
      typedef std::pair<typename Group::GroupMember, typename Group::GroupMember> pGrpMembGrMemb;
      typedef std::vector< pGrpMembGrMemb > vpGrpMembGrMembs;
      typedef typename vpGrpMembGrMembs::iterator vpGrpMembGrMembs_it;

      struct GroupMatchHypothesis {
        inline GroupMatchHypothesis()  {}
        double matchingStrength;
        double sumRotation;
        double sumScale;
        double sumTranslationU;
        double sumTranslationV;
        double angleErr;
        int count;
        geom::RectangleDouble rect;
        Group* groupref;
        Group* groupmatch;
        vpGrpMembGrMembs individualmatches;
      };

      class GroupsMatchSourceInfo : public MatchSourceInfoType, public boost::noncopyable {
      public:
        GroupsMatchSourceInfo( DetectionResult points, GroupsMatcher* gm);
        ~GroupsMatchSourceInfo();
        virtual void reinit();
        inline vGroups groups() { return m_groups; }
        inline const vInterestFeatureInfos& pointsInfo() { return m_pointsInfo; }
        //        friend std::ostream& operator <<(std::ostream& s, const GroupsMatchSourceInfo& p_);
      private:
        vGroups m_groups;
        vInterestFeatureInfos m_pointsInfo;
      };
      public:
        GroupsMatcher(gfm::MatchFocus* matchFocus = 0, 
                      unsigned int groupsize = 12, 
                      unsigned int groupsizeseedsearch = 12, 
                      unsigned int minimalgroupsize = 2, 
                      double distance = 20.0, 
                      double closeness = 0.6, 
                      double weakcloseness = 0.6, 
                      double distinguability = 0.1, 
                      double rotationTolerance = 0.2, 
                      double scaleTolerance = 10000.0, 
                      double distanceTolerance = 10.0, 
                      double rotationDifferenceWithMedian = 0.4, 
                      double thresholdofenoughtgroup = 0. ) :
          m_matchFocus(matchFocus),
          m_groupSize(groupsize),
          m_groupSizeSeedSearch(groupsizeseedsearch),
          m_minimalGroupSize(minimalgroupsize),
          m_distance(distance),
          m_closeness(closeness),
          m_weakCloseness(weakcloseness),
          m_distinguability(distinguability),
          m_rotationTolerance(rotationTolerance),
          m_scaleTolerance(scaleTolerance),
          m_distanceTolerance(distanceTolerance),
          m_rotationDifferenceWithMedian(rotationDifferenceWithMedian),
          m_thresholdOfEnoughtGroup(thresholdofenoughtgroup),
          m_rotationTolerancePropagation(rotationTolerance)
      { }
      GroupsMatcher(GroupsMatcher const& gm);
      
      GroupsMatcher& operator=(GroupsMatcher const& m) { if (&m != this) *this = *(new GroupsMatcher(m)); return *this; }
      virtual ~GroupsMatcher();
    public:
      virtual MatchingResult computeMatch( MatchSourceInfoType* refsourceinfo, MatchSourceInfoType* matchsourceinfo);
      virtual MatchSourceInfoType* initMatchSourceInfo( const DetectionResult& pointsource );
    public:
      void setMatchFocus(gfm::MatchFocus* matchFocus);
    private:
      GroupMatchHypothesis* searchBestMatchFor( Group* groupref, vGroups& groupsmatch );
      void propageMatching( vInterestFeatureInfos& matches, InterestFeatureInfo* startingpoint , GroupMatchHypothesis* hypothesis);
      void initiatePropagation(vInterestFeatureInfos& matches, GroupMatchHypothesis* match, vpGrpMembGrMembs individualmatches);
#ifndef SWIG
      JFR_DEFINE_PARAM_RO(gfm::MatchFocus*, matchFocus)
        JFR_DEFINE_PARAM_RW(unsigned int, groupSize, setGroupSize)
        JFR_DEFINE_PARAM_RW(unsigned int, groupSizeSeedSearch, setGroupSizeSeedSearch)
        JFR_DEFINE_PARAM_RW(unsigned int, minimalGroupSize, setMinimalGroupSize)
        JFR_DEFINE_PARAM_RW(double, distance, setDistance)
        JFR_DEFINE_PARAM_RW(double, closeness, setCloseness)
        JFR_DEFINE_PARAM_RW(double, weakCloseness, setWeakCloseness)
        JFR_DEFINE_PARAM_RW(double, distinguability, setDistinguability)
        JFR_DEFINE_PARAM_RW(double, rotationTolerance, setRotationTolerance)
        JFR_DEFINE_PARAM_RW(double, scaleTolerance, setScaleTolerance)
        JFR_DEFINE_PARAM_RW(double, distanceTolerance, setDistanceTolerance)
        JFR_DEFINE_PARAM_RW(double, rotationDifferenceWithMedian, setRotationDifferenceWithMedian)
        JFR_DEFINE_PARAM_RW(double, thresholdOfEnoughtGroup, setThresholdOfEnoughtGroup)
        JFR_DEFINE_PARAM_RW(double, rotationTolerancePropagation, setRotationTolerancePropagation)
#endif
      private:
        // Variable use for debugging and mark as to be removed in the final version
        int md_currentnumberofmatch;

      // friend std::ostream& operator <<(std::ostream& s, const GroupsMatcher::InterestFeatureInfo& p_);
      // friend std::ostream& operator <<(std::ostream& s, const GroupsMatcher::Group::GroupMember& p_);
      // friend std::ostream& operator <<(std::ostream& s, const GroupsMatcher::Group& p_);
      // friend std::ostream& operator <<(std::ostream& s, const GroupsMatcher::GroupsMatchSourceInfo& p_);
      friend class boost::serialization::access;
      template<class Archive>
      void serialize(Archive &ar, const unsigned int version)
      {
        ar & BOOST_SERIALIZATION_NVP(m_matchFocus);
        ar & BOOST_SERIALIZATION_NVP(m_groupSize);
        ar & BOOST_SERIALIZATION_NVP(m_groupSizeSeedSearch);
        ar & BOOST_SERIALIZATION_NVP(m_minimalGroupSize);
        ar & BOOST_SERIALIZATION_NVP(m_distance);
        ar & BOOST_SERIALIZATION_NVP(m_closeness);
        ar & BOOST_SERIALIZATION_NVP(m_weakCloseness);
        ar & BOOST_SERIALIZATION_NVP(m_distinguability);
        ar & BOOST_SERIALIZATION_NVP(m_rotationTolerance);
        ar & BOOST_SERIALIZATION_NVP(m_scaleTolerance);
        ar & BOOST_SERIALIZATION_NVP(m_distanceTolerance);
        ar & BOOST_SERIALIZATION_NVP(m_rotationDifferenceWithMedian);
        ar & BOOST_SERIALIZATION_NVP(m_thresholdOfEnoughtGroup);
        ar & BOOST_SERIALIZATION_NVP(m_rotationTolerancePropagation);
      }
    };
  }
}

#include "gfm/GroupsMatcher.hxx"
#endif