void Graph::addEdge(const string& tail, const string& head, const float cost, const string& name)
{
// find the pointers to nodes at head and tail, based on the strings
// throw errors if either is not found
nodePtr nodeTail = findNode(tail);
if (!nodeTail)
{
string errorString = "File error : Node " + tail + " does not exist.";
throw runtime_error(errorString.c_str());
}
nodePtr nodeHead = findNode(head);
if (!nodeHead)
{
string errorString = "File error : Node " + head + " does not exist.";
throw runtime_error(errorString.c_str());
}
// after eliminating potential errors, create the edge, add it to the list
// and add it to the tail node's list of leaving edges
edgePtr newEdge = edgePtr(new Edge( nodeTail, nodeHead, cost, name) );
edgeList.push_back( newEdge );
nodeTail->addEdge( newEdge );
}
/** Method for getting a bound graph from the one stored into the object.
* \param boundGraph: resulting graph, it must be empty when the method
* is called.
* \param nodesToBound: a map containing as key the id of a node, and
* as value the class/state to be bound.
*/
void getBoundGraph( CGraph &boundGraph,
std::map<size_t,size_t> nodesToBound )
{
// Check that the graph is empty
assert( boundGraph.getNodes().size() == 0 );
// Copy the graph into boundGraph
// Copy nodes
for ( size_t node = 0; node < m_nodes.size(); node++ )
{
CNodePtr nodePtr(new CNode(*m_nodes[node]));
boundGraph.addNode( nodePtr );
}
// Copy edges
for ( size_t edge = 0; edge < m_edges.size(); edge++ )
{
CEdgePtr edgePtr( new CEdge(*m_edges[edge]));
boundGraph.addEdge( edgePtr );
}
// Okey, now iterate over the nodes to be bound, removing then from
// the resulting graph by expanding the potential associated with
// their bound state
for ( std::map<size_t,size_t>::iterator it = nodesToBound.begin();
it != nodesToBound.end();
it++ )
{
size_t nodeID = it->first;
size_t nodeState = it->second;
// Potential of the state to be bounded
double nodePotential = boundGraph.getNodeWithID( nodeID )->getPotentials()( nodeState );
// Iterate over the neighbours, updating their node potentials
// according to the state of the node to be bound and its potential
pair<multimap<size_t,CEdgePtr>::iterator,multimap<size_t,CEdgePtr>::iterator > neighbors;
neighbors = boundGraph.getEdgesF().equal_range(nodeID);
for ( multimap<size_t,CEdgePtr>::iterator it = neighbors.first;
it != neighbors.second;
it++ )
{
CEdgePtr edgePtr( (*it).second );
Eigen::MatrixXd edgePotentials = edgePtr->getPotentials();
size_t ID1, ID2;
edgePtr->getNodesID(ID1,ID2);
// If the node to be bound is the first one appearing in the
// edge.
if ( ID1 == nodeID )
{
CNodePtr nodePtr = boundGraph.getNodeWithID( ID2 );
Eigen::VectorXd boundPotentials = edgePotentials.row(nodeState).transpose()*nodePotential;
Eigen::VectorXd newPotentials = nodePtr->getPotentials().cwiseProduct(boundPotentials);
nodePtr->setPotentials( newPotentials );
}
else // If it is the second one in the edge
{
CNodePtr nodePtr = boundGraph.getNodeWithID( ID1 );
Eigen::VectorXd boundPotentials = edgePotentials.col( nodeState )*nodePotential;
Eigen::VectorXd newPotentials = nodePtr->getPotentials().cwiseProduct(boundPotentials);
nodePtr->setPotentials( newPotentials );
}
}
// Now that the potential of the state of the node to bound
// has been expanded, delete the node from the graph.
boundGraph.deleteNode( nodeID );
}
}
