Esempio n. 1
0
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");

}
Esempio n. 2
0
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;
}
Esempio n. 3
0
/**
 * 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*) &timestamp, 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;
}
Esempio n. 4
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;
}