MRPT  2.0.1
CSimpleMap.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "obs-precomp.h" // Precompiled headers
11 
14 #include <mrpt/maps/CSimpleMap.h>
17 
18 using namespace mrpt::obs;
19 using namespace mrpt::maps;
20 using namespace mrpt::poses;
21 using namespace mrpt::poses;
22 using namespace std;
23 using namespace mrpt::serialization;
25 
27 
28 const auto fn_pair_make_unique = [](auto& ptr) {
29  ptr.first.reset(dynamic_cast<mrpt::poses::CPose3DPDF*>(ptr.first->clone()));
30  ptr.second.reset(
31  dynamic_cast<mrpt::obs::CSensoryFrame*>(ptr.second->clone()));
32 };
33 
34 CSimpleMap::CSimpleMap(const CSimpleMap& o) : m_posesObsPairs(o.m_posesObsPairs)
35 {
36  for_each(
38 }
39 
41 {
43  if (this == &o) return *this; // It may be used sometimes
44 
46  for_each(
48 
49  return *this;
50  MRPT_END
51 }
52 
53 size_t CSimpleMap::size() const { return m_posesObsPairs.size(); }
54 bool CSimpleMap::empty() const { return m_posesObsPairs.empty(); }
57  size_t index, CPose3DPDF::Ptr& out_posePDF,
58  CSensoryFrame::Ptr& out_SF) const
59 {
60  if (index >= m_posesObsPairs.size()) THROW_EXCEPTION("Index out of bounds");
61  out_posePDF = m_posesObsPairs[index].first;
62  out_SF = m_posesObsPairs[index].second;
63 }
64 
65 void CSimpleMap::remove(size_t index)
66 {
68  if (index >= m_posesObsPairs.size()) THROW_EXCEPTION("Index out of bounds");
69  m_posesObsPairs.erase(m_posesObsPairs.begin() + index);
70  MRPT_END
71 }
72 
74  size_t index, const CPose3DPDF::Ptr& in_posePDF,
75  const CSensoryFrame::Ptr& in_SF)
76 {
78  if (index >= m_posesObsPairs.size()) THROW_EXCEPTION("Index out of bounds");
79  if (in_posePDF) m_posesObsPairs[index].first = in_posePDF;
80  if (in_SF) m_posesObsPairs[index].second = in_SF;
81  MRPT_END
82 }
83 
85  size_t index, const CPosePDF::Ptr& in_posePDF,
86  const CSensoryFrame::Ptr& in_SF)
87 {
89 
90  if (index >= m_posesObsPairs.size()) THROW_EXCEPTION("Index out of bounds");
91 
92  if (in_posePDF)
93  m_posesObsPairs[index].first =
94  CPose3DPDF::Ptr(CPose3DPDF::createFrom2D(*in_posePDF));
95  if (in_SF) m_posesObsPairs[index].second = in_SF;
96 
97  MRPT_END
98 }
99 
101  const CPose3DPDF* in_posePDF, const CSensoryFrame::Ptr& in_SF)
102 {
103  MRPT_START
104 
106 
107  pair.second = in_SF;
108  pair.first =
109  CPose3DPDF::Ptr(dynamic_cast<CPose3DPDF*>(in_posePDF->clone()));
110  ASSERT_(pair.first);
111 
112  m_posesObsPairs.push_back(pair);
113 
114  MRPT_END
115 }
116 
117 /*---------------------------------------------------------------
118  insert
119  ---------------------------------------------------------------*/
121  const CPose3DPDF::Ptr& in_posePDF, const CSensoryFrame::Ptr& in_SF)
122 {
123  MRPT_START
124 
126 
127  pair.second = in_SF;
128  pair.first = in_posePDF;
129 
130  m_posesObsPairs.push_back(pair);
131 
132  MRPT_END
133 }
134 
135 /*---------------------------------------------------------------
136  insert
137  ---------------------------------------------------------------*/
139  const CPose3DPDF* in_posePDF, const CSensoryFrame& in_SF)
140 {
141  MRPT_START
142 
144 
145  pair.second = CSensoryFrame::Create(in_SF);
146  pair.first =
147  CPose3DPDF::Ptr(dynamic_cast<CPose3DPDF*>(in_posePDF->clone()));
148  ASSERT_(pair.first);
149 
150  m_posesObsPairs.push_back(pair);
151 
152  MRPT_END
153 }
154 
155 /*---------------------------------------------------------------
156  insert
157  ---------------------------------------------------------------*/
158 void CSimpleMap::insert(const CPosePDF* in_posePDF, const CSensoryFrame& in_SF)
159 {
160  MRPT_START
161 
163 
164  pair.second = CSensoryFrame::Create(in_SF);
165  pair.first =
166  CPose3DPDF::Ptr(dynamic_cast<CPose3DPDF*>(in_posePDF->clone()));
167  ASSERT_(pair.first);
168 
169  m_posesObsPairs.push_back(pair);
170 
171  MRPT_END
172 }
173 
174 /*---------------------------------------------------------------
175  insert
176  ---------------------------------------------------------------*/
178  const CPosePDF* in_posePDF, const CSensoryFrame::Ptr& in_SF)
179 {
180  MRPT_START
181 
183 
184  pair.second = in_SF;
185  pair.first =
186  CPose3DPDF::Ptr(dynamic_cast<CPose3DPDF*>(in_posePDF->clone()));
187  ASSERT_(pair.first);
188 
189  m_posesObsPairs.push_back(pair);
190 
191  MRPT_END
192 }
193 
194 /*---------------------------------------------------------------
195  insert 2D
196  ---------------------------------------------------------------*/
198  const CPosePDF::Ptr& in_posePDF, const CSensoryFrame::Ptr& in_SF)
199 {
200  insert(CPose3DPDF::Ptr(CPose3DPDF::createFrom2D(*in_posePDF)), in_SF);
201 }
202 
204  size_t index, const CPose3DPDF::Ptr& in_posePDF,
205  const CSensoryFrame::Ptr& in_SF)
206 {
207  if (index >= m_posesObsPairs.size()) return;
208 
209  MRPT_START
210 
212 
213  pair.second = in_SF;
214  pair.first = in_posePDF;
215 
216  auto iter = m_posesObsPairs.begin() + index;
217  m_posesObsPairs.insert(iter, pair);
218 
219  MRPT_END
220 }
221 
222 uint8_t CSimpleMap::serializeGetVersion() const { return 1; }
224 {
225  out.WriteAs<uint32_t>(m_posesObsPairs.size());
226  for (const auto& p : m_posesObsPairs) out << *p.first << *p.second;
227 }
228 
230  mrpt::serialization::CArchive& in, uint8_t version)
231 {
232  switch (version)
233  {
234  case 1:
235  {
236  uint32_t i, n;
237  clear();
238  in >> n;
239  m_posesObsPairs.resize(n);
240  for (i = 0; i < n; i++)
241  in >> m_posesObsPairs[i].first >> m_posesObsPairs[i].second;
242  }
243  break;
244  case 0:
245  {
246  // There are 2D poses PDF instead of 3D: transform them:
247  uint32_t i, n;
248  clear();
249  in >> n;
250  m_posesObsPairs.resize(n);
251  for (i = 0; i < n; i++)
252  {
253  CPosePDF::Ptr aux2Dpose;
254  in >> aux2Dpose >> m_posesObsPairs[i].second;
255  m_posesObsPairs[i].first =
256  CPose3DPDF::Ptr(CPose3DPDF::createFrom2D(*aux2Dpose));
257  }
258  }
259  break;
260  default:
262  };
263 }
264 
265 /*---------------------------------------------------------------
266  changeCoordinatesOrigin
267  ---------------------------------------------------------------*/
269 {
270  for (auto& m_posesObsPair : m_posesObsPairs)
271  m_posesObsPair.first->changeCoordinatesReference(newOrigin);
272 }
273 
274 /** Save this object to a .simplemap binary file (compressed with gzip)
275  * \sa loadFromFile
276  * \return false on any error.
277  */
278 bool CSimpleMap::saveToFile(const std::string& filName) const
279 {
280  try
281  {
282  mrpt::io::CFileGZOutputStream fo(filName);
283  archiveFrom(fo) << *this;
284  return true;
285  }
286  catch (...)
287  {
288  return false;
289  }
290 }
291 
292 /** Load the contents of this object from a .simplemap binary file (possibly
293  * compressed with gzip)
294  * \sa saveToFile
295  * \return false on any error.
296  */
297 bool CSimpleMap::loadFromFile(const std::string& filName)
298 {
299  try
300  {
301  mrpt::io::CFileGZInputStream fi(filName);
302  archiveFrom(fi) >> *this;
303  return true;
304  }
305  catch (...)
306  {
307  return false;
308  }
309 }
std::pair< mrpt::poses::CPose3DPDF::Ptr, mrpt::obs::CSensoryFrame::Ptr > TPosePDFSensFramePair
Definition: CSimpleMap.h:176
bool empty() const
Returns size()!=0.
Definition: CSimpleMap.cpp:54
#define MRPT_START
Definition: exceptions.h:241
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:32
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
STL namespace.
CSimpleMap & operator=(const CSimpleMap &o)
Copy.
Definition: CSimpleMap.cpp:40
bool loadFromFile(const std::string &filName)
Load the contents of this object from a .simplemap binary file (possibly compressed with gzip) ...
Definition: CSimpleMap.cpp:297
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT&#39;s CStream, std::istream, std::ostream, std::stringstream.
Definition: CArchive.h:592
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
Definition: CSimpleMap.cpp:229
virtual CObject * clone() const =0
Returns a deep copy (clone) of the object, indepently of its class.
This namespace contains representation of robot actions and observations.
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
Definition: CSensoryFrame.h:51
void set(size_t index, const mrpt::poses::CPose3DPDF::Ptr &in_posePDF, const mrpt::obs::CSensoryFrame::Ptr &in_SF)
Changes the i&#39;th pair, first one is index &#39;0&#39;.
Definition: CSimpleMap.cpp:73
size_t size() const
Returns the count of pairs (pose,sensory data)
Definition: CSimpleMap.cpp:53
void get(size_t index, mrpt::poses::CPose3DPDF::Ptr &out_posePDF, mrpt::obs::CSensoryFrame::Ptr &out_SF) const
Access to the i&#39;th pair, first one is index &#39;0&#39;.
Definition: CSimpleMap.cpp:56
Declares a class that represents a probability density function (pdf) of a 2D pose (x...
Definition: CPosePDF.h:38
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
Definition: CSimpleMap.cpp:222
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::vision::TStereoCalibResults out
#define MRPT_END
Definition: exceptions.h:245
void insertToPos(size_t index, const mrpt::poses::CPose3DPDF::Ptr &in_posePDF, const mrpt::obs::CSensoryFrame::Ptr &in_SF)
Insert a new pair to the sequence, making a copy of the smart pointer (it&#39;s not made unique) to...
Definition: CSimpleMap.cpp:203
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:30
Transparently opens a compressed "gz" file and reads uncompressed data from it.
Saves data to a file and transparently compress the data using the given compression level...
void insert(const mrpt::poses::CPose3DPDF *in_posePDF, const mrpt::obs::CSensoryFrame &in_SF)
Add a new pair to the sequence.
Definition: CSimpleMap.cpp:138
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:268
TPosePDFSensFramePairList m_posesObsPairs
The stored data.
Definition: CSimpleMap.h:203
Declares a class that represents a Probability Density Function (PDF) of a 3D pose (6D actually)...
Definition: CPose3DPDF.h:39
const auto fn_pair_make_unique
Definition: CSimpleMap.cpp:28
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Definition: CSimpleMap.cpp:223
bool saveToFile(const std::string &filName) const
Save this object to a .simplemap binary file (compressed with gzip)
Definition: CSimpleMap.cpp:278
void clear()
Remove all stored pairs.
Definition: CSimpleMap.cpp:55
void remove(size_t index)
Deletes the i&#39;th pair, first one is index &#39;0&#39;.
Definition: CSimpleMap.cpp:65



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020