Commits

Mark Moll committed 570a715

automatically choose nearest neighbor structure appropriate for state space

Comments (1)

  1. Luis Torres

    Thanks for handling the nearest-neighbor issue for non-metric StateSpaces, Mark. There are some other issues involved with non-metric state spaces which I'm fixing in my fork. I'm nearly finished, so you can expect a pull request sometime soon!

Files changed (13)

src/ompl/base/StateSpace.h

             /** \brief Check if this is a hybrid state space (i.e., both discrete and continuous components exist)*/
             virtual bool isHybrid(void) const;
 
+            /** \brief Return true if the distance function associated with the space
+                is a metric */
+            virtual bool isMetricSpace(void) const
+            {
+                return true;
+            }
+
             /** \brief Get the name of the state space */
             const std::string& getName(void) const;
 

src/ompl/base/spaces/DubinsStateSpace.h

     {
 
         /** \brief An SE(2) state space where distance is measured by the
-            length of Dubins curves. Note that this Dubins distance is \b not
-            a proper distance metric, so nearest neighbor methods that rely
-            on distance() being a metric (such ompl::NearestNeighborsGNAT)
-            will not always return the true nearest neighbors or get stuck
-            in an infinite loop. This means that if you use any of the RRT-based
-            planners (which use GNAT by default), you need to do the following:
-\code
-ob::StateSpacePtr stateSpace(new ob::DubinsStateSpace);
-og::SimpleSetup setup(stateSpace);
-og::RRTConnect* planner = new og::RRTConnect(setup.getSpaceInformation());
-planner->setNearestNeighbors<ompl::NearestNeighborsSqrtApprox>();
-setup.setPlanner(ompl::base::PlannerPtr(planner));
-\endcode
+            length of Dubins curves.
+
+            Note that this Dubins distance is \b not a proper distance metric,
+            so nearest neighbor methods that rely on distance() being a metric
+            (such as ompl::NearestNeighborsGNAT) will not always return the
+            true nearest neighbors or get stuck in an infinite loop.
+
             The notation and solutions in the code are taken from:<br>
             A.M. Shkel and V. Lumelsky, “Classification of the Dubins set,”
             Robotics and Autonomous Systems, 34(4):179-202, 2001.
             {
             }
 
+            virtual bool isMetricSpace()
+            {
+                return false;
+            }
+
             virtual double distance(const State *state1, const State *state2) const;
 
             virtual void interpolate(const State *from, const State *to, const double t,

src/ompl/contrib/rrt_star/src/RRTstar.cpp

 
 #include "ompl/contrib/rrt_star/RRTstar.h"
 #include "ompl/base/goals/GoalSampleableRegion.h"
-#include "ompl/datastructures/NearestNeighborsGNAT.h"
 #include "ompl/tools/config/SelfConfig.h"
 #include <algorithm>
 #include <limits>
         ballRadiusConst_ = maxDistance_ * sqrt((double)si_->getStateSpace()->getDimension());
 
     if (!nn_)
-        nn_.reset(new NearestNeighborsGNAT<Motion*>());
+        nn_.reset(NearestNeighbors<Motion*>::getDefault(si_->getStateSpace()));
     nn_->setDistanceFunction(boost::bind(&RRTstar::distanceFunction, this, _1, _2));
 }
 

src/ompl/control/planners/rrt/src/RRT.cpp

 
 #include "ompl/control/planners/rrt/RRT.h"
 #include "ompl/base/goals/GoalSampleableRegion.h"
-#include "ompl/datastructures/NearestNeighborsGNAT.h"
 #include <limits>
 
 ompl::control::RRT::RRT(const SpaceInformationPtr &si) : base::Planner(si, "RRT")
 {
     base::Planner::setup();
     if (!nn_)
-        nn_.reset(new NearestNeighborsGNAT<Motion*>());
+        nn_.reset(NearestNeighbors<Motion*>::getDefault(si_->getStateSpace()));
     nn_->setDistanceFunction(boost::bind(&RRT::distanceFunction, this, _1, _2));
 }
 

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

 
 #include "ompl/control/planners/syclop/SyclopRRT.h"
 #include "ompl/base/goals/GoalSampleableRegion.h"
-#include "ompl/datastructures/NearestNeighborsGNAT.h"
 
 void ompl::control::SyclopRRT::setup(void)
 {
     // the default regionalNN check from the discretization
     if (!nn_ && !regionalNN_)
     {
-        nn_.reset(new NearestNeighborsGNAT<Motion*>());
+        nn_.reset(NearestNeighbors<Motion*>::getDefault(si_->getStateSpace()));
         nn_->setDistanceFunction(boost::bind(&SyclopRRT::distanceFunction, this, _1, _2));
     }
 }

src/ompl/datastructures/NearestNeighbors.h

 #include <vector>
 #include <boost/bind.hpp>
 #include <boost/function.hpp>
+#include <ompl/base/StateSpace.h>
 
 namespace ompl
 {
         /** \brief Get all the elements in the datastructure */
         virtual void list(std::vector<_T> &data) const = 0;
 
+        /** \brief Select a default nearest neighbor datastructure for the given
+             space */
+        static NearestNeighbors<_T>* getDefault(const base::StateSpacePtr space);
+
     protected:
 
         /** \brief The used distance function */
     };
 }
 
+
+#include "ompl/datastructures/NearestNeighborsGNAT.h"
+#include "ompl/datastructures/NearestNeighborsSqrtApprox.h"
+
+namespace ompl
+{
+    template<typename _T>
+    NearestNeighbors<_T>* NearestNeighbors<_T>::getDefault(const base::StateSpacePtr space)
+    {
+        if (space->isMetricSpace())
+            return new NearestNeighborsGNAT<_T>();
+        else
+            return new NearestNeighborsSqrtApprox<_T>();
+    }
+}
+
 #endif

