Esempio n. 1
0
        /** 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 );
            }

        }
Esempio n. 2
0
        /** 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;
        }
Esempio n. 3
0
        /** 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);

        }
Esempio n. 4
0
        /** 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());
        }
Esempio n. 5
0
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;
}
Esempio n. 6
0
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;
}
Esempio n. 7
0
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);
}
Esempio n. 8
0
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);
}
Esempio n. 9
0
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);
}
Esempio n. 10
0
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;
}
Esempio n. 11
0
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;
}
Esempio n. 12
0
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);
}
Esempio n. 13
0
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);
}
Esempio n. 14
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 );
        }
    }
}
Esempio n. 15
0
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;
}
Esempio n. 16
0
        /** 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++;

            }

        }
Esempio n. 17
0
        /** 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 );
            }
        }
Esempio n. 18
0
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;
}
Esempio n. 19
0
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;
}
Esempio n. 20
0
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;

}
Esempio n. 21
0
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 );
            }
        }
    }

}
Esempio n. 22
0
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;

}