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 
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  ---------------------------------------------------------------*/
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  ---------------------------------------------------------------*/
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  m_observations.erase(it);
159  MRPT_END
160 }
161 
162 /*---------------------------------------------------------------
163  getObservationByIndex
164  ---------------------------------------------------------------*/
166 {
167  MRPT_START
168  ASSERT_BELOW_(idx, size());
169  auto it = begin() + idx;
170  return *it;
171  MRPT_END
172 }
174 {
175  MRPT_START
176  ASSERT_BELOW_(idx, size());
177  auto it = begin() + idx;
178  return *it;
179  MRPT_END
180 }
181 
182 /*---------------------------------------------------------------
183  erase
184  ---------------------------------------------------------------*/
186 {
187  MRPT_START
188  ASSERT_(it != end());
189  m_cachedMap.reset();
190 
191  return m_observations.erase(it);
192  MRPT_END
193 }
194 
195 /*---------------------------------------------------------------
196  getObservationBySensorLabel
197  ---------------------------------------------------------------*/
199  const std::string& label, size_t idx) const
200 {
201  MRPT_START
202 
203  size_t foundCount = 0;
204  for (const auto& it : *this)
205  if (!os::_strcmpi(it->sensorLabel.c_str(), label.c_str()))
206  if (foundCount++ == idx) return it;
207 
208  return CObservation::Ptr();
209 
210  MRPT_END
211 }
212 
213 /*---------------------------------------------------------------
214  swap
215  ---------------------------------------------------------------*/
217 {
218  m_observations.swap(sf.m_observations);
219  std::swap(m_cachedMap, sf.m_cachedMap);
220 }
221 
222 /*---------------------------------------------------------------
223  eraseByLabel
224  ---------------------------------------------------------------*/
225 void CSensoryFrame::eraseByLabel(const std::string& label)
226 {
227  for (auto it = begin(); it != end();)
228  {
229  if (!os::_strcmpi((*it)->sensorLabel.c_str(), label.c_str()))
230  {
231  it = erase(it);
232  }
233  else
234  it++;
235  }
236  m_cachedMap.reset();
237 }
238 
239 namespace mrpt::obs
240 {
241 // Tricky way to call to a library that depends on us, a sort of "run-time"
242 // linking: ptr_internal_build_points_map_from_scan2D is a functor in
243 // "mrpt-obs", set by "mrpt-maps" at its startup.
244 using scan2pts_functor = void (*)(
246  mrpt::maps::CMetricMap::Ptr& out_map, const void* insertOps);
248 // CObservation2DRangeScan.cpp
249 } // namespace mrpt::obs
250 /*---------------------------------------------------------------
251  internal_buildAuxPointsMap
252  ---------------------------------------------------------------*/
253 void CSensoryFrame::internal_buildAuxPointsMap(const void* options) const
254 {
256  throw std::runtime_error(
257  "[CSensoryFrame::buildAuxPointsMap] ERROR: This function needs "
258  "linking against mrpt-maps.\n");
259 
260  for (const auto& it : *this)
262  (*ptr_internal_build_points_map_from_scan2D)(
263  dynamic_cast<CObservation2DRangeScan&>(*it.get()), m_cachedMap,
264  options);
265 }
266 
268  mrpt::maps::CMetricMap* theMap, const CPose3D* robotPose) const
269 {
270  bool anyone = false;
271  for (const auto& it : *this)
272  anyone |= it->insertObservationInto(theMap, robotPose);
273  return anyone;
274 }
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.
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:146
std::shared_ptr< CObservation > Ptr
Definition: CObservation.h:45
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.
#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: 7fcfece08 Sat Feb 22 22:47:46 2020 +0100 at sáb feb 22 23:00:10 CET 2020