converteSegmentLine.cpp File Reference

#include <signal.h>
#include <iostream>
#include <iomanip>
#include <string>
#include <fstream>
#include <sstream>
#include <algorithm>
#include <cassert>
#include "dl_wrapper.h"
#include "output_helper.h"
#include "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/core/hyper_graph_action.h"
#include "g2o/core/batch_stats.h"
#include "g2o/core/optimization_algorithm.h"
#include "g2o/core/sparse_optimizer_terminate_action.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 "g2o/types/slam2d/types_slam2d.h"

Include dependency graph for converteSegmentLine.cpp:

Go to the source code of this file.

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

Typedefs
typedef std::map< int, LineInfo >	LineInfoMap

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

Variables
static bool	hasToStop =false

Typedef Documentation

typedef std::map<int, LineInfo> LineInfoMap

Definition at line 112 of file converteSegmentLine.cpp.

Function Documentation

int main	(	int	argc,
		char **	argv
	)

Definition at line 114 of file converteSegmentLine.cpp.

References g2o::OptimizableGraph::addEdge(), g2o::OptimizableGraph::addVertex(), g2o::OptimizableGraph::Vertex::clone(), g2o::computeLineParameters(), g2o::VertexSegment2D::estimateP1(), g2o::VertexSegment2D::estimateP2(), g2o::HyperGraph::Vertex::id(), g2o::BaseEdge< D, E >::information(), LineInfo::line, g2o::loadStandardTypes(), g2o::BaseEdge< D, E >::measurement(), g2o::EdgeSE2Segment2D::measurementP1(), g2o::EdgeSE2Segment2D::measurementP2(), LineInfo::p1, LineInfo::p2, g2o::CommandArgs::param(), g2o::CommandArgs::paramLeftOver(), g2o::CommandArgs::parseArgs(), g2o::EdgeSE2Segment2DPointLine::point(), g2o::EdgeSE2Segment2DPointLine::pointNum(), g2o::BaseVertex< D, T >::setEstimate(), g2o::OptimizableGraph::Vertex::setId(), g2o::BaseEdge< D, E >::setInformation(), g2o::BaseEdge< D, E >::setMeasurement(), g2o::EdgeSE2Segment2DPointLine::theta(), g2o::OptimizableGraph::vertex(), g2o::HyperGraph::Edge::vertices(), and g2o::HyperGraph::vertices().

