Ejemplo n.º 1
0
void JacobianWorkspace::updateSize(const OptimizableGraph& graph)
{
  for (OptimizableGraph::EdgeSet::iterator it = graph.edges().begin(); it != graph.edges().end(); ++it) {
    const OptimizableGraph::Edge* e = static_cast<const OptimizableGraph::Edge*>(*it);
    updateSize(e);
  }
}
Ejemplo n.º 2
0
bool saveGnuplot(const std::string& gnudump, const OptimizableGraph& optimizer)
{
  HyperGraph::VertexSet vset;
  for (HyperGraph::VertexIDMap::const_iterator it=optimizer.vertices().begin(); it!=optimizer.vertices().end(); it++){
    vset.insert(it->second);
  }
  return saveGnuplot(gnudump, vset, optimizer.edges());
}
Ejemplo n.º 3
0
bool dumpEdges(std::ostream& os, const OptimizableGraph& optimizer)
{
  // seek for an action whose name is writeGnuplot in the library
  HyperGraphElementAction* saveGnuplot = HyperGraphActionLibrary::instance()->actionByName("writeGnuplot");
  if (! saveGnuplot ){
    cerr << __PRETTY_FUNCTION__ << ": no action \"writeGnuplot\" registered" << endl;
    return false;
  }
  WriteGnuplotAction::Parameters params;
  params.os = &os;

  // writing all edges
  os << "set terminal x11 noraise" << endl;
  os << "set size ratio -1" << endl;
  os << "plot \"-\" w l" << endl;
  for (HyperGraph::EdgeSet::const_iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
    OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
    (*saveGnuplot)(e, &params);
  }
  os << "e" << endl;

  return true;
}
int main(int argc, char** argv) {
  CommandArgs arg;


  std::string outputFilename;
  std::string inputFilename;

  arg.param("o", outputFilename, "", "output file name"); 
  arg.paramLeftOver("input-filename ", inputFilename, "", "graph file to read", true);
  arg.parseArgs(argc, argv);
  OptimizableGraph graph;
  if (!graph.load(inputFilename.c_str())){
    cerr << "Error: cannot load a file from \"" << inputFilename << "\", aborting." << endl;
    return 0;
  }
  HyperGraph::EdgeSet removedEdges;
  HyperGraph::VertexSet removedVertices;
  for (HyperGraph::EdgeSet::iterator it = graph.edges().begin(); it!=graph.edges().end(); it++) {
    HyperGraph::Edge* e = *it;
    EdgeSE2PointXY* edgePointXY = dynamic_cast<EdgeSE2PointXY*>(e);
    if (edgePointXY) {
      VertexSE2* pose    = dynamic_cast<VertexSE2*>(edgePointXY->vertex(0));
      VertexPointXY* landmark = dynamic_cast<VertexPointXY*>(edgePointXY->vertex(1));
      FeaturePointXYData * feature = new FeaturePointXYData();
      feature->setPositionMeasurement(edgePointXY->measurement());
      feature->setPositionInformation(edgePointXY->information());
      pose->addUserData(feature); 
      removedEdges.insert(edgePointXY);
      removedVertices.insert(landmark);
    }
  }
  
  for (HyperGraph::EdgeSet::iterator it = removedEdges.begin(); it!=removedEdges.end(); it++){
    OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(*it);
    graph.removeEdge(e);
  }

  for (HyperGraph::VertexSet::iterator it = removedVertices.begin(); it!=removedVertices.end(); it++){
    OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*>(*it);
    graph.removeVertex(v);
  }

  
  if (outputFilename.length()){
    graph.save(outputFilename.c_str());
  }
 
}
Ejemplo n.º 5
0
int main(int argc, char** argv)
{
  string inputFilename;
  string outputFilename;
  // command line parsing
  CommandArgs commandLineArguments;
  commandLineArguments.paramLeftOver("gm2dl-input", inputFilename, "", "gm2dl file which will be processed");
  commandLineArguments.paramLeftOver("gm2dl-output", outputFilename, "", "name of the output file");

  commandLineArguments.parseArgs(argc, argv);

  OptimizableGraph inputGraph;
  bool loadStatus = inputGraph.load(inputFilename.c_str());
  if (! loadStatus) {
    cerr << "Error while loading input data" << endl;
    return 1;
  }

  OptimizableGraph outputGraph;

  // process all the vertices first
  double fx = -1;
  double baseline = -1;
  bool firstCam = true;
  for (OptimizableGraph::VertexIDMap::const_iterator it = inputGraph.vertices().begin(); it != inputGraph.vertices().end(); ++it) {
    if (dynamic_cast<VertexCam*>(it->second)) {
      VertexCam* v = static_cast<VertexCam*>(it->second);
      if (firstCam) {
        firstCam = false;
        g2o::ParameterCamera* camParams = new g2o::ParameterCamera;
        camParams->setId(0);
        const SBACam& c = v->estimate();
        baseline = c.baseline;
        fx = c.Kcam(0,0);
        camParams->setKcam(c.Kcam(0,0), c.Kcam(1,1), c.Kcam(0,2), c.Kcam(1,2));
        outputGraph.addParameter(camParams);
      }

      VertexSE3* ov = new VertexSE3;
      ov->setId(v->id());
      Eigen::Isometry3d p;
      p = v->estimate().rotation();
      p.translation() = v->estimate().translation();
      ov->setEstimate(p);
      if (! outputGraph.addVertex(ov)) {
        assert(0 && "Failure adding camera vertex");
      }
    }
    else if (dynamic_cast<VertexSBAPointXYZ*>(it->second)) {
      VertexSBAPointXYZ* v = static_cast<VertexSBAPointXYZ*>(it->second);

      VertexPointXYZ* ov = new VertexPointXYZ;
      ov->setId(v->id());
      ov->setEstimate(v->estimate());
      if (! outputGraph.addVertex(ov)) {
        assert(0 && "Failure adding camera vertex");
      }
    }
  }
  
  for (OptimizableGraph::EdgeSet::const_iterator it = inputGraph.edges().begin(); it != inputGraph.edges().end(); ++it) {
    if (dynamic_cast<EdgeProjectP2SC*>(*it)) {
      EdgeProjectP2SC* e = static_cast<EdgeProjectP2SC*>(*it);

      EdgeSE3PointXYZDisparity* oe = new EdgeSE3PointXYZDisparity;
      oe->vertices()[0] = outputGraph.vertex(e->vertices()[1]->id());
      oe->vertices()[1] = outputGraph.vertex(e->vertices()[0]->id());

      double kx = e->measurement().x();
      double ky = e->measurement().y();
      double disparity = kx - e->measurement()(2);

      oe->setMeasurement(Eigen::Vector3d(kx, ky, disparity / (fx * baseline)));
      oe->setInformation(e->information()); // TODO convert information matrix
      oe->setParameterId(0,0);
      if (! outputGraph.addEdge(oe)) {
        assert(0 && "error adding edge");
      }
    }
  }

  cout << "Vertices in/out:\t" << inputGraph.vertices().size() << " " << outputGraph.vertices().size() << endl;
  cout << "Edges in/out:\t" << inputGraph.edges().size() << " " << outputGraph.edges().size() << endl;

  cout << "Writing output ... " << flush;
  outputGraph.save(outputFilename.c_str());
  cout << "done." << endl;
  return 0;
}
Ejemplo n.º 6
0
int main(int argc, char** argv) {
  CommandArgs arg;
  int nlandmarks;
  int nSegments;
  int simSteps;
  double worldSize;
  bool hasOdom;
  bool hasPoseSensor;
  bool hasPointSensor;
  bool hasPointBearingSensor;
  bool hasSegmentSensor;
  bool hasCompass;
  bool hasGPS;

  int segmentGridSize;
  double minSegmentLenght, maxSegmentLenght;

  std::string outputFilename;
  arg.param("simSteps", simSteps, 100, "number of simulation steps");
  arg.param("nLandmarks", nlandmarks, 1000, "number of landmarks");
  arg.param("nSegments", nSegments, 1000, "number of segments");
  arg.param("segmentGridSize", segmentGridSize, 50, "number of cells of the grid where to align the segments");
  arg.param("minSegmentLenght", minSegmentLenght, 0.5, "minimal lenght of a segment in the world");
  arg.param("maxSegmentLenght", maxSegmentLenght, 3,  "maximal lenght of a segment in the world");
  arg.param("worldSize", worldSize, 25.0, "size of the world");
  arg.param("hasOdom",        hasOdom, false,  "the robot has an odometry" );
  arg.param("hasPointSensor", hasPointSensor, false, "the robot has a point sensor" );
  arg.param("hasPointBearingSensor", hasPointBearingSensor, false, "the robot has a point bearing sensor" );
  arg.param("hasPoseSensor",  hasPoseSensor, false,  "the robot has a pose sensor" );
  arg.param("hasCompass",     hasCompass, false, "the robot has a compass");
  arg.param("hasGPS",         hasGPS, false, "the robot has a GPS");
  arg.param("hasSegmentSensor", hasSegmentSensor, false, "the robot has a segment sensor" );
  arg.paramLeftOver("graph-output", outputFilename, "simulator_out.g2o", "graph file which will be written", true);


  arg.parseArgs(argc, argv);

  std::tr1::ranlux_base_01 generator;
  OptimizableGraph graph;
  World world(&graph);
  for (int i=0; i<nlandmarks; i++){
    WorldObjectPointXY * landmark = new WorldObjectPointXY;
    double x = sampleUniform(-.5,.5, &generator)*worldSize;
    double y = sampleUniform(-.5,.5, &generator)*worldSize;
    landmark->vertex()->setEstimate(Vector2d(x,y));
    world.addWorldObject(landmark);
  }

  cerr << "nSegments = " << nSegments << endl;

  for (int i=0; i<nSegments; i++){
    WorldObjectSegment2D * segment = new WorldObjectSegment2D;
    int ix = sampleUniform(-segmentGridSize,segmentGridSize, &generator);
    int iy = sampleUniform(-segmentGridSize,segmentGridSize, &generator);
    int ith = sampleUniform(0,3, &generator);
    double th= (M_PI/2)*ith;
    th=atan2(sin(th),cos(th));
    double xc = ix*(worldSize/segmentGridSize);
    double yc = iy*(worldSize/segmentGridSize);

    double l2 = sampleUniform(minSegmentLenght, maxSegmentLenght, &generator);

    double x1 = xc + cos(th)*l2;
    double y1 = yc + sin(th)*l2;
    double x2 = xc - cos(th)*l2;
    double y2 = yc - sin(th)*l2;


    segment->vertex()->setEstimateP1(Vector2d(x1,y1));
    segment->vertex()->setEstimateP2(Vector2d(x2,y2));
    world.addWorldObject(segment);
  }


  Robot2D robot(&world, "myRobot");
  world.addRobot(&robot);


  stringstream ss;
  ss << "-ws" << worldSize;
  ss << "-nl" << nlandmarks;
  ss << "-steps" << simSteps;

  if (hasOdom) {
    SensorOdometry2D* odometrySensor=new SensorOdometry2D("odometry");
    robot.addSensor(odometrySensor);
    Matrix3d odomInfo = odometrySensor->information();
    odomInfo.setIdentity();
    odomInfo*=100;
    odomInfo(2,2)=1000;
    odometrySensor->setInformation(odomInfo);
    ss << "-odom";
  }

  if (hasPoseSensor) {
    SensorPose2D* poseSensor = new SensorPose2D("poseSensor");
    robot.addSensor(poseSensor);
    Matrix3d poseInfo = poseSensor->information();
    poseInfo.setIdentity();
    poseInfo*=100;
    poseInfo(2,2)=1000;
    poseSensor->setInformation(poseInfo);
    ss << "-pose";
  }

  if (hasPointSensor) {
    SensorPointXY* pointSensor = new SensorPointXY("pointSensor");
    robot.addSensor(pointSensor);
    Matrix2d pointInfo = pointSensor->information();
    pointInfo.setIdentity();
    pointInfo*=100;
    pointSensor->setInformation(pointInfo);
    ss << "-pointXY";
  }

  if (hasPointBearingSensor) {
    SensorPointXYBearing* bearingSensor = new SensorPointXYBearing("bearingSensor");
    robot.addSensor(bearingSensor);
    bearingSensor->setInformation(bearingSensor->information()*1000);
    ss << "-pointBearing";
  }

  if (hasSegmentSensor) {
    SensorSegment2D* segmentSensor = new SensorSegment2D("segmentSensorSensor");
    segmentSensor->setMaxRange(3);
    segmentSensor->setMinRange(.1);
    robot.addSensor(segmentSensor);
    segmentSensor->setInformation(segmentSensor->information()*1000);

    SensorSegment2DLine* segmentSensorLine = new SensorSegment2DLine("segmentSensorSensorLine");
    segmentSensorLine->setMaxRange(3);
    segmentSensorLine->setMinRange(.1);
    robot.addSensor(segmentSensorLine);
    Matrix2d m=segmentSensorLine->information();
    m=m*1000;
    m(0,0)*=10;
    segmentSensorLine->setInformation(m);

    SensorSegment2DPointLine* segmentSensorPointLine = new SensorSegment2DPointLine("segmentSensorSensorPointLine");
    segmentSensorPointLine->setMaxRange(3);
    segmentSensorPointLine->setMinRange(.1);
    robot.addSensor(segmentSensorPointLine);
    Matrix3d m3=segmentSensorPointLine->information();
    m3=m3*1000;
    m3(3,3)*=10;
    segmentSensorPointLine->setInformation(m3);

    ss << "-segment2d";
  }



  robot.move(SE2());
  double pStraight=0.7;
  SE2 moveStraight; moveStraight.setTranslation(Vector2d(1., 0.));
  double pLeft=0.15;
  SE2 moveLeft; moveLeft.setRotation(Rotation2Dd(M_PI/2));
  //double pRight=0.15;
  SE2 moveRight; moveRight.setRotation(Rotation2Dd(-M_PI/2));

  for (int i=0; i<simSteps; i++){
    cerr << "m";
    SE2 move;
    SE2 pose=robot.pose();
    double dtheta=-100;
    Vector2d dt;
    if (pose.translation().x() < -.5*worldSize){
      dtheta = 0;
    }

    if (pose.translation().x() >  .5*worldSize){
      dtheta = -M_PI;
    }

    if (pose.translation().y() < -.5*worldSize){
      dtheta = M_PI/2;
    }

    if (pose.translation().y() >  .5*worldSize){
      dtheta = -M_PI/2;
    }
    if (dtheta< -M_PI) {
      // select a random move of the robot
      double sampled=sampleUniform(0.,1.,&generator);
      if (sampled<pStraight)
        move=moveStraight;
      else if (sampled<pStraight+pLeft)
        move=moveLeft;
      else
        move=moveRight;
    } else {
      double mTheta=dtheta-pose.rotation().angle();
      move.setRotation(Rotation2Dd(mTheta));
      if (move.rotation().angle()<std::numeric_limits<double>::epsilon()){
        move.setTranslation(Vector2d(1., 0.));
      }
    }
    robot.relativeMove(move);
    // do a sense
    cerr << "s";
    robot.sense();
  }
  //string fname=outputFilename + ss.str() + ".g2o";
  ofstream testStream(outputFilename.c_str());
  graph.save(testStream);

  return 0;
}
Ejemplo n.º 7
0
int main(int argc, char** argv) {
  CommandArgs arg;
  int nlandmarks;
  int simSteps;
  double worldSize;
  bool hasOdom;
  bool hasPoseSensor;
  bool hasPointSensor;
  bool hasPointBearingSensor;
  bool hasSegmentSensor;
  bool hasCompass;
  bool hasGPS;


  std::string outputFilename;
  arg.param("simSteps", simSteps, 100, "number of simulation steps");
  arg.param("worldSize", worldSize, 25.0, "size of the world");
  arg.param("hasOdom",        hasOdom, false,  "the robot has an odometry" );
  arg.param("hasPointSensor", hasPointSensor, false, "the robot has a point sensor" );
  arg.param("hasPointBearingSensor", hasPointBearingSensor, false, "the robot has a point bearing sensor" );
  arg.param("hasPoseSensor",  hasPoseSensor, false,  "the robot has a pose sensor" );
  arg.param("hasCompass",     hasCompass, false, "the robot has a compass");
  arg.param("hasGPS",         hasGPS, false, "the robot has a GPS");
  arg.param("hasSegmentSensor", hasSegmentSensor, false, "the robot has a segment sensor" );
  arg.paramLeftOver("graph-output", outputFilename, "simulator_out.g2o", "graph file which will be written", true);


  arg.parseArgs(argc, argv);
  
  std::tr1::ranlux_base_01 generator;
  OptimizableGraph graph;
  World world(&graph);
  for (int i=0; i<nlandmarks; i++){
    WorldObjectPointXY * landmark = new WorldObjectPointXY;
    double x = sampleUniform(-.5,.5, &generator)*worldSize;
    double y = sampleUniform(-.5,.5, &generator)*worldSize;
    landmark->vertex()->setEstimate(Vector2d(x,y));
    world.addWorldObject(landmark);
  }


  Robot2D robot(&world, "myRobot");
  world.addRobot(&robot);


  stringstream ss;
  ss << "-ws" << worldSize;
  ss << "-nl" << nlandmarks;
  ss << "-steps" << simSteps;

  if (hasOdom) {
    SensorOdometry2D* odometrySensor=new SensorOdometry2D("odometry");
    robot.addSensor(odometrySensor);
    Matrix3d odomInfo = odometrySensor->information();
    odomInfo.setIdentity();
    odomInfo*=100;
    odomInfo(2,2)=1000;
    odometrySensor->setInformation(odomInfo);
    ss << "-odom";
  }

  if (hasPoseSensor) {
    SensorPose2D* poseSensor = new SensorPose2D("poseSensor");
    robot.addSensor(poseSensor);
    Matrix3d poseInfo = poseSensor->information();
    poseInfo.setIdentity();
    poseInfo*=100;
    poseInfo(2,2)=1000;
    poseSensor->setInformation(poseInfo);
    ss << "-pose";
  }
  
  if (hasPointSensor) {
    SensorPointXY* pointSensor = new SensorPointXY("pointSensor");
    robot.addSensor(pointSensor);
    Matrix2d pointInfo = pointSensor->information();
    pointInfo.setIdentity();
    pointInfo*=100;
    pointSensor->setInformation(pointInfo);
    ss << "-pointXY";
  }

  if (hasPointBearingSensor) {
    SensorPointXYBearing* bearingSensor = new SensorPointXYBearing("bearingSensor");
    robot.addSensor(bearingSensor);
    bearingSensor->setInformation(bearingSensor->information()*1000);
    ss << "-pointBearing";
  }

  
  robot.move(SE2());
  double pStraight=0.7;
  SE2 moveStraight; moveStraight.setTranslation(Vector2d(1., 0.));
  double pLeft=0.15;
  SE2 moveLeft; moveLeft.setRotation(Rotation2Dd(M_PI/2));
  //double pRight=0.15;
  SE2 moveRight; moveRight.setRotation(Rotation2Dd(-M_PI/2));

  for (int i=0; i<simSteps; i++){
    cerr << "m";
    SE2 move;
    SE2 pose=robot.pose();
    double dtheta=-100;
    Vector2d dt;
    if (pose.translation().x() < -.5*worldSize){
      dtheta = 0;
    }

    if (pose.translation().x() >  .5*worldSize){
      dtheta = -M_PI;
    }
    
    if (pose.translation().y() < -.5*worldSize){
      dtheta = M_PI/2;
    }

    if (pose.translation().y() >  .5*worldSize){
      dtheta = -M_PI/2;
    }
    if (dtheta< -M_PI) {
      // select a random move of the robot
      double sampled=sampleUniform(0.,1.,&generator);
      if (sampled<pStraight)
        move=moveStraight;
      else if (sampled<pStraight+pLeft)
        move=moveLeft;
      else
        move=moveRight;
    } else {
      double mTheta=dtheta-pose.rotation().angle();
      move.setRotation(Rotation2Dd(mTheta));
      if (move.rotation().angle()<std::numeric_limits<double>::epsilon()){
        move.setTranslation(Vector2d(1., 0.));
      }
    }
    robot.relativeMove(move);
    // do a sense
    cerr << "s";
    robot.sense();
  }
  //string fname=outputFilename + ss.str() + ".g2o";
  ofstream testStream(outputFilename.c_str());
  graph.save(testStream);
 
}