void printDist(minHeap* h) {
int s = h->size; int i = 0;
while(s--) {
i++; if (isPQ(h, i)) { s++; continue; }
if (dist[i] == preDist[i]) {
printf("Q[%d] : d[%c] = %d\n", i-1, printVertex(i), dist[i]);
}
else {
printf("Q[%d] : d[%c] = %d -> d[%c] = %d\n", i-1, printVertex(i), preDist[i], printVertex(i), dist[i]);
}
}
printf("\n\n");
}
void dijkstra(int start,int size) {
minHeap* h = (minHeap*)malloc(sizeof(minHeap));
h->size = 0; h->array[0] = unused;
dist[start] = 0; int count = 0; preDist[start] = 0;
for (int i = 1; i <= size; i++) {
push(h, i);
}
while (!isEmpty(h)) {
int top = pop(h);
printf("------------------------------------------------------\n");
printf("S[%d] : d[%c] = %d\n", count,printVertex(top), dist[top]); count++;
printf("------------------------------------------------------\n");
for (int i = 1; i <= size; i++) {
if (graph[top-1][i-1]!=inf) { //연결
if (dist[i] > dist[top] + graph[top-1][i-1]) {
dist[i] = dist[top] + graph[top-1][i-1];
buildMinHeap(h);
}
}
}
printDist(h);
for(int i=1; i<=size; i++){
preDist[i] = dist[i];
}
}
}
bool G2oSlamInterface::queryState(const std::vector<int>& nodes)
{
//return true;
cout << "BEGIN" << endl;
#if 1
if (nodes.size() == 0) {
for (OptimizableGraph::VertexIDMap::const_iterator it = _optimizer->vertices().begin(); it != _optimizer->vertices().end(); ++it) {
OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
printVertex(v);
}
} else {
for (size_t i = 0; i < nodes.size(); ++i) {
OptimizableGraph::Vertex* v = _optimizer->vertex(nodes[i]);
if (v)
printVertex(v);
}
}
#endif
cout << "END" << endl << flush;
return true;
}
bool HogmanSlamInterface::queryState(const std::vector<int>& nodes)
{
cout << "BEGIN" << endl;
if (_optimizer) { // 2D SLAM
if (nodes.size() == 0) {
for (Graph::VertexIDMap::const_iterator it = _optimizer->vertices().begin(); it != _optimizer->vertices().end(); it++){
PoseGraph2D::Vertex* v = static_cast<PoseGraph2D::Vertex*>(it->second);
printVertex(v);
}
} else {
for (size_t i = 0; i < nodes.size(); ++i) {
PoseGraph2D::Vertex* v = static_cast<PoseGraph2D::Vertex*>(_optimizer->vertex(nodes[i]));
if (v)
printVertex(v);
}
}
}
else if (_optimizer3D) { // 3D SLAM
if (nodes.size() == 0) {
for (Graph::VertexIDMap::const_iterator it = _optimizer3D->vertices().begin(); it != _optimizer3D->vertices().end(); it++){
PoseGraph3D::Vertex* v = static_cast<PoseGraph3D::Vertex*>(it->second);
printVertex(v);
}
} else {
for (size_t i = 0; i < nodes.size(); ++i) {
PoseGraph3D::Vertex* v = static_cast<PoseGraph3D::Vertex*>(_optimizer3D->vertex(nodes[i]));
if (v)
printVertex(v);
}
}
}
cout << "END" << endl << flush;
return true;
}
int main(){
struct vertex* head = NULL;
GraphAddVertex(&head,6);
GraphAddVertex(&head,5);
GraphAddVertex(&head,4);
GraphAddVertex(&head,3);
GraphAddVertex(&head,2);
GraphAddVertex(&head,1);
printVertex(head);
GraphAddEdge(head,1,2,1);
struct edge* ed = head->edges;
printf("%d\n",ed==NULL);
printEdge(head);
return 0;
}
