g2o
parameter_container.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 "parameter_container.h"
28 
29 #include <iostream>
30 
31 #include "factory.h"
32 #include "parameter.h"
33 
34 #include "g2o/stuff/macros.h"
35 #include "g2o/stuff/color_macros.h"
36 #include "g2o/stuff/string_tools.h"
37 
38 namespace g2o {
39 
40  using namespace std;
41 
43  _isMainStorage(isMainStorage_)
44  {
45  }
46 
48  if (!_isMainStorage)
49  return;
50  for (iterator it = begin(); it!=end(); it++){
51  delete it->second;
52  }
53  BaseClass::clear();
54  }
55 
57  clear();
58  }
59 
61  if (p->id()<0)
62  return false;
63  iterator it=find(p->id());
64  if (it!=end())
65  return false;
66  insert(make_pair(p->id(), p));
67  return true;
68  }
69 
71  iterator it=find(id);
72  if (it==end())
73  return 0;
74  return it->second;
75  }
76 
78  const_iterator it=find(id);
79  if (it==end())
80  return 0;
81  return it->second;
82  }
83 
85  iterator it=find(id);
86  if (it==end())
87  return 0;
88  Parameter* p=it->second;
89  erase(it);
90  return p;
91  }
92 
93  bool ParameterContainer::write(std::ostream& os) const{
94  Factory* factory = Factory::instance();
95  for (const_iterator it=begin(); it!=end(); it++){
96  os << factory->tag(it->second) << " ";
97  os << it->second->id() << " ";
98  it->second->write(os);
99  os << endl;
100  }
101  return true;
102  }
103 
104  bool ParameterContainer::read(std::istream& is, const std::map<std::string, std::string>* _renamedTypesLookup){
105  stringstream currentLine;
106  string token;
107 
108  Factory* factory = Factory::instance();
109  HyperGraph::GraphElemBitset elemBitset;
110  elemBitset[HyperGraph::HGET_PARAMETER] = 1;
111 
112  while (1) {
113  int bytesRead = readLine(is, currentLine);
114  if (bytesRead == -1)
115  break;
116  currentLine >> token;
117  if (bytesRead == 0 || token.size() == 0 || token[0] == '#')
118  continue;
119  if (_renamedTypesLookup && _renamedTypesLookup->size()>0){
120  map<string, string>::const_iterator foundIt = _renamedTypesLookup->find(token);
121  if (foundIt != _renamedTypesLookup->end()) {
122  token = foundIt->second;
123  }
124  }
125 
126  HyperGraph::HyperGraphElement* element = factory->construct(token, elemBitset);
127  if (! element) // not a parameter or otherwise unknown tag
128  continue;
129  assert(element->elementType() == HyperGraph::HGET_PARAMETER && "Should be a param");
130 
131  Parameter* p = static_cast<Parameter*>(element);
132  int pid;
133  currentLine >> pid;
134  p->setId(pid);
135  bool r = p->read(currentLine);
136  if (! r) {
137  cerr << __PRETTY_FUNCTION__ << ": Error reading data " << token << " for parameter " << pid << endl;
138  delete p;
139  } else {
140  if (! addParameter(p) ){
141  cerr << __PRETTY_FUNCTION__ << ": Parameter of type:" << token << " id:" << pid << " already defined" << endl;
142  }
143  }
144  } // while read line
145 
146  return true;
147  }
148 
149 } // end namespace
virtual bool read(std::istream &is, const std::map< std::string, std::string > *renamedMap=0)
read parameters from a stream
Parameter * detachParameter(int id)
remove a parameter from the container, i.e., the user now owns the pointer
#define __PRETTY_FUNCTION__
Definition: macros.h:89
virtual bool write(std::ostream &os) const
write the data to a stream
static Factory * instance()
return the instance
Definition: factory.cpp:61
std::bitset< HyperGraph::HGET_NUM_ELEMS > GraphElemBitset
Definition: hyper_graph.h:74
Parameter * getParameter(int id)
return a parameter based on its ID
HyperGraph::HyperGraphElement * construct(const std::string &tag) const
Definition: factory.cpp:147
SlamParser::Parser::token token
void setId(int id_)
Definition: parameter.cpp:35
const std::string & tag(const HyperGraph::HyperGraphElement *v) const
return the TAG given a vertex
Definition: factory.cpp:157
create vertices and edges based on TAGs in, for example, a file
Definition: factory.h:49
int id() const
Definition: parameter.h:45
int readLine(std::istream &is, std::stringstream &currentLine)
virtual HyperGraphElementType elementType() const =0
ParameterContainer(bool isMainStorage_=true)
bool addParameter(Parameter *p)
add parameter to the container
HGET_PARAMETER
Definition: hyper_graph.h:63