LeafHistogram uses class Foliation.

Conflicts:
	include/hpp/manipulation/graph/edge.hh
	include/hpp/manipulation/graph/statistics.hh
	src/graph/edge.cc
	src/graph/statistics.cc

include/hpp/manipulation/graph/edge.hh

@@ -268,7 +268,7 @@ namespace hpp {
 
           virtual bool applyConstraints (core::NodePtr_t n_offset, ConfigurationOut_t q) const;
 
-          void buildHistogram ();
+          void histogram (LeafHistogramPtr_t hist);
 
           LeafHistogramPtr_t histogram () const;
 
@@ -304,14 +304,8 @@ namespace hpp {
 
           /// See pathConstraint member function.
           Constraint_t* extraConstraints_;
-          virtual ConstraintSetPtr_t extraConfigConstraint () const;
-
-          /// Extra NumericalConstraints_t
-          NumericalConstraints_t extraNumericalConstraints_;
-          IntervalsContainer_t extraPassiveDofs_;
-
-          /// Extra LockedJoints_t
-          LockedJoints_t extraLockedJoints_;
+          ConstraintSetPtr_t extraConfigConstraint () const;
+          void buildExtraConfigConstraint () const;
 
           /// This histogram will be used to find a good level set.
           LeafHistogramPtr_t hist_;

include/hpp/manipulation/graph/statistics.hh

@@ -142,18 +142,14 @@ namespace hpp {
       {
         public:
           typedef ::hpp::statistics::Statistics < LeafBin > Parent;
-          /// Constructor
-          /// \param constraint The constraint that create the foliation being
-          ///        studied.
-          LeafHistogram (const ConstraintSetPtr_t& constraint);
+
+          static LeafHistogramPtr_t create (const Foliation f);
 
           /// Insert an occurence of a value in the histogram
           void add (const RoadmapNodePtr_t& n);
 
           std::ostream& print (std::ostream& os) const;
 
-          const ConstraintSetPtr_t& constraint () const;
-
           virtual HistogramPtr_t clone () const;
 
           statistics::DiscreteDistribution < RoadmapNodePtr_t > getDistribOutOfConnectedComponent (
@@ -163,9 +159,19 @@ namespace hpp {
 
           void clear () { Parent::clear(); }
 
+          const Foliation& foliation () const {
+            return f_;
+          }
+
+        protected:
+          /// Constructor
+          /// \param node defines the submanifold containing the foliation.
+          /// \param constraint The constraint that create the foliation being
+          ///        studied.
+          LeafHistogram (const Foliation f);
+
         private:
-          /// The constraint that creates the foliation.
-          ConstraintSetPtr_t constraint_;
+          Foliation f_;
 
           /// Threshold used for equality between offset values.
           value_type threshold_;

include/hpp/manipulation/roadmap.hh

@@ -39,9 +39,6 @@ namespace hpp {
         /// Return a shared pointer to a new instance
         static RoadmapPtr_t create (const core::DistancePtr_t& distance, const core::DevicePtr_t& robot);
 
-        /// Add a ConstraintSet that creates a foliation.
-        void statAddFoliation (ConstraintSetPtr_t constraint);
-
         /// Register histogram so that each time a node is added to the roadmap,
         /// it is also added to the histogram
         void insertHistogram (const graph::HistogramPtr_t hist);

src/graph/edge.cc

@@ -459,12 +459,15 @@ namespace hpp {
         GraphComponent::populateTooltip (tp);
         tp.addLine ("");
         tp.addLine ("Extra numerical constraints are:");
-        for (NumericalConstraints_t::const_iterator it = extraNumericalConstraints_.begin ();
-            it != extraNumericalConstraints_.end (); ++it) {
+        ConfigProjectorPtr_t param = hist_->foliation().parametrizer();
+        const NumericalConstraints_t& nc = param->numericalConstraints();
+        const LockedJoints_t& lj = param->lockedJoints ();
+        for (NumericalConstraints_t::const_iterator it = nc.begin ();
+            it != nc.end (); ++it) {
           tp.addLine ("- " + (*it)->function ().name ());
         }
-        for (LockedJoints_t::const_iterator it = extraLockedJoints_.begin ();
-            it != extraLockedJoints_.end (); ++it) {
+        for (LockedJoints_t::const_iterator it = lj.begin ();
+            it != lj.end (); ++it) {
           tp.addLine ("- " + (*it)->jointName ());
         }
       }
@@ -473,6 +476,10 @@ namespace hpp {
       {
         // First, get an offset from the histogram
         statistics::DiscreteDistribution < RoadmapNodePtr_t > distrib = hist_->getDistrib ();
+        if (distrib.size () == 0) {
+          hppDout (warning, "Edge " << name() << ": Distrib is empty");
+          return false;
+        }
         const Configuration_t& qlevelset = *(distrib ()->configuration ());
 
         return applyConstraintsWithOffset (qoffset, qlevelset, q);
@@ -482,6 +489,10 @@ namespace hpp {
       {
         // First, get an offset from the histogram that is not in the same connected component.
         statistics::DiscreteDistribution < RoadmapNodePtr_t > distrib = hist_->getDistribOutOfConnectedComponent (n_offset->connectedComponent ());
+        if (distrib.size () == 0) {
+          hppDout (warning, "Edge " << name() << ": Distrib is empty");
+          return false;
+        }
         const Configuration_t& qlevelset = *(distrib ()->configuration ()),
                                qoffset = *(n_offset->configuration ());
 
@@ -491,6 +502,7 @@ namespace hpp {
       bool LevelSetEdge::applyConstraintsWithOffset (ConfigurationIn_t qoffset,
           ConfigurationIn_t qlevelset, ConfigurationOut_t q) const
       {
+        /*
         // First, set the offset.
         ConstraintSetPtr_t cs = extraConfigConstraint ();
         const ConfigProjectorPtr_t cp = cs->configProjector ();
@@ -505,6 +517,25 @@ namespace hpp {
 	     it != extraLockedJoints_.end (); ++it) {
           (*it)->rightHandSideFromConfig (qlevelset);
         }
+        // */
+  //*
+        ConfigProjectorPtr_t param = hist_->foliation().parametrizer();
+        const NumericalConstraints_t& nc = param->numericalConstraints();
+        const LockedJoints_t& lj = param->lockedJoints ();
+
+        // Then, set the offset.
+        ConstraintSetPtr_t cs = extraConfigConstraint ();
+        const ConfigProjectorPtr_t cp = cs->configProjector ();
+        assert (cp);
+	cp->rightHandSideFromConfig (q_offset);
+	for (NumericalConstraints_t::const_iterator it = nc.begin ();
+	     it != nc.end (); ++it) {
+          (*it)->rightHandSideFromConfig (levelsetTarget);
+        }
+        for (LockedJoints_t::const_iterator it = lj.begin ();
+	     it != lj.end (); ++it) {
+          (*it)->rightHandSideFromConfig (levelsetTarget);
+        } //*/
 	cp->updateRightHandSide ();
 
         // Eventually, do the projection.
@@ -539,39 +570,55 @@ namespace hpp {
         return shPtr;
       }
 
-      void LevelSetEdge::buildHistogram ()
+      void LevelSetEdge::histogram (LeafHistogramPtr_t hist)
       {
-        std::string n = "(" + name () + ")";
+        hist_ = hist;
+      }
+
+      LeafHistogramPtr_t LevelSetEdge::histogram () const
+      {
+        return hist_;
+      }
+
+      void LevelSetEdge::buildExtraConfigConstraint () const
+      {
+        /// First get the numerical constraints
+        ConfigProjectorPtr_t param = hist_->foliation().parametrizer();
+        const NumericalConstraints_t& nc = param->numericalConstraints();
+        const LockedJoints_t& lj = param->lockedJoints ();
+
+        /// Build the constraint set.
+        std::string n = "(" + name () + "_extra)";
         GraphPtr_t g = graph_.lock ();
 
         ConstraintSetPtr_t constraint = ConstraintSet::create (g->robot (), "Set " + n);
 
         ConfigProjectorPtr_t proj = ConfigProjector::create(g->robot(), "proj_" + n, g->errorThreshold(), g->maxIterations());
-        IntervalsContainer_t::const_iterator itpdof = extraPassiveDofs_.begin ();
-        for (NumericalConstraints_t::const_iterator it = extraNumericalConstraints_.begin ();
-            it != extraNumericalConstraints_.end (); ++it) {
-          proj->add (*it, *itpdof);
-          ++itpdof;
-        }
-
-        for (LockedJoints_t::const_iterator it = extraLockedJoints_.begin ();
-            it != extraLockedJoints_.end (); ++it)
+        g->insertNumericalConstraints (proj);
+        for (NumericalConstraints_t::const_iterator it = nc.begin ();
+            it != nc.end (); ++it) {
           proj->add (*it);
+        }
 
+        insertNumericalConstraints (proj);
+        to ()->insertNumericalConstraints (proj);
         constraint->addConstraint (proj);
         constraint->edge (wkPtr_.lock ());
 
-        hist_ = graph::LeafHistogramPtr_t (new graph::LeafHistogram (constraint));
-      }
-
-      LeafHistogramPtr_t LevelSetEdge::histogram () const
-      {
-        return hist_;
+        g->insertLockedJoints (proj);
+        for (LockedJoints_t::const_iterator it = lj.begin ();
+            it != lj.end (); ++it) {
+          proj->add (*it);
+        }
+        insertLockedJoints (proj);
+        to ()->insertLockedJoints (proj);
+        extraConstraints_->set (constraint);
       }
 
       ConstraintSetPtr_t LevelSetEdge::extraConfigConstraint () const
       {
         if (!*extraConstraints_) {
+          /*
           std::string n = "(" + name () + "_extra)";
           GraphPtr_t g = graph_.lock ();
 
@@ -600,27 +647,12 @@ namespace hpp {
 
           constraint->edge (wkPtr_.lock ());
           extraConstraints_->set (constraint);
+          */
+          buildExtraConfigConstraint ();
         }
         return extraConstraints_->get ();
       }
 
-      void LevelSetEdge::insertConfigConstraint (const NumericalConstraintPtr_t& nm,
-              const SizeIntervals_t& passiveDofs)
-      {
-        extraNumericalConstraints_.push_back (nm);
-        extraPassiveDofs_.push_back (passiveDofs);
-      }
-
-      void LevelSetEdge::insertConfigConstraint (const DifferentiableFunctionPtr_t function, const ComparisonTypePtr_t ineq)
-      {
-        insertConfigConstraint (NumericalConstraint::create (function, ineq));
-      }
-
-      void LevelSetEdge::insertConfigConstraint (const LockedJointPtr_t lockedJoint)
-      {
-        extraLockedJoints_.push_back (lockedJoint);
-      }
-
       LevelSetEdge::LevelSetEdge
       (const std::string& name) :
 	Edge (name), extraConstraints_ (new Constraint_t())

src/graph/statistics.cc

@@ -155,10 +155,15 @@ namespace hpp {
         return os << "NodeBin (" << node_->name () << ")";
       }
 
-      LeafHistogram::LeafHistogram (const ConstraintSetPtr_t& constraint) :
-        constraint_ (constraint), threshold_ (0)
+      LeafHistogramPtr_t LeafHistogram::create (const Foliation f)
       {
-        ConfigProjectorPtr_t p = constraint_->configProjector ();
+        return LeafHistogramPtr_t (new LeafHistogram (f));
+      }
+
+      LeafHistogram::LeafHistogram (const Foliation f) :
+        f_ (f), threshold_ (0)
+      {
+        ConfigProjectorPtr_t p = f_.parametrizer ();
         if (p) {
           threshold_ = p->errorThreshold () /
 	    sqrt((double)p->rightHandSide ().size ());
@@ -167,15 +172,9 @@ namespace hpp {
 
       void LeafHistogram::add (const RoadmapNodePtr_t& n)
       {
-	iterator it;
-	if (constraint_->configProjector ()) {
-	  it = insert
-	    (LeafBin (constraint_->configProjector ()->rightHandSideFromConfig
-		      (*n->configuration ()),
-                      &threshold_));
-	} else {
-	  it = insert (LeafBin (vector_t (), &threshold_));
-	}
+        if (!f_.contains (*n->configuration())) return;
+	iterator it = insert (LeafBin (f_.parameter (*n->configuration()),
+                              &threshold_));
         it->push_back (n);
         if (numberOfObservations()%10 == 0) {
           hppDout (info, *this);
@@ -184,19 +183,13 @@ namespace hpp {
 
       std::ostream& LeafHistogram::print (std::ostream& os) const
       {
-        os << "Histogram contains ConstraintSet: "
-          << constraint_->name () << std::endl;
+        os << "Leaf Histogram of foliation " << f_.condition()->name() << std::endl;
         return Parent::print (os);
       }
 
-      const ConstraintSetPtr_t& LeafHistogram::constraint () const
-      {
-        return constraint_;
-      }
-
       HistogramPtr_t LeafHistogram::clone () const
       {
-        return HistogramPtr_t (new LeafHistogram (constraint_));
+        return HistogramPtr_t (new LeafHistogram (f_));
       }
 
       NodeHistogram::NodeHistogram (const graph::GraphPtr_t& graph) :

src/roadmap.cc

@@ -58,11 +58,6 @@ namespace hpp {
       }
     }
 
-    void Roadmap::statAddFoliation (ConstraintSetPtr_t constraint)
-    {
-      insertHistogram (graph::HistogramPtr_t (new graph::LeafHistogram (constraint)));
-    }
-
     void Roadmap::insertHistogram (const graph::HistogramPtr_t hist)
     {
       histograms_.push_back (hist);