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 "../stuff/macros.h"
35 #include "../stuff/color_macros.h"
36 #include "../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  iterator it=find(id);
79  if (it==end())
80  return 0;
81  Parameter* p=it->second;
82  erase(it);
83  return p;
84  }
85 
86  bool ParameterContainer::write(std::ostream& os) const{
87  Factory* factory = Factory::instance();
88  for (const_iterator it=begin(); it!=end(); it++){
89  os << factory->tag(it->second) << " ";
90  os << it->second->id() << " ";
91  it->second->write(os);
92  os << endl;
93  }
94  return true;
95  }
96 
97  bool ParameterContainer::read(std::istream& is, const std::map<std::string, std::string>* _renamedTypesLookup){
98  stringstream currentLine;
99  string token;
100 
101  Factory* factory = Factory::instance();
102  HyperGraph::GraphElemBitset elemBitset;
103  elemBitset[HyperGraph::HGET_PARAMETER] = 1;
104 
105  while (1) {
106  int bytesRead = readLine(is, currentLine);
107  if (bytesRead == -1)
108  break;
109  currentLine >> token;
110  if (bytesRead == 0 || token.size() == 0 || token[0] == '#')
111  continue;
112  if (_renamedTypesLookup && _renamedTypesLookup->size()>0){
113  map<string, string>::const_iterator foundIt = _renamedTypesLookup->find(token);
114  if (foundIt != _renamedTypesLookup->end()) {
115  token = foundIt->second;
116  }
117  }
118 
119  HyperGraph::HyperGraphElement* element = factory->construct(token, elemBitset);
120  if (! element) // not a parameter or otherwise unknown tag
121  continue;
122  assert(element->elementType() == HyperGraph::HGET_PARAMETER && "Should be a param");
123 
124  Parameter* p = static_cast<Parameter*>(element);
125  int pid;
126  currentLine >> pid;
127  p->setId(pid);
128  bool r = p->read(currentLine);
129  if (! r) {
130  cerr << __PRETTY_FUNCTION__ << ": Error reading data " << token << " for parameter " << pid << endl;
131  delete p;
132  } else {
133  if (! addParameter(p) ){
134  cerr << __PRETTY_FUNCTION__ << ": Parameter of type:" << token << " id:" << pid << " already defined" << endl;
135  }
136  }
137  } // while read line
138 
139  return true;
140  }
141 
142 } // 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:95
virtual bool write(std::ostream &os) const
write the data to a stream
static Factory * instance()
return the instance
Definition: factory.cpp:61
int readLine(std::istream &is, std::stringstream &currentLine)
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
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
virtual HyperGraphElementType elementType() const =0
ParameterContainer(bool isMainStorage_=true)
bool addParameter(Parameter *p)
add parameter to the container


orb_slam2_with_maps_odom
Author(s): teng zhang
autogenerated on Fri Sep 25 2020 03:24:47