17 template <
class RoadmapArgs>
20 typedef boost::graph_traits<typename RoadmapArgs::Graph> GraphTypes;
21 typedef typename GraphTypes::vertex_descriptor Vertex;
22 typedef typename GraphTypes::edge_descriptor Edge;
29 unsigned int _num_per_batch;
30 double _radius_first_batch;
44 _radius_first_batch(0.0),
47 _sampler(this->space->allocStateSampler())
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!");
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);
69 void setNumPerBatch(
unsigned int num_per_batch)
71 if (num_per_batch == _num_per_batch)
73 if (this->initialized)
74 throw std::runtime_error(
"cannot set num_per_batch, already initialized!");
75 _num_per_batch = num_per_batch;
78 unsigned int getNumPerBatch()
const
80 return _num_per_batch;
83 void setRadiusFirstBatch(
double radius_first_batch)
85 if (radius_first_batch == _radius_first_batch)
87 if (this->initialized)
88 throw std::runtime_error(
"cannot set radius_first_batch, already initialized!");
89 _radius_first_batch = radius_first_batch;
92 double getRadiusFirstBatch()
const
94 return _radius_first_batch;
97 void setSeed(
unsigned int seed)
99 if (_seed_set && seed == _seed)
101 if (this->initialized)
102 throw std::runtime_error(
"cannot set seed, already initialized!");
107 unsigned int getSeed()
const
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");
120 missings.push_back(
"seed");
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);
129 _gamma = _radius_first_batch / pow(
log(_num_per_batch)/_num_per_batch, 1./_dim);
130 printf(
"RoadmapRGGDens calculated gamma=%f\n", _gamma);
132 ompl_lemur::StateSamplerSetSeed(_sampler, _seed);
134 this->initialized =
true;
139 std::stringstream ss(ser_data);
140 ss >> ompl_lemur::SamplerGenMonkeyPatch(_sampler);
146 unsigned int n = (1 + i_batch) * _num_per_batch;
147 return _gamma * pow(
log(n)/n, 1./_dim);
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++)
159 Vertex v_new = add_vertex(this->g);
161 put(this->vertex_batch_map, v_new, this->num_batches_generated);
162 put(this->is_shadow_map, v_new,
false);
165 put(this->state_map, v_new, this->space->allocState());
167 _sampler->sampleUniform(v_state);
168 this->nn->add(v_new);
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++)
175 if (vs_near[ui] == v_new)
177 Edge e = add_edge(v_new, vs_near[ui], this->g).first;
179 put(this->distance_map, e, this->space->distance(v_state,vnear_state));
180 put(this->edge_batch_map, e, this->num_batches_generated);
183 this->num_batches_generated++;
188 std::stringstream ss;
189 ss << ompl_lemur::SamplerGenMonkeyPatch(_sampler);
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