g2o
Classes | Functions | Variables
generate_commands.cpp File Reference
#include <signal.h>
#include <iostream>
#include <iomanip>
#include <string>
#include <fstream>
#include <sstream>
#include <algorithm>
#include <cassert>
#include "g2o/apps/g2o_cli/dl_wrapper.h"
#include "g2o/apps/g2o_cli/g2o_common.h"
#include "g2o/core/estimate_propagator.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/core/factory.h"
#include "g2o/core/optimization_algorithm_factory.h"
#include "g2o/core/hyper_dijkstra.h"
#include "g2o/stuff/macros.h"
#include "g2o/stuff/color_macros.h"
#include "g2o/stuff/command_args.h"
#include "g2o/stuff/filesys_tools.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

SparseOptimizer::Method str2method (const std::string &strMethod_)
 
void sigquit_handler (int sig)
 
int main (int argc, char **argv)
 

Variables

static bool hasToStop =false
 

Function Documentation

int main ( int  argc,
char **  argv 
)

Definition at line 109 of file generate_commands.cpp.

References g2o::OptimizableGraph::addEdge(), g2o::OptimizableGraph::addVertex(), g2o::HyperGraph::edges(), g2o::OptimizableGraph::load(), g2o::loadStandardTypes(), g2o::CommandArgs::param(), g2o::CommandArgs::paramLeftOver(), g2o::CommandArgs::parseArgs(), g2o::OptimizableGraph::setRenamedTypesFromString(), g2o::SparseOptimizer::setVerbose(), g2o::OptimizableGraph::vertex(), vertices, and g2o::HyperGraph::vertices().

