Robotics Library  0.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros
Prm.h
Go to the documentation of this file.
1 //
2 // Copyright (c) 2009, Markus Rickert
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright notice,
11 // this list of conditions and the following disclaimer in the documentation
12 // and/or other materials provided with the distribution.
13 // * Neither the name of the Technische Universitaet Muenchen nor the names of
14 // its contributors may be used to endorse or promote products derived from
15 // this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 
30 #ifndef _RL_PLAN_PRM_H_
31 #define _RL_PLAN_PRM_H_
32 
33 #include <queue>
34 #include <boost/graph/adjacency_list.hpp>
35 #include <boost/pending/disjoint_sets.hpp>
36 #include <CGAL/Search_traits.h>
37 
39 #include "Planner.h"
40 #include "VectorPtr.h"
41 
42 namespace rl
43 {
44  namespace plan
45  {
46  class Model;
47  class Sampler;
48  class Verifier;
49 
53  class Prm : public Planner
54  {
55  public:
56  Prm();
57 
58  virtual ~Prm();
59 
60  virtual void construct(const ::std::size_t& steps);
61 
62  virtual ::std::string getName() const;
63 
64  ::std::size_t getNumEdges() const;
65 
66  ::std::size_t getNumVertices() const;
67 
68  void getPath(VectorList& path);
69 
70  void reset();
71 
72  bool solve();
73 
75  ::std::size_t degree;
76 
78  ::std::size_t k;
79 
81  bool kd;
82 
85 
87 
89 
90  protected:
91  struct EdgeBundle
92  {
94  };
95 
96  struct GraphBundle;
97 
98  typedef ::boost::adjacency_list_traits<
99  ::boost::listS,
100  ::boost::listS,
101  ::boost::undirectedS,
102  ::boost::listS
103  >::vertex_descriptor Vertex;
104 
106  {
108 
109  ::std::size_t index;
110 
112 
114 
116 
117  ::std::size_t rank;
118  };
119 
120  typedef ::boost::adjacency_list<
121  ::boost::listS,
122  ::boost::listS,
123  ::boost::undirectedS,
124  VertexBundle,
125  EdgeBundle,
127  > Graph;
128 
129  typedef ::std::pair< const ::rl::math::Vector*, Vertex > QueryItem;
130 
132  {
134 
136 
137  const ::rl::math::Real* operator()(const QueryItem& p, const int&) const;
138  };
139 
140  struct Distance
141  {
143 
144  Distance();
145 
146  Distance(Model* model);
147 
148  template< typename SearchTraits > ::rl::math::Real max_distance_to_rectangle(const Query_item& q, const ::CGAL::Kd_tree_rectangle< SearchTraits >& r) const;
149 
150  template< typename SearchTraits > ::rl::math::Real min_distance_to_rectangle(const Query_item& q, const ::CGAL::Kd_tree_rectangle< SearchTraits >& r) const;
151 
152  ::rl::math::Real min_distance_to_rectangle(const ::rl::math::Real& q, const ::rl::math::Real& min, const ::rl::math::Real& max, const ::std::size_t& cutting_dimension) const;
153 
154  ::rl::math::Real new_distance(const ::rl::math::Real& dist, const ::rl::math::Real& old_off, const ::rl::math::Real& new_off, const int& cutting_dimension) const;
155 
157 
158  ::rl::math::Real transformed_distance(const Query_item& q1, const Query_item& q2) const;
159 
161  };
162 
163  typedef ::CGAL::Search_traits< ::rl::math::Real, QueryItem, const ::rl::math::Real*, CartesianIterator > SearchTraits;
164 
166 
167  typedef NeighborSearch::Tree NeighborSearchTree;
168 
169  typedef ::boost::shared_ptr< NeighborSearchTree > NeighborSearchTreePtr;
170 
171  typedef ::std::vector< NeighborSearchTreePtr > NearestNeighbors;
172 
173  struct GraphBundle
174  {
176  };
177 
178  typedef ::boost::graph_traits< Graph >::edge_descriptor Edge;
179 
180  typedef ::boost::graph_traits< Graph >::edge_iterator EdgeIterator;
181 
182  typedef ::std::pair< EdgeIterator, EdgeIterator > EdgeIteratorPair;
183 
184  typedef ::boost::graph_traits< Graph >::vertex_iterator VertexIterator;
185 
186  typedef ::std::pair< VertexIterator, VertexIterator > VertexIteratorPair;
187 
188  typedef ::boost::property_map< Graph, void* VertexBundle::* >::type VertexParentMap;
189 
190  typedef ::boost::property_map< Graph, ::std::size_t VertexBundle::* >::type VertexRankMap;
191 
192  typedef ::std::pair< Vertex, ::rl::math::Real > Neighbor;
193 
194  struct Compare
195  {
196  bool operator()(const Neighbor& x, const Neighbor& y) const;
197  };
198 
199  typedef ::std::priority_queue< Neighbor, ::std::vector< Neighbor >, Compare > NeighborQueue;
200 
201  Edge addEdge(const Vertex& u, const Vertex& v, const ::rl::math::Real& weight);
202 
203  void addPoint(NearestNeighbors& nn, const QueryItem& p);
204 
205  Vertex addVertex(const VectorPtr& q);
206 
207  void insert(const Vertex& vertex);
208 
210 
211  ::boost::disjoint_sets< VertexRankMap, VertexParentMap > ds;
212 
214 
216 
217  private:
218 
219  };
220  }
221 }
222 
223 #endif // _RL_PLAN_PRM_H_