generate_commands.cpp File Reference

#include <algorithm>
#include <cassert>
#include <csignal>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <string>
#include "g2o/core/factory.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/stuff/color_macros.h"
#include "g2o/stuff/command_args.h"
#include "g2o/stuff/filesys_tools.h"
#include "g2o/stuff/macros.h"
#include "g2o/stuff/string_tools.h"
#include "g2o/stuff/timeutil.h"

Include dependency graph for generate_commands.cpp:

Go to the source code of this file.

Classes
struct	IncrementalEdgesCompare
	Sort Edges for inserting them sequentially. More...

Functions
void	sigquit_handler (int sig)
int	main (int argc, char **argv)

Variables
static bool	hasToStop = false

Function Documentation

main()

int main	(	int	argc,
		char **	argv
	)

Definition at line 88 of file generate_commands.cpp.

                                {
  string inputFilename;
  string loadLookup;
  bool listTypes;
  int updateGraphEachN = 10;
  string dummy;
  // command line parsing
  CommandArgs arg;
  arg.param("update", updateGraphEachN, 10,
            "updates after x odometry nodes, (default: 10)");
  arg.param("listTypes", listTypes, false, "list the registered types");
  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.paramLeftOver("graph-input", inputFilename, "",
                    "graph file which will be processed", true);
 
  arg.parseArgs(argc, argv);
 
  if (listTypes) {
    Factory::instance()->printRegisteredTypes(cout, true);
  }
 
  SparseOptimizer optimizer;
 
  // 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;
  }
 
  if (1) {
    int maxDim = 0;
 
    cerr << "# incremental settings" << endl;
    cerr << "#\t solve every " << updateGraphEachN << 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());
 
    int vertexCount = 0;
    int lastOptimizedVertexCount = 0;
    bool addNextEdge = true;
    bool freshlyOptimized = false;
    HyperGraph::VertexSet verticesAdded;
    int maxInGraph = -1;
    for (vector<SparseOptimizer::Edge*>::iterator it = edges.begin();
         it != edges.end(); ++it) {
      SparseOptimizer::Edge* e = *it;
      bool optimize = false;
 
      if (addNextEdge && !optimizer.vertices().empty()) {
        int idMax = (max)(e->vertices()[0]->id(), e->vertices()[1]->id());
        if (maxInGraph < idMax && !freshlyOptimized) {
          addNextEdge = false;
          optimize = true;
        } else {
          addNextEdge = true;
          optimize = false;
        }
      }
 
      SparseOptimizer::Vertex* v1 = optimizer.vertex(e->vertices()[0]->id());
      SparseOptimizer::Vertex* v2 = optimizer.vertex(e->vertices()[1]->id());
      if (!v1 && addNextEdge) {
        // cerr << " adding vertex " << it->id1 << endl;
        SparseOptimizer::Vertex* v =
            dynamic_cast<SparseOptimizer::Vertex*>(e->vertices()[0]);
        bool v1Added = optimizer.addVertex(v);
        maxInGraph = (max)(maxInGraph, v->id());
        // cerr << "adding" << v->id() << "(" << v->dimension() << ")" << endl;
        assert(v1Added);
        if (!v1Added)
          cerr << "Error adding vertex " << v->id() << endl;
        else
          verticesAdded.insert(v);
        if (v->dimension() == maxDim) vertexCount++;
 
        if (v->dimension() == 3) {
          cout << "ADD VERTEX_XYT " << v->id() << ";" << endl;
        } else if (v->dimension() == 6) {
          cout << "ADD VERTEX_XYZRPY " << v->id() << ";" << endl;
        }
      }
 
      if (!v2 && addNextEdge) {
        SparseOptimizer::Vertex* v =
            dynamic_cast<SparseOptimizer::Vertex*>(e->vertices()[1]);
        // cerr << " adding vertex " << v->id() << endl;
        bool v2Added = optimizer.addVertex(v);
        maxInGraph = (max)(maxInGraph, v->id());
        // cerr << "adding" << v->id() << "(" << v->dimension() << ")" << endl;
        assert(v2Added);
        if (!v2Added)
          cerr << "Error adding vertex " << v->id() << endl;
        else
          verticesAdded.insert(v);
        if (v->dimension() == maxDim) vertexCount++;
 
        if (v->dimension() == 3) {
          cout << "ADD VERTEX_XYT " << v->id() << ";" << endl;
        } else if (v->dimension() == 6) {
          cout << "ADD VERTEX_XYZRPY " << v->id() << ";" << endl;
        }
      }
 
      if (addNextEdge) {
        if (e->dimension() == 3) {
          static int edgeCnt = 0;
          double* information = e->informationData();
          double meas[3];
          e->getMeasurementData(meas);
          // ADD EDGE_XYT 1 1 2 .1 .2 .3 1 0 0 1 0 1;
          cout << "ADD EDGE_XYT " << edgeCnt++ << " " << e->vertices()[0]->id()
               << " " << e->vertices()[1]->id() << " " << meas[0] << " "
               << meas[1] << " " << meas[2];
          for (int i = 0; i < 3; ++i)
            for (int j = i; j < 3; ++j) cout << " " << information[i * 3 + j];
          cout << ";" << endl;
        } else if (e->dimension() == 6) {
          // TODO convert to EULER angles
          cerr << "NOT IMPLEMENTED YET" << endl;
        }
        static bool firstEdge = true;
        if (firstEdge) {
          firstEdge = false;
          cout << "FIX 0;" << endl;
        }
 
        // 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;
        }
      }
 
      freshlyOptimized = false;
      if (optimize) {
        // cerr << "Optimize" << endl;
        if (vertexCount - lastOptimizedVertexCount >= updateGraphEachN) {
          cout << "SOLVE_STATE;" << endl;
          cout << "QUERY_STATE;" << endl;
          lastOptimizedVertexCount = vertexCount;
        }
 
        addNextEdge = true;
        freshlyOptimized = true;
        --it;
      }
 
    }  // for all edges
  }
 
  return 0;
}

References g2o::OptimizableGraph::addEdge(), g2o::OptimizableGraph::addVertex(), g2o::HyperGraph::edges(), g2o::HyperGraph::Vertex::id(), g2o::Factory::instance(), g2o::OptimizableGraph::load(), g2o::CommandArgs::param(), g2o::CommandArgs::paramLeftOver(), g2o::CommandArgs::parseArgs(), g2o::Factory::printRegisteredTypes(), g2o::OptimizableGraph::setRenamedTypesFromString(), g2o::SparseOptimizer::setVerbose(), g2o::OptimizableGraph::vertex(), g2o::HyperGraph::Edge::vertices(), and g2o::HyperGraph::vertices().

sigquit_handler()

void sigquit_handler

(

int

sig

)

Definition at line 77 of file generate_commands.cpp.

                              {
  if (sig == SIGINT) {
    hasToStop = 1;
    static int cnt = 0;
    if (cnt++ == 2) {
      cerr << __PRETTY_FUNCTION__ << " forcing exit" << endl;
      exit(1);
    }
  }
}

References __PRETTY_FUNCTION__, and hasToStop.

Variable Documentation

hasToStop

bool hasToStop = false

static

Definition at line 45 of file generate_commands.cpp.

Referenced by sigquit_handler().