size_t vertexEdgesInStar(HyperGraph::EdgeSet& eset, HyperGraph::Vertex* v, Star* s, EdgeStarMap& esmap){
  eset.clear();
  for (HyperGraph::EdgeSet::iterator it=v->edges().begin(); it!=v->edges().end(); it++){
    HyperGraph::Edge* e=*it;
    EdgeStarMap::iterator eit=esmap.find(e);
    if (eit!=esmap.end() && eit->second == s)
      eset.insert(e);
  }
  return eset.size();
}
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());
  }
 
}
  void computeSimpleStars(StarSet& stars,
			  SparseOptimizer* optimizer,
			  EdgeLabeler* labeler,
			  EdgeCreator* creator,
			  OptimizableGraph::Vertex* gauge_,
			  std::string edgeTag,
			  std::string vertexTag,
			  int level,
			  int step,
			  int backboneIterations,
			  int starIterations,
			  double rejectionThreshold,
			  bool debug){

    cerr << "preforming the tree actions" << endl;
    HyperDijkstra d(optimizer);
    // compute a spanning tree based on the types of edges and vertices in the pool
    EdgeTypesCostFunction f(edgeTag, vertexTag, level);
    d.shortestPaths(gauge_,
        &f,
        std::numeric_limits< double >::max(),
        1e-6,
        false,
        std::numeric_limits< double >::max()/2);

    HyperDijkstra::computeTree(d.adjacencyMap());
    // constructs the stars on the backbone

    BackBoneTreeAction bact(optimizer, vertexTag, level, step);
    bact.init();

    cerr << "free edges size " << bact.freeEdges().size() << endl;

    // perform breadth-first visit of the visit tree and create the stars on the backbone
    d.visitAdjacencyMap(d.adjacencyMap(),&bact,true);
    stars.clear();

    for (VertexStarMultimap::iterator it=bact.vertexStarMultiMap().begin();
        it!=bact.vertexStarMultiMap().end(); it++){
      stars.insert(it->second);
    }
    cerr << "stars.size: " << stars.size() << endl;
    cerr << "size: " << bact.vertexStarMultiMap().size() << endl;


    //  for each star

    //    for all vertices in the backbone, select all edges leading/leaving from that vertex
    //    that are contained in freeEdges.

    //      mark the corresponding "open" vertices and add them to a multimap (vertex->star)

    //    select a gauge in the backbone

    //    push all vertices on the backbone

    //    compute an initial guess on the backbone

    //    one round of optimization backbone

    //    lock all vertices in the backbone

    //    push all "open" vertices

    //    for each open vertex,
    //      compute an initial guess given the backbone
    //      do some rounds of solveDirect
    //      if (fail)
    //        - remove the vertex and the edges in that vertex from the star
    //   - make the structures consistent

    //    pop all "open" vertices
    //    pop all "vertices" in the backbone
    //    unfix the vertices in the backbone

    int starNum=0;
    for (StarSet::iterator it=stars.begin(); it!=stars.end(); it++){
      Star* s =*it;
      HyperGraph::VertexSet backboneVertices = s->_lowLevelVertices;
      HyperGraph::EdgeSet backboneEdges = s->_lowLevelEdges;
      if (backboneEdges.empty())
	continue;


      // cerr << "optimizing backbone" << endl;
      // one of these  should be the gauge, to be simple we select the fisrt one in the backbone
      OptimizableGraph::VertexSet gauge;
      gauge.insert(*backboneVertices.begin());
      s->gauge()=gauge;
      s->optimizer()->push(backboneVertices);
      s->optimizer()->setFixed(gauge,true);
      s->optimizer()->initializeOptimization(backboneEdges);
      s->optimizer()->computeInitialGuess();
      s->optimizer()->optimize(backboneIterations);
      s->optimizer()->setFixed(backboneVertices, true);

      // cerr << "assignind edges.vertices not in bbone" << endl;
      HyperGraph::EdgeSet otherEdges;
      HyperGraph::VertexSet otherVertices;
      std::multimap<HyperGraph::Vertex*, HyperGraph::Edge*> vemap;
      for (HyperGraph::VertexSet::iterator bit=backboneVertices.begin(); bit!=backboneVertices.end(); bit++){
	HyperGraph::Vertex* v=*bit;
	for (HyperGraph::EdgeSet::iterator eit=v->edges().begin(); eit!=v->edges().end(); eit++){
	  OptimizableGraph::Edge* e = (OptimizableGraph::Edge*) *eit;
	  HyperGraph::EdgeSet::iterator feit=bact.freeEdges().find(e);
	  if (feit!=bact.freeEdges().end()){ // edge is admissible
	    otherEdges.insert(e);
	    bact.freeEdges().erase(feit);
	    for (size_t i=0; i<e->vertices().size(); i++){
	      OptimizableGraph::Vertex* ve= (OptimizableGraph::Vertex*)e->vertices()[i];
	      if (backboneVertices.find(ve)==backboneVertices.end()){
		otherVertices.insert(ve);
		vemap.insert(make_pair(ve,e));
	      }
	    }
	  }
	}
      }

      // RAINER TODO maybe need a better solution than dynamic casting here??
      OptimizationAlgorithmWithHessian* solverWithHessian = dynamic_cast<OptimizationAlgorithmWithHessian*>(s->optimizer()->solver());
      if (solverWithHessian) {
        s->optimizer()->push(otherVertices);
        // cerr << "optimizing vertices out of bbone" << endl;
        // cerr << "push" << endl;
        // cerr << "init" << endl;
        s->optimizer()->initializeOptimization(otherEdges);
        // cerr << "guess" << endl;
        s->optimizer()->computeInitialGuess();
        // cerr << "solver init" << endl;
        s->optimizer()->solver()->init();
        // cerr << "structure" << endl;
        if (!solverWithHessian->buildLinearStructure())
          cerr << "FATAL: failure while building linear structure" << endl;
        // cerr << "errors" << endl;
        s->optimizer()->computeActiveErrors();
        // cerr << "system" << endl;
        solverWithHessian->updateLinearSystem();
        // cerr << "directSolove" << endl;
      } else {
        cerr << "FATAL: hierarchical thing cannot be used with a solver that does not support the system structure construction" << endl;
      }


      // // then optimize the vertices one at a time to check if a solution is good
      for (HyperGraph::VertexSet::iterator vit=otherVertices.begin(); vit!=otherVertices.end(); vit++){
        OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)(*vit);
        v->solveDirect();
        // cerr << " " << d;
        // if  a solution is found, add a vertex and all the edges in
        //othervertices that are pointing to that edge to the star
        s->_lowLevelVertices.insert(v);
        for (HyperGraph::EdgeSet::iterator eit=v->edges().begin(); eit!=v->edges().end(); eit++){
          OptimizableGraph::Edge* e = (OptimizableGraph::Edge*) *eit;
          if (otherEdges.find(e)!=otherEdges.end())
            s->_lowLevelEdges.insert(e);
        }
      }
      //cerr <<  endl;

      // relax the backbone and optimize it all
      // cerr << "relax bbone" << endl;
      s->optimizer()->setFixed(backboneVertices, false);
      //cerr << "fox gauge bbone" << endl;
      s->optimizer()->setFixed(s->gauge(),true);

      //cerr << "opt init" << endl;
      s->optimizer()->initializeOptimization(s->_lowLevelEdges);
      optimizer->computeActiveErrors();
      double initialChi = optimizer->activeChi2();
      int starOptResult = s->optimizer()->optimize(starIterations);
      //cerr << starOptResult << "(" << starIterations << ")  " << endl;
      double finalchi=-1.;

      cerr <<  "computing star: " << starNum << endl;

      int vKept=0, vDropped=0;
      if (!starIterations || starOptResult > 0  ){
	optimizer->computeActiveErrors();
	finalchi = optimizer->activeChi2();

#if 1

        s->optimizer()->computeActiveErrors();
        // cerr << "system" << endl;
        if (solverWithHessian)
          solverWithHessian->updateLinearSystem();
        HyperGraph::EdgeSet prunedStarEdges = backboneEdges;
        HyperGraph::VertexSet prunedStarVertices = backboneVertices;
        for (HyperGraph::VertexSet::iterator vit=otherVertices.begin(); vit!=otherVertices.end(); vit++){

	  //discard the vertices whose error is too big


          OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)(*vit);
          MatrixXd h(v->dimension(), v->dimension());
          for (int i=0; i<v->dimension(); i++){
            for (int j=0; j<v->dimension(); j++)
              h(i,j)=v->hessian(i,j);
          }
          EigenSolver<Eigen::MatrixXd> esolver;
          esolver.compute(h);
          VectorXcd ev= esolver.eigenvalues();
          double emin = std::numeric_limits<double>::max();
          double emax = -std::numeric_limits<double>::max();
          for (int i=0; i<ev.size(); i++){
            emin = ev(i).real()>emin ? emin : ev(i).real();
            emax = ev(i).real()<emax ? emax : ev(i).real();
          }

          double d=emin/emax;


          // cerr << " " << d;
          if (d>rejectionThreshold){
	  // if  a solution is found, add a vertex and all the edges in
	  //othervertices that are pointing to that edge to the star
            prunedStarVertices.insert(v);
            for (HyperGraph::EdgeSet::iterator eit=v->edges().begin(); eit!=v->edges().end(); eit++){
              OptimizableGraph::Edge* e = (OptimizableGraph::Edge*) *eit;
              if (otherEdges.find(e)!=otherEdges.end())
                prunedStarEdges.insert(e);
            }
            //cerr << "K( " << v->id() << "," << d << ")" ;
            vKept ++;
          } else {
            vDropped++;
            //cerr << "R( " << v->id() << "," << d << ")" ;
          }
        }
        s->_lowLevelEdges=prunedStarEdges;
        s->_lowLevelVertices=prunedStarVertices;

#endif
	//cerr << "addHedges" << endl;
	//now add to the star the hierarchical edges
	std::vector<OptimizableGraph::Vertex*> vertices(2);
	vertices[0]= (OptimizableGraph::Vertex*) *s->_gauge.begin();

	for (HyperGraph::VertexSet::iterator vit=s->_lowLevelVertices.begin(); vit!=s->_lowLevelVertices.end(); vit++){
	  OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)*vit;
	  vertices[1]=v;
	  if (v==vertices[0])
	    continue;
	  OptimizableGraph::Edge* e=creator->createEdge(vertices);
	  //rr << "creating edge" << e <<  Factory::instance()->tag(vertices[0]) << "->" <<  Factory::instance()->tag(v) <endl;
	  if (e) {
	    e->setLevel(level+1);
	    optimizer->addEdge(e);
	    s->_starEdges.insert(e);
	  } else {
            cerr << "HERE" << endl;
	    cerr << "FATAL, cannot create edge" << endl;
	  }
	}
      }

      cerr << " gauge: " << (*s->_gauge.begin())->id()
           << " kept: " << vKept
           << " dropped: " << vDropped
	   << " edges:" << s->_lowLevelEdges.size()
	   << " hedges" << s->_starEdges.size()
	   << " initial chi " << initialChi
	   << " final chi " << finalchi << endl;

      if (debug) {
	char starLowName[100];
	sprintf(starLowName, "star-%04d-low.g2o", starNum);
	ofstream starLowStream(starLowName);
	optimizer->saveSubset(starLowStream, s->_lowLevelEdges);
      }
      bool labelOk=false;
      if (!starIterations || starOptResult > 0)
        labelOk = s->labelStarEdges(0, labeler);
      if (labelOk) {
        if (debug) {
          char starHighName[100];
          sprintf(starHighName, "star-%04d-high.g2o", starNum);
          ofstream starHighStream(starHighName);
          optimizer->saveSubset(starHighStream, s->_starEdges);
        }
      } else {

        cerr << "FAILURE: " << starOptResult << endl;
      }
      starNum++;

      //label each hierarchical edge
      s->optimizer()->pop(otherVertices);
      s->optimizer()->pop(backboneVertices);
      s->optimizer()->setFixed(s->gauge(),false);
    }


    StarSet stars2;
    // now erase the stars that have 0 edges. They r useless
    for (StarSet::iterator it=stars.begin(); it!=stars.end(); it++){
      Star* s=*it;
      if (s->lowLevelEdges().size()==0) {
        delete s;
      } else
        stars2.insert(s);
    }
    stars=stars2;
  }
Exemple #4
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;
    for (size_t i = 0; i < kernels.size(); ++i) {
      cout << kernels[i] << endl;
    }
  }

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

  SparseOptimizerTerminateAction* terminateAction = 0;
  if (maxIterations < 0) {
    cerr << "# setup termination criterion based on the gain of the iteration" << endl;
    maxIterations = maxIterationsWithGain;
    terminateAction = new SparseOptimizerTerminateAction;
    terminateAction->setGainThreshold(gain);
    terminateAction->setMaxIterations(maxIterationsWithGain);
    optimizer.addPostIterationAction(terminateAction);
  }

  // allocating the desired solver + testing whether the solver is okay
  OptimizationAlgorithmProperty solverProperty;
  optimizer.setAlgorithm(solverFactory->construct(strSolver, solverProperty));
  if (! optimizer.solver()) {
    cerr << "Error allocating solver. Allocating \"" << strSolver << "\" failed!" << endl;
    return 0;
  }
  
  if (solverProperties.size() > 0) {
    bool updateStatus = optimizer.solver()->updatePropertiesFromString(solverProperties);
    if (! updateStatus) {
      cerr << "Failure while updating the solver properties from the given string" << endl;
    }
  }
  if (solverProperties.size() > 0 || printSolverProperties) {
    optimizer.solver()->printProperties(cerr);
  }

  // Loading the input data
  if (loadLookup.size() > 0) {
    optimizer.setRenamedTypesFromString(loadLookup);
  }
  if (inputFilename.size() == 0) {
    cerr << "No input data specified" << endl;
    return 0;
  } else if (inputFilename == "-") {
    cerr << "Read input from stdin" << endl;
    if (!optimizer.load(cin)) {
      cerr << "Error loading graph" << endl;
      return 2;
    }
  } else {
    cerr << "Read input from " << inputFilename << endl;
    ifstream ifs(inputFilename.c_str());
    if (!ifs) {
      cerr << "Failed to open file" << endl;
      return 1;
    }
    if (!optimizer.load(ifs)) {
      cerr << "Error loading graph" << endl;
      return 2;
    }
  }
  cerr << "Loaded " << optimizer.vertices().size() << " vertices" << endl;
  cerr << "Loaded " << optimizer.edges().size() << " edges" << endl;

  if (optimizer.vertices().size() == 0) {
    cerr << "Graph contains no vertices" << endl;
    return 1;
  }

  set<int> vertexDimensions = optimizer.dimensions();
  if (! optimizer.isSolverSuitable(solverProperty, vertexDimensions)) {
    cerr << "The selected solver is not suitable for optimizing the given graph" << endl;
    return 3;
  }
  assert (optimizer.solver());
  //optimizer.setMethod(str2method(strMethod));
  //optimizer.setUserLambdaInit(lambdaInit);

 
  // check for vertices to fix to remove DoF
  bool gaugeFreedom = optimizer.gaugeFreedom();
  OptimizableGraph::Vertex* gauge=0;
  if (gaugeList.size()){
    cerr << "Fixing gauges: ";
    for (size_t i=0; i<gaugeList.size(); i++){
      int id=gaugeList[i];
      OptimizableGraph::Vertex* v=optimizer.vertex(id);
      if (!v){
        cerr << "fatal, not found the vertex of id " << id << " in the gaugeList. Aborting";
        return -1;
      } else {
        if (i==0)
          gauge = v;
        cerr << v->id() << " ";
        v->setFixed(1);
      }
    }
    cerr << endl;
    gaugeFreedom = false;
  } else {
      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 or already fixed vertex" << endl;
  }

  // if schur, we wanna marginalize the landmarks...
  if (marginalize || solverProperty.requiresMarginalize) {
    int maxDim = *vertexDimensions.rbegin();
    int minDim = *vertexDimensions.begin();
    if (maxDim != minDim) {
      cerr << "# Preparing Marginalization of the Landmarks ... ";
      for (HyperGraph::VertexIDMap::iterator it=optimizer.vertices().begin(); it!=optimizer.vertices().end(); it++){
        OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second);
        if (v->dimension() != maxDim) {
          v->setMarginalized(true);
        }
      }
      cerr << "done." << endl;
    }
  }

  if (robustKernel.size() > 0) {
    AbstractRobustKernelCreator* creator = RobustKernelFactory::instance()->creator(robustKernel);
    cerr << "# Preparing robust error function ... ";
    if (creator) {
      if (nonSequential) {
        for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
          SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it);
          if (e->vertices().size() >= 2 && std::abs(e->vertex(0)->id() - e->vertex(1)->id()) != 1) {
            e->setRobustKernel(creator->construct());
            if (huberWidth > 0)
              e->robustKernel()->setDelta(huberWidth);
          }
        }
      } else {
        for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
          SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it);
          e->setRobustKernel(creator->construct());
          if (huberWidth > 0)
            e->robustKernel()->setDelta(huberWidth);
        }
      }
      cerr << "done." << endl;
    } else {
      cerr << "Unknown Robust Kernel: " << robustKernel << endl;
    }
  }

  // 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 (incremental) {
    cerr << CL_RED("# Note: this variant performs batch steps in each time step") << endl;
    cerr << CL_RED("#       For a variant which updates the Cholesky factor use the binary g2o_incremental") << endl;
    int incIterations = maxIterations;
    if (! arg.parsedParam("i")) {
      cerr << "# Setting default number of iterations" << endl;
      incIterations = 1;
    }
    int updateDisplayEveryN = updateGraphEachN;
    int maxDim = 0;

    cerr << "# incremental settings" << endl;
    cerr << "#\t solve every " << updateGraphEachN << endl;
    cerr << "#\t iterations  " << incIterations << endl;

    SparseOptimizer::VertexIDMap vertices = optimizer.vertices();
    for (SparseOptimizer::VertexIDMap::const_iterator it = vertices.begin(); it != vertices.end(); ++it) {
      const SparseOptimizer::Vertex* v = static_cast<const SparseOptimizer::Vertex*>(it->second);
      maxDim = max(maxDim, v->dimension());
    }

    vector<SparseOptimizer::Edge*> edges;
    for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
      SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it);
      edges.push_back(e);
    }
    optimizer.edges().clear();
    optimizer.vertices().clear();
    optimizer.setVerbose(false);

    // sort the edges in a way that inserting them makes sense
    sort(edges.begin(), edges.end(), IncrementalEdgesCompare());
    
    double cumTime = 0.;
    int vertexCount=0;
    int lastOptimizedVertexCount = 0;
    int lastVisUpdateVertexCount = 0;
    bool freshlyOptimized=false;
    bool firstRound = true;
    HyperGraph::VertexSet verticesAdded;
    HyperGraph::EdgeSet edgesAdded;
    for (vector<SparseOptimizer::Edge*>::iterator it = edges.begin(); it != edges.end(); ++it) {
      SparseOptimizer::Edge* e = *it;

      int doInit = 0;
      SparseOptimizer::Vertex* v1 = optimizer.vertex(e->vertices()[0]->id());
      SparseOptimizer::Vertex* v2 = optimizer.vertex(e->vertices()[1]->id());

      if (! v1) {
        SparseOptimizer::Vertex* v = v1 = dynamic_cast<SparseOptimizer::Vertex*>(e->vertices()[0]);
        bool v1Added = optimizer.addVertex(v);
        //cerr << "adding" << v->id() << "(" << v->dimension() << ")" << endl;
        assert(v1Added);
        if (! v1Added)
          cerr << "Error adding vertex " << v->id() << endl;
        else
          verticesAdded.insert(v);
        doInit = 1;
        if (v->dimension() == maxDim)
          vertexCount++;
      }

      if (! v2) {
        SparseOptimizer::Vertex* v = v2 = dynamic_cast<SparseOptimizer::Vertex*>(e->vertices()[1]);
        bool v2Added = optimizer.addVertex(v);
        //cerr << "adding" << v->id() << "(" << v->dimension() << ")" << endl;
        assert(v2Added);
        if (! v2Added)
          cerr << "Error adding vertex " << v->id() << endl;
        else
          verticesAdded.insert(v);
        doInit = 2;
        if (v->dimension() == maxDim)
          vertexCount++;
      }

      // adding the edge and initialization of the vertices
      {
        //cerr << " adding edge " << e->vertices()[0]->id() <<  " " << e->vertices()[1]->id() << endl;
        if (! optimizer.addEdge(e)) {
          cerr << "Unable to add edge " << e->vertices()[0]->id() << " -> " << e->vertices()[1]->id() << endl;
        } else {
          edgesAdded.insert(e);
        }

        if (doInit) {
          OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]);
          OptimizableGraph::Vertex* to   = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]);
          switch (doInit){
            case 1: // initialize v1 from v2
              {
                HyperGraph::VertexSet toSet;
                toSet.insert(to);
                if (e->initialEstimatePossible(toSet, from) > 0.) {
                  //cerr << "init: " 
                    //<< to->id() << "(" << to->dimension() << ") -> " 
                    //<< from->id() << "(" << from->dimension() << ") " << endl;
                   e->initialEstimate(toSet, from);
                } else {
                  assert(0 && "Added unitialized variable to the graph");
                }
                break;
              }
            case 2: 
              {
                HyperGraph::VertexSet fromSet;
                fromSet.insert(from);
                if (e->initialEstimatePossible(fromSet, to) > 0.) {
                  //cerr << "init: " 
                    //<< from->id() << "(" << from->dimension() << ") -> " 
                    //<< to->id() << "(" << to->dimension() << ") " << endl;
                  e->initialEstimate(fromSet, to);  
                } else {
                  assert(0 && "Added unitialized variable to the graph");
                }
                break;
              }
            default: cerr << "doInit wrong value\n"; 
          }

        }

      }

      freshlyOptimized=false;
      {
        //cerr << "Optimize" << endl;
        if (vertexCount - lastOptimizedVertexCount >= updateGraphEachN) {
          if (firstRound) {
            if (!optimizer.initializeOptimization()){
              cerr << "initialization failed" << endl;
              return 0;
            }
          } else {
            if (! optimizer.updateInitialization(verticesAdded, edgesAdded)) {
              cerr << "updating initialization failed" << endl;
              return 0;
            }
          }
          verticesAdded.clear();
          edgesAdded.clear();
          double ts = get_monotonic_time();
          int currentIt=optimizer.optimize(incIterations, !firstRound);
          double dts = get_monotonic_time() - ts;
          cumTime += dts;
          firstRound = false;
          //optimizer->setOptimizationTime(cumTime);
          if (verbose) {
            double chi2 = optimizer.chi2();
            cerr << "nodes= " << optimizer.vertices().size() << "\t edges= " << optimizer.edges().size() << "\t chi2= " << chi2
              << "\t time= " << dts << "\t iterations= " << currentIt <<  "\t cumTime= " << cumTime << endl;
          }
          lastOptimizedVertexCount = vertexCount;
          freshlyOptimized = true;

          if (guiOut) {
            if (vertexCount - lastVisUpdateVertexCount >= updateDisplayEveryN) {
              dumpEdges(cout, optimizer);
              lastVisUpdateVertexCount = vertexCount;
            }
          }

        }

        if (! verbose)
          cerr << ".";
      }
      
    } // for all edges

    if (! freshlyOptimized) {
      double ts = get_monotonic_time();
      int currentIt=optimizer.optimize(incIterations, !firstRound);
      double dts = get_monotonic_time() - ts;
      cumTime += dts;
      //optimizer->setOptimizationTime(cumTime);
      if (verbose) {
        double chi2 = optimizer.chi2();
        cerr << "nodes= " << optimizer.vertices().size() << "\t edges= " << optimizer.edges().size() << "\t chi2= " << chi2
          << "\t time= " << dts << "\t iterations= " << currentIt <<  "\t cumTime= " << cumTime << endl;
      }

    }

  } else {

    // BATCH optimization

    if (statsFile!=""){
      // allocate buffer for statistics;
      optimizer.setComputeBatchStatistics(true);
    }
    optimizer.initializeOptimization();
    optimizer.computeActiveErrors();
    double loadChi = optimizer.chi2();
    cerr << "Initial chi2 = " << FIXED(loadChi) << endl;

    if (initialGuess) {
      optimizer.computeInitialGuess();
    } else if (initialGuessOdometry) {
      EstimatePropagatorCostOdometry costFunction(&optimizer);
      optimizer.computeInitialGuess(costFunction);
    }
    double initChi = optimizer.chi2();

    signal(SIGINT, sigquit_handler);
    int result=optimizer.optimize(maxIterations);
    if (maxIterations > 0 && result==OptimizationAlgorithm::Fail){
      cerr << "Cholesky failed, result might be invalid" << endl;
    } else if (computeMarginals){
      std::vector<std::pair<int, int> > blockIndices;
      for (size_t i=0; i<optimizer.activeVertices().size(); i++){
        OptimizableGraph::Vertex* v=optimizer.activeVertices()[i];
        if (v->hessianIndex()>=0){
          blockIndices.push_back(make_pair(v->hessianIndex(), v->hessianIndex()));
        }
        if (v->hessianIndex()>0){
          blockIndices.push_back(make_pair(v->hessianIndex()-1, v->hessianIndex()));
        }
      }
      SparseBlockMatrix<MatrixXd> spinv;
      if (optimizer.computeMarginals(spinv, blockIndices)) {
        for (size_t i=0; i<optimizer.activeVertices().size(); i++){
          OptimizableGraph::Vertex* v=optimizer.activeVertices()[i];
          cerr << "Vertex id:" << v->id() << endl;
          if (v->hessianIndex()>=0){
            cerr << "inv block :" << v->hessianIndex() << ", " << v->hessianIndex()<< endl;
            cerr << *(spinv.block(v->hessianIndex(), v->hessianIndex()));
            cerr << endl;
          }
          if (v->hessianIndex()>0){
            cerr << "inv block :" << v->hessianIndex()-1 << ", " << v->hessianIndex()<< endl;
            cerr << *(spinv.block(v->hessianIndex()-1, v->hessianIndex()));
            cerr << endl;
          }
        }
      }
    }
    
    optimizer.computeActiveErrors();
    double finalChi=optimizer.chi2();

    if  (summaryFile!="") {
      PropertyMap summary;
      summary.makeProperty<StringProperty>("filename", inputFilename);
      summary.makeProperty<IntProperty>("n_vertices", optimizer.vertices().size());
      summary.makeProperty<IntProperty>("n_edges", optimizer.edges().size());
      
      int nLandmarks=0;
      int nPoses=0;
      int maxDim = *vertexDimensions.rbegin();
      for (HyperGraph::VertexIDMap::iterator it=optimizer.vertices().begin(); it!=optimizer.vertices().end(); it++){
	OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second);
	if (v->dimension() != maxDim) {
	  nLandmarks++;
	} else
	  nPoses++;
      }
      set<string> edgeTypes;
      for (HyperGraph::EdgeSet::iterator it=optimizer.edges().begin(); it!=optimizer.edges().end(); it++){
	edgeTypes.insert(Factory::instance()->tag(*it));
      }
      stringstream edgeTypesString;
      for (std::set<string>::iterator it=edgeTypes.begin(); it!=edgeTypes.end(); it++){
	edgeTypesString << *it << " ";
      }

      summary.makeProperty<IntProperty>("n_poses", nPoses);
      summary.makeProperty<IntProperty>("n_landmarks", nLandmarks);
      summary.makeProperty<StringProperty>("edge_types", edgeTypesString.str());
      summary.makeProperty<DoubleProperty>("load_chi", loadChi);
      summary.makeProperty<StringProperty>("solver", strSolver);
      summary.makeProperty<BoolProperty>("robustKernel", robustKernel.size() > 0);
      summary.makeProperty<DoubleProperty>("init_chi", initChi);
      summary.makeProperty<DoubleProperty>("final_chi", finalChi);
      summary.makeProperty<IntProperty>("maxIterations", maxIterations);
      summary.makeProperty<IntProperty>("realIterations", result);
      ofstream os;
      os.open(summaryFile.c_str(), ios::app);
      summary.writeToCSV(os);
    }
    

    if (statsFile!=""){
      cerr << "writing stats to file \"" << statsFile << "\" ... ";
      ofstream os(statsFile.c_str());
      const BatchStatisticsContainer& bsc = optimizer.batchStatistics();
      
      for (int i=0; i<maxIterations; i++) {
        os << bsc[i] << endl;
      }
      cerr << "done." << endl;
    }

  }

  // saving again
  if (gnudump.size() > 0) {
    bool gnuPlotStatus = saveGnuplot(gnudump, optimizer);
    if (! gnuPlotStatus) {
      cerr << "Error while writing gnuplot files" << endl;
    }
  }

  if (outputfilename.size() > 0) {
    if (outputfilename == "-") {
      cerr << "saving to stdout";
      optimizer.save(cout);
    } else {
      cerr << "saving " << outputfilename << " ... ";
      optimizer.save(outputfilename.c_str());
    }
    cerr << "done." << endl;
  }

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

  return 0;
}