All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
ProjectionEvaluator.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 // We need this to create a temporary uBLAS vector from a C-style array without copying data
38 #define BOOST_UBLAS_SHALLOW_ARRAY_ADAPTOR
39 #include "ompl/base/StateSpace.h"
40 #include "ompl/base/ProjectionEvaluator.h"
41 #include "ompl/util/Exception.h"
42 #include "ompl/util/RandomNumbers.h"
43 #include "ompl/tools/config/MagicConstants.h"
44 #include <boost/numeric/ublas/matrix_proxy.hpp>
45 #include <boost/numeric/ublas/io.hpp>
46 #include <boost/lexical_cast.hpp>
47 #include <boost/bind.hpp>
48 #include <cmath>
49 #include <cstring>
50 #include <limits>
51 
52 // static const double DIMENSION_UPDATE_FACTOR = 1.2;
53 
54 ompl::base::ProjectionMatrix::Matrix ompl::base::ProjectionMatrix::ComputeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale)
55 {
56  using namespace boost::numeric::ublas;
57 
58  RNG rng;
59  Matrix projection(to, from);
60 
61  for (unsigned int i = 0 ; i < to ; ++i)
62  {
63  for (unsigned int j = 0 ; j < from ; ++j)
64  projection(i, j) = rng.gaussian01();
65  }
66 
67  for (unsigned int i = 0 ; i < to ; ++i)
68  {
69  matrix_row<Matrix> row(projection, i);
70  for (unsigned int j = 0 ; j < i ; ++j)
71  {
72  matrix_row<Matrix> prevRow(projection, j);
73  // subtract projection
74  row -= inner_prod(row, prevRow) * prevRow;
75  }
76  // normalize
77  row /= norm_2(row);
78  }
79 
80  assert(scale.size() == from || scale.size() == 0);
81  if (scale.size() == from)
82  for (unsigned int i = 0 ; i < from ; ++i)
83  {
84  if (fabs(scale[i]) < std::numeric_limits<double>::epsilon())
85  throw Exception("Scaling factor must be non-zero");
86  boost::numeric::ublas::column(projection, i) /= scale[i];
87  }
88  return projection;
89 }
90 
92 {
93  return ComputeRandom(from, to, std::vector<double>());
94 }
95 
96 void ompl::base::ProjectionMatrix::computeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale)
97 {
98  mat = ComputeRandom(from, to, scale);
99 }
100 
101 void ompl::base::ProjectionMatrix::computeRandom(const unsigned int from, const unsigned int to)
102 {
103  mat = ComputeRandom(from, to);
104 }
105 
107 {
108  using namespace boost::numeric::ublas;
109  // create a temporary uBLAS vector from a C-style array without copying data
110  shallow_array_adaptor<const double> tmp1(mat.size2(), from);
111  vector<double, shallow_array_adaptor<const double> > tmp2(mat.size2(), tmp1);
112  to = prod(mat, tmp2);
113 }
114 
115 void ompl::base::ProjectionMatrix::print(std::ostream &out) const
116 {
117  out << mat << std::endl;
118 }
119 
120 ompl::base::ProjectionEvaluator::ProjectionEvaluator(const StateSpace *space) : space_(space), defaultCellSizes_(true), cellSizesWereInferred_(false)
121 {
122  params_.declareParam<double>("cellsize_factor", boost::bind(&ProjectionEvaluator::mulCellSizes, this, _1));
123 }
124 
125 ompl::base::ProjectionEvaluator::ProjectionEvaluator(const StateSpacePtr &space) : space_(space.get()), defaultCellSizes_(true), cellSizesWereInferred_(false)
126 {
127  params_.declareParam<double>("cellsize_factor", boost::bind(&ProjectionEvaluator::mulCellSizes, this, _1));
128 }
129 
130 ompl::base::ProjectionEvaluator::~ProjectionEvaluator(void)
131 {
132 }
133 
135 {
136  return !defaultCellSizes_ && !cellSizesWereInferred_;
137 }
138 
139 void ompl::base::ProjectionEvaluator::setCellSizes(const std::vector<double> &cellSizes)
140 {
141  defaultCellSizes_ = false;
142  cellSizesWereInferred_ = false;
143  cellSizes_ = cellSizes;
144  checkCellSizes();
145 }
146 
147 void ompl::base::ProjectionEvaluator::setCellSizes(unsigned int dim, double cellSize)
148 {
149  if (cellSizes_.size() >= dim)
150  OMPL_ERROR("Dimension %u is not defined for projection evaluator", dim);
151  else
152  {
153  std::vector<double> c = cellSizes_;
154  c[dim] = cellSize;
155  setCellSizes(c);
156  }
157 }
158 
159 double ompl::base::ProjectionEvaluator::getCellSizes(unsigned int dim) const
160 {
161  if (cellSizes_.size() > dim)
162  return cellSizes_[dim];
163  OMPL_ERROR("Dimension %u is not defined for projection evaluator", dim);
164  return 0.0;
165 }
166 
168 {
169  if (cellSizes_.size() == getDimension())
170  {
171  std::vector<double> c(cellSizes_.size());
172  for (std::size_t i = 0 ; i < cellSizes_.size() ; ++i)
173  c[i] = cellSizes_[i] * factor;
174  setCellSizes(c);
175  }
176 }
177 
179 {
180  if (getDimension() <= 0)
181  throw Exception("Dimension of projection needs to be larger than 0");
182  if (cellSizes_.size() != getDimension())
183  throw Exception("Number of dimensions in projection space does not match number of cell sizes");
184 }
185 
187 {
188 }
189 
191 namespace ompl
192 {
193  namespace base
194  {
195 
196  static inline void computeCoordinatesHelper(const std::vector<double> &cellSizes, const EuclideanProjection &projection, ProjectionCoordinates &coord)
197  {
198  const std::size_t dim = cellSizes.size();
199  coord.resize(dim);
200  for (unsigned int i = 0 ; i < dim ; ++i)
201  coord[i] = (int)floor(projection(i)/cellSizes[i]);
202  }
203  /*
204  static Grid<int> constructGrid(unsigned int dim, const std::vector<ProjectionCoordinates> &coord)
205  {
206  Grid<int> g(dim);
207  for (std::size_t i = 0 ; i < coord.size() ; ++i)
208  {
209  Grid<int>::Cell *c = g.getCell(coord[i]);
210  if (c)
211  c->data++;
212  else
213  {
214  Grid<int>::Cell *c = g.createCell(coord[i]);
215  c->data = 1;
216  g.add(c);
217  }
218  }
219  return g;
220  }
221 
222  static unsigned int getComponentCount(const std::vector<EuclideanProjection*> &proj,
223  const std::vector<double> &cellSizes)
224  {
225  std::vector<ProjectionCoordinates> coord(proj.size());
226  for (std::size_t i = 0 ; i < proj.size() ; ++i)
227  computeCoordinatesHelper(cellSizes, *proj[i], coord[i]);
228  return constructGrid(cellSizes.size(), coord).components().size();
229  }
230 
231  static int updateComponentCountDimension(const std::vector<EuclideanProjection*> &proj,
232  std::vector<double> &cellSizes, bool increase)
233  {
234  unsigned int dim = cellSizes.size();
235  const double factor = increase ? DIMENSION_UPDATE_FACTOR : 1.0 / DIMENSION_UPDATE_FACTOR;
236 
237  int bestD = -1;
238  unsigned int best = 0;
239  for (unsigned int d = 0 ; d < dim ; ++d)
240  {
241  double backup = cellSizes[d];
242  cellSizes[d] *= factor;
243  unsigned int nc = getComponentCount(proj, cellSizes);
244  if (bestD < 0 || (increase && nc > best) || (!increase && nc < best))
245  {
246  best = nc;
247  bestD = d;
248  }
249  cellSizes[d] = backup;
250  }
251  cellSizes[bestD] *= factor;
252  return bestD;
253  }
254  */
255  }
256 }
258 
259 
260 
261 /*
262 void ompl::base::ProjectionEvaluator::computeCellSizes(const std::vector<const State*> &states)
263 {
264  setup();
265 
266  msg_.debug("Computing projections from %u states", states.size());
267 
268  unsigned int dim = getDimension();
269  std::vector<double> low(dim, std::numeric_limits<double>::infinity());
270  std::vector<double> high(dim, -std::numeric_limits<double>::infinity());
271  std::vector<EuclideanProjection*> proj(states.size());
272  std::vector<ProjectionCoordinates> coord(states.size());
273 
274  for (std::size_t i = 0 ; i < states.size() ; ++i)
275  {
276  proj[i] = new EuclideanProjection(dim);
277  project(states[i], *proj[i]);
278  for (std::size_t j = 0 ; j < dim ; ++j)
279  {
280  if (low[j] > proj[i]->values[j])
281  low[j] = proj[i]->values[j];
282  if (high[j] < proj[i]->values[j])
283  high[j] = proj[i]->values[j];
284  }
285  }
286 
287  bool dir1 = false, dir2 = false;
288  do
289  {
290  for (std::size_t i = 0 ; i < proj.size() ; ++i)
291  computeCoordinates(*proj[i], coord[i]);
292  const Grid<int> &g = constructGrid(dim, coord);
293 
294  const std::vector< std::vector<Grid<int>::Cell*> > &comp = g.components();
295  if (comp.size() > 0)
296  {
297  std::size_t n = comp.size() / 10;
298  if (n < 1)
299  n = 1;
300  std::size_t s = 0;
301  for (std::size_t i = 0 ; i < n ; ++i)
302  s += comp[i].size();
303  double f = (double)s / (double)g.size();
304 
305  OMPL_DEBUG("There are %u connected components in the projected grid. The first 10%% fractions is %f", comp.size(), f);
306 
307  if (f < 0.7)
308  {
309  dir1 = true;
310  int bestD = updateComponentCountDimension(proj, cellSizes_, true);
311  OMPL_DEBUG("Increasing cell size in dimension %d to %f", bestD, cellSizes_[bestD]);
312  }
313  else
314  if (f > 0.9)
315  {
316  dir2 = true;
317  int bestD = updateComponentCountDimension(proj, cellSizes_, false);
318  OMPL_DEBUG("Decreasing cell size in dimension %d to %f", bestD, cellSizes_[bestD]);
319  }
320  else
321  {
322  OMPL_DEBUG("No more changes made to cell sizes");
323  break;
324  }
325  }
326  } while (dir1 ^ dir2);
327 
328  for (unsigned int i = 0 ; i < proj.size() ; ++i)
329  delete proj[i];
330 
331  // make sure all flags are set correctly
332  setCellSizes(cellSizes_);
333 }
334 */
335 
337 {
338  cellSizesWereInferred_ = true;
339  unsigned int dim = getDimension();
340  if (dim > 0)
341  {
342  StateSamplerPtr sampler = space_->allocStateSampler();
343  State *s = space_->allocState();
344  EuclideanProjection proj(dim);
345 
346  std::vector<double> low(dim, std::numeric_limits<double>::infinity());
347  std::vector<double> high(dim, -std::numeric_limits<double>::infinity());
348 
349  for (unsigned int i = 0 ; i < magic::PROJECTION_EXTENTS_SAMPLES ; ++i)
350  {
351  sampler->sampleUniform(s);
352  project(s, proj);
353  for (unsigned int j = 0 ; j < dim ; ++j)
354  {
355  if (low[j] > proj[j])
356  low[j] = proj[j];
357  if (high[j] < proj[j])
358  high[j] = proj[j];
359  }
360  }
361 
362  space_->freeState(s);
363 
364  cellSizes_.resize(dim);
365  for (unsigned int j = 0 ; j < dim ; ++j)
366  {
367  cellSizes_[j] = (high[j] - low[j]) / magic::PROJECTION_DIMENSION_SPLITS;
368  if (cellSizes_[j] < std::numeric_limits<double>::epsilon())
369  {
370  cellSizes_[j] = 1.0;
371  OMPL_WARN("Inferred cell size for dimension %u of a projection for state space %s is 0. Setting arbitrary value of 1 instead.",
372  j, space_->getName().c_str());
373  }
374  }
375  }
376 }
377 
379 {
380  if (defaultCellSizes_)
381  defaultCellSizes();
382 
383  if ((cellSizes_.size() == 0 && getDimension() > 0) || cellSizesWereInferred_)
384  inferCellSizes();
385 
386  checkCellSizes();
387 
388  unsigned int dim = getDimension();
389  for (unsigned int i = 0 ; i < dim ; ++i)
390  params_.declareParam<double>("cellsize." + boost::lexical_cast<std::string>(i),
391  boost::bind(&ProjectionEvaluator::setCellSizes, this, i, _1),
392  boost::bind(&ProjectionEvaluator::getCellSizes, this, i));
393 }
394 
396 {
397  computeCoordinatesHelper(cellSizes_, projection, coord);
398 }
399 
401 {
402  out << "Projection of dimension " << getDimension() << std::endl;
403  out << "Cell sizes";
404  if (cellSizesWereInferred_)
405  out << " (inferred by sampling)";
406  else
407  {
408  if (defaultCellSizes_)
409  out << " (computed defaults)";
410  else
411  out << " (set by user)";
412  }
413  out << ": [";
414  for (unsigned int i = 0 ; i < cellSizes_.size() ; ++i)
415  {
416  out << cellSizes_[i];
417  if (i + 1 < cellSizes_.size())
418  out << ' ';
419  }
420  out << ']' << std::endl;
421 }
422 
423 void ompl::base::ProjectionEvaluator::printProjection(const EuclideanProjection &projection, std::ostream &out) const
424 {
425  out << projection << std::endl;
426 }
427 
429  ProjectionEvaluator(space), index_(index), specifiedProj_(projToUse)
430 {
431  if (!space_->isCompound())
432  throw Exception("Cannot construct a subspace projection evaluator for a space that is not compound");
434  throw Exception("State space " + space_->getName() + " does not have a subspace at index " + boost::lexical_cast<std::string>(index_));
435 }
436 
438 {
439  if (specifiedProj_)
440  proj_ = specifiedProj_;
441  else
442  proj_ = space_->as<CompoundStateSpace>()->getSubspace(index_)->getDefaultProjection();
443  if (!proj_)
444  throw Exception("No projection specified for subspace at index " + boost::lexical_cast<std::string>(index_));
445 
446  cellSizes_ = proj_->getCellSizes();
448 }
449 
451 {
452  return proj_->getDimension();
453 }
454 
456 {
457  proj_->project(state->as<CompoundState>()->components[index_], projection);
458 }