ompl/base/src/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 ompl::base::ProjectionMatrix::Matrix ompl::base::ProjectionMatrix::ComputeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale)
00053 {
00054     using namespace boost::numeric::ublas;
00055 
00056     RNG rng;
00057     Matrix projection(to, from);
00058 
00059     for (unsigned int j = 0 ; j < from ; ++j)
00060     {
00061         if (scale.size() == from && fabs(scale[j]) < std::numeric_limits<double>::epsilon())
00062             boost::numeric::ublas::column(projection, j) = boost::numeric::ublas::zero_vector<double>(to);
00063         else
00064             for (unsigned int i = 0 ; i < to ; ++i)
00065                 projection(i, j) = rng.gaussian01();
00066     }
00067 
00068     for (unsigned int i = 0 ; i < to ; ++i)
00069     {
00070         matrix_row<Matrix> row(projection, i);
00071         for (unsigned int j = 0 ; j < i ; ++j)
00072         {
00073             matrix_row<Matrix> prevRow(projection, j);
00074             // subtract projection
00075             row -= inner_prod(row, prevRow) * prevRow;
00076         }
00077         // normalize
00078         row /= norm_2(row);
00079     }
00080 
00081     assert(scale.size() == from || scale.size() == 0);
00082     if (scale.size() == from)
00083     {
00084         unsigned int z = 0;
00085         for (unsigned int i = 0 ; i < from ; ++i)
00086         {
00087             if (fabs(scale[i]) < std::numeric_limits<double>::epsilon())
00088                 z++;
00089             else
00090                 boost::numeric::ublas::column(projection, i) /= scale[i];
00091         }
00092         if (z == from)
00093             OMPL_WARN("Computed projection matrix is all 0s");
00094     }
00095     return projection;
00096 }
00097 
00098 ompl::base::ProjectionMatrix::Matrix ompl::base::ProjectionMatrix::ComputeRandom(const unsigned int from, const unsigned int to)
00099 {
00100     return ComputeRandom(from, to, std::vector<double>());
00101 }
00102 
00103 void ompl::base::ProjectionMatrix::computeRandom(const unsigned int from, const unsigned int to, const std::vector<double> &scale)
00104 {
00105     mat = ComputeRandom(from, to, scale);
00106 }
00107 
00108 void ompl::base::ProjectionMatrix::computeRandom(const unsigned int from, const unsigned int to)
00109 {
00110     mat = ComputeRandom(from, to);
00111 }
00112 
00113 void ompl::base::ProjectionMatrix::project(const double *from, EuclideanProjection& to) const
00114 {
00115     using namespace boost::numeric::ublas;
00116     // create a temporary uBLAS vector from a C-style array without copying data
00117     shallow_array_adaptor<const double> tmp1(mat.size2(), from);
00118     vector<double, shallow_array_adaptor<const double> > tmp2(mat.size2(), tmp1);
00119     to = prod(mat, tmp2);
00120 }
00121 
00122 void ompl::base::ProjectionMatrix::print(std::ostream &out) const
00123 {
00124     out << mat << std::endl;
00125 }
00126 
00127 ompl::base::ProjectionEvaluator::ProjectionEvaluator(const StateSpace *space) :
00128     space_(space),
00129     bounds_(0), estimatedBounds_(0),
00130     defaultCellSizes_(true), cellSizesWereInferred_(false)
00131 {
00132     params_.declareParam<double>("cellsize_factor", boost::bind(&ProjectionEvaluator::mulCellSizes, this, _1));
00133 }
00134 
00135 ompl::base::ProjectionEvaluator::ProjectionEvaluator(const StateSpacePtr &space) :
00136     space_(space.get()),
00137     bounds_(0), estimatedBounds_(0),
00138     defaultCellSizes_(true), cellSizesWereInferred_(false)
00139 {
00140     params_.declareParam<double>("cellsize_factor", boost::bind(&ProjectionEvaluator::mulCellSizes, this, _1));
00141 }
00142 
00143 ompl::base::ProjectionEvaluator::~ProjectionEvaluator()
00144 {
00145 }
00146 
00147 bool ompl::base::ProjectionEvaluator::userConfigured() const
00148 {
00149     return !defaultCellSizes_ && !cellSizesWereInferred_;
00150 }
00151 
00152 void ompl::base::ProjectionEvaluator::setCellSizes(const std::vector<double> &cellSizes)
00153 {
00154     defaultCellSizes_ = false;
00155     cellSizesWereInferred_ = false;
00156     cellSizes_ = cellSizes;
00157     checkCellSizes();
00158 }
00159 
00160 void ompl::base::ProjectionEvaluator::setBounds(const RealVectorBounds &bounds)
00161 {
00162     bounds_ = bounds;
00163     checkBounds();
00164 }
00165 
00166 void ompl::base::ProjectionEvaluator::setCellSizes(unsigned int dim, double cellSize)
00167 {
00168     if (cellSizes_.size() >= dim)
00169         OMPL_ERROR("Dimension %u is not defined for projection evaluator", dim);
00170     else
00171     {
00172         std::vector<double> c = cellSizes_;
00173         c[dim] = cellSize;
00174         setCellSizes(c);
00175     }
00176 }
00177 
00178 double ompl::base::ProjectionEvaluator::getCellSizes(unsigned int dim) const
00179 {
00180     if (cellSizes_.size() > dim)
00181         return cellSizes_[dim];
00182     OMPL_ERROR("Dimension %u is not defined for projection evaluator", dim);
00183     return 0.0;
00184 }
00185 
00186 void ompl::base::ProjectionEvaluator::mulCellSizes(double factor)
00187 {
00188     if (cellSizes_.size() == getDimension())
00189     {
00190         std::vector<double> c(cellSizes_.size());
00191         for (std::size_t i = 0 ; i < cellSizes_.size() ; ++i)
00192             c[i] = cellSizes_[i] * factor;
00193         setCellSizes(c);
00194     }
00195 }
00196 
00197 void ompl::base::ProjectionEvaluator::checkCellSizes() const
00198 {
00199     if (getDimension() <= 0)
00200         throw Exception("Dimension of projection needs to be larger than 0");
00201     if (cellSizes_.size() != getDimension())
00202         throw Exception("Number of dimensions in projection space does not match number of cell sizes");
00203 }
00204 
00205 void ompl::base::ProjectionEvaluator::checkBounds() const
00206 {
00207     bounds_.check();
00208     if (hasBounds() && bounds_.low.size() != getDimension())
00209         throw Exception("Number of dimensions in projection space does not match dimension of bounds");
00210 }
00211 
00212 void ompl::base::ProjectionEvaluator::defaultCellSizes()
00213 {
00214 }
00215 
00217 namespace ompl
00218 {
00219     namespace base
00220     {
00221 
00222         static inline void computeCoordinatesHelper(const std::vector<double> &cellSizes, const EuclideanProjection &projection, ProjectionCoordinates &coord)
00223         {
00224             const std::size_t dim = cellSizes.size();
00225             coord.resize(dim);
00226             for (unsigned int i = 0 ; i < dim ; ++i)
00227                 coord[i] = (int)floor(projection(i)/cellSizes[i]);
00228         }
00229     }
00230 }
00232 
00233 void ompl::base::ProjectionEvaluator::inferBounds()
00234 {
00235     if (estimatedBounds_.low.empty())
00236         estimateBounds();
00237     bounds_ = estimatedBounds_;
00238 }
00239 
00240 void ompl::base::ProjectionEvaluator::estimateBounds()
00241 {
00242     unsigned int dim = getDimension();
00243     estimatedBounds_.resize(dim);
00244     if (dim > 0)
00245     {
00246         StateSamplerPtr sampler = space_->allocStateSampler();
00247         State *s = space_->allocState();
00248         EuclideanProjection proj(dim);
00249 
00250         estimatedBounds_.setLow(std::numeric_limits<double>::infinity());
00251         estimatedBounds_.setHigh(-std::numeric_limits<double>::infinity());
00252 
00253         for (unsigned int i = 0 ; i < magic::PROJECTION_EXTENTS_SAMPLES ; ++i)
00254         {
00255             sampler->sampleUniform(s);
00256             project(s, proj);
00257             for (unsigned int j = 0 ; j < dim ; ++j)
00258             {
00259                 if (estimatedBounds_.low[j] > proj[j])
00260                     estimatedBounds_.low[j] = proj[j];
00261                 if (estimatedBounds_.high[j] < proj[j])
00262                     estimatedBounds_.high[j] = proj[j];
00263             }
00264         }
00265         // make bounding box 10% larger (5% padding on each side)
00266         std::vector<double> diff(estimatedBounds_.getDifference());
00267         for (unsigned int j = 0; j < dim; ++j)
00268         {
00269             estimatedBounds_.low[j] -= magic::PROJECTION_EXPAND_FACTOR * diff[j];
00270             estimatedBounds_.high[j] += magic::PROJECTION_EXPAND_FACTOR * diff[j];
00271         }
00272 
00273         space_->freeState(s);
00274     }
00275 }
00276 
00277 void ompl::base::ProjectionEvaluator::inferCellSizes()
00278 {
00279     cellSizesWereInferred_ = true;
00280     if (!hasBounds())
00281         inferBounds();
00282     unsigned int dim = getDimension();
00283     cellSizes_.resize(dim);
00284     for (unsigned int j = 0 ; j < dim ; ++j)
00285     {
00286         cellSizes_[j] = (bounds_.high[j] - bounds_.low[j]) / magic::PROJECTION_DIMENSION_SPLITS;
00287         if (cellSizes_[j] < std::numeric_limits<double>::epsilon())
00288         {
00289             cellSizes_[j] = 1.0;
00290             OMPL_WARN("Inferred cell size for dimension %u of a projection for state space %s is 0. Setting arbitrary value of 1 instead.",
00291                       j, space_->getName().c_str());
00292         }
00293     }
00294 }
00295 
00296 void ompl::base::ProjectionEvaluator::setup()
00297 {
00298     if (defaultCellSizes_)
00299         defaultCellSizes();
00300 
00301     if ((cellSizes_.size() == 0 && getDimension() > 0) || cellSizesWereInferred_)
00302         inferCellSizes();
00303 
00304     checkCellSizes();
00305     checkBounds();
00306 
00307     unsigned int dim = getDimension();
00308     for (unsigned int i = 0 ; i < dim ; ++i)
00309         params_.declareParam<double>("cellsize." + boost::lexical_cast<std::string>(i),
00310                                      boost::bind(&ProjectionEvaluator::setCellSizes, this, i, _1),
00311                                      boost::bind(&ProjectionEvaluator::getCellSizes, this, i));
00312 }
00313 
00314 void ompl::base::ProjectionEvaluator::computeCoordinates(const EuclideanProjection &projection, ProjectionCoordinates &coord) const
00315 {
00316     computeCoordinatesHelper(cellSizes_, projection, coord);
00317 }
00318 
00319 void ompl::base::ProjectionEvaluator::printSettings(std::ostream &out) const
00320 {
00321     out << "Projection of dimension " << getDimension() << std::endl;
00322     out << "Cell sizes";
00323     if (cellSizesWereInferred_)
00324         out << " (inferred by sampling)";
00325     else
00326     {
00327         if (defaultCellSizes_)
00328             out << " (computed defaults)";
00329         else
00330             out << " (set by user)";
00331     }
00332     out << ": [";
00333     for (unsigned int i = 0 ; i < cellSizes_.size() ; ++i)
00334     {
00335         out << cellSizes_[i];
00336         if (i + 1 < cellSizes_.size())
00337             out << ' ';
00338     }
00339     out << ']' << std::endl;
00340 }
00341 
00342 void ompl::base::ProjectionEvaluator::printProjection(const EuclideanProjection &projection, std::ostream &out) const
00343 {
00344     out << projection << std::endl;
00345 }
00346 
00347 ompl::base::SubspaceProjectionEvaluator::SubspaceProjectionEvaluator(const StateSpace *space, unsigned int index, const ProjectionEvaluatorPtr &projToUse) :
00348     ProjectionEvaluator(space), index_(index), specifiedProj_(projToUse)
00349 {
00350     if (!space_->isCompound())
00351         throw Exception("Cannot construct a subspace projection evaluator for a space that is not compound");
00352     if (space_->as<CompoundStateSpace>()->getSubspaceCount() <= index_)
00353         throw Exception("State space " + space_->getName() + " does not have a subspace at index " + boost::lexical_cast<std::string>(index_));
00354 }
00355 
00356 void ompl::base::SubspaceProjectionEvaluator::setup()
00357 {
00358     if (specifiedProj_)
00359         proj_ = specifiedProj_;
00360     else
00361         proj_ = space_->as<CompoundStateSpace>()->getSubspace(index_)->getDefaultProjection();
00362     if (!proj_)
00363         throw Exception("No projection specified for subspace at index " + boost::lexical_cast<std::string>(index_));
00364 
00365     cellSizes_ = proj_->getCellSizes();
00366     ProjectionEvaluator::setup();
00367 }
00368 
00369 unsigned int ompl::base::SubspaceProjectionEvaluator::getDimension() const
00370 {
00371     return proj_->getDimension();
00372 }
00373 
00374 void ompl::base::SubspaceProjectionEvaluator::project(const State *state, EuclideanProjection &projection) const
00375 {
00376     proj_->project(state->as<CompoundState>()->components[index_], projection);
00377 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines