TopologicalMap.h

Go to the documentation of this file.

#ifndef PMPL_TOPOLOGICAL_MAP_H_
#define PMPL_TOPOLOGICAL_MAP_H_
 
#include "MPLibrary/MPBaseObject.h"
#include "Utilities/Hash.h"
#include "Utilities/SSSP.h"
#include "Utilities/XMLNode.h"
#include "Workspace/GridOverlay.h"
#include "Workspace/WorkspaceDecomposition.h"
 
#include "glutils/obj_file.h"
#include "glutils/triangulated_model.h"
 
//    Topological Nearest-Neighbor Filtering". Under review for RA-L @ IROS 20.
//    - and -
//    Read Sandstrom. "Approximating Configuration Space Topology with Workspace
//    Models". PhD Thesis, May 2020.
class TopologicalMap final : public MPBaseObject {
 public:
 
  typedef typename MPBaseObject::WeightType WeightType;
  typedef typename MPBaseObject::RoadmapType RoadmapType;
  typedef typename RoadmapType::VID VID;
  typedef typename RoadmapType::VI VI;
  typedef typename RoadmapType::VertexSet VertexSet;
 
  typedef std::vector<const WorkspaceRegion*> NeighborhoodKey;
 
  typedef std::unordered_map<const WorkspaceRegion*, double> DistanceMap;
 
  typedef std::map<const WorkspaceRegion*, VertexSet> OccupancyMap;
 
 
  TopologicalMap();
  TopologicalMap(double gridSize, string decomp_label) {
    this->m_gridSize = gridSize;
    this->m_decompositionLabel = decomp_label;
  }
  TopologicalMap(XMLNode& _node);
 
  virtual ~TopologicalMap();
 
 
  virtual void Initialize() override;
 
 
  const VertexSet* GetMappedVIDs(RoadmapType* const _r,
                                 const WorkspaceRegion* const _region,
                                 const size_t _bodyIndex = 0) const;
 
  const WorkspaceRegion* GetMappedRegion(RoadmapType* const _r,
                                         const VID _vid,
                                         const size_t _bodyIndex = 0) const;
 
  bool IsPopulated(RoadmapType* const _r,
                   const WorkspaceRegion* const _region,
                   const size_t _bodyIndex = 0) const;
 
 
  const WorkspaceRegion* GetRandomRegion() const;
 
  const WorkspaceRegion* LocateRegion(RoadmapType* const _r,
                                      const VID _vid,
                                      const size_t _bodyIndex = 0) const;
 
  const WorkspaceRegion* LocateRegion(const Cfg& _c,
                                      const size_t _bodyIndex = 0) const;
 
  const WorkspaceRegion* LocateRegion(const Point3d& _p) const;
 
  const WorkspaceRegion* LocateNearestRegion(const Cfg& _c,
                                             const size_t _bodyIndex = 0) const;
 
  const WorkspaceRegion* LocateNearestRegion(const Point3d& _p) const;
 
  size_t LocateCell(RoadmapType* const _r,
                    const VID _v,
                    const size_t _bodyIndex = 0) const;
 
  size_t LocateCell(const Cfg& _c, const size_t _bodyIndex = 0) const;
 
  size_t LocateCell(const Point3d& _p) const;
 
 
  NeighborhoodKey LocateNeighborhood(RoadmapType* const _r, const VID _v) const;
  NeighborhoodKey LocateNeighborhood(const Cfg& _c) const;  
 
 
  const WorkspaceDecomposition* GetDecomposition() const;
 
 
  double ApproximateMinimumInnerDistance(const WorkspaceRegion* const _source,
                                         const WorkspaceRegion* const _target);
 
  const DistanceMap& ComputeApproximateMinimumInnerDistances(
      const WorkspaceRegion* const _source,
      const double _radius);
 
 
 private:
 
  void EnsureMap(RoadmapType* const _r);
 
  void MapCfg(RoadmapType* const _r, const VI _vertex);
 
  void UnmapCfg(RoadmapType* const _r, const VI _vertex);
 
  void ClearCfgMaps();
 
  const OccupancyMap& GetForwardMap(RoadmapType* const _r,
                                    const size_t _bodyIndex) const;
 
  const NeighborhoodKey& GetInverseMap(RoadmapType* const _r,
                                       const VID _vid) const;
 
 
  std::string m_decompositionLabel;  
 
  std::string m_pqpLabel;  
 
  std::unique_ptr<const GridOverlay> m_grid;
 
  double m_gridSize{.1};  
 
  GridOverlay::DecompositionMap m_cellToRegions;
 
  std::unordered_map<RoadmapType*, std::vector<OccupancyMap>> m_regionToVIDs;
 
  std::unordered_map<RoadmapType*, std::unordered_map<VID, NeighborhoodKey>>
      m_vidToNeighborhood;
 
  std::unordered_set<size_t> m_boundaryCells;
 
  std::unordered_map<const WorkspaceRegion*, DistanceMap> m_innerDistanceMap;
 
};
 
#endif