{
  string outputfilename;
  CommandArgs arg;
  arg.param("o", outputfilename, "", "output final version of the graph");
  arg.paramLeftOver("graph-input", inputFilename, "", "graph file which will be processed", true);
  

  arg.parseArgs(argc, argv);

  // registering all the types from the libraries
  DlWrapper dlTypesWrapper;
  loadStandardTypes(dlTypesWrapper, argc, argv);

  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;

  LineInfoMap lmap;
  OptimizableGraph outGraph();
  // insert all lines in the infomap
  int currentId = 0;
  for (OptimizableGraph::VertexIDMap::iterator it=optimizer.vertices().begin(); it!=optimizer->vertices().end(); it++){
    VertexSE2 *p=dynamic_cast<VertexSegment2D*> (it->second());
    if (p){
      VertexSE2 *np=p->clone();
      np->setId(p->id());
      outGraph->addVertex(np);
    }
    VertexSegment2D* s=dynamic_cast<VertexSegment2D*> (it->second());
    if (s){
      lmap.insert(make_pair(s->id(), LineInfo(s)));
    }
    currentId = it->first;
  }
  currentId++;
  for (OptimizableGraph::EdgeSet::iterator it=optimizer.edges().begin(); it!=optimizer->edges().end(); it++){
    EdgeSE2* ods=dynamic_cast<EdgeSE2*> (*it);
    if (ods){
      EdgeSE2* ods2=new EdgeSE2();
      ods2->setMeasdurement(ods2->measurement());
      ods2->setInformation(ods2->information());
      ods2->vertices[0]=outGraph->vertices(ods->vertices()[0]->id());
      ods2->vertices[1]=outGraph->vertices(ods->vertices()[1]->id());
      outGraph->addEdge(ods2);
    }

    EdgeSE2Segment2D* es=dynamic_cast<EdgeSE2Segment2D*> (*it);
    EdgeSE2Segment2DLine* el=dynamic_cast<EdgeSE2Segment2DLine*> (*it);
    EdgeSE2Segment2DPointLine* espl=dynamic_cast<EdgeSE2Segment2DPointLine*> (*it);

    if (es || el || espl){
      VertexSE2* pose = dynamic_cast<VertexSE2*>((*it)->vertices()[0]);
      VertexSegment2D* segment = dynamic_cast<VertexSegment2D*>((*it)->vertices()[1]);
      if (!pose)
        continue;
      pose=dynamic_cast<VertexSE2*>(outGraph->vertex(pose->id()));
      LineInfo& linfo = lmap[(*it)->vertices()[0]->id()];
      VertexLine* line =linfo->line;

      EdgeSE2Line el2=new EdgeSE2Line();
      VertexPoint* & p1=linfo->p1;
      VertexPoint* & p2=linfo->p2;
      el2->vertices()[0]=p;
      el2->vertices()[1]=l;
      if (el) {
        el2->setMeasurement(el->measurement());
        el2->setInformation(el->information());
        outGraph->addEdge(el2);
      }
      if (es) {
        el2-setMeasurement(computeLineParameters(s->measurementP1(), s->measurementP2()));
        if (!p1){
          p1=new VertexPointXY();
          p1->setEstimate(segment->estimateP1());
          p1->setId(currentId++);
          outGraph->addVertex(p1);
          
          EdgeLine2DPointXY* p1e=new EdgeLine2DPointXY();
          p1e->vertices()[0]=line;
          p1e->vertices()[1]=p1;
          p1e->setMeasurement(0);
          Matrix1d p1i;
          p1i(0,0)=1e6;
          p1e->setInformation(p1i);
          outGraph->addEdge(p1);
        }
        if (! p2) {
          p2=new VertexPointXY();
          p2->setEstimate(segment->estimateP2());
          p2->setId(currentId++);
          outGraph->addVertex(p2);

          EdgeLine2DPointXY* p2e=new EdgeLine2DPointXY();
          p2e->vertices()[0]=line;
          p2e->vertices()[1]=p2;
          p2e->setMeasurement(0);
          Matrix1d p2i;
          p2i(0,0)=1e6;
          p2e->setInformation(p2i);
          outGraph->addEdge(p2);
        }
        EdgeSE2PointXY* p1e = new EdgeSE2PointXY();
        p1e->vertices()[0]=line;
        p1e->vertices()[1]=p1;
        p1e->setMeasurement(es->measurementP1());
        Matrix4d si=segment->information();
        Matrix2d p1i=si.block<2,2>(0,0);
        p1e->setInformation(p1i);
        outGraph->addEdge(p1e);

        EdgeSE2PointXY* p2e = new EdgeSE2PointXY();
        p2e->vertices()[0]=line;
        p2e->vertices()[1]=p2;
        p2e->setMeasurement(es->measurementP2());
        Matrix4d si=segment->information();
        Matrix2d p2i=si.block<2,2>(2,2);
        p2e->setInformation(p2i);
        outGraph->addEdge(p2e);
      }

      if (espl) {
        Vector2d lparams;
        lparams[0]=espl->theta();
        Vector2d n(cos(espl->theta()), sin(espl->theta()));
        lparams[1]=n.dot(espl->point());
        Matrix2d li;
        li << 1000, 0, 0, 100;
        el2->setMeasurement(lparams);
        el2->setInformation(li);

        VertexPointXY*& pX = (espl->pointNum()==0 )? p1:p2;

        if (!pX){
          pX=new VertexPointXY();
          pX->setEstimate(segment->estimatePX());
          pX->setId(currentId++);
          outGraph->addVertex(pX);
          
          EdgeLine2DPointXY* pXe=new EdgeLine2DPointXY();
          pXe->vertices()[0]=line;
          pXe->vertices()[1]=pX;
          pXe->setMeasurement(0);
          Matrix1d pXi;
          pXi(0,0)=1e6;
          pXe->setInformation(pXi);
          outGraph->addEdge(pX);
        }

        EdgeSE2PointXY* pXe = new EdgeSE2PointXY();
        pXe->vertices()[0]=line;
        pXe->vertices()[1]=pX;
        pXe->setMeasurement(es->measurementPX());
        Matrix4d si=segment->information();
        Matrix2d pXi=si.block<2,2>(0,0);
        pXe->setInformation(pXi);
        outGraph->addEdge(pXe);
      }
    }
  }

  
  if (outputfilename.size() > 0) {
    if (outputfilename == "-") {
      cerr << "saving to stdout";
      optimizer.save(cout);
    } else {
      cerr << "saving " << outputfilename << " ... ";
      optimizer.save(outputfilename.c_str());
    }
    cerr << "done." << endl;
  }

  // destroy all the singletons
  //Factory::destroy();
  //OptimizationAlgorithmFactory::destroy();
  //HyperGraphActionLibrary::destroy();

  return 0;
}

void sigquit_handler

(

int

sig

)

Definition at line 87 of file converteSegmentLine.cpp.

References __PRETTY_FUNCTION__, and hasToStop.

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

Variable Documentation

bool hasToStop =false

static

Definition at line 58 of file converteSegmentLine.cpp.

Referenced by sigquit_handler().