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 }