Implement oriented roadmap

  - new implementation of connected components.

CMakeLists.txt

@@ -75,6 +75,7 @@ SET(${PROJECT_NAME}_HEADERS
   include/hpp/core/straight-path.hh
   include/hpp/core/weighed-distance.hh
   include/hpp/core/k-d-tree.hh
+  include/hpp/core/graph-connected-component.hh
   )
 
 ADD_REQUIRED_DEPENDENCY("hpp-util >= 3")

include/hpp/core/connected-component.hh

@@ -22,6 +22,7 @@
 # include <hpp/core/fwd.hh>
 # include <hpp/core/config.hh>
 # include <hpp/core/node.hh>
+# include <hpp/core/graph-connected-component.hh>
 
 namespace hpp {
   namespace core {
@@ -30,7 +31,14 @@ namespace hpp {
     /// Set of nodes reachable from one another.
     class HPP_CORE_DLLAPI ConnectedComponent {
     public:
+      // List of nodes within the connected component
       typedef std::list <NodePtr_t> Nodes_t;
+      // List of CCs that can be reached from this connected component
+      ConnectedComponents_t reachableTo_;
+      // List of CCs from which this connected component can be reached
+      ConnectedComponents_t reachableFrom_;
+      // variable for ranking connected components
+      static unsigned int globalFinishTime_;
       static ConnectedComponentPtr_t create ()
       {
 	ConnectedComponent* ptr = new ConnectedComponent ();
@@ -38,6 +46,38 @@ namespace hpp {
 	ptr->init (shPtr);
 	return shPtr;
       }
+      void setExplored ()
+      {
+	explored_ = true;
+      }
+      void resetExplored ()
+      {
+	explored_ = false;
+      }
+      void setLeader (const ConnectedComponentPtr_t& cc)
+      {
+	leaderCC_ = cc;
+      }
+      ConnectedComponentPtr_t getLeader ()
+      {
+	return leaderCC_;
+      }
+      void setFinishTime ()
+      {
+	finishTimeCC_ = globalFinishTime_;
+      }
+      void resetFinishTime ()
+      {
+	finishTimeCC_ = 0;
+      }
+      int getFinishTime ()
+      {
+	return finishTimeCC_;
+      }
+      bool isExplored ()
+      {
+	return explored_;
+      }
       /// Merge two connected components.
       ///
       /// \param other connected component to merge into this one.
@@ -50,6 +90,12 @@ namespace hpp {
 	}
 
 	nodes_.splice (nodes_.end (), other->nodes_);
+	reachableTo_.splice (reachableTo_.end (),
+			     other->reachableTo_);
+	reachableFrom_.splice (reachableFrom_.end(),
+			       other->reachableFrom_);
+	reachableTo_.sort (); reachableTo_.unique ();
+	reachableFrom_.sort (); reachableFrom_.unique ();
       }
       /// Add node in connected component
       /// \param node node to add.
@@ -62,17 +108,37 @@ namespace hpp {
       {
 	return nodes_;
       }
+      struct compareCCFinishTime {
+	bool operator () (const ConnectedComponentPtr_t& cc1,
+			  const ConnectedComponentPtr_t& cc2) const
+	{ return cc1->getFinishTime () > cc2->getFinishTime (); }
+      };
+      struct emptyCC {
+	bool operator () (const ConnectedComponentPtr_t& cc1) const
+	{ return cc1->nodes ().empty (); }
+      };
+
     protected:
       /// Constructor
-      ConnectedComponent () : nodes_ (), weak_ ()
-	{
-	}
+      ConnectedComponent () : nodes_ (), weak_ (),
+	finishTimeCC_ (0), explored_ (false),
+	leaderCC_ ()
+	  {
+	  }
       void init (const ConnectedComponentPtr_t& shPtr){
 	weak_ = shPtr;
       }
     private:
       Nodes_t nodes_;
       ConnectedComponentWkPtr_t weak_;
+      // rank of the connected component in the CCGraph
+      unsigned int finishTimeCC_;
+      // status variable to indicate whether or not CC has been
+      // visited
+      bool explored_;
+      // information about the leader of a given CC
+      ConnectedComponentPtr_t leaderCC_;
+
     }; // class ConnectedComponent
   } //   namespace core
 } // namespace hpp

include/hpp/core/fwd.hh

@@ -40,6 +40,7 @@ namespace hpp {
     HPP_PREDEF_CLASS (ConfigValidation);
     HPP_PREDEF_CLASS (ConfigValidations);
     HPP_PREDEF_CLASS (ConnectedComponent);
+    HPP_PREDEF_CLASS (CCGraph);
     HPP_PREDEF_CLASS (Constraint);
     HPP_PREDEF_CLASS (ConstraintSet);
     HPP_PREDEF_CLASS (DifferentiableFunction);
@@ -90,6 +91,8 @@ namespace hpp {
     typedef boost::shared_ptr <ConfigValidations> ConfigValidationsPtr_t;
     typedef boost::shared_ptr <ConnectedComponent> ConnectedComponentPtr_t;
     typedef std::list <ConnectedComponentPtr_t> ConnectedComponents_t;
+    typedef boost::shared_ptr <CCGraph> CCGraphPtr_t;
+    typedef std::list <CCGraphPtr_t> CCGraphs_t;
     typedef boost::shared_ptr <Constraint> ConstraintPtr_t;
     typedef boost::shared_ptr <ConstraintSet> ConstraintSetPtr_t;
     typedef model::Device Device_t;

include/hpp/core/graph-connected-component.hh

+//
+// Copyright (c) 2014 CNRS
+// Authors: Florent Lamiraux
+//          Mukunda Bharatheesha
+//
+// This file is part of hpp-core
+// hpp-core is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-core is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Lesser Public License for more details.  You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-core  If not, see
+// <http://www.gnu.org/licenses/>.
+
+#ifndef HPP_CORE_GRAPH_CONNECTED_COMPONENT_HH
+# define HPP_CORE_GRAPH_CONNECTED_COMPONENT_HH
+
+# include <hpp/core/fwd.hh>
+# include <hpp/core/config.hh>
+# include <hpp/core/connected-component.hh>
+
+namespace hpp {
+  namespace core {
+    /// Graph of Connected components
+    ///
+    /// List of all (strongly) connected components
+    /// List of connected components that can be reached from
+    /// a given connected component
+    /// List of connected components that reach a given connected
+    /// component
+    class HPP_CORE_DLLAPI CCGraph {
+    public:
+      static CCGraphPtr_t create ()
+      {
+	CCGraph* ptr = new CCGraph ();
+	CCGraphPtr_t shPtr (ptr);
+	ptr->init (shPtr);
+	return shPtr;
+      }
+      void addConnectedComponent (const ConnectedComponentPtr_t& cc)
+      {
+	connectedComponents_.push_back (cc);
+      }
+      /// Check and update reachability between two connected components
+      /// \param connectedComponent1: the first connected component
+      /// \param connectedComponent2: the second connected component
+      /// If both connected components are reachable to each other,
+      /// they are merged. Else, their respective reachability lists
+      /// are updated
+      void updateCCReachability (const ConnectedComponentPtr_t&
+				 connectedComponent1, const ConnectedComponentPtr_t&
+				 connectedComponent2);
+
+
+      /// Find Strongly Connected Components (SCC) in roadmap
+      /// \param roadMap: entire roadmap for finding SCC
+      void findSCC ();
+
+      void clear ()
+      {
+	connectedComponents_.clear ();
+	sccHeadsList_.clear ();
+      }
+
+      void setSCCHead (const ConnectedComponentPtr_t& headCC);
+
+      void DFS (const ConnectedComponentPtr_t& cc, bool reverse);
+
+      ///Access to connected components
+      const ConnectedComponents_t& connectedComponents () const
+      {
+	return connectedComponents_;
+      }
+      const ConnectedComponents_t& sccHeads () const
+      {
+	return sccHeadsList_;
+      }
+
+    protected:
+      /// Constructor
+      CCGraph () : connectedComponents_ (), sccHeadsList_ (),
+	weak_ ()
+	  {
+	  }
+      void init (const CCGraphPtr_t& shPtr){
+	weak_ = shPtr;
+      }
+    private:
+      //The complete list of (strongly) connected components
+      ConnectedComponents_t connectedComponents_;
+
+      /// Elements needed for SCC detection algorithm
+      // Leader connected component
+      ConnectedComponents_t sccHeadsList_;
+      CCGraphWkPtr_t weak_;
+    }; // class CCGraph
+  } //   namespace core
+} // namespace hpp
+#endif // HPP_CORE_GRAPH_CONNECTED_COMPONENT_HH

include/hpp/core/node.hh

@@ -59,4 +59,5 @@ namespace hpp {
     }; // class Node
   } //   namespace core
 } // namespace hpp
+std::ostream& operator<< (std::ostream& os, const hpp::core::Node& n);
 #endif // HPP_CORE_NODE_HH

include/hpp/core/roadmap.hh

@@ -19,9 +19,11 @@
 #ifndef HPP_CORE_ROADMAP_HH
 # define HPP_CORE_ROADMAP_HH
 
+# include <iostream>
 # include <hpp/core/fwd.hh>
 # include <hpp/core/config.hh>
 # include <hpp/core/k-d-tree.hh>
+# include <hpp/core/graph-connected-component.hh>
 
 namespace hpp {
   namespace core {
@@ -94,10 +96,6 @@ namespace hpp {
       {
 	return edges_;
       }
-      const ConnectedComponents_t& connectedComponents () const
-      {
-	return connectedComponents_;
-      }
       NodePtr_t initNode () const
       {
 	return initNode_;
@@ -106,6 +104,14 @@ namespace hpp {
       {
 	return goalNodes_;
       }
+      const ConnectedComponents_t& connectedComponents () const
+      {
+	return ccGraph_->connectedComponents ();
+      }
+      const CCGraphPtr_t& ccGraph () const
+      {
+	return ccGraph_;
+      }
       /// \name Distance used for nearest neighbor search
       /// \{
       /// Get distance function
@@ -140,11 +146,12 @@ namespace hpp {
 			 ConnectedComponentPtr_t connectedComponent);
 
       const DistancePtr_t& distance_;
-      ConnectedComponents_t connectedComponents_;
       Nodes_t nodes_;
       Edges_t edges_;
       NodePtr_t initNode_;
       Nodes_t goalNodes_;
+      CCGraphPtr_t ccGraph_;
+
       // use KDTree instead of NearestNeighbor
       //NearetNeighborMap_t nearestNeighbor_;
       KDTree kdTree_;
@@ -152,4 +159,5 @@ namespace hpp {
     }; // class Roadmap
   } //   namespace core
 } // namespace hpp
+std::ostream& operator<< (std::ostream& os, const hpp::core::Roadmap& r);
 #endif // HPP_CORE_ROADMAP_HH

src/CMakeLists.txt

@@ -43,6 +43,7 @@ SET(${LIBRARY_NAME}_SOURCES
   straight-path.cc
   weighed-distance.cc
   k-d-tree.cc
+  graph-connected-component.cc
   )
 
 ADD_LIBRARY(${LIBRARY_NAME}

src/graph-connected-component.cc

+//
+// Copyright (c) 2014 CNRS
+// Authors: Mukunda Bharatheesha
+//          Florent Lamiraux
+//
+// This file is part of hpp-core
+// hpp-core is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// hpp-core is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+// General Lesser Public License for more details.  You should have
+// received a copy of the GNU Lesser General Public License along with
+// hpp-core  If not, see
+// <http://www.gnu.org/licenses/>.
+
+#include <algorithm>
+#include <hpp/util/debug.hh>
+#include <hpp/core/connected-component.hh>
+#include <hpp/core/graph-connected-component.hh>
+
+namespace hpp {
+  namespace core {
+
+    void CCGraph::updateCCReachability (const ConnectedComponentPtr_t& cc1,
+					const ConnectedComponentPtr_t& cc2)
+    {
+      //Update the respective reachability lists of the connected components
+      //CC Iterator for connectivity update
+      ConnectedComponents_t::iterator itcc;
+
+      itcc = cc1->reachableTo_.end ();
+      itcc = cc1->reachableTo_.insert (itcc, cc2);
+      //Remove multiple copies
+      cc1->reachableTo_.sort ();cc1->reachableTo_.unique ();
+
+      itcc = cc2->reachableFrom_.end ();
+      itcc = cc2->reachableFrom_.insert (itcc, cc1);
+      //Remove multiple copies
+      cc2->reachableFrom_.sort ();cc2->reachableFrom_.unique ();
+    }
+
+    /// Finds Strongly Connected Components (SCCs) in the CCGraph
+    /// using "Kosaraju's Two-Pass algorithm".
+    void CCGraph::findSCC ()
+    {
+      ConnectedComponents_t::iterator itcc;
+      ConnectedComponents_t::iterator itSCCHeads;
+
+      itSCCHeads = sccHeadsList_.begin ();
+      //variable to indicate direction of DFS
+      bool reverseDFS_ = true;
+      //Search for loops in a reverse manner using DFS
+      for (itcc = connectedComponents_.begin ();
+	   itcc != connectedComponents_.end ();
+	   itcc++) {
+	if (!((*itcc)->isExplored ())) {
+	  setSCCHead (*itcc);
+	  DFS (*itcc, reverseDFS_);
+	}
+      }
+      //Sort the connected components in decreasing order
+      //of finish time
+      connectedComponents_.sort
+	(ConnectedComponent::compareCCFinishTime ());
+
+      //Clear the leader list for the forward DFS
+      sccHeadsList_.clear ();
+
+      //Reset explored status of all CCs
+      for (itcc = connectedComponents_.begin ();
+	   itcc != connectedComponents_.end ();
+	   itcc++) {
+	(*itcc)->resetExplored ();
+      }
+
+      itSCCHeads = sccHeadsList_.begin ();
+      bool isAdvanced_;
+      //Reset the reverse DFS flag
+      reverseDFS_ = false;
+
+      //Search for loops in a forward manner using DFS
+      for (itcc = connectedComponents_.begin ();
+	   itcc != connectedComponents_.end ();
+	   itcc++) {
+	if (!((*itcc)->isExplored ())) {
+	  setSCCHead (*itcc);
+	  (*itcc)->setLeader(*itcc);
+	  isAdvanced_=false;
+	  DFS (*itcc, reverseDFS_);
+	}
+	else {
+	  if(!isAdvanced_) {
+	    std::advance(itSCCHeads, 1);
+	    isAdvanced_ = true;
+	  }
+	  //Merge mutually reachable CCs
+	  (*itcc)->setLeader (*(itSCCHeads));
+	  if ( itSCCHeads != sccHeadsList_.end () ) {
+	    (*itSCCHeads)->merge (*itcc);
+	  }
+	}
+      }
+      //Remove the merged connected components
+      for (itcc = sccHeadsList_.begin ();
+	   itcc != sccHeadsList_.end ();
+	   itcc++) {
+	for (ConnectedComponents_t::iterator itcc1 =
+	       (*itcc)->reachableTo_.begin ();
+	     itcc1 != (*itcc)->reachableTo_.end ();
+	     itcc1++) {
+	  ConnectedComponents_t::iterator itLeader;
+	  itLeader = std::find (sccHeadsList_.begin (),
+				sccHeadsList_.end (),
+				(*itcc1)->getLeader ());
+	  *itcc1 = *itLeader;
+	}
+	(*itcc)->reachableTo_.sort ();
+	(*itcc)->reachableTo_.unique();
+	for (ConnectedComponents_t::iterator itcc1 =
+	       (*itcc)->reachableFrom_.begin ();
+	     itcc1 != (*itcc)->reachableFrom_.end ();
+	     itcc1++) {
+	  ConnectedComponents_t::iterator itLeader;
+	  itLeader = std::find (sccHeadsList_.begin (),
+				sccHeadsList_.end (),
+				(*itcc1)->getLeader ());
+	  *itcc1 = *itLeader;
+	}
+	(*itcc)->reachableFrom_.sort ();
+	(*itcc)->reachableFrom_.unique();
+      }
+      connectedComponents_.remove_if
+	(ConnectedComponent::emptyCC ());
+
+      //Clear the leader list for the forward DFS
+      sccHeadsList_.clear ();
+
+      //Reset explored status of all CCs
+      for (itcc = connectedComponents_.begin ();
+	   itcc != connectedComponents_.end ();
+	   itcc++) {
+	(*itcc)->resetExplored ();
+      }
+      //Reset current ranks of all CCs
+      for (itcc = connectedComponents_.begin ();
+	   itcc != connectedComponents_.end ();
+	   itcc ++) {
+	(*itcc)->resetFinishTime ();
+      }
+      ConnectedComponent::globalFinishTime_ = 0;
+    }
+
+    unsigned int ConnectedComponent::globalFinishTime_ = 0;
+
+    void CCGraph::setSCCHead
+    (const ConnectedComponentPtr_t& headCC)
+    {
+      sccHeadsList_.push_back (headCC);
+    }
+
+    void CCGraph::DFS
+    (const ConnectedComponentPtr_t& cc, bool reverse)
+    {
+      ConnectedComponents_t::iterator itcc_dfs;
+      cc->setExplored ();
+
+      if (reverse) {
+	//The reverse CC Graph traversal is accomplished
+	//by using the reachableFrom list of the connected
+	//components
+	for (itcc_dfs = cc->reachableFrom_.begin (); itcc_dfs != cc->reachableFrom_.end ();
+	     itcc_dfs++) {
+	  if (!((*itcc_dfs)->isExplored ())) {
+	    DFS (*itcc_dfs, reverse);
+	  }
+	}
+      }
+      else {
+	for (itcc_dfs = cc->reachableTo_.begin (); itcc_dfs != cc->reachableTo_.end ();
+	     itcc_dfs++) {
+	  if (!((*itcc_dfs)->isExplored ())) {
+	    DFS (*itcc_dfs, reverse);
+	  }
+	}
+      }
+      ConnectedComponent::globalFinishTime_++;
+      cc->setFinishTime();
+    }
+
+  } //    namespace core
+}// namespace hpp

src/node.cc

@@ -103,3 +103,9 @@ namespace hpp {
     }
   } //   namespace core
 } // namespace hpp
+
+std::ostream& operator<< (std::ostream& os, const hpp::core::Node& n)
+{
+  os << n.configuration ()->transpose () << std::endl;
+  return os;
+}

src/roadmap.cc

@@ -46,10 +46,8 @@ namespace hpp {
     }
 
     Roadmap::Roadmap (const DistancePtr_t& distance, const DevicePtr_t& robot) :
-      distance_ (distance), connectedComponents_ (), nodes_ (), edges_ (),
-      initNode_ (), goalNodes_ (),
-      //nearestNeighbor_ (),
-      kdTree_(robot, distance, 30)
+      distance_ (distance), nodes_ (), edges_ (), initNode_ (),goalNodes_ (),
+      ccGraph_ (CCGraph::create ()), kdTree_(robot, distance, 30)
     {
     }
 
@@ -60,7 +58,7 @@ namespace hpp {
 
     void Roadmap::clear ()
     {
-      connectedComponents_.clear ();
+      ccGraph_->clear ();
 
       for (Nodes_t::iterator it = nodes_.begin (); it != nodes_.end (); it++) {
 	delete *it;
@@ -94,8 +92,8 @@ namespace hpp {
       hppDout (info, "Added node: " << displayConfig (*configuration));
       nodes_.push_back (node);
       // Node constructor creates a new connected component. This new
-      // connected component needs to be added in the roadmap and the
-      // new node needs to be registered in the connected component.
+      // connected component needs to be added in the graph of connected and the
+      // components and new node needs to be registered in the connected component.
       addConnectedComponent (node);
       return node;
     }
@@ -130,11 +128,9 @@ namespace hpp {
 				       const ConfigurationPtr_t& to,
 				       const PathPtr_t path)
     {
-      interval_t timeRange = path->timeRange ();
       NodePtr_t nodeTo = addNode (to, from->connectedComponent ());
       addEdge (from, nodeTo, path);
-      addEdge (nodeTo, from, path->extract
-	       (interval_t (timeRange.second, timeRange.first)));
+      addEdge (nodeTo, from, path->reverse ());
       return nodeTo;
     }
 
@@ -145,8 +141,8 @@ namespace hpp {
       NodePtr_t closest;
       minDistance = std::numeric_limits<value_type>::infinity ();
       for (ConnectedComponents_t::const_iterator itcc =
-	     connectedComponents_.begin ();
-	   itcc != connectedComponents_.end (); itcc++) {
+	     ccGraph_->connectedComponents ().begin ();
+	   itcc != ccGraph_->connectedComponents ().end (); itcc++) {
 	value_type distance;
 	NodePtr_t node;
 	//node = nearestNeighbor_ [*itcc]->nearest (configuration, distance);
@@ -166,8 +162,6 @@ namespace hpp {
 			  value_type& minDistance)
     {
       assert (connectedComponent);
-      //return nearestNeighbor_ [connectedComponent]->nearest (configuration,
-      //						     minDistance);
       return kdTree_.search(configuration, connectedComponent, minDistance);
     }
 
@@ -193,34 +187,24 @@ namespace hpp {
 	       displayConfig (*(n1->configuration ())));
       hppDout (info, "               and: " <<
 	       displayConfig (*(n2->configuration ())));
-      // If node connected components are different, merge them
+
       ConnectedComponentPtr_t cc1 = n1->connectedComponent ();
       ConnectedComponentPtr_t cc2 = n2->connectedComponent ();
-      if (cc1 != cc2) {
-	cc1->merge (cc2);
-	//nearestNeighbor_ [cc1]->merge (nearestNeighbor_ [cc2]);
-	kdTree_.merge(cc1, cc2);
-	// Remove cc2 from list of connected components
-	ConnectedComponents_t::iterator itcc =
-	  std::find (connectedComponents_.begin (), connectedComponents_.end (),
-		     cc2);
-	assert (itcc != connectedComponents_.end ());
-	connectedComponents_.erase (itcc);
-	// remove cc2 from map of nearest neighbors
-	//NearetNeighborMap_t::iterator itnear = nearestNeighbor_.find (cc2);
-	//assert (itnear != nearestNeighbor_.end ());
-	//nearestNeighbor_.erase (itnear);
-      }
+
+      //Check and update reachability of the connected components
+      ccGraph_->updateCCReachability (cc1, cc2);
+      //Find Strongly Connected Components (SCC) in CCGraph and merge them
+      ccGraph_->findSCC ();
+
       return edge;
     }
 
     void Roadmap::addConnectedComponent (const NodePtr_t& node)
     {