C++ CommandArgs::paramLeftOver方法代码示例

int main(int argc, char** argv)
{
  QApplication qapp(argc, argv);
#ifdef G2O_HAVE_GLUT
  glutInit(&argc, argv);
#endif

  string dummy;
  string inputFilename;
  CommandArgs arg;
  arg.param("solverlib", dummy, "", "specify a solver library which will be loaded");
  arg.param("typeslib", dummy, "", "specify a types library which will be loaded");
  arg.paramLeftOver("graph-input", inputFilename, "", "graph file which will be processed", true);

  arg.parseArgs(argc, argv);
  
  // loading the standard solver /  types
  DlWrapper dlTypesWrapper;
  loadStandardTypes(dlTypesWrapper, argc, argv);

  // register all the solvers
  DlWrapper dlSolverWrapper;
  loadStandardSolver(dlSolverWrapper, argc, argv);

  MainWindow mw;
  mw.updateDisplayedSolvers();
  mw.show();

  // redirect the output that normally goes to cerr to the textedit in the viewer
  StreamRedirect redirect(cerr, mw.plainTextEdit);

  // setting up the optimizer
  SparseOptimizer* optimizer = new SparseOptimizer();
  mw.viewer->graph = optimizer;

  // set up the GUI action
  GuiHyperGraphAction guiHyperGraphAction;
  guiHyperGraphAction.viewer = mw.viewer;
  optimizer->addPostIterationAction(&guiHyperGraphAction);

  if (inputFilename.size() > 0) {
    mw.loadFromFile(QString::fromStdString(inputFilename));
  }

  while (mw.isVisible()) {
    guiHyperGraphAction.dumpScreenshots = mw.actionDump_Images->isChecked();
    qapp.processEvents();
    SleepThread::msleep(10);
  }

  delete optimizer;

  // destroy all the singletons
  //Factory::destroy();
  //OptimizationAlgorithmFactory::destroy();
  //HyperGraphActionLibrary::destroy();

  return 0;
}

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());
  }
 
}

int RunG2OViewer::run(int argc, char** argv, CommandArgs& arg)
{
  std::string inputFilename;
  arg.paramLeftOver("graph-input", inputFilename, "", "graph file which will be processed", true);
  arg.parseArgs(argc, argv);

  MainWindow mw;
  mw.updateDisplayedSolvers();
  mw.updateRobustKernels();
  mw.show();

  // redirect the output that normally goes to cerr to the textedit in the viewer
  StreamRedirect redirect(std::cerr, mw.plainTextEdit);

  // setting up the optimizer
  SparseOptimizer* optimizer = new SparseOptimizer();
  mw.viewer->graph = optimizer;

  // set up the GUI action
  GuiHyperGraphAction guiHyperGraphAction;
  guiHyperGraphAction.viewer = mw.viewer;
  //optimizer->addPostIterationAction(&guiHyperGraphAction);
  optimizer->addPreIterationAction(&guiHyperGraphAction);

  if (inputFilename.size() > 0) {
    mw.loadFromFile(QString::fromLocal8Bit(inputFilename.c_str()));
  }

  QCoreApplication* myapp = QApplication::instance();
  while (mw.isVisible()) {
    guiHyperGraphAction.dumpScreenshots = mw.actionDump_Images->isChecked();
    if (myapp)
      myapp->processEvents();
    SleepThread::msleep(10);
  }

  delete optimizer;
  return 0;
}

