/** Function for computing the potentials of the nodes and the edges in * the graph. */ void computePotentials() { // Method steps: // 1. Compute node potentials // 2. Compute edge potentials // // 1. Node potentials // std::vector<CNodePtr>::iterator it; //cout << "NODE POTENTIALS" << endl; for ( it = m_nodes.begin(); it != m_nodes.end(); it++ ) { CNodePtr nodePtr = *it; if ( !nodePtr->finalPotentials() ) { // Get the node type //size_t type = nodePtr->getType()->getID(); // Compute the node potentials according to the node type and its // extracted features Eigen::VectorXd potentials = nodePtr->getType()->computePotentials( nodePtr->getFeatures() ); // Apply the node class multipliers potentials = potentials.cwiseProduct( nodePtr->getClassMultipliers() ); /*Eigen::VectorXd fixed = nodePtr->getFixed(); potentials = potentials.cwiseProduct( fixed );*/ nodePtr->setPotentials( potentials ); } } // // 2. Edge potentials // std::vector<CEdgePtr>::iterator it2; //cout << "EDGE POTENTIALS" << endl; for ( it2 = m_edges.begin(); it2 != m_edges.end(); it2++ ) { CEdgePtr edgePtr = *it2; Eigen::MatrixXd potentials = edgePtr->getType()->computePotentials( edgePtr->getFeatures() ); edgePtr->setPotentials ( potentials ); } }
/** 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; }
/** Set the type of a node. This method is consistent with the internal * vector for storing the different types of the nodes within this graph. * If the function setType of a node is used instead this method, * this vector could became inconsistent. * /param nodePtr node to change. * /param nodeTypePtr node type to set. */ void setNodeType( CNodePtr nodePtr, CNodeTypePtr nodeTypePtr) { size_t previousNodeTypeID = nodePtr->getType()->getID(); nodePtr->setType( nodeTypePtr ); size_t nodeTypeID = nodeTypePtr->getID(); // Check if other nodes have the same nodeType, and delete that type if not. bool nodeTypeStillInUse = false; for ( size_t i = 0; i < m_nodes.size(); i++ ) if ( m_nodes[i]->getType()->getID() == previousNodeTypeID ) { nodeTypeStillInUse = true; break; } if ( !nodeTypeStillInUse ) { std::vector<CNodeTypePtr>::iterator it; for ( it = m_nodeTypes.begin(); it != m_nodeTypes.end(); it++ ) { CNodeTypePtr nodeTypePtrAux = *it; if ( nodeTypePtrAux->getID() == previousNodeTypeID ) { m_nodeTypes.erase( it ); break; } } } // Check if the node type already exists bool nodeTypeAlreadyInserted = false; for ( size_t i = 0; i < m_nodeTypes.size(); i++ ) if ( m_nodeTypes[i]->getID() == nodeTypeID ) { nodeTypeAlreadyInserted = true; break; } if ( !nodeTypeAlreadyInserted ) m_nodeTypes.push_back(nodeTypePtr); }
/** Function for adding a node to the graph. * \param node: Smart pointer to the node to add. */ void addNode( CNodePtr node ) { m_nodes.push_back(node); bool nodeTypeAlreadyInserted = false; for ( size_t i = 0; i < m_nodeTypes.size(); i++ ) if ( m_nodeTypes[i]->getID() == node->getType()->getID() ) { nodeTypeAlreadyInserted = true; break; } if ( !nodeTypeAlreadyInserted ) m_nodeTypes.push_back(node->getType()); }
Action::ResultE ShadingCallbacks::switchRender(CNodePtr &pNode, Action *action) { Action::ResultE returnValue = Action::Continue; DrawActionBase *da = dynamic_cast<DrawActionBase *>(action ); Switch *pSw = dynamic_cast<Switch *>(pNode.getCPtr()); if((pSw->getChoice() >= 0 ) && (pSw->getChoice() < action->getNNodes()) ) { da->useNodeList(); if(da->isVisible(action->getNode(pSw->getChoice()).getCPtr())) { da->addNode(action->getNode(pSw->getChoice())); } } else if(pSw->getChoice() == Switch::ALL) { if(da->selectVisibles() == 0) returnValue = Action::Skip; } else { returnValue = Action::Skip; } return returnValue; }
Action::ResultE ShadingCallbacks::matDrawableRenderEnter(CNodePtr &pNode, Action *action) { ShadingAction *a = dynamic_cast<ShadingAction *>(action ); MaterialDrawable *pMD = dynamic_cast<MaterialDrawable *>(pNode.getCPtr()); Material::DrawFunctor func; func = osgTypedMethodFunctor1ObjPtr(pMD, &MaterialDrawable::drawPrimitives); Material *m = a->getMaterial(); if(m == NULL) { if(pMD->getMaterial() != NullFC) { m = pMD->getMaterial().getCPtr(); } else { fprintf(stderr, "MaterialDrawable::render: no Material!?!\n"); return Action::Continue; } } a->dropFunctor(func, m); return Action::Continue; }
Action::ResultE ShadingCallbacks::pointlightRenderLeave(CNodePtr &pNode, Action *action) { PointLight *pPl = dynamic_cast<PointLight *>(pNode.getCPtr()); if(pPl->getOn() == false) return Action::Continue; return lightRenderLeave(pPl, action); }
Action::ResultE ShadingCallbacks::spotlightRenderLeave(CNodePtr &pNode, Action *action) { SpotLight *pSp = dynamic_cast<SpotLight *>(pNode.getCPtr()); if(pSp->getOn() == false) return Action::Continue; return lightRenderLeave(pSp, action); }
Action::ResultE ShadingCallbacks::dirlightRenderLeave(CNodePtr &pNode, Action *action) { DirectionalLight *pDl = dynamic_cast<DirectionalLight *>(pNode.getCPtr()); if(pDl->getOn() == false) return Action::Continue; return lightRenderLeave(pDl, action); }
Action::ResultE ShadingCallbacks::transformRenderEnter(CNodePtr &pNode, Action *action) { ShadingAction *pAct = dynamic_cast<ShadingAction *>(action ); Transform *pTr = dynamic_cast<Transform *>(pNode.getCPtr()); pAct->push_matrix(pTr->getMatrix()); pAct->selectVisibles(); return Action::Continue; }
Action::ResultE ShadingCallbacks::matGroupRenderEnter(CNodePtr &pNode, Action *action) { ShadingAction *da = dynamic_cast<ShadingAction *>(action ); MaterialGroup *pMG = dynamic_cast<MaterialGroup *>(pNode.getCPtr()); if(da != NULL && pMG->getSFMaterial()->getValue() != NullFC) { da->setMaterial(&(*(pMG->getSFMaterial()->getValue()))); } return Action::Continue; }
Action::ResultE ShadingCallbacks::spotlightRenderEnter(CNodePtr &pNode, Action *action) { SpotLight *pSp = dynamic_cast<SpotLight *>(pNode.getCPtr()); if(pSp->getOn() == false) return Action::Continue; DrawActionBase *da = dynamic_cast<DrawActionBase *>(action); da->getStatistics()->getElem(SpotLight::statNSpotLights)->inc(); return pointlightRenderEnter(pNode, action); }
Action::ResultE ShadingCallbacks::dirlightRenderEnter(CNodePtr &pNode, Action *action) { DirectionalLight *pDl = dynamic_cast<DirectionalLight *>(pNode.getCPtr()); if(pDl->getOn() == false) return Action::Continue; DrawActionBase *da = dynamic_cast<DrawActionBase *>(action); da->getStatistics()->getElem( DirectionalLight::statNDirectionalLights)->inc(); return lightRenderEnter(pDl, action); }
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 ); } } }
OSG_USING_NAMESPACE Action::ResultE ShadingCallbacks::billboardRenderEnter(CNodePtr &pNode, Action *action) { ShadingAction *pAct = dynamic_cast<ShadingAction *>(action ); Billboard *pBB = dynamic_cast<Billboard *>(pNode.getCPtr()); Matrix mMat; // cerr << "BB::render" << std::endl; pBB->calcMatrix(pAct, pAct->top_matrix(), mMat); pAct->push_matrix(mMat); // !!! can't use visibles, as ToWorld gives garbage leading to wrong culling // pAct->selectVisibles(); return Action::Continue; }
/** Delete a node from the graph. It could also produce the deletion of * its associated edges. * \param ID: ID of the node to delete from the graph. */ void deleteNode( size_t ID ) { // Remove from the vector of nodes std::vector<CNodePtr>::iterator it; for ( it = m_nodes.begin(); it != m_nodes.end(); it++ ) { CNodePtr nodePtr = *it; if ( nodePtr->getID() == ID ) break; } size_t nodeTypeID = (*it)->getType()->getID(); // Remove! if ( it != m_nodes.end() ) m_nodes.erase( it ); // Check if other nodes have the same nodeType, and delete that type if not. bool nodeTypeStillInUse = false; for ( size_t i = 0; i < m_nodes.size(); i++ ) if ( m_nodes[i]->getType()->getID() == nodeTypeID ) { nodeTypeStillInUse = true; break; } if ( !nodeTypeStillInUse ) { std::vector<CNodeTypePtr>::iterator it; for ( it = m_nodeTypes.begin(); it != m_nodeTypes.end(); it++ ) { CNodeTypePtr nodeTypePtr = *it; if ( nodeTypePtr->getID() == nodeTypeID ) { m_nodeTypes.erase( it ); break; } } } // Remove edges than contain that node from m_edges and m_edges_f for ( size_t i = 0; i < m_edges.size(); ) { CEdgePtr edgePtr = m_edges[i]; size_t ID1, ID2; edgePtr->getNodesID( ID1, ID2 ); if ( ( ID1 == ID ) || ( ID2 == ID ) ) deleteEdge( edgePtr->getID() ); else i++; } }
/** 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 ); } }
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; }
Action::ResultE ShadingCallbacks::distanceLODRender(CNodePtr &pNode, Action *action) { DrawActionBase *da = dynamic_cast<DrawActionBase *>(action); ShadingAction *ra = dynamic_cast<ShadingAction *>(action); DistanceLOD *pDLOD = dynamic_cast<DistanceLOD *>(pNode.getCPtr()); UInt32 numLevels = action->getNNodes (); UInt32 numRanges = pDLOD ->getMFRange()->size(); UInt32 limit = osgMin(numLevels, numRanges); Int32 index = -1; Pnt3f eyepos(0.f, 0.f, 0.f); Pnt3f objpos; da->getCameraToWorld().mult(eyepos); if(ra != NULL) { ra->top_matrix() .mult(pDLOD->getCenter(), objpos); } else { da->getActNode()->getToWorld().mult(pDLOD->getCenter(), objpos); } Real32 dist = osgsqrt((eyepos[0] - objpos[0])*(eyepos[0] - objpos[0]) + (eyepos[1] - objpos[1])*(eyepos[1] - objpos[1]) + (eyepos[2] - objpos[2])*(eyepos[2] - objpos[2])); da->useNodeList(); if(numRanges != 0 && numLevels!=0 ) { if(dist < (*(pDLOD->getMFRange()))[0]) { index = 0; } else if(dist >= (*(pDLOD->getMFRange()))[numRanges-1]) { index = (numLevels > numRanges) ? numRanges : (limit-1); } else { UInt32 i = 1; while( (i < numRanges) && !( ((*(pDLOD->getMFRange()))[i-1] <= dist) && (dist < (*(pDLOD->getMFRange()))[i] ) ) ) { i++; } index = osgMin(i, limit-1); } if(da->isVisible(action->getNode(index).getCPtr())) { da->addNode(action->getNode(index)); } } return Action::Continue; }
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 fillGraph( CGraph &graph, const Eigen::MatrixXd &graph_nodeFeatures, CNodeTypePtr nodeType, const Eigen::MatrixXi &graph_adj, CEdgeTypePtr edgeType, vector<Eigen::MatrixXi> &g1_relations, const Eigen::VectorXi &g1_groundTruth, std::map<size_t,size_t> &groundTruth // returned GT taking into account the node IDs ) { // 1. Add nodes to the graph // 2. Add edges size_t n_nodes = graph_nodeFeatures.rows(); size_t n_features = graph_nodeFeatures.cols(); vector<CNodePtr> nodes; // // ADD NODES // for ( size_t i = 0; i < n_nodes; i++ ) { Eigen::VectorXd node_feat(n_features); node_feat = graph_nodeFeatures.row(i); Eigen::VectorXd multipliers(5); multipliers << 90, 100, 50, 40, 50; //multipliers << 50, 60, 50, 50, 50; //node_feat = node_feat.cwiseProduct( multipliers ); CNodePtr nodePtr ( new CNode( nodeType, node_feat ) ); nodes.push_back( nodePtr ); graph.addNode( nodePtr ); groundTruth[ nodePtr->getID() ] = g1_groundTruth(i); } // // ADD EDGES // for ( size_t row = 0; row < n_nodes; row++ ) { for ( size_t col = row; col < n_nodes; col++ ) { if ( graph_adj(row,col) == 1 ) { // Retrieve the nodes linked by the edge and their features CNodePtr node1 = nodes.at(row); CNodePtr node2 = nodes.at(col); Eigen::VectorXd &feat1 = node1->getFeatures(); Eigen::VectorXd &feat2 = node2->getFeatures(); // Compute edge features Eigen::VectorXd edgeFeatures( edgeType->getWeights().size()); // Perpendicularity edgeFeatures(0) = (std::abs(feat1(0) - feat2(0))==0) ? 0 : 1; // Height distance between centers edgeFeatures(1) = std::abs((float)feat1(1) - (float)feat2(1)); // Ratio between areas float a1 = feat1(2); float a2 = feat2(2); if ( a1 < a2 ) { float aux = a2; a2 = a1; a1 = aux; } //edgeFeatures(2) = a1 / a2; // Difference between elongations //edgeFeatures(3) = std::abs(feat1(3) - feat2(3)); // Use the isOn semantic relation //edgeFeatures(4) = (g1_relations[0])(row,col); edgeFeatures(2) = (g1_relations[0])(row,col); // Coplanar semantic relation //edgeFeatures(5) = (g1_relations[1])(row,col); // Bias feature //edgeFeatures(6) = 1; edgeFeatures(3) = 1; //Eigen::VectorXd multipliers(7); //multipliers << 0, 0, 0, 0, 0, 0, 1; //multipliers << 0, 0, 0, 0, 0, 0, 0; Eigen::VectorXd multipliers(4); multipliers << 1, 1, 10, 1; //edgeFeatures = edgeFeatures.cwiseProduct( multipliers ); CEdgePtr edgePtr ( new CEdge( node1, node2, edgeType, edgeFeatures ) ); graph.addEdge( edgePtr ); } } } }
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; }