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 #include "ompl/base/StateManifold.h"
00038 #include "ompl/base/ProjectionEvaluator.h"
00039 #include "ompl/util/Exception.h"
00040 #include "ompl/util/RandomNumbers.h"
00041 #include "ompl/util/MagicConstants.h"
00042 #include "ompl/datastructures/Grid.h"
00043 #include <cmath>
00044 #include <cstring>
00045 #include <limits>
00046
00047 static const double DIMENSION_UPDATE_FACTOR = 1.2;
00048
00049 ompl::base::ProjectionMatrix::Matrix ompl::base::ProjectionMatrix::ComputeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale)
00050 {
00051 RNG rng;
00052
00053 Matrix projection(to);
00054 for (unsigned int i = 0 ; i < to ; ++i)
00055 {
00056 projection[i].resize(from);
00057 for (unsigned int j = 0 ; j < from ; ++j)
00058 projection[i][j] = rng.gaussian01();
00059 }
00060
00061 for (unsigned int i = 1 ; i < to ; ++i)
00062 {
00063 std::valarray<double> &row = projection[i];
00064 for (unsigned int j = 0 ; j < i ; ++j)
00065 {
00066 std::valarray<double> &prevRow = projection[j];
00067
00068 row -= (row * prevRow).sum() * prevRow;
00069 }
00070
00071 row /= sqrt((row * row).sum());
00072 }
00073
00074
00075 if (scale.size() == from)
00076 {
00077 for (unsigned int i = 0 ; i < to ; ++i)
00078 for (unsigned int j = 0 ; j < from ; ++j)
00079 {
00080 if (fabs(scale[i]) < std::numeric_limits<double>::epsilon())
00081 throw Exception("Scaling factor must be non-zero");
00082 projection[i][j] /= scale[i];
00083 }
00084 }
00085
00086 return projection;
00087 }
00088
00089 ompl::base::ProjectionMatrix::Matrix ompl::base::ProjectionMatrix::ComputeRandom(const unsigned int from, const unsigned int to)
00090 {
00091 return ComputeRandom(from, to, std::vector<double>());
00092 }
00093
00094 void ompl::base::ProjectionMatrix::computeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale)
00095 {
00096 mat = ComputeRandom(from, to, scale);
00097 }
00098
00099 void ompl::base::ProjectionMatrix::computeRandom(const unsigned int from, const unsigned int to)
00100 {
00101 mat = ComputeRandom(from, to);
00102 }
00103
00104 void ompl::base::ProjectionMatrix::project(const double *from, double *to) const
00105 {
00106 for (unsigned int i = 0 ; i < mat.size() ; ++i)
00107 {
00108 const std::valarray<double> &vec = mat[i];
00109 const unsigned int dim = vec.size();
00110 double *pos = to + i;
00111 *pos = 0.0;
00112 for (unsigned int j = 0 ; j < dim ; ++j)
00113 *pos += from[j] * vec[j];
00114 }
00115 }
00116
00117 void ompl::base::ProjectionMatrix::print(std::ostream &out) const
00118 {
00119 for (unsigned int i = 0 ; i < mat.size() ; ++i)
00120 {
00121 const std::valarray<double> &vec = mat[i];
00122 const unsigned int dim = vec.size();
00123 for (unsigned int j = 0 ; j < dim ; ++j)
00124 out << vec[j] << " ";
00125 out << std::endl;
00126 }
00127 }
00128
00129 bool ompl::base::ProjectionEvaluator::userConfigured(void) const
00130 {
00131 return !defaultCellSizes_ && !cellSizesWereInferred_;
00132 }
00133
00134 void ompl::base::ProjectionEvaluator::setCellSizes(const std::vector<double> &cellSizes)
00135 {
00136 defaultCellSizes_ = false;
00137 cellSizesWereInferred_ = false;
00138 cellSizes_ = cellSizes;
00139 checkCellSizes();
00140 }
00141
00142 void ompl::base::ProjectionEvaluator::checkCellSizes(void) const
00143 {
00144 if (getDimension() <= 0)
00145 throw Exception("Dimension of projection needs to be larger than 0");
00146 if (cellSizes_.size() != getDimension())
00147 throw Exception("Number of dimensions in projection space does not match number of cell sizes");
00148 }
00149
00150 void ompl::base::ProjectionEvaluator::defaultCellSizes(void)
00151 {
00152 }
00153
00155 namespace ompl
00156 {
00157 namespace base
00158 {
00159
00160 static inline void computeCoordinatesHelper(const std::vector<double> &cellSizes, const EuclideanProjection &projection, ProjectionCoordinates &coord)
00161 {
00162 const std::size_t dim = cellSizes.size();
00163 coord.resize(dim);
00164 for (unsigned int i = 0 ; i < dim ; ++i)
00165 coord[i] = (int)floor(projection.values[i]/cellSizes[i]);
00166 }
00167
00168 static Grid<int> constructGrid(unsigned int dim, const std::vector<ProjectionCoordinates> &coord)
00169 {
00170 Grid<int> g(dim);
00171 for (std::size_t i = 0 ; i < coord.size() ; ++i)
00172 {
00173 Grid<int>::Cell *c = g.getCell(coord[i]);
00174 if (c)
00175 c->data++;
00176 else
00177 {
00178 Grid<int>::Cell *c = g.createCell(coord[i]);
00179 c->data = 1;
00180 g.add(c);
00181 }
00182 }
00183 return g;
00184 }
00185
00186 static unsigned int getComponentCount(const std::vector<EuclideanProjection*> &proj,
00187 const std::vector<double> &cellSizes)
00188 {
00189 std::vector<ProjectionCoordinates> coord(proj.size());
00190 for (std::size_t i = 0 ; i < proj.size() ; ++i)
00191 computeCoordinatesHelper(cellSizes, *proj[i], coord[i]);
00192 return constructGrid(cellSizes.size(), coord).components().size();
00193 }
00194
00195 static int updateComponentCountDimension(const std::vector<EuclideanProjection*> &proj,
00196 std::vector<double> &cellSizes, bool increase)
00197 {
00198 unsigned int dim = cellSizes.size();
00199 const double factor = increase ? DIMENSION_UPDATE_FACTOR : 1.0 / DIMENSION_UPDATE_FACTOR;
00200
00201 int bestD = -1;
00202 unsigned int best = 0;
00203 for (unsigned int d = 0 ; d < dim ; ++d)
00204 {
00205 double backup = cellSizes[d];
00206 cellSizes[d] *= factor;
00207 unsigned int nc = getComponentCount(proj, cellSizes);
00208 if (bestD < 0 || (increase && nc > best) || (!increase && nc < best))
00209 {
00210 best = nc;
00211 bestD = d;
00212 }
00213 cellSizes[d] = backup;
00214 }
00215 cellSizes[bestD] *= factor;
00216 return bestD;
00217 }
00218
00219 }
00220 }
00222
00223
00224
00225
00226 void ompl::base::ProjectionEvaluator::computeCellSizes(const std::vector<const State*> &states)
00227 {
00228 setup();
00229
00230 msg_.debug("Computing projections from %u states", states.size());
00231
00232 unsigned int dim = getDimension();
00233 std::vector<double> low(dim, std::numeric_limits<double>::infinity());
00234 std::vector<double> high(dim, -std::numeric_limits<double>::infinity());
00235 std::vector<EuclideanProjection*> proj(states.size());
00236 std::vector<ProjectionCoordinates> coord(states.size());
00237
00238 for (std::size_t i = 0 ; i < states.size() ; ++i)
00239 {
00240 proj[i] = new EuclideanProjection(dim);
00241 project(states[i], *proj[i]);
00242 for (std::size_t j = 0 ; j < dim ; ++j)
00243 {
00244 if (low[j] > proj[i]->values[j])
00245 low[j] = proj[i]->values[j];
00246 if (high[j] < proj[i]->values[j])
00247 high[j] = proj[i]->values[j];
00248 }
00249 }
00250
00251 bool dir1 = false, dir2 = false;
00252 do
00253 {
00254 for (std::size_t i = 0 ; i < proj.size() ; ++i)
00255 computeCoordinates(*proj[i], coord[i]);
00256 const Grid<int> &g = constructGrid(dim, coord);
00257
00258 const std::vector< std::vector<Grid<int>::Cell*> > &comp = g.components();
00259 if (comp.size() > 0)
00260 {
00261 std::size_t n = comp.size() / 10;
00262 if (n < 1)
00263 n = 1;
00264 std::size_t s = 0;
00265 for (std::size_t i = 0 ; i < n ; ++i)
00266 s += comp[i].size();
00267 double f = (double)s / (double)g.size();
00268
00269 msg_.debug("There are %u connected components in the projected grid. The first 10%% fractions is %f", comp.size(), f);
00270
00271 if (f < 0.7)
00272 {
00273 dir1 = true;
00274 int bestD = updateComponentCountDimension(proj, cellSizes_, true);
00275 msg_.debug("Increasing cell size in dimension %d to %f", bestD, cellSizes_[bestD]);
00276 }
00277 else
00278 if (f > 0.9)
00279 {
00280 dir2 = true;
00281 int bestD = updateComponentCountDimension(proj, cellSizes_, false);
00282 msg_.debug("Decreasing cell size in dimension %d to %f", bestD, cellSizes_[bestD]);
00283 }
00284 else
00285 {
00286 msg_.debug("No more changes made to cell sizes");
00287 break;
00288 }
00289 }
00290 } while (dir1 ^ dir2);
00291
00292 for (unsigned int i = 0 ; i < proj.size() ; ++i)
00293 delete proj[i];
00294
00295
00296 setCellSizes(cellSizes_);
00297 }
00298
00299 void ompl::base::ProjectionEvaluator::inferCellSizes(void)
00300 {
00301 cellSizesWereInferred_ = true;
00302 unsigned int dim = getDimension();
00303 if (dim > 0)
00304 {
00305 ManifoldStateSamplerPtr sampler = manifold_->allocStateSampler();
00306 State *s = manifold_->allocState();
00307 EuclideanProjection proj(dim);
00308
00309 std::vector<double> low(dim, std::numeric_limits<double>::infinity());
00310 std::vector<double> high(dim, -std::numeric_limits<double>::infinity());
00311
00312 for (unsigned int i = 0 ; i < magic::PROJECTION_EXTENTS_SAMPLES ; ++i)
00313 {
00314 sampler->sampleUniform(s);
00315 project(s, proj);
00316 for (unsigned int j = 0 ; j < dim ; ++j)
00317 {
00318 if (low[j] > proj.values[j])
00319 low[j] = proj.values[j];
00320 if (high[j] < proj.values[j])
00321 high[j] = proj.values[j];
00322 }
00323 }
00324
00325 manifold_->freeState(s);
00326
00327 cellSizes_.resize(dim);
00328 for (unsigned int j = 0 ; j < dim ; ++j)
00329 {
00330 cellSizes_[j] = (high[j] - low[j]) / magic::PROJECTION_DIMENSION_SPLITS;
00331 if (cellSizes_[j] < std::numeric_limits<double>::epsilon())
00332 {
00333 cellSizes_[j] = 1.0;
00334 msg_.warn("Inferred cell size for dimension %u of a projection for manifold %s is 0. Setting arbitrary value of 1 instead.",
00335 j, manifold_->getName().c_str());
00336 }
00337 }
00338 }
00339 }
00340
00341 void ompl::base::ProjectionEvaluator::setup(void)
00342 {
00343 if (defaultCellSizes_)
00344 defaultCellSizes();
00345
00346 if ((cellSizes_.size() == 0 && getDimension() > 0) || cellSizesWereInferred_)
00347 inferCellSizes();
00348
00349 checkCellSizes();
00350 }
00351
00352 void ompl::base::ProjectionEvaluator::computeCoordinates(const EuclideanProjection &projection, ProjectionCoordinates &coord) const
00353 {
00354 computeCoordinatesHelper(cellSizes_, projection, coord);
00355 }
00356
00357 void ompl::base::ProjectionEvaluator::printSettings(std::ostream &out) const
00358 {
00359 out << "Projection of dimension " << getDimension() << std::endl;
00360 out << "Cell sizes";
00361 if (cellSizesWereInferred_)
00362 out << " (inferred by sampling)";
00363 else
00364 {
00365 if (defaultCellSizes_)
00366 out << " (computed defaults)";
00367 else
00368 out << " (set by user)";
00369 }
00370 out << ": [";
00371 for (unsigned int i = 0 ; i < cellSizes_.size() ; ++i)
00372 {
00373 out << cellSizes_[i];
00374 if (i + 1 < cellSizes_.size())
00375 out << ' ';
00376 }
00377 out << ']' << std::endl;
00378 }
00379
00380 void ompl::base::ProjectionEvaluator::printProjection(const EuclideanProjection &projection, std::ostream &out) const
00381 {
00382 unsigned int d = getDimension();
00383 if (d > 0)
00384 {
00385 --d;
00386 for (unsigned int i = 0 ; i < d ; ++i)
00387 out << projection.values[i] << ' ';
00388 out << projection.values[d] << std::endl;
00389 }
00390 else
00391 out << "NULL" << std::endl;
00392 }