00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #define BOOST_UBLAS_SHALLOW_ARRAY_ADAPTOR
00039 #include "ompl/base/StateSpace.h"
00040 #include "ompl/base/ProjectionEvaluator.h"
00041 #include "ompl/util/Exception.h"
00042 #include "ompl/util/RandomNumbers.h"
00043 #include "ompl/tools/config/MagicConstants.h"
00044 #include <boost/numeric/ublas/matrix_proxy.hpp>
00045 #include <boost/numeric/ublas/io.hpp>
00046 #include <boost/lexical_cast.hpp>
00047 #include <boost/bind.hpp>
00048 #include <cmath>
00049 #include <cstring>
00050 #include <limits>
00051
00052
00053
00054 ompl::base::ProjectionMatrix::Matrix ompl::base::ProjectionMatrix::ComputeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale)
00055 {
00056 using namespace boost::numeric::ublas;
00057
00058 RNG rng;
00059 Matrix projection(to, from);
00060
00061 for (unsigned int i = 0 ; i < to ; ++i)
00062 {
00063 for (unsigned int j = 0 ; j < from ; ++j)
00064 projection(i, j) = rng.gaussian01();
00065 }
00066
00067 for (unsigned int i = 1 ; i < to ; ++i)
00068 {
00069 matrix_row<Matrix> row(projection, i);
00070 for (unsigned int j = 0 ; j < i ; ++j)
00071 {
00072 matrix_row<Matrix> prevRow(projection, j);
00073
00074 row -= inner_prod(row, prevRow) * prevRow;
00075 }
00076
00077 row /= norm_2(row);
00078 }
00079
00080 assert(scale.size() == from || scale.size() == 0);
00081 if (scale.size() == from)
00082 for (unsigned int i = 0 ; i < from ; ++i)
00083 {
00084 if (fabs(scale[i]) < std::numeric_limits<double>::epsilon())
00085 throw Exception("Scaling factor must be non-zero");
00086 boost::numeric::ublas::column(projection, i) /= scale[i];
00087 }
00088 return projection;
00089 }
00090
00091 ompl::base::ProjectionMatrix::Matrix ompl::base::ProjectionMatrix::ComputeRandom(const unsigned int from, const unsigned int to)
00092 {
00093 return ComputeRandom(from, to, std::vector<double>());
00094 }
00095
00096 void ompl::base::ProjectionMatrix::computeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale)
00097 {
00098 mat = ComputeRandom(from, to, scale);
00099 }
00100
00101 void ompl::base::ProjectionMatrix::computeRandom(const unsigned int from, const unsigned int to)
00102 {
00103 mat = ComputeRandom(from, to);
00104 }
00105
00106 void ompl::base::ProjectionMatrix::project(const double *from, EuclideanProjection& to) const
00107 {
00108 using namespace boost::numeric::ublas;
00109
00110 shallow_array_adaptor<const double> tmp1(mat.size2(), from);
00111 vector<double, shallow_array_adaptor<const double> > tmp2(mat.size2(), tmp1);
00112 to = prod(mat, tmp2);
00113 }
00114
00115 void ompl::base::ProjectionMatrix::print(std::ostream &out) const
00116 {
00117 out << mat << std::endl;
00118 }
00119
00120 ompl::base::ProjectionEvaluator::ProjectionEvaluator(const StateSpace *space) : space_(space), defaultCellSizes_(true), cellSizesWereInferred_(false)
00121 {
00122 params_.declareParam<double>("cellsize_factor", boost::bind(&ProjectionEvaluator::mulCellSizes, this, _1));
00123 }
00124
00125 ompl::base::ProjectionEvaluator::ProjectionEvaluator(const StateSpacePtr &space) : space_(space.get()), defaultCellSizes_(true), cellSizesWereInferred_(false)
00126 {
00127 params_.declareParam<double>("cellsize_factor", boost::bind(&ProjectionEvaluator::mulCellSizes, this, _1));
00128 }
00129
00130 ompl::base::ProjectionEvaluator::~ProjectionEvaluator(void)
00131 {
00132 }
00133
00134 bool ompl::base::ProjectionEvaluator::userConfigured(void) const
00135 {
00136 return !defaultCellSizes_ && !cellSizesWereInferred_;
00137 }
00138
00139 void ompl::base::ProjectionEvaluator::setCellSizes(const std::vector<double> &cellSizes)
00140 {
00141 defaultCellSizes_ = false;
00142 cellSizesWereInferred_ = false;
00143 cellSizes_ = cellSizes;
00144 checkCellSizes();
00145 }
00146
00147 void ompl::base::ProjectionEvaluator::setCellSizes(unsigned int dim, double cellSize)
00148 {
00149 if (cellSizes_.size() >= dim)
00150 msg_.error("Dimension %u is not defined for projection evaluator", dim);
00151 else
00152 {
00153 std::vector<double> c = cellSizes_;
00154 c[dim] = cellSize;
00155 setCellSizes(c);
00156 }
00157 }
00158
00159 double ompl::base::ProjectionEvaluator::getCellSizes(unsigned int dim) const
00160 {
00161 if (cellSizes_.size() > dim)
00162 return cellSizes_[dim];
00163 msg_.error("Dimension %u is not defined for projection evaluator", dim);
00164 return 0.0;
00165 }
00166
00167 void ompl::base::ProjectionEvaluator::mulCellSizes(double factor)
00168 {
00169 if (cellSizes_.size() == getDimension())
00170 {
00171 std::vector<double> c(cellSizes_.size());
00172 for (std::size_t i = 0 ; i < cellSizes_.size() ; ++i)
00173 c[i] = cellSizes_[i] * factor;
00174 setCellSizes(c);
00175 }
00176 }
00177
00178 void ompl::base::ProjectionEvaluator::checkCellSizes(void) const
00179 {
00180 if (getDimension() <= 0)
00181 throw Exception("Dimension of projection needs to be larger than 0");
00182 if (cellSizes_.size() != getDimension())
00183 throw Exception("Number of dimensions in projection space does not match number of cell sizes");
00184 }
00185
00186 void ompl::base::ProjectionEvaluator::defaultCellSizes(void)
00187 {
00188 }
00189
00191 namespace ompl
00192 {
00193 namespace base
00194 {
00195
00196 static inline void computeCoordinatesHelper(const std::vector<double> &cellSizes, const EuclideanProjection &projection, ProjectionCoordinates &coord)
00197 {
00198 const std::size_t dim = cellSizes.size();
00199 coord.resize(dim);
00200 for (unsigned int i = 0 ; i < dim ; ++i)
00201 coord[i] = (int)floor(projection(i)/cellSizes[i]);
00202 }
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255 }
00256 }
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289
00290
00291
00292
00293
00294
00295
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336 void ompl::base::ProjectionEvaluator::inferCellSizes(void)
00337 {
00338 cellSizesWereInferred_ = true;
00339 unsigned int dim = getDimension();
00340 if (dim > 0)
00341 {
00342 StateSamplerPtr sampler = space_->allocStateSampler();
00343 State *s = space_->allocState();
00344 EuclideanProjection proj(dim);
00345
00346 std::vector<double> low(dim, std::numeric_limits<double>::infinity());
00347 std::vector<double> high(dim, -std::numeric_limits<double>::infinity());
00348
00349 for (unsigned int i = 0 ; i < magic::PROJECTION_EXTENTS_SAMPLES ; ++i)
00350 {
00351 sampler->sampleUniform(s);
00352 project(s, proj);
00353 for (unsigned int j = 0 ; j < dim ; ++j)
00354 {
00355 if (low[j] > proj[j])
00356 low[j] = proj[j];
00357 if (high[j] < proj[j])
00358 high[j] = proj[j];
00359 }
00360 }
00361
00362 space_->freeState(s);
00363
00364 cellSizes_.resize(dim);
00365 for (unsigned int j = 0 ; j < dim ; ++j)
00366 {
00367 cellSizes_[j] = (high[j] - low[j]) / magic::PROJECTION_DIMENSION_SPLITS;
00368 if (cellSizes_[j] < std::numeric_limits<double>::epsilon())
00369 {
00370 cellSizes_[j] = 1.0;
00371 msg_.warn("Inferred cell size for dimension %u of a projection for state space %s is 0. Setting arbitrary value of 1 instead.",
00372 j, space_->getName().c_str());
00373 }
00374 }
00375 }
00376 }
00377
00378 void ompl::base::ProjectionEvaluator::setup(void)
00379 {
00380 if (defaultCellSizes_)
00381 defaultCellSizes();
00382
00383 if ((cellSizes_.size() == 0 && getDimension() > 0) || cellSizesWereInferred_)
00384 inferCellSizes();
00385
00386 checkCellSizes();
00387
00388 unsigned int dim = getDimension();
00389 for (unsigned int i = 0 ; i < dim ; ++i)
00390 params_.declareParam<double>("cellsize." + boost::lexical_cast<std::string>(i),
00391 boost::bind(&ProjectionEvaluator::setCellSizes, this, i, _1),
00392 boost::bind(&ProjectionEvaluator::getCellSizes, this, i));
00393 }
00394
00395 void ompl::base::ProjectionEvaluator::computeCoordinates(const EuclideanProjection &projection, ProjectionCoordinates &coord) const
00396 {
00397 computeCoordinatesHelper(cellSizes_, projection, coord);
00398 }
00399
00400 void ompl::base::ProjectionEvaluator::printSettings(std::ostream &out) const
00401 {
00402 out << "Projection of dimension " << getDimension() << std::endl;
00403 out << "Cell sizes";
00404 if (cellSizesWereInferred_)
00405 out << " (inferred by sampling)";
00406 else
00407 {
00408 if (defaultCellSizes_)
00409 out << " (computed defaults)";
00410 else
00411 out << " (set by user)";
00412 }
00413 out << ": [";
00414 for (unsigned int i = 0 ; i < cellSizes_.size() ; ++i)
00415 {
00416 out << cellSizes_[i];
00417 if (i + 1 < cellSizes_.size())
00418 out << ' ';
00419 }
00420 out << ']' << std::endl;
00421 }
00422
00423 void ompl::base::ProjectionEvaluator::printProjection(const EuclideanProjection &projection, std::ostream &out) const
00424 {
00425 out << projection << std::endl;
00426 }
00427
00428 ompl::base::SubSpaceProjectionEvaluator::SubSpaceProjectionEvaluator(const StateSpace *space, unsigned int index, const ProjectionEvaluatorPtr &projToUse) :
00429 ProjectionEvaluator(space), index_(index), specifiedProj_(projToUse)
00430 {
00431 if (!space_->isCompound())
00432 throw Exception("Cannot construct a subspace projection evaluator for a space that is not compound");
00433 if (space_->as<CompoundStateSpace>()->getSubSpaceCount() >= index_)
00434 throw Exception("State space " + space_->getName() + " does not have a subspace at index " + boost::lexical_cast<std::string>(index_));
00435 }
00436
00437 void ompl::base::SubSpaceProjectionEvaluator::setup(void)
00438 {
00439 if (specifiedProj_)
00440 proj_ = specifiedProj_;
00441 else
00442 proj_ = space_->as<CompoundStateSpace>()->getSubSpace(index_)->getDefaultProjection();
00443 if (!proj_)
00444 throw Exception("No projection specified for subspace at index " + boost::lexical_cast<std::string>(index_));
00445
00446 cellSizes_ = proj_->getCellSizes();
00447 ProjectionEvaluator::setup();
00448 }
00449
00450 unsigned int ompl::base::SubSpaceProjectionEvaluator::getDimension(void) const
00451 {
00452 return proj_->getDimension();
00453 }
00454
00455 void ompl::base::SubSpaceProjectionEvaluator::project(const State *state, EuclideanProjection &projection) const
00456 {
00457 proj_->project(state->as<CompoundState>()->components[index_], projection);
00458 }