MRPT  1.9.9
CSensoryFrame.h
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 #pragma once
10 
11 #include <mrpt/maps/CMetricMap.h>
12 #include <mrpt/obs/CObservation.h>
14 
15 namespace mrpt::obs
16 {
17 /** Declares a class for storing a "sensory frame", a set of "observations"
18  * taken by the robot approximately at the same time as one "snapshot" of the
19  * environment.
20  * It can contain "observations" of many different kinds.
21  *
22  * New observations can be added using:
23  *
24  * \code
25  * CObservationXXX::Ptr o = std::make_shared<CObservationXXX>();
26  * // Create
27  * a smart pointer containing an object of class "CObservationXXX"
28  * o->(...)
29  *
30  * CSensoryFrame sf;
31  * sf.insert(o);
32  * \endcode
33  *
34  * The following methods are equivalent for adding new observations to a
35  * "sensory frame":
36  * - CSensoryFrame::operator +=
37  * - CSensoryFrame::push_back
38  * - CSensoryFrame::insert
39  *
40  * To examine the objects within a sensory frame, the following methods exist:
41  * - CSensoryFrame::getObservationByClass : Looks for some specific observation
42  * class.
43  * - CSensoryFrame::begin : To iterate over all observations.
44  * - CSensoryFrame::getObservationByIndex : To query by index.
45  *
46  * Notice that contained observations objects are automatically deleted on
47  * this object's destruction or clear.
48  * \sa CObservation
49  * \ingroup mrpt_obs_grp
50  */
52 {
54 
55  public:
56  /** Default constructor
57  */
58  CSensoryFrame() = default;
59 
60  /** Copy constructor
61  */
63 
64  /** @name Cached points map
65  @{ */
66  protected:
67  /** A points map, build only under demand by the methods getAuxPointsMap()
68  * and buildAuxPointsMap().
69  * It's a generic smart pointer to avoid depending here in the library
70  * mrpt-obs on classes on other libraries.
71  */
73 
74  /** Internal method, used from buildAuxPointsMap() */
75  void internal_buildAuxPointsMap(const void* options = nullptr) const;
76 
77  public:
78  /** Returns the cached points map representation of the scan, if already
79  * build with buildAuxPointsMap(), or nullptr otherwise.
80  * Usage:
81  * \code
82  * mrpt::maps::CPointsMap *map =
83  * obs->getAuxPointsMap<mrpt::maps::CPointsMap>();
84  * \endcode
85  * \sa buildAuxPointsMap
86  */
87  template <class POINTSMAP>
88  inline const POINTSMAP* getAuxPointsMap() const
89  {
90  return static_cast<POINTSMAP*>(m_cachedMap.get());
91  }
92 
93  /** Returns a cached points map representing this laser scan, building it
94  * upon the first call.
95  * \param options Can be nullptr to use default point maps' insertion
96  * options, or a pointer to a "CPointsMap::TInsertionOptions" structure to
97  * override some params.
98  * Usage:
99  * \code
100  * mrpt::maps::CPointsMap *map =
101  * sf->buildAuxPointsMap<mrpt::maps::CPointsMap>(&options or nullptr);
102  * \endcode
103  * \sa getAuxPointsMap
104  */
105  template <class POINTSMAP>
106  inline const POINTSMAP* buildAuxPointsMap(
107  const void* options = nullptr) const
108  {
110  return static_cast<POINTSMAP*>(m_cachedMap.get());
111  }
112 
113  /** @} */
114 
115  /** Copy
116  */
118 
119  /** Clear all current observations.
120  */
121  void clear();
122 
123  /** Insert all the observations in this SF into a metric map or any kind
124  *(see mrpt::maps::CMetricMap).
125  * It calls CObservation::insertObservationInto for all stored observation.
126  * \param theMap The map where this observation is to be inserted: the map
127  *will be updated.
128  * \param robotPose The pose of the robot base for this observation,
129  *relative to the target metric map. Set to nullptr (default) to use
130  *(0,0,0deg)
131  *
132  * \return Returns true if the map has been updated, or false if this
133  *observations
134  * has nothing to do with a metric map (for example, a sound
135  *observation).
136  *
137  * \sa mrpt::maps::CMetricMap, CObservation::insertObservationInto,
138  *CMetricMap::insertObservation
139  */
141  mrpt::maps::CMetricMap* theMap,
142  const mrpt::poses::CPose3D* robotPose = nullptr) const;
143 
144  /** Insert all the observations in this SF into a metric map or any kind
145  *(see mrpt::maps::CMetricMap).
146  * It calls CObservation::insertObservationInto for all stored observation.
147  * \param theMap The map where this observation is to be inserted: the map
148  *will be updated.
149  * \param robotPose The pose of the robot base for this observation,
150  *relative to the target metric map. Set to nullptr (default) to use
151  *(0,0,0deg)
152  *
153  * \return Returns true if the map has been updated, or false if this
154  *observations
155  * has nothing to do with a metric map (for example, a sound
156  *observation).
157  *
158  * \sa mrpt::maps::CMetricMap, CObservation::insertObservationInto,
159  *CMetricMap::insertObservation
160  */
163  const mrpt::poses::CPose3D* robotPose = nullptr) const
164  {
165  return insertObservationsInto(theMap.get(), robotPose);
166  }
167 
168  /** You can use "sf1+=sf2;" to add observations in sf2 to sf1. Objects are
169  * copied, not referenced, thus the source can be safely deleted next.
170  */
171  void operator+=(const CSensoryFrame& sf);
172 
173  /** You can use "sf+=obs;" to add the observation "obs" to the "sf1".
174  * Objects are copied, using the smart pointer, thus the original pointer
175  * can be safely deleted next.
176  */
177  void operator+=(const CObservation::Ptr& obs);
178 
179  /** Inserts a new observation to the list: The pointer to the objects is
180  * copied, thus DO NOT delete the passed object, this class will do at
181  * destructor or when appropriate.
182  */
183  void push_back(const CObservation::Ptr& obs);
184 
185  /** Inserts a new observation to the list: The pointer to the objects is
186  * copied, thus DO NOT delete the passed object, this class will do at
187  * destructor or when appropriate.
188  */
189  void insert(const CObservation::Ptr& obs);
190 
191  /** Returns the i'th observation of a given class (or of a descendant
192  class), or nullptr if there is no such observation in the array.
193  * Example:
194  * \code
195  CObservationImage::Ptr obs =
196  m_SF->getObservationByClass<CObservationImage>();
197  * \endcode
198  * By default (ith=0), the first observation is returned.
199  */
200  template <typename T>
201  typename T::Ptr getObservationByClass(size_t ith = 0) const
202  {
203  MRPT_START
204  size_t foundCount = 0;
205  const mrpt::rtti::TRuntimeClassId* class_ID =
206  &T::GetRuntimeClassIdStatic();
207  for (const auto& it : *this)
208  if (it->GetRuntimeClass()->derivedFrom(class_ID))
209  if (foundCount++ == ith)
210  return std::dynamic_pointer_cast<T>(it);
211  return typename T::Ptr(); // Not found: return empty smart pointer
212  MRPT_END
213  }
214 
215  /** You can use CSensoryFrame::begin to get a iterator to the first element.
216  */
217  using iterator = std::deque<CObservation::Ptr>::iterator;
218 
219  /** You can use CSensoryFrame::begin to get a iterator to the first element.
220  */
221  using const_iterator = std::deque<CObservation::Ptr>::const_iterator;
222 
223  /** Returns a constant iterator to the first observation: this is an example
224  *of usage:
225  * \code
226  * CSensoryFrame sf;
227  * ...
228  * for (CSensoryFrame::const_iterator it=sf.begin();it!=sf.end();++it)
229  * {
230  * (*it)->... // (*it) is a "CObservation*"
231  * }
232  *
233  * \endcode
234  */
235  const_iterator begin() const { return m_observations.begin(); }
236  /** Returns a constant iterator to the end of the list of observations: this
237  *is an example of usage:
238  * \code
239  * CSensoryFrame sf;
240  * ...
241  * for (CSensoryFrame::const_iterator it=sf.begin();it!=sf.end();++it)
242  * {
243  * (*it)->... // (*it) is a "CObservation*"
244  * }
245  *
246  * \endcode
247  */
248  const_iterator end() const { return m_observations.end(); }
249  /** Returns a iterator to the first observation: this is an example of
250  *usage:
251  * \code
252  * CSensoryFrame sf;
253  * ...
254  * for (CSensoryFrame::iterator it=sf.begin();it!=sf.end();++it)
255  * {
256  * (*it)->... // (*it) is a "CObservation*"
257  * }
258  *
259  * \endcode
260  */
261  iterator begin() { return m_observations.begin(); }
262  /** Returns a iterator to the end of the list of observations: this is an
263  *example of usage:
264  * \code
265  * CSensoryFrame sf;
266  * ...
267  * for (CSensoryFrame::iterator it=sf.begin();it!=sf.end();++it)
268  * {
269  * (*it)->... // (*it) is a "CObservation*"
270  * }
271  *
272  * \endcode
273  */
274  inline iterator end() { return m_observations.end(); }
275  /** Returns the number of observations in the list. */
276  inline size_t size() const { return m_observations.size(); }
277  /** Returns true if there are no observations in the list. */
278  inline bool empty() const { return m_observations.empty(); }
279  /** Removes the i'th observation in the list (0=first). */
280  void eraseByIndex(size_t idx);
281 
282  /** Removes the given observation in the list, and return an iterator to the
283  * next element (or this->end() if it was the last one).
284  */
285  iterator erase(const iterator& it);
286 
287  /** Removes all the observations that match a given sensorLabel.
288  */
289  void eraseByLabel(const std::string& label);
290 
291  /** Returns the i'th observation in the list (0=first).
292  * \sa begin, size
293  */
294  const CObservation::Ptr& getObservationByIndex(size_t idx) const;
295  /// \overload
297 
298  /** Returns the i'th observation in the list (0=first), and as a different
299  * smart pointer type:
300  * \code
301  * sf.getObservationByIndexAs<CObservationStereoImages::Ptr>(i);
302  * \endcode
303  * \sa begin, size
304  */
305  template <typename T>
306  T getObservationByIndexAs(size_t idx) const
307  {
308  return std::dynamic_pointer_cast<typename T::element_type>(
309  getObservationByIndex(idx));
310  }
311 
312  /** Returns the i'th observation in the list with the given "sensorLabel"
313  * (0=first).
314  * \return The observation, or nullptr if not found.
315  * \sa begin, size
316  */
318  const std::string& label, size_t idx = 0) const;
319 
320  /** Returns the i'th observation in the list with the given "sensorLabel"
321  * (0=first), and as a different smart pointer type:
322  * \code
323  * sf.getObservationBySensorLabelAs<CObservationStereoImages::Ptr>(i);
324  * \endcode
325  * \sa begin, size
326  */
327  template <typename T>
329  const std::string& label, size_t idx = 0) const
330  {
331  return std::dynamic_pointer_cast<typename T::element_type>(
332  getObservationBySensorLabel(label, idx));
333  }
334 
335  /** Efficiently swaps the contents of two objects.
336  */
337  void swap(CSensoryFrame& sf);
338 
339  protected:
340  /** The set of observations taken at the same time instant. See the top of
341  * this page for instructions on accessing this.
342  */
343  std::deque<CObservation::Ptr> m_observations;
344 
345 }; // End of class def.
346 
347 } // namespace mrpt::obs
void clear()
Clear all current observations.
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...
CSensoryFrame & operator=(const CSensoryFrame &o)
Copy.
A structure that holds runtime class type information.
Definition: CObject.h:31
mrpt::maps::CMetricMap::Ptr m_cachedMap
A points map, build only under demand by the methods getAuxPointsMap() and buildAuxPointsMap().
Definition: CSensoryFrame.h:72
T::Ptr getObservationByClass(size_t ith=0) const
Returns the i&#39;th observation of a given class (or of a descendant class), or nullptr if there is no s...
iterator erase(const iterator &it)
Removes the given observation in the list, and return an iterator to the next element (or this->end()...
T getObservationByIndexAs(size_t idx) const
Returns the i&#39;th observation in the list (0=first), and as a different smart pointer type: ...
CSensoryFrame()=default
Default constructor.
const_iterator begin() const
Returns a constant iterator to the first observation: this is an example of usage: ...
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.
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:51
void operator+=(const CSensoryFrame &sf)
You can use "sf1+=sf2;" to add observations in sf2 to sf1.
const POINTSMAP * buildAuxPointsMap(const void *options=nullptr) const
Returns a cached points map representing this laser scan, building it upon the first call...
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
const POINTSMAP * getAuxPointsMap() const
Returns the cached points map representation of the scan, if already build with buildAuxPointsMap(), or nullptr otherwise.
Definition: CSensoryFrame.h:88
#define MRPT_END
Definition: exceptions.h:245
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:30
iterator begin()
Returns a iterator to the first observation: this is an example of usage:
T getObservationBySensorLabelAs(const std::string &label, size_t idx=0) const
Returns the i&#39;th observation in the list with the given "sensorLabel" (0=first), and as a different s...
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
bool insertObservationsInto(mrpt::maps::CMetricMap::Ptr &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).
const_iterator end() const
Returns a constant iterator to the end of the list of observations: this is an example of usage: ...
iterator end()
Returns a iterator to the end of the list of observations: this is an example of usage: ...
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).
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).
size_t size() const
Returns the number of observations in the list.
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.
bool empty() const
Returns true if there are no observations in the list.
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