/** 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::applyMaskToPotentials(CGraph &graph, map<size_t,vector<size_t> > &mask )
{
vector<CNodePtr> &nodes = graph.getNodes();
for ( size_t node_index = 0; node_index < nodes.size(); node_index++ )
{
CNodePtr nodePtr = nodes[node_index];
size_t nodeID = nodePtr->getID();
if ( mask.count(nodeID) )
{
Eigen::VectorXd nodePot = nodePtr->getPotentials();
Eigen::VectorXd potMask( nodePot.rows() );
potMask.fill(0);
for ( size_t mask_index = 0; mask_index < mask[nodeID].size(); mask_index++ )
potMask(mask[nodeID][mask_index]) = 1;
nodePot = nodePot.cwiseProduct(potMask);
nodePtr->setPotentials( nodePot );
}
}
}
size_t UPGMpp::messagesLBP(CGraph &graph,
TInferenceOptions &options,
vector<vector<VectorXd> > &messages ,
bool maximize,
const vector<size_t> &tree)
{
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();
bool is_tree = (tree.size()>0) ? true : false;
//graph.computePotentials();
//
// Build the messages structure
//
double totalSumOfMsgs = 0;
if ( !messages.size() )
messages.resize( N_edges);
for ( size_t i = 0; i < N_edges; i++ )
{
if ( !messages[i].size() )
{
messages[i].resize(2);
size_t ID1, ID2;
edges[i]->getNodesID(ID1,ID2);
// Messages from first node of the edge to the second one, so the size of
// the message has to be the same as the number of classes of the second node.
double N_classes = graph.getNodeWithID( ID2 )->getPotentials( options.considerNodeFixedValues ).rows();
messages[i][0].resize( N_classes );
messages[i][0].fill(1.0/N_classes);
// Just the opposite as before.
N_classes = graph.getNodeWithID( ID1 )->getPotentials( options.considerNodeFixedValues ).rows();
messages[i][1].resize( N_classes );
messages[i][1].fill(1.0/N_classes);
}
totalSumOfMsgs += messages[i][0].rows() + messages[i][1].rows();
}
// cout << "Initial Messages:" << endl;
// for ( size_t i=0; i < messages.size(); i++)
// for ( size_t j=0; j < messages[i].size(); j++)
// for ( size_t k=0; k < messages[i][j].size(); k++ )
// cout << messages[i][j][k] << " ";
vector<vector<VectorXd> > previousMessages;
if ( options.particularS["order"] == "RBP" )
{
previousMessages = messages;
for ( size_t i = 0; i < previousMessages.size(); i++ )
{
previousMessages[i][0].fill(0);
previousMessages[i][1].fill(0);
}
}
//
// Iterate until convergence or a certain maximum number of iterations is reached
//
size_t iteration;
// cout << endl;
for ( iteration = 0; iteration < options.maxIterations; iteration++ )
{
// cout << "Messages " << iteration << ":" << endl;
// for ( size_t i=0; i < messages.size(); i++)
// for ( size_t j=0; j < messages[i].size(); j++)
// for ( size_t k=0; k < messages[i][j].size(); k++ )
// cout << messages[i][j][k] << " ";
// cout << endl;
// Variables used by Residual Belief Propagation
int edgeWithMaxDiffIndex = -1;
VectorXd associatedMessage;
bool from1to2;
double maxDifference = -1;
//
// Iterate over all the nodes
//
for ( size_t nodeIndex = 0; nodeIndex < N_nodes; nodeIndex++ )
{
const CNodePtr nodePtr = graph.getNode( nodeIndex );
size_t nodeID = nodePtr->getID();
// Check if we are calibrating a tree, and so if the node is not member of the tree,
// so we dont have to update its messages
if ( is_tree && ( std::find(tree.begin(), tree.end(), nodeID ) == tree.end() ) )
continue;
NEIGHBORS_IT neighbors;
neighbors = edges_f.equal_range(nodeID);
//cout << " Sending messages ... " << endl;
//
// Send a message to each neighbor
//
for ( multimap<size_t,CEdgePtr>::iterator itNeigbhor = neighbors.first;
itNeigbhor != neighbors.second;
itNeigbhor++ )
{
// cout << "sending msg to neighbor..." << endl;
VectorXd nodePotPlusIncMsg = nodePtr->getPotentials( options.considerNodeFixedValues );
// cout << "nodePotPlusIncMsg Orig: " << nodePotPlusIncMsg.transpose() << endl;
size_t neighborID;
size_t ID1, ID2;
CEdgePtr edgePtr( (*itNeigbhor).second );
edgePtr->getNodesID(ID1,ID2);
( ID1 == nodeID ) ? neighborID = ID2 : neighborID = ID1;
// cout << "all ready" << endl;
// Check if we are calibrating a tree, and so if the neighbor node
// is not member of the tree, so we dont have to update its messages
if ( is_tree && ( std::find(tree.begin(), tree.end(), neighborID ) == tree.end() ))
continue;
//
// Compute the message from current node as a product of all the
// incoming messages less the one from the current neighbor
// plus the node potential of the current node.
//
for ( multimap<size_t,CEdgePtr>::iterator itNeigbhor2 = neighbors.first;
itNeigbhor2 != neighbors.second;
itNeigbhor2++ )
{
size_t ID11, ID12;
CEdgePtr edgePtr2( (*itNeigbhor2).second );
edgePtr2->getNodesID(ID11,ID12);
size_t edgeIndex = graph.getEdgeIndex( edgePtr2->getID() );
// cout << "Edge index: " << edgeIndex << endl << "node pot" << nodePotPlusIncMsg << endl;
// cout << "Node ID: " << nodeID << " node11 " << ID11 << " node12 " << ID12 << endl;
CNodePtr n1,n2;
edgePtr2->getNodes(n1,n2);
// cout << "Node 1 type: " << n1->getType()->getID() << " label " << n1->getType()->getLabel() << endl;
// cout << "Node 2 type: " << n2->getType()->getID() << " label " << n2->getType()->getLabel() << endl;
// Check if the current neighbor appears in the edge
if ( ( neighborID != ID11 ) && ( neighborID != ID12 ) )
{
if ( nodeID == ID11 )
{
// cout << "nodePotPlusIncMsg Prod: " << messages[ edgeIndex ][ 1 ].transpose() << endl;
// cout << "nodePotPlusIncMsg Bis : " << messages[ edgeIndex ][ 0 ].transpose() << endl;
nodePotPlusIncMsg =
nodePotPlusIncMsg.cwiseProduct(messages[ edgeIndex ][ 1 ]);
// cout << "nodePotPlusIncMsg Prod2: " << nodePotPlusIncMsg.transpose() << endl;
}
else // nodeID == ID2
{
// cout << "nodePotPlusIncMsg Prod: " << messages[ edgeIndex ][ 0 ].transpose() << endl;
// cout << "nodePotPlusIncMsg Bis : " << messages[ edgeIndex ][ 1 ].transpose() << endl;
nodePotPlusIncMsg =
nodePotPlusIncMsg.cwiseProduct(messages[ edgeIndex ][ 0 ]);
// cout << "nodePotPlusIncMsg Prod2: " << nodePotPlusIncMsg.transpose() << endl;
}
}
}
// cout << "Node pot" << endl;
//cout << "Node pot" << nodePotPlusIncMsg << endl;
//
// Take also the potential between the two nodes
//
MatrixXd edgePotentials;
if ( nodeID != ID1 )
edgePotentials = edgePtr->getPotentials();
else
edgePotentials = edgePtr->getPotentials().transpose();
VectorXd newMessage;
size_t edgeIndex = graph.getEdgeIndex( edgePtr->getID() );
// cout << "get new message" << endl;
if ( !maximize )
{
// Multiply both, and update the potential
// cout << "Edge potentials:" << edgePotentials.transpose() << endl;
// cout << "nodePotPlusIncMsg:" << nodePotPlusIncMsg.transpose() << endl;
newMessage = edgePotentials * nodePotPlusIncMsg;
// Normalize new message
if (newMessage.sum())
newMessage = newMessage / newMessage.sum();
//cout << "New message 3:" << newMessage.transpose() << endl;
}
else
{
if ( nodeID == ID1 )
newMessage.resize(messages[ edgeIndex ][0].rows());
else
newMessage.resize(messages[ edgeIndex ][1].rows());
for ( size_t row = 0; row < edgePotentials.rows(); row++ )
{
double maxRowValue = std::numeric_limits<double>::min();
for ( size_t col = 0; col < edgePotentials.cols(); col++ )
{
double value = edgePotentials(row,col)*nodePotPlusIncMsg(col);
if ( value > maxRowValue )
maxRowValue = value;
}
newMessage(row) = maxRowValue;
}
// Normalize new message
if (newMessage.sum())
newMessage = newMessage / newMessage.sum();
//cout << "New message: " << endl << newMessage << endl;
}
//
// Set the message!
//
VectorXd smoothedOldMessage(newMessage.rows());
smoothedOldMessage.setZero();
double smoothing = options.particularD["smoothing"];
if ( smoothing != 0 )
if ( nodeID == ID1 )
newMessage = newMessage + (1-smoothing) * messages[ edgeIndex ][0];
else
newMessage = newMessage + (1-smoothing) * messages[ edgeIndex ][1];
//cout << "New message:" << endl << newMessage << endl << "Smoothed" << endl << smoothedOldMessage << endl;
// If residual belief propagation is activated, just check if the
// newMessage is the one with the higest residual till the
// moment. Otherwise, set the new message as the current one
if ( options.particularS["order"] == "RBP" )
{
if ( nodeID == ID1 )
{
VectorXd differences = messages[edgeIndex][0] - newMessage;
double difference = differences.cwiseAbs().sum();
if ( difference > maxDifference )
{
from1to2 = true;
edgeWithMaxDiffIndex = edgeIndex;
maxDifference = difference;
associatedMessage = newMessage;
}
}
else
{
VectorXd differences = messages[edgeIndex][1] - newMessage;
double difference = differences.cwiseAbs().sum();
if ( difference > maxDifference )
{
from1to2 = false;
edgeWithMaxDiffIndex = edgeIndex;
maxDifference = difference;
associatedMessage = newMessage;
}
}
}
else
{
// cout << newMessage.cols() << " " << newMessage.rows() << endl;
// cout << "edgeIndex" << edgeIndex << endl;
if ( nodeID == ID1 )
{
// cout << messages[ edgeIndex ][0].cols() << " " << messages[ edgeIndex ][0].rows() << endl;
messages[ edgeIndex ][0] = newMessage;
}
else
{
// cout << messages[ edgeIndex ][1].cols() << " " << messages[ edgeIndex ][1].rows() << endl;
messages[ edgeIndex ][1] = newMessage;
}
// cout << "Wop " << endl;
}
}
} // Nodes
if ( options.particularS["order"] == "RBP" && ( edgeWithMaxDiffIndex =! -1 ))
{
if ( from1to2 )
messages[ edgeWithMaxDiffIndex ][0] = associatedMessage;
else
messages[ edgeWithMaxDiffIndex ][1] = associatedMessage;
}
//
// Check convergency!!
//
double newTotalSumOfMsgs = 0;
for ( size_t i = 0; i < N_edges; i++ )
{
newTotalSumOfMsgs += messages[i][0].sum() + messages[i][1].sum();
}
//printf("%4.10f\n",std::abs( totalSumOfMsgs - newTotalSumOfMsgs ));
if ( std::abs( totalSumOfMsgs - newTotalSumOfMsgs ) <
options.convergency )
break;
totalSumOfMsgs = newTotalSumOfMsgs;
// Show messages
/*cout << "Iteration:" << iteration << endl;
for ( size_t i = 0; i < messages.size(); i++ )
{
cout << messages[i][0] << " " << messages[i][1] << endl;
}*/
} // Iterations
return 1;
}
/** 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 );
}
}
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;
}