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 {
168  MRPT_START
169  if (idx >= size())
171  "Index %u out of range.", static_cast<unsigned>(idx));
172 
173  auto 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 auto& it : *this)
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  swap
212  ---------------------------------------------------------------*/
214 {
215  m_observations.swap(sf.m_observations);
216  std::swap(m_cachedMap, sf.m_cachedMap);
217 }
218 
219 /*---------------------------------------------------------------
220  eraseByLabel
221  ---------------------------------------------------------------*/
223 {
224  for (auto it = begin(); it != end();)
225  {
226  if (!os::_strcmpi((*it)->sensorLabel.c_str(), label.c_str()))
227  {
228  it = erase(it);
229  }
230  else
231  it++;
232  }
233  m_cachedMap.reset();
234 }
235 
236 namespace mrpt::obs
237 {
238 // Tricky way to call to a library that depends on us, a sort of "run-time"
239 // linking: ptr_internal_build_points_map_from_scan2D is a functor in
240 // "mrpt-obs", set by "mrpt-maps" at its startup.
241 using scan2pts_functor = void (*)(
243  mrpt::maps::CMetricMap::Ptr& out_map, const void* insertOps);
245 // CObservation2DRangeScan.cpp
246 } // namespace mrpt::obs
247 /*---------------------------------------------------------------
248  internal_buildAuxPointsMap
249  ---------------------------------------------------------------*/
250 void CSensoryFrame::internal_buildAuxPointsMap(const void* options) const
251 {
253  throw std::runtime_error(
254  "[CSensoryFrame::buildAuxPointsMap] ERROR: This function needs "
255  "linking against mrpt-maps.\n");
256 
257  for (const auto& it : *this)
259  (*ptr_internal_build_points_map_from_scan2D)(
260  dynamic_cast<CObservation2DRangeScan&>(*it.get()), m_cachedMap,
261  options);
262 }
263 
265  mrpt::maps::CMetricMap* theMap, const CPose3D* robotPose) const
266 {
267  bool anyone = false;
268  for (const auto& it : *this)
269  anyone |= it->insertObservationInto(theMap, robotPose);
270  return anyone;
271 }
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 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
#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
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: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: 8fe78517f Sun Jul 14 19:43:28 2019 +0200 at lun oct 28 02:10:00 CET 2019