MRPT  1.9.9
CObservationBearingRange.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, 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 
10 #include "obs-precomp.h" // Precompiled headers
11 
12 #include <mrpt/math/matrix_serialization.h> // for << ops
13 #include <mrpt/math/wrap2pi.h>
16 #include <mrpt/system/os.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  : fieldOfView_yaw(DEG2RAD(180)),
31  fieldOfView_pitch(DEG2RAD(90)),
32  sensorLocationOnRobot(),
33  sensedData()
34 
35 {
36 }
37 
41 {
42  uint32_t i, n;
43  // The data
44  out << minSensorDistance << maxSensorDistance << fieldOfView_yaw
45  << fieldOfView_pitch << sensorLocationOnRobot << timestamp;
46  out << validCovariances;
47  if (!validCovariances)
48  out << sensor_std_range << sensor_std_yaw << sensor_std_pitch;
49 
50  // Detect duplicate landmarks ID, which is an error!
51  std::set<int32_t> lstIDs;
52 
53  n = sensedData.size();
54  out << n;
55  for (i = 0; i < n; i++)
56  {
57  int32_t id = sensedData[i].landmarkID;
58  if (id != INVALID_LANDMARK_ID)
59  {
60  if (0 != lstIDs.count(id))
61  THROW_EXCEPTION_FMT("Duplicate landmark ID=%i found.", (int)id);
62  lstIDs.insert(id);
63  }
64 
65  out << sensedData[i].range << sensedData[i].yaw << sensedData[i].pitch
66  << id;
67 
68  if (validCovariances) out << sensedData[i].covariance;
69  }
70 
71  out << sensorLabel;
72 }
73 
76 {
77  switch (version)
78  {
79  case 0:
80  case 1:
81  case 2:
82  case 3:
83  {
84  uint32_t i, n;
85 
86  // The data
87  in >> minSensorDistance >> maxSensorDistance;
88 
89  if (version >= 3)
90  {
91  in >> fieldOfView_yaw >> fieldOfView_pitch;
92  }
93  else
94  {
95  float fieldOfView;
96  in >> fieldOfView;
97 
98  fieldOfView_yaw = fieldOfView_pitch = fieldOfView;
99  }
100 
101  in >> sensorLocationOnRobot;
102 
103  if (version >= 2)
104  in >> timestamp;
105  else
106  timestamp = INVALID_TIMESTAMP;
107 
108  if (version >= 3)
109  {
110  in >> validCovariances;
111  if (!validCovariances)
112  in >> sensor_std_range >> sensor_std_yaw >>
113  sensor_std_pitch;
114  }
115  else
116  validCovariances = false;
117 
118  in >> n;
119  sensedData.resize(n);
120 
121  // Detect duplicate landmarks ID, what is an error!
122  std::set<int32_t> lstIDs;
123 
124  for (i = 0; i < n; i++)
125  {
126  in >> sensedData[i].range >> sensedData[i].yaw >>
127  sensedData[i].pitch >> sensedData[i].landmarkID;
128 
129  if (version >= 3 && validCovariances)
130  in >> sensedData[i].covariance;
131 
132  int32_t id = sensedData[i].landmarkID;
133  if (id != INVALID_LANDMARK_ID)
134  {
135  if (0 != lstIDs.count(id))
137  "Duplicate landmark ID=%i found.", (int)id);
138  lstIDs.insert(id);
139  }
140  }
141 
142  if (version >= 1)
143  in >> sensorLabel;
144  else
145  sensorLabel = "";
146  }
147  break;
148  default:
150  };
151 }
152 
154 {
155  printf("[CObservationBearingRange::debugPrintOut] Dumping:\n");
156  printf(
157  "[CObservationBearingRange::debugPrintOut] minSensorDistance:\t%f\n",
158  minSensorDistance);
159  printf(
160  "[CObservationBearingRange::debugPrintOut] maxSensorDistance:\t%f:\n",
161  maxSensorDistance);
162  printf(
163  "[CObservationBearingRange::debugPrintOut] %u landmarks:\n",
164  static_cast<unsigned>(sensedData.size()));
165 
166  size_t i, n = sensedData.size();
167  for (i = 0; i < n; i++)
168  printf(
169  "[CObservationBearingRange::debugPrintOut] \tID[%i]: y:%fdeg "
170  "p:%fdeg range: %f\n",
171  sensedData[i].landmarkID, RAD2DEG(sensedData[i].yaw),
172  RAD2DEG(sensedData[i].pitch), sensedData[i].range);
173 }
174 
176 {
177  using namespace std;
179 
180  o << "Homogeneous matrix for the sensor's 3D pose, relative to robot "
181  "base:\n";
182  o << sensorLocationOnRobot.getHomogeneousMatrixVal<CMatrixDouble44>()
183  << sensorLocationOnRobot << endl
184  << endl;
185 
186  o << "Do observations have individual covariance matrices? "
187  << (validCovariances ? "YES" : "NO") << endl
188  << endl;
189 
190  o << "Default noise sigmas:" << endl;
191  o << "sensor_std_range (m) : " << sensor_std_range << endl;
192  o << "sensor_std_yaw (deg) : " << RAD2DEG(sensor_std_yaw) << endl;
193  o << "sensor_std_pitch (deg) : " << RAD2DEG(sensor_std_pitch) << endl;
194 
195  o << endl;
196 
197  // For each entry in this sequence:
198  o << " LANDMARK_ID RANGE (m) YAW (deg) PITCH (deg) COV. MATRIX "
199  "(optional)"
200  << endl;
201  o << "---------------------------------------------------------------------"
202  "-----------------"
203  << endl;
204  for (const auto& q : sensedData)
205  {
206  o << " ";
207  if (q.landmarkID == INVALID_LANDMARK_ID)
208  o << "(NO ID)";
209  else
210  o << format("%7u", q.landmarkID);
211 
212  o << format(
213  " %10.03f %10.03f %10.03f ", q.range,
215  RAD2DEG(mrpt::math::wrapToPi(q.pitch)));
216 
217  if (validCovariances)
218  o << q.covariance.inMatlabFormat() << endl;
219  else
220  o << " (N/A)\n";
221  }
222 }
GLsizei range
Definition: glext.h:5993
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
double RAD2DEG(const double x)
Radians to degrees.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
double DEG2RAD(const double x)
Degrees to radians.
GLdouble GLdouble GLdouble GLdouble q
Definition: glext.h:3727
GLenum GLsizei n
Definition: glext.h:5136
STL namespace.
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
unsigned char uint8_t
Definition: rptypes.h:44
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
This base provides a set of functions for maths stuff.
This namespace contains representation of robot actions and observations.
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:50
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
__int32 int32_t
Definition: rptypes.h:49
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:53
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.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
This file implements matrix/vector text and binary serialization.
GLuint id
Definition: glext.h:3920
This observation represents a number of range-bearing value pairs, each one for a detected landmark...
GLuint in
Definition: glext.h:7391
#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:69
unsigned __int32 uint32_t
Definition: rptypes.h:50
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:43
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: 582f6a819 Mon Jul 22 16:41:38 2019 -0700 at mar jul 23 01:50:13 CEST 2019