Loading...
Searching...
No Matches
ProjectionEvaluator.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35/* Author: Ioan Sucan */
36
37#include "ompl/base/StateSpace.h"
38#include "ompl/base/ProjectionEvaluator.h"
39#include "ompl/util/Exception.h"
40#include "ompl/util/RandomNumbers.h"
41#include "ompl/tools/config/MagicConstants.h"
42#include <Eigen/SVD>
43#include <cmath>
44#include <cstring>
45#include <limits>
46#include <utility>
47
49 const unsigned int to,
50 const std::vector<double> &scale)
51{
52 RNG rng;
53 Matrix projection(to, from);
54
55 for (unsigned int j = 0; j < from; ++j)
56 {
57 if (scale.size() == from && fabs(scale[j]) < std::numeric_limits<double>::epsilon())
58 projection.col(j).setZero();
59 else
60 for (unsigned int i = 0; i < to; ++i)
61 projection(i, j) = rng.gaussian01();
62 }
63
64 projection = Eigen::JacobiSVD<Eigen::MatrixXd>(projection, Eigen::ComputeThinV).matrixV().transpose();
65
66 assert(scale.size() == from || scale.size() == 0);
67 if (scale.size() == from)
68 {
69 unsigned int z = 0;
70 for (unsigned int i = 0; i < from; ++i)
71 {
72 if (fabs(scale[i]) < std::numeric_limits<double>::epsilon())
73 z++;
74 else
75 projection.col(i) /= scale[i];
76 }
77 if (z == from)
78 OMPL_WARN("Computed projection matrix is all 0s");
79 }
80 return projection;
81}
82
84 const unsigned int to)
85{
86 return ComputeRandom(from, to, std::vector<double>());
87}
88
89void ompl::base::ProjectionMatrix::computeRandom(const unsigned int from, const unsigned int to,
90 const std::vector<double> &scale)
91{
92 mat = ComputeRandom(from, to, scale);
93}
94
95void ompl::base::ProjectionMatrix::computeRandom(const unsigned int from, const unsigned int to)
96{
97 mat = ComputeRandom(from, to);
98}
99
100void ompl::base::ProjectionMatrix::project(const double *from, Eigen::Ref<Eigen::VectorXd> to) const
101{
102 to = mat * Eigen::Map<const Eigen::VectorXd>(from, mat.cols());
103}
104
105void ompl::base::ProjectionMatrix::print(std::ostream &out) const
106{
107 out << mat << std::endl;
108}
109
110ompl::base::ProjectionEvaluator::ProjectionEvaluator(const StateSpace *space)
111 : space_(space), bounds_(0), estimatedBounds_(0), defaultCellSizes_(true), cellSizesWereInferred_(false)
112{
113 params_.declareParam<double>("cellsize_factor", [this](double factor) { mulCellSizes(factor); });
114}
115
116ompl::base::ProjectionEvaluator::ProjectionEvaluator(const StateSpacePtr &space)
117 : space_(space.get()), bounds_(0), estimatedBounds_(0), defaultCellSizes_(true), cellSizesWereInferred_(false)
118{
119 params_.declareParam<double>("cellsize_factor", [this](double factor) { mulCellSizes(factor); });
120}
121
122ompl::base::ProjectionEvaluator::~ProjectionEvaluator() = default;
123
125{
126 return !defaultCellSizes_ && !cellSizesWereInferred_;
127}
128
129void ompl::base::ProjectionEvaluator::setCellSizes(const std::vector<double> &cellSizes)
130{
131 defaultCellSizes_ = false;
132 cellSizesWereInferred_ = false;
133 cellSizes_ = cellSizes;
134 checkCellSizes();
135}
136
138{
139 bounds_ = bounds;
140 checkBounds();
141}
142
143void ompl::base::ProjectionEvaluator::setCellSizes(unsigned int dim, double cellSize)
144{
145 if (cellSizes_.size() <= dim)
146 OMPL_ERROR("Dimension %u is not defined for projection evaluator", dim);
147 else
148 {
149 std::vector<double> c = cellSizes_;
150 c[dim] = cellSize;
151 setCellSizes(c);
152 }
153}
154
156{
157 if (cellSizes_.size() > dim)
158 return cellSizes_[dim];
159 OMPL_ERROR("Dimension %u is not defined for projection evaluator", dim);
160 return 0.0;
161}
162
164{
165 if (cellSizes_.size() == getDimension())
166 {
167 std::vector<double> c(cellSizes_.size());
168 for (std::size_t i = 0; i < cellSizes_.size(); ++i)
169 c[i] = cellSizes_[i] * factor;
170 setCellSizes(c);
171 }
172}
173
175{
176 if (getDimension() <= 0)
177 throw Exception("Dimension of projection needs to be larger than 0");
178 if (cellSizes_.size() != getDimension())
179 throw Exception("Number of dimensions in projection space does not match number of cell sizes");
180}
181
183{
184 bounds_.check();
185 if (hasBounds() && bounds_.low.size() != getDimension())
186 throw Exception("Number of dimensions in projection space does not match dimension of bounds");
187}
188
192
194namespace ompl
195{
196 namespace base
197 {
198 static inline void computeCoordinatesHelper(const std::vector<double> &cellSizes,
199 const Eigen::Ref<Eigen::VectorXd> &projection,
200 Eigen::Ref<Eigen::VectorXi> coord)
201 {
202 // compute floor(projection ./ cellSizes)
203 coord = (projection.array() / Eigen::Map<const Eigen::VectorXd>(cellSizes.data(), cellSizes.size()).array())
204 .floor()
205 .cast<int>();
206 }
207 } // namespace base
208} // namespace ompl
210
212{
213 if (estimatedBounds_.low.empty())
214 estimateBounds();
215 bounds_ = estimatedBounds_;
216}
217
219{
220 unsigned int dim = getDimension();
221 estimatedBounds_.resize(dim);
222 if (dim > 0)
223 {
224 StateSamplerPtr sampler = space_->allocStateSampler();
225 State *s = space_->allocState();
226 Eigen::VectorXd proj(dim);
227
228 estimatedBounds_.setLow(std::numeric_limits<double>::infinity());
229 estimatedBounds_.setHigh(-std::numeric_limits<double>::infinity());
230
231 for (unsigned int i = 0; i < magic::PROJECTION_EXTENTS_SAMPLES; ++i)
232 {
233 sampler->sampleUniform(s);
234 project(s, proj);
235 for (unsigned int j = 0; j < dim; ++j)
236 {
237 if (estimatedBounds_.low[j] > proj[j])
238 estimatedBounds_.low[j] = proj[j];
239 if (estimatedBounds_.high[j] < proj[j])
240 estimatedBounds_.high[j] = proj[j];
241 }
242 }
243 // make bounding box 10% larger (5% padding on each side)
244 std::vector<double> diff(estimatedBounds_.getDifference());
245 for (unsigned int j = 0; j < dim; ++j)
246 {
247 estimatedBounds_.low[j] -= magic::PROJECTION_EXPAND_FACTOR * diff[j];
248 estimatedBounds_.high[j] += magic::PROJECTION_EXPAND_FACTOR * diff[j];
249 }
250
251 space_->freeState(s);
252 }
253}
254
256{
257 cellSizesWereInferred_ = true;
258 if (!hasBounds())
259 inferBounds();
260 unsigned int dim = getDimension();
261 cellSizes_.resize(dim);
262 for (unsigned int j = 0; j < dim; ++j)
263 {
264 cellSizes_[j] = (bounds_.high[j] - bounds_.low[j]) / magic::PROJECTION_DIMENSION_SPLITS;
265 if (cellSizes_[j] < std::numeric_limits<double>::epsilon())
266 {
267 cellSizes_[j] = 1.0;
268 OMPL_WARN("Inferred cell size for dimension %u of a projection for state space %s is 0. Setting arbitrary "
269 "value of 1 instead.",
270 j, space_->getName().c_str());
271 }
272 }
273}
274
276{
277 if (defaultCellSizes_)
278 defaultCellSizes();
279
280 if ((cellSizes_.size() == 0 && getDimension() > 0) || cellSizesWereInferred_)
281 inferCellSizes();
282
283 checkCellSizes();
284 checkBounds();
285
286 unsigned int dim = getDimension();
287 for (unsigned int i = 0; i < dim; ++i)
288 params_.declareParam<double>("cellsize." + std::to_string(i),
289 [this, i](double cellsize) { setCellSizes(i, cellsize); },
290 [this, i] { return getCellSizes(i); });
291}
292
293void ompl::base::ProjectionEvaluator::computeCoordinates(const Eigen::Ref<Eigen::VectorXd> &projection,
294 Eigen::Ref<Eigen::VectorXi> coord) const
295{
296 computeCoordinatesHelper(cellSizes_, projection, coord);
297}
298
300{
301 out << "Projection of dimension " << getDimension() << std::endl;
302 out << "Cell sizes";
303 if (cellSizesWereInferred_)
304 out << " (inferred by sampling)";
305 else
306 {
307 if (defaultCellSizes_)
308 out << " (computed defaults)";
309 else
310 out << " (set by user)";
311 }
312 out << ": [";
313 for (unsigned int i = 0; i < cellSizes_.size(); ++i)
314 {
315 out << cellSizes_[i];
316 if (i + 1 < cellSizes_.size())
317 out << ' ';
318 }
319 out << ']' << std::endl;
320}
321
322void ompl::base::ProjectionEvaluator::printProjection(const Eigen::Ref<Eigen::VectorXd> &projection,
323 std::ostream &out) const
324{
325 out << projection << std::endl;
326}
327
329 ProjectionEvaluatorPtr projToUse)
330 : ProjectionEvaluator(space), index_(index), specifiedProj_(std::move(projToUse))
331{
332 if (!space_->isCompound())
333 throw Exception("Cannot construct a subspace projection evaluator for a space that is not compound");
335 throw Exception("State space " + space_->getName() + " does not have a subspace at index " +
336 std::to_string(index_));
337}
338
340{
341 if (specifiedProj_)
342 proj_ = specifiedProj_;
343 else
344 proj_ = space_->as<CompoundStateSpace>()->getSubspace(index_)->getDefaultProjection();
345 if (!proj_)
346 throw Exception("No projection specified for subspace at index " + std::to_string(index_));
347
348 cellSizes_ = proj_->getCellSizes();
350}
351
353{
354 return proj_->getDimension();
355}
356
357void ompl::base::SubspaceProjectionEvaluator::project(const State *state, Eigen::Ref<Eigen::VectorXd> projection) const
358{
359 proj_->project(state->as<CompoundState>()->components[index_], projection);
360}
The exception type for ompl.
Definition Exception.h:47
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
double gaussian01()
Generate a random real using a normal distribution with mean 0 and variance 1.
A space to allow the composition of state spaces.
Definition StateSpace.h:574
unsigned int getSubspaceCount() const
Get the number of state spaces that make up the compound state space.
Definition of a compound state.
Definition State.h:87
State ** components
The components that make up a compound state.
Definition State.h:128
void declareParam(const std::string &name, const typename SpecificParam< T >::SetterFn &setter, const typename SpecificParam< T >::GetterFn &getter=[] { return T();})
This function declares a parameter name, and specifies the setter and getter functions.
A shared pointer wrapper for ompl::base::ProjectionEvaluator.
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
bool userConfigured() const
Return true if any user configuration has been done to this projection evaluator (setCellSizes() was ...
void setBounds(const RealVectorBounds &bounds)
Set bounds on the projection. The PDST planner needs to known the bounds on the projection....
virtual void setCellSizes(const std::vector< double > &cellSizes)
Define the size (in each dimension) of a grid cell. The number of sizes set here must be the same as ...
virtual void defaultCellSizes()
Set the default cell dimensions for this projection. The default implementation of this function is e...
void mulCellSizes(double factor)
Multiply the cell sizes in each dimension by a specified factor factor. This function does nothing if...
virtual void printSettings(std::ostream &out=std::cout) const
Print settings about this projection.
const StateSpace * space_
The state space this projection operates on.
void inferCellSizes()
Sample the state space and decide on default cell sizes. This function is called by setup() if no cel...
virtual void printProjection(const Eigen::Ref< Eigen::VectorXd > &projection, std::ostream &out=std::cout) const
Print a euclidean projection.
ParamSet params_
The set of parameters for this projection.
void estimateBounds()
Fill estimatedBounds_ with an approximate bounding box for the projection space (via sampling)
const std::vector< double > & getCellSizes() const
Get the size (each dimension) of a grid cell
void checkCellSizes() const
Check if cell dimensions match projection dimension.
void checkBounds() const
Check if the projection dimension matched the dimension of the bounds.
void inferBounds()
Compute an approximation of the bounds for this projection space. getBounds() will then report the co...
void computeCoordinates(const Eigen::Ref< Eigen::VectorXd > &projection, Eigen::Ref< Eigen::VectorXi > coord) const
Compute integer coordinates for a projection.
virtual void setup()
Perform configuration steps, if needed.
void project(const double *from, Eigen::Ref< Eigen::VectorXd > to) const
Multiply the vector from by the contained projection matrix to obtain the vector to.
void computeRandom(unsigned int from, unsigned int to, const std::vector< double > &scale)
Wrapper for ComputeRandom(from, to, scale)
void print(std::ostream &out=std::cout) const
Print the contained projection matrix to a stram.
Eigen::MatrixXd Matrix
Datatype for projection matrices.
static Matrix ComputeRandom(unsigned int from, unsigned int to, const std::vector< double > &scale)
Compute a random projection matrix with from columns and to rows. A vector with from elements can be ...
The lower and upper bounds for an Rn space.
A shared pointer wrapper for ompl::base::StateSampler.
A shared pointer wrapper for ompl::base::StateSpace.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition StateSpace.h:71
ProjectionEvaluatorPtr getDefaultProjection() const
Get the default projection.
T * as()
Cast this instance to a desired type.
Definition StateSpace.h:87
virtual bool isCompound() const
Check if the state space is compound.
const std::string & getName() const
Get the name of the state space.
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66
unsigned int index_
The index of the subspace from which to project.
void setup() override
Perform configuration steps, if needed.
SubspaceProjectionEvaluator(const StateSpace *space, unsigned int index, ProjectionEvaluatorPtr projToUse=ProjectionEvaluatorPtr())
The constructor states that for space space, the projection to use is the same as the component at po...
void project(const State *state, Eigen::Ref< Eigen::VectorXd > projection) const override
Compute the projection as an array of double values.
unsigned int getDimension() const override
Return the dimension of the projection defined by this evaluator.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition Console.h:66
Main namespace. Contains everything in this library.
STL namespace.