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