/** 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; }