g2o
Loading...
Searching...
No Matches
Static Public Member Functions | Static Public Attributes | List of all members
g2o::Gm2dlIO Class Reference

read / write gm2dl file into / out of a SparseOptimizer More...

#include <gm2dl_io.h>

Static Public Member Functions

static bool readGm2dl (const std::string &filename, SparseOptimizer &optimizer, bool overrideCovariances=false)
 
static bool updateLaserData (SparseOptimizer &optimizer)
 
static bool writeGm2dl (const std::string &filename, const SparseOptimizer &optimizer)
 
static int readRobotLaser (const std::string &filename, DataQueue &queue)
 

Static Public Attributes

static const int ID_LASERPOSE = numeric_limits<int>::max()
 
static const int ID_ODOMCALIB = numeric_limits<int>::max() - 1
 

Detailed Description

read / write gm2dl file into / out of a SparseOptimizer

Definition at line 42 of file gm2dl_io.h.

Member Function Documentation

◆ readGm2dl()

bool g2o::Gm2dlIO::readGm2dl ( const std::string &  filename,
SparseOptimizer &  optimizer,
bool  overrideCovariances = false 
)
static

Definition at line 46 of file gm2dl_io.cpp.

47 {
48 optimizer.clear();
49 ifstream is(filename.c_str());
50 if (!is.good()) return false;
51
52 bool laserOffsetInitDone = false;
53 VertexSE2* laserOffset = new VertexSE2;
54 // laserOffset->fixed() = true;
55 laserOffset->setId(ID_LASERPOSE);
56 if (!optimizer.addVertex(laserOffset)) {
57 cerr << "Unable to add laser offset" << endl;
58 return false;
59 }
60
61 // parse the GM2DL file an extract the vertices, edges, and the laser data
62 stringstream currentLine;
63 VertexSE2* previousVertex = 0;
64 while (1) {
65 int bytesRead = readLine(is, currentLine);
66 if (bytesRead == -1) break;
67 string tag;
68 currentLine >> tag;
69 if (tag == "VERTEX" || tag == "VERTEX2" || tag == "VERTEX_SE2") {
70 int id;
71 Eigen::Vector3d p;
72 currentLine >> id >> p.x() >> p.y() >> p.z();
73 // adding the robot pose
74 VertexSE2* v = new VertexSE2;
75 v->setId(id);
76 // cerr << "Read vertex id " << id << endl;
77 if (!optimizer.addVertex(v)) {
78 cerr << "vertex " << id << " is already in the graph, reassigning "
79 << endl;
80 delete v;
81 v = dynamic_cast<VertexSE2*>(optimizer.vertex(id));
82 assert(v);
83 }
84 v->setEstimate(p);
85 previousVertex = v;
86
87 } else if (tag == "EDGE" || tag == "EDGE2" || tag == "EDGE_SE2") {
88 if (!laserOffsetInitDone) {
89 cerr << "Error: need laser offset" << endl;
90 return false;
91 }
92 int id1, id2;
93 EdgeSE2SensorCalib* e = new EdgeSE2SensorCalib;
94 Eigen::Vector3d p;
95 Eigen::Matrix3d& m = e->information();
96 currentLine >> id1 >> id2 >> p.x() >> p.y() >> p.z();
97 if (overrideCovariances) {
98 m = Eigen::Matrix3d::Identity();
99 } else {
100 if (tag == "EDGE_SE2")
101 currentLine >> m(0, 0) >> m(0, 1) >> m(0, 2) >> m(1, 1) >> m(1, 2) >>
102 m(2, 2);
103 else // old stupid order of the information matrix
104 currentLine >> m(0, 0) >> m(0, 1) >> m(1, 1) >> m(2, 2) >> m(0, 2) >>
105 m(1, 2);
106 m(1, 0) = m(0, 1);
107 m(2, 0) = m(0, 2);
108 m(2, 1) = m(1, 2);
109 }
110 previousVertex = 0;
111 VertexSE2* v1 = dynamic_cast<VertexSE2*>(optimizer.vertex(id1));
112 VertexSE2* v2 = dynamic_cast<VertexSE2*>(optimizer.vertex(id2));
113 if (!v1) {
114 cerr << "vertex " << id1 << " is not existing, cannot add edge (" << id1
115 << "," << id2 << ")" << endl;
116 delete e;
117 continue;
118 }
119 if (!v2) {
120 cerr << "vertex " << id2 << " is not existing, cannot add edge (" << id1
121 << "," << id2 << ")" << endl;
122 delete e;
123 continue;
124 }
125
126 // if (0)
127 // if (abs(id1 - id2) != 1)
128 // m *= 1e-6;
129
130 // TODO transform measurement covariance by considering the laserOffset to
131 // measurement between the lasers
132 SE2 transf;
133 transf.fromVector(p);
134 e->setMeasurement(laserOffset->estimate().inverse() * transf *
135 laserOffset->estimate());
136 // e->inverseMeasurement() = e->measurement().inverse();
137
138 e->setVertex(0, v1);
139 e->setVertex(1, v2);
140 e->setVertex(2, laserOffset);
141 if (!optimizer.addEdge(e)) {
142 cerr << "error in adding edge " << id1 << "," << id2 << endl;
143 delete e;
144 }
145 // cerr << PVAR(e->inverseMeasurement().toVector().transpose()) << endl;
146 // cerr << PVAR(e->information()) << endl;
147
148 } else if (tag == "ROBOTLASER1") {
149 if (previousVertex) {
150 RobotLaser* rl2 = new RobotLaser;
151 rl2->read(currentLine);
152 if (!laserOffsetInitDone) {
153 laserOffsetInitDone = true;
154 // cerr << "g2o Laseroffset is " <<
155 // rl2->laserParams().laserPose.toVector().transpose() << endl;
156 laserOffset->setEstimate(rl2->laserParams().laserPose);
157 }
158 previousVertex->setUserData(rl2);
159 previousVertex = 0;
160 }
161 }
162 }
163
164 return true;
165}
g2o::Gm2dlIO::ID_LASERPOSE
static const int ID_LASERPOSE
Definition gm2dl_io.h:47
g2o::readLine
int readLine(std::istream &is, std::stringstream &currentLine)
Definition string_tools.cpp:161

