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");
}
void alignMesh( Polyhedron & poly )
{
int num = poly.size_of_vertices();
// initialization here is very important!!
Point3 ave( 0.0, 0.0, 0.0 );
for ( Vertex_iterator vi = poly.vertices_begin(); vi != poly.vertices_end(); ++vi ) {
ave = ave + ( vi->point() - CGAL::ORIGIN );
}
ave = CGAL::ORIGIN + ( ave - CGAL::ORIGIN )/( double )num;
unsigned int dim = 3;
double * data = new double [ num*dim ];
int nPoints = 0;
for ( Vertex_iterator vi = poly.vertices_begin(); vi != poly.vertices_end(); ++vi ) {
data[ nPoints * dim + 0 ] = vi->point().x() - ave.x();
data[ nPoints * dim + 1 ] = vi->point().y() - ave.y();
data[ nPoints * dim + 2 ] = vi->point().z() - ave.z();
nPoints++;
}
assert( nPoints == ( int )num );
/*****************************************
analyze the engenstructure of X^T X
*****************************************/
/* define the matrix X */
gsl_matrix_view X = gsl_matrix_view_array( data, num, dim );
/* memory reallocation */
// proj = ( double * )realloc( proj, sizeof( double ) * PDIM * num );
/* calculate the covariance matrix B */
gsl_matrix * B = gsl_matrix_alloc( dim, dim );
gsl_blas_dgemm( CblasTrans, CblasNoTrans, 1.0,
&(X.matrix),
&(X.matrix), 0.0,
B );
/* divided by the number of samples */
gsl_matrix_scale( B, 1.0/(double)num );
gsl_vector * eVal = gsl_vector_alloc( dim );
gsl_matrix * eVec = gsl_matrix_alloc( dim, dim );
gsl_eigen_symmv_workspace * w
= gsl_eigen_symmv_alloc( dim );
// eigenanalysis of the matrix B
gsl_eigen_symmv( B, eVal, eVec, w );
// release the memory of w
gsl_eigen_symmv_free( w );
// sort eigenvalues in a descending order
gsl_eigen_symmv_sort( eVal, eVec, GSL_EIGEN_SORT_VAL_DESC );
// #ifdef MYDEBUG
for ( unsigned int i = 0; i < dim; ++i ) {
cerr << "Eigenvalue No. " << i << " = " << gsl_vector_get( eVal, i ) << endl;
cerr << "Eigenvector No. " << i << endl;
double length = 0.0;
for ( unsigned int j = 0; j < dim; ++j ) {
cerr << gsl_matrix_get( eVec, i, j ) << " ";
length += gsl_matrix_get( eVec, i, j )*gsl_matrix_get( eVec, i, j );
}
cerr << " length = " << length << endl;
}
// #endif // MYDEBUG
// 0 1 2, 0 2 1,
Transformation3 map;
map =
Transformation3( gsl_matrix_get(eVec,1,0), gsl_matrix_get(eVec,0,0), gsl_matrix_get(eVec,2,0),
gsl_matrix_get(eVec,1,1), gsl_matrix_get(eVec,0,1), gsl_matrix_get(eVec,2,1),
gsl_matrix_get(eVec,1,2), gsl_matrix_get(eVec,0,2), gsl_matrix_get(eVec,2,2) );
if ( map.is_odd() ) {
cerr << " Transformation matrix reflected" << endl;
map =
Transformation3( gsl_matrix_get(eVec,1,0), gsl_matrix_get(eVec,0,0), -gsl_matrix_get(eVec,2,0),
gsl_matrix_get(eVec,1,1), gsl_matrix_get(eVec,0,1), -gsl_matrix_get(eVec,2,1),
gsl_matrix_get(eVec,1,2), gsl_matrix_get(eVec,0,2), -gsl_matrix_get(eVec,2,2) );
}
for ( unsigned int i = 0; i < dim; ++i ) {
cerr << "| ";
for ( unsigned int j = 0; j < dim; ++j ) {
cerr << map.cartesian( i, j ) << " ";
}
cerr << "|" << endl;
}
transformMesh( poly, map );
return;
}
/**
* 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;
}
PoseMap hogman_solver(KinematicGraphType &graph, PoseIndex &poseIndex,
PoseMap &observed, PoseMap &predictedEmpty, double sigma_position, double sigma_orientation,
double timestamp) {
bool visualize = false;
bool verbose = false;
bool guess = 0;
int iterations = 10;
GraphOptimizer3D* optimizer = new CholOptimizer3D();
optimizer->verbose() = verbose;
optimizer->visualizeToStdout() = visualize;
optimizer->guessOnEdges() = guess;
optimizer->clear();
// reference
tf::Transform root = poseToTransform(observed.begin()->second);
int n=1000;
// add predicted markers
for (PoseMap::iterator it = predictedEmpty.begin(); it != predictedEmpty.end(); it++) {
tf::Transform m = poseToTransform(it->second);
add_vertex(optimizer, it->first, root.inverseTimes(m));
}
// add observed markers
for (PoseMap::iterator it = observed.begin(); it != observed.end(); it++) {
tf::Transform m = poseToTransform(it->second);
add_vertex(optimizer, it->first + n, root.inverseTimes(m));
}
for (KinematicGraphType::iterator it = graph.begin(); it != graph.end(); it++) {
int idx = poseIndex[it->first.first][it->first.second][timestamp];
Pose& pose_pred = it->second->model.track.pose_projected[idx];
ROS_ASSERT(!(isnan(pose_pred.position.x) || isnan(pose_pred.position.y) || isnan(pose_pred.position.z) ||
isnan(pose_pred.orientation.x) || isnan(pose_pred.orientation.y) || isnan(pose_pred.orientation.z)|| isnan(pose_pred.orientation.w)));
add_edge(optimizer, it->first.first, it->first.second, poseToTransform(
pose_pred), sigma_position, sigma_orientation);
}
// add strong edges between observed markers (actually, the observed markers should be kept fixed)
for (PoseMap::iterator it = observed.begin(); it != observed.end(); it++) {
if (it == observed.begin())
it++;// skip first
// relative transformation between marker 0 and marker it
tf::Transform m = poseToTransform(observed.begin()->second).inverseTimes(
poseToTransform(it->second));
add_edge(optimizer, observed.begin()->first + n,
it->first + n, m, sigma_position / 100,
sigma_orientation / 100);
}
// add weak edges between predicted markers and observed markers (keep them in place)
for (PoseMap::iterator it = observed.begin(); it != observed.end(); it++) {
// relative transformation between marker 0 and marker it
tf::Transform m = tf::Transform::getIdentity();
add_edge(optimizer, it->first, it->first + n, m,
sigma_position * 100, sigma_orientation * 100);
}
optimizer->initialize(0);
optimizer->optimize(iterations, false);
PoseMap predictedMarkers;
GraphOptimizer3D::VertexIDMap &vertices = optimizer->vertices();
for (PoseMap::iterator it = predictedEmpty.begin(); it
!= predictedEmpty.end(); it++) {
const PoseGraph3D::Vertex* v;
v= reinterpret_cast<const PoseGraph3D::Vertex*>(vertices[it->first]);
if (v == 0) {
cerr << "VERTEX not found!!" << endl;
}
Transformation3 t = v->transformation;
Vector6 p = t.toVector();
tf::Transform d = tf::Transform(RPY_to_MAT(p[3], p[4], p[5]), tf::Vector3(
p[0], p[1], p[2]));
predictedMarkers[it->first] = transformToPose(root * d);
}
const PoseGraph3D::Vertex* v = reinterpret_cast<const PoseGraph3D::Vertex*>(vertices[observed.begin()->first + n]);
Transformation3 t = v->transformation;
Vector6 p = t.toVector();
tf::Transform d = tf::Transform(RPY_to_MAT(p[3], p[4], p[5]), tf::Vector3(p[0],
p[1], p[2]));
for (PoseMap::iterator it = predictedMarkers.begin(); it
!= predictedMarkers.end(); it++) {
// now correct
it->second = transformToPose(root * d.inverseTimes(root.inverseTimes(
poseToTransform(it->second))));
}
delete optimizer;
return predictedMarkers;
}