Commit 7322ccda authored by Mukunda Bharatheesha's avatar Mukunda Bharatheesha Committed by Florent Lamiraux
Browse files

Implement oriented roadmap

  - new implementation of connected components.
parent a2f586d4
......@@ -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")
......
......@@ -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
......
......@@ -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;
......
//
// 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
......@@ -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
......@@ -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
......@@ -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}
......
//
// 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
......@@ -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;
}
......@@ -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)
{