src/ompl/geometric/planners/prm/src/PRM.cpp

 #include "ompl/geometric/planners/prm/PRM.h"
 #include "ompl/geometric/planners/prm/ConnectionStrategy.h"
 #include "ompl/base/goals/GoalSampleableRegion.h"
-#include "ompl/datastructures/NearestNeighborsGNAT.h"
 #include "ompl/datastructures/PDF.h"
 #include <boost/lambda/bind.hpp>
 #include <boost/graph/astar_search.hpp>
 {
     Planner::setup();
     if (!nn_)
-        nn_.reset(new NearestNeighborsGNAT<Vertex>());
+        nn_.reset(NearestNeighbors<Vertex>::getDefault(si_->getStateSpace()));
     nn_->setDistanceFunction(boost::bind(&PRM::distanceFunction, this, _1, _2));
     if (!connectionStrategy_)
     {

src/ompl/geometric/planners/rrt/src/LazyRRT.cpp

 
 #include "ompl/geometric/planners/rrt/LazyRRT.h"
 #include "ompl/base/goals/GoalSampleableRegion.h"
-#include "ompl/datastructures/NearestNeighborsSqrtApprox.h"
 #include "ompl/tools/config/SelfConfig.h"
 #include <cassert>
 
     sc.configurePlannerRange(maxDistance_);
 
     if (!nn_)
-        nn_.reset(new NearestNeighborsSqrtApprox<Motion*>());
+        nn_.reset(NearestNeighbors<Motion*>::getDefault(si_->getStateSpace()));
     nn_->setDistanceFunction(boost::bind(&LazyRRT::distanceFunction, this, _1, _2));
 }
 

src/ompl/geometric/planners/rrt/src/RRT.cpp

 
 #include "ompl/geometric/planners/rrt/RRT.h"
 #include "ompl/base/goals/GoalSampleableRegion.h"
-#include "ompl/datastructures/NearestNeighborsGNAT.h"
 #include "ompl/tools/config/SelfConfig.h"
 #include <limits>
 
     sc.configurePlannerRange(maxDistance_);
 
     if (!nn_)
-        nn_.reset(new NearestNeighborsGNAT<Motion*>());
+        nn_.reset(NearestNeighbors<Motion*>::getDefault(si_->getStateSpace()));
     nn_->setDistanceFunction(boost::bind(&RRT::distanceFunction, this, _1, _2));
 }
 

src/ompl/geometric/planners/rrt/src/RRTConnect.cpp

 /* Author: Ioan Sucan */
 
 #include "ompl/geometric/planners/rrt/RRTConnect.h"
-#include "ompl/datastructures/NearestNeighborsGNAT.h"
 #include "ompl/base/goals/GoalSampleableRegion.h"
 #include "ompl/tools/config/SelfConfig.h"
 
     sc.configurePlannerRange(maxDistance_);
 
     if (!tStart_)
-        tStart_.reset(new NearestNeighborsGNAT<Motion*>());
+        tStart_.reset(NearestNeighbors<Motion*>::getDefault(si_->getStateSpace()));
     if (!tGoal_)
-        tGoal_.reset(new NearestNeighborsGNAT<Motion*>());
+        tGoal_.reset(NearestNeighbors<Motion*>::getDefault(si_->getStateSpace()));
     tStart_->setDistanceFunction(boost::bind(&RRTConnect::distanceFunction, this, _1, _2));
     tGoal_->setDistanceFunction(boost::bind(&RRTConnect::distanceFunction, this, _1, _2));
 }

src/ompl/geometric/planners/rrt/src/TRRT.cpp

 
 #include "ompl/geometric/planners/rrt/TRRT.h"
 #include "ompl/base/goals/GoalSampleableRegion.h"
-#include "ompl/datastructures/NearestNeighborsGNAT.h"
 #include "ompl/tools/config/SelfConfig.h"
 #include "ompl/tools/config/MagicConstants.h"
 #include <limits>
 
     // Create the nearest neighbor function the first time setup is run
     if (!nearestNeighbors_)
-        nearestNeighbors_.reset(new NearestNeighborsGNAT<Motion*>());
+        nearestNeighbors_.reset(NearestNeighbors<Motion*>::getDefault(si_->getStateSpace()));
 
     // Set the distance function
     nearestNeighbors_->setDistanceFunction(boost::bind(&TRRT::distanceFunction, this, _1, _2));

src/ompl/geometric/planners/rrt/src/pRRT.cpp

 /* Author: Ioan Sucan */
 
 #include "ompl/geometric/planners/rrt/pRRT.h"
-#include "ompl/datastructures/NearestNeighborsGNAT.h"
 #include "ompl/base/goals/GoalSampleableRegion.h"
 #include "ompl/tools/config/SelfConfig.h"
 #include <boost/thread/thread.hpp>
     sc.configurePlannerRange(maxDistance_);
 
     if (!nn_)
-        nn_.reset(new NearestNeighborsGNAT<Motion*>());
+        nn_.reset(NearestNeighbors<Motion*>::getDefault(si_->getStateSpace()));
     nn_->setDistanceFunction(boost::bind(&pRRT::distanceFunction, this, _1, _2));
 }
 

tests/datastructures/nearestneighbors.cpp

 #include <boost/unordered_set.hpp>
 
 #include "ompl/config.h"
-#include "ompl/datastructures/NearestNeighborsSqrtApprox.h"
-#include "ompl/datastructures/NearestNeighborsGNAT.h"
+#include "ompl/datastructures/NearestNeighbors.h"
 #if OMPL_HAVE_FLANN
 #include "ompl/datastructures/NearestNeighborsFLANN.h"
 #endif