Commit 937b0451 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Implement dynamic computation of connected components.

parent 7095e81a
......@@ -32,10 +32,6 @@ namespace hpp {
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 ()
......@@ -45,57 +41,12 @@ 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.
/// \note other will be empty after calling this method.
void merge (const ConnectedComponentPtr_t& other)
{
for (Nodes_t::iterator itNode = other->nodes_.begin ();
itNode != other->nodes_.end (); itNode++) {
(*itNode)->connectedComponent (weak_.lock ());
}
void merge (const ConnectedComponentPtr_t& other);
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.
void addNode (const NodePtr_t& node)
......@@ -107,21 +58,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 (); }
};
/// \name Reachability
/// \{
/// Whether this connected component can reach cc
/// \param cc a connected component
bool canReach (const ConnectedComponentPtr_t& cc);
/// Whether this connected component can reach cc
/// \param cc a connected component
/// \retval cc2Tocc1 list of connected components between cc2 and cc1
/// that should be merged.
bool canReach (const ConnectedComponentPtr_t& cc,
ConnectedComponents_t& cc2Tocc1);
// Get connected components reachable from this
const ConnectedComponents_t& reachableTo () const
{
return reachableTo_;
}
// Get connected components that can reach this
const ConnectedComponents_t& reachableFrom () const
{
return reachableFrom_;
}
/// \}
protected:
/// Constructor
ConnectedComponent () : nodes_ (), weak_ (),
finishTimeCC_ (0), explored_ (false),
leaderCC_ ()
ConnectedComponent () : nodes_ (), explored_ (false), weak_ ()
{
}
void init (const ConnectedComponentPtr_t& shPtr){
......@@ -129,15 +96,15 @@ namespace hpp {
}
private:
Nodes_t nodes_;
// List of CCs from which this connected component can be reached
ConnectedComponents_t reachableFrom_;
// List of CCs that can be reached from this connected component
ConnectedComponents_t reachableTo_;
// status variable to indicate whether or not CC has been visited
mutable bool explored_;
ConnectedComponentWkPtr_t weak_;
// rank of the connected component in the ConnectedComponentGraph
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_;
friend class Roadmap;
friend void clean (ConnectedComponents_t& set);
}; // class ConnectedComponent
} // namespace core
} // namespace hpp
......
......@@ -23,6 +23,7 @@
# include <vector>
# include <deque>
# include <list>
# include <set>
# include <hpp/util/pointer.hh>
# include <roboptim/core/function.hh>
# include <hpp/model/fwd.hh>
......@@ -40,7 +41,6 @@ namespace hpp {
HPP_PREDEF_CLASS (ConfigValidation);
HPP_PREDEF_CLASS (ConfigValidations);
HPP_PREDEF_CLASS (ConnectedComponent);
HPP_PREDEF_CLASS (ConnectedComponentGraph);
HPP_PREDEF_CLASS (Constraint);
HPP_PREDEF_CLASS (ConstraintSet);
HPP_PREDEF_CLASS (DifferentiableFunction);
......@@ -90,11 +90,7 @@ namespace hpp {
typedef boost::shared_ptr <ConfigValidation> ConfigValidationPtr_t;
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 <ConnectedComponentGraph>
ConnectedComponentGraphPtr_t;
typedef std::list <ConnectedComponentGraphPtr_t>
ConnectedComponentGraphs_t;
typedef std::set <ConnectedComponentPtr_t> ConnectedComponents_t;
typedef boost::shared_ptr <Constraint> ConstraintPtr_t;
typedef boost::shared_ptr <ConstraintSet> ConstraintSetPtr_t;
typedef model::Device Device_t;
......@@ -121,7 +117,6 @@ namespace hpp {
typedef Eigen::Ref <const matrix_t> matrixIn_t;
typedef Eigen::Ref <matrix_t> matrixOut_t;
typedef std::list <Node*> Nodes_t;
typedef std::list <Node*> Nodes_t;
typedef Node* NodePtr_t;
typedef model::ObjectVector_t ObjectVector_t;
typedef boost::shared_ptr <Path> PathPtr_t;
......
......@@ -26,9 +26,6 @@
namespace hpp {
namespace core {
HPP_PREDEF_CLASS (NearestNeighbor);
typedef boost::shared_ptr <NearestNeighbor> NearestNeighborPtr_t;
/// Roadmap built by random path planning methods
/// Nodes are configurations, paths are collision-free paths.
class HPP_CORE_DLLAPI Roadmap {
......@@ -109,11 +106,6 @@ namespace hpp {
// Get list of connected component of the roadmap
const ConnectedComponents_t& connectedComponents () const;
// Get graph of connected components.
const ConnectedComponentGraphPtr_t& ccGraph () const
{
return ccGraph_;
}
/// \name Distance used for nearest neighbor search
/// \{
/// Get distance function
......@@ -132,10 +124,12 @@ namespace hpp {
/// \param node node pointing to the connected component.
/// \note The node is added in the connected component.
void addConnectedComponent (const NodePtr_t& node);
/// Whether nodes of cc can be reached by nodes of this
/// \param cc a connected component.
bool canReach (const ConnectedComponentPtr_t& cc);
private:
typedef std::map <ConnectedComponentPtr_t, NearestNeighborPtr_t>
NearetNeighborMap_t;
/// Add a node with given configuration
/// \param config configuration
/// \param connectedComponent Connected component the node will belong
......@@ -155,15 +149,24 @@ namespace hpp {
void addEdges (const NodePtr_t from, const NodePtr_t& to,
const PathPtr_t& path);
/// Update the graph of connected components after new connection
/// \param cc1, cc2 the two connected components that have just been
/// connected.
void connect (const ConnectedComponentPtr_t& cc1,
const ConnectedComponentPtr_t& cc2);
/// Merge two connected components
/// \param cc1 the connected component to merge into
/// \param the connected components to merge into cc1.
void merge (const ConnectedComponentPtr_t& cc1,
ConnectedComponents_t& ccs);
const DistancePtr_t& distance_;
ConnectedComponents_t connectedComponents_;
Nodes_t nodes_;
Edges_t edges_;
NodePtr_t initNode_;
Nodes_t goalNodes_;
ConnectedComponentGraphPtr_t ccGraph_;
// use KDTree instead of NearestNeighbor
//NearetNeighborMap_t nearestNeighbor_;
KDTree kdTree_;
}; // class Roadmap
......
......@@ -24,6 +24,7 @@ SET(${LIBRARY_NAME}_SOURCES
collision-validation.cc
config-projector.cc
config-validations.cc
connected-component.cc
constraint.cc
constraint-set.cc
diffusing-planner.cc
......@@ -43,8 +44,6 @@ SET(${LIBRARY_NAME}_SOURCES
straight-path.cc
weighed-distance.cc
k-d-tree.cc
connected-component-graph.hh
connected-component-graph.cc
)
ADD_LIBRARY(${LIBRARY_NAME}
......
//
// 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 "connected-component-graph.hh"
namespace hpp {
namespace core {
void ConnectedComponentGraph::updateCCReachability
(const ConnectedComponentPtr_t& cc1, const ConnectedComponentPtr_t& cc2)
{
//Update the respective reachability lists of the connected components
//CC Iterator for connectivity update
cc1->reachableTo_.push_back (cc2);
//Remove multiple copies
cc1->reachableTo_.sort ();cc1->reachableTo_.unique ();
cc2->reachableFrom_.push_back (cc1);
//Remove multiple copies
cc2->reachableFrom_.sort ();cc2->reachableFrom_.unique ();
}
/// Finds Strongly Connected Components (SCCs) in the
/// ConnectedComponentGraph using "Kosaraju's Two-Pass algorithm".
void ConnectedComponentGraph::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 ConnectedComponentGraph::setSCCHead
(const ConnectedComponentPtr_t& headCC)
{
sccHeadsList_.push_back (headCC);
}
void ConnectedComponentGraph::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
//
// 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_CONNECTED_COMPONENT_GRAPH_HH
# define HPP_CORE_CONNECTED_COMPONENT_GRAPH_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 ConnectedComponentGraph {
public:
static ConnectedComponentGraphPtr_t create ()
{
ConnectedComponentGraph* ptr = new ConnectedComponentGraph ();
ConnectedComponentGraphPtr_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
ConnectedComponentGraph () : connectedComponents_ (), sccHeadsList_ (),
weak_ ()
{
}
void init (const ConnectedComponentGraphPtr_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_;
ConnectedComponentGraphWkPtr_t weak_;
}; // class ConnectedComponentGraph
} // namespace core
} // namespace hpp
#endif // HPP_CORE_CONNECTED_COMPONENT_GRAPH_HH
//
// Copyright (c) 2014 CNRS
// Authors: 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/core/connected-component.hh>
#include <hpp/core/edge.hh>
namespace hpp {
namespace core {
// Mark connected components of a list as unexplored.
void clean (ConnectedComponents_t& set)
{
for (ConnectedComponents_t::iterator it = set.begin ();
it != set.end (); ++it) {
(*it)->explored_ = false;
}
}
void ConnectedComponent::merge (const ConnectedComponentPtr_t& other)
{
// Tell other's nodes that they now belong to this connected component
for (Nodes_t::iterator itNode = other->nodes_.begin ();
itNode != other->nodes_.end (); itNode++) {
(*itNode)->connectedComponent (weak_.lock ());
}
// Add other's nodes to this list.
nodes_.splice (nodes_.end (), other->nodes_);
// Tell other's reachableTo's that other has been replaced by this
for (ConnectedComponents_t::iterator itcc = other->reachableTo_.begin ();
itcc != other->reachableTo_.end (); ++itcc) {
(*itcc)->reachableFrom_.erase (other);
(*itcc)->reachableFrom_.insert (weak_.lock ());
}
// Tell other's reachableFrom's that other has been replaced by this
for (ConnectedComponents_t::iterator itcc=other->reachableFrom_.begin ();
itcc != other->reachableFrom_.end (); ++itcc) {
(*itcc)->reachableTo_.erase (other);
(*itcc)->reachableTo_.insert (weak_.lock ());
}
ConnectedComponents_t tmp;
std::set_union (reachableTo_.begin (), reachableTo_.end (),
other->reachableTo_.begin (), other->reachableTo_.end (),
std::inserter (tmp, tmp.begin ()));