sclam_laser_calib.cpp File Reference

#include <csignal>
#include <fstream>
#include <iostream>
#include <map>
#include "g2o/core/factory.h"
#include "g2o/core/hyper_dijkstra.h"
#include "g2o/core/optimization_algorithm_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 "g2o/types/data/types_data.h"
#include "g2o/types/sclam2d/types_sclam2d.h"
#include "gm2dl_io.h"

Include dependency graph for sclam_laser_calib.cpp:

Go to the source code of this file.

Functions
	G2O_USE_OPTIMIZATION_LIBRARY (eigen)
	G2O_USE_TYPE_GROUP (slam2d)
void	sigquit_handler (int sig)
int	main (int argc, char **argv)

Variables
static bool	hasToStop = false

Function Documentation

G2O_USE_OPTIMIZATION_LIBRARY()

G2O_USE_OPTIMIZATION_LIBRARY

(

eigen

)

G2O_USE_TYPE_GROUP()

G2O_USE_TYPE_GROUP

(

slam2d

)

main()

int main	(	int	argc,
		char **	argv
	)

Definition at line 65 of file sclam_laser_calib.cpp.

                                {
  int maxIterations;
  bool verbose;
  string inputFilename;
  string gnudump;
  string outputfilename;
  bool initialGuess;
  // command line parsing
  CommandArgs commandLineArguments;
  commandLineArguments.param("i", maxIterations, 10, "perform n iterations");
  commandLineArguments.param("v", verbose, false,
                             "verbose output of the optimization process");
  commandLineArguments.param("guess", initialGuess, false,
                             "initial guess based on spanning tree");
  commandLineArguments.param("gnudump", gnudump, "",
                             "dump to gnuplot data file");
  commandLineArguments.param("o", outputfilename, "",
                             "output final version of the graph");
  commandLineArguments.paramLeftOver("gm2dl-input", inputFilename, "",
                                     "gm2dl file which will be processed");
 
  commandLineArguments.parseArgs(argc, argv);
 
  OptimizationAlgorithmFactory* solverFactory =
      OptimizationAlgorithmFactory::instance();
 
  SparseOptimizer optimizer;
  optimizer.setVerbose(verbose);
  optimizer.setForceStopFlag(&hasToStop);
 
  OptimizationAlgorithmProperty solverProperty;
  optimizer.setAlgorithm(solverFactory->construct("lm_var", solverProperty));
 
  // loading
  if (!Gm2dlIO::readGm2dl(inputFilename, optimizer, false)) {
    cerr << "Error while loading gm2dl file" << endl;
  }
 
  VertexSE2* laserOffset =
      dynamic_cast<VertexSE2*>(optimizer.vertex(numeric_limits<int>::max()));
  // laserOffset->setEstimate(SE2()); // set to Identity
  if (laserOffset) {
    cerr << "Initial laser offset "
         << laserOffset->estimate().toVector().transpose() << endl;
  }
  bool gaugeFreedom = optimizer.gaugeFreedom();
 
  OptimizableGraph::Vertex* 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" << 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 (1)
      for (SparseOptimizer::VertexIDMap::const_iterator it =
               optimizer.vertices().begin();
           it != optimizer.vertices().end(); ++it) {
        OptimizableGraph::Vertex* v =
            static_cast<OptimizableGraph::Vertex*>(it->second);
        if (d.visited().count(v) == 0) {
          cerr << "\t unvisited vertex " << it->first << " "
               << static_cast<void*>(v) << endl;
          v->setFixed(true);
        }
      }
  }
 
  optimizer.initializeOptimization();
  optimizer.computeActiveErrors();
  cerr << "Initial chi2 = " << FIXED(optimizer.chi2()) << endl;
 
  // if (guessCostFunction)
  // optimizer.computeInitialGuess(guessCostFunction);
 
  signal(SIGINT, sigquit_handler);
 
  int i = optimizer.optimize(maxIterations);
  if (maxIterations > 0 && !i) {
    cerr << "optimize failed, result might be invalid" << endl;
  }
 
  if (laserOffset) {
    cerr << "Calibrated laser offset "
         << laserOffset->estimate().toVector().transpose() << endl;
  }
 
  if (outputfilename.size() > 0) {
    Gm2dlIO::updateLaserData(optimizer);
    cerr << "Writing " << outputfilename << " ... ";
    bool writeStatus = Gm2dlIO::writeGm2dl(outputfilename, optimizer);
    cerr << (writeStatus ? "done." : "failed") << endl;
  }
 
  if (gnudump.size() > 0) {
    ofstream fout(gnudump.c_str());
    for (SparseOptimizer::VertexIDMap::const_iterator it =
             optimizer.vertices().begin();
         it != optimizer.vertices().end(); ++it) {
      VertexSE2* v = dynamic_cast<VertexSE2*>(it->second);
      fout << v->estimate().toVector().transpose() << endl;
    }
  }
 
  return 0;
}

References g2o::OptimizableGraph::chi2(), CL_RED, g2o::SparseOptimizer::computeActiveErrors(), g2o::OptimizationAlgorithmFactory::construct(), g2o::BaseVertex< D, T >::estimate(), g2o::SparseOptimizer::findGauge(), g2o::SparseOptimizer::gaugeFreedom(), hasToStop, g2o::HyperGraph::Vertex::id(), g2o::SparseOptimizer::initializeOptimization(), g2o::OptimizationAlgorithmFactory::instance(), g2o::SparseOptimizer::optimize(), g2o::CommandArgs::param(), g2o::CommandArgs::paramLeftOver(), g2o::CommandArgs::parseArgs(), g2o::Gm2dlIO::readGm2dl(), g2o::SparseOptimizer::setAlgorithm(), g2o::OptimizableGraph::Vertex::setFixed(), g2o::SparseOptimizer::setForceStopFlag(), g2o::SparseOptimizer::setVerbose(), g2o::HyperDijkstra::shortestPaths(), sigquit_handler(), g2o::SE2::toVector(), g2o::Gm2dlIO::updateLaserData(), g2o::OptimizableGraph::vertex(), g2o::HyperGraph::vertices(), g2o::HyperDijkstra::visited(), and g2o::Gm2dlIO::writeGm2dl().

sigquit_handler()

void sigquit_handler

(

int

sig

)

Definition at line 54 of file sclam_laser_calib.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.

Referenced by main().

Variable Documentation

hasToStop

bool hasToStop = false

static

Definition at line 49 of file sclam_laser_calib.cpp.

Referenced by main(), and sigquit_handler().