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 ); } }