Commits

Matt Maly  committed 8db1a98

fix up integer datatypes in decompositions

  • Participants
  • Parent commits bd534b3
  • Branches syclop-triangulate

Comments (0)

Files changed (7)

File src/ompl/control/planners/syclop/Decomposition.h

         {
         public:
 
-            /** \brief Constructor. Creates a Decomposition with a given number of regions, a given dimension, and a given set of bounds. */
-            Decomposition(const std::size_t dim, const base::RealVectorBounds& b, const int n = 0) : numRegions_(n), dimension_(dim), bounds_(b)
+            /** \brief Constructor. Creates a Decomposition with a given dimension
+                and a given set of bounds. Accepts as an optional argument a given
+                number of regions. */
+            Decomposition(unsigned int dim, const base::RealVectorBounds& b, unsigned int nreg = 0) : numRegions_(nreg), dimension_(dim), bounds_(b)
             {
                 if (dim > b.low.size())
                     throw Exception("Decomposition", "argument 'dim' exceeds dimension of given bounds");
             }
 
             /** \brief Returns the number of regions in this Decomposition. */
-            virtual int getNumRegions() const
+            virtual unsigned int getNumRegions() const
             {
                 return numRegions_;
             }
 
             /** \brief Returns the dimension of this Decomposition. */
-            virtual std::size_t getDimension() const
+            virtual unsigned int getDimension() const
             {
                 return dimension_;
             }
             }
 
             /** \brief Returns the volume of a given region in this Decomposition. */
-            virtual double getRegionVolume(const int rid) = 0;
+            virtual double getRegionVolume(unsigned int rid) = 0;
 
             /** \brief Returns the index of the region containing a given State.
              * Most often, this is obtained by first calling project().
             virtual void project(const base::State* s, std::vector<double>& coord) const = 0;
 
             /** \brief Stores a given region's neighbors into a given vector. */
-            virtual void getNeighbors(const int rid, std::vector<int>& neighbors) const = 0;
+            virtual void getNeighbors(unsigned int rid, std::vector<unsigned int>& neighbors) const = 0;
 
             /** \brief Samples a projected coordinate from a given region. */
-            virtual void sampleFromRegion(const int rid, RNG& rng, std::vector<double>& coord) const = 0;
+            virtual void sampleFromRegion(unsigned int rid, RNG& rng, std::vector<double>& coord) const = 0;
 
             /** \brief Samples a State using a projected coordinate and a StateSampler. */
             virtual void sampleFullState(const base::StateSamplerPtr& sampler, const std::vector<double>& coord, base::State* s) const = 0;
 
         protected:
-            virtual void setNumRegions(const int n)
+            virtual void setNumRegions(unsigned int n)
             {
                 numRegions_ = n;
             }
 
-            int numRegions_;
-            const std::size_t dimension_;
-            const base::RealVectorBounds bounds_;
+            unsigned int numRegions_;
+            unsigned int dimension_;
+            base::RealVectorBounds bounds_;
         };
     }
 }

