tf::Transform hogman2TF(const Transformation3 hogman_trans) {
std::clock_t starttime=std::clock();
tf::Transform result;
tf::Vector3 translation;
translation.setX(hogman_trans.translation().x());
translation.setY(hogman_trans.translation().y());
translation.setZ(hogman_trans.translation().z());
tf::Quaternion rotation;
rotation.setX(hogman_trans.rotation().x());
rotation.setY(hogman_trans.rotation().y());
rotation.setZ(hogman_trans.rotation().z());
rotation.setW(hogman_trans.rotation().w());
result.setOrigin(translation);
result.setRotation(rotation);
return result;
ROS_INFO_STREAM_COND_NAMED(( (std::clock()-starttime) / (double)CLOCKS_PER_SEC) > 0.01, "timings", "function runtime: "<< ( std::clock() - starttime ) / (double)CLOCKS_PER_SEC <<"sec");
}
/**
* this thread will read the stuff that arrives via stdin and create a graph out of it
*/
void* readStdinThread(void* arg)
{
bool switchedGraph = false;
PoseGraph3DVis* poseGraph = static_cast<PoseGraph3DVis*>(arg);
string token, line;
stringstream auxStream;
int graphIdx = 0;
double timestamp = 0.0;
// read stdin data
char c = 0;
while (cin.get(c)) { // if cin is not valid, we are done
int nextIdx = (graphIdx + 1) & 1;
PoseGraph3D& nextGraph = graphs[nextIdx].graph;
std::vector<PoseGraph3DVis::HEdgeVector>& nextHirarchy = graphs[nextIdx].hirarchy;
if (c == 'V') {
switchedGraph = false;
// read vertex
int id;
Transformation3 t;
static Matrix6 identity = Matrix6::eye(1.);
cin.read((char*)&id, sizeof(int));
cin.read((char*)&t.translation()[0], sizeof(double));
cin.read((char*)&t.translation()[1], sizeof(double));
cin.read((char*)&t.translation()[2], sizeof(double));
cin.read((char*)&t.rotation().w(), sizeof(double));
cin.read((char*)&t.rotation().x(), sizeof(double));
cin.read((char*)&t.rotation().y(), sizeof(double));
cin.read((char*)&t.rotation().z(), sizeof(double));
PoseGraph3D::Vertex* v = nextGraph.addVertex(id, t, identity);
if (! v) {
cerr << "vertex " << id << " is already in the graph, reassigning "<< endl;
v = nextGraph.vertex(id);
assert(v);
}
v->transformation = t;
v->localTransformation = t;
}
else if (c == 'E') {
switchedGraph = false;
// read edge
int id1, id2;
Transformation3 t;
Matrix6 m = Matrix6::eye(1.);
cin.read((char*)&id1, sizeof(int));
cin.read((char*)&id2, sizeof(int));
cin.read((char*)&t.translation()[0], sizeof(double));
cin.read((char*)&t.translation()[1], sizeof(double));
cin.read((char*)&t.translation()[2], sizeof(double));
cin.read((char*)&t.rotation().w(), sizeof(double));
cin.read((char*)&t.rotation().x(), sizeof(double));
cin.read((char*)&t.rotation().y(), sizeof(double));
cin.read((char*)&t.rotation().z(), sizeof(double));
if (overrideCovariances) {
double dummy; // just read over the information matrix
for (int i=0; i<6; i++)
for (int j=i; j<6; j++)
cin.read((char*)&dummy, sizeof(double));
} else {
for (int i=0; i<6; i++)
for (int j=i; j<6; j++) {
cin.read((char*)&m[i][j], sizeof(double));
if (i != j)
m[j][i] = m[i][j];
}
}
PoseGraph3D::Vertex* v1 = nextGraph.vertex(id1);
PoseGraph3D::Vertex* v2 = nextGraph.vertex(id2);
if (! v1 ) {
cerr << "vertex " << id1 << " is not existing, cannot add edge (" << id1 << "," << id2 << ")" << endl;
continue;
}
if (! v2 ) {
cerr << "vertex " << id2 << " is not existing, cannot add edge (" << id1 << "," << id2 << ")" << endl;
continue;
}
PoseGraph3D::Edge* e = nextGraph.addEdge(v1, v2, t, m);
if (! e){
cerr << "error in adding edge " << id1 << "," << id2 << endl;
}
}
else if (c == 'H') { // read hirarchy
PoseGraph3DVis::HEdge e;
int l;
cin.read((char*) &l, sizeof(int));
cin.read((char*) &e.id1, sizeof(int));
cin.read((char*) &e.id2, sizeof(int));
if (l + 1 > (int)nextHirarchy.size())
nextHirarchy.resize(l+1);
nextHirarchy[l].push_back(e);
}
else if (c == 'T') {
cin.read((char*) ×tamp, sizeof(double));
}
else if (c == 'F') { // finished reading... switch the graph display
switchedGraph = true;
updateDisplayedGraph(poseGraph, graphIdx, nextIdx);
graphIdx = nextIdx;
}
}
// done reading from stdin, check wether there was an END read
if (!switchedGraph) {
int nextIdx = (graphIdx + 1) & 1;
updateDisplayedGraph(poseGraph, graphIdx, nextIdx);
graphIdx = nextIdx;
}
return 0;
}