110 {
111  int maxIterations;
112  bool verbose;
113  string inputFilename;
114  string gnudump;
115  string outputfilename;
116  string strMethod;
117  string strSolver;
118  string loadLookup;
119  bool initialGuess;
120  bool marginalize;
121  bool listTypes;
122  bool listSolvers;
123  bool incremental;
124  bool guiOut;
125  bool robustKernel;
126  double huberWidth;
127  double lambdaInit;
128  int updateGraphEachN = 10;
129  string statsFile;
130  string dummy;
131  // command line parsing
132  CommandArgs arg;
133  arg.param("i", maxIterations, 5, "perform n iterations");
134  arg.param("v", verbose, false, "verbose output of the optimization process");
135  arg.param("guess", initialGuess, false, "initial guess based on spanning tree");
136  arg.param("inc", incremental, false, "run incremetally");
137  arg.param("update", updateGraphEachN, 10, "updates after x odometry nodes, (default: 10)");
138  arg.param("guiout", guiOut, false, "gui output while running incrementally");
139  arg.param("lambdaInit", lambdaInit, 0, "user specified lambda init for levenberg");
140  arg.param("marginalize", marginalize, false, "on or off");
141  arg.param("method", strMethod, "Gauss", "Gauss or Levenberg");
142  arg.param("gnudump", gnudump, "", "dump to gnuplot data file");
143  arg.param("robustKernel", robustKernel, false, "use robust error functions");
144  arg.param("huberWidth", huberWidth, -1., "width for the robust Huber Kernel (only if robustKernel)");
145  arg.param("o", outputfilename, "", "output final version of the graph");
146  arg.param("solver", strSolver, "var", "specify which solver to use underneat\n\t {var, fix3_2, fix6_3, fix_7_3}");
147  arg.param("solverlib", dummy, "", "specify a solver library which will be loaded");
148  arg.param("typeslib", dummy, "", "specify a types library which will be loaded");
149  arg.param("stats", statsFile, "", "specify a file for the statistics");
150  arg.param("listTypes", listTypes, false, "list the registered types");
151  arg.param("listSolvers", listSolvers, false, "list the available solvers");
152  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");
153  arg.paramLeftOver("graph-input", inputFilename, "", "graph file which will be processed", true);
154 
155  arg.parseArgs(argc, argv);
156 
157  // registering all the types from the libraries
158  DlWrapper dlTypesWrapper;
159  loadStandardTypes(dlTypesWrapper, argc, argv);
160 
161  // register all the solvers
162  //OptimizationAlgorithmFactory* solverFactory = OptimizationAlgorithmFactory::instance();
163  //DlWrapper dlSolverWrapper;
164  //loadStandardSolver(dlSolverWrapper, argc, argv);
165  //if (listSolvers)
166  //solverFactory->listSolvers(cerr);
167 
168  if (listTypes) {
169  Factory::instance()->printRegisteredTypes(cout, true);
170  }
171 
172  SparseOptimizer optimizer;
173  //optimizer.setVerbose(verbose);
174  //optimizer.setForceStopFlag(&hasToStop);
175 
176  // Loading the input data
177  if (loadLookup.size() > 0) {
178  optimizer.setRenamedTypesFromString(loadLookup);
179  }
180  if (inputFilename.size() == 0) {
181  cerr << "No input data specified" << endl;
182  return 0;
183  } else if (inputFilename == "-") {
184  cerr << "Read input from stdin" << endl;
185  if (!optimizer.load(cin)) {
186  cerr << "Error loading graph" << endl;
187  return 2;
188  }
189  } else {
190  cerr << "Read input from " << inputFilename << endl;
191  ifstream ifs(inputFilename.c_str());
192  if (!ifs) {
193  cerr << "Failed to open file" << endl;
194  return 1;
195  }
196  if (!optimizer.load(ifs)) {
197  cerr << "Error loading graph" << endl;
198  return 2;
199  }
200  }
201  cerr << "Loaded " << optimizer.vertices().size() << " vertices" << endl;
202  cerr << "Loaded " << optimizer.edges().size() << " edges" << endl;
203 
204  if (optimizer.vertices().size() == 0) {
205  cerr << "Graph contains no vertices" << endl;
206  return 1;
207  }
208 
209  if (1) {
210  int incIterations = maxIterations;
211  int maxDim = 0;
212 
213  cerr << "# incremental setttings" << endl;
214  cerr << "#\t solve every " << updateGraphEachN << endl;
215  cerr << "#\t iterations " << incIterations << endl;
216 
217  SparseOptimizer::VertexIDMap vertices = optimizer.vertices();
218  for (SparseOptimizer::VertexIDMap::const_iterator it = vertices.begin(); it != vertices.end(); ++it) {
219  const SparseOptimizer::Vertex* v = static_cast<const SparseOptimizer::Vertex*>(it->second);
220  maxDim = (max)(maxDim, v->dimension());
221  }
222 
223  vector<SparseOptimizer::Edge*> edges;
224  for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
225  SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it);
226  edges.push_back(e);
227  }
228  optimizer.edges().clear();
229  optimizer.vertices().clear();
230  optimizer.setVerbose(false);
231 
232  // sort the edges in a way that inserting them makes sense
233  sort(edges.begin(), edges.end(), IncrementalEdgesCompare());
234 
235  int vertexCount=0;
236  int lastOptimizedVertexCount = 0;
237  bool addNextEdge=true;
238  bool freshlyOptimized=false;
239  HyperGraph::VertexSet verticesAdded;
240  int maxInGraph = -1;
241  for (vector<SparseOptimizer::Edge*>::iterator it = edges.begin(); it != edges.end(); ++it) {
242  SparseOptimizer::Edge* e = *it;
243  bool optimize=false;
244 
245  if (addNextEdge && !optimizer.vertices().empty()){
246  int idMax = (max)(e->vertices()[0]->id(), e->vertices()[1]->id());
247  if (maxInGraph < idMax && ! freshlyOptimized){
248  addNextEdge=false;
249  optimize=true;
250  } else {
251  addNextEdge=true;
252  optimize=false;
253  }
254  }
255 
256  int doInit = 0;
257  SparseOptimizer::Vertex* v1 = optimizer.vertex(e->vertices()[0]->id());
258  SparseOptimizer::Vertex* v2 = optimizer.vertex(e->vertices()[1]->id());
259  if (! v1 && addNextEdge) {
260  //cerr << " adding vertex " << it->id1 << endl;
261  SparseOptimizer::Vertex* v = dynamic_cast<SparseOptimizer::Vertex*>(e->vertices()[0]);
262  bool v1Added = optimizer.addVertex(v);
263  maxInGraph = (max)(maxInGraph, v->id());
264  //cerr << "adding" << v->id() << "(" << v->dimension() << ")" << endl;
265  assert(v1Added);
266  if (! v1Added)
267  cerr << "Error adding vertex " << v->id() << endl;
268  else
269  verticesAdded.insert(v);
270  doInit = 1;
271  if (v->dimension() == maxDim)
272  vertexCount++;
273 
274  if (v->dimension() == 3) {
275  cout << "ADD VERTEX_XYT " << v->id() << ";" << endl;
276  }
277  else if (v->dimension() == 6) {
278  cout << "ADD VERTEX_XYZRPY " << v->id() << ";" << endl;
279  }
280 
281  }
282 
283  if (! v2 && addNextEdge) {
284  SparseOptimizer::Vertex* v = dynamic_cast<SparseOptimizer::Vertex*>(e->vertices()[1]);
285  //cerr << " adding vertex " << v->id() << endl;
286  bool v2Added = optimizer.addVertex(v);
287  maxInGraph = (max)(maxInGraph, v->id());
288  //cerr << "adding" << v->id() << "(" << v->dimension() << ")" << endl;
289  assert(v2Added);
290  if (! v2Added)
291  cerr << "Error adding vertex " << v->id() << endl;
292  else
293  verticesAdded.insert(v);
294  doInit = 2;
295  if (v->dimension() == maxDim)
296  vertexCount++;
297 
298  if (v->dimension() == 3) {
299  cout << "ADD VERTEX_XYT " << v->id() << ";" << endl;
300  }
301  else if (v->dimension() == 6) {
302  cout << "ADD VERTEX_XYZRPY " << v->id() << ";" << endl;
303  }
304  }
305 
306  if (addNextEdge){
307 
308  static int edgeCnt = 0;
309 
310  if (e->dimension() == 3) {
311  double* information = e->informationData();
312  double meas[3];
313  e->getMeasurementData(meas);
314  //ADD EDGE_XYT 1 1 2 .1 .2 .3 1 0 0 1 0 1;
315  cout << "ADD EDGE_XYT " << edgeCnt++ << " " << e->vertices()[0]->id() << " " << e->vertices()[1]->id() << " "
316  << meas[0] << " " << meas[1] << " " << meas[2];
317  for (int i = 0; i < 3; ++i)
318  for (int j = i; j < 3; ++j)
319  cout << " " << information[i*3 + j];
320  cout << ";" << endl;
321  }
322  else if (e->dimension() == 6) {
323  // TODO convert to EULER angles
324  cerr << "NOT IMPLEMENTED YET" << endl;
325  }
326  static bool firstEdge = true;
327  if (firstEdge) {
328  firstEdge = false;
329  cout << "FIX 0;" << endl;
330  }
331 
332  //cerr << " adding edge " << e->vertices()[0]->id() << " " << e->vertices()[1]->id() << endl;
333  if (! optimizer.addEdge(e)) {
334  cerr << "Unable to add edge " << e->vertices()[0]->id() << " -> " << e->vertices()[1]->id() << endl;
335  }
336  }
337 
338  freshlyOptimized=false;
339  if (optimize){
340  //cerr << "Optimize" << endl;
341  if (vertexCount - lastOptimizedVertexCount >= updateGraphEachN) {
342  cout << "SOLVE_STATE;" << endl;
343  cout << "QUERY_STATE;" << endl;
344  lastOptimizedVertexCount = vertexCount;
345  }
346 
347  addNextEdge=true;
348  freshlyOptimized=true;
349  it--;
350  }
351 
352  } // for all edges
353 
354  }
355 
356  return 0;
357 }
Command line parsing of argc and argv.
Definition: command_args.h:46
void loadStandardTypes(DlWrapper &dlTypesWrapper, int argc, char **argv)
Definition: g2o_common.cpp:96
std::set< Vertex * > VertexSet
Definition: hyper_graph.h:136
const VertexIDMap & vertices() const
Definition: hyper_graph.h:225
Vertex * vertex(int id)
returns the vertex number id appropriately casted
Protocol The SLAM executable accepts such as solving the and retrieving or vertices in the explicitly state the reprensentation poses are represented by poses by VERTEX_XYZRPY In the Quaternions and other representations could be but note that it is up to the SLAM algorithm to choose the internal representation of the angles The keyword is followed by a unique vertex ID and an optional initialization of the or edges in the explicitly state the type of the constraint pose constraints are given by pose constraints by EDGE_XYZRPY The keyword is followed by a unique edge the IDs of the referenced vertices
Definition: protocol.txt:7
bool parseArgs(int argc, char **argv, bool exitOnError=true)
void setVerbose(bool verbose)
const EdgeSet & edges() const
Definition: hyper_graph.h:230
void paramLeftOver(const std::string &name, std::string &p, const std::string &defValue, const std::string &desc, bool optional=false)
void param(const std::string &name, bool &p, bool defValue, const std::string &desc)
Loading libraries during run-time.
Definition: dl_wrapper.h:44
void setRenamedTypesFromString(const std::string &types)
virtual bool load(std::istream &is, bool createEdges=true)
load the graph from a stream. Uses the Factory singleton for creating the vertices and edges...
Sort Edges for inserting them sequentially.
Definition: g2o.cpp:67
virtual bool addEdge(HyperGraph::Edge *e)
virtual bool addVertex(HyperGraph::Vertex *v, Data *userData)
void sigquit_handler ( int  sig)

