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>
56 using namespace boost::numeric::ublas;
59 Matrix projection(to, from);
61 for (
unsigned int i = 0 ; i < to ; ++i)
63 for (
unsigned int j = 0 ; j < from ; ++j)
64 projection(i, j) = rng.gaussian01();
67 for (
unsigned int i = 0 ; i < to ; ++i)
69 matrix_row<Matrix> row(projection, i);
70 for (
unsigned int j = 0 ; j < i ; ++j)
72 matrix_row<Matrix> prevRow(projection, j);
74 row -= inner_prod(row, prevRow) * prevRow;
80 assert(scale.size() == from || scale.size() == 0);
81 if (scale.size() == from)
82 for (
unsigned int i = 0 ; i < from ; ++i)
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];
108 using namespace boost::numeric::ublas;
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);
117 out <<
mat << std::endl;
130 ompl::base::ProjectionEvaluator::~ProjectionEvaluator(
void)
136 return !defaultCellSizes_ && !cellSizesWereInferred_;
141 defaultCellSizes_ =
false;
142 cellSizesWereInferred_ =
false;
143 cellSizes_ = cellSizes;
149 if (cellSizes_.size() >= dim)
150 logError(
"Dimension %u is not defined for projection evaluator", dim);
153 std::vector<double> c = cellSizes_;
161 if (cellSizes_.size() > dim)
162 return cellSizes_[dim];
163 logError(
"Dimension %u is not defined for projection evaluator", dim);
169 if (cellSizes_.size() == getDimension())
171 std::vector<double> c(cellSizes_.size());
172 for (std::size_t i = 0 ; i < cellSizes_.size() ; ++i)
173 c[i] = cellSizes_[i] * factor;
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");
198 const std::size_t dim = cellSizes.size();
200 for (
unsigned int i = 0 ; i < dim ; ++i)
201 coord[i] = (
int)floor(projection(i)/cellSizes[i]);
338 cellSizesWereInferred_ =
true;
339 unsigned int dim = getDimension();
343 State *s = space_->allocState();
346 std::vector<double> low(dim, std::numeric_limits<double>::infinity());
347 std::vector<double> high(dim, -std::numeric_limits<double>::infinity());
351 sampler->sampleUniform(s);
353 for (
unsigned int j = 0 ; j < dim ; ++j)
355 if (low[j] > proj[j])
357 if (high[j] < proj[j])
362 space_->freeState(s);
364 cellSizes_.resize(dim);
365 for (
unsigned int j = 0 ; j < dim ; ++j)
368 if (cellSizes_[j] < std::numeric_limits<double>::epsilon())
371 logWarn(
"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());
380 if (defaultCellSizes_)
383 if ((cellSizes_.size() == 0 && getDimension() > 0) || cellSizesWereInferred_)
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),
397 computeCoordinatesHelper(cellSizes_, projection, coord);
402 out <<
"Projection of dimension " << getDimension() << std::endl;
404 if (cellSizesWereInferred_)
405 out <<
" (inferred by sampling)";
408 if (defaultCellSizes_)
409 out <<
" (computed defaults)";
411 out <<
" (set by user)";
414 for (
unsigned int i = 0 ; i < cellSizes_.size() ; ++i)
416 out << cellSizes_[i];
417 if (i + 1 < cellSizes_.size())
420 out <<
']' << std::endl;
425 out << projection << std::endl;
432 throw Exception(
"Cannot construct a subspace projection evaluator for a space that is not compound");
440 proj_ = specifiedProj_;
444 throw Exception(
"No projection specified for subspace at index " + boost::lexical_cast<std::string>(index_));
446 cellSizes_ = proj_->getCellSizes();
452 return proj_->getDimension();