Main MRPT website > C++ reference for MRPT 1.9.9
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-2018, 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 
14 #include <mrpt/system/os.h>
15 
16 using namespace mrpt::obs;
17 using namespace mrpt::poses;
18 
19 // This must be added to any CSerializable class implementation file.
21 
22 /** Default constructor.
23  */
25  : minSensorDistance(0),
26  maxSensorDistance(1e2f),
27  stdError(1e-2f),
28  sensedData(),
29  auxEstimatePose()
30 {
31 }
32 
36 {
37  uint32_t i, n;
38  // The data
39  out << minSensorDistance << maxSensorDistance << stdError;
40  n = sensedData.size();
41  out << n;
42  for (i = 0; i < n; i++)
43  out << sensedData[i].sensorLocationOnRobot
44  << sensedData[i].sensedDistance << sensedData[i].beaconID;
45  out << auxEstimatePose;
46  out << sensorLabel << timestamp;
47 }
48 
51 {
52  switch (version)
53  {
54  case 0:
55  case 1:
56  case 2:
57  case 3:
58  {
59  uint32_t i, n, id;
60 
61  // The data
62  in >> minSensorDistance >> maxSensorDistance >> stdError;
63 
64  in >> n;
65  sensedData.resize(n);
66  for (i = 0; i < n; i++)
67  {
68  in >> sensedData[i].sensorLocationOnRobot >>
69  sensedData[i].sensedDistance;
70  in >> id;
71  sensedData[i].beaconID = id;
72  }
73 
74  if (version >= 1) in >> auxEstimatePose;
75 
76  if (version >= 2)
77  in >> sensorLabel;
78  else
79  sensorLabel = "";
80 
81  if (version >= 3)
82  in >> timestamp;
83  else
84  timestamp = INVALID_TIMESTAMP;
85  }
86  break;
87  default:
89  };
90 }
91 
93 {
94  printf("[CObservationBeaconRanges::debugPrintOut] Dumping:\n");
95  printf(
96  "[CObservationBeaconRanges::debugPrintOut] minSensorDistance:\t%f\n",
97  minSensorDistance);
98  printf(
99  "[CObservationBeaconRanges::debugPrintOut] maxSensorDistance:\t%f:\n",
100  maxSensorDistance);
101  printf(
102  "[CObservationBeaconRanges::debugPrintOut] stdError:\t%f\n", stdError);
103  printf(
104  "[CObservationBeaconRanges::debugPrintOut] %u ranges:\n",
105  static_cast<unsigned>(sensedData.size()));
106 
107  size_t i, n = sensedData.size();
108  for (i = 0; i < n; i++)
109  printf(
110  "[CObservationBeaconRanges::debugPrintOut] \tID[%u]: %f\n",
111  sensedData[i].beaconID, sensedData[i].sensedDistance);
112 }
113 
114 /*---------------------------------------------------------------
115  getSensorPose
116  ---------------------------------------------------------------*/
118 {
119  if (!sensedData.empty())
120  out_sensorPose = CPose3D(sensedData[0].sensorLocationOnRobot);
121  else
122  out_sensorPose = CPose3D(0, 0, 0);
123 }
124 
125 /*---------------------------------------------------------------
126  setSensorPose
127  ---------------------------------------------------------------*/
129 {
130  size_t i, n = sensedData.size();
131  if (n)
132  for (i = 0; i < n; i++)
133  sensedData[i].sensorLocationOnRobot = CPoint3D(newSensorPose);
134 }
135 
136 /*---------------------------------------------------------------
137  getSensedRangeByBeaconID
138  ---------------------------------------------------------------*/
140 {
141  for (size_t i = 0; i < sensedData.size(); i++)
142  if (sensedData[i].beaconID == beaconID)
143  return sensedData[i].sensedDistance;
144  return 0;
145 }
146 
148 {
149  using namespace std;
151 
152  o << "Auxiliary estimated pose (if available): " << auxEstimatePose << endl;
153 
154  o << format("minSensorDistance=%f m\n", minSensorDistance);
155  o << format("maxSensorDistance=%f m\n", maxSensorDistance);
156  o << format("stdError=%f m\n\n", stdError);
157 
158  o << format(
159  "There are %u range measurements:\n\n", (unsigned)sensedData.size());
160 
161  o << " BEACON RANGE SENSOR POSITION ON ROBOT \n";
162  o << "------------------------------------------------\n";
164  sensedData.begin();
165  it != sensedData.end(); it++)
166  {
167  o << format(
168  " %i %.04f (%.03f,%.03f,%.03f)\n", (int)it->beaconID,
169  it->sensedDistance, it->sensorLocationOnRobot.x(),
170  it->sensorLocationOnRobot.y(), it->sensorLocationOnRobot.z());
171  }
172 }
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const override
A general method to retrieve the sensor pose on the robot.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) override
A general method to change the sensor pose on the robot.
GLenum GLsizei n
Definition: glext.h:5074
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
Declares a class derived from "CObservation" that represents one (or more) range measurements to labe...
STL namespace.
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 ...
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
This namespace contains representation of robot actions and observations.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
A class used to store a 3D point.
Definition: CPoint3D.h:33
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:15
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
__int32 int32_t
Definition: rptypes.h:46
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:88
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
GLuint id
Definition: glext.h:3909
GLuint in
Definition: glext.h:7274
void debugPrintOut()
Prints out the contents of the object.
unsigned __int32 uint32_t
Definition: rptypes.h:47
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
const Scalar * const_iterator
Definition: eigen_plugins.h:27
virtual void getDescriptionAsText(std::ostream &o) const
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019