int main(int argc, char** argv)
{
  bool fixLaser;
  int maxIterations;
  bool verbose;
  string inputFilename;
  string outputfilename;
  string rawFilename;
  string odomTestFilename;
  string dumpGraphFilename;
  // command line parsing
  CommandArgs commandLineArguments;
  commandLineArguments.param("i", maxIterations, 10, "perform n iterations");
  commandLineArguments.param("v", verbose, false, "verbose output of the optimization process");
  commandLineArguments.param("o", outputfilename, "", "output final version of the graph");
  commandLineArguments.param("test", odomTestFilename, "", "apply odometry calibration to some test data");
  commandLineArguments.param("dump", dumpGraphFilename, "", "write the graph to the disk");
  commandLineArguments.param("fixLaser", fixLaser, false, "keep the laser offset fixed during optimization");
  commandLineArguments.paramLeftOver("gm2dl-input", inputFilename, "", "gm2dl file which will be processed");
  commandLineArguments.paramLeftOver("raw-log", rawFilename, "", "raw log file containing the odometry");

  commandLineArguments.parseArgs(argc, argv);

  SparseOptimizer optimizer;
  optimizer.setVerbose(verbose);

  allocateSolverForSclam(optimizer);

  // loading
  DataQueue odometryQueue;
  int numLaserOdom = Gm2dlIO::readRobotLaser(rawFilename, odometryQueue);
  if (numLaserOdom == 0) {
    cerr << "No raw information read" << endl;
    return 0;
  }
  cerr << "Read " << numLaserOdom << " laser readings from file" << endl;

  Eigen::Vector3d odomCalib(1., 1., 1.);
  SE2 initialLaserPose;
  DataQueue robotLaserQueue;
  int numRobotLaser = Gm2dlIO::readRobotLaser(inputFilename, robotLaserQueue);
  if (numRobotLaser == 0) {
    cerr << "No robot laser read" << endl;
    return 0;
  } else {
    RobotLaser* rl = dynamic_cast<RobotLaser*>(robotLaserQueue.buffer().begin()->second);
    initialLaserPose = rl->odomPose().inverse() * rl->laserPose();
    cerr << PVAR(initialLaserPose.toVector().transpose()) << endl;
  }

  // adding the measurements
  vector<MotionInformation, Eigen::aligned_allocator<MotionInformation> > motions;
  {
    std::map<double, RobotData*>::const_iterator it = robotLaserQueue.buffer().begin();
    std::map<double, RobotData*>::const_iterator prevIt = it++;
    for (; it != robotLaserQueue.buffer().end(); ++it) {
      MotionInformation mi;
      RobotLaser* prevLaser = dynamic_cast<RobotLaser*>(prevIt->second);
      RobotLaser* curLaser = dynamic_cast<RobotLaser*>(it->second);
      mi.laserMotion = prevLaser->laserPose().inverse() * curLaser->laserPose();
      // get the motion of the robot in that time interval
      RobotLaser* prevOdom = dynamic_cast<RobotLaser*>(odometryQueue.findClosestData(prevLaser->timestamp()));
      RobotLaser* curOdom = dynamic_cast<RobotLaser*>(odometryQueue.findClosestData(curLaser->timestamp()));
      mi.odomMotion = prevOdom->odomPose().inverse() * curOdom->odomPose();
      mi.timeInterval = prevOdom->timestamp() - curOdom->timestamp();
      prevIt = it;
      motions.push_back(mi);
    }
  }

  if (1) {
    VertexSE2* laserOffset = new VertexSE2;
    laserOffset->setId(Gm2dlIO::ID_LASERPOSE);
    laserOffset->setEstimate(initialLaserPose);
    optimizer.addVertex(laserOffset);
    VertexOdomDifferentialParams* odomParamsVertex = new VertexOdomDifferentialParams;
    odomParamsVertex->setId(Gm2dlIO::ID_ODOMCALIB);
    odomParamsVertex->setEstimate(Eigen::Vector3d(1., 1., 1.));
    optimizer.addVertex(odomParamsVertex);
    for (size_t i = 0; i < motions.size(); ++i) {
      const SE2& odomMotion = motions[i].odomMotion;
      const SE2& laserMotion = motions[i].laserMotion;
      const double& timeInterval = motions[i].timeInterval;
      // add the edge
      MotionMeasurement mm(odomMotion.translation().x(), odomMotion.translation().y(), odomMotion.rotation().angle(), timeInterval);
      OdomAndLaserMotion meas;
      meas.velocityMeasurement = OdomConvert::convertToVelocity(mm);
      meas.laserMotion = laserMotion;
      EdgeSE2PureCalib* calibEdge = new EdgeSE2PureCalib;
      calibEdge->setVertex(0, laserOffset);
      calibEdge->setVertex(1, odomParamsVertex);
      calibEdge->setInformation(Eigen::Matrix3d::Identity());
      calibEdge->setMeasurement(meas);
      if (! optimizer.addEdge(calibEdge)) {
        cerr << "Error adding calib edge" << endl;
        delete calibEdge;
      }
    }

    if (fixLaser) {
//.........这里部分代码省略.........

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;
}

int main(int argc, char** argv)
{
  OptimizableGraph::initMultiThreading();
  int maxIterations;
  bool verbose;
  string inputFilename;
  string gnudump;
  string outputfilename;
  string solverProperties;
  string strSolver;
  string loadLookup;
  bool initialGuess;
  bool initialGuessOdometry;
  bool marginalize;
  bool listTypes;
  bool listSolvers;
  bool listRobustKernels;
  bool incremental;
  bool guiOut;
  int gaugeId;
  string robustKernel;
  bool computeMarginals;
  bool printSolverProperties;
  double huberWidth;
  double gain;
  int maxIterationsWithGain;
  //double lambdaInit;
  int updateGraphEachN = 10;
  string statsFile;
  string summaryFile;
  bool nonSequential;
  // command line parsing
  std::vector<int> gaugeList;
  CommandArgs arg;
  arg.param("i", maxIterations, 5, "perform n iterations, if negative consider the gain");
  arg.param("gain", gain, 1e-6, "the gain used to stop optimization (default = 1e-6)");
  arg.param("ig",maxIterationsWithGain, std::numeric_limits<int>::max(), "Maximum number of iterations with gain enabled (default: inf)");
  arg.param("v", verbose, false, "verbose output of the optimization process");
  arg.param("guess", initialGuess, false, "initial guess based on spanning tree");
  arg.param("guessOdometry", initialGuessOdometry, false, "initial guess based on odometry");
  arg.param("inc", incremental, false, "run incremetally");
  arg.param("update", updateGraphEachN, 10, "updates after x odometry nodes");
  arg.param("guiout", guiOut, false, "gui output while running incrementally");
  arg.param("marginalize", marginalize, false, "on or off");
  arg.param("printSolverProperties", printSolverProperties, false, "print the properties of the solver");
  arg.param("solverProperties", solverProperties, "", "set the internal properties of a solver,\n\te.g., initialLambda=0.0001,maxTrialsAfterFailure=2");
  arg.param("gnudump", gnudump, "", "dump to gnuplot data file");
  arg.param("robustKernel", robustKernel, "", "use this robust error function");
  arg.param("robustKernelWidth", huberWidth, -1., "width for the robust Kernel (only if robustKernel)");
  arg.param("computeMarginals", computeMarginals, false, "computes the marginal covariances of something. FOR TESTING ONLY");
  arg.param("gaugeId", gaugeId, -1, "force the gauge");
  arg.param("o", outputfilename, "", "output final version of the graph");
  arg.param("solver", strSolver, "gn_var", "specify which solver to use underneat\n\t {gn_var, lm_fix3_2, gn_fix6_3, lm_fix7_3}");
#ifndef G2O_DISABLE_DYNAMIC_LOADING_OF_LIBRARIES
  string dummy;
  arg.param("solverlib", dummy, "", "specify a solver library which will be loaded");
  arg.param("typeslib", dummy, "", "specify a types library which will be loaded");
#endif
  arg.param("stats", statsFile, "", "specify a file for the statistics");
  arg.param("listTypes", listTypes, false, "list the registered types");
  arg.param("listRobustKernels", listRobustKernels, false, "list the registered robust kernels");
  arg.param("listSolvers", listSolvers, false, "list the available solvers");
  arg.param("renameTypes", loadLookup, "", "create a lookup for loading types into other types,\n\t TAG_IN_FILE=INTERNAL_TAG_FOR_TYPE,TAG2=INTERNAL2\n\t e.g., VERTEX_CAM=VERTEX_SE3:EXPMAP");
  arg.param("gaugeList", gaugeList, std::vector<int>(), "set the list of gauges separated by commas without spaces \n  e.g: 1,2,3,4,5 ");
  arg.param("summary", summaryFile, "", "append a summary of this optimization run to the summary file passed as argument");
  arg.paramLeftOver("graph-input", inputFilename, "", "graph file which will be processed", true);
  arg.param("nonSequential", nonSequential, false, "apply the robust kernel only on loop closures and not odometries");
  

  arg.parseArgs(argc, argv);

  if (verbose) {
    cout << "# Used Compiler: " << G2O_CXX_COMPILER << endl;
  }

#ifndef G2O_DISABLE_DYNAMIC_LOADING_OF_LIBRARIES
  // registering all the types from the libraries
  DlWrapper dlTypesWrapper;
  loadStandardTypes(dlTypesWrapper, argc, argv);
  // register all the solvers
  DlWrapper dlSolverWrapper;
  loadStandardSolver(dlSolverWrapper, argc, argv);
#else
  if (verbose)
    cout << "# linked version of g2o" << endl;
#endif

  OptimizationAlgorithmFactory* solverFactory = OptimizationAlgorithmFactory::instance();
  if (listSolvers) {
    solverFactory->listSolvers(cout);
  }

  if (listTypes) {
    Factory::instance()->printRegisteredTypes(cout, true);
  }

  if (listRobustKernels) {
    std::vector<std::string> kernels;
    RobustKernelFactory::instance()->fillKnownKernels(kernels);
    cout << "Robust Kernels:" << endl;
//.........这里部分代码省略.........

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;
//.........这里部分代码省略.........

int main(int argc, char** argv) {
    /************************************************************************
     *                           Input Handling                             *
     ************************************************************************/
    string configFile;
    string logFile;
    CommandArgs arg;
    int nscale;
    int mscale;

    // Optional input parameters.
    arg.param("nscale", nscale, 1, "image scaling for the normal extraction");
    arg.param("mscale", mscale, 1, "image scaling for matching");

    // Last parameter has to be the working directory.
    arg.paramLeftOver("config_file", configFile, "", "file where the configuration will be written", true);
    arg.paramLeftOver("log_file", logFile, "", "synchronized log file", true);
    arg.parseArgs(argc, argv);

    //Aligner* aligner;
    //DepthImageConverter* converter;

    cerr << "processing log file [" << logFile << "] with parameters read from [" << configFile << "]" << endl;

    cerr << "reading the configuration from file [" << configFile << "]" << endl;
    std::vector<Serializable*> alignerInstances =  readConfig(argv[0], aligner, converter, configFile);

    cerr<< "Aligner: " << aligner << endl;
    cerr<< "Converter: " << converter << endl;


    if (logFile=="") {
        cerr << "logfile not supplied" << endl;
    }


    Deserializer des;
    des.setFilePath(logFile);

    std::vector<BaseSensorData*> sensorDatas;
    RobotConfiguration* conf = readLog(sensorDatas, des);
    cerr << "# frames: " << conf->frameMap().size() << endl;
    cerr << "# sensors: " << conf->sensorMap().size() << endl;
    cerr << "# sensorDatas: " << sensorDatas.size() << endl;

    TSCompare comp;
    std::sort(sensorDatas.begin(), sensorDatas.end(), comp);

    MapManager* manager = new MapManager;
    SensingFrameNodeMaker* sensingFrameMaker = new SensingFrameNodeMaker;
    sensingFrameMaker->init(manager, conf);

    ImuRelationAdder* imuAdder = new ImuRelationAdder(manager, conf);
    OdometryRelationAdder* odomAdder = new OdometryRelationAdder(manager, conf);
    TwoDepthImageAlignerNode* pairwiseAligner = new TwoDepthImageAlignerNode(manager, conf, converter, aligner,  "/kinect/depth_registered/image_raw");
    for (size_t i = 0; i<sensorDatas.size(); i++) {
        // see if you make a sensing frame with all the infos you find
        SensingFrameNode* s = sensingFrameMaker->processData(sensorDatas[i]);
        if (s) {

            // add a unary relation modeling the imu to the sensing frame
            imuAdder->processNode(s);

            // add a binary relation modeling the odometry between two sensing frames
            odomAdder->processNode(s);

            // add a pwn sensing data(if you manage)
            pairwiseAligner->processNode(s);
        }
    }

    cerr << "writing out things" << endl;
    Serializer ser;
    ser.setFilePath("out.log");
    for (size_t i = 0; i< alignerInstances.size(); i++) {
        ser.writeObject(*alignerInstances[i]);
    }
    conf->serializeInternals(ser);
    ser.writeObject(*conf);

    ReferenceFrame* lastFrame = 0;
    for (size_t i = 0; i<sensorDatas.size(); i++) {
        if (lastFrame != sensorDatas[i]->robotReferenceFrame()) {
            lastFrame = sensorDatas[i]->robotReferenceFrame();
            ser.writeObject(*lastFrame);
        }
        ser.writeObject(*sensorDatas[i]);
    }
    ser.writeObject(*manager);
    for (std::set<MapNode*>::iterator it = manager->nodes().begin(); it!=manager->nodes().end(); it++)
        ser.writeObject(**it);
    cerr << "writing out " << manager->relations().size() << " relations" << endl;
    for (std::set<MapNodeRelation*>::iterator it = manager->relations().begin(); it!=manager->relations().end(); it++)
        ser.writeObject(**it);


    // write the aligner stuff

    // write the robot configuration

//.........这里部分代码省略.........

int main(int argc, char** argv)
{
  bool fixLaser;
  int maxIterations;
  bool verbose;
  string inputFilename;
  string outputfilename;
  string rawFilename;
  string odomTestFilename;
  string dumpGraphFilename;
  // command line parsing
  CommandArgs commandLineArguments;
  commandLineArguments.param("i", maxIterations, 10, "perform n iterations");
  commandLineArguments.param("v", verbose, false, "verbose output of the optimization process");
  commandLineArguments.param("o", outputfilename, "", "output final version of the graph");
  commandLineArguments.param("test", odomTestFilename, "", "apply odometry calibration to some test data");
  commandLineArguments.param("dump", dumpGraphFilename, "", "write the graph to the disk");
  commandLineArguments.param("fixLaser", fixLaser, false, "keep the laser offset fixed during optimization");
  commandLineArguments.paramLeftOver("gm2dl-input", inputFilename, "", "gm2dl file which will be processed");
  commandLineArguments.paramLeftOver("raw-log", rawFilename, "", "raw log file containing the odometry");

  commandLineArguments.parseArgs(argc, argv);

  SparseOptimizer optimizer;
  optimizer.setVerbose(verbose);
  optimizer.setForceStopFlag(&hasToStop);

  allocateSolverForSclam(optimizer);

  // loading
  if (! Gm2dlIO::readGm2dl(inputFilename, optimizer, false)) {
    cerr << "Error while loading gm2dl file" << endl;
  }
  DataQueue robotLaserQueue;
  int numLaserOdom = Gm2dlIO::readRobotLaser(rawFilename, robotLaserQueue);
  if (numLaserOdom == 0) {
    cerr << "No raw information read" << endl;
    return 0;
  }
  cerr << "Read " << numLaserOdom << " laser readings from file" << endl;

  bool gaugeFreedom = optimizer.gaugeFreedom();

  OptimizableGraph::Vertex* gauge = optimizer.findGauge();
  if (gaugeFreedom) {
    if (! gauge) {
      cerr <<  "# cannot find a vertex to fix in this thing" << endl;
      return 2;
    } else {
      cerr << "# graph is fixed by node " << gauge->id() << endl;
      gauge->setFixed(true);
    }
  } else {
    cerr << "# graph is fixed by priors" << endl;
  }

  addOdometryCalibLinksDifferential(optimizer, robotLaserQueue);

  // sanity check
  HyperDijkstra d(&optimizer);
  UniformCostFunction f;
  d.shortestPaths(gauge, &f);
  //cerr << PVAR(d.visited().size()) << endl;

  if (d.visited().size()!=optimizer.vertices().size()) {
    cerr << CL_RED("Warning: d.visited().size() != optimizer.vertices().size()") << endl;
    cerr << "visited: " << d.visited().size() << endl;
    cerr << "vertices: " << optimizer.vertices().size() << endl;
    if (1)
      for (SparseOptimizer::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) {
        OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
        if (d.visited().count(v) == 0) {
          cerr << "\t unvisited vertex " << it->first << " " << (void*)v << endl;
          v->setFixed(true);
        }
      }
  }

  for (SparseOptimizer::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) {
    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
    if (v->fixed()) {
      cerr << "\t fixed vertex " << it->first << endl;
    }
  }

  VertexSE2* laserOffset = dynamic_cast<VertexSE2*>(optimizer.vertex(Gm2dlIO::ID_LASERPOSE));
  VertexOdomDifferentialParams* odomParamsVertex = dynamic_cast<VertexOdomDifferentialParams*>(optimizer.vertex(Gm2dlIO::ID_ODOMCALIB));

  if (fixLaser) {
    cerr << "Fix position of the laser offset" << endl;
    laserOffset->setFixed(true);
  }

  signal(SIGINT, sigquit_handler);
  cerr << "Doing full estimation" << endl;
  optimizer.initializeOptimization();
  optimizer.computeActiveErrors();
  cerr << "Initial chi2 = " << FIXED(optimizer.chi2()) << endl;

  int i=optimizer.optimize(maxIterations);
//.........这里部分代码省略.........

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";
//.........这里部分代码省略.........