g2o
Classes | Functions | Variables
g2o-unfold.cpp File Reference
#include <signal.h>
#include <iostream>
#include <string>
#include <fstream>
#include <sstream>
#include <algorithm>
#include "g2o/types/slam2d/types_three_dof.h"
#include "g2o/types/sba/types_six_dof_expmap.h"
#include "g2o/types/sim3/types_seven_dof_expmap.h"
#include "g2o/types/slam3d/types_six_dof_quat.h"
#include "g2o/types/sba/types_sba.h"
#include "g2o/core/estimate_propagator.h"
#include "g2o/core/sparse_optimizer.h"
#include "g2o/stuff/macros.h"
#include "g2o/stuff/command_args.h"
#include "g2o/stuff/filesys_tools.h"
#include "g2o/stuff/string_tools.h"
#include "tools.h"
#include "g2o/core/block_solver.h"
#include "g2o/solvers/cholmod/linear_solver_cholmod.h"
#include "g2o/solvers/csparse/linear_solver_csparse.h"
#include "g2o/solvers/pcg/linear_solver_pcg.h"
Include dependency graph for g2o-unfold.cpp:

Go to the source code of this file.

Classes

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

Functions

void updateDisplay (ostream &os, const SparseOptimizer &optimizer)
 
SparseOptimizer::Method str2method (const std::string &strMethod)
 
template<typename T >
T::LinearSolverType * allocateLinearSolver (bool useCholmod)
 
Solver * str2solver (const std::string &strSolver_, SparseOptimizer *opt)
 
void gnudump_edges (string gnudump, string file_suffix, HyperGraph::EdgeSet::const_iterator begin, HyperGraph::EdgeSet::const_iterator end, bool dumpEdges, bool dumpFeatures)
 
void gnudump_features (string gnudump, string file_suffix, HyperGraph::EdgeSet::const_iterator begin, HyperGraph::EdgeSet::const_iterator end)
 
void sigquit_handler (int q __attribute__((unused)))
 
int main (int argc, char **argv)
 

Variables

bool hasToStop =false
 

Function Documentation

template<typename T >
T::LinearSolverType* allocateLinearSolver ( bool  useCholmod)

Definition at line 153 of file g2o-unfold.cpp.

154 {
155  if (useCholmod) {
156 #ifdef GO_CHOLMOD_SUPPORT
157  return new LinearSolverCholmod<typename T::PoseMatrixType>;
158 #else
159  return 0;
160 #endif
161  } else {
162  return new LinearSolverCSparse<typename T::PoseMatrixType>;
163  }
164 }
g2o::LinearSolverCSparse
linear solver which uses CSparse
Definition: linear_solver_csparse.h:71
g2o::LinearSolverCholmod
basic solver for Ax = b which has to reimplemented for different linear algebra libraries ...
Definition: linear_solver_cholmod.h:76
void gnudump_edges ( string  gnudump,
string  file_suffix,
HyperGraph::EdgeSet::const_iterator  begin,
HyperGraph::EdgeSet::const_iterator  end,
bool  dumpEdges,
bool  dumpFeatures 
)

Definition at line 220 of file g2o-unfold.cpp.

References g2o::OptimizableGraph::Edge::chi2(), g2o::OptimizableGraph::Edge::computeError(), g2o::BaseVertex< D, T >::estimate(), g2o::formatString(), g2o::getFileExtension(), g2o::getPureFilename(), g2o::OptimizableGraph::Edge::robustKernel(), and g2o::SE2::toVector().

Referenced by main().

225  {
226 
227  // -------
228  if (gnudump.size() > 0) {
229  string baseName = getPureFilename(gnudump);
230  string extension = getFileExtension(gnudump);
231  string newFilename = formatString("%s_%s.%s", baseName.c_str(), file_suffix.c_str(), extension.c_str());
232 
233  cerr << "Gnudump (" << newFilename << ")... ";
234 
235 
236  ofstream fout(newFilename.c_str());
237 
238  for (HyperGraph::EdgeSet::const_iterator eit = begin; eit != end; ++eit) {
239 
240  OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(const_cast<HyperGraph::Edge*>(*eit));
241  e->computeError();
242  if(e->robustKernel())
243  e->robustifyError();
244  double edgeChi2 = e->chi2();
245 
246  const VertexSE2* from = dynamic_cast<const VertexSE2*>((*eit)->vertex(0));
247  const VertexSE2* to = dynamic_cast<const VertexSE2*>((*eit)->vertex(1));
248 
249  if(dumpEdges) {
250  if (from && to) {
251  Vector3d p1 = from->estimate().toVector();
252  Vector3d p2 = to->estimate().toVector();
253  fout << p1[0] << " " << p1[1] << " " << p1[2] << " " << edgeChi2 << " " << endl;
254  fout << p2[0] << " " << p2[1] << " " << p1[2] << " " << edgeChi2 << " " << endl;
255  fout << endl;
256  continue;
257  }
258  }
259 
260  if(dumpFeatures) {
261  const VertexPointXY* tox = dynamic_cast<const VertexPointXY*>((*eit)->vertex(1));
262  if (from && tox) {
263  Vector3d p1 = from->estimate().toVector();
264  Vector2d p2 = tox->estimate();
265  fout << p1[0] << " " << p1[1] << " " << p1[2] << " " << edgeChi2 << " " << endl;
266  fout << p2[0] << " " << p2[1] << " " << edgeChi2 << " " << endl;
267  fout << endl;
268  continue;
269  }
270  }
271 
272 
273  }
274  fout.close();
275  cerr << " done" << endl;
276  }
277 }
g2o::getFileExtension
std::string getFileExtension(const std::string &filename)
Definition: filesys_tools.cpp:60
g2o::SE2::toVector
Vector3D toVector() const
convert to a 3D vector (x, y, theta)
Definition: se2.h:108
g2o::dumpEdges
bool dumpEdges(std::ostream &os, const OptimizableGraph &optimizer)
Definition: output_helper.cpp:180
g2o::OptimizableGraph::Edge::chi2
virtual double chi2() const =0
computes the chi2 based on the cached error value, only valid after computeError has been called...
g2o::VertexSE2
2D pose Vertex, (x,y,theta)
Definition: vertex_se2.h:41
g2o::formatString
std::string formatString(const char *fmt,...)
Definition: string_tools.cpp:95
g2o::OptimizableGraph::Edge::robustKernel
RobustKernel * robustKernel() const
if NOT NULL, error of this edge will be robustifed with the kernel
Definition: optimizable_graph.h:383
g2o::OptimizableGraph::Edge::computeError
virtual void computeError()=0
g2o::getPureFilename
std::string getPureFilename(const std::string &filename)
Definition: filesys_tools.cpp:69
g2o::BaseVertex::estimate
const EstimateType & estimate() const
return the current estimate of the vertex
Definition: base_vertex.h:99
void gnudump_features ( string  gnudump,
string  file_suffix,
HyperGraph::EdgeSet::const_iterator  begin,
HyperGraph::EdgeSet::const_iterator  end 
)

