All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
ProjectionEvaluator.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan */
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             // subtract projection
00068             row -= (row * prevRow).sum() * prevRow;
00069         }
00070         // normalize
00071         row /= sqrt((row * row).sum());
00072     }
00073 
00074     // if we need to apply scaling, do so
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     // make sure all flags are set correctly
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 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator