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 "ompl/datastructures/Grid.h"
00045 #include <boost/numeric/ublas/matrix_proxy.hpp>
00046 #include <boost/numeric/ublas/io.hpp>
00047 #include <cmath>
00048 #include <cstring>
00049 #include <limits>
00050
00051 static const double DIMENSION_UPDATE_FACTOR = 1.2;
00052
00053 ompl::base::ProjectionMatrix::Matrix ompl::base::ProjectionMatrix::ComputeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale)
00054 {
00055 using namespace boost::numeric::ublas;
00056
00057 RNG rng;
00058 Matrix projection(to, from);
00059
00060 for (unsigned int i = 0 ; i < to ; ++i)
00061 {
00062 for (unsigned int j = 0 ; j < from ; ++j)
00063 projection(i, j) = rng.gaussian01();
00064 }
00065
00066 for (unsigned int i = 1 ; i < to ; ++i)
00067 {
00068 matrix_row<Matrix> row(projection, i);
00069 for (unsigned int j = 0 ; j < i ; ++j)
00070 {
00071 matrix_row<Matrix> prevRow(projection, j);
00072
00073 row -= inner_prod(row, prevRow) * prevRow;
00074 }
00075
00076 row /= norm_2(row);
00077 }
00078
00079 return projection;
00080 }
00081
00082 ompl::base::ProjectionMatrix::Matrix ompl::base::ProjectionMatrix::ComputeRandom(const unsigned int from, const unsigned int to)
00083 {
00084 return ComputeRandom(from, to);
00085 }
00086
00087 void ompl::base::ProjectionMatrix::computeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale)
00088 {
00089 mat = ComputeRandom(from, to);
00090
00091 assert(scale.size() == from);
00092 for (unsigned int i = 0 ; i < from ; ++i)
00093 {
00094 if (fabs(scale[i]) < std::numeric_limits<double>::epsilon())
00095 throw Exception("Scaling factor must be non-zero");
00096 boost::numeric::ublas::column(mat, i) /= scale[i];
00097 }
00098 }
00099
00100 void ompl::base::ProjectionMatrix::computeRandom(const unsigned int from, const unsigned int to)
00101 {
00102 mat = ComputeRandom(from, to);
00103 }
00104
00105 void ompl::base::ProjectionMatrix::project(const double *from, EuclideanProjection& to) const
00106 {
00107 using namespace boost::numeric::ublas;
00108
00109 shallow_array_adaptor<const double> tmp1(mat.size2(), from);
00110 vector<double, shallow_array_adaptor<const double> > tmp2(mat.size2(), tmp1);
00111 to = prod(mat, tmp2);
00112 }
00113
00114 void ompl::base::ProjectionMatrix::print(std::ostream &out) const
00115 {
00116 out << mat << std::endl;
00117 }
00118
00119 bool ompl::base::ProjectionEvaluator::userConfigured(void) const
00120 {
00121 return !defaultCellSizes_ && !cellSizesWereInferred_;
00122 }
00123
00124 void ompl::base::ProjectionEvaluator::setCellSizes(const std::vector<double> &cellSizes)
00125 {
00126 defaultCellSizes_ = false;
00127 cellSizesWereInferred_ = false;
00128 cellSizes_ = cellSizes;
00129 checkCellSizes();
00130 }
00131
00132 void ompl::base::ProjectionEvaluator::checkCellSizes(void) const
00133 {
00134 if (getDimension() <= 0)
00135 throw Exception("Dimension of projection needs to be larger than 0");
00136 if (cellSizes_.size() != getDimension())
00137 throw Exception("Number of dimensions in projection space does not match number of cell sizes");
00138 }
00139
00140 void ompl::base::ProjectionEvaluator::defaultCellSizes(void)
00141 {
00142 }
00143
00145 namespace ompl
00146 {
00147 namespace base
00148 {
00149
00150 static inline void computeCoordinatesHelper(const std::vector<double> &cellSizes, const EuclideanProjection &projection, ProjectionCoordinates &coord)
00151 {
00152 const std::size_t dim = cellSizes.size();
00153 coord.resize(dim);
00154 for (unsigned int i = 0 ; i < dim ; ++i)
00155 coord[i] = (int)floor(projection(i)/cellSizes[i]);
00156 }
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209 }
00210 }
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
00257
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 void ompl::base::ProjectionEvaluator::inferCellSizes(void)
00291 {
00292 cellSizesWereInferred_ = true;
00293 unsigned int dim = getDimension();
00294 if (dim > 0)
00295 {
00296 StateSamplerPtr sampler = space_->allocStateSampler();
00297 State *s = space_->allocState();
00298 EuclideanProjection proj(dim);
00299
00300 std::vector<double> low(dim, std::numeric_limits<double>::infinity());
00301 std::vector<double> high(dim, -std::numeric_limits<double>::infinity());
00302
00303 for (unsigned int i = 0 ; i < magic::PROJECTION_EXTENTS_SAMPLES ; ++i)
00304 {
00305 sampler->sampleUniform(s);
00306 project(s, proj);
00307 for (unsigned int j = 0 ; j < dim ; ++j)
00308 {
00309 if (low[j] > proj[j])
00310 low[j] = proj[j];
00311 if (high[j] < proj[j])
00312 high[j] = proj[j];
00313 }
00314 }
00315
00316 space_->freeState(s);
00317
00318 cellSizes_.resize(dim);
00319 for (unsigned int j = 0 ; j < dim ; ++j)
00320 {
00321 cellSizes_[j] = (high[j] - low[j]) / magic::PROJECTION_DIMENSION_SPLITS;
00322 if (cellSizes_[j] < std::numeric_limits<double>::epsilon())
00323 {
00324 cellSizes_[j] = 1.0;
00325 msg_.warn("Inferred cell size for dimension %u of a projection for state space %s is 0. Setting arbitrary value of 1 instead.",
00326 j, space_->getName().c_str());
00327 }
00328 }
00329 }
00330 }
00331
00332 void ompl::base::ProjectionEvaluator::setup(void)
00333 {
00334 if (defaultCellSizes_)
00335 defaultCellSizes();
00336
00337 if ((cellSizes_.size() == 0 && getDimension() > 0) || cellSizesWereInferred_)
00338 inferCellSizes();
00339
00340 checkCellSizes();
00341 }
00342
00343 void ompl::base::ProjectionEvaluator::computeCoordinates(const EuclideanProjection &projection, ProjectionCoordinates &coord) const
00344 {
00345 computeCoordinatesHelper(cellSizes_, projection, coord);
00346 }
00347
00348 void ompl::base::ProjectionEvaluator::printSettings(std::ostream &out) const
00349 {
00350 out << "Projection of dimension " << getDimension() << std::endl;
00351 out << "Cell sizes";
00352 if (cellSizesWereInferred_)
00353 out << " (inferred by sampling)";
00354 else
00355 {
00356 if (defaultCellSizes_)
00357 out << " (computed defaults)";
00358 else
00359 out << " (set by user)";
00360 }
00361 out << ": [";
00362 for (unsigned int i = 0 ; i < cellSizes_.size() ; ++i)
00363 {
00364 out << cellSizes_[i];
00365 if (i + 1 < cellSizes_.size())
00366 out << ' ';
00367 }
00368 out << ']' << std::endl;
00369 }
00370
00371 void ompl::base::ProjectionEvaluator::printProjection(const EuclideanProjection &projection, std::ostream &out) const
00372 {
00373 out << projection << std::endl;
00374 }