Definition at line 279 of file g2o-unfold.cpp.

References g2o::OptimizableGraph::Edge::chi2(), g2o::OptimizableGraph::Edge::computeError(), g2o::BaseVertex< D, T >::estimate(), g2o::formatString(), g2o::getFileExtension(), g2o::getPureFilename(), and g2o::OptimizableGraph::Edge::robustKernel().

Referenced by main().

282  {
283 
284  // -------
285  if (gnudump.size() > 0) {
286  string baseName = getPureFilename(gnudump);
287  string extension = getFileExtension(gnudump);
288  string newFilename = formatString("%s_%s.%s", baseName.c_str(), file_suffix.c_str(), extension.c_str());
289 
290  cerr << "Gnudump (" << newFilename << ")... ";
291 
292 
293  ofstream fout(newFilename.c_str());
294 
295  for (HyperGraph::EdgeSet::const_iterator eit = begin; eit != end; ++eit) {
296 
297  OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*eit);
298  e->computeError();
299  if(e->robustKernel())
300  e->robustifyError();
301  double edgeChi2 = e->chi2();
302 
303  const VertexPointXY* tox = dynamic_cast<const VertexPointXY*>((*eit)->vertex(1));
304  if (tox) {
305  Vector2d p2 = tox->estimate();
306  fout << p2[0] << " " << p2[1] << " " << edgeChi2 << " " << endl;
307  continue;
308  }
309  }
310  fout.close();
311  cerr << " done" << endl;
312  }
313 }
g2o::getFileExtension
std::string getFileExtension(const std::string &filename)
Definition: filesys_tools.cpp:60
g2o::OptimizableGraph::Edge::chi2
virtual double chi2() const =0
computes the chi2 based on the cached error value, only valid after computeError has been called...
g2o::formatString
std::string formatString(const char *fmt,...)
Definition: string_tools.cpp:95
g2o::OptimizableGraph::Edge::robustKernel
RobustKernel * robustKernel() const
if NOT NULL, error of this edge will be robustifed with the kernel
Definition: optimizable_graph.h:383
g2o::OptimizableGraph::Edge::computeError
virtual void computeError()=0
g2o::getPureFilename
std::string getPureFilename(const std::string &filename)
Definition: filesys_tools.cpp:69
g2o::BaseVertex::estimate
const EstimateType & estimate() const
return the current estimate of the vertex
Definition: base_vertex.h:99
int main ( int  argc,
char **  argv 
)

Definition at line 324 of file g2o-unfold.cpp.

References g2o::OptimizableGraph::addEdge(), g2o::OptimizableGraph::addVertex(), g2o::OptimizableGraph::Edge::chi2(), g2o::OptimizableGraph::chi2(), g2o::OptimizableGraph::Edge::computeError(), g2o::OptimizableGraph::Vertex::dimension(), g2o::HyperGraph::Vertex::edges(), g2o::HyperGraph::edges(), g2o::findConnectedEdgesWithCostLimit(), g2o::OptimizableGraph::Vertex::fixed(), g2o::get_monotonic_time(), gnudump_edges(), gnudump_features(), g2o::SparseOptimizer::initializeOptimization(), g2o::OptimizableGraph::load(), g2o::OptimizableGraph::Vertex::marginalized(), g2o::SparseOptimizer::optimize(), g2o::CommandArgs::param(), g2o::CommandArgs::paramLeftOver(), g2o::CommandArgs::parseArgs(), g2o::OptimizableGraph::Edge::robustKernel(), g2o::OptimizableGraph::save(), g2o::SparseOptimizer::setForceStopFlag(), g2o::OptimizableGraph::setRenamedTypesFromString(), g2o::SparseOptimizer::solver(), str2method(), str2solver(), updateDisplay(), g2o::SparseOptimizer::verbose(), g2o::HyperGraph::Edge::vertex(), g2o::OptimizableGraph::vertex(), vertices, and g2o::HyperGraph::vertices().

