// ======================================== Graph Extensions ========================================
void testGraphExtension(CGraphExt& graphExt, CGraph& graph)
{
const byte nStates = graph.getNumStates();
Size graphSize = Size(random::u<int>(10, 100), random::u<int>(10, 100));
graphExt.buildGraph(graphSize);
ASSERT_EQ(graphSize, graphExt.getSize());
ASSERT_EQ(graphSize.width * graphSize.height, graph.getNumNodes());
graphSize = Size(random::u<int>(10, 100), random::u<int>(10, 100));
Mat pots = random::U(graphSize, CV_32FC(nStates));
graphExt.setGraph(pots);
ASSERT_EQ(graphSize, graphExt.getSize());
Mat test_pots;
graph.getNodes(0, 0, test_pots);
test_pots = test_pots.clone().reshape(graph.getNumStates(), graphSize.height);
ASSERT_EQ(pots.rows, test_pots.rows);
for (int y = 0; y < test_pots.rows; y++) {
float *pPots = pots.ptr<float>(y);
float *pTestPots = test_pots.ptr<float>(y);
for (int x = 0; x < test_pots.cols; x++)
for (int c = 0; c < test_pots.channels(); c++)
ASSERT_EQ(pPots[x * nStates + c], pTestPots[x * nStates + c]);
}
// void addDefaultEdgesModel(float val, float weight = 1.0f);
// void addDefaultEdgesModel(const Mat &featureVectors, float val, float weight);
// void addDefaultEdgesModel(const vec_mat_t &featureVectors, float val, float weight);
}
void UPGMpp::getRandomAssignation(CGraph &graph,
map<size_t,size_t> &assignation,
TInferenceOptions &options )
{
static boost::mt19937 rng1;
static bool firstExecution = true;
if ( firstExecution )
{
rng1.seed(std::time(0));
firstExecution = false;
}
vector<CNodePtr> &nodes = graph.getNodes();
for ( size_t node = 0; node < nodes.size(); node++ )
{
// TODO: Check if a node has a fixed value and consider it
size_t N_classes = nodes[node]->getType()->getNumberOfClasses();
boost::uniform_int<> generator(0,N_classes-1);
int state = generator(rng1);
assignation[nodes[node]->getID()] = state;
}
/*map<size_t,size_t>::iterator it;
for ( it = assignation.begin(); it != assignation.end(); it++ )
cout << "[" << it->first << "] " << it->second << endl;*/
}
void UPGMpp::getMostProbableNodeAssignation( CGraph &graph,
map<size_t,size_t> &assignation,
TInferenceOptions &options)
{
vector<CNodePtr> &nodes = graph.getNodes();
size_t N_nodes = nodes.size();
// Choose as initial class for all the nodes their more probable class
// according to the node potentials
for ( size_t index = 0; index < N_nodes; index++ )
{
size_t nodeMAP;
size_t ID = nodes[index]->getID();
nodes[index]->getPotentials( options.considerNodeFixedValues ).maxCoeff(&nodeMAP);
assignation[ID] = nodeMAP;
}
}
// ======================================== CGraph Building ========================================
void testGraphBuilding(CGraph& graph, byte nStates)
{
ASSERT_EQ(nStates, graph.getNumStates());
int nNodes = random::u<int>(100, 100000);
Mat pots1 = random::U(Size(nStates, nNodes), CV_32FC1, 0.0, 100.0);
ASSERT_EQ(0, graph.addNode());
ASSERT_EQ(1, graph.addNode(pots1.row(1).t()));
graph.addNodes(pots1);
ASSERT_EQ(nNodes + 2, graph.getNumNodes());
graph.setNode(0, pots1.row(0).t());
Mat pot0, pot1, pot2;
graph.getNode(0, pot0);
graph.getNode(1, pot1);
graph.getNodes(0, 10, pot2);
float *pPot0 = pots1.ptr<float>(0);
float *pPot1 = pots1.ptr<float>(1);
for (byte s = 0; s < nStates; s++) {
ASSERT_EQ(pot0.at<float>(s, 0), pPot0[s]);
ASSERT_EQ(pot1.at<float>(s, 0), pPot1[s]);
ASSERT_EQ(pot2.at<float>(0, s), pPot0[s]);
ASSERT_EQ(pot2.at<float>(1, s), pPot1[s]);
}
Mat pots2 = random::U(Size(nStates, static_cast<int>(graph.getNumNodes()) - 10), CV_32FC1, 0.0, 100.0);
graph.setNodes(2, pots2);
Mat pot;
for (size_t n = 2; n < graph.getNumNodes(); n++) {
graph.getNode(n, pot);
float *pPot = (static_cast<int>(n) - 2 < pots2.rows) ? pots2.ptr<float>(static_cast<int>(n) - 2) : pots1.ptr<float>(static_cast<int>(n) - 2);
for (byte s = 0; s < nStates; s++)
ASSERT_EQ(pot.at<float>(s, 0), pPot[s]);
}
graph.reset();
ASSERT_EQ(graph.getNumEdges(), 0);
}
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 );
}
}
}
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)
}
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;
}
