LEMUR Packages: ompl_lemur or_lemur pr_bgl prpy_lemur
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Groups Pages
RoadmapRGGDens.h
Go to the documentation of this file.
1 
7 /* requires:
8 #include <ompl_lemur/SamplerGenMonkeyPatch.h>
9 */
10 
11 namespace ompl_lemur
12 {
13 
14 // for now this is an r-disk prm,
15 // uniform milestone sampling with given seed,
16 // uses the space's default sampler
17 template <class RoadmapArgs>
18 class RoadmapRGGDens : public Roadmap<RoadmapArgs>
19 {
20  typedef boost::graph_traits<typename RoadmapArgs::Graph> GraphTypes;
21  typedef typename GraphTypes::vertex_descriptor Vertex;
22  typedef typename GraphTypes::edge_descriptor Edge;
23 
24  // set on construction
25  unsigned int _dim;
27 
28  // params
29  unsigned int _num_per_batch;
30  double _radius_first_batch;
31  unsigned int _seed;
32  bool _seed_set;
33 
34  // set on initialization
35  double _gamma;
37 
38 public:
40  Roadmap<RoadmapArgs>(args, "RGGDens", 0),
41  _dim(0),
42  _bounds(0),
43  _num_per_batch(0),
44  _radius_first_batch(0.0),
45  _seed(0),
46  _seed_set(false),
47  _sampler(this->space->allocStateSampler())
48  {
49  // check that we're in a real vector state space
50  if (this->space->getType() != ompl::base::STATE_SPACE_REAL_VECTOR)
51  throw std::runtime_error("NewRoadmapRGGDens only supports rel vector state spaces!");
52  _dim = this->space->getDimension();
53  if (0 == ompl_lemur::util::get_prime(_dim-1))
54  throw std::runtime_error("not enough primes hardcoded!");
55  ompl::base::StateSpacePtr myspace(this->space);
56  _bounds = myspace->as<ompl::base::RealVectorStateSpace>()->getBounds();
57 
58  this->template declareParam<unsigned int>("num_per_batch", this,
61  this->template declareParam<double>("radius_first_batch", this,
62  &RoadmapRGGDens::setRadiusFirstBatch,
63  &RoadmapRGGDens::getRadiusFirstBatch);
64  this->template declareParam<unsigned int>("seed", this,
65  &RoadmapRGGDens::setSeed,
66  &RoadmapRGGDens::getSeed);
67  }
68 
69  void setNumPerBatch(unsigned int num_per_batch)
70  {
71  if (num_per_batch == _num_per_batch)
72  return;
73  if (this->initialized)
74  throw std::runtime_error("cannot set num_per_batch, already initialized!");
75  _num_per_batch = num_per_batch;
76  }
77 
78  unsigned int getNumPerBatch() const
79  {
80  return _num_per_batch;
81  }
82 
83  void setRadiusFirstBatch(double radius_first_batch)
84  {
85  if (radius_first_batch == _radius_first_batch)
86  return;
87  if (this->initialized)
88  throw std::runtime_error("cannot set radius_first_batch, already initialized!");
89  _radius_first_batch = radius_first_batch;
90  }
91 
92  double getRadiusFirstBatch() const
93  {
94  return _radius_first_batch;
95  }
96 
97  void setSeed(unsigned int seed)
98  {
99  if (_seed_set && seed == _seed)
100  return;
101  if (this->initialized)
102  throw std::runtime_error("cannot set seed, already initialized!");
103  _seed = seed;
104  _seed_set = true;
105  }
106 
107  unsigned int getSeed() const
108  {
109  return _seed;
110  }
111 
112  void initialize()
113  {
114  std::vector<std::string> missings;
115  if (_num_per_batch == 0)
116  missings.push_back("num_per_batch");
117  if (_radius_first_batch == 0.0)
118  missings.push_back("radius_first_batch");
119  if (!_seed_set)
120  missings.push_back("seed");
121  if (missings.size())
122  {
123  std::string str = "Cannot initialize, parameters not set:";
124  for (unsigned int ui=0; ui<missings.size(); ui++)
125  str += " " + missings[ui];
126  throw std::runtime_error(str);
127  }
128 
129  _gamma = _radius_first_batch / pow(log(_num_per_batch)/_num_per_batch, 1./_dim);
130  printf("RoadmapRGGDens calculated gamma=%f\n", _gamma);
131 
132  ompl_lemur::StateSamplerSetSeed(_sampler, _seed);
133 
134  this->initialized = true;
135  }
136 
137  void deserialize(const std::string & ser_data)
138  {
139  std::stringstream ss(ser_data);
140  ss >> ompl_lemur::SamplerGenMonkeyPatch(_sampler);
141  }
142 
143  // should be stateless
144  double root_radius(std::size_t i_batch)
145  {
146  unsigned int n = (1 + i_batch) * _num_per_batch;
147  return _gamma * pow(log(n)/n, 1./_dim);
148  }
149 
150  // sets all of these maps
151  // generates one additional batch
152  void generate()
153  {
154  // compute radius
155  double radius = root_radius(this->num_batches_generated);
156  std::size_t n = (this->num_batches_generated+1) * _num_per_batch;
157  for (std::size_t v_index=num_vertices(this->g); v_index<n; v_index++)
158  {
159  Vertex v_new = add_vertex(this->g);
160 
161  put(this->vertex_batch_map, v_new, this->num_batches_generated);
162  put(this->is_shadow_map, v_new, false);
163 
164  // allocate a new state for this vertex
165  put(this->state_map, v_new, this->space->allocState());
166  ompl::base::State * v_state = get(this->state_map, v_new);
167  _sampler->sampleUniform(v_state);
168  this->nn->add(v_new);
169 
170  // allocate new undirected edges
171  std::vector<Vertex> vs_near;
172  this->nn->nearestR(v_new, radius, vs_near);
173  for (unsigned int ui=0; ui<vs_near.size(); ui++)
174  {
175  if (vs_near[ui] == v_new)
176  continue;
177  Edge e = add_edge(v_new, vs_near[ui], this->g).first;
178  ompl::base::State * vnear_state = get(this->state_map,vs_near[ui]);
179  put(this->distance_map, e, this->space->distance(v_state,vnear_state));
180  put(this->edge_batch_map, e, this->num_batches_generated);
181  }
182  }
183  this->num_batches_generated++;
184  }
185 
186  void serialize(std::string & ser_data)
187  {
188  std::stringstream ss;
189  ss << ompl_lemur::SamplerGenMonkeyPatch(_sampler);
190  ser_data = ss.str();
191  }
192 };
193 
194 } // namespace ompl_lemur
void serialize(std::string &ser_data)
Serialize the internal generator state into the passed string.
Definition: RoadmapRGGDens.h:186
void initialize()
Initialize roadmap; must be called once after setting parameters.
Definition: RoadmapRGGDens.h:112
void log(const char *file, int line, LogLevel level, const char *m,...)
void deserialize(const std::string &ser_data)
Re-constitute the internal generator state from serialized data.
Definition: RoadmapRGGDens.h:137
void generate()
Generates one additional batch.
Definition: RoadmapRGGDens.h:152
double root_radius(std::size_t i_batch)
Calcuate the root radius to be used for connecting to potential root vertices.
Definition: RoadmapRGGDens.h:144
Interface for generating roadmaps over OMPL state spaces into Boost Graph objects.
Definition: Roadmap.h:76
Definition: RoadmapRGGDens.h:18
Definition: Roadmap.h:11