3dpcp/.svn/pristine/3b/3b852e8951c4a2896edcec1896c7b3ec64cfe9ed.svn-base
2012-09-16 14:33:11 +02:00

324 lines
9.3 KiB
Text

/*
* elch6D implementation
*
* Copyright (C) Jochen Sprickerhof
*
* Released under the GPL version 3.
*
*/
/** @file graph balancer implementation and utility functions
* @author Jochen Sprickerhof. Institute of Computer Science, University of Osnabrueck, Germany.
*
* This file implements the paper (ecmr2009.pdf):
* Jochen Sprickerhof, Andreas Nuechter, Kai Lingemann, Joachim Hertzberg. An Explicit
* Loop Closing Technique for 6D SLAM, In Proceedings of the 4th European Conference on
* Mobile Robots (ECMR '09), Mlini/Dubrovnic, Croatia, September 2009.
*/
#include <fstream>
using std::ofstream;
#include <string>
using std::string;
#include <list>
using std::list;
#include <algorithm>
using std::swap;
#include <limits> //for old boost and new gcc
using std::numeric_limits;
#include <boost/graph/graphviz.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/graph_traits.hpp>
using boost::graph_traits;
#include "slam6d/globals.icc"
#include "slam6d/elch6D.h"
#ifdef _MSC_VER
#define tie tr1::tie
#endif
/*void printout(graph_t &g, vector<Vertex> &p, vector<int> &d, double *weights)
{
cout << "distances and parents:" << endl;
graph_traits <graph_t>::vertex_iterator vi, vend;
for(tie(vi, vend) = vertices(g); vi != vend; vi++) {
cout << "distance(" << *vi << ") = " << d[*vi] <<
", parent(" << *vi << ") = " << p[*vi] <<
*vi << " " << weights[*vi] << endl;
}
}*/
/**
* sets a filename for graph_weight_out and calls it
* @param g the graph to save
* @param first the first node
* @param last the last node
* @param weights the computed weights
*/
void elch6D::graph_weight_out(graph_t &g, int first, int last, double *weights)
{
string name("graph_weight_" + to_string(num_vertices(g), 3) + ".dot");
graph_weight_out(g, first, last, weights, name);
}
/**
* writes a graphviz file with the graph labled with the computed weights
* @param g the graph to save
* @param first the first node
* @param last the last node
* @param weights the computed weights
* @param out_file the file to write to
*/
void elch6D::graph_weight_out(graph_t &g, int first, int last, double *weights, string &out_file)
{
ofstream dot_file(out_file.c_str());
dot_file << "graph D {" << endl;
graph_traits <graph_t>::vertex_iterator vi, vend;
for(tie(vi, vend) = vertices(g); vi != vend; vi++) {
dot_file << *vi << "[label=\"" << *vi << " (" <<
weights[*vi] << ")\"];" << endl;
}
boost::property_map<graph_t, edge_weight_t>::type weightmap = get(boost::edge_weight, g);
graph_traits <graph_t>::edge_iterator ei, ei_end;
for(tie(ei, ei_end) = edges(g); ei != ei_end; ei++) {
dot_file << source(*ei, g) << " -- " << target(*ei, g) <<
"[label=\"" << get(weightmap, *ei) << "\"];" << endl;
}
dot_file << first << " -- " << last << "[color=\"green\"] }";
dot_file.close();
}
/**
* sets a filename and calls graph_pos_out
* @param g the graph to save
* @param allScans all laser scans
*/
void elch6D::graph_pos_out(graph_t &g, const vector <Scan *> &allScans)
{
string name("graph_pos_" + to_string(num_vertices(g), 3) + ".dot");
graph_pos_out(g, allScans, name);
}
/**
* writes the graph using the computed scan poses
* @param g the graph to save
* @param allScans all laser scans
* @param out_file the file to write to
*/
void elch6D::graph_pos_out(graph_t &g, const vector <Scan *> &allScans, string &out_file)
{
ofstream graph_file(out_file.c_str());
ofstream graph2_file(("2" + out_file).c_str());
graph_traits <graph_t>::edge_iterator ei, ei_end;
for(tie(ei, ei_end) = edges(g); ei != ei_end; ei++) {
if(source(*ei, g) + 1 != target(*ei, g)) {
graph2_file << allScans[source(*ei, g)]->get_rPos()[0] << " " << allScans[source(*ei, g)]->get_rPos()[1] << " " << allScans[source(*ei, g)]->get_rPos()[2] << endl;
graph2_file << allScans[target(*ei, g)]->get_rPos()[0] << " " << allScans[target(*ei, g)]->get_rPos()[1] << " " << allScans[target(*ei, g)]->get_rPos()[2] << endl << endl;
} else {
graph_file << allScans[source(*ei, g)]->get_rPos()[0] << " " << allScans[source(*ei, g)]->get_rPos()[1] << " " << allScans[source(*ei, g)]->get_rPos()[2] << endl;
graph_file << allScans[target(*ei, g)]->get_rPos()[0] << " " << allScans[target(*ei, g)]->get_rPos()[1] << " " << allScans[target(*ei, g)]->get_rPos()[2] << endl << endl;
}
}
graph_file.close();
graph2_file.close();
}
/**
* write graphviz file with real poses
* @param g the graph
* @param allScans all laser scans
* @param out_file the file to write to
*/
void elch6D::dot_pos_out(graph_t &g, const vector <Scan *> &allScans, string &out_file)
{
ofstream dot_file(out_file.c_str());
dot_file << "graph D {" << endl << "size=\"20,20\"" << endl;
int n = num_vertices(g);
for(int i = 0; i < n; i++) {
dot_file << i << "[pos=\"" <<
allScans[i]->get_rPos()[0] << "," <<
allScans[i]->get_rPos()[2] << "\", label=\"\", height=0.1, width=0.1, fixedsize=true];" << endl;
}
graph_traits <graph_t>::edge_iterator ei, ei_end;
for(tie(ei, ei_end) = edges(g); ei != ei_end; ei++) {
if(source(*ei, g) + 1 != target(*ei, g)) {
dot_file << source(*ei, g) << " -- " << target(*ei, g) << "[color=\"green\"];" << endl;
} else {
dot_file << source(*ei, g) << " -- " << target(*ei, g) << ";" << endl;
}
}
dot_file << "}";
dot_file.close();
}
/**
* sets filename and calls graph_out
* @param g the graph
*/
void elch6D::graph_out(graph_t &g)
{
string name("graph_" + to_string(num_vertices(g), 3) + ".dot");
graph_out(g, name);
}
/**
* uses boost write_graphviz to write the graph
* @param g the graph
* @param out_file the file to write to
*/
void elch6D::graph_out(graph_t &g, string &out_file)
{
ofstream dot_file(out_file.c_str());
write_graphviz(dot_file, g);
dot_file.close();
}
/**
* sets filename and calls slim_graph_out
*/
void elch6D::slim_graph_out(graph_t g)
{
string name("slim_graph_" + to_string(num_vertices(g), 3) + ".dot");
slim_graph_out(g, name);
}
/**
* writes slim graph (supressing unimportant nodes)
* @param g the graph
* @param out_file the file to write to
*/
void elch6D::slim_graph_out(graph_t g, string &out_file)
{
bool todo;
graph_traits < graph_t >::vertex_iterator vi, vend;
graph_traits < graph_t >::adjacency_iterator ai;
do {
todo = false;
for(tie(vi, vend) = vertices(g); vi != vend; vi++) {
int me = *vi;
if(degree(me, g) == 2) {
ai = adjacent_vertices(me, g).first;
int one = *ai;
ai++;
int two = *ai;
if(degree(one, g) == 2 && degree(two, g) == 2 && one != me && two != me && one != two) {
clear_vertex(me, g);
add_edge(one, two, g);
remove_vertex(me, g);
todo = true;
break;
}
}
}
} while(todo);
ofstream dot_file(out_file.c_str());
write_graphviz(dot_file, g);
dot_file.close();
}
/**
* graph balancer algorithm computes the weights
* @param g the graph
* @param f index of the first node
* @param l index of the last node
* @param weights array for the weights
*/
void elch6D::graph_balancer(graph_t &g, int f, int l, double *weights)
{
list<int> crossings, branches;
crossings.push_back(f);
crossings.push_back(l);
weights[f] = 0;
weights[l] = 1;
int *p = new int[num_vertices(g)];
int *p_min = new int[num_vertices(g)];
double *d = new double[num_vertices(g)];
double *d_min = new double[num_vertices(g)];
double dist;
bool do_swap = false;
list<int>::iterator si, ei, s_min, e_min;
// process all junctions
while(!crossings.empty()) {
dist = -1;
// find shortest crossing for all vertices on the loop
for(si = crossings.begin(); si != crossings.end(); ) {
dijkstra_shortest_paths(g, *si, boost::predecessor_map(p).distance_map(d));
ei = si;
ei++;
// find shortest crossing for one vertex
for(; ei != crossings.end(); ei++) {
if(*ei != p[*ei] && (dist < 0 || d[*ei] < dist)) {
dist = d[*ei];
s_min = si;
e_min = ei;
do_swap = true;
}
}
if(do_swap) {
swap(p, p_min);
swap(d, d_min);
do_swap = false;
}
// vertex starts a branch
if(dist < 0) {
branches.push_back(*si);
si = crossings.erase(si);
} else {
si++;
}
}
if(dist > -1) {
remove_edge(*e_min, p_min[*e_min], g);
for(int i = p_min[*e_min]; i != *s_min; i = p_min[i]) {
//even right with weights[*s_min] > weights[*e_min]! (math works)
weights[i] = weights[*s_min] + (weights[*e_min] - weights[*s_min]) * d_min[i] / d_min[*e_min];
remove_edge(i, p_min[i], g);
if(degree(i, g) > 0) {
crossings.push_back(i);
}
}
if(degree(*s_min, g) == 0) {
crossings.erase(s_min);
}
if(degree(*e_min, g) == 0) {
crossings.erase(e_min);
}
}
}
delete[] p;
delete[] p_min;
delete[] d;
delete[] d_min;
graph_traits <graph_t>::adjacency_iterator ai, ai_end;
int s;
// error propagation
while(!branches.empty()) {
s = branches.front();
branches.pop_front();
for(tie(ai, ai_end) = adjacent_vertices(s, g); ai != ai_end; ++ai) {
weights[*ai] = weights[s];
if(degree(*ai, g) > 1) {
branches.push_back(*ai);
}
}
clear_vertex(s, g);
}
}