References g2o::OptimizableGraph::addEdge(), g2o::OptimizableGraph::addVertex(), g2o::SparseOptimizer::clear(), g2o::BaseVertex< D, T >::estimate(), g2o::SE2::fromVector(), ID_LASERPOSE, g2o::BaseEdge< D, E >::information(), g2o::SE2::inverse(), g2o::RawLaser::laserParams(), g2o::LaserParameters::laserPose, g2o::RobotLaser::read(), g2o::readLine(), g2o::BaseVertex< D, T >::setEstimate(), g2o::OptimizableGraph::Vertex::setId(), g2o::EdgeSE2SensorCalib::setMeasurement(), g2o::HyperGraph::DataContainer::setUserData(), g2o::HyperGraph::Edge::setVertex(), and g2o::OptimizableGraph::vertex().

Referenced by main().

◆ readRobotLaser()

int g2o::Gm2dlIO::readRobotLaser ( const std::string &  filename,
DataQueue &  queue 
)
static

Definition at line 252 of file gm2dl_io.cpp.

252 {
253 ifstream is(filename.c_str());
254 if (!is.good()) return false;
255
256 int cnt = 0;
257
258 // parse the GM2DL file and extract the vertices, edges, and the laser data
259 stringstream currentLine;
260 while (1) {
261 int bytesRead = readLine(is, currentLine);
262 if (bytesRead == -1) break;
263 string tag;
264 currentLine >> tag;
265 if (tag == "ROBOTLASER1") {
266 RobotLaser* rl2 = new RobotLaser;
267 rl2->read(currentLine);
268 queue.add(rl2);
269 cnt++;
270 }
271 }
272 return cnt;
273}

References g2o::DataQueue::add(), g2o::RobotLaser::read(), and g2o::readLine().

Referenced by main().

◆ updateLaserData()

bool g2o::Gm2dlIO::updateLaserData ( SparseOptimizer &  optimizer)
static

Definition at line 227 of file gm2dl_io.cpp.

227 {
228 VertexSE2* laserOffset =
229 dynamic_cast<VertexSE2*>(optimizer.vertex(ID_LASERPOSE));
230 if (!laserOffset) {
231 cerr << "Laser offset not found" << endl;
232 return false;
233 }
234
235 for (SparseOptimizer::VertexIDMap::const_iterator it =
236 optimizer.vertices().begin();
237 it != optimizer.vertices().end(); ++it) {
238 VertexSE2* v = dynamic_cast<VertexSE2*>(it->second);
239 if (!v) continue;
240 if (v->id() == ID_LASERPOSE) continue;
241 RobotLaser* robotLaser = dynamic_cast<RobotLaser*>(v->userData());
242 if (robotLaser) { // writing the data via the factory
243 robotLaser->setOdomPose(v->estimate());
244 LaserParameters params = robotLaser->laserParams();
245 params.laserPose = laserOffset->estimate();
246 robotLaser->setLaserParams(params);
247 }
248 }
249 return true;
250}