Definition at line 97 of file generate_commands.cpp.

References __PRETTY_FUNCTION__, and hasToStop.

98 {
99  if (sig == SIGINT) {
100  hasToStop = 1;
101  static int cnt = 0;
102  if (cnt++ == 2) {
103  cerr << __PRETTY_FUNCTION__ << " forcing exit" << endl;
104  exit(1);
105  }
106  }
107 }
#define __PRETTY_FUNCTION__
Definition: macros.h:89
static bool hasToStop
SparseOptimizer::Method str2method ( const std::string &  strMethod_)

Definition at line 83 of file generate_commands.cpp.

References g2o::strToLower().

83  {
84  string strMethod = strToLower(strMethod_);
85  if (strMethod=="gauss") {
86  cerr << "# Doing Gauss" << endl;
87  return SparseOptimizer::GaussNewton;
88  }
89  if (strMethod=="levenberg") {
90  cerr << "# Doing Levenberg-Marquardt" << endl;
91  return SparseOptimizer::LevenbergMarquardt;
92  }
93  cerr << "# Unknown optimization method: " << strMethod << ", setting default to Levenberg" << endl;
94  return SparseOptimizer::LevenbergMarquardt;
95 }
std::string strToLower(const std::string &s)

Variable Documentation

bool hasToStop =false
static

Definition at line 52 of file generate_commands.cpp.

Referenced by sigquit_handler().