LEMUR Packages: ompl_lemur or_lemur pr_bgl prpy_lemur
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Groups Pages
RoadmapHaltonOffDens.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 template <class RoadmapArgs>
15 class RoadmapHaltonOffDens : public Roadmap<RoadmapArgs>
16 {
17  typedef boost::graph_traits<typename RoadmapArgs::Graph> GraphTypes;
18  typedef typename GraphTypes::vertex_descriptor Vertex;
19  typedef typename GraphTypes::edge_descriptor Edge;
20 
21  // set on construction
22  unsigned int _dim;
24 
25  // params
26  unsigned int _num_per_batch;
27  double _gamma_factor;
28  enum t_scaling
29  {
30  SCALING_NOTSET,
31  SCALING_LOG_N,
32  SCALING_LOGLOG_N,
33  SCALING_1_N
34  } _scaling;
35  unsigned int _seed;
36  bool _seed_set;
37 
38  // set on initialization
39  double _gamma;
41  double * _offset_values;
42 
43 public:
45  Roadmap<RoadmapArgs>(args, "HaltonOffDens", 0),
46  _dim(0),
47  _bounds(0),
48  _num_per_batch(0),
49  _gamma_factor(0.0),
50  _scaling(SCALING_NOTSET),
51  _seed(0),
52  _seed_set(false),
53  _offset_state(this->space)
54  {
55  // check that we're in a real vector state space
56  if (this->space->getType() != ompl::base::STATE_SPACE_REAL_VECTOR)
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!");
61  ompl::base::StateSpacePtr myspace(this->space);
62  _bounds = myspace->as<ompl::base::RealVectorStateSpace>()->getBounds();
63  _offset_values = _offset_state.get()->as<ompl::base::RealVectorStateSpace::StateType>()->values;
64 
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);
77  }
78 
79  void setNumPerBatch(unsigned int num_per_batch)
80  {
81  if (num_per_batch == _num_per_batch)
82  return;
83  if (this->initialized)
84  throw std::runtime_error("cannot set num_per_batch, already initialized!");
85  _num_per_batch = num_per_batch;
86  }
87 
88  unsigned int getNumPerBatch() const
89  {
90  return _num_per_batch;
91  }
92 
93  void setGammaFactor(double gamma_factor)
94  {
95  ompl_lemur::util::snap_decimal(gamma_factor);
96  if (gamma_factor == _gamma_factor)
97  return;
98  if (this->initialized)
99  throw std::runtime_error("cannot set gamma_factor, already initialized!");
100  _gamma_factor = gamma_factor;
101  }
102 
103  double getGammaFactor() const
104  {
105  return _gamma_factor;
106  }
107 
108  void setScaling(std::string str_scaling)
109  {
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")
116  scaling=SCALING_1_N;
117  else
118  throw std::runtime_error("cannot set scaling, unknown value!");
119  if (scaling == _scaling)
120  return;
121  if (this->initialized)
122  throw std::runtime_error("cannot set scaling, already initialized!");
123  _scaling = scaling;
124  }
125 
126  std::string getScaling() const
127  {
128  switch (_scaling)
129  {
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";
134  }
135  }
136 
137 
138  void setSeed(unsigned int seed)
139  {
140  if (_seed_set && seed == _seed)
141  return;
142  if (this->initialized)
143  throw std::runtime_error("cannot set seed, already initialized!");
144  _seed = seed;
145  _seed_set = true;
146  }
147 
148  unsigned int getSeed() const
149  {
150  return _seed;
151  }
152 
153  void initialize()
154  {
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");
162  if (!_seed_set)
163  missings.push_back("seed");
164  if (missings.size())
165  {
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);
170  }
171 
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);
174 
175  ompl::base::StateSamplerPtr sampler(this->space->allocStateSampler());
176 
177  ompl_lemur::StateSamplerSetSeed(sampler, _seed);
178  sampler->sampleUniform(_offset_state.get());
179 
180  this->initialized = true;
181  }
182 
183  // nothing to deserialize!
184  void deserialize(const std::string & ser_data)
185  {
186  }
187 
188  // should be stateless
189  double root_radius(std::size_t i_batch)
190  {
191  std::size_t n = (i_batch+1) * _num_per_batch;
192  switch (_scaling)
193  {
194  case SCALING_LOG_N:
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);
198  case SCALING_1_N:
199  return _gamma * pow(1./(1.*n), 1./_dim);
200  default:
201  break;
202  }
203  throw std::runtime_error("scaling not set!");
204  }
205 
206  // sets all of these maps
207  // generates one additional batch
208  void generate()
209  {
210  // compute radius
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++)
214  {
215  Vertex v_new = add_vertex(this->g);
216 
217  put(this->vertex_batch_map, v_new, this->num_batches_generated);
218  put(this->is_shadow_map, v_new, false);
219 
220  // allocate a new state for this vertex
221  put(this->state_map, v_new, this->space->allocState());
222  ompl::base::State * v_state = get(this->state_map, v_new);
223  double * values = v_state->as<ompl::base::RealVectorStateSpace::StateType>()->values;
224  for (unsigned int ui=0; ui<_dim; ui++)
225  {
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]);
231  values[ui] = value;
232  }
233  this->nn->add(v_new);
234 
235  // allocate new undirected edges
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++)
239  {
240  if (vs_near[ui] == v_new)
241  continue;
242  Edge e = add_edge(v_new, vs_near[ui], this->g).first;
243  ompl::base::State * vnear_state = get(this->state_map,vs_near[ui]);
244  put(this->distance_map, e, this->space->distance(v_state,vnear_state));
245  put(this->edge_batch_map, e, this->num_batches_generated);
246  }
247  }
248  this->num_batches_generated++;
249  }
250 
251  // nothing to serialize (offset from seed, and num_generated known)
252  void serialize(std::string & ser_data)
253  {
254  }
255 };
256 
257 } // namespace ompl_lemur
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
const T * as() const
Definition: Roadmap.h:11