g2o
convert_sba_slam3d.cpp
Go to the documentation of this file.
1 // g2o - General Graph Optimization
2 // Copyright (C) 2011 R. Kuemmerle, G.Grisetti, W. Burgard
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // * Redistributions of source code must retain the above copyright notice,
10 // this list of conditions and the following disclaimer.
11 // * Redistributions in binary form must reproduce the above copyright
12 // notice, this list of conditions and the following disclaimer in the
13 // documentation and/or other materials provided with the distribution.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 
27 #include <iostream>
28 #include <fstream>
29 
30 #include "g2o/stuff/macros.h"
31 #include "g2o/stuff/command_args.h"
32 
34 
38 
39 using namespace std;
40 using namespace g2o;
41 
42 int main(int argc, char** argv)
43 {
44  string inputFilename;
45  string outputFilename;
46  // command line parsing
47  CommandArgs commandLineArguments;
48  commandLineArguments.paramLeftOver("gm2dl-input", inputFilename, "", "gm2dl file which will be processed");
49  commandLineArguments.paramLeftOver("gm2dl-output", outputFilename, "", "name of the output file");
50 
51  commandLineArguments.parseArgs(argc, argv);
52 
53  OptimizableGraph inputGraph;
54  bool loadStatus = inputGraph.load(inputFilename.c_str());
55  if (! loadStatus) {
56  cerr << "Error while loading input data" << endl;
57  return 1;
58  }
59 
60  OptimizableGraph outputGraph;
61 
62  // process all the vertices first
63  double fx = -1;
64  double baseline = -1;
65  bool firstCam = true;
66  for (OptimizableGraph::VertexIDMap::const_iterator it = inputGraph.vertices().begin(); it != inputGraph.vertices().end(); ++it) {
67  if (dynamic_cast<VertexCam*>(it->second)) {
68  VertexCam* v = static_cast<VertexCam*>(it->second);
69  if (firstCam) {
70  firstCam = false;
72  camParams->setId(0);
73  const SBACam& c = v->estimate();
74  baseline = c.baseline;
75  fx = c.Kcam(0,0);
76  camParams->setKcam(c.Kcam(0,0), c.Kcam(1,1), c.Kcam(0,2), c.Kcam(1,2));
77  outputGraph.addParameter(camParams);
78  }
79 
80  VertexSE3* ov = new VertexSE3;
81  ov->setId(v->id());
82  Eigen::Isometry3d p;
83  p = v->estimate().rotation();
84  p.translation() = v->estimate().translation();
85  ov->setEstimate(p);
86  if (! outputGraph.addVertex(ov)) {
87  assert(0 && "Failure adding camera vertex");
88  }
89  }
90  else if (dynamic_cast<VertexSBAPointXYZ*>(it->second)) {
91  VertexSBAPointXYZ* v = static_cast<VertexSBAPointXYZ*>(it->second);
92 
94  ov->setId(v->id());
95  ov->setEstimate(v->estimate());
96  if (! outputGraph.addVertex(ov)) {
97  assert(0 && "Failure adding camera vertex");
98  }
99  }
100  }
101 
102  for (OptimizableGraph::EdgeSet::const_iterator it = inputGraph.edges().begin(); it != inputGraph.edges().end(); ++it) {
103  if (dynamic_cast<EdgeProjectP2SC*>(*it)) {
104  EdgeProjectP2SC* e = static_cast<EdgeProjectP2SC*>(*it);
105 
107  oe->vertices()[0] = outputGraph.vertex(e->vertices()[1]->id());
108  oe->vertices()[1] = outputGraph.vertex(e->vertices()[0]->id());
109 
110  double kx = e->measurement().x();
111  double ky = e->measurement().y();
112  double disparity = kx - e->measurement()(2);
113 
114  oe->setMeasurement(Eigen::Vector3d(kx, ky, disparity / (fx * baseline)));
115  oe->setInformation(e->information()); // TODO convert information matrix
116  oe->setParameterId(0,0);
117  if (! outputGraph.addEdge(oe)) {
118  assert(0 && "error adding edge");
119  }
120  }
121  }
122 
123  cout << "Vertices in/out:\t" << inputGraph.vertices().size() << " " << outputGraph.vertices().size() << endl;
124  cout << "Edges in/out:\t" << inputGraph.edges().size() << " " << outputGraph.edges().size() << endl;
125 
126  cout << "Writing output ... " << flush;
127  outputGraph.save(outputFilename.c_str());
128  cout << "done." << endl;
129  return 0;
130 }
int id() const
returns the id
Definition: hyper_graph.h:148
double baseline
Definition: sbacam.h:64
Command line parsing of argc and argv.
Definition: command_args.h:46
parameters for a camera
Eigen::Matrix3d Kcam
Definition: sbacam.h:63
const VertexIDMap & vertices() const
Definition: hyper_graph.h:225
Point vertex, XYZ.
Definition: types_sba.h:137
void setId(int id_)
Definition: parameter.cpp:35
Vertex * vertex(int id)
returns the vertex number id appropriately casted
SBACam Vertex, (x,y,z,qw,qx,qy,qz) the parameterization for the increments constructed is a 6d vector...
Definition: types_sba.h:71
virtual void setId(int id)
sets the id of the node in the graph be sure that the graph keeps consistent after changing the id ...
edge from a track to a depth camera node using a disparity measurement
bool setParameterId(int argNum, int paramId)
const VertexContainer & vertices() const
Definition: hyper_graph.h:178
bool parseArgs(int argc, char **argv, bool exitOnError=true)
virtual bool save(std::ostream &os, int level=0) const
save the graph to a stream. Again uses the Factory system.
int main(int argc, char **argv)
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)
EIGEN_STRONG_INLINE void setInformation(const InformationType &information)
Definition: base_edge.h:69
const Eigen::Quaterniond & rotation() const
Definition: se3quat.h:97
Vertex for a tracked point in space.
bool addParameter(Parameter *p)
void setEstimate(const EstimateType &et)
set the estimate for the vertex also calls updateCache()
Definition: base_vertex.h:101
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...
EIGEN_STRONG_INLINE const InformationType & information() const
information matrix of the constraint
Definition: base_edge.h:67
virtual void setMeasurement(const Vector3D &m)
const EstimateType & estimate() const
return the current estimate of the vertex
Definition: base_vertex.h:99
3D pose Vertex, represented as an Isometry3D
Definition: vertex_se3.h:50
void setKcam(double fx, double fy, double cx, double cy)
virtual bool addEdge(HyperGraph::Edge *e)
virtual bool addVertex(HyperGraph::Vertex *v, Data *userData)
EIGEN_STRONG_INLINE const Measurement & measurement() const
accessor functions for the measurement represented by the edge
Definition: base_edge.h:75
const Vector3D & translation() const
Definition: se3quat.h:93