325 {
326  int maxIterations;
327  bool verbose;
328  string inputFilename;
329  string gnudump;
330  string outputfilename;
331  string strMethod;
332  string strSolver;
333  string loadLookup;
334  bool initialGuess;
335  bool marginalize;
336  bool listTypes;
337  bool incremental;
338  bool guiOut;
339  double lambdaInit;
340  int updateGraphEachN = 10;
341  string statsFile;
342  // command line parsing
343  CommandArgs arg;
344  arg.param("i", maxIterations, 5, "perform n iterations");
345  arg.param("v", verbose, false, "verbose output of the optimization process");
346  arg.param("guess", initialGuess, false, "initial guess based on spanning tree");
347  arg.param("inc", incremental, false, "run incremetally");
348  arg.param("update", updateGraphEachN, 10, "updates afert x odometry nodes, (default: 10)");
349  arg.param("guiOut", guiOut, false, "gui output while running incrementally");
350  arg.param("lambdaInit", lambdaInit, 0, "user specified lambda init for levenberg");
351  arg.param("marginalize", marginalize, false, "on or off");
352  arg.param("method", strMethod, "Gauss", "Gauss or Levenberg");
353  arg.param("gnudump", gnudump, "", "dump to gnuplot data file");
354  arg.param("o", outputfilename, "", "output final version of the graph");
355  arg.param("solver", strSolver, "var", "specify which solver to use underneat\n\t {var, fix3_2, fix6_3, fix_7_3}");
356  arg.param("stats", statsFile, "", "specify a file for the statistics");
357  arg.param("listTypes", listTypes, false, "list the registered types");
358  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");
359  arg.paramLeftOver("graph-input", inputFilename, "", "graph file which will be processed");
360 
361  arg.parseArgs(argc, argv);
362 
363  if (listTypes) {
364  OptimizableGraph::printRegisteredTypes(cout, true);
365  }
366 
367  SparseOptimizer optimizer;
368  optimizer.verbose() = verbose;
369  optimizer.setForceStopFlag(&hasToStop);
370 
371  optimizer.method() = str2method(strMethod);
372  optimizer.solver() = str2solver(strSolver, &optimizer);
373  if (! optimizer.solver()) {
374  cerr << "Error allocating solver" << endl;
375  return 0;
376  }
377  optimizer.userLambdaInit()=lambdaInit;
378 
379  assert (optimizer.solver());
380  ActivePathCostFunction* guessCostFunction=0;
381  if (initialGuess)
382  guessCostFunction=new ActivePathCostFunction(&optimizer);
383 
384  cerr << "Read input from " << inputFilename << endl;
385  ifstream ifs(inputFilename.c_str());
386  if (!ifs) {
387  cerr << "Failed to open file" << endl;
388  return 1;
389  }
390 
391  if (loadLookup.size() > 0) {
392  optimizer.setRenamedTypesFromString(loadLookup);
393  }
394 
395  if (!optimizer.load(ifs)) {
396  cerr << "Error loading graph" << endl;
397  return 2;
398  }
399 
400  cerr << "Loaded " << optimizer.vertices().size() << " vertices" << endl;
401  cerr << "Loaded " << optimizer.edges().size() << " edges" << endl;
402 
403  if (optimizer.vertices().size() == 0) {
404  cerr << "HyperGraph contains no vertices" << endl;
405  return 1;
406  }
407 
408  // HACK if schur, we wanna marginalize the landmarks...
409  if (marginalize)
410  {
411  cerr << "Preparing Marginalization of the Landmarks ... ";
412  int maxDim = -1;
413  for (HyperGraph::VertexIDMap::iterator it=optimizer.vertices().begin(); it!=optimizer.vertices().end(); it++){
414  OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second);
415  maxDim = (max)(v->dimension(), maxDim);
416  }
417  for (HyperGraph::VertexIDMap::iterator it=optimizer.vertices().begin(); it!=optimizer.vertices().end(); it++){
418  OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second);
419  if (v->dimension() != maxDim) {
420  //cerr << "m";
421  v->marginalized() = true;
422  }
423  }
424  cerr << "done." << endl;
425  }
426 
427  // sanity check
428  //HyperDijkstra d(&optimizer);
429  //UniformCostFunction f;
430  //d.shortestPaths(optimizer.findOneOrigin(),&f);
431  //cerr << PVAR(d.visited().size()) << endl;
432  //assert (d.visited().size()==optimizer.vertices().size());
433 
434 
435  if (incremental) {
436  int incIterations = 1;
437  int updateDisplayEveryN = updateGraphEachN;
438  int maxDim = 0;
439 
440  SparseOptimizer::VertexIDMap vertices = optimizer.vertices();
441  for (SparseOptimizer::VertexIDMap::const_iterator it = vertices.begin(); it != vertices.end(); ++it) {
442  const SparseOptimizer::Vertex* v = static_cast<const SparseOptimizer::Vertex*>(it->second);
443  maxDim = (max)(maxDim, v->dimension());
444  }
445 
446  vector<SparseOptimizer::Edge*> edges;
447  for (SparseOptimizer::EdgeSet::iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
448  SparseOptimizer::Edge* e = dynamic_cast<SparseOptimizer::Edge*>(*it);
449  edges.push_back(e);
450  }
451  optimizer.edges().clear();
452  optimizer.vertices().clear();
453  optimizer.verbose() = false;
454 
455  // sort the edges in a way that inserting them makes sense
456  sort(edges.begin(), edges.end(), IncrementalEdgesCompare());
457 
458  double cumTime = 0.;
459  int vertexCount=0;
460  int lastOptimizedVertexCount = 0;
461  int lastVisUpdateVertexCount = 0;
462  bool addNextEdge=true;
463  bool freshlyOptimized=false;
464  for (vector<SparseOptimizer::Edge*>::iterator it = edges.begin(); it != edges.end(); ++it) {
465  SparseOptimizer::Edge* e = *it;
466  bool optimize=false;
467 
468  if (addNextEdge && !optimizer.vertices().empty()){
469  int maxInGraph = optimizer.vertices().rbegin()->first;
470  int idMax = max(e->vertex(0)->id(), e->vertex(1)->id());
471  if (maxInGraph < idMax && ! freshlyOptimized){
472  addNextEdge=false;
473  optimize=true;
474  } else {
475  addNextEdge=true;
476  optimize=false;
477  }
478  }
479 
480  int doInit = 0;
481  SparseOptimizer::Vertex* v1 = optimizer.vertex(e->vertex(0)->id());
482  SparseOptimizer::Vertex* v2 = optimizer.vertex(e->vertex(1)->id());
483  if (! v1 && addNextEdge) {
484  //cerr << " adding vertex " << it->id1 << endl;
485  SparseOptimizer::Vertex* v = dynamic_cast<SparseOptimizer::Vertex*>(e->vertex(0));
486  bool v1Added = optimizer.addVertex(v);
487  assert(v1Added);
488  doInit = 2;
489  if (v->dimension() == maxDim)
490  vertexCount++;
491  }
492 
493  if (! v2 && addNextEdge) {
494  //cerr << " adding vertex " << it->id2 << endl;
495  SparseOptimizer::Vertex* v = dynamic_cast<SparseOptimizer::Vertex*>(e->vertex(1));
496  bool v2Added = optimizer.addVertex(v);
497  assert(v2Added);
498  doInit = 1;
499  if (v->dimension() == maxDim)
500  vertexCount++;
501  }
502 
503  if (addNextEdge){
504  //cerr << " adding edge " << it->id1 << " " << it->id2 << " " << it->mean << endl;
505  if (! optimizer.addEdge(e)) {
506  cerr << "Unable to add edge " << e->vertex(0)->id() << " -> " << e->vertex(1)->id() << endl;
507  }
508 
509  if (doInit) {
510  OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertex(0));
511  OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(e->vertex(1));
512  switch (doInit){
513  case 1: // initialize v1 from v2
514  if (e->initialEstimatePossible(to, from)) {
515  // cerr << "init: "
516  // << to->id() << "(" << to->dimension() << ") -> "
517  // << from->id() << "(" << from->dimension() << ") " << endl;
518  e->initialEstimate(to, from);
519  }
520  break;
521  case 2:
522  if (e->initialEstimatePossible(from, to)) {
523  // cerr << "init: "
524  // << from->id() << "(" << from->dimension() << ") -> "
525  // << to->id() << "(" << to->dimension() << ") " << endl;
526  e->initialEstimate(from, to);
527  }
528  break;
529  default: cerr << "doInit wrong value\n";
530  }
531  }
532 
533  }
534 
535  freshlyOptimized=false;
536  if (optimize){
537  //cerr << "Optimize" << endl;
538  if (vertexCount - lastOptimizedVertexCount >= updateGraphEachN) {
539  double ts = get_monotonic_time();
540  int currentIt=optimizer.optimize(incIterations);
541  double dts = get_monotonic_time() - ts;
542  cumTime += dts;
543  //optimizer->setOptimizationTime(cumTime);
544  if (verbose) {
545  double chi2 = optimizer.chi2();
546  cerr << "nodes= " << optimizer.vertices().size() << "\t edges= " << optimizer.edges().size() << "\t chi2= " << chi2
547  << "\t time= " << dts << "\t iterations= " << currentIt << "\t cumTime= " << cumTime << endl;
548  }
549  lastOptimizedVertexCount = vertexCount;
550  }
551 
552  if (! verbose)
553  cerr << ".";
554  addNextEdge=true;
555  freshlyOptimized=true;
556  it--;
557  }
558 
559  if (guiOut) {
560  if (vertexCount - lastVisUpdateVertexCount >= updateDisplayEveryN) {
561  updateDisplay(cout, optimizer);
562  lastVisUpdateVertexCount = vertexCount;
563  }
564  }
565 
566  } // for all edges
567 
568 
569  } else {
570 
571  if (gnudump.size() > 0)
572  gnudump_edges(gnudump, "inputtraj", optimizer.edges().begin(), optimizer.edges().end(), true, false);
573  if (gnudump.size() > 0)
574  gnudump_features(gnudump, "inputfeatures", optimizer.edges().begin(), optimizer.edges().end());
575  if (gnudump.size() > 0)
576  gnudump_edges(gnudump, "input", optimizer.edges().begin(), optimizer.edges().end(), true, true);
577 
578  // BATCH optimization
579  // signal(SIGINT, sigquit_handler);
580  cerr << "Initial chi2 = " << optimizer.chi2() << endl;
581 
582  if (statsFile!=""){
583  // allocate buffer for statistics;
584  optimizer._statistics=new G2OBatchStatistics[maxIterations];
585  }
586 
587  int i=optimizer.optimize(maxIterations,guessCostFunction);
588  if (!i){
589  cerr << "Cholesky failed, result might be invalid" << endl;
590  }
591 
592  if (gnudump.size() > 0)
593  gnudump_edges(gnudump, "regularopttraj", optimizer.edges().begin(), optimizer.edges().end(), true, false);
594  if (gnudump.size() > 0)
595  gnudump_features(gnudump, "regularoptfeatures", optimizer.edges().begin(), optimizer.edges().end());
596  if (gnudump.size() > 0)
597  gnudump_edges(gnudump, "regularopt", optimizer.edges().begin(), optimizer.edges().end(), true, true);
598 
599 
600  cerr << "Computing chi2 statistics ";
601  double sumChi2=0.0;
602  double chi2Thres=0.0;
603  int cnt=0;
604 
605  for (HyperGraph::EdgeSet::const_iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
606  OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
607  e->computeError();
608  if(e->robustKernel())
609  e->robustifyError();
610  sumChi2 += e->chi2();
611  cnt++;
612  }
613  chi2Thres = sumChi2/cnt * 1.4;
614  cerr << " threshold=" << chi2Thres << " done" << endl;
615 
616  cerr << "Searchin for high error edges .. ";
617  HyperGraph::EdgeSet highErrorEdges;
618  HyperGraph::EdgeSet highErrorEdgesToFeatures;
619  HyperGraph::EdgeSet edgesToOptimize;
620 
621  HyperGraph::EdgeSet edgesToOptimize_selected;
622  HyperGraph::EdgeSet edgesToOptimize_border;
623 
624  for (HyperGraph::EdgeSet::const_iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
625  OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
626  e->computeError();
627  if(e->robustKernel())
628  e->robustifyError();
629  double edgeChi2 = e->chi2();
630  if (edgeChi2 > chi2Thres) {
631 
632  const VertexSE2* from = dynamic_cast<const VertexSE2*>(e->vertex(0));
633  const VertexSE2* to = dynamic_cast<const VertexSE2*>(e->vertex(1));
634 
635  if (from && to) {
636  highErrorEdges.insert(*it);
637  continue;
638  }
639  const VertexPointXY* tox = dynamic_cast<const VertexPointXY*>(e->vertex(1));
640  if (from && tox) {
641  highErrorEdgesToFeatures.insert(*it);
642  continue;
643  }
644  }
645  }
646  cerr << " found=" << highErrorEdges.size() << "/" << highErrorEdgesToFeatures.size() << " edges with high errors .. done" << endl;
647 
648 
649  if (gnudump.size() > 0)
650  gnudump_edges(gnudump, "hee", highErrorEdges.begin(), highErrorEdges.end(), true, false);
651 
652  if (gnudump.size() > 0)
653  gnudump_edges(gnudump, "heef_", highErrorEdgesToFeatures.begin(), highErrorEdgesToFeatures.end(), true, true);
654 
655 
656  // fix all vertices
657  cerr << "Fixing the whole graph ";
658  for (std::map<int, HyperGraph::Vertex*>::iterator vit = optimizer.vertices().begin();
659  vit != optimizer.vertices().end(); ++vit) {
660  OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>((*vit).second);
661  v->fixed() = true;
662  }
663  cerr << " done" << endl;
664 
665  typedef std::pair< HyperGraph::EdgeSet, HyperGraph::EdgeSet > EdgeSetPair;
666  typedef std::vector< EdgeSetPair > EdgeSetPairVector;
667 
668  EdgeSetPairVector myEdgeStack;
669 
670  cerr << "Collecting clusters...";
671 
672  while (!highErrorEdges.empty()) {
673 
674  HyperGraph::EdgeSet::iterator it = highErrorEdges.begin();
675 
676  HyperGraph::EdgeSet selected;
677  HyperGraph::EdgeSet border;
678  InvChi2CostFunction c2cost;
679 
680  HyperGraph::Edge* start = *it;
681  bool edgesSelected = false;
682 
683  findConnectedEdgesWithCostLimit(selected, border, start, &c2cost, 2.0/chi2Thres);
684 
685 
686  if (selected.size() > 10) {
687  edgesSelected = true;
688  cerr << " (" << selected.size() << ", " << border.size() << ")";
689 
690  // high error edges and their neighbor edges will be considered for optimization
691 
692  edgesToOptimize_selected.insert(selected.begin(), selected.end());
693  edgesToOptimize_border.insert(border.begin(), border.end());
694  edgesToOptimize.insert(selected.begin(), selected.end());
695  edgesToOptimize.insert(border.begin(), border.end());
696  }
697  else {
698  cerr << " [" << selected.size() << ", " << border.size() << "]";
699  }
700 
701  // removed edges from that cluster from the open list of high error edges
702  for (HyperGraph::EdgeSet::iterator eit = selected.begin(); eit != selected.end(); ++eit) {
703  HyperGraph::EdgeSet::iterator removeMe = highErrorEdges.find(*eit);
704  if (removeMe != highErrorEdges.end()) {
705  highErrorEdges.erase(removeMe);
706  }
707  }
708 
709  if (edgesSelected) {
710  myEdgeStack.push_back( EdgeSetPair(selected,border) );
711  }
712  }
713  cerr << " done" << endl;
714 
715  cerr << "Found " << myEdgeStack.size()
716  << " clusters in sum with " << edgesToOptimize_selected.size() << " high error edges"
717  << " and " << edgesToOptimize_border.size() << " border edges." << endl;
718 
719 
720  if (gnudump.size() > 0)
721  gnudump_edges(gnudump, "selected", edgesToOptimize_selected.begin(), edgesToOptimize_selected.end(),true, false);
722 
723  if (gnudump.size() > 0)
724  gnudump_edges(gnudump, "border", edgesToOptimize_border.begin(), edgesToOptimize_border.end(),true, false);
725 
726  if (gnudump.size() > 0)
727  gnudump_edges(gnudump, "reoptimizeme", edgesToOptimize.begin(), edgesToOptimize.end(),true, false);
728 
729  cerr << "Unfixing high error edges ";
730  for (EdgeSetPairVector::iterator it = myEdgeStack.begin();
731  it != myEdgeStack.end(); ++it) {
732 
733  // un-fix bad vertices
734  for (HyperGraph::EdgeSet::iterator eit = (*it).first.begin(); eit != (*it).first.end(); ++eit) {
735  OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*eit);
736 
737  OptimizableGraph::Vertex* vfrom = static_cast<OptimizableGraph::Vertex*>(e->vertex(0));
738  vfrom->fixed() = false;
739  OptimizableGraph::Vertex* vto = static_cast<OptimizableGraph::Vertex*>(e->vertex(1));
740  vto->fixed() = false;
741  }
742  }
743  cerr << "done" << endl;
744 
745 
746  // generate that for all nodes in all clusters
747  cerr << "Initialize optimization of subgraphs .. ";
748  optimizer.initializeOptimization(edgesToOptimize);
749  cerr << "done" << endl;
750 
751  if (guessCostFunction==0)
752  guessCostFunction=new ActivePathCostFunction(&optimizer);
753 
754 
755  cerr << "Searching for init nodes and re-initialize subgraphs .. ";
756  for (EdgeSetPairVector::iterator it = myEdgeStack.begin();
757  it != myEdgeStack.end(); ++it) {
758 
759  // determine good node for initialization (one out of the borders!)
760  cerr << "S";
761  HyperGraph::EdgeSet::iterator eit = (*it).second.begin();
762 
763  OptimizableGraph::Vertex* initNode = 0;
764  OptimizableGraph::Vertex* vfrom = static_cast<OptimizableGraph::Vertex*>((*eit)->vertex(0));
765  OptimizableGraph::Vertex* vto = static_cast<OptimizableGraph::Vertex*>((*eit)->vertex(1));
766 
767  if (vfrom && vfrom->fixed())
768  initNode = vfrom;
769  else if (vto && vto->fixed())
770  initNode = vto;
771  else
772  initNode = vfrom;
773 
774  // reinitialize bad vertices
775  cerr << "I";
776  optimizer.initializeActiveSubsetViaMeasurements(initNode, guessCostFunction);
777  cerr << "-";
778  }
779  cerr << " .. done" << endl;
780 
781  if (gnudump.size() > 0)
782  gnudump_edges(gnudump, "subsetinitialized", optimizer.edges().begin(), optimizer.edges().end(),true, false);
783 
784 
785  // run all local optimization
786  cerr << "Optimize subgraphs .. "<< endl;
787  optimizer.optimizeLoop(maxIterations);
788  cerr << "done" << endl;
789 
790  if (gnudump.size() > 0)
791  gnudump_edges(gnudump, "subsetoptimized", optimizer.edges().begin(), optimizer.edges().end(),true, false);
792 
793 
794  if (0) // no effect visible, why??
795  if (highErrorEdgesToFeatures.size() > 0) {
796 
797  HyperGraph::EdgeSet edgesToBadFeatures;
798 
799  // fix all vertices
800  cerr << "Fixing the whole graph ";
801  for (std::map<int, HyperGraph::Vertex*>::iterator vit = optimizer.vertices().begin();
802  vit != optimizer.vertices().end(); ++vit) {
803  OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>((*vit).second);
804  v->fixed() = true;
805  }
806  cerr << " done" << endl;
807 
808  cerr << "Unfixing bad features";
809  for (HyperGraph::EdgeSet::iterator eit = highErrorEdgesToFeatures.begin();
810  eit != highErrorEdgesToFeatures.end(); ++eit) {
811 
812  OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*>((*eit)->vertex(1));
813  v->fixed() = false;
814 
815  edgesToBadFeatures.insert(v->edges().begin(), v->edges().end());
816  }
817 
818  cerr << " considering " << edgesToBadFeatures.size() << " edges .. done"<< endl;
819 
820  if (gnudump.size() > 0)
821  gnudump_edges(gnudump, "featurereinit", edgesToBadFeatures.begin(), edgesToBadFeatures.end(), true, true);
822  if (gnudump.size() > 0)
823  gnudump_features(gnudump, "featurereinitfeatures", edgesToBadFeatures.begin(), edgesToBadFeatures.end());
824 
825 
826  cerr << "Initialize optimization of bad features .. ";
827  optimizer.initializeOptimization(edgesToBadFeatures);
828  cerr << "done" << endl;
829 
830 
831  // run all local optimization
832  cerr << "Optimize bad features .. "<< endl;
833  optimizer.optimizeLoop(maxIterations/2);
834  cerr << "done" << endl;
835 
836  if (gnudump.size() > 0)
837  gnudump_edges(gnudump, "subgraphsfinal", optimizer.edges().begin(), optimizer.edges().end(),true, false);
838  }
839 
840 
841  // un-fix all vertices
842  for (std::map<int, HyperGraph::Vertex*>::iterator vit = optimizer.vertices().begin();
843  vit != optimizer.vertices().end(); ++vit) {
844  OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>((*vit).second);
845  v->fixed() = false;
846  }
847 
848  // final global optimization
849  cerr << "Initialize final optimization .. ";
850  optimizer.initializeOptimization(optimizer.edges());// ALLLL
851  cerr << "done" << endl;
852  cerr << "Optimize .. " << endl;
853  optimizer.optimizeLoop(maxIterations);
854  cerr << "done" << endl;
855 
856  }
857 
858 
859  if (gnudump.size() > 0)
860  gnudump_edges(gnudump, "finaltraj", optimizer.edges().begin(), optimizer.edges().end(),true,false);
861  if (gnudump.size() > 0)
862  gnudump_features(gnudump, "finalfeatures", optimizer.edges().begin(), optimizer.edges().end());
863  if (gnudump.size() > 0)
864  gnudump_edges(gnudump, "final", optimizer.edges().begin(), optimizer.edges().end(),true,true);
865 
866 
867  if (statsFile!=""){
868  cerr << "writing stats to file \"" << statsFile << "\"";
869  ofstream os(statsFile.c_str());
870  for (int i=0; i<maxIterations; i++){
871  os << optimizer._statistics[i] << endl;
872  }
873  }
874 
875 
876 
877  // // HACK write landmarks to anotherfile
878  // # if 1
879  // bool hasLandmarks = false;
880  // for (HyperGraph::EdgeSet::const_iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
881  // const VertexSE2* from = dynamic_cast<const VertexSE2*>((*it)->vertex(0]);
882  // const VertexPointXY* to = dynamic_cast<const VertexPointXY*>((*it)->vertex(1]);
883  // if (from && to) {
884  // hasLandmarks = true;
885  // break;
886  // }
887  // }
888  // if (hasLandmarks) {
889  // string baseName = getPureFilename(gnudump);
890  // string extension = getFileExtension(gnudump);
891  // string landmarkFilename = formatString("%s_landmarks.%s", baseName.c_str(), extension.c_str());
892  // cerr << "saving " << landmarkFilename << " ... ";
893  // fout.open(landmarkFilename.c_str());
894  // for (HyperGraph::EdgeSet::const_iterator it = optimizer.edges().begin(); it != optimizer.edges().end(); ++it) {
895  // const VertexPointXY* to = dynamic_cast<const VertexPointXY*>((*it)->vertex(1]);
896  // if (to) {
897  // const Vector2d& p2 = to->estimate();
898  // fout << p2[0] << " " << p2[1] << endl;
899  // fout << endl;
900  // }
901  // }
902  // cerr << "done." << endl;
903  // }
904  // # endif
905 
906  //}
907 
908  if (outputfilename.size() > 0) {
909  cerr << "saving " << outputfilename << " ... ";
910  optimizer.save(outputfilename.c_str());
911  cerr << "done." << endl;
912  }
913 
914  return 0;
915 }
g2o::get_monotonic_time
double get_monotonic_time()
Definition: timeutil.cpp:113
g2o::G2OBatchStatistics
statistics about the optimization
Definition: batch_stats.h:40
g2o::HyperGraph::Edge::vertex
const Vertex * vertex(size_t i) const
Definition: hyper_graph.h:186
g2o::CommandArgs
Command line parsing of argc and argv.
Definition: command_args.h:46
g2o::OptimizableGraph::Edge::chi2
virtual double chi2() const =0
computes the chi2 based on the cached error value, only valid after computeError has been called...
g2o::SparseOptimizer::optimize
int optimize(int iterations, bool online=false)
Definition: sparse_optimizer.cpp:364
g2o::SparseOptimizer::solver
OptimizationAlgorithm * solver()
Definition: sparse_optimizer.h:219
g2o::VertexSE2
2D pose Vertex, (x,y,theta)
Definition: vertex_se2.h:41
g2o::HyperGraph::vertices
const VertexIDMap & vertices() const
Definition: hyper_graph.h:225
g2o::OptimizableGraph::vertex
Vertex * vertex(int id)
returns the vertex number id appropriately casted
Definition: optimizable_graph.h:496
str2method
SparseOptimizer::Method str2method(const std::string &strMethod)
Definition: g2o-unfold.cpp:139
g2o::HyperGraph::EdgeSet
std::set< Edge * > EdgeSet
Definition: hyper_graph.h:135
g2o::OptimizableGraph::Vertex::dimension
int dimension() const
dimension of the estimated state belonging to this node
Definition: optimizable_graph.h:283
vertices
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
g2o::OptimizableGraph::chi2
double chi2() const
returns the chi2 of the current configuration
Definition: optimizable_graph.cpp:315
g2o::CommandArgs::parseArgs
bool parseArgs(int argc, char **argv, bool exitOnError=true)
Definition: command_args.cpp:111
g2o::HyperGraph::Vertex::edges
const EdgeSet & edges() const
returns the set of hyper-edges that are leaving/entering in this vertex
Definition: hyper_graph.h:151
g2o::OptimizableGraph::save
virtual bool save(std::ostream &os, int level=0) const
save the graph to a stream. Again uses the Factory system.
g2o::OptimizableGraph::Edge::robustKernel
RobustKernel * robustKernel() const
if NOT NULL, error of this edge will be robustifed with the kernel
Definition: optimizable_graph.h:383
g2o::HyperGraph::edges
const EdgeSet & edges() const
Definition: hyper_graph.h:230
g2o::SparseOptimizer::setForceStopFlag
void setForceStopFlag(bool *flag)
Definition: sparse_optimizer.cpp:583
g2o::CommandArgs::paramLeftOver
void paramLeftOver(const std::string &name, std::string &p, const std::string &defValue, const std::string &desc, bool optional=false)
Definition: command_args.cpp:342
g2o::OptimizableGraph::Edge::computeError
virtual void computeError()=0
gnudump_edges
void gnudump_edges(string gnudump, string file_suffix, HyperGraph::EdgeSet::const_iterator begin, HyperGraph::EdgeSet::const_iterator end, bool dumpEdges, bool dumpFeatures)
Definition: g2o-unfold.cpp:220
g2o::CommandArgs::param
void param(const std::string &name, bool &p, bool defValue, const std::string &desc)
Definition: command_args.cpp:198
g2o::OptimizableGraph::Vertex::marginalized
bool marginalized() const
true => this node is marginalized out during the optimization
Definition: optimizable_graph.h:278
g2o::OptimizableGraph::Vertex
A general case Vertex for optimization.
Definition: optimizable_graph.h:107
g2o::OptimizableGraph::setRenamedTypesFromString
void setRenamedTypesFromString(const std::string &types)
Definition: optimizable_graph.cpp:774
g2o::OptimizableGraph::load
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...
IncrementalEdgesCompare
Sort Edges for inserting them sequentially.
Definition: g2o.cpp:67
g2o::OptimizableGraph::Vertex::fixed
bool fixed() const
true => this node is fixed during the optimization
Definition: optimizable_graph.h:273
g2o::findConnectedEdgesWithCostLimit
void findConnectedEdgesWithCostLimit(HyperGraph::EdgeSet &selected, HyperGraph::EdgeSet &border, HyperGraph::Edge *start, HyperDijkstra::CostFunction *cost, double maxEdgeCost, double comparisonConditioner)
Definition: tools.cpp:17
updateDisplay
void updateDisplay(ostream &os, const SparseOptimizer &optimizer)
Definition: g2o-unfold.cpp:83
g2o::SparseOptimizer::initializeOptimization
virtual bool initializeOptimization(HyperGraph::EdgeSet &eset)
Definition: sparse_optimizer.cpp:272
g2o::OptimizableGraph::addEdge
virtual bool addEdge(HyperGraph::Edge *e)
Definition: optimizable_graph.cpp:257
g2o::OptimizableGraph::addVertex
virtual bool addVertex(HyperGraph::Vertex *v, Data *userData)
Definition: optimizable_graph.cpp:231
g2o::SparseOptimizer::verbose
bool verbose() const
verbose information during optimization
Definition: sparse_optimizer.h:179
str2solver
Solver * str2solver(const std::string &strSolver_, SparseOptimizer *opt)
Definition: g2o-unfold.cpp:166
hasToStop
bool hasToStop
Definition: g2o-unfold.cpp:318
gnudump_features
void gnudump_features(string gnudump, string file_suffix, HyperGraph::EdgeSet::const_iterator begin, HyperGraph::EdgeSet::const_iterator end)
Definition: g2o-unfold.cpp:279
void sigquit_handler ( int q   __attribute__(unused))

