113 string inputFilename;
115 string outputfilename;
128 int updateGraphEachN = 10;
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);
169 Factory::instance()->printRegisteredTypes(cout,
true);
177 if (loadLookup.size() > 0) {
180 if (inputFilename.size() == 0) {
181 cerr <<
"No input data specified" << endl;
183 }
else if (inputFilename ==
"-") {
184 cerr <<
"Read input from stdin" << endl;
185 if (!optimizer.
load(cin)) {
186 cerr <<
"Error loading graph" << endl;
190 cerr <<
"Read input from " << inputFilename << endl;
191 ifstream ifs(inputFilename.c_str());
193 cerr <<
"Failed to open file" << endl;
196 if (!optimizer.
load(ifs)) {
197 cerr <<
"Error loading graph" << endl;
201 cerr <<
"Loaded " << optimizer.
vertices().size() <<
" vertices" << endl;
202 cerr <<
"Loaded " << optimizer.
edges().size() <<
" edges" << endl;
204 if (optimizer.
vertices().size() == 0) {
205 cerr <<
"Graph contains no vertices" << endl;
210 int incIterations = maxIterations;
213 cerr <<
"# incremental setttings" << endl;
214 cerr <<
"#\t solve every " << updateGraphEachN << endl;
215 cerr <<
"#\t iterations " << incIterations << endl;
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());
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);
228 optimizer.
edges().clear();
236 int lastOptimizedVertexCount = 0;
237 bool addNextEdge=
true;
238 bool freshlyOptimized=
false;
241 for (vector<SparseOptimizer::Edge*>::iterator it = edges.begin(); it != edges.end(); ++it) {
242 SparseOptimizer::Edge* e = *it;
245 if (addNextEdge && !optimizer.
vertices().empty()){
246 int idMax = (max)(e->vertices()[0]->id(), e->vertices()[1]->id());
247 if (maxInGraph < idMax && ! freshlyOptimized){
257 SparseOptimizer::Vertex* v1 = optimizer.
vertex(e->vertices()[0]->id());
258 SparseOptimizer::Vertex* v2 = optimizer.
vertex(e->vertices()[1]->id());
259 if (! v1 && addNextEdge) {
261 SparseOptimizer::Vertex* v =
dynamic_cast<SparseOptimizer::Vertex*
>(e->vertices()[0]);
263 maxInGraph = (max)(maxInGraph, v->id());
267 cerr <<
"Error adding vertex " << v->id() << endl;
269 verticesAdded.insert(v);
271 if (v->dimension() == maxDim)
274 if (v->dimension() == 3) {
275 cout <<
"ADD VERTEX_XYT " << v->id() <<
";" << endl;
277 else if (v->dimension() == 6) {
278 cout <<
"ADD VERTEX_XYZRPY " << v->id() <<
";" << endl;
283 if (! v2 && addNextEdge) {
284 SparseOptimizer::Vertex* v =
dynamic_cast<SparseOptimizer::Vertex*
>(e->vertices()[1]);
287 maxInGraph = (max)(maxInGraph, v->id());
291 cerr <<
"Error adding vertex " << v->id() << endl;
293 verticesAdded.insert(v);
295 if (v->dimension() == maxDim)
298 if (v->dimension() == 3) {
299 cout <<
"ADD VERTEX_XYT " << v->id() <<
";" << endl;
301 else if (v->dimension() == 6) {
302 cout <<
"ADD VERTEX_XYZRPY " << v->id() <<
";" << endl;
308 static int edgeCnt = 0;
310 if (e->dimension() == 3) {
311 double* information = e->informationData();
313 e->getMeasurementData(meas);
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];
322 else if (e->dimension() == 6) {
324 cerr <<
"NOT IMPLEMENTED YET" << endl;
326 static bool firstEdge =
true;
329 cout <<
"FIX 0;" << endl;
334 cerr <<
"Unable to add edge " << e->vertices()[0]->id() <<
" -> " << e->vertices()[1]->id() << endl;
338 freshlyOptimized=
false;
341 if (vertexCount - lastOptimizedVertexCount >= updateGraphEachN) {
342 cout <<
"SOLVE_STATE;" << endl;
343 cout <<
"QUERY_STATE;" << endl;
344 lastOptimizedVertexCount = vertexCount;
348 freshlyOptimized=
true;
Command line parsing of argc and argv.
void loadStandardTypes(DlWrapper &dlTypesWrapper, int argc, char **argv)
std::set< Vertex * > VertexSet
const VertexIDMap & vertices() const
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
bool parseArgs(int argc, char **argv, bool exitOnError=true)
void setVerbose(bool verbose)
const EdgeSet & edges() const
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.
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.
virtual bool addEdge(HyperGraph::Edge *e)
virtual bool addVertex(HyperGraph::Vertex *v, Data *userData)