//number of nodes as would return vector.size()
void SkeletonGraph::CreateGraphFromSkeleton(SkeletonMatchNode* root, int numOfNodes) {
FreeNodes();
nodes.reserve(numOfNodes);
for (int i = 0; i < numOfNodes; i++) {
SkeletonGraphNode* node = new SkeletonGraphNode(i);
nodes.push_back(node);
}
deque<SkeletonMatchNode*> queue;
queue.push_back(root);
int id = 0;
while (!queue.empty()) {
SkeletonMatchNode* sklNode = queue.front();
queue.pop_front();
nodes[id]->point = sklNode->point;
nodes[id]->oldID = sklNode->oldID;
//edges are not oriented and parent would handle parent edge so we add only child edges
for (int i = 0; i < sklNode->nodes.size(); i++) {
GraphEdge ge = GraphEdge(sklNode->betweenNodes[i], sklNode->dists[i]);
ge.oldIDs = sklNode->oldIDs[i];
ge.positions = sklNode->positions[i];
ge.fromId = sklNode->id;
ge.toId = sklNode->nodes[i]->id;
CreateEdge(sklNode->id, sklNode->nodes[i]->id, ge);
queue.push_back(sklNode->nodes[i]);
}
id++;
}
CollectBranchNodes();
}
int main(void)
{
typedef gtl::sparse_graph<GraphNode, GraphEdge> graph_type;
graph_type g;
g.add_node(GraphNode(0));
g.add_node(GraphNode(1));
g.add_node(GraphNode(2));
g.add_node(GraphNode(3));
g.add_node(GraphNode(4));
g.add_node(GraphNode(5));
g.add_edge(GraphEdge(0, 1));
std::cout << g;
gtl::graphsearch_dfs<graph_type> pathFinder(g, 0 , 1);
if (pathFinder.found()) {
std::cout << "found!" << std::endl;
}
else {
std::cout << "not found!" << std::endl;
}
system("pause");
return 0;
}
inline RectangularGraphNode( int32_t x_pos, int32_t y_pos ) {
for (int i=0; i<NumDirections; i++) {
neighbors.push_back(GraphEdge(this,NULL,EDGE_FAR));
}
x = x_pos;
y = y_pos;
}
long Graph::AddEdge(long v1, long v2)
{
size_t name = m_edges.size();
m_edges.push_back(GraphEdge());
m_edges[name].m_name = static_cast<long>(name);
m_edges[name].m_v1 = v1;
m_edges[name].m_v2 = v2;
m_vertices[v1].AddEdge(static_cast<long>(name));
m_vertices[v2].AddEdge(static_cast<long>(name));
m_nE++;
return static_cast<long>(name);
}
bool Bfs::SearchWithTrail(int from, int to, Trail* trail)
{
// Need to remember which nodes we have already visited
std::set<int> visited;
Breadcrumbs breadcrumbs;
// Breadth first uses a queue to hold nodes we will visit
std::queue<GraphEdge> nodesToVisit;
float cost = 0; // dummy value
// Put dummy edge in stack to start search. TODO better way, e.g. do/while ?
nodesToVisit.push(GraphEdge(from, from, cost));
while (!nodesToVisit.empty())
{
GraphEdge e = nodesToVisit.front();
nodesToVisit.pop();
std::cout << "Trying edge from " << e.GetFrom() << " to " << e.GetTo() << "\n";
breadcrumbs[e.GetTo()] = e.GetFrom(); // map version
//breadcrumbs.push_back(e); // vector version
// In addition to here....
visited.insert(e.GetTo());
if (e.GetTo() == to)
{
// We have found the finish!
std::cout << "Successfully got to the finish!\n";
MakeTrail(from, to, breadcrumbs, trail);
return true;
}
else
{
// Push unvisited neighbours onto stack
const EdgeList& edgelist = m_graph->GetEdgeList(e.GetTo());
for (EdgeList::const_iterator it = edgelist.begin(); it != edgelist.end(); ++it)
{
const GraphEdge& childEdge = *it;
if (visited.count(childEdge.GetTo()) == 0)
{
nodesToVisit.push(childEdge);
// ...we mark node as visited right away here
visited.insert(childEdge.GetTo());
}
}
}
}
return false;
}
void CCLayerSorter::createGraphEdges()
{
#if !defined( NDEBUG )
LOG(CCLayerSorter, "Edges:\n");
#endif
// Fraction of the total zRange below which z differences
// are not considered reliable.
const float zThresholdFactor = 0.01;
float zThreshold = m_zRange * zThresholdFactor;
for (unsigned na = 0; na < m_nodes.size(); na++) {
GraphNode& nodeA = m_nodes[na];
if (!nodeA.layer->drawsContent() && !nodeA.layer->renderSurface())
continue;
for (unsigned nb = na + 1; nb < m_nodes.size(); nb++) {
GraphNode& nodeB = m_nodes[nb];
if (!nodeB.layer->drawsContent() && !nodeB.layer->renderSurface())
continue;
float weight = 0;
ABCompareResult overlapResult = checkOverlap(&nodeA.shape, &nodeB.shape, zThreshold, weight);
GraphNode* startNode = 0;
GraphNode* endNode = 0;
if (overlapResult == ABeforeB) {
startNode = &nodeA;
endNode = &nodeB;
} else if (overlapResult == BBeforeA) {
startNode = &nodeB;
endNode = &nodeA;
}
if (startNode) {
#if !defined( NDEBUG )
LOG(CCLayerSorter, "%d -> %d\n", startNode->layer->debugID(), endNode->layer->debugID());
#endif
m_edges.append(GraphEdge(startNode, endNode, weight));
}
}
}
for (unsigned i = 0; i < m_edges.size(); i++) {
GraphEdge& edge = m_edges[i];
m_activeEdges.add(&edge, &edge);
edge.from->outgoing.append(&edge);
edge.to->incoming.append(&edge);
edge.to->incomingEdgeWeight += edge.weight;
}
}
bool Dfs::SearchNoTrail(int from, int to)
{
// Need to remember which nodes we have already visited
std::set<int> visited;
// Depth first uses a stack to hold nodes we will visit
std::stack<GraphEdge> nodesToVisit;
float cost = 0; // dummy value
// Put dummy edge in stack to start search. TODO better way, e.g. do/while ?
nodesToVisit.push(GraphEdge(from, from, cost));
while (!nodesToVisit.empty())
{
GraphEdge e = nodesToVisit.top();
nodesToVisit.pop();
std::cout << "Trying edge from " << e.GetFrom() << " to " << e.GetTo() << "\n";
visited.insert(e.GetTo());
if (e.GetTo() == to)
{
// We have found the finish!
return true;
}
else
{
// Push unvisited neighbours onto stack
const EdgeList& edgelist = m_graph->GetEdgeList(e.GetTo());
for (EdgeList::const_iterator it = edgelist.begin(); it != edgelist.end(); ++it)
{
const GraphEdge& childEdge = *it;
if (visited.count(childEdge.GetTo()) == 0)
{
nodesToVisit.push(childEdge);
}
}
}
}
return false;
}
bool Dfs::SearchWithTrail(int from, int to, Trail* trail)
{
std::set<int> visited;
Breadcrumbs breadcrumbs;
std::stack<GraphEdge> nodesToVisit;
float cost = 0;
// Put dummy edge in stack
nodesToVisit.push(GraphEdge(from, from, cost));
while (!nodesToVisit.empty())
{
GraphEdge e = nodesToVisit.top();
nodesToVisit.pop();
std::cout << "Trying edge from " << e.GetFrom() << " to " << e.GetTo() << "\n";
breadcrumbs[e.GetTo()] = e.GetFrom(); // map version
// breadcrumbs.push_back(e); // vector version
visited.insert(e.GetTo());
if (e.GetTo() == to)
{
// We have found the finish! Loop back through the breadcrumbs to create the trail.
MakeTrail(from, to, breadcrumbs, trail);
return true;
}
else
{
// Push unvisited neighbours onto stack
const EdgeList& edgelist = m_graph->GetEdgeList(e.GetTo());
for (EdgeList::const_iterator it = edgelist.begin(); it != edgelist.end(); ++it)
{
const GraphEdge& childEdge = *it;
if (visited.count(childEdge.GetTo()) == 0)
{
nodesToVisit.push(childEdge);
}
}
}
}
return false;
}
void CurvatureDetector::computeGraph(const LaserReading& reading, std::vector<Point2D>& graphPoints, Graph& graph, std::vector<unsigned int>& maxRangeMapping) const
{
const std::vector<Point2D>& worldPoints = reading.getWorldCartesian();
graphPoints.reserve(worldPoints.size());
std::vector<GraphEdge> edges;
std::vector< boost::property < boost::edge_weight_t, double > > weights;
edges.reserve(worldPoints.size()*worldPoints.size());
weights.reserve(worldPoints.size()*worldPoints.size());
unsigned int currentVertexNumber = 0;
for(unsigned int i = 0; i < worldPoints.size(); i++){
if(m_useMaxRange || reading.getRho()[i] < reading.getMaxRange()){
graphPoints.push_back(worldPoints[i]);
maxRangeMapping.push_back(i);
unsigned int targetVertexNumber = currentVertexNumber + 1;
for(unsigned int j = i + 1; j < worldPoints.size(); j++){
if(m_useMaxRange || reading.getRho()[j] < reading.getMaxRange()){
Point2D difference = worldPoints[i] - worldPoints[j];
double weight = hypot(difference.x, difference.y);
edges.push_back(GraphEdge(currentVertexNumber,targetVertexNumber));
weights.push_back(weight);
targetVertexNumber++;
}
}
currentVertexNumber++;
}
}
MatrixGraph costGraph(currentVertexNumber);
for(unsigned int i = 0; i < edges.size(); i++){
boost::add_edge(edges[i].first, edges[i].second, weights[i], costGraph);
}
boost::remove_edge(0, currentVertexNumber - 1, costGraph);
for(unsigned int iter = 0; iter <= m_dmst; iter++){
std::vector < boost::graph_traits < Graph >::vertex_descriptor > predecessor(boost::num_vertices(costGraph));
boost::prim_minimum_spanning_tree(costGraph, &predecessor[0]);
for(unsigned int i = 1; i < predecessor.size(); i++){
boost::add_edge(predecessor[i], i, boost::get(boost::edge_weight, costGraph, boost::edge(predecessor[i], i, costGraph).first), graph);
boost::remove_edge(predecessor[i], i, costGraph);
}
}
}
void CCLayerSorter::createGraphEdges()
{
#if !defined( NDEBUG )
LOG(CCLayerSorter, "Edges:\n");
#endif
for (unsigned na = 0; na < m_nodes.size(); na++) {
GraphNode& nodeA = m_nodes[na];
if (!nodeA.layer->drawsContent() && !nodeA.layer->renderSurface())
continue;
for (unsigned nb = na + 1; nb < m_nodes.size(); nb++) {
GraphNode& nodeB = m_nodes[nb];
if (!nodeB.layer->drawsContent() && !nodeB.layer->renderSurface())
continue;
ABCompareResult overlapResult = checkOverlap(&nodeA, &nodeB);
GraphNode* startNode = 0;
GraphNode* endNode = 0;
if (overlapResult == ABeforeB) {
startNode = &nodeA;
endNode = &nodeB;
} else if (overlapResult == BBeforeA) {
startNode = &nodeB;
endNode = &nodeA;
}
if (startNode) {
#if !defined( NDEBUG )
LOG(CCLayerSorter, "%d -> %d\n", startNode->layer->debugID(), endNode->layer->debugID());
#endif
m_edges.append(GraphEdge(startNode, endNode));
}
}
}
for (unsigned i = 0; i < m_edges.size(); i++) {
GraphEdge& edge = m_edges[i];
m_activeEdges.add(&edge, &edge);
edge.from->outgoing.append(&edge);
edge.to->incoming.append(&edge);
}
}