File src/ompl/control/planners/syclop/GridDecomposition.h

         class GridDecomposition : public Decomposition
         {
         public:
-            /** \brief Constructor. Creates a GridDecomposition as a hypercube with a given dimension, side length, and bounds. */
-            GridDecomposition(const int len, const std::size_t dim, const base::RealVectorBounds& b);
+            /** \brief Constructor. Creates a GridDecomposition as a hypercube with a given dimension, side length, and bounds.
+                The cells of the hypercube are referenced by integer coordinates of the form
+                \f$(r_1,\ldots,r_k)\f$, where \f$ 0 \leq r_i < \texttt{len}\f$. */
+            GridDecomposition(unsigned int len, unsigned int dim, const base::RealVectorBounds& b);
 
             virtual ~GridDecomposition()
             {
             }
 
-            virtual double getRegionVolume(const int rid)
+            virtual double getRegionVolume(unsigned int rid)
             {
                 return cellVolume_;
             }
 
-            virtual void getNeighbors(const int rid, std::vector<int>& neighbors) const;
+            virtual void getNeighbors(unsigned int rid, std::vector<unsigned int>& neighbors) const;
 
             virtual int locateRegion(const base::State* s) const;
 
-            virtual void sampleFromRegion(const int rid, RNG& rng, std::vector<double>& coord) const;
+            virtual void sampleFromRegion(unsigned int rid, RNG& rng, std::vector<double>& coord) const;
 
         protected:
             /** \brief Helper method to return the bounds of a given region. */
-            virtual const base::RealVectorBounds& getRegionBounds(const int rid) const;
+            virtual const base::RealVectorBounds& getRegionBounds(unsigned int rid) const;
 
             /** \brief Converts a given region to a coordinate in the grid. */
-            void regionToGridCoord(int rid, std::vector<int>& coord) const;
+            void regionToGridCoord(unsigned int rid, std::vector<unsigned int>& coord) const;
 
             /** \brief Converts the given grid coordinate to its corresponding region ID. */
-            int gridCoordToRegion (const std::vector<int> &coord) const;
+            unsigned int gridCoordToRegion (const std::vector<unsigned int> &coord) const;
 
             /** \brief Converts a decomposition space coordinate to the ID of the region that contains iit. */
-            int coordToRegion(const std::vector<double>& coord) const;
+            unsigned int coordToRegion(const std::vector<double>& coord) const;
 
             /** \brief Converts a decomposition space coordinate to a grid coordinate. */
-            void coordToGridCoord(const std::vector<double>& coord, std::vector<int>& gridCoord) const;
+            void coordToGridCoord(const std::vector<double>& coord, std::vector<unsigned int>& gridCoord) const;
 
             /** \brief Computes the neighbors of the given region in a n-dimensional grid */
-            void computeGridNeighbors (int rid, std::vector <int> &neighbors) const;
+            void computeGridNeighbors (unsigned int rid, std::vector <unsigned int> &neighbors) const;
 
             /** Recursive subroutine for grid neighbor computation */
-            void computeGridNeighborsSub (const std::vector <int>&coord, std::vector <int> &neighbors,
-                                          unsigned int dim, std::vector <int> &candidate) const;
+            void computeGridNeighborsSub (const std::vector <unsigned int>&coord, std::vector <unsigned int> &neighbors,
+                                          unsigned int dim, std::vector <unsigned int> &candidate) const;
 
-            const int length_;
+            unsigned int length_;
             double cellVolume_;
             mutable boost::unordered_map<int, boost::shared_ptr<base::RealVectorBounds> > regToBounds_;
 
         private:
             /** \brief Helper method to return len^dim in call to super-constructor. */
-            int calcNumRegions(const int len, const std::size_t dim) const;
+            unsigned int calcNumRegions(unsigned int len, unsigned int dim) const;
         };
     }
 }

File src/ompl/control/planners/syclop/src/GridDecomposition.cpp

 
 #include "ompl/control/planners/syclop/GridDecomposition.h"
 
-ompl::control::GridDecomposition::GridDecomposition(const int len, const std::size_t dim, const base::RealVectorBounds& b) :
+ompl::control::GridDecomposition::GridDecomposition(unsigned int len, unsigned int dim, const base::RealVectorBounds& b) :
     Decomposition(dim, b, calcNumRegions(len,dim)), length_(len), cellVolume_(b.getVolume())
 {
-    const double lenInv = 1.0 / len;
-    for (std::size_t i = 0; i < dim; ++i)
+    double lenInv = 1.0 / len;
+    for (unsigned int i = 0; i < dim; ++i)
         cellVolume_ *= lenInv;
 }
 
-void ompl::control::GridDecomposition::getNeighbors(const int rid, std::vector<int>& neighbors) const
+void ompl::control::GridDecomposition::getNeighbors(unsigned int rid, std::vector<unsigned int>& neighbors) const
 {
     //We efficiently compute neighbors for dim = 1, 2, or 3; for higher dimensions we use a general approach.
     if (dimension_ == 1)
              0, +1,
             +1, +1
         };
-        std::vector<int> coord(2);
+        std::vector<unsigned int> coord(2);
         regionToGridCoord(rid, coord);
         std::vector<int> nc(2);
         for (std::size_t i = 0; i < 16; i += 2)
         {
             nc[0] = coord[0] + offset[i];
             nc[1] = coord[1] + offset[i+1];
-            if (nc[0] >= 0 && nc[0] < length_ && nc[1] >= 0 && nc[1] < length_)
+            if (nc[0] >= 0 && (unsigned int) nc[0] < length_ && nc[1] >= 0
+               && (unsigned int) nc[1] < length_)
                 neighbors.push_back(nc[0]*length_ + nc[1]);
         }
     }
             0, 0, -1,
             0, 0, +1
         };
