14 template <
class RoadmapArgs>
17 typedef boost::graph_traits<typename RoadmapArgs::Graph> GraphTypes;
18 typedef typename GraphTypes::vertex_descriptor Vertex;
19 typedef typename GraphTypes::edge_descriptor Edge;
26 unsigned int _num_per_batch;
41 double * _offset_values;
50 _scaling(SCALING_NOTSET),
53 _offset_state(this->space)
57 throw std::runtime_error(
"RoadmapHaltonOffDens only supports rel vector state spaces!");
58 _dim = this->space->getDimension();
59 if (0 == ompl_lemur::util::get_prime(_dim-1))
60 throw std::runtime_error(
"not enough primes hardcoded!");
65 this->
template declareParam<unsigned int>(
"num_per_batch",
this,
68 this->
template declareParam<double>(
"gamma_factor",
this,
69 &RoadmapHaltonOffDens::setGammaFactor,
70 &RoadmapHaltonOffDens::getGammaFactor);
71 this->
template declareParam<std::string>(
"scaling",
this,
72 &RoadmapHaltonOffDens::setScaling,
73 &RoadmapHaltonOffDens::getScaling);
74 this->
template declareParam<unsigned int>(
"seed",
this,
75 &RoadmapHaltonOffDens::setSeed,
76 &RoadmapHaltonOffDens::getSeed);
79 void setNumPerBatch(
unsigned int num_per_batch)
81 if (num_per_batch == _num_per_batch)
83 if (this->initialized)
84 throw std::runtime_error(
"cannot set num_per_batch, already initialized!");
85 _num_per_batch = num_per_batch;
88 unsigned int getNumPerBatch()
const
90 return _num_per_batch;
93 void setGammaFactor(
double gamma_factor)
95 ompl_lemur::util::snap_decimal(gamma_factor);
96 if (gamma_factor == _gamma_factor)
98 if (this->initialized)
99 throw std::runtime_error(
"cannot set gamma_factor, already initialized!");
100 _gamma_factor = gamma_factor;
103 double getGammaFactor()
const
105 return _gamma_factor;
108 void setScaling(std::string str_scaling)
110 enum t_scaling scaling;
111 if (str_scaling ==
"log_n")
112 scaling=SCALING_LOG_N;
113 else if (str_scaling ==
"loglog_n")
114 scaling=SCALING_LOGLOG_N;
115 else if (str_scaling ==
"1_n")
118 throw std::runtime_error(
"cannot set scaling, unknown value!");
119 if (scaling == _scaling)
121 if (this->initialized)
122 throw std::runtime_error(
"cannot set scaling, already initialized!");
126 std::string getScaling()
const
130 case SCALING_LOG_N:
return "log_n";
131 case SCALING_LOGLOG_N:
return "loglog_n";
132 case SCALING_1_N:
return "1_n";
133 default:
return "notset";
138 void setSeed(
unsigned int seed)
140 if (_seed_set && seed == _seed)
142 if (this->initialized)
143 throw std::runtime_error(
"cannot set seed, already initialized!");
148 unsigned int getSeed()
const
155 std::vector<std::string> missings;
156 if (_num_per_batch == 0)
157 missings.push_back(
"num_per_batch");
158 if (_gamma_factor == 0.0)
159 missings.push_back(
"gamma_factor");
160 if (_scaling == SCALING_NOTSET)
161 missings.push_back(
"scaling");
163 missings.push_back(
"seed");
166 std::string str =
"Cannot initialize, parameters not set:";
167 for (
unsigned int ui=0; ui<missings.size(); ui++)
168 str +=
" " + missings[ui];
169 throw std::runtime_error(str);
172 double frac = this->space->getMeasure() / ompl_lemur::util::volume_n_ball(_dim);
173 _gamma = _gamma_factor * 2.0 * pow((1.+1./_dim) * frac, 1./_dim);
177 ompl_lemur::StateSamplerSetSeed(sampler, _seed);
178 sampler->sampleUniform(_offset_state.
get());
180 this->initialized =
true;
191 std::size_t n = (i_batch+1) * _num_per_batch;
195 return _gamma * pow(
log(n)/(1.*n), 1./_dim);
196 case SCALING_LOGLOG_N:
197 return _gamma * pow(
log(
log(n))/(1.*n), 1./_dim);
199 return _gamma * pow(1./(1.*n), 1./_dim);
203 throw std::runtime_error(
"scaling not set!");
211 double radius =
root_radius(this->num_batches_generated);
212 std::size_t n = (this->num_batches_generated+1) * _num_per_batch;
213 for (std::size_t v_index=num_vertices(this->g); v_index<n; v_index++)
215 Vertex v_new = add_vertex(this->g);
217 put(this->vertex_batch_map, v_new, this->num_batches_generated);
218 put(this->is_shadow_map, v_new,
false);
221 put(this->state_map, v_new, this->space->allocState());
224 for (
unsigned int ui=0; ui<_dim; ui++)
226 double value = _offset_values[ui];
227 value += (_bounds.
high[ui] - _bounds.
low[ui])
228 * ompl_lemur::util::halton(ompl_lemur::util::get_prime(ui), v_index);
229 if (_bounds.
high[ui] < value)
230 value -= (_bounds.
high[ui] - _bounds.
low[ui]);
233 this->nn->add(v_new);
236 std::vector<Vertex> vs_near;
237 this->nn->nearestR(v_new, radius, vs_near);
238 for (
unsigned int ui=0; ui<vs_near.size(); ui++)
240 if (vs_near[ui] == v_new)
242 Edge e = add_edge(v_new, vs_near[ui], this->g).first;
244 put(this->distance_map, e, this->space->distance(v_state,vnear_state));
245 put(this->edge_batch_map, e, this->num_batches_generated);
248 this->num_batches_generated++;
double root_radius(std::size_t i_batch)
Calcuate the root radius to be used for connecting to potential root vertices.
Definition: RoadmapHaltonOffDens.h:189
void log(const char *file, int line, LogLevel level, const char *m,...)
std::vector< double > low
void serialize(std::string &ser_data)
Serialize the internal generator state into the passed string.
Definition: RoadmapHaltonOffDens.h:252
void generate()
Generates one additional batch.
Definition: RoadmapHaltonOffDens.h:208
void initialize()
Initialize roadmap; must be called once after setting parameters.
Definition: RoadmapHaltonOffDens.h:153
Interface for generating roadmaps over OMPL state spaces into Boost Graph objects.
Definition: Roadmap.h:76
std::vector< double > high
void deserialize(const std::string &ser_data)
Re-constitute the internal generator state from serialized data.
Definition: RoadmapHaltonOffDens.h:184
Definition: RoadmapHaltonOffDens.h:15