References g2o::BaseVertex< D, T >::estimate(), g2o::HyperGraph::Vertex::id(), ID_LASERPOSE, g2o::RawLaser::laserParams(), g2o::LaserParameters::laserPose, g2o::RawLaser::setLaserParams(), g2o::RobotLaser::setOdomPose(), g2o::HyperGraph::DataContainer::userData(), g2o::OptimizableGraph::vertex(), and g2o::HyperGraph::vertices().

Referenced by main().

◆ writeGm2dl()

bool g2o::Gm2dlIO::writeGm2dl ( const std::string &  filename,
const SparseOptimizer &  optimizer 
)
static

Definition at line 167 of file gm2dl_io.cpp.

168 {
169 ofstream fout(filename.c_str());
170 if (!fout.good()) {
171 return false;
172 }
173 Factory* factory = Factory::instance();
174
175 for (SparseOptimizer::VertexIDMap::const_iterator it =
176 optimizer.vertices().begin();
177 it != optimizer.vertices().end(); ++it) {
178 OptimizableGraph::Vertex* v =
179 static_cast<OptimizableGraph::Vertex*>(it->second);
180 fout << "VERTEX2 " << v->id() << " ";
181 v->write(fout);
182 fout << endl;
183 HyperGraph::Data* data = v->userData();
184 if (data) { // writing the data via the factory
185 string tag = factory->tag(data);
186 if (tag.size() > 0) {
187 fout << tag << " ";
188 data->write(fout);
189 fout << endl;
190 }
191 }
192 }
193
194 OptimizableGraph::EdgeContainer edgesToSave; // sorting edges to have them in
195 // the order of insertion again
196 for (HyperGraph::EdgeSet::const_iterator it = optimizer.edges().begin();
197 it != optimizer.edges().end(); ++it) {
198 const OptimizableGraph::Edge* e =
199 static_cast<const OptimizableGraph::Edge*>(*it);
200 edgesToSave.push_back(const_cast<OptimizableGraph::Edge*>(e));
201 }
202 sort(edgesToSave.begin(), edgesToSave.end(),
203 OptimizableGraph::EdgeIDCompare());
204
205 for (OptimizableGraph::EdgeContainer::const_iterator it = edgesToSave.begin();
206 it != edgesToSave.end(); ++it) {
207 OptimizableGraph::Edge* e = *it;
208 EdgeSE2SensorCalib* calibEdge = dynamic_cast<EdgeSE2SensorCalib*>(e);
209 if (calibEdge) {
210 // write back in the gm2dl format
211 fout << "EDGE2 " << calibEdge->vertex(0)->id() << " "
212 << calibEdge->vertex(1)->id();
213 Eigen::Vector3d meas = calibEdge->measurement().toVector();
214 fout << " " << meas.x() << " " << meas.y() << " " << meas.z();
215 const Eigen::Matrix3d& m = calibEdge->information();
216 fout << " " << m(0, 0) << " " << m(0, 1) << " " << m(1, 1) << " "
217 << m(2, 2) << " " << m(0, 2) << " " << m(1, 2);
218 fout << endl;
219 } else {
220 // cerr << "Strange Edge Type: " << factory->tag(e) << endl;
221 }
222 }
223
224 return fout.good();
225}
g2o::Factory::instance
static Factory * instance()
return the instance
Definition factory.cpp:46
g2o::OptimizableGraph::EdgeContainer
std::vector< OptimizableGraph::Edge * > EdgeContainer
vector container for edges
Definition optimizable_graph.h:102

References g2o::HyperGraph::edges(), g2o::HyperGraph::Vertex::id(), g2o::BaseEdge< D, E >::information(), g2o::Factory::instance(), g2o::BaseEdge< D, E >::measurement(), g2o::Factory::tag(), g2o::HyperGraph::DataContainer::userData(), g2o::HyperGraph::Edge::vertex(), g2o::HyperGraph::vertices(), g2o::HyperGraph::Data::write(), and g2o::OptimizableGraph::Vertex::write().

Referenced by main().

Member Data Documentation

◆ ID_LASERPOSE

const int g2o::Gm2dlIO::ID_LASERPOSE = numeric_limits<int>::max()
static

the global ID we assign the laser pose vertex

Definition at line 47 of file gm2dl_io.h.

Referenced by main(), readGm2dl(), and updateLaserData().

◆ ID_ODOMCALIB

const int g2o::Gm2dlIO::ID_ODOMCALIB = numeric_limits<int>::max() - 1
static

Definition at line 48 of file gm2dl_io.h.

Referenced by g2o::addOdometryCalibLinksDifferential(), and main().

The documentation for this class was generated from the following files:
Generated on Tue Nov 11 2025 05:18:45 for g2o by doxygen 1.9.8