Main MRPT website > C++ reference for MRPT 1.5.6
CSimpleMap.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "obs-precomp.h" // Precompiled headers
11 
12 #include <mrpt/maps/CSimpleMap.h>
15 #include <mrpt/utils/CStream.h>
16 
17 using namespace mrpt::obs;
18 using namespace mrpt::maps;
19 using namespace mrpt::utils;
20 using namespace mrpt::poses;
21 using namespace mrpt::poses;
22 using namespace std;
23 
25 using namespace mrpt::utils::metaprogramming;
26 
28 
29 /*---------------------------------------------------------------
30  Constructor
31  ---------------------------------------------------------------*/
32 CSimpleMap::CSimpleMap() : m_posesObsPairs()
33 {
34 }
35 
36 /*---------------------------------------------------------------
37  Copy
38  ---------------------------------------------------------------*/
39 CSimpleMap::CSimpleMap( const CSimpleMap &o ) :
40  m_posesObsPairs( o.m_posesObsPairs )
41 {
42  for_each( m_posesObsPairs.begin(), m_posesObsPairs.end(), ObjectPairMakeUnique() );
43 }
44 
45 /*---------------------------------------------------------------
46  Copy
47  ---------------------------------------------------------------*/
49 {
51 
52  //TPosePDFSensFramePair pair;
53 
54  if (this == &o) return *this; // It may be used sometimes
55 
57  for_each( m_posesObsPairs.begin(), m_posesObsPairs.end(), ObjectPairMakeUnique() );
58 
59  return *this;
60 
61  MRPT_END
62 }
63 
64 /*---------------------------------------------------------------
65  size
66  ---------------------------------------------------------------*/
67 size_t CSimpleMap::size() const
68 {
69  return m_posesObsPairs.size();
70 }
71 
72 bool CSimpleMap::empty() const {
73  return m_posesObsPairs.empty();
74 }
75 
76 /*---------------------------------------------------------------
77  clear
78  ---------------------------------------------------------------*/
80 {
81  m_posesObsPairs.clear();
82 }
83 
84 /*---------------------------------------------------------------
85  Destructor
86  ---------------------------------------------------------------*/
88 {
89  clear();
90 }
91 
92 /*---------------------------------------------------------------
93  get const
94  ---------------------------------------------------------------*/
96  size_t index,
97  CPose3DPDFPtr &out_posePDF,
98  CSensoryFramePtr &out_SF ) const
99 {
100  if (index>=m_posesObsPairs.size())
101  THROW_EXCEPTION("Index out of bounds");
102 
103  out_posePDF = m_posesObsPairs[index].first;
104  out_SF = m_posesObsPairs[index].second;
105 }
106 
107 /*---------------------------------------------------------------
108  remove
109  ---------------------------------------------------------------*/
111 {
112  MRPT_START
113 
114  if (index>=m_posesObsPairs.size())
115  THROW_EXCEPTION("Index out of bounds");
116 
117  m_posesObsPairs.erase( m_posesObsPairs.begin() + index );
118 
119  MRPT_END
120 }
121 
122 
123 /*---------------------------------------------------------------
124  set
125  ---------------------------------------------------------------*/
126 void CSimpleMap::set(
127  size_t index,
128  const CPose3DPDFPtr &in_posePDF,
129  const CSensoryFramePtr & in_SF )
130 {
131  MRPT_START
132 
133  if (index>=m_posesObsPairs.size())
134  THROW_EXCEPTION("Index out of bounds");
135 
136  if (in_posePDF) m_posesObsPairs[index].first = in_posePDF;
137  if (in_SF) m_posesObsPairs[index].second = in_SF;
138 
139  MRPT_END
140 }
141 
142 /*---------------------------------------------------------------
143  set 2D
144  ---------------------------------------------------------------*/
145 void CSimpleMap::set(
146  size_t index,
147  const CPosePDFPtr &in_posePDF,
148  const CSensoryFramePtr &in_SF )
149 {
150  MRPT_START
151 
152  if (index>=m_posesObsPairs.size())
153  THROW_EXCEPTION("Index out of bounds");
154 
155  if (in_posePDF) m_posesObsPairs[index].first = CPose3DPDFPtr( CPose3DPDF::createFrom2D( *in_posePDF ) );
156  if (in_SF) m_posesObsPairs[index].second = in_SF;
157 
158  MRPT_END
159 }
160 
161 
162 /*---------------------------------------------------------------
163  insert
164  ---------------------------------------------------------------*/
165 void CSimpleMap::insert( const CPose3DPDF *in_posePDF, const CSensoryFramePtr &in_SF )
166 {
167  MRPT_START
168 
170 
171  pair.second = in_SF;
172  pair.first = CPose3DPDFPtr( static_cast<CPose3DPDF*>(in_posePDF->duplicate()) );
173 
174  m_posesObsPairs.push_back( pair );
175 
176  MRPT_END
177 }
178 
179 /*---------------------------------------------------------------
180  insert
181  ---------------------------------------------------------------*/
182 void CSimpleMap::insert(
183  const CPose3DPDFPtr &in_posePDF,
184  const CSensoryFramePtr &in_SF )
185 {
186  MRPT_START
187 
189 
190  pair.second = in_SF;
191  pair.first = in_posePDF;
192 
193  m_posesObsPairs.push_back( pair );
194 
195  MRPT_END
196 }
197 
198 /*---------------------------------------------------------------
199  insert
200  ---------------------------------------------------------------*/
201 void CSimpleMap::insert( const CPose3DPDF *in_posePDF, const CSensoryFrame &in_SF )
202 {
203  MRPT_START
204 
206 
207  pair.second = CSensoryFramePtr( new CSensoryFrame(in_SF) );
208  pair.first = CPose3DPDFPtr( static_cast<CPose3DPDF*>(in_posePDF->duplicate()) );
209 
210  m_posesObsPairs.push_back( pair );
211 
212  MRPT_END
213 }
214 
215 /*---------------------------------------------------------------
216  insert
217  ---------------------------------------------------------------*/
218 void CSimpleMap::insert( const CPosePDF *in_posePDF, const CSensoryFrame &in_SF )
219 {
220  MRPT_START
221 
223 
224  pair.second = CSensoryFramePtr( new CSensoryFrame(in_SF) );
225  pair.first = CPose3DPDFPtr( static_cast<CPose3DPDF*>(in_posePDF->duplicate()) );
226 
227  m_posesObsPairs.push_back( pair );
228 
229  MRPT_END
230 }
231 
232 /*---------------------------------------------------------------
233  insert
234  ---------------------------------------------------------------*/
235 void CSimpleMap::insert( const CPosePDF *in_posePDF, const CSensoryFramePtr &in_SF )
236 {
237  MRPT_START
238 
240 
241  pair.second = in_SF;
242  pair.first = CPose3DPDFPtr( static_cast<CPose3DPDF*>(in_posePDF->duplicate()) );
243 
244  m_posesObsPairs.push_back( pair );
245 
246  MRPT_END
247 }
248 
249 /*---------------------------------------------------------------
250  insert 2D
251  ---------------------------------------------------------------*/
252 void CSimpleMap::insert(
253  const CPosePDFPtr &in_posePDF,
254  const CSensoryFramePtr &in_SF )
255 {
256  insert( CPose3DPDFPtr( CPose3DPDF::createFrom2D( *in_posePDF ) ) ,in_SF);
257 }
258 
259 /*---------------------------------------------------------------
260  writeToStream
261  Implements the writing to a CStream capability of
262  CSerializable objects
263  ---------------------------------------------------------------*/
265 {
266  if (version)
267  *version = 1;
268  else
269  {
270  uint32_t i,n;
271  n = m_posesObsPairs.size();
272  out << n;
273  for (i=0;i<n;i++)
274  out << *m_posesObsPairs[i].first << *m_posesObsPairs[i].second;
275  }
276 }
277 
278 /*---------------------------------------------------------------
279  readFromStream
280  ---------------------------------------------------------------*/
282 {
283  switch(version)
284  {
285  case 1:
286  {
287  uint32_t i,n;
288  clear();
289  in >> n;
290  m_posesObsPairs.resize(n);
291  for (i=0;i<n;i++)
292  in >> m_posesObsPairs[i].first >> m_posesObsPairs[i].second;
293  } break;
294  case 0:
295  {
296  // There are 2D poses PDF instead of 3D: transform them:
297  uint32_t i,n;
298  clear();
299  in >> n;
300  m_posesObsPairs.resize(n);
301  for (i=0;i<n;i++)
302  {
303  CPosePDFPtr aux2Dpose;
304  in >> aux2Dpose >> m_posesObsPairs[i].second;
305  m_posesObsPairs[i].first = CPose3DPDFPtr( CPose3DPDF::createFrom2D( *aux2Dpose ) );
306  }
307  } break;
308  default:
310 
311  };
312 }
313 
314 
315 /*---------------------------------------------------------------
316  changeCoordinatesOrigin
317  ---------------------------------------------------------------*/
319 {
321  it->first->changeCoordinatesReference(newOrigin);
322 }
323 
324 /** Save this object to a .simplemap binary file (compressed with gzip)
325 * \sa loadFromFile
326 * \return false on any error.
327 */
328 bool CSimpleMap::saveToFile(const std::string &filName) const
329 {
330  try
331  {
333  f << *this;
334  return true;
335  }
336  catch (...)
337  {
338  return false;
339  }
340 }
341 
342 /** Load the contents of this object from a .simplemap binary file (possibly compressed with gzip)
343 * \sa saveToFile
344 * \return false on any error.
345 */
347 {
348  try
349  {
351  f >> *this;
352  return true;
353  }
354  catch (...)
355  {
356  return false;
357  }
358 }
std::pair< mrpt::poses::CPose3DPDFPtr, mrpt::obs::CSensoryFramePtr > TPosePDFSensFramePair
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
#define THROW_EXCEPTION(msg)
Scalar * iterator
Definition: eigen_plugins.h:23
void get(size_t index, mrpt::poses::CPose3DPDFPtr &out_posePDF, mrpt::obs::CSensoryFramePtr &out_SF) const
Access to the i'th pair, first one is index '0'.
Definition: CSimpleMap.cpp:95
bool saveToFile(const std::string &filName) const
Save this object to a .simplemap binary file (compressed with gzip)
Definition: CSimpleMap.cpp:328
CSimpleMap & operator=(const CSimpleMap &o)
Copy.
Definition: CSimpleMap.cpp:48
bool loadFromFile(const std::string &filName)
Load the contents of this object from a .simplemap binary file (possibly compressed with gzip) ...
Definition: CSimpleMap.cpp:346
GLuint in
Definition: glew.h:7146
size_t size() const
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:67
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
GLsizei n
Definition: glew.h:5051
#define MRPT_END
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Transparently opens a compressed "gz" file and reads uncompressed data from it.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
int version
Definition: mrpt_jpeglib.h:898
GLint * first
Definition: glew.h:1433
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:39
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
Definition: CSimpleMap.cpp:281
#define MRPT_START
An object for making smart pointers unique (ie, making copies if necessary), intended for being used ...
virtual CObject * duplicate() const =0
Returns a copy of the object, indepently of its class.
#define CFileGZOutputStream
Saves data to a file and transparently compress the data using the given compression level...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
GLuint index
Definition: glew.h:1721
virtual ~CSimpleMap()
Destructor:
Definition: CSimpleMap.cpp:87
bool empty() const
Returns size()!=0.
Definition: CSimpleMap.cpp:72
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
Definition: CSimpleMap.cpp:264
void insert(const mrpt::poses::CPose3DPDF *in_posePDF, const mrpt::obs::CSensoryFrame &in_SF)
Add a new pair to the sequence.
Definition: CSimpleMap.cpp:201
unsigned __int32 uint32_t
Definition: rptypes.h:49
void changeCoordinatesOrigin(const mrpt::poses::CPose3D &newOrigin)
Change the coordinate origin of all stored poses, for consistency with future new poses to enter in t...
Definition: CSimpleMap.cpp:318
void set(size_t index, const mrpt::poses::CPose3DPDFPtr &in_posePDF, const mrpt::obs::CSensoryFramePtr &in_SF)
Changes the i'th pair, first one is index '0'.
TPosePDFSensFramePairList m_posesObsPairs
The stored data.
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:40
void clear()
Remove all stored pairs.
Definition: CSimpleMap.cpp:79
void remove(size_t index)
Deletes the i'th pair, first one is index '0'.
Definition: CSimpleMap.cpp:110
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018