Main MRPT website > C++ reference for MRPT 1.5.6
CObservationBeaconRanges.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 
13 #include <mrpt/utils/CStream.h>
15 #include <mrpt/system/os.h>
16 
17 using namespace mrpt::obs;
18 using namespace mrpt::utils;
19 using namespace mrpt::poses;
20 
21 
22 // This must be added to any CSerializable class implementation file.
24 
25 /** Default constructor.
26  */
28  minSensorDistance ( 0 ),
29  maxSensorDistance ( 1e2f ),
30  stdError ( 1e-2f ),
31  sensedData(),
32  auxEstimatePose()
33 {
34 }
35 
36 /*---------------------------------------------------------------
37  Implements the writing to a CStream capability of CSerializable objects
38  ---------------------------------------------------------------*/
40 {
41  if (version)
42  *version = 3;
43  else
44  {
45  uint32_t i,n;
46 
47  // The data
48  out << minSensorDistance << maxSensorDistance << stdError;
49 
50  n = sensedData.size();
51  out << n;
52  for (i=0;i<n;i++)
53  out << sensedData[i].sensorLocationOnRobot << sensedData[i].sensedDistance << sensedData[i].beaconID;
54 
55  out << auxEstimatePose;
56 
57  out << sensorLabel
58  << timestamp;
59  }
60 }
61 
62 /*---------------------------------------------------------------
63  Implements the reading from a CStream capability of CSerializable objects
64  ---------------------------------------------------------------*/
66 {
67  switch(version)
68  {
69  case 0:
70  case 1:
71  case 2:
72  case 3:
73  {
74  uint32_t i,n,id;
75 
76  // The data
77  in >> minSensorDistance >> maxSensorDistance >> stdError;
78 
79  in >> n;
80  sensedData.resize(n);
81  for (i=0;i<n;i++)
82  {
83  in >> sensedData[i].sensorLocationOnRobot >> sensedData[i].sensedDistance;
84  in >> id; sensedData[i].beaconID = id;
85  }
86 
87  if (version>=1)
88  in >> auxEstimatePose;
89 
90  if (version>=2)
91  in >> sensorLabel;
92  else sensorLabel="";
93 
94  if (version>=3)
95  in >> timestamp;
96  else timestamp = INVALID_TIMESTAMP;
97 
98 
99  } break;
100  default:
102 
103  };
104 
105 }
106 
107 /*---------------------------------------------------------------
108  Implements the writing to a CStream capability of CSerializable objects
109  ---------------------------------------------------------------*/
111 {
112  printf("[CObservationBeaconRanges::debugPrintOut] Dumping:\n");
113  printf("[CObservationBeaconRanges::debugPrintOut] minSensorDistance:\t%f\n",minSensorDistance);
114  printf("[CObservationBeaconRanges::debugPrintOut] maxSensorDistance:\t%f:\n",maxSensorDistance);
115  printf("[CObservationBeaconRanges::debugPrintOut] stdError:\t%f\n",stdError);
116  printf("[CObservationBeaconRanges::debugPrintOut] %u ranges:\n",static_cast<unsigned>( sensedData.size() ));
117 
118  size_t i, n = sensedData.size();
119  for (i=0;i<n;i++)
120  printf("[CObservationBeaconRanges::debugPrintOut] \tID[%u]: %f\n",
121  sensedData[i].beaconID,
122  sensedData[i].sensedDistance );
123 }
124 
125 /*---------------------------------------------------------------
126  getSensorPose
127  ---------------------------------------------------------------*/
129 {
130  if (!sensedData.empty())
131  out_sensorPose=CPose3D(sensedData[0].sensorLocationOnRobot);
132  else out_sensorPose = CPose3D(0,0,0);
133 }
134 
135 /*---------------------------------------------------------------
136  setSensorPose
137  ---------------------------------------------------------------*/
139 {
140  size_t i, n = sensedData.size();
141  if (n)
142  for (i=0;i<n;i++)
143  sensedData[i].sensorLocationOnRobot=CPoint3D(newSensorPose);
144 }
145 
146 /*---------------------------------------------------------------
147  getSensedRangeByBeaconID
148  ---------------------------------------------------------------*/
150 {
151  for (size_t i=0;i<sensedData.size();i++)
152  if (sensedData[i].beaconID==beaconID)
153  return sensedData[i].sensedDistance;
154  return 0;
155 }
156 
158 {
159  using namespace std;
161 
162  o << "Auxiliary estimated pose (if available): " << auxEstimatePose << endl;
163 
164  o << format("minSensorDistance=%f m\n",minSensorDistance);
165  o << format("maxSensorDistance=%f m\n",maxSensorDistance);
166  o << format("stdError=%f m\n\n",stdError);
167 
168  o << format("There are %u range measurements:\n\n",(unsigned)sensedData.size());
169 
170  o << " BEACON RANGE SENSOR POSITION ON ROBOT \n";
171  o << "------------------------------------------------\n";
172  for (deque<CObservationBeaconRanges::TMeasurement>::const_iterator it=sensedData.begin(); it!=sensedData.end(); it++)
173  {
174  o << format(" %i %.04f (%.03f,%.03f,%.03f)\n",
175  (int)it->beaconID,it->sensedDistance,
176  it->sensorLocationOnRobot.x(),it->sensorLocationOnRobot.y(),it->sensorLocationOnRobot.z());
177  }
178 }
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
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...
Declares a class derived from "CObservation" that represents one (or more) range measurements to labe...
const Scalar * const_iterator
Definition: eigen_plugins.h:24
float getSensedRangeByBeaconID(int32_t beaconID)
Easy look-up into the vector sensedData, returns the range for a given beacon, or 0 if the beacon is ...
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const MRPT_OVERRIDE
A general method to retrieve the sensor pose on the robot.
GLuint in
Definition: glew.h:7146
virtual void getDescriptionAsText(std::ostream &o) const
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
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
GLuint id
Definition: glew.h:1584
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) MRPT_OVERRIDE
A general method to change the sensor pose on the robot.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
int version
Definition: mrpt_jpeglib.h:898
void getDescriptionAsText(std::ostream &o) const MRPT_OVERRIDE
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
A class used to store a 3D point.
Definition: CPoint3D.h:32
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:17
__int32 int32_t
Definition: rptypes.h:48
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
Declares a class that represents any robot's observation.
void debugPrintOut()
Prints out the contents of the object */.
unsigned __int32 uint32_t
Definition: rptypes.h:49
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092



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