LEMUR Packages: ompl_lemur or_lemur pr_bgl prpy_lemur
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Groups Pages
RoadmapHaltonDens.h
Go to the documentation of this file.
1 
7 namespace ompl_lemur
8 {
9 
10 template <class RoadmapArgs>
11 class RoadmapHaltonDens : public Roadmap<RoadmapArgs>
12 {
13  typedef boost::graph_traits<typename RoadmapArgs::Graph> GraphTypes;
14  typedef typename GraphTypes::vertex_descriptor Vertex;
15  typedef typename GraphTypes::edge_descriptor Edge;
16 
17  // set on construction
18  unsigned int _dim;
20 
21  // params
22  unsigned int _num_per_batch;
23  double _radius_first_batch;
24 
25 public:
27  Roadmap<RoadmapArgs>(args, "HaltonDens", 0),
28  _dim(0),
29  _bounds(0),
30  _num_per_batch(0),
31  _radius_first_batch(0.0)
32  {
33  // check that we're in a real vector state space
34  if (this->space->getType() != ompl::base::STATE_SPACE_REAL_VECTOR)
35  throw std::runtime_error("RoadmapHaltonDens only supports rel vector state spaces!");
36  _dim = this->space->getDimension();
37  if (0 == ompl_lemur::util::get_prime(_dim-1))
38  throw std::runtime_error("not enough primes hardcoded!");
39  ompl::base::StateSpacePtr myspace(this->space);
40  _bounds = myspace->as<ompl::base::RealVectorStateSpace>()->getBounds();
41 
42  this->template declareParam<unsigned int>("num_per_batch", this,
45  this->template declareParam<double>("radius_first_batch", this,
46  &RoadmapHaltonDens::setRadiusFirstBatch,
47  &RoadmapHaltonDens::getRadiusFirstBatch);
48  }
49 
50  void setNumPerBatch(unsigned int num_per_batch)
51  {
52  if (num_per_batch == _num_per_batch)
53  return;
54  if (this->initialized)
55  throw std::runtime_error("cannot set num_per_batch, already initialized!");
56  _num_per_batch = num_per_batch;
57  }
58 
59  unsigned int getNumPerBatch() const
60  {
61  return _num_per_batch;
62  }
63 
64  void setRadiusFirstBatch(double radius_first_batch)
65  {
66  if (radius_first_batch == _radius_first_batch)
67  return;
68  if (this->initialized)
69  throw std::runtime_error("cannot set radius_first_batch, already initialized!");
70  _radius_first_batch = radius_first_batch;
71  }
72 
73  double getRadiusFirstBatch() const
74  {
75  return _radius_first_batch;
76  }
77 
78  void initialize()
79  {
80  std::vector<std::string> missings;
81  if (_num_per_batch == 0)
82  missings.push_back("num_per_batch");
83  if (_radius_first_batch == 0.0)
84  missings.push_back("radius_first_batch");
85  if (missings.size())
86  {
87  std::string str = "Cannot initialize, parameters not set:";
88  for (unsigned int ui=0; ui<missings.size(); ui++)
89  str += " " + missings[ui];
90  throw std::runtime_error(str);
91  }
92  this->initialized = true;
93  }
94 
95  void deserialize(const std::string & ser_data)
96  {
97  throw std::runtime_error("RoadmapHaltonDens deserialize from ser_data not supported!");
98  }
99 
100  // should be stateless
101  double root_radius(std::size_t i_batch)
102  {
103  return _radius_first_batch * pow(1./(i_batch+1.), 1./_dim);
104  }
105 
106  // sets all of these maps
107  // generates one additional batch
108  void generate()
109  {
110  // compute radius
111  double radius = root_radius(this->num_batches_generated);
112  std::size_t n = (this->num_batches_generated+1) * _num_per_batch;
113  for (std::size_t v_index=num_vertices(this->g); v_index<n; v_index++)
114  {
115  Vertex v_new = add_vertex(this->g);
116 
117  put(this->vertex_batch_map, v_new, this->num_batches_generated);
118  put(this->is_shadow_map, v_new, false);
119 
120  // allocate a new state for this vertex
121  put(this->state_map, v_new, this->space->allocState());
122  ompl::base::State * v_state = get(this->state_map, v_new);
123  double * values = v_state->as<ompl::base::RealVectorStateSpace::StateType>()->values;
124  for (unsigned int ui=0; ui<_dim; ui++)
125  {
126  values[ui] = _bounds.low[ui] + (_bounds.high[ui] - _bounds.low[ui])
127  * ompl_lemur::util::halton(
128  ompl_lemur::util::get_prime(ui), v_index);
129  }
130  this->nn->add(v_new);
131 
132  // allocate new undirected edges
133  std::vector<Vertex> vs_near;
134  this->nn->nearestR(v_new, radius, vs_near);
135  for (unsigned int ui=0; ui<vs_near.size(); ui++)
136  {
137  if (vs_near[ui] == v_new)
138  continue;
139  Edge e = add_edge(v_new, vs_near[ui], this->g).first;
140  ompl::base::State * vnear_state = get(this->state_map,vs_near[ui]);
141  put(this->distance_map, e, this->space->distance(v_state,vnear_state));
142  put(this->edge_batch_map, e, this->num_batches_generated);
143  }
144  }
145  this->num_batches_generated++;
146  }
147 
148  void serialize(std::string & ser_data)
149  {
150  throw std::runtime_error("RoadmapHaltonDens serialize to ser_data not supported!");
151  }
152 };
153 
154 } // namespace ompl_lemur
void generate()
Generates one additional batch.
Definition: RoadmapHaltonDens.h:108
std::vector< double > low
double root_radius(std::size_t i_batch)
Calcuate the root radius to be used for connecting to potential root vertices.
Definition: RoadmapHaltonDens.h:101
Interface for generating roadmaps over OMPL state spaces into Boost Graph objects.
Definition: Roadmap.h:76
Definition: RoadmapHaltonDens.h:11
void initialize()
Initialize roadmap; must be called once after setting parameters.
Definition: RoadmapHaltonDens.h:78
std::vector< double > high
const T * as() const
void serialize(std::string &ser_data)
Serialize the internal generator state into the passed string.
Definition: RoadmapHaltonDens.h:148
Definition: Roadmap.h:11
void deserialize(const std::string &ser_data)
Re-constitute the internal generator state from serialized data.
Definition: RoadmapHaltonDens.h:95