MRPT  1.9.9
CSensoryFrame.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-2019, 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 
13 #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 
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 (auto 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  auto it = begin() + idx;
157  ASSERT_(!*it);
158  // delete (*it);
159  m_observations.erase(it);
160  MRPT_END
161 }
162 
163 /*---------------------------------------------------------------
164  getObservationByIndex
165  ---------------------------------------------------------------*/
167  const size_t& idx) const
168 {
169  MRPT_START
170  ASSERT_BELOW_(idx, size());
171  auto it = begin() + idx;
172  return *it;
173  MRPT_END
174 }
176 {
177  MRPT_START
178  ASSERT_BELOW_(idx, size());
179  auto it = begin() + idx;
180  return *it;
181  MRPT_END
182 }
183 
184 /*---------------------------------------------------------------
185  erase
186  ---------------------------------------------------------------*/
188 {
189  MRPT_START
190  ASSERT_(it != end());
191  m_cachedMap.reset();
192 
193  return m_observations.erase(it);
194  MRPT_END
195 }
196 
197 /*---------------------------------------------------------------
198  getObservationBySensorLabel
199  ---------------------------------------------------------------*/
201  const std::string& label, const size_t& idx) const
202 {
203  MRPT_START
204 
205  size_t foundCount = 0;
206  for (const auto& it : *this)
207  if (!os::_strcmpi(it->sensorLabel.c_str(), label.c_str()))
208  if (foundCount++ == idx) return it;
209 
210  return CObservation::Ptr();
211 
212  MRPT_END
213 }
214 
215 /*---------------------------------------------------------------
216  swap
217  ---------------------------------------------------------------*/
219 {
220  m_observations.swap(sf.m_observations);
221  std::swap(m_cachedMap, sf.m_cachedMap);
222 }
223 
224 /*---------------------------------------------------------------
225  eraseByLabel
226  ---------------------------------------------------------------*/
228 {
229  for (auto it = begin(); it != end();)
230  {
231  if (!os::_strcmpi((*it)->sensorLabel.c_str(), label.c_str()))
232  {
233  it = erase(it);
234  }
235  else
236  it++;
237  }
238  m_cachedMap.reset();
239 }
240 
241 namespace mrpt::obs
242 {
243 // Tricky way to call to a library that depends on us, a sort of "run-time"
244 // linking: ptr_internal_build_points_map_from_scan2D is a functor in
245 // "mrpt-obs", set by "mrpt-maps" at its startup.
246 using scan2pts_functor = void (*)(
248  mrpt::maps::CMetricMap::Ptr& out_map, const void* insertOps);
250 // CObservation2DRangeScan.cpp
251 } // namespace mrpt::obs
252 /*---------------------------------------------------------------
253  internal_buildAuxPointsMap
254  ---------------------------------------------------------------*/
255 void CSensoryFrame::internal_buildAuxPointsMap(const void* options) const
256 {
258  throw std::runtime_error(
259  "[CSensoryFrame::buildAuxPointsMap] ERROR: This function needs "
260  "linking against mrpt-maps.\n");
261 
262  for (const auto& it : *this)
264  (*ptr_internal_build_points_map_from_scan2D)(
265  dynamic_cast<CObservation2DRangeScan&>(*it.get()), m_cachedMap,
266  options);
267 }
268 
270  mrpt::maps::CMetricMap* theMap, const CPose3D* robotPose) const
271 {
272  bool anyone = false;
273  for (const auto& it : *this)
274  anyone |= it->insertObservationInto(theMap, robotPose);
275  return anyone;
276 }
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:241
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...
scan2pts_functor ptr_internal_build_points_map_from_scan2D
#define ASSERT_BELOW_(__A, __B)
Definition: exceptions.h:149
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
CSensoryFrame & operator=(const CSensoryFrame &o)
Copy.
GLenum GLsizei n
Definition: glext.h:5136
mrpt::maps::CMetricMap::Ptr m_cachedMap
A points map, build only under demand by the methods getAuxPointsMap() and buildAuxPointsMap().
Definition: CSensoryFrame.h:72
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:157
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:44
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:40
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:3532
void operator+=(const CSensoryFrame &sf)
You can use "sf1+=sf2;" to add observations in sf2 to sf1.
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
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
Definition: CObject.h:133
std::shared_ptr< CObservation > Ptr
Definition: CObservation.h:45
GLsizei const GLchar ** string
Definition: glext.h:4116
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
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).
const_iterator begin() const
Definition: ts_hash_map.h:229
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:53
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
const CObservation::Ptr & getObservationByIndex(const size_t &idx) const
Returns the i&#39;th observation in the list (0=first).
#define MRPT_END
Definition: exceptions.h:245
GLuint in
Definition: glext.h:7391
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:3934
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
unsigned __int32 uint32_t
Definition: rptypes.h:50
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:182
std::deque< CObservation::Ptr > m_observations
The set of observations taken at the same time instant.
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
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:322
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: 034c2ee2a Tue Aug 20 02:15:02 2019 +0200 at mar ago 20 02:20:10 CEST 2019