-        std::vector<int> coord(3);
+        std::vector<unsigned int> coord(3);
         regionToGridCoord(rid, coord);
         std::vector<int> nc(3);
-        for (std::size_t i = 0; i < 78; i += 3)
+        for (unsigned int i = 0; i < 78; i += 3)
         {
             nc[0] = coord[0] + offset[i];
             nc[1] = coord[1] + offset[i+1];
             nc[2] = coord[2] + offset[i+2];
-            if (nc[0] >= 0 && nc[0] < length_ && nc[1] >= 0 && nc[1] < length_ && nc[2] >= 0 && nc[2] < length_)
+            if (nc[0] >= 0 && (unsigned int) nc[0] < length_
+              && nc[1] >= 0 && (unsigned int) nc[1] < length_
+              && nc[2] >= 0 && (unsigned int) nc[2] < length_)
                 neighbors.push_back(nc[0]*length_*length_ + nc[1]*length_ + nc[2]);
         }
     }
     return coordToRegion(coord);
 }
 
-void ompl::control::GridDecomposition::sampleFromRegion(const int rid, RNG& rng, std::vector<double>& coord) const
+void ompl::control::GridDecomposition::sampleFromRegion(unsigned int rid, RNG& rng, std::vector<double>& coord) const
 {
     coord.resize(dimension_);
     const base::RealVectorBounds& regionBounds(getRegionBounds(rid));
         coord[i] = rng.uniformReal(regionBounds.low[i], regionBounds.high[i]);
 }
 
-void ompl::control::GridDecomposition::computeGridNeighbors (int rid, std::vector <int> &neighbors) const
+void ompl::control::GridDecomposition::computeGridNeighbors (unsigned int rid, std::vector <unsigned int> &neighbors) const
 {
-    std::vector <int> candidate (dimension_, -1);
-    std::vector <int> coord;
+    std::vector <unsigned int> candidate (dimension_, -1);
+    std::vector <unsigned int> coord;
     regionToGridCoord (rid, coord);
 
     computeGridNeighborsSub (coord, neighbors, 0, candidate);
 }
 
