Main MRPT website > C++ reference for MRPT 1.9.9
CSensoryFrame.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-2018, 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/obs/CSensoryFrame.h>
16 #include <mrpt/system/os.h>
17 #include <iterator>
18 
19 using namespace mrpt::obs;
20 using namespace mrpt::poses;
21 using namespace mrpt::system;
22 using namespace std;
23 
24 IMPLEMENTS_MRPT_OBJECT_ALT_NAME(CSensoryFrame, CSerializable, mrpt::obs, "CSensorialFrame")
25 
26 CSensoryFrame::CSensoryFrame(const CSensoryFrame& o) : m_observations()
27 {
28  *this = o;
29 }
30 
32 {
34  clear();
35  if (this == &o) return *this; // It may be used sometimes
36  m_observations = o.m_observations;
37  m_cachedMap.reset();
38  return *this;
39  MRPT_END
40 }
41 
43 {
44  m_observations.clear();
45  m_cachedMap.reset();
46 }
47 
50 {
51  out.WriteAs<uint32_t>(m_observations.size());
52  for (const auto& o : m_observations)
53  {
54  ASSERT_(o);
55  out << *o;
56  }
57 }
58 
61 {
63  switch (version)
64  {
65  case 0:
66  case 1:
67  case 2:
68  {
69  uint32_t i, n;
71 
72  clear();
73  if (version < 2) // ID was removed in version 2
74  {
75  uint32_t ID;
76  in >> ID;
77  }
78 
79  if (version == 0) in.ReadBufferFixEndianness(&tempTimeStamp, 1);
80 
81  in >> n;
82  m_observations.resize(n);
83  for_each(
84  m_observations.begin(), m_observations.end(),
86  &in));
87 
88  if (version == 0)
89  for (i = 0; i < n; i++)
90  m_observations[i]->timestamp = tempTimeStamp;
91  }
92  break;
93  default:
95  };
96 
97  m_cachedMap.reset();
98 
99  MRPT_END
100 }
101 
102 /*---------------------------------------------------------------
103  operator +=
104  ---------------------------------------------------------------*/
106 {
107  MRPT_UNUSED_PARAM(sf);
108  m_cachedMap.reset();
109  for (const_iterator it = begin(); it != end(); ++it)
110  {
111  CObservation::Ptr newObs = *it;
112  newObs.reset(dynamic_cast<CObservation*>(newObs->clone()));
113  m_observations.push_back(
114  newObs); // static_cast<CObservation*>( (*it)->clone()) );
115  }
116 }
117 
118 /*---------------------------------------------------------------
119  operator +=
120  ---------------------------------------------------------------*/
122 {
123  m_cachedMap.reset();
124  m_observations.push_back(obs);
125 }
126 
127 /*---------------------------------------------------------------
128  push_back
129  ---------------------------------------------------------------*/
131 {
132  m_cachedMap.reset();
133  m_observations.push_back(obs);
134 }
135 
136 /*---------------------------------------------------------------
137  insert
138  ---------------------------------------------------------------*/
140 {
141  m_cachedMap.reset();
142  m_observations.push_back(obs);
143 }
144 
145 /*---------------------------------------------------------------
146  eraseByIndex
147  ---------------------------------------------------------------*/
148 void CSensoryFrame::eraseByIndex(const size_t& idx)
149 {
150  MRPT_START
151  if (idx >= size())
153  "Index %u out of range.", static_cast<unsigned>(idx));
154 
155  m_cachedMap.reset();
156  iterator it = begin() + idx;
157  ASSERT_(!*it);
158  // delete (*it);
159  m_observations.erase(it);
160  MRPT_END
161 }
162 
163 /*---------------------------------------------------------------
164  getObservationByIndex
165  ---------------------------------------------------------------*/
167 {
168  MRPT_START
169  if (idx >= size())
171  "Index %u out of range.", static_cast<unsigned>(idx));
172 
173  const_iterator it = begin() + idx;
174  return *it;
175 
176  MRPT_END
177 }
178 
179 /*---------------------------------------------------------------
180  erase
181  ---------------------------------------------------------------*/
183 {
184  MRPT_START
185  ASSERT_(it != end());
186  m_cachedMap.reset();
187 
188  return m_observations.erase(it);
189  MRPT_END
190 }
191 
192 /*---------------------------------------------------------------
193  getObservationBySensorLabel
194  ---------------------------------------------------------------*/
196  const std::string& label, const size_t& idx) const
197 {
198  MRPT_START
199 
200  size_t foundCount = 0;
201  for (const_iterator it = begin(); it != end(); ++it)
202  if (!os::_strcmpi((*it)->sensorLabel.c_str(), label.c_str()))
203  if (foundCount++ == idx) return *it;
204 
205  return CObservation::Ptr();
206 
207  MRPT_END
208 }
209 
210 /*---------------------------------------------------------------
211  moveFrom
212  ---------------------------------------------------------------*/
214 {
215  copy(
216  sf.m_observations.begin(), sf.m_observations.end(),
217  back_inserter(m_observations));
218  sf.m_observations.clear();
219  m_cachedMap.reset();
220 }
221 
222 /*---------------------------------------------------------------
223  swap
224  ---------------------------------------------------------------*/
226 {
227  m_observations.swap(sf.m_observations);
228  std::swap(m_cachedMap, sf.m_cachedMap);
229 }
230 
231 /*---------------------------------------------------------------
232  eraseByLabel
233  ---------------------------------------------------------------*/
235 {
236  for (iterator it = begin(); it != end();)
237  {
238  if (!os::_strcmpi((*it)->sensorLabel.c_str(), label.c_str()))
239  {
240  it = erase(it);
241  }
242  else
243  it++;
244  }
245  m_cachedMap.reset();
246 }
247 
248 namespace mrpt
249 {
250 namespace obs
251 {
252 // Tricky way to call to a library that depends on us, a sort of "run-time"
253 // linking: ptr_internal_build_points_map_from_scan2D is a functor in
254 // "mrpt-obs", set by "mrpt-maps" at its startup.
255 using scan2pts_functor = void (*)(
257  mrpt::maps::CMetricMap::Ptr& out_map, const void* insertOps);
259 // CObservation2DRangeScan.cpp
260 }
261 }
262 
263 /*---------------------------------------------------------------
264  internal_buildAuxPointsMap
265  ---------------------------------------------------------------*/
266 void CSensoryFrame::internal_buildAuxPointsMap(const void* options) const
267 {
269  throw std::runtime_error(
270  "[CSensoryFrame::buildAuxPointsMap] ERROR: This function needs "
271  "linking against mrpt-maps.\n");
272 
273  for (const_iterator it = begin(); it != end(); ++it)
276  dynamic_cast<CObservation2DRangeScan&>(*it->get()), m_cachedMap,
277  options);
278 }
279 
281  mrpt::maps::CMetricMap* theMap, const CPose3D* robotPose) const
282 {
283  bool anyone = false;
284  for (const_iterator it = begin(); it != end(); ++it)
285  anyone |= (*it)->insertObservationInto(theMap, robotPose);
286  return anyone;
287 }
void clear()
Clear all current observations.
An object for reading objects from a stream, intended for being used in STL algorithms.
void insert(const CObservation::Ptr &obs)
Inserts a new observation to the list: The pointer to the objects is copied, thus DO NOT delete the p...
std::deque< CObservation::Ptr >::iterator iterator
You can use CSensoryFrame::begin to get a iterator to the first element.
#define MRPT_START
Definition: exceptions.h:262
void push_back(const CObservation::Ptr &obs)
Inserts a new observation to the list: The pointer to the objects is copied, thus DO NOT delete the p...
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:25
scan2pts_functor ptr_internal_build_points_map_from_scan2D
void moveFrom(CSensoryFrame &sf)
Copies all the observation from another object, then erase them from the origin object (this method i...
CSensoryFrame & operator=(const CSensoryFrame &o)
Copy.
GLenum GLsizei n
Definition: glext.h:5074
EIGEN_STRONG_INLINE iterator begin()
Definition: eigen_plugins.h:29
mrpt::maps::CMetricMap::Ptr m_cachedMap
A points map, build only under demand by the methods getAuxPointsMap() and buildAuxPointsMap().
Definition: CSensoryFrame.h:75
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
STL namespace.
void WriteAs(const TYPE_FROM_ACTUAL &value)
Definition: CArchive.h:152
iterator erase(const iterator &it)
Removes the given observation in the list, and return an iterator to the next element (or this->end()...
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
void swap(CSensoryFrame &sf)
Efficiently swaps the contents of two objects.
void eraseByIndex(const size_t &idx)
Removes the i&#39;th observation in the list (0=first).
GLuint GLuint end
Definition: glext.h:3528
void operator+=(const CSensoryFrame &sf)
You can use "sf1+=sf2;" to add observations in sf2 to sf1.
std::deque< CObservation::Ptr >::const_iterator const_iterator
You can use CSensoryFrame::begin to get a iterator to the first element.
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:54
std::shared_ptr< CObservation > Ptr
Definition: CObservation.h:45
GLsizei const GLchar ** string
Definition: glext.h:4101
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:15
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
CObservation::Ptr getObservationBySensorLabel(const std::string &label, const size_t &idx=0) const
Returns the i&#39;th observation in the list with the given "sensorLabel" (0=first).
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
#define IMPLEMENTS_MRPT_OBJECT_ALT_NAME(class_name, base, NameSpace, alt_name)
This must be inserted in all CObject classes implementation files.
Definition: CObject.h:217
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:56
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::rtti::CObject) is of the give...
Definition: CObject.h:103
#define MRPT_END
Definition: exceptions.h:266
GLuint in
Definition: glext.h:7274
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:31
void(*)(const mrpt::obs::CObservation2DRangeScan &obs, mrpt::maps::CMetricMap::Ptr &out_map, const void *insertOps) scan2pts_functor
typedef void(APIENTRYP PFNGLBLENDCOLORPROC)(GLclampf red
GLsizeiptr size
Definition: glext.h:3923
CObservation::Ptr getObservationByIndex(const size_t &idx) const
Returns the i&#39;th observation in the list (0=first).
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
unsigned __int32 uint32_t
Definition: rptypes.h:47
bool insertObservationsInto(mrpt::maps::CMetricMap *theMap, const mrpt::poses::CPose3D *robotPose=nullptr) const
Insert all the observations in this SF into a metric map or any kind (see mrpt::maps::CMetricMap).
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:188
std::deque< CObservation::Ptr > m_observations
The set of observations taken at the same time instant.
void eraseByLabel(const std::string &label)
Removes all the observations that match a given sensorLabel.
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
int _strcmpi(const char *str1, const char *str2) noexcept
An OS-independent version of strcmpi.
Definition: os.cpp:320
void internal_buildAuxPointsMap(const void *options=nullptr) const
Internal method, used from buildAuxPointsMap()



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019