/** Function for computing the potentials of the nodes and the edges in
* the graph.
*/
void computePotentials()
{
// Method steps:
// 1. Compute node potentials
// 2. Compute edge potentials
//
// 1. Node potentials
//
std::vector<CNodePtr>::iterator it;
//cout << "NODE POTENTIALS" << endl;
for ( it = m_nodes.begin(); it != m_nodes.end(); it++ )
{
CNodePtr nodePtr = *it;
if ( !nodePtr->finalPotentials() )
{
// Get the node type
//size_t type = nodePtr->getType()->getID();
// Compute the node potentials according to the node type and its
// extracted features
Eigen::VectorXd potentials = nodePtr->getType()->computePotentials( nodePtr->getFeatures() );
// Apply the node class multipliers
potentials = potentials.cwiseProduct( nodePtr->getClassMultipliers() );
/*Eigen::VectorXd fixed = nodePtr->getFixed();
potentials = potentials.cwiseProduct( fixed );*/
nodePtr->setPotentials( potentials );
}
}
//
// 2. Edge potentials
//
std::vector<CEdgePtr>::iterator it2;
//cout << "EDGE POTENTIALS" << endl;
for ( it2 = m_edges.begin(); it2 != m_edges.end(); it2++ )
{
CEdgePtr edgePtr = *it2;
Eigen::MatrixXd potentials
= edgePtr->getType()->computePotentials( edgePtr->getFeatures() );
edgePtr->setPotentials ( potentials );
}
}
/** Delete and edge from the graph.
* \param ID: ID of the edge.
*/
void deleteEdge( size_t ID )
{
CEdgePtr edgePtr;
std::vector<CEdgePtr>::iterator it;
for ( it = m_edges.begin(); it != m_edges.end(); it++ )
{
if ( (*it)->getID() == ID )
{
edgePtr = *it;
break;
}
}
if ( it != m_edges.end() )
{
// Delete the edge from the m_edge_f multimap
size_t ID1, ID2;
edgePtr->getNodesID(ID1,ID2);
std::multimap<size_t,CEdgePtr>::iterator it_n1= m_edges_f.find( ID1 );
for ( ; it_n1 != m_edges_f.end(); it_n1++ )
{
if ( (it_n1->second)->getID() == ID )
{
m_edges_f.erase( it_n1 );
break;
}
}
std::multimap<size_t,CEdgePtr>::iterator it_n2= m_edges_f.find( ID2 );
for ( ; it_n2 != m_edges_f.end(); it_n2++ )
{
if ( (it_n2->second)->getID() == ID )
{
m_edges_f.erase( it_n2 );
break;
}
}
// Delete the edge from the edges vector
m_edges.erase( it );
}
}
/** Add an edge to the graph.
* \param edge: Smart pointer to the edge.
*/
void addEdge( CEdgePtr edge )
{
CNodePtr n1, n2;
edge->getNodes(n1,n2);
size_t n1_id = n1->getID();
size_t n2_id = n2->getID();
m_edges.push_back(edge);
m_edges_f.insert( std::pair<size_t, CEdgePtr> (n1_id,edge) );
m_edges_f.insert( std::pair<size_t, CEdgePtr> (n2_id,edge) );
}
/** This method permits the user to get the unnormalized log likelihood
* of the graph given a certain assignation to all its nodes.
* \param classes: Classes assignation to all the nodes.
* \return Unnormalized log likelihood.
*/
double getUnnormalizedLogLikelihood( std::map<size_t,size_t> &classes, bool debug = false )
{
DEBUG("Computing likelihood...");
double unlikelihood = 0;
//size_t N_nodes = m_nodes.size();
size_t N_edges = m_edges.size();
std::map<size_t,size_t>::iterator it;
DEBUG("Computing nodes likelihood...");
for ( it = classes.begin(); it != classes.end(); it++ )
{
CNodePtr node = getNodeWithID( it->first );
//unlikelihood *= node->getPotentials()(classes[node->getID()]);
unlikelihood += node->getPotentials()(it->second);
}
DEBUG("Computing edges likelihood...");
for ( size_t index = 0; index < N_edges; index++ )
{
CEdgePtr edge = m_edges[index];
CNodePtr n1, n2;
edge->getNodes(n1,n2);
size_t ID1 = n1->getID();
size_t ID2 = n2->getID();
size_t IDNodeType1 = n1->getType()->getID();
size_t IDNodeType2 = n2->getType()->getID();
// Check if the nodes share the same type.
if ( IDNodeType1 == IDNodeType2 )
// If they do, then the node with the lower id is the one appearing
// first in the edge;
if ( ID1 > ID2 )
//unlikelihood *= edge->getPotentials()(classes[ID2],classes[ID1]);
unlikelihood += std::log(edge->getPotentials()(classes[ID2],classes[ID1]));
else
unlikelihood += std::log(edge->getPotentials()(classes[ID1],classes[ID2]));
else
// If not, the node with the lower nodeType is the first
if ( IDNodeType1 > IDNodeType2 )
unlikelihood += std::log(edge->getPotentials()(classes[ID2],classes[ID1]));
else
unlikelihood += std::log(edge->getPotentials()(classes[ID1],classes[ID2]));
}
//unlikelihood = std::log( unlikelihood );
DEBUG("Done.");
return unlikelihood;
}
void UPGMpp::getSpanningTree( CGraph &graph, std::vector<size_t> &tree)
{
// TODO: The efficiency of this method can be improved
// Reset tree
tree.clear();
static boost::mt19937 rng;
static bool firstExecution = true;
if ( firstExecution )
{
rng.seed(time(0));
firstExecution = false;
}
vector<CNodePtr> &v_nodes = graph.getNodes();
vector<CNodePtr> v_nodesToExplore;
map<size_t,size_t> nodesToExploreMap;
size_t N_nodes = v_nodes.size();
//cout << "Random: ";
for ( size_t i_node = 0; i_node < N_nodes; i_node++ )
{
boost::uniform_real<> real_generator(0,1);
real_generator.reset();
double rand = real_generator(rng);
//cout << rand << " ";
if ( rand > 0.25 )
{
v_nodesToExplore.push_back( v_nodes[i_node] );
nodesToExploreMap[ v_nodes[i_node]->getID() ] = v_nodesToExplore.size()-1;
}
}
//cout << endl;
//SHOW_VECTOR_NODES_ID("Nodes to explore: ", v_nodesToExplore)
size_t N_nodesToExplore = v_nodesToExplore.size();
if ( !N_nodesToExplore )
return;
bool nodeAdded = true;
vector<bool> v_nodesAdded( N_nodesToExplore, false );
//
// Randomly select the root node
//
boost::uniform_int<> generator(0,N_nodesToExplore-1);
int rand = generator(rng);
//cout << "Root:" << rand << endl;
v_nodesAdded[rand] = true;
multimap<size_t, CEdgePtr> &edgesF = graph.getEdgesF();
while ( nodeAdded )
{
nodeAdded = false;
for ( size_t i_node = 0; i_node < N_nodesToExplore; i_node++ )
{
//cout << "Node " << i_node << " ID "<< v_nodesToExplore[i_node]->getID() << " Added? " << v_nodesAdded[i_node] << endl;
// Check that the node has not been added to the tree yet
if ( !v_nodesAdded[i_node] )
{
size_t nodeID = v_nodesToExplore[i_node]->getID();
NEIGHBORS_IT neighbors = edgesF.equal_range(nodeID);
for ( multimap<size_t,CEdgePtr>::iterator itNeigbhor = neighbors.first;
itNeigbhor != neighbors.second;
itNeigbhor++ )
{
CEdgePtr edgePtr = (*itNeigbhor).second;
size_t neighborID;
if ( !edgePtr->getNodePosition( nodeID ) )
neighborID = edgePtr->getSecondNodeID();
else
neighborID = edgePtr->getFirstNodeID();
//cout << "Current node ID: " << nodeID << " neighbor: " << neighborID << endl;
if ( v_nodesAdded[nodesToExploreMap[neighborID]] )
{
v_nodesAdded[i_node] = true;
nodeAdded = true;
}
}
}
}
}
//SHOW_VECTOR("Nodes in tree: ", v_nodesAdded)
for ( size_t i_node = 0; i_node < v_nodesToExplore.size(); i_node++ )
{
if ( v_nodesAdded[i_node] )
tree.push_back( v_nodesToExplore[i_node]->getID() );
}
//SHOW_VECTOR("Tree: ", tree)
}
/** Delete a node from the graph. It could also produce the deletion of
* its associated edges.
* \param ID: ID of the node to delete from the graph.
*/
void deleteNode( size_t ID )
{
// Remove from the vector of nodes
std::vector<CNodePtr>::iterator it;
for ( it = m_nodes.begin(); it != m_nodes.end(); it++ )
{
CNodePtr nodePtr = *it;
if ( nodePtr->getID() == ID )
break;
}
size_t nodeTypeID = (*it)->getType()->getID();
// Remove!
if ( it != m_nodes.end() )
m_nodes.erase( it );
// Check if other nodes have the same nodeType, and delete that type if not.
bool nodeTypeStillInUse = false;
for ( size_t i = 0; i < m_nodes.size(); i++ )
if ( m_nodes[i]->getType()->getID() == nodeTypeID )
{
nodeTypeStillInUse = true;
break;
}
if ( !nodeTypeStillInUse )
{
std::vector<CNodeTypePtr>::iterator it;
for ( it = m_nodeTypes.begin(); it != m_nodeTypes.end(); it++ )
{
CNodeTypePtr nodeTypePtr = *it;
if ( nodeTypePtr->getID() == nodeTypeID )
{
m_nodeTypes.erase( it );
break;
}
}
}
// Remove edges than contain that node from m_edges and m_edges_f
for ( size_t i = 0; i < m_edges.size(); )
{
CEdgePtr edgePtr = m_edges[i];
size_t ID1, ID2;
edgePtr->getNodesID( ID1, ID2 );
if ( ( ID1 == ID ) || ( ID2 == ID ) )
deleteEdge( edgePtr->getID() );
else
i++;
}
}
void CLBPInference::infer(CGraph &graph,
map<size_t,VectorXd> &nodeBeliefs,
map<size_t,MatrixXd> &edgeBeliefs,
double &logZ)
{
//
// Algorithm workflow:
// 1. Compute the messages passed
// 2. Compute node beliefs
// 3. Compute edge beliefs
// 4. Compute logZ
//
nodeBeliefs.clear();
edgeBeliefs.clear();
const vector<CNodePtr> nodes = graph.getNodes();
const vector<CEdgePtr> edges = graph.getEdges();
multimap<size_t,CEdgePtr> edges_f = graph.getEdgesF();
size_t N_nodes = nodes.size();
size_t N_edges = edges.size();
//
// 1. Compute the messages passed
//
vector<vector<VectorXd> > messages;
bool maximize = false;
messagesLBP( graph, m_options, messages, maximize );
//
// 2. Compute node beliefs
//
for ( size_t nodeIndex = 0; nodeIndex < N_nodes; nodeIndex++ )
{
const CNodePtr nodePtr = graph.getNode( nodeIndex );
size_t nodeID = nodePtr->getID();
VectorXd nodePotPlusIncMsg = nodePtr->getPotentials( m_options.considerNodeFixedValues );
NEIGHBORS_IT neighbors = edges_f.equal_range(nodeID);
//
// Get the messages for all the neighbors, and multiply them with the node potential
//
for ( multimap<size_t,CEdgePtr>::iterator itNeigbhor = neighbors.first;
itNeigbhor != neighbors.second;
itNeigbhor++ )
{
CEdgePtr edgePtr( (*itNeigbhor).second );
size_t edgeIndex = graph.getEdgeIndex( edgePtr->getID() );
if ( !edgePtr->getNodePosition( nodeID ) ) // nodeID is the first node in the edge
nodePotPlusIncMsg = nodePotPlusIncMsg.cwiseProduct(messages[ edgeIndex ][ 1 ]);
else // nodeID is the second node in the dege
nodePotPlusIncMsg = nodePotPlusIncMsg.cwiseProduct(messages[ edgeIndex ][ 0 ]);
}
// Normalize
nodePotPlusIncMsg = nodePotPlusIncMsg / nodePotPlusIncMsg.sum();
nodeBeliefs[ nodeID ] = nodePotPlusIncMsg;
//cout << "Beliefs of node " << nodeIndex << endl << nodePotPlusIncMsg << endl;
}
//
// 3. Compute edge beliefs
//
for ( size_t edgeIndex = 0; edgeIndex < N_edges; edgeIndex++ )
{
CEdgePtr edgePtr = edges[edgeIndex];
size_t edgeID = edgePtr->getID();
size_t ID1, ID2;
edgePtr->getNodesID( ID1, ID2 );
MatrixXd edgePotentials = edgePtr->getPotentials();
MatrixXd edgeBelief = edgePotentials;
VectorXd &message1To2 = messages[edgeIndex][0];
VectorXd &message2To1 = messages[edgeIndex][1];
//cout << "----------------------" << endl;
//cout << nodeBeliefs[ ID1 ] << endl;
//cout << "----------------------" << endl;
//cout << message2To1 << endl;
VectorXd node1Belief = nodeBeliefs[ ID1 ].cwiseQuotient( message2To1 );
VectorXd node2Belief = nodeBeliefs[ ID2 ].cwiseQuotient( message1To2 );
//cout << "----------------------" << endl;
MatrixXd node1BeliefMatrix ( edgePotentials.rows(), edgePotentials.cols() );
for ( size_t row = 0; row < edgePotentials.rows(); row++ )
for ( size_t col = 0; col < edgePotentials.cols(); col++ )
node1BeliefMatrix(row,col) = node1Belief(row);
//cout << "Node 1 belief matrix: " << endl << node1BeliefMatrix << endl;
edgeBelief = edgeBelief.cwiseProduct( node1BeliefMatrix );
MatrixXd node2BeliefMatrix ( edgePotentials.rows(), edgePotentials.cols() );
for ( size_t row = 0; row < edgePotentials.rows(); row++ )
for ( size_t col = 0; col < edgePotentials.cols(); col++ )
node2BeliefMatrix(row,col) = node2Belief(col);
//cout << "Node 2 belief matrix: " << endl << node2BeliefMatrix << endl;
edgeBelief = edgeBelief.cwiseProduct( node2BeliefMatrix );
//cout << "Edge potentials" << endl << edgePotentials << endl;
//cout << "Edge beliefs" << endl << edgeBelief << endl;
// Normalize
edgeBelief = edgeBelief / edgeBelief.sum();
edgeBeliefs[ edgeID ] = edgeBelief;
}
//
// 4. Compute logZ
//
double energyNodes = 0;
double energyEdges = 0;
double entropyNodes = 0;
double entropyEdges = 0;
// Compute energy and entropy from nodes
for ( size_t nodeIndex = 0; nodeIndex < nodes.size(); nodeIndex++ )
{
CNodePtr nodePtr = nodes[ nodeIndex ];
size_t nodeID = nodePtr->getID();
size_t N_Neighbors = graph.getNumberOfNodeNeighbors( nodeID );
// Useful computations and shorcuts
VectorXd &nodeBelief = nodeBeliefs[nodeID];
VectorXd logNodeBelief = nodeBeliefs[nodeID].array().log();
VectorXd nodePotentials = nodePtr->getPotentials( m_options.considerNodeFixedValues );
VectorXd logNodePotentials = nodePotentials.array().log();
// Entropy from the node
energyNodes += N_Neighbors*( nodeBelief.cwiseProduct( logNodeBelief ).sum() );
// Energy from the node
entropyNodes += N_Neighbors*( nodeBelief.cwiseProduct( logNodePotentials ).sum() );
}
// Compute energy and entropy from nodes
for ( size_t edgeIndex = 0; edgeIndex < N_edges; edgeIndex++ )
{
CEdgePtr edgePtr = edges[ edgeIndex ];
size_t edgeID = edgePtr->getID();
// Useful computations and shorcuts
MatrixXd &edgeBelief = edgeBeliefs[ edgeID ];
MatrixXd logEdgeBelief = edgeBelief.array().log();
MatrixXd &edgePotentials = edgePtr->getPotentials();
MatrixXd logEdgePotentials = edgePotentials.array().log();
// Entropy from the edge
energyEdges += edgeBelief.cwiseProduct( logEdgeBelief ).sum();
// Energy from the edge
entropyEdges += edgeBelief.cwiseProduct( logEdgePotentials ).sum();
}
// Final Bethe free energy
double BethefreeEnergy = ( energyNodes - energyEdges ) - ( entropyNodes - entropyEdges );
// Compute logZ
logZ = - BethefreeEnergy;
}
void CTRPBPInference::infer(CGraph &graph,
map<size_t,VectorXd> &nodeBeliefs,
map<size_t,MatrixXd> &edgeBeliefs,
double &logZ)
{
//
// Algorithm workflow:
// 1. Compute the messages passed
// 2. Compute node beliefs
// 3. Compute edge beliefs
// 4. Compute logZ
//
nodeBeliefs.clear();
edgeBeliefs.clear();
const vector<CNodePtr> nodes = graph.getNodes();
const vector<CEdgePtr> edges = graph.getEdges();
multimap<size_t,CEdgePtr> edges_f = graph.getEdgesF();
size_t N_nodes = nodes.size();
size_t N_edges = edges.size();
//
// 1. Create spanning trees
//
bool allNodesAdded = false;
vector<vector<size_t > > v_trees;
vector<bool> v_addedNodes(N_nodes,false);
map<size_t,size_t> addedNodesMap;
for (size_t i = 0; i < N_nodes; i++)
addedNodesMap[ nodes[i]->getID() ] = i;
while (!allNodesAdded)
{
allNodesAdded = true;
vector<size_t> tree;
getSpanningTree( graph, tree );
// Check that the tree is not empty
if ( tree.size() )
v_trees.push_back( tree );
cout << "Tree: ";
for ( size_t i_node = 0; i_node < tree.size(); i_node++ )
{
v_addedNodes[ addedNodesMap[tree[i_node]] ] = true;
cout << tree[i_node] << " ";
}
cout << endl;
for ( size_t i_node = 0; i_node < N_nodes; i_node++ )
if ( !v_addedNodes[i_node] )
{
allNodesAdded = false;
break;
}
}
//
// 1. Compute messages passed in each tree until convergence
//
vector<vector<VectorXd> > messages;
bool maximize = false;
double totalSumOfMsgs = std::numeric_limits<double>::max();
size_t iteration;
for ( iteration = 0; iteration < m_options.maxIterations; iteration++ )
{
for ( size_t i_tree=0; i_tree < v_trees.size(); i_tree++ )
messagesLBP( graph, m_options, messages, maximize, v_trees[i_tree] );
double newTotalSumOfMsgs = 0;
for ( size_t i = 0; i < N_edges; i++ )
{
newTotalSumOfMsgs += messages[i][0].sum() + messages[i][1].sum();
}
if ( std::abs( totalSumOfMsgs - newTotalSumOfMsgs ) <
m_options.convergency )
break;
totalSumOfMsgs = newTotalSumOfMsgs;
}
//
// 2. Compute node beliefs
//
for ( size_t nodeIndex = 0; nodeIndex < N_nodes; nodeIndex++ )
{
const CNodePtr nodePtr = graph.getNode( nodeIndex );
size_t nodeID = nodePtr->getID();
VectorXd nodePotPlusIncMsg = nodePtr->getPotentials( m_options.considerNodeFixedValues );
NEIGHBORS_IT neighbors = edges_f.equal_range(nodeID);
//
// Get the messages for all the neighbors, and multiply them with the node potential
//
for ( multimap<size_t,CEdgePtr>::iterator itNeigbhor = neighbors.first;
itNeigbhor != neighbors.second;
itNeigbhor++ )
{
CEdgePtr edgePtr( (*itNeigbhor).second );
size_t edgeIndex = graph.getEdgeIndex( edgePtr->getID() );
if ( !edgePtr->getNodePosition( nodeID ) ) // nodeID is the first node in the edge
nodePotPlusIncMsg = nodePotPlusIncMsg.cwiseProduct(messages[ edgeIndex ][ 1 ]);
else // nodeID is the second node in the dege
nodePotPlusIncMsg = nodePotPlusIncMsg.cwiseProduct(messages[ edgeIndex ][ 0 ]);
}
// Normalize
nodePotPlusIncMsg = nodePotPlusIncMsg / nodePotPlusIncMsg.sum();
nodeBeliefs[ nodeID ] = nodePotPlusIncMsg;
//cout << "Beliefs of node " << nodeIndex << endl << nodePotPlusIncMsg << endl;
}
//
// 3. Compute edge beliefs
//
for ( size_t edgeIndex = 0; edgeIndex < N_edges; edgeIndex++ )
{
CEdgePtr edgePtr = edges[edgeIndex];
size_t edgeID = edgePtr->getID();
size_t ID1, ID2;
edgePtr->getNodesID( ID1, ID2 );
MatrixXd edgePotentials = edgePtr->getPotentials();
MatrixXd edgeBelief = edgePotentials;
VectorXd &message1To2 = messages[edgeIndex][0];
VectorXd &message2To1 = messages[edgeIndex][1];
//cout << "----------------------" << endl;
//cout << nodeBeliefs[ ID1 ] << endl;
//cout << "----------------------" << endl;
//cout << message2To1 << endl;
VectorXd node1Belief = nodeBeliefs[ ID1 ].cwiseQuotient( message2To1 );
VectorXd node2Belief = nodeBeliefs[ ID2 ].cwiseQuotient( message1To2 );
//cout << "----------------------" << endl;
MatrixXd node1BeliefMatrix ( edgePotentials.rows(), edgePotentials.cols() );
for ( size_t row = 0; row < edgePotentials.rows(); row++ )
for ( size_t col = 0; col < edgePotentials.cols(); col++ )
node1BeliefMatrix(row,col) = node1Belief(row);
//cout << "Node 1 belief matrix: " << endl << node1BeliefMatrix << endl;
edgeBelief = edgeBelief.cwiseProduct( node1BeliefMatrix );
MatrixXd node2BeliefMatrix ( edgePotentials.rows(), edgePotentials.cols() );
for ( size_t row = 0; row < edgePotentials.rows(); row++ )
for ( size_t col = 0; col < edgePotentials.cols(); col++ )
node2BeliefMatrix(row,col) = node2Belief(col);
//cout << "Node 2 belief matrix: " << endl << node2BeliefMatrix << endl;
edgeBelief = edgeBelief.cwiseProduct( node2BeliefMatrix );
//cout << "Edge potentials" << endl << edgePotentials << endl;
//cout << "Edge beliefs" << endl << edgeBelief << endl;
// Normalize
edgeBelief = edgeBelief / edgeBelief.sum();
edgeBeliefs[ edgeID ] = edgeBelief;
}
//
// 4. Compute logZ
//
double energyNodes = 0;
double energyEdges = 0;
double entropyNodes = 0;
double entropyEdges = 0;
// Compute energy and entropy from nodes
for ( size_t nodeIndex = 0; nodeIndex < nodes.size(); nodeIndex++ )
{
CNodePtr nodePtr = nodes[ nodeIndex ];
size_t nodeID = nodePtr->getID();
size_t N_Neighbors = graph.getNumberOfNodeNeighbors( nodeID );
// Useful computations and shorcuts
VectorXd &nodeBelief = nodeBeliefs[nodeID];
VectorXd logNodeBelief = nodeBeliefs[nodeID].array().log();
VectorXd nodePotentials = nodePtr->getPotentials( m_options.considerNodeFixedValues );
VectorXd logNodePotentials = nodePotentials.array().log();
// Entropy from the node
energyNodes += N_Neighbors*( nodeBelief.cwiseProduct( logNodeBelief ).sum() );
// Energy from the node
entropyNodes += N_Neighbors*( nodeBelief.cwiseProduct( logNodePotentials ).sum() );
}
// Compute energy and entropy from nodes
for ( size_t edgeIndex = 0; edgeIndex < N_edges; edgeIndex++ )
{
CEdgePtr edgePtr = edges[ edgeIndex ];
size_t edgeID = edgePtr->getID();
// Useful computations and shorcuts
MatrixXd &edgeBelief = edgeBeliefs[ edgeID ];
MatrixXd logEdgeBelief = edgeBelief.array().log();
MatrixXd &edgePotentials = edgePtr->getPotentials();
MatrixXd logEdgePotentials = edgePotentials.array().log();
// Entropy from the edge
energyEdges += edgeBelief.cwiseProduct( logEdgeBelief ).sum();
// Energy from the edge
entropyEdges += edgeBelief.cwiseProduct( logEdgePotentials ).sum();
}
// Final Bethe free energy
double BethefreeEnergy = ( energyNodes - energyEdges ) - ( entropyNodes - entropyEdges );
// Compute logZ
logZ = - BethefreeEnergy;
}