-void ompl::control::GridDecomposition::computeGridNeighborsSub (const std::vector <int> &coord,
-                                                                std::vector <int> &neighbors,
+void ompl::control::GridDecomposition::computeGridNeighborsSub (const std::vector <unsigned int> &coord,
+                                                                std::vector <unsigned int> &neighbors,
                                                                 unsigned int dim,
-                                                                std::vector <int> &candidate) const
+                                                                std::vector <unsigned int> &candidate) const
 {
     // Stopping condition for recursive method.
     if (dim == dimension_)
     {
         // Make sure we don't push back ourselves as a neighbor
         bool same = true;
-        for (size_t i = 0; i < coord.size () && same; ++i)
+        for (unsigned int i = 0; i < coord.size () && same; ++i)
             same = (coord[i] == candidate[i]);
 
         if (!same)
     else
     {
         // Check neighbor in the cell preceding this one in this dimension
-        if (coord[dim] -1 >= 0)
+        if (coord[dim] >= 1)
         {
             candidate[dim] = coord[dim]-1;
             computeGridNeighborsSub (coord, neighbors, dim+1, candidate);
     }
 }
 
-void ompl::control::GridDecomposition::regionToGridCoord(int rid, std::vector<int>& coord) const
+void ompl::control::GridDecomposition::regionToGridCoord(unsigned int rid, std::vector<unsigned int>& coord) const
 {
     coord.resize(dimension_);
     for (int i = dimension_-1; i >= 0; --i)
     {
-        int remainder = rid % length_;
+        unsigned int remainder = rid % length_;
         coord[i] = remainder;
         rid /= length_;
     }
 }
 
-int ompl::control::GridDecomposition::gridCoordToRegion (const std::vector <int> &coord) const
+unsigned int ompl::control::GridDecomposition::gridCoordToRegion (const std::vector <unsigned int> &coord) const
 {
-    int region = 0;
-    for (size_t i = 0; i < coord.size (); i++)
+    unsigned int region = 0;
+    for (unsigned int i = 0; i < coord.size (); i++)
     {
         // Computing length_^(dimension of coord -1)
-        int multiplicand = 1;
-        for (size_t j = 1; j < coord.size () - i; j++)
+        unsigned int multiplicand = 1;
+        for (unsigned int j = 1; j < coord.size () - i; j++)
             multiplicand *= length_;
 
         region += (coord[i] * multiplicand);
     return region;
 }
 
-int ompl::control::GridDecomposition::coordToRegion(const std::vector<double>& coord) const
+unsigned int ompl::control::GridDecomposition::coordToRegion(const std::vector<double>& coord) const
 {
-    int region = 0;
-    int factor = 1;
-    int index;
+    unsigned int region = 0;
+    unsigned int factor = 1;
+    unsigned int index;
     for (int i = dimension_-1; i >= 0; --i)
     {
-        index = (int) (length_*(coord[i]-bounds_.low[i])/(bounds_.high[i]-bounds_.low[i]));
+        index = (unsigned int) (length_*(coord[i]-bounds_.low[i])/(bounds_.high[i]-bounds_.low[i]));
 
         // There is an edge case when the coordinate lies exactly on the upper bound where
         // the region index will be out of bounds.  Ensure index lies within [0, length_)
     return region;
 }
 
-void ompl::control::GridDecomposition::coordToGridCoord(const std::vector<double>& coord, std::vector<int>& gridCoord) const
+void ompl::control::GridDecomposition::coordToGridCoord(const std::vector<double>& coord, std::vector<unsigned int>& gridCoord) const
 {
     gridCoord.resize(dimension_);
-    for (int i = 0; i < dimension_; ++i)
+    for (unsigned int i = 0; i < dimension_; ++i)
     {
-        gridCoord[i] = (int) (length_*(coord[i]-bounds_.low[i])/(bounds_.high[i]-bounds_.low[i]));
+        gridCoord[i] = (unsigned int) (length_*(coord[i]-bounds_.low[i])/(bounds_.high[i]-bounds_.low[i]));
 
         // There is an edge case when the coordinate lies exactly on the upper bound where
         // the region index will be out of bounds.  Ensure index lies within [0, length_)
     }
 }
 
-const ompl::base::RealVectorBounds& ompl::control::GridDecomposition::getRegionBounds(const int rid) const
+const ompl::base::RealVectorBounds& ompl::control::GridDecomposition::getRegionBounds(unsigned int rid) const
 {
     if (regToBounds_.count(rid) > 0)
         return *regToBounds_[rid].get();
     ompl::base::RealVectorBounds* regionBounds = new ompl::base::RealVectorBounds(dimension_);
-    std::vector<int> rc(dimension_);
+    std::vector<unsigned int> rc(dimension_);
     regionToGridCoord(rid, rc);
-    for (std::size_t i = 0; i < dimension_; ++i)
+    for (unsigned int i = 0; i < dimension_; ++i)
     {
         const double length = (bounds_.high[i] - bounds_.low[i]) / length_;
         regionBounds->low[i] = bounds_.low[i] + length*rc[i];
     return *regToBounds_[rid].get();
 }
 
-int ompl::control::GridDecomposition::calcNumRegions(const int len, const std::size_t dim) const
+unsigned int ompl::control::GridDecomposition::calcNumRegions(unsigned int len, unsigned int dim) const
 {
-    int numRegions = 1;
-    for (std::size_t i = 0; i < dim; ++i)
+    unsigned int numRegions = 1;
+    for (unsigned int i = 0; i < dim; ++i)
         numRegions *= len;
     return numRegions;
 }

File src/ompl/control/planners/syclop/src/Syclop.cpp

     }
     si_->freeState(s);
 
-    for (int i = 0; i < decomp_->getNumRegions(); ++i)
+    for (unsigned int i = 0; i < decomp_->getNumRegions(); ++i)
     {
         Region& r = graph_[boost::vertex(i, graph_)];
         r.volume = decomp_->getRegionVolume(i);
 void ompl::control::Syclop::buildGraph(void)
 {
     VertexIndexMap index = get(boost::vertex_index, graph_);
-    std::vector<int> neighbors;
-    for (int i = 0; i < decomp_->getNumRegions(); ++i)
+    std::vector<unsigned int> neighbors;
+    for (unsigned int i = 0; i < decomp_->getNumRegions(); ++i)
     {
         const RegionGraph::vertex_descriptor v = boost::add_vertex(graph_);
         Region& r = graph_[boost::vertex(v,graph_)];
         /* Create an edge between this vertex and each of its neighboring regions in the decomposition,
             and initialize the edge's Adjacency object. */
         decomp_->getNeighbors(index[*vi], neighbors);
-        for (std::vector<int>::const_iterator j = neighbors.begin(); j != neighbors.end(); ++j)
+        for (std::vector<unsigned int>::const_iterator j = neighbors.begin(); j != neighbors.end(); ++j)
         {
             RegionGraph::edge_descriptor edge;
             bool ignore;

File src/ompl/control/planners/syclop/src/SyclopRRT.cpp

     {
         /* Instead of querying the nearest neighbors datastructure over the entire tree of motions,
          * here we perform a linear search over all motions in the selected region and its neighbors. */
-        std::vector<int> searchRegions;
+        std::vector<unsigned int> searchRegions;
         decomp_->getNeighbors(region.index, searchRegions);
         searchRegions.push_back(region.index);
 
         std::vector<Motion*> motions;
-        for (std::vector<int>::const_iterator i = searchRegions.begin(); i != searchRegions.end(); ++i)
+        for (std::vector<unsigned int>::const_iterator i = searchRegions.begin(); i != searchRegions.end(); ++i)
         {
             const std::vector<Motion*>& regionMotions = getRegionFromIndex(*i).motions;
             motions.insert(motions.end(), regionMotions.begin(), regionMotions.end());

File src/ompl/extensions/triangle/TriangularDecomposition.h

             {
             }
 
-            virtual double getRegionVolume(const int triID);
+            virtual double getRegionVolume(unsigned int triID);
 
-            virtual void getNeighbors(const int triID, std::vector<int>& neighbors) const;
+            virtual void getNeighbors(unsigned int triID, std::vector<unsigned int>& neighbors) const;
 
             virtual int locateRegion(const base::State* s) const;
 
-            virtual void sampleFromRegion(const int triID, RNG& rng, std::vector<double>& coord) const;
+            virtual void sampleFromRegion(unsigned int triID, RNG& rng, std::vector<double>& coord) const;
 
             //Debug method: prints this decomposition as a list of polygons
             void print(std::ostream& out) const;
             {
                 Triangle(void) : Polygon(3) {}
                 virtual ~Triangle() {}
-                std::vector<int> neighbors;
+                std::vector<unsigned int> neighbors;
                 double volume;
             };
 
             /** \brief Constructor. Creates a TriangularDecomposition over the given bounds, which must be 2-dimensional.
                 The triangulation will respect any given obstacles, which are assumed to be convex polygons.
              */
-            TriangularDecomposition(const std::size_t dim, const base::RealVectorBounds& b,
+            TriangularDecomposition(unsigned int dim, const base::RealVectorBounds& b,
                 const std::vector<Polygon>& holes = std::vector<Polygon>());
 
             /** \brief Helper method to triangulate the space and return the number of triangles. */
             class LocatorGrid : public GridDecomposition
             {
             public:
-                LocatorGrid(const int len, const Decomposition* d) :
+                LocatorGrid(unsigned int len, const Decomposition* d) :
                     GridDecomposition(len, d->getDimension(), d->getBounds()),
                     triDecomp(d)
                 {
                 {
                 }
 
-                const std::vector<int>& locateTriangles(const base::State* s) const
+                const std::vector<unsigned int>& locateTriangles(const base::State* s) const
                 {
                     return regToTriangles_[locateRegion(s)];
                 }
                 const Decomposition* triDecomp;
                 /* map from locator grid cell ID to set of triangles with which
                  * that cell intersects */
-                std::vector<std::vector<int> > regToTriangles_;
+                std::vector<std::vector<unsigned int> > regToTriangles_;
             };
 
             /** \brief Helper method to build a locator grid to help locate states in triangles. */

File src/ompl/extensions/triangle/src/TriangularDecomposition.cpp

 #include "ompl/extensions/triangle/TriangularDecomposition.h"
 #include <boost/lexical_cast.hpp>
 
-ompl::control::TriangularDecomposition::TriangularDecomposition(const std::size_t dim, const base::RealVectorBounds& b, const std::vector<Polygon>& holes) :
+ompl::control::TriangularDecomposition::TriangularDecomposition(unsigned int dim, const base::RealVectorBounds& b, const std::vector<Polygon>& holes) :
     Decomposition(dim, b),
     holes_(holes),
     locator(64, this)
     buildLocatorGrid();
 }
 
-double ompl::control::TriangularDecomposition::getRegionVolume(const int triID)
+double ompl::control::TriangularDecomposition::getRegionVolume(unsigned int triID)
 {
     Triangle& tri = triangles_[triID];
     if (tri.volume < 0)
     return tri.volume;
 }
 
-void ompl::control::TriangularDecomposition::sampleFromRegion(const int triID, RNG& rng, std::vector<double>& coord) const
+void ompl::control::TriangularDecomposition::sampleFromRegion(unsigned int triID, RNG& rng, std::vector<double>& coord) const
 {
     /* Uniformly sample a point from within a triangle, using the approach discussed in
      * http://math.stackexchange.com/questions/18686/uniform-random-point-in-triangle */
     coord[1] = (1-r1)*tri.pts[0].y + r1*(1-r2)*tri.pts[1].y + r1*r2*tri.pts[2].y;
 }
 
-void ompl::control::TriangularDecomposition::getNeighbors(const int triID, std::vector<int>& neighbors) const
+void ompl::control::TriangularDecomposition::getNeighbors(unsigned int triID, std::vector<unsigned int>& neighbors) const
 {
     neighbors = triangles_[triID].neighbors;
 }
 {
     std::vector<double> coord(2);
     project(s, coord);
-    const std::vector<int>& gridTriangles = locator.locateTriangles(s);
+    const std::vector<unsigned int>& gridTriangles = locator.locateTriangles(s);
     int triangle = -1;
-    for (std::vector<int>::const_iterator i = gridTriangles.begin(); i != gridTriangles.end(); ++i)
+    for (std::vector<unsigned int>::const_iterator i = gridTriangles.begin(); i != gridTriangles.end(); ++i)
     {
-        const int triID = *i;
+        unsigned int triID = *i;
         if (triContains(triangles_[triID], coord))
         {
             if (triangle >= 0)
     regToTriangles_.resize(getNumRegions());
     std::vector<double> bboxLow(2);
     std::vector<double> bboxHigh(2);
-    std::vector<int> gridCoord[2];
-    for (int i = 0; i < triangles.size(); ++i)
+    std::vector<unsigned int> gridCoord[2];
+    for (unsigned int i = 0; i < triangles.size(); ++i)
     {
         /* for Triangle tri, compute the smallest rectangular
          * bounding box that contains tri. */
         bboxHigh[0] = bboxLow[0];
         bboxHigh[1] = bboxLow[1];
 
-        for (int j = 1; j < 3; ++j)
+        for (unsigned int j = 1; j < 3; ++j)
         {
             if (tri.pts[j].x < bboxLow[0])
                 bboxLow[0] = tri.pts[j].x;
 
         /* Every grid cell within bounding box gets
            tri added to its map entry */
-        std::vector<int> c(2);
-        for (int x = gridCoord[0][0]; x <= gridCoord[1][0]; ++x)
+        std::vector<unsigned int> c(2);
+        for (unsigned int x = gridCoord[0][0]; x <= gridCoord[1][0]; ++x)
         {
-            for (int y = gridCoord[0][1]; y <= gridCoord[1][1]; ++y)
+            for (unsigned int y = gridCoord[0][1]; y <= gridCoord[1][1]; ++y)
             {
                 c[0] = x;
                 c[1] = y;
-                int cellID = gridCoordToRegion(c);
+                unsigned int cellID = gridCoordToRegion(c);
                 regToTriangles_[cellID].push_back(i);
             }
         }