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-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 
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 
48 uint8_t CSensoryFrame::serializeGetVersion() const { return 2; }
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 
60  mrpt::serialization::CArchive& in, uint8_t version)
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  ---------------------------------------------------------------*/
105 void CSensoryFrame::operator+=([[maybe_unused]] const CSensoryFrame& sf)
106 {
107  m_cachedMap.reset();
108  for (auto it = begin(); it != end(); ++it)
109  {
110  CObservation::Ptr newObs = *it;
111  newObs.reset(dynamic_cast<CObservation*>(newObs->clone()));
112  m_observations.push_back(
113  newObs); // static_cast<CObservation*>( (*it)->clone()) );
114  }
115 }
116 
117 /*---------------------------------------------------------------
118  operator +=
119  ---------------------------------------------------------------*/
121 {
122  m_cachedMap.reset();
123  m_observations.push_back(obs);
124 }
125 
126 /*---------------------------------------------------------------
127  push_back
128  ---------------------------------------------------------------*/
130 {
131  m_cachedMap.reset();
132  m_observations.push_back(obs);
133 }
134 
135 /*---------------------------------------------------------------
136  insert
137  ---------------------------------------------------------------*/
139 {
140  m_cachedMap.reset();
141  m_observations.push_back(obs);
142 }
143 
144 /*---------------------------------------------------------------
145  eraseByIndex
146  ---------------------------------------------------------------*/
148 {
149  MRPT_START
150  if (idx >= size())
152  "Index %u out of range.", static_cast<unsigned>(idx));
153 
154  m_cachedMap.reset();
155  auto it = begin() + idx;
156  ASSERT_(!*it);
157  m_observations.erase(it);
158  MRPT_END
159 }
160 
161 /*---------------------------------------------------------------
162  getObservationByIndex
163  ---------------------------------------------------------------*/
165 {
166  MRPT_START
167  ASSERT_BELOW_(idx, size());
168  auto it = begin() + idx;
169  return *it;
170  MRPT_END
171 }
173 {
174  MRPT_START
175  ASSERT_BELOW_(idx, size());
176  auto it = begin() + idx;
177  return *it;
178  MRPT_END
179 }
180 
181 /*---------------------------------------------------------------
182  erase
183  ---------------------------------------------------------------*/
185 {
186  MRPT_START
187  ASSERT_(it != end());
188  m_cachedMap.reset();
189 
190  return m_observations.erase(it);
191  MRPT_END
192 }
193 
194 /*---------------------------------------------------------------
195  getObservationBySensorLabel
196  ---------------------------------------------------------------*/
198  const std::string& label, size_t idx) const
199 {
200  MRPT_START
201 
202  size_t foundCount = 0;
203  for (const auto& it : *this)
204  if (!os::_strcmpi(it->sensorLabel.c_str(), label.c_str()))
205  if (foundCount++ == idx) return it;
206 
207  return CObservation::Ptr();
208 
209  MRPT_END
210 }
211 
212 /*---------------------------------------------------------------
213  swap
214  ---------------------------------------------------------------*/
216 {
217  m_observations.swap(sf.m_observations);
218  std::swap(m_cachedMap, sf.m_cachedMap);
219 }
220 
221 /*---------------------------------------------------------------
222  eraseByLabel
223  ---------------------------------------------------------------*/
224 void CSensoryFrame::eraseByLabel(const std::string& label)
225 {
226  for (auto it = begin(); it != end();)
227  {
228  if (!os::_strcmpi((*it)->sensorLabel.c_str(), label.c_str()))
229  {
230  it = erase(it);
231  }
232  else
233  it++;
234  }
235  m_cachedMap.reset();
236 }
237 
238 namespace mrpt::obs
239 {
240 // Tricky way to call to a library that depends on us, a sort of "run-time"
241 // linking: ptr_internal_build_points_map_from_scan2D is a functor in
242 // "mrpt-obs", set by "mrpt-maps" at its startup.
243 using scan2pts_functor = void (*)(
245  mrpt::maps::CMetricMap::Ptr& out_map, const void* insertOps);
247 // CObservation2DRangeScan.cpp
248 } // namespace mrpt::obs
249 /*---------------------------------------------------------------
250  internal_buildAuxPointsMap
251  ---------------------------------------------------------------*/
252 void CSensoryFrame::internal_buildAuxPointsMap(const void* options) const
253 {
255  throw std::runtime_error(
256  "[CSensoryFrame::buildAuxPointsMap] ERROR: This function needs "
257  "linking against mrpt-maps.\n");
258 
259  for (const auto& it : *this)
261  (*ptr_internal_build_points_map_from_scan2D)(
262  dynamic_cast<CObservation2DRangeScan&>(*it.get()), m_cachedMap,
263  options);
264 }
265 
267  mrpt::maps::CMetricMap* theMap, const CPose3D* robotPose) const
268 {
269  bool anyone = false;
270  for (const auto& it : *this)
271  anyone |= it->insertObservationInto(theMap, robotPose);
272  return anyone;
273 }
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 eraseByIndex(size_t idx)
Removes the i&#39;th observation in the list (0=first).
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
size_t size(const MATRIXLIKE &m, const int dim)
#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.
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.
iterator erase(const iterator &it)
Removes the given observation in the list, and return an iterator to the next element (or this->end()...
#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.
const CObservation::Ptr & getObservationByIndex(size_t idx) const
Returns the i&#39;th observation in the list (0=first).
void swap(CSensoryFrame &sf)
Efficiently swaps the contents of two objects.
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:146
std::shared_ptr< CObservation > Ptr
Definition: CObservation.h:45
void operator+=(const CSensoryFrame &sf)
You can use "sf1+=sf2;" to add observations in sf2 to sf1.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
const_iterator end() const
Definition: ts_hash_map.h:246
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
const_iterator begin() const
Definition: ts_hash_map.h:240
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
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:85
mrpt::vision::TStereoCalibResults out
#define MRPT_END
Definition: exceptions.h:245
void(*)(const mrpt::obs::CObservation2DRangeScan &obs, mrpt::maps::CMetricMap::Ptr &out_map, const void *insertOps) scan2pts_functor
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
CObservation::Ptr getObservationBySensorLabel(const std::string &label, size_t idx=0) const
Returns the i&#39;th observation in the list with the given "sensorLabel" (0=first).
size_t ReadBufferFixEndianness(T *ptr, size_t ElementCount)
Reads a sequence of elemental datatypes, taking care of reordering their bytes from the MRPT stream s...
Definition: CArchive.h:94
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:183
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.
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: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020