All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
ProjectionEvaluator.cpp
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2011, 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 <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 // static const double DIMENSION_UPDATE_FACTOR = 1.2;
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             // subtract projection
00074             row -= inner_prod(row, prevRow) * prevRow;
00075         }
00076         // normalize
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     // create a temporary uBLAS vector from a C-style array without copying data
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         static Grid<int> constructGrid(unsigned int dim, const std::vector<ProjectionCoordinates> &coord)
00205         {
00206             Grid<int> g(dim);
00207             for (std::size_t i = 0 ; i < coord.size() ; ++i)
00208             {
00209                 Grid<int>::Cell *c = g.getCell(coord[i]);
00210                 if (c)
00211                     c->data++;
00212                 else
00213                 {
00214                     Grid<int>::Cell *c = g.createCell(coord[i]);
00215                     c->data = 1;
00216                     g.add(c);
00217                 }
00218             }
00219             return g;
00220         }
00221 
00222         static unsigned int getComponentCount(const std::vector<EuclideanProjection*> &proj,
00223                                               const std::vector<double> &cellSizes)
00224         {
00225             std::vector<ProjectionCoordinates> coord(proj.size());
00226             for (std::size_t i = 0 ; i < proj.size() ; ++i)
00227                 computeCoordinatesHelper(cellSizes, *proj[i], coord[i]);
00228             return constructGrid(cellSizes.size(), coord).components().size();
00229         }
00230 
00231         static int updateComponentCountDimension(const std::vector<EuclideanProjection*> &proj,
00232                                                  std::vector<double> &cellSizes, bool increase)
00233         {
00234             unsigned int dim = cellSizes.size();
00235             const double factor = increase ? DIMENSION_UPDATE_FACTOR : 1.0 / DIMENSION_UPDATE_FACTOR;
00236 
00237             int bestD = -1;
00238             unsigned int best = 0;
00239             for (unsigned int d = 0 ; d < dim ; ++d)
00240             {
00241                 double backup = cellSizes[d];
00242                 cellSizes[d] *= factor;
00243                 unsigned int nc = getComponentCount(proj, cellSizes);
00244                 if (bestD < 0 || (increase && nc > best) || (!increase && nc < best))
00245                 {
00246                     best = nc;
00247                     bestD = d;
00248                 }
00249                 cellSizes[d] = backup;
00250             }
00251             cellSizes[bestD] *= factor;
00252             return bestD;
00253         }
00254         */
00255     }
00256 }
00258 
00259 
00260 
00261 /*
00262 void ompl::base::ProjectionEvaluator::computeCellSizes(const std::vector<const State*> &states)
00263 {
00264     setup();
00265 
00266     msg_.debug("Computing projections from %u states", states.size());
00267 
00268     unsigned int dim = getDimension();
00269     std::vector<double> low(dim, std::numeric_limits<double>::infinity());
00270     std::vector<double> high(dim, -std::numeric_limits<double>::infinity());
00271     std::vector<EuclideanProjection*>  proj(states.size());
00272     std::vector<ProjectionCoordinates> coord(states.size());
00273 
00274     for (std::size_t i = 0 ; i < states.size() ; ++i)
00275     {
00276         proj[i] = new EuclideanProjection(dim);
00277         project(states[i], *proj[i]);
00278         for (std::size_t j = 0 ; j < dim ; ++j)
00279         {
00280             if (low[j] > proj[i]->values[j])
00281                 low[j] = proj[i]->values[j];
00282             if (high[j] < proj[i]->values[j])
00283                 high[j] = proj[i]->values[j];
00284         }
00285     }
00286 
00287     bool dir1 = false, dir2 = false;
00288     do
00289     {
00290         for (std::size_t i = 0 ; i < proj.size() ; ++i)
00291             computeCoordinates(*proj[i], coord[i]);
00292         const Grid<int> &g = constructGrid(dim, coord);
00293 
00294         const std::vector< std::vector<Grid<int>::Cell*> > &comp = g.components();
00295         if (comp.size() > 0)
00296         {
00297             std::size_t n = comp.size() / 10;
00298             if (n < 1)
00299                 n = 1;
00300             std::size_t s = 0;
00301             for (std::size_t i = 0 ; i < n ; ++i)
00302                 s += comp[i].size();
00303             double f = (double)s / (double)g.size();
00304 
00305             msg_.debug("There are %u connected components in the projected grid. The first 10%% fractions is %f", comp.size(), f);
00306 
00307             if (f < 0.7)
00308             {
00309                 dir1 = true;
00310                 int bestD = updateComponentCountDimension(proj, cellSizes_, true);
00311                 msg_.debug("Increasing cell size in dimension %d to %f", bestD, cellSizes_[bestD]);
00312             }
00313             else
00314                 if (f > 0.9)
00315                 {
00316                     dir2 = true;
00317                     int bestD = updateComponentCountDimension(proj, cellSizes_, false);
00318                     msg_.debug("Decreasing cell size in dimension %d to %f", bestD, cellSizes_[bestD]);
00319                 }
00320                 else
00321                 {
00322                     msg_.debug("No more changes made to cell sizes");
00323                     break;
00324                 }
00325         }
00326     } while (dir1 ^ dir2);
00327 
00328     for (unsigned int i = 0 ; i < proj.size() ; ++i)
00329         delete proj[i];
00330 
00331     // make sure all flags are set correctly
00332     setCellSizes(cellSizes_);
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 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends