Main MRPT website > C++ reference for MRPT 1.5.6
CSensoryFrame.cpp
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 
10 #include "obs-precomp.h" // Precompiled headers
11 
12 #include <mrpt/obs/CSensoryFrame.h>
14 #include <mrpt/utils/CStream.h>
15 #include <mrpt/system/os.h>
16 #include <iterator>
17 
18 using namespace mrpt::obs;
19 using namespace mrpt::poses;
20 using namespace mrpt::utils;
21 using namespace mrpt::system;
22 using namespace std;
23 
25 using namespace mrpt::utils::metaprogramming;
26 
28 
29 /*---------------------------------------------------------------
30  Default constructor
31  ---------------------------------------------------------------*/
33  m_cachedMap(),
34  m_observations()
35 {
36 }
37 
38 /*---------------------------------------------------------------
39  Copy constructor
40  ---------------------------------------------------------------*/
42  m_observations()
43 {
44  *this = o;
45 }
46 
47 /*---------------------------------------------------------------
48  Copy
49  ---------------------------------------------------------------*/
51 {
53 
54  clear();
55 
56  if (this == &o) return *this; // It may be used sometimes
57 
59 
60  m_cachedMap.clear();
61 
62  return *this;
63 
64  MRPT_END
65 
66 }
67 
68 /*---------------------------------------------------------------
69  Destructor
70  ---------------------------------------------------------------*/
72 {
73  clear();
74 }
75 
76 
77 /*---------------------------------------------------------------
78  clear
79  ---------------------------------------------------------------*/
81 {
82  m_observations.clear();
83  m_cachedMap.clear();
84 }
85 
86 /*---------------------------------------------------------------
87  writeToStream
88  ---------------------------------------------------------------*/
90 {
91  if (version)
92  *version = 2;
93  else
94  {
95  uint32_t i,n;
96 
97  n = static_cast<uint32_t>(m_observations.size());
98  out << n;
99  for (i=0;i<n;i++)
100  out << *m_observations[i];
101  }
102 }
103 
104 /*---------------------------------------------------------------
105  readFromStream
106  ---------------------------------------------------------------*/
108 {
109  MRPT_START
110 
111  switch(version)
112  {
113  case 0:
114  case 1:
115  case 2:
116  {
117  uint32_t i,n;
119 
120  clear();
121  if (version<2) // ID was removed in version 2
122  {
123  uint32_t ID;
124  in >> ID;
125  }
126 
127  if (version==0)
128  in.ReadBufferFixEndianness( &tempTimeStamp, 1);
129 
130  in >> n;
131  m_observations.resize(n);
132  for_each( m_observations.begin(), m_observations.end(), ObjectReadFromStream(&in) );
133 
134  if (version==0)
135  for (i=0;i<n;i++)
136  m_observations[i]->timestamp = tempTimeStamp;
137 
138  } break;
139  default:
141 
142  };
143 
144  m_cachedMap.clear();
145 
146  MRPT_END
147 }
148 
149 
150 /*---------------------------------------------------------------
151  operator +=
152  ---------------------------------------------------------------*/
154 {
155  MRPT_UNUSED_PARAM(sf);
156  m_cachedMap.clear();
157  for (const_iterator it = begin();it!=end();++it)
158  {
159  CObservationPtr newObs = *it;
160  newObs.make_unique();
161  m_observations.push_back( newObs ); //static_cast<CObservation*>( (*it)->duplicate()) );
162  }
163 }
164 
165 /*---------------------------------------------------------------
166  operator +=
167  ---------------------------------------------------------------*/
168 void CSensoryFrame::operator += (const CObservationPtr &obs)
169 {
170  m_cachedMap.clear();
171  m_observations.push_back( obs );
172 }
173 
174 /*---------------------------------------------------------------
175  push_back
176  ---------------------------------------------------------------*/
177 void CSensoryFrame::push_back(const CObservationPtr &obs)
178 {
179  m_cachedMap.clear();
180  m_observations.push_back( obs );
181 }
182 
183 /*---------------------------------------------------------------
184  insert
185  ---------------------------------------------------------------*/
186 void CSensoryFrame::insert(const CObservationPtr &obs)
187 {
188  m_cachedMap.clear();
189  m_observations.push_back( obs );
190 }
191 
192 /*---------------------------------------------------------------
193  eraseByIndex
194  ---------------------------------------------------------------*/
195 void CSensoryFrame::eraseByIndex(const size_t &idx)
196 {
197  MRPT_START
198  if (idx>=size()) THROW_EXCEPTION_FMT("Index %u out of range.", static_cast<unsigned>(idx) );
199 
200  m_cachedMap.clear();
201  iterator it = begin()+idx;
202  ASSERT_(it->present());
203  //delete (*it);
204  m_observations.erase( it );
205  MRPT_END
206 }
207 
208 
209 /*---------------------------------------------------------------
210  getObservationByIndex
211  ---------------------------------------------------------------*/
212 CObservationPtr CSensoryFrame::getObservationByIndex( const size_t &idx ) const
213 {
214  MRPT_START
215  if (idx>=size()) THROW_EXCEPTION_FMT("Index %u out of range.", static_cast<unsigned>(idx) );
216 
217  const_iterator it = begin()+idx;
218  return *it;
219 
220  MRPT_END
221 }
222 
223 /*---------------------------------------------------------------
224  erase
225  ---------------------------------------------------------------*/
227 {
228  MRPT_START
229  ASSERT_(it!=end())
230 
231  m_cachedMap.clear();
232 
233  return m_observations.erase(it);
234  MRPT_END
235 }
236 
237 /*---------------------------------------------------------------
238  getObservationBySensorLabel
239  ---------------------------------------------------------------*/
241  const std::string &label,
242  const size_t &idx) const
243 {
244  MRPT_START
245 
246  size_t foundCount = 0;
247  for (const_iterator it = begin();it!=end();++it)
248  if ( !os::_strcmpi( (*it)->sensorLabel.c_str(), label.c_str() ) )
249  if (foundCount++ == idx)
250  return *it;
251 
252  return CObservationPtr();
253 
254  MRPT_END
255 }
256 
257 /*---------------------------------------------------------------
258  moveFrom
259  ---------------------------------------------------------------*/
261 {
262  copy(sf.m_observations.begin(),sf.m_observations.end(), back_inserter(m_observations) );
263  sf.m_observations.clear();
264  m_cachedMap.clear();
265 }
266 
267 /*---------------------------------------------------------------
268  swap
269  ---------------------------------------------------------------*/
271 {
273  std::swap(m_cachedMap, sf.m_cachedMap);
274 }
275 
276 /*---------------------------------------------------------------
277  eraseByLabel
278  ---------------------------------------------------------------*/
280 {
281  for (iterator it = begin();it!=end(); )
282  {
283  if ( !os::_strcmpi( (*it)->sensorLabel.c_str(), label.c_str() ) )
284  {
285  it = erase(it);
286  }
287  else it++;
288  }
289  m_cachedMap.clear();
290 }
291 
292 namespace mrpt
293 {
294  namespace obs
295  {
296  // Tricky way to call to a library that depends on us, a sort of "run-time" linking:
297  // ptr_internal_build_points_map_from_scan2D is a functor in "mrpt-obs", set by "mrpt-maps" at its startup.
298  extern void (*ptr_internal_build_points_map_from_scan2D)(const mrpt::obs::CObservation2DRangeScan &obs, mrpt::maps::CMetricMapPtr &out_map, const void *insertOps);
299  }
300 }
301 
302 
303 /*---------------------------------------------------------------
304  internal_buildAuxPointsMap
305  ---------------------------------------------------------------*/
306 void CSensoryFrame::internal_buildAuxPointsMap( const void *options ) const
307 {
309  throw std::runtime_error("[CSensoryFrame::buildAuxPointsMap] ERROR: This function needs linking against mrpt-maps.\n");
310 
311  for (const_iterator it = begin();it!=end();++it)
314 }
315 
316 
318 {
319  bool anyone = false;
320  for (const_iterator it = begin();it!=end();++it)
321  anyone|= (*it)->insertObservationInto(theMap, robotPose);
322  return anyone;
323 }
324 
bool insertObservationsInto(mrpt::maps::CMetricMap *theMap, const mrpt::poses::CPose3D *robotPose=NULL) const
Insert all the observations in this SF into a metric map or any kind (see mrpt::maps::CMetricMap).
void clear()
Clear all current observations.
std::deque< CObservationPtr > m_observations
The set of observations taken at the same time instant.
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
void internal_buildAuxPointsMap(const void *options=NULL) const
Internal method, used from buildAuxPointsMap()
GLvoid *typedef void(GLAPIENTRY *PFNGLGETVERTEXATTRIBDVPROC)(GLuint
Definition: glew.h:1745
CSensoryFrame()
Default constructor.
The virtual base class which provides a unified interface for all persistent objects in MRPT...
Definition: CSerializable.h:39
void OBS_IMPEXP(* ptr_internal_build_points_map_from_scan2D)(const mrpt::obs::CObservation2DRangeScan &obs, mrpt::maps::CMetricMapPtr &out_map, const void *insertOps)
void moveFrom(CSensoryFrame &sf)
Copies all the observation from another object, then erase them from the origin object (this method i...
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
CSensoryFrame & operator=(const CSensoryFrame &o)
Copy.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
iterator erase(const iterator &it)
Removes the given observation in the list, and return an iterator to the next element (or this->end()...
GLuint in
Definition: glew.h:7146
std::deque< CObservationPtr >::const_iterator const_iterator
You can use CSensoryFrame::begin to get a iterator to the first element.
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
GLsizei n
Definition: glew.h:5051
void swap(CSensoryFrame &sf)
Efficiently swaps the contents of two objects.
#define MRPT_END
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
std::deque< CObservationPtr >::iterator iterator
You can use CSensoryFrame::begin to get a iterator to the first element.
const_iterator end() const
Returns a constant iterator to the end of the list of observations: this is an example of usage: ...
virtual ~CSensoryFrame()
Destructor.
void eraseByIndex(const size_t &idx)
Removes the i'th observation in the list (0=first).
void operator+=(const CSensoryFrame &sf)
You can use "sf1+=sf2;" to add observations in sf2 to sf1.
void insert(const CObservationPtr &obs)
Inserts a new observation to the list: The pointer to the objects is copied, thus DO NOT delete the p...
Declares a class for storing a "sensory frame", a set of "observations" taken by the robot approximat...
An object for reading objects from a stream, intended for being used in STL algorithms.
int version
Definition: mrpt_jpeglib.h:898
const_iterator begin() const
Returns a constant iterator to the first observation: this is an example of usage: ...
size_t size() const
Returns the number of observations in the list.
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:17
CObservationPtr getObservationBySensorLabel(const std::string &label, const size_t &idx=0) const
Returns the i'th observation in the list with the given "sensorLabel" (0=first).
#define MRPT_START
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
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: CStream.h:95
Declares a virtual base class for all metric maps storage classes.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
int BASE_IMPEXP _strcmpi(const char *str1, const char *str2) MRPT_NO_THROWS
An OS-independent version of strcmpi.
Definition: os.cpp:320
#define IS_CLASS(ptrObj, class_name)
Evaluates to true if the given pointer to an object (derived from mrpt::utils::CSerializable) is of t...
Definition: CObject.h:103
#define ASSERT_(f)
mrpt::maps::CMetricMapPtr m_cachedMap
A points map, build only under demand by the methods getAuxPointsMap() and buildAuxPointsMap().
unsigned __int32 uint32_t
Definition: rptypes.h:49
void push_back(const CObservationPtr &obs)
Inserts a new observation to the list: The pointer to the objects is copied, thus DO NOT delete the p...
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
void eraseByLabel(const std::string &label)
Removes all the observations that match a given sensorLabel.
CObservationPtr getObservationByIndex(const size_t &idx) const
Returns the i'th observation in the list (0=first).



Page generated by Doxygen 1.8.6 for MRPT 1.5.6 Git: 4c65e84 Tue Apr 24 08:18:17 2018 +0200 at mar abr 24 08:26:17 CEST 2018