コード例 #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");

}
コード例 #2
0
ファイル: graph_viewer.cpp プロジェクト: huang-qiao/HOG-Man
/**
 * 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;
}