Main MRPT website > C++ reference for MRPT 1.9.9
CObservationBearingRange.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 #include <mrpt/math/matrix_serialization.h> // for << ops
16 #include <mrpt/math/wrap2pi.h>
17 #include <set>
18 
19 using namespace mrpt::obs;
20 using namespace mrpt::poses;
21 using namespace mrpt::math;
22 
23 // This must be added to any CSerializable class implementation file.
25 
26 /*---------------------------------------------------------------
27  Default constructor.
28  ---------------------------------------------------------------*/
30  : minSensorDistance(0),
31  maxSensorDistance(0),
32  fieldOfView_yaw(DEG2RAD(180)),
33  fieldOfView_pitch(DEG2RAD(90)),
34  sensorLocationOnRobot(),
35  sensedData(),
36  validCovariances(false),
37  sensor_std_range(0),
38  sensor_std_yaw(0),
39  sensor_std_pitch(0)
40 {
41 }
42 
46 {
47  uint32_t i, n;
48  // The data
49  out << minSensorDistance << maxSensorDistance << fieldOfView_yaw
50  << fieldOfView_pitch << sensorLocationOnRobot << timestamp;
51  out << validCovariances;
52  if (!validCovariances)
53  out << sensor_std_range << sensor_std_yaw << sensor_std_pitch;
54 
55  // Detect duplicate landmarks ID, which is an error!
56  std::set<int32_t> lstIDs;
57 
58  n = sensedData.size();
59  out << n;
60  for (i = 0; i < n; i++)
61  {
62  int32_t id = sensedData[i].landmarkID;
63  if (id != INVALID_LANDMARK_ID)
64  {
65  if (0 != lstIDs.count(id))
66  THROW_EXCEPTION_FMT("Duplicate landmark ID=%i found.", (int)id);
67  lstIDs.insert(id);
68  }
69 
70  out << sensedData[i].range << sensedData[i].yaw << sensedData[i].pitch
71  << id;
72 
73  if (validCovariances) out << sensedData[i].covariance;
74  }
75 
76  out << sensorLabel;
77 }
78 
81 {
82  switch (version)
83  {
84  case 0:
85  case 1:
86  case 2:
87  case 3:
88  {
89  uint32_t i, n;
90 
91  // The data
92  in >> minSensorDistance >> maxSensorDistance;
93 
94  if (version >= 3)
95  {
96  in >> fieldOfView_yaw >> fieldOfView_pitch;
97  }
98  else
99  {
100  float fieldOfView;
101  in >> fieldOfView;
102 
103  fieldOfView_yaw = fieldOfView_pitch = fieldOfView;
104  }
105 
106  in >> sensorLocationOnRobot;
107 
108  if (version >= 2)
109  in >> timestamp;
110  else
111  timestamp = INVALID_TIMESTAMP;
112 
113  if (version >= 3)
114  {
115  in >> validCovariances;
116  if (!validCovariances)
117  in >> sensor_std_range >> sensor_std_yaw >>
118  sensor_std_pitch;
119  }
120  else
121  validCovariances = false;
122 
123  in >> n;
124  sensedData.resize(n);
125 
126  // Detect duplicate landmarks ID, what is an error!
127  std::set<int32_t> lstIDs;
128 
129  for (i = 0; i < n; i++)
130  {
131  in >> sensedData[i].range >> sensedData[i].yaw >>
132  sensedData[i].pitch >> sensedData[i].landmarkID;
133 
134  if (version >= 3 && validCovariances)
135  in >> sensedData[i].covariance;
136 
137  int32_t id = sensedData[i].landmarkID;
138  if (id != INVALID_LANDMARK_ID)
139  {
140  if (0 != lstIDs.count(id))
142  "Duplicate landmark ID=%i found.", (int)id);
143  lstIDs.insert(id);
144  }
145  }
146 
147  if (version >= 1)
148  in >> sensorLabel;
149  else
150  sensorLabel = "";
151  }
152  break;
153  default:
155  };
156 }
157 
159 {
160  printf("[CObservationBearingRange::debugPrintOut] Dumping:\n");
161  printf(
162  "[CObservationBearingRange::debugPrintOut] minSensorDistance:\t%f\n",
163  minSensorDistance);
164  printf(
165  "[CObservationBearingRange::debugPrintOut] maxSensorDistance:\t%f:\n",
166  maxSensorDistance);
167  printf(
168  "[CObservationBearingRange::debugPrintOut] %u landmarks:\n",
169  static_cast<unsigned>(sensedData.size()));
170 
171  size_t i, n = sensedData.size();
172  for (i = 0; i < n; i++)
173  printf(
174  "[CObservationBearingRange::debugPrintOut] \tID[%i]: y:%fdeg "
175  "p:%fdeg range: %f\n",
176  sensedData[i].landmarkID, RAD2DEG(sensedData[i].yaw),
177  RAD2DEG(sensedData[i].pitch), sensedData[i].range);
178 }
179 
181 {
182  using namespace std;
184 
185  o << "Homogeneous matrix for the sensor's 3D pose, relative to robot "
186  "base:\n";
187  o << sensorLocationOnRobot.getHomogeneousMatrixVal<CMatrixDouble44>()
188  << sensorLocationOnRobot << endl
189  << endl;
190 
191  o << "Do observations have individual covariance matrices? "
192  << (validCovariances ? "YES" : "NO") << endl
193  << endl;
194 
195  o << "Default noise sigmas:" << endl;
196  o << "sensor_std_range (m) : " << sensor_std_range << endl;
197  o << "sensor_std_yaw (deg) : " << RAD2DEG(sensor_std_yaw) << endl;
198  o << "sensor_std_pitch (deg) : " << RAD2DEG(sensor_std_pitch) << endl;
199 
200  o << endl;
201 
202  // For each entry in this sequence:
203  o << " LANDMARK_ID RANGE (m) YAW (deg) PITCH (deg) COV. MATRIX "
204  "(optional)"
205  << endl;
206  o << "---------------------------------------------------------------------"
207  "-----------------"
208  << endl;
209  for (size_t q = 0; q < sensedData.size(); q++)
210  {
211  o << " ";
212  if (sensedData[q].landmarkID == INVALID_LANDMARK_ID)
213  o << "(NO ID)";
214  else
215  o << format("%7u", sensedData[q].landmarkID);
216 
217  o << format(
218  " %10.03f %10.03f %10.03f ", sensedData[q].range,
219  RAD2DEG(mrpt::math::wrapToPi(sensedData[q].yaw)),
220  RAD2DEG(mrpt::math::wrapToPi(sensedData[q].pitch)));
221 
222  if (validCovariances)
223  o << sensedData[q].covariance.inMatlabFormat() << endl;
224  else
225  o << " (N/A)\n";
226  }
227 }
GLsizei range
Definition: glext.h:5907
double RAD2DEG(const double x)
Radians to degrees.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
double DEG2RAD(const double x)
Degrees to radians.
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3721
GLenum GLsizei n
Definition: glext.h:5074
STL namespace.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
A numeric matrix of compile-time fixed size.
This base provides a set of functions for maths stuff.
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
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:53
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
__int32 int32_t
Definition: rptypes.h:46
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
This file implements matrix/vector text and binary serialization.
GLuint id
Definition: glext.h:3909
This observation represents a number of range-bearing value pairs, each one for a detected landmark...
GLuint in
Definition: glext.h:7274
#define INVALID_LANDMARK_ID
Used for CObservationBearingRange::TMeasurement::beaconID and others.
Definition: CObservation.h:27
void debugPrintOut()
Prints out the contents of the object.
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:43
unsigned __int32 uint32_t
Definition: rptypes.h:47
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