10 template <
class RoadmapArgs>
13 typedef boost::graph_traits<typename RoadmapArgs::Graph> GraphTypes;
14 typedef typename GraphTypes::vertex_descriptor Vertex;
15 typedef typename GraphTypes::edge_descriptor Edge;
22 unsigned int _num_per_batch;
23 double _radius_first_batch;
31 _radius_first_batch(0.0)
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!");
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);
50 void setNumPerBatch(
unsigned int num_per_batch)
52 if (num_per_batch == _num_per_batch)
54 if (this->initialized)
55 throw std::runtime_error(
"cannot set num_per_batch, already initialized!");
56 _num_per_batch = num_per_batch;
59 unsigned int getNumPerBatch()
const
61 return _num_per_batch;
64 void setRadiusFirstBatch(
double radius_first_batch)
66 if (radius_first_batch == _radius_first_batch)
68 if (this->initialized)
69 throw std::runtime_error(
"cannot set radius_first_batch, already initialized!");
70 _radius_first_batch = radius_first_batch;
73 double getRadiusFirstBatch()
const
75 return _radius_first_batch;
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");
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);
92 this->initialized =
true;
97 throw std::runtime_error(
"RoadmapHaltonDens deserialize from ser_data not supported!");
103 return _radius_first_batch * pow(1./(i_batch+1.), 1./_dim);
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++)
115 Vertex v_new = add_vertex(this->g);
117 put(this->vertex_batch_map, v_new, this->num_batches_generated);
118 put(this->is_shadow_map, v_new,
false);
121 put(this->state_map, v_new, this->space->allocState());
124 for (
unsigned int ui=0; ui<_dim; ui++)
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);
130 this->nn->add(v_new);
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++)
137 if (vs_near[ui] == v_new)
139 Edge e = add_edge(v_new, vs_near[ui], this->g).first;
141 put(this->distance_map, e, this->space->distance(v_state,vnear_state));
142 put(this->edge_batch_map, e, this->num_batches_generated);
145 this->num_batches_generated++;
150 throw std::runtime_error(
"RoadmapHaltonDens serialize to ser_data not supported!");
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
void serialize(std::string &ser_data)
Serialize the internal generator state into the passed string.
Definition: RoadmapHaltonDens.h:148
void deserialize(const std::string &ser_data)
Re-constitute the internal generator state from serialized data.
Definition: RoadmapHaltonDens.h:95