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 // We need this to create a temporary uBLAS vector from a C-style array without copying data
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             // subtract projection
00073             row -= inner_prod(row, prevRow) * prevRow;
00074         }
00075         // normalize
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     // create a temporary uBLAS vector from a C-style array without copying data
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         static Grid<int> constructGrid(unsigned int dim, const std::vector<ProjectionCoordinates> &coord)
00159         {
00160             Grid<int> g(dim);
00161             for (std::size_t i = 0 ; i < coord.size() ; ++i)
00162             {
00163                 Grid<int>::Cell *c = g.getCell(coord[i]);
00164                 if (c)
00165                     c->data++;
00166                 else
00167                 {
00168                     Grid<int>::Cell *c = g.createCell(coord[i]);
00169                     c->data = 1;
00170                     g.add(c);
00171                 }
00172             }
00173             return g;
00174         }
00175 
00176         static unsigned int getComponentCount(const std::vector<EuclideanProjection*> &proj,
00177                                               const std::vector<double> &cellSizes)
00178         {
00179             std::vector<ProjectionCoordinates> coord(proj.size());
00180             for (std::size_t i = 0 ; i < proj.size() ; ++i)
00181                 computeCoordinatesHelper(cellSizes, *proj[i], coord[i]);
00182             return constructGrid(cellSizes.size(), coord).components().size();
00183         }
00184 
00185         static int updateComponentCountDimension(const std::vector<EuclideanProjection*> &proj,
00186                                                  std::vector<double> &cellSizes, bool increase)
00187         {
00188             unsigned int dim = cellSizes.size();
00189             const double factor = increase ? DIMENSION_UPDATE_FACTOR : 1.0 / DIMENSION_UPDATE_FACTOR;
00190 
00191             int bestD = -1;
00192             unsigned int best = 0;
00193             for (unsigned int d = 0 ; d < dim ; ++d)
00194             {
00195                 double backup = cellSizes[d];
00196                 cellSizes[d] *= factor;
00197                 unsigned int nc = getComponentCount(proj, cellSizes);
00198                 if (bestD < 0 || (increase && nc > best) || (!increase && nc < best))
00199                 {
00200                     best = nc;
00201                     bestD = d;
00202                 }
00203                 cellSizes[d] = backup;
00204             }
00205             cellSizes[bestD] *= factor;
00206             return bestD;
00207         }
00208         */
00209     }
00210 }
00212 
00213 
00214 
00215 /*
00216 void ompl::base::ProjectionEvaluator::computeCellSizes(const std::vector<const State*> &states)
00217 {
00218     setup();
00219 
00220     msg_.debug("Computing projections from %u states", states.size());
00221 
00222     unsigned int dim = getDimension();
00223     std::vector<double> low(dim, std::numeric_limits<double>::infinity());
00224     std::vector<double> high(dim, -std::numeric_limits<double>::infinity());
00225     std::vector<EuclideanProjection*>  proj(states.size());
00226     std::vector<ProjectionCoordinates> coord(states.size());
00227 
00228     for (std::size_t i = 0 ; i < states.size() ; ++i)
00229     {
00230         proj[i] = new EuclideanProjection(dim);
00231         project(states[i], *proj[i]);
00232         for (std::size_t j = 0 ; j < dim ; ++j)
00233         {
00234             if (low[j] > proj[i]->values[j])
00235                 low[j] = proj[i]->values[j];
00236             if (high[j] < proj[i]->values[j])
00237                 high[j] = proj[i]->values[j];
00238         }
00239     }
00240 
00241     bool dir1 = false, dir2 = false;
00242     do
00243     {
00244         for (std::size_t i = 0 ; i < proj.size() ; ++i)
00245             computeCoordinates(*proj[i], coord[i]);
00246         const Grid<int> &g = constructGrid(dim, coord);
00247 
00248         const std::vector< std::vector<Grid<int>::Cell*> > &comp = g.components();
00249         if (comp.size() > 0)
00250         {
00251             std::size_t n = comp.size() / 10;
00252             if (n < 1)
00253                 n = 1;
00254             std::size_t s = 0;
00255             for (std::size_t i = 0 ; i < n ; ++i)
00256                 s += comp[i].size();
00257             double f = (double)s / (double)g.size();
00258 
00259             msg_.debug("There are %u connected components in the projected grid. The first 10%% fractions is %f", comp.size(), f);
00260 
00261             if (f < 0.7)
00262             {
00263                 dir1 = true;
00264                 int bestD = updateComponentCountDimension(proj, cellSizes_, true);
00265                 msg_.debug("Increasing cell size in dimension %d to %f", bestD, cellSizes_[bestD]);
00266             }
00267             else
00268                 if (f > 0.9)
00269                 {
00270                     dir2 = true;
00271                     int bestD = updateComponentCountDimension(proj, cellSizes_, false);
00272                     msg_.debug("Decreasing cell size in dimension %d to %f", bestD, cellSizes_[bestD]);
00273                 }
00274                 else
00275                 {
00276                     msg_.debug("No more changes made to cell sizes");
00277                     break;
00278                 }
00279         }
00280     } while (dir1 ^ dir2);
00281 
00282     for (unsigned int i = 0 ; i < proj.size() ; ++i)
00283         delete proj[i];
00284 
00285     // make sure all flags are set correctly
00286     setCellSizes(cellSizes_);
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 }