首页 文章

提升隐式图和astar_search_no_init

提问于
浏览
2

我想实现机器人的路径规划子系统 . 我将使用来自boost库的A * .

我需要隐式图 . 我必须使用 astar_search_no_init 函数(它写在文档中) . 不幸的是,我找不到使用 astar_search_no_init 和隐式图的示例 .

我找到了"A* Graph Search Within the BGL Framework" . 作者使用 astar_search 作为隐式图 . 他试图在访客的 examine_vertex 方法中添加顶点 . 但是在增强1.49中是不可能的,因为图形在此方法中作为常量引用传递 .

谁能帮我?

2 回答

  • 7

    我刚刚提出了一个隐式图的工作示例 . 该图也是无限的 . 我刚把它发布给了用户,但我也把它放在这里,所以很容易找到 .

    /**
     * Example use of boost::astar_search_no_init on an infinite, implicitly-defined graph.
     *
     * The graph type used here is XYGraph, representing an infinite grid of squares.  Each
     * square is connected to its eight neighbors; however, the example shows how to use
     * boost::filtered_graph to make the search take place only along orthogonal edges.
     */
    
    #include <iostream>
    #include <list>
    #include <map>
    #include <set>
    #include <utility>
    
    #include <boost/graph/graph_traits.hpp>
    #include <boost/graph/astar_search.hpp>
    #include <boost/graph/filtered_graph.hpp>
    #include <boost/operators.hpp>
    #include <boost/ref.hpp>
    
    namespace Direction
    {
    enum id
    {
        MIN = 0,
        N = MIN, S, E, W, NW, NE, SE, SW, NONE
    };
    }
    
    struct XY : public boost::additive<XY,
        boost::totally_ordered<XY,
        boost::equivalent<XY>
        > >
    {
        typedef int X;
        typedef int Y;
    
        XY(X x = 0, Y y = 0);
    
        // Same square counts.
        bool adjacentTo(XY const& that) const;
    
        XY & operator=(XY const& that);
        XY & operator+=(XY const& that);
    
        bool operator<(XY const& that) const;
    
        X x;
        Y y;
    
        XY neighbor(Direction::id direction) const;
        std::set<XY> allNeighbors() const;
    };
    
    std::ostream & operator<<(std::ostream & os, XY const& xy);
    
    struct neighbor_iterator;
    
    /*
     * Model of:
     *  * Graph
     *  * IncidenceGraph
     */
    struct XYGraph
    {
        XYGraph();
    
        // Graph concept requirements
        typedef XY                                vertex_descriptor;
        typedef std::pair<XY, XY>                 edge_descriptor;
        typedef boost::undirected_tag             directed_category;
        typedef boost::disallow_parallel_edge_tag edge_parallel_category;
        typedef boost::incidence_graph_tag        traversal_category;
    
        // IncidenceGraph concept requirements
        typedef neighbor_iterator          out_edge_iterator;
        typedef int                        degree_size_type;
    };
    
    namespace boost
    {
        template <> struct graph_traits<XYGraph>
        {
            typedef XYGraph G;
    
            typedef G::vertex_descriptor      vertex_descriptor;
            typedef G::edge_descriptor        edge_descriptor;
            typedef G::out_edge_iterator      out_edge_iterator;
    
            typedef G::directed_category      directed_category;
            typedef G::edge_parallel_category edge_parallel_category;
            typedef G::traversal_category     traversal_category;
    
            typedef G::degree_size_type       degree_size_type;
    
            typedef void in_edge_iterator;
            typedef void vertex_iterator;
            typedef void vertices_size_type;
            typedef void edge_iterator;
            typedef void edges_size_type;
        };
    }
    
    // IncidenceGraph concept requirements
    std::pair<XYGraph::out_edge_iterator, 
    XYGraph::out_edge_iterator> out_edges(XYGraph::vertex_descriptor v, XYGraph const& g);
    XYGraph::degree_size_type out_degree(XYGraph::vertex_descriptor v, XYGraph const& g);
    XYGraph::vertex_descriptor source(XYGraph::edge_descriptor e, XYGraph const& g);
    XYGraph::vertex_descriptor target(XYGraph::edge_descriptor e, XYGraph const& g);
    
    // Iterator
    struct neighbor_iterator : 
        public boost::iterator_facade<neighbor_iterator,
                                      std::pair<XY,XY>,
                                      boost::forward_traversal_tag,
                                      std::pair<XY,XY> >
    {
    public:
        neighbor_iterator();
        neighbor_iterator(XY xy, Direction::id direction);
    
        neighbor_iterator & operator=(neighbor_iterator const& that);
    
        std::pair<XY,XY> operator*() const;
        neighbor_iterator& operator++();
        bool operator==(neighbor_iterator const& that) const;
    
        bool equal(neighbor_iterator const& that) const { return operator==(that); }
        void increment() { operator++(); }
    
    private:
        XY xy;
        Direction::id direction;
    };
    
    
    // Filter used to traverse grid only along orthogonal (non-diagonal) edges.
    struct orthogonal_only
    {
        typedef std::pair<XY,XY> Edge;
        bool operator()(Edge const& edge) const
        {
            return edge.first.x == edge.second.x || edge.first.y == edge.second.y;
        }
    };
    
    template <typename Graph> class distance_heuristic;
    
    struct found_goal {}; // exception for termination
    
    // visitor that terminates when we find the goal
    class astar_goal_visitor : public boost::default_astar_visitor
    {
    public:
        astar_goal_visitor(XY goal) : m_goal(goal) {}
    
        void examine_vertex(XY xy, XYGraph const& g) {
            (void)g;
            std::cout << "Exploring " << xy << "..." << std::endl;
            if(xy == m_goal)
                throw found_goal();
        }
        void examine_vertex(XY xy, boost::filtered_graph<XYGraph, orthogonal_only> const& g) {
            (void)g;
            std::cout << "Exploring " << xy << "..." << std::endl;
            if(xy == m_goal)
                throw found_goal();
        }
    private:
        XY m_goal;
    };
    
    template <typename K, typename V>
    class default_map
    {
    public:
        typedef K key_type;
        typedef V data_type;
        typedef std::pair<K,V> value_type;
    
        default_map(V const& defaultValue)
            : m()
            , defaultValue(defaultValue)
        {}
    
        V & operator[](K const& k)
        {
            if (m.find(k) == m.end())
            {
                m[k] = defaultValue;
            }
            return m[k];
        }
    
    private:
        std::map<K,V> m;
        V const defaultValue;
    };
    
    struct PredecessorMap
    {
        PredecessorMap() : m() {}
        PredecessorMap(PredecessorMap const& that) : m(that.m) {}
    
        typedef XY key_type;
        typedef XY value_type;
        typedef XY & reference_type;
        typedef boost::read_write_property_map_tag category;
    
        XY & operator[](XY xy) { return m[xy]; }
    
        std::map<XY,XY> m;
    };
    
    XY get(PredecessorMap const& pm, XY xy)
    {
        std::map<XY,XY>::const_iterator found = pm.m.find(xy);
        return (found != pm.m.end()) ? found->second : xy;
    }
    
    void put(PredecessorMap & pm, XY key, XY value)
    {
        pm.m[key] = value;
    }
    
    // Euclidean distance heuristic (square root omitted)
    template <typename Graph>
    class distance_heuristic : public boost::astar_heuristic<Graph, int>
    {
    public:
        distance_heuristic(XY goal)
            : m_goal(goal) {}
        unsigned operator()(XY xy)
        {
            int dx = m_goal.x - xy.x;
            int dy = m_goal.y - xy.y;
            unsigned retval = static_cast<unsigned>(dx * dx + dy * dy);
            return retval;
        }
    private:
        XY m_goal;
    };
    
    int main()
    {
        XYGraph baseGraph;
        boost::filtered_graph<XYGraph, orthogonal_only> g(baseGraph, orthogonal_only());
        //BOOST_CONCEPT_ASSERT((IncidenceGraphConcept< boost::filtered_graph<XYGraph, orthogonal_only> >));
    
        XY start(0,0);
        XY goal(5,7);
    
        std::cout << "Start vertex: " << start << std::endl;
        std::cout << "Goal vertex: " << goal << std::endl;
    
        PredecessorMap p;
        typedef boost::associative_property_map< default_map<XY,unsigned> > DistanceMap;
        typedef default_map<XY,unsigned> WrappedDistanceMap;
        WrappedDistanceMap wrappedMap = WrappedDistanceMap(std::numeric_limits<unsigned>::max());
        wrappedMap[start] = 0;
        DistanceMap d = DistanceMap(wrappedMap);
        auto weight_map = default_map<std::pair<XY,XY>,unsigned>(1);
        auto vertex_index_map = std::map<XY,unsigned>();
        auto rank_map = std::map<XY,unsigned>();
        auto color_map = std::map<XY,boost::default_color_type>();
    
        try {
            astar_search_no_init(g, 
                start,
                distance_heuristic<XYGraph>(goal)
                , visitor(astar_goal_visitor(goal))
                . distance_map(d)
                . predecessor_map(boost::ref(p))
                . weight_map(boost::associative_property_map< default_map<std::pair<XY,XY>,unsigned> >(weight_map))
                . vertex_index_map(boost::associative_property_map< std::map<XY,unsigned> >(vertex_index_map))
                . rank_map(boost::associative_property_map< std::map<XY,unsigned> >(rank_map))
                . color_map(boost::associative_property_map< std::map<XY,boost::default_color_type> >(color_map))
                . distance_compare(std::less<unsigned>())
                . distance_combine(std::plus<unsigned>())
                );
        } catch(found_goal const&) { // found a path to the goal
            std::list<XY> shortest_path;
            for(XY xy = goal;; xy = p[xy]) {
                shortest_path.push_front(xy);
                if(p[xy] == xy)
                    break;
            }
            std::cout << "Shortest path from " << start << " to "
                << goal << ": ";
            std::list<XY>::iterator spi = shortest_path.begin();
            std::cout << start;
            for(++spi; spi != shortest_path.end(); ++spi) 
                std::cout << " -> " << (*spi);
            std::cout << std::endl;
            return 0;
        }
    
        std::cout << "Didn't find a path from " << start << "to"
            << goal << "!" << std::endl;
        return 0;
    }
    
    XYGraph::XYGraph()
    {}
    
    std::pair<XYGraph::out_edge_iterator, XYGraph::out_edge_iterator> 
    out_edges(XYGraph::vertex_descriptor v,
              XYGraph const& g)
    {
        (void)g;
        return std::make_pair(
            XYGraph::out_edge_iterator(v, Direction::MIN), 
            XYGraph::out_edge_iterator(v, Direction::NONE) );
    }
    
    XYGraph::degree_size_type 
    out_degree(XYGraph::vertex_descriptor v,
               XYGraph const& g)
    {
        (void)g;
        return v.allNeighbors().size();
    }
    
    XYGraph::vertex_descriptor 
    source(XYGraph::edge_descriptor e,
           XYGraph const& g)
    {
        (void)g;
        return e.first;
    }
    
    XYGraph::vertex_descriptor target(
        XYGraph::edge_descriptor e,
        XYGraph const& g)
    {
        (void)g;
        return e.second;
    }
    
    neighbor_iterator::neighbor_iterator() : xy() , direction() { }
    
    neighbor_iterator::neighbor_iterator(XY xy, Direction::id direction)
    : xy(xy)
    , direction(direction)
    {
    }
    
    neighbor_iterator & neighbor_iterator::operator=(neighbor_iterator const& that)
    {
        xy = that.xy;
        direction = that.direction;
        return *this;
    }
    
    std::pair<XY,XY> neighbor_iterator::operator*() const
    {
        std::pair<XY,XY> const retval = std::make_pair(xy, xy.neighbor(direction));
        return retval;
    }
    
    neighbor_iterator& neighbor_iterator::operator++()
    {
        direction = static_cast<Direction::id>(int(direction) + 1);
        return *this;
    }
    
    bool neighbor_iterator::operator==(neighbor_iterator const& that) const
    {
        return xy == that.xy && direction == that.direction;
    }
    
    
    XY::XY(X x, Y y)
    : x(x)
    , y(y)
    {
    }
    
    bool XY::adjacentTo(XY const& that) const
    {
        return abs(x - that.x) <= 1 && abs(y - that.y) <= 1;
    }
    
    XY & XY::operator=(XY const& that)
    {
        x = that.x;
        y = that.y;
        return *this;
    }
    
    XY & XY::operator+=(XY const& that)
    {
        x += that.x;
        y += that.y;
        return *this;
    }
    
    bool XY::operator<(XY const& that) const
    {
        return x < that.x || (x == that.x && y < that.y);
    }
    
    std::ostream & operator<<(std::ostream & os, XY const& xy)
    {
        os << "(" << xy.x << "," << xy.y << ")";
        return os;
    }
    
    XY XY::neighbor(Direction::id direction) const
    {
        using namespace Direction;
    
        int dx = 0, dy = 0;
        switch (direction)
        {
        case NW:
        case W:
        case SW:
            dx = -1;
            break;
        case NE:
        case E:
        case SE:
            dx = 1;
            break;
        default:
            dy = 0;
        }
        switch (direction)
        {
        case NW:
        case N:
        case NE:
            dy = -1;   
            break;
        case SW:
        case S:
        case SE:
            dy = 1;
            break;
        default:
            dy = 0;
        }
        XY const neighbor(x + dx, y + dy);
        return neighbor;
    }
    
    std::set<XY> XY::allNeighbors() const
    {
        std::set<XY> neighbors;
    
        for (int dx = -1; dx <= 1; ++dx)
            for (int dy = -1; dy <= 1; ++dy)
                neighbors.insert(XY(x+dx,y+dy));
    
        return neighbors;
    }
    
  • 0

    如果图搜索是您唯一的目标,您可以试用YAGSBPL库:http://code.google.com/p/yagsbpl/ . 使用起来要简单得多,而且开销更快,因为它的开销很小 .

相关问题