Definition at line 320 of file g2o-unfold.cpp.

320  {
321  hasToStop=1;
322 }
hasToStop
bool hasToStop
Definition: g2o-unfold.cpp:318
SparseOptimizer::Method str2method ( const std::string &  strMethod)

Definition at line 139 of file g2o-unfold.cpp.

Referenced by main().

139  {
140  if (strMethod=="Gauss") {
141  cerr << "# Doing Gauss" << endl;
142  return SparseOptimizer::GaussNewton;
143  }
144  if (strMethod=="Levenberg") {
145  cerr << "# Doing Levenberg-Marquardt" << endl;
146  return SparseOptimizer::LevenbergMarquardt;
147  }
148  cerr << "# Unknown optimization method: " << strMethod << ", setting default to Levenberg" << endl;
149  return SparseOptimizer::LevenbergMarquardt;
150 }
Solver* str2solver ( const std::string &  strSolver_,
SparseOptimizer *  opt 
)

Definition at line 166 of file g2o-unfold.cpp.

References g2o::strEndsWith(), and g2o::strStartsWith().

Referenced by main().

167 {
168  string strSolver = strSolver_;
169  bool useCholmod = false;
170  if (strEndsWith(strSolver, "_cholmod")) {
171  string::size_type idx = strSolver.rfind("_cholmod");
172  strSolver = strSolver.substr(0, idx);
173  useCholmod = true;
174  }
175 #ifndef GO_CHOLMOD_SUPPORT
176  if (useCholmod) {
177  cerr << "Error: Cholmod is not supported in this build" << endl;
178  return 0;
179  }
180 #endif
181  if (! strStartsWith(strSolver, "pcg"))
182  cerr << "# Using " << (useCholmod ? "cholmod" : "CSparse") << " " << strSolver << endl;
183  else
184  cerr << "# Using PCG " << strSolver<< endl;
185 
186  if (strSolver=="var") {
187  BlockSolverX::LinearSolverType* linearSolver = allocateLinearSolver<BlockSolverX>(useCholmod);
188  return new BlockSolverX(opt, linearSolver);
189  }
190  if (strSolver=="fix3_2") {
191  BlockSolver_3_2::LinearSolverType* linearSolver = allocateLinearSolver<BlockSolver_3_2>(useCholmod);
192  return new BlockSolver_3_2(opt, linearSolver);
193  }
194  if (strSolver=="fix6_3") {
195  BlockSolver_6_3::LinearSolverType* linearSolver = allocateLinearSolver<BlockSolver_6_3>(useCholmod);
196  return new BlockSolver_6_3(opt, linearSolver);
197  }
198  if (strSolver=="fix7_3") {
199  BlockSolver_7_3::LinearSolverType* linearSolver = allocateLinearSolver<BlockSolver_7_3>(useCholmod);
200  return new BlockSolver_7_3(opt, linearSolver);
201  }
202  if (strSolver == "pcg") {
203  BlockSolverX::LinearSolverType* linearSolver = new LinearSolverPCG<BlockSolverX::PoseMatrixType>();
204  return new BlockSolverX(opt, linearSolver);
205  }
206  if (strSolver == "pcg6_3") {
207  BlockSolver_6_3::LinearSolverType* linearSolver = new LinearSolverPCG<BlockSolver_6_3::PoseMatrixType>();
208  return new BlockSolver_6_3(opt, linearSolver);
209  }
210  if (strSolver == "pcg3_2") {
211  BlockSolver_3_2::LinearSolverType* linearSolver = new LinearSolverPCG<BlockSolver_3_2::PoseMatrixType>();
212  return new BlockSolver_3_2(opt, linearSolver);
213  }
214  cerr << "Unknown solver, setting the general but slower \"var\" "<< endl;
215  BlockSolverX::LinearSolverType* linearSolver = allocateLinearSolver<BlockSolverX>(useCholmod);
216  return new BlockSolverX(opt, linearSolver);
217 }
g2o::BlockSolver_6_3
BlockSolver< BlockSolverTraits< 6, 3 > > BlockSolver_6_3
Definition: block_solver.h:181
g2o::LinearSolverPCG
linear solver using PCG, pre-conditioner is block Jacobi
Definition: linear_solver_pcg.h:47
g2o::BlockSolver::LinearSolverType
Traits::LinearSolverType LinearSolverType
Definition: block_solver.h:110
g2o::BlockSolver_3_2
BlockSolver< BlockSolverTraits< 3, 2 > > BlockSolver_3_2
Definition: block_solver.h:185
g2o::strStartsWith
bool strStartsWith(const std::string &s, const std::string &start)
Definition: string_tools.cpp:158
g2o::BlockSolver_7_3
BlockSolver< BlockSolverTraits< 7, 3 > > BlockSolver_7_3
Definition: block_solver.h:183
g2o::strEndsWith
bool strEndsWith(const std::string &s, const std::string &end)
Definition: string_tools.cpp:165
g2o::BlockSolverX
BlockSolver< BlockSolverTraits< Eigen::Dynamic, Eigen::Dynamic > > BlockSolverX
Definition: block_solver.h:179
void updateDisplay ( ostream &  os,
const SparseOptimizer &  optimizer 
)

Definition at line 83 of file g2o-unfold.cpp.

References g2o::BaseVertex< D, T >::estimate(), g2o::SE2::rotation(), g2o::SE2::translation(), and g2o::HyperGraph::vertices().

Referenced by main(), and g2o::G2oQGLViewer::updateDisplay().

84 {
85  // TODO edges missing
86  // TODO other than 2D types missing
87  for (SparseOptimizer::VertexIDMap::const_iterator it = optimizer.vertices().begin();
88  it != optimizer.vertices().end(); ++it) {
89  const SparseOptimizer::Vertex* v = static_cast<const SparseOptimizer::Vertex*>(it->second);
90  Eigen::Quaterniond quat;
91  Eigen::Vector3d trans;
92 
93  bool writeVertex = false;
94  bool writePoint = false;
95 
96  const VertexSE2* v2se = dynamic_cast<const VertexSE2*>(v);
97  if (v2se) {
98  writeVertex = true;
99  trans[0] = v2se->estimate().translation()[0];
100  trans[1] = v2se->estimate().translation()[1];
101  trans[2] = 0.0;
102  static Eigen::Vector3d axis(0,0,1);
103  quat = AngleAxis<double>(v2se->estimate().rotation().angle(), axis);
104  }
105 
106  const VertexPointXY* v2point = dynamic_cast<const VertexPointXY*>(v);
107  if (v2point) {
108  writePoint = true;
109  trans[0] = v2point->estimate()[0];
110  trans[1] = v2point->estimate()[1];
111  trans[2] = 0.0;
112  }
113 
114  if (writeVertex) {
115  os.put('V');
116  int id = v->id();
117  os.write((char*)&id, sizeof(int));
118  os.write((char*)&trans.x(), sizeof(double));
119  os.write((char*)&trans.y(), sizeof(double));
120  os.write((char*)&trans.z(), sizeof(double));
121  os.write((char*)&quat.w(), sizeof(double));
122  os.write((char*)&quat.x(), sizeof(double));
123  os.write((char*)&quat.y(), sizeof(double));
124  os.write((char*)&quat.z(), sizeof(double));
125  }
126 
127  if (writePoint) {
128  os.put('P');
129  os.write((char*)&trans.x(), sizeof(double));
130  os.write((char*)&trans.y(), sizeof(double));
131  os.write((char*)&trans.z(), sizeof(double));
132  }
133  }
134 
135  // done
136  os << "F" << flush;
137 }
g2o::VertexSE2
2D pose Vertex, (x,y,theta)
Definition: vertex_se2.h:41
g2o::HyperGraph::vertices
const VertexIDMap & vertices() const
Definition: hyper_graph.h:225
g2o::SE2::rotation
const Eigen::Rotation2Dd & rotation() const
rotational component
Definition: se2.h:58
g2o::BaseVertex::estimate
const EstimateType & estimate() const
return the current estimate of the vertex
Definition: base_vertex.h:99
g2o::SE2::translation
const Vector2D & translation() const
translational component
Definition: se2.h:54

Variable Documentation

bool hasToStop =false

Definition at line 318 of file g2o-unfold.cpp.

Generated on Mon Mar 18 2019 20:14:42 for g2o by   doxygen 1.8.11