Commits

Mark Moll  committed 8c16f9a

add scale-invariant SPARSE* {sparse,dense}DeltaFraction_ parameter and make {sparse,dense}Delta_ a private variable that is automatically updated.

  • Participants
  • Parent commits 737c498

Comments (0)

Files changed (4)

File src/ompl/geometric/planners/prm/SPARS.h

                 maxFailures_ = m;
             }
 
-            /** \brief Set the delta value for interface detection.  If two nodes in the dense graph
-                are more than delta distance appart, then the algorithm cannot consider them to have
-                accurately approximated the location of an interface. */
-            void setDenseDelta( double d )
+            /** \brief Set the delta fraction for interface detection.  If two nodes in the dense graph
+                are more than a delta fraction of the max. extent apart, then the algorithm cannot
+                consider them to have accurately approximated the location of an interface. */
+            void setDenseDeltaFraction( double d )
             {
-                denseDelta_ = d;
+                denseDeltaFraction_ = d;
+                if (denseDelta_ > 0) // setup was previously called
+                    denseDelta_ = d * si_->getMaximumExtent();
             }
 
-            /** \brief Set the delta value for connection distance on the sparse spanner.  This
+            /** \brief Set the delta fraction for connection distance on the sparse spanner.  This
                 value represents the visibility range of sparse samples.  A sparse node
-                represents all dense nodes within this distance if it is also the closest sparse
-                node to that dense node.*/
-            void setSparseDelta( double d )
+                represents all dense nodes within a delta fraction of the max. extent if it is
+                also the closest sparse node to that dense node. */
+            void setSparseDeltaFraction( double d )
             {
-                sparseDelta_ = d;
+                sparseDeltaFraction_ = d;
+                if (sparseDelta_ > 0) // setup was previously called
+                    sparseDelta_ = d * si_->getMaximumExtent();
             }
 
             /** \brief Set the roadmap spanner stretch factor.  This value represents a
                 return maxFailures_;
             }
 
-            /** \brief Retrieve the dense graph interface support delta. */
-            double getDenseDelta( ) const
+            /** \brief Retrieve the dense graph interface support delta fraction. */
+            double getDenseDeltaFraction( ) const
             {
-                return denseDelta_;
+                return denseDeltaFraction_;
             }
 
-            /** \brief Retrieve the sparse graph visibility range delta. */
-            double getSparseDelta( ) const
+            /** \brief Retrieve the sparse graph visibility range delta fraction. */
+            double getSparseDeltaFraction( ) const
             {
-                return sparseDelta_;
+                return sparseDeltaFraction_;
             }
 
             /** \brief Retrieve the spanner's set stretch factor. */
             /** \brief The maximum number of failures before terminating the algorithm */
             unsigned int                                                        maxFailures_;
 
-            /** \brief SPARS parameter for dense graph connection distance */
-            double                                                              denseDelta_;
+            /** \brief SPARS parameter for dense graph connection distance as a fraction of max. extent */
+            double                                                              denseDeltaFraction_;
 
-            /** \brief SPARS parameter for Sparse Roadmap connection distance */
-            double                                                              sparseDelta_;
+            /** \brief SPARS parameter for Sparse Roadmap connection distance as a fraction of max. extent */
+            double                                                              sparseDeltaFraction_;
 
             /** \brief A holder for the last state added to D */
             base::State*                                                        lastState_;
                 return si_->distance( sparseStateProperty_[a], sparseStateProperty_[b] );
             }
 
+            /** \brief SPARS parameter for dense graph connection distance */
+            double                                                              denseDelta_;
+
+            /** \brief SPARS parameter for Sparse Roadmap connection distance */
+            double                                                              sparseDelta_;
+
         };
 
     }

File src/ompl/geometric/planners/prm/SPARStwo.h

                 stretchFactor_ = t;
             }
 
-            /** \brief Sets vertex visibility range */
-            void setSparseDelta( double D )
+            /** \brief Sets vertex visibility range as a fraction of max. extent. */
+            void setSparseDeltaFraction( double D )
             {
-                sparseDelta_ = D;
+                sparseDeltaFraction_ = D;
+                if (sparseDelta_ > 0) // setup was previously called
+                    sparseDelta_ = D * si_->getMaximumExtent();
             }
 
-            /** \brief Sets interface support tolerance */
-            void setDenseDelta( double d )
+            /** \brief Sets interface support tolerance as a fraction of max. extent. */
+            void setDenseDeltaFraction( double d )
             {
-                denseDelta_ = d;
+                denseDeltaFraction_ = d;
+                if (denseDelta_ > 0) // setup was previously called
+                    denseDelta_ = d * si_->getMaximumExtent();
             }
 
             /** \brief Sets the maximum failures until termination */
             }
 
             /** \brief Retrieve the dense graph interface support delta. */
-            double getDenseDelta( ) const
+            double getDenseDeltaFraction( ) const
             {
-                return denseDelta_;
+                return denseDeltaFraction_;
             }
 
             /** \brief Retrieve the sparse graph visibility range delta. */
-            double getSparseDelta( ) const
+            double getSparseDeltaFraction( ) const
             {
-                return sparseDelta_;
+                return sparseDeltaFraction_;
             }
 
             /** \brief Retrieve the spanner's set stretch factor. */
             /** \brief Stretch Factor as per graph spanner literature (multiplicative bound on path quality) */
             double                                                              stretchFactor_;
 
-            /** \brief Maximum visibility range for nodes in the graph */
-            double                                                              sparseDelta_;
+            /** \brief Maximum visibility range for nodes in the graph as a fraction of maximum extent. */
+            double                                                              sparseDeltaFraction_;
 
-            /** \brief Maximum range for allowing two samples to support an interface */
-            double                                                              denseDelta_;
+            /** \brief Maximum range for allowing two samples to support an interface as a fraction of maximum extent. */
+            double                                                              denseDeltaFraction_;
 
             /** \brief The number of consecutive failures to add to the graph before termination */
             unsigned int                                                        maxFailures_;
                 return si_->distance(stateProperty_[a], stateProperty_[b]);
             }
 
+            /** \brief Maximum visibility range for nodes in the graph */
+            double                                                              sparseDelta_;
+
+            /** \brief Maximum range for allowing two samples to support an interface */
+            double                                                              denseDelta_;
+
         };
 
     }

File src/ompl/geometric/planners/prm/src/SPARS.cpp

     sparseDJSets_(boost::get(boost::vertex_rank, s_),
                   boost::get(boost::vertex_predecessor, s_)),
     iterations_(0),
-    stretchFactor_(3),
+    stretchFactor_(3.),
     maxFailures_(1000),
-    denseDelta_(0.5),
-    sparseDelta_(20),
-    lastState_(NULL)
+    denseDeltaFraction_(.001),
+    sparseDeltaFraction_(.25),
+    lastState_(NULL),
+    denseDelta_(0.),
+    sparseDelta_(0.)
 {
     specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
     specs_.approximateSolutions = false;
     psimp_->freeStates(false);
 
     Planner::declareParam<double>("stretch_factor", this, &SPARS::setStretchFactor, &SPARS::getStretchFactor, "1.1:0.1:3.0");
-    Planner::declareParam<double>("sparse_delta", this, &SPARS::setSparseDelta, &SPARS::getSparseDelta, "1.0:0.5:30.0");
-    Planner::declareParam<double>("dense_delta", this, &SPARS::setDenseDelta, &SPARS::getDenseDelta, "0.02:0.02:1.0");
+    Planner::declareParam<double>("sparse_delta_fraction", this, &SPARS::setSparseDeltaFraction, &SPARS::getSparseDeltaFraction, "0.0:0.01:1.0");
+    Planner::declareParam<double>("dense_delta_fraction", this, &SPARS::setDenseDeltaFraction, &SPARS::getDenseDeltaFraction, "0.0:0.0001:0.1");
     Planner::declareParam<unsigned int>("max_failures", this, &SPARS::setMaxFailures, &SPARS::getMaxFailures, "100:10:3000");
 }
 
     snn_->setDistanceFunction(boost::bind(&SPARS::sparseDistanceFunction, this, _1, _2));
     if (!connectionStrategy_)
         connectionStrategy_ = KStarStrategy<DenseVertex>(boost::bind(&SPARS::milestoneCount, this), nn_, si_->getStateDimension());
+    sparseDelta_ = sparseDeltaFraction_ * si_->getMaximumExtent();
+    denseDelta_ = denseDeltaFraction_ * si_->getMaximumExtent();
 }
 
 void ompl::geometric::SPARS::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)

File src/ompl/geometric/planners/prm/src/SPARStwo.cpp

 
 ompl::geometric::SPARStwo::SPARStwo(const base::SpaceInformationPtr &si) :
     base::Planner(si, "SPARStwo"),
-    stretchFactor_(3),
-    sparseDelta_(15),
-    denseDelta_(0.5),
+    stretchFactor_(3.),
+    sparseDeltaFraction_(.25),
+    denseDeltaFraction_(.001),
     maxFailures_(5000),
     nearSamplePoints_((2*si_->getStateDimension())),
     stateProperty_(boost::get(vertex_state_t(), g_)),
     disjointSets_(boost::get(boost::vertex_rank, g_),
                   boost::get(boost::vertex_predecessor, g_)),
     addedSolution_(false),
-    iterations_(0)
+    iterations_(0),
+    sparseDelta_(0.),
+    denseDelta_(0.)
 {
     specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
     specs_.approximateSolutions = false;
     simpleSampler_ = si_->allocStateSampler();
 
     Planner::declareParam<double>("stretch_factor", this, &SPARStwo::setStretchFactor, &SPARStwo::getStretchFactor, "1.1:0.1:3.0");
-    Planner::declareParam<double>("sparse_delta", this, &SPARStwo::setSparseDelta, &SPARStwo::getSparseDelta, "1.0:0.5:30.0");
-    Planner::declareParam<double>("dense_delta", this, &SPARStwo::setDenseDelta, &SPARStwo::getDenseDelta, "0.02:0.02:1.0");
+    Planner::declareParam<double>("sparse_delta_fraction", this, &SPARStwo::setSparseDeltaFraction, &SPARStwo::getSparseDeltaFraction, "0.0:0.01:1.0");
+    Planner::declareParam<double>("dense_delta_fraction", this, &SPARStwo::setDenseDeltaFraction, &SPARStwo::getDenseDeltaFraction, "0.0:0.0001:0.1");
     Planner::declareParam<unsigned int>("max_failures", this, &SPARStwo::setMaxFailures, &SPARStwo::getMaxFailures, "100:10:3000");
 }
 
     if (!nn_)
         nn_.reset(new NearestNeighborsGNAT<Vertex>());
     nn_->setDistanceFunction(boost::bind(&SPARStwo::distanceFunction, this, _1, _2));
+    sparseDelta_ = sparseDeltaFraction_ * si_->getMaximumExtent();
+    denseDelta_ = denseDeltaFraction_ * si_->getMaximumExtent();
 }
 
 void ompl::geometric::SPARStwo::setProblemDefinition(const base::ProblemDefinitionPtr &pdef)