KdTreeNF.h

Go to the documentation of this file.

#ifndef PMPL_KDTREE_NF_H_
#define PMPL_KDTREE_NF_H_
 
#include "NeighborhoodFinderMethod.h"
 
#include <CGAL/Cartesian_d.h>
#include <CGAL/Search_traits.h>
#include <CGAL/Orthogonal_k_neighbor_search.h>
 
#include <map>
 
 
class KdTreeNF : virtual public NeighborhoodFinderMethod {
 
  public:
 
    typedef typename MPBaseObject::RoadmapType  RoadmapType;
    typedef typename RoadmapType::VI            VI;
    typedef typename RoadmapType::VID           VID;
    typedef typename RoadmapType::VertexSet     VertexSet;
    typedef typename RoadmapType::CCTrackerType CCTrackerType;
    typedef typename MPBaseObject::GroupRoadmapType  GroupRoadmapType;
    typedef typename MPBaseObject::GroupCfgType      GroupCfgType;
 
 
    using typename NeighborhoodFinderMethod::Type;
    using typename NeighborhoodFinderMethod::OutputIterator;
 
 
  private:
 
 
    typedef CGAL::Cartesian_d<double> Kernel;
 
    struct PointD : public Kernel::Point_d {
 
      PointD() : Kernel::Point_d() {}
 
      PointD(const Cfg& _cfg) : PointD(INVALID_VID, _cfg)
      {}
 
      PointD(const VID _vid, const Cfg& _cfg) :
          Kernel::Point_d(_cfg.DOF(), _cfg.GetData().begin(),
              _cfg.GetData().end()), vid(_vid)
      {}
 
      VID vid{INVALID_VID}; 
 
    };
 
    typedef CGAL::Search_traits<Kernel::FT,               // Field type.
            PointD,                                       // Point type.
            Kernel::Cartesian_const_iterator_d,           // Coordinate iterator.
            Kernel::Construct_cartesian_const_iterator_d  // Constructs iterator.
            > TreeTraits;
 
    typedef CGAL::Orthogonal_k_neighbor_search<TreeTraits> NeighborSearch;
 
    typedef typename NeighborSearch::Tree KdTree;
 
    typedef std::pair<RoadmapType*, const VertexSet*> ModelKey;
 
 
  public:
 
 
    KdTreeNF();
 
    KdTreeNF(XMLNode& _node);
 
    virtual ~KdTreeNF() = default;
 
 
    virtual void Initialize() override;
 
    virtual void Print(std::ostream& _os) const override;
 
 
    virtual void FindNeighbors(RoadmapType* const _r, const Cfg& _cfg,
        const VertexSet& _candidates, OutputIterator _out) override;
 
    virtual void FindNeighbors(GroupRoadmapType* const _r,
        const GroupCfgType& _cfg, const VertexSet& _candidates,
        OutputIterator _out) override;
 
 
  private:
 
 
    void SetupModels(RoadmapType* const _r);
 
    KdTree* GetModel(RoadmapType* const _r, const VertexSet& _candidates);
 
 
    double m_epsilon{0.};        
 
    std::map<ModelKey, KdTree> m_trees; 
 
    std::unique_ptr<KdTree> m_tmpTree; 
 
 
};
 
#endif