MRPT  2.0.4
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-2020, 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 uint8_t CObservationBearingRange::serializeGetVersion() const { return 3; }
29 {
30  uint32_t i, n;
31  // The data
32  out << minSensorDistance << maxSensorDistance << fieldOfView_yaw
33  << fieldOfView_pitch << sensorLocationOnRobot << timestamp;
34  out << validCovariances;
35  if (!validCovariances)
36  out << sensor_std_range << sensor_std_yaw << sensor_std_pitch;
37 
38  // Detect duplicate landmarks ID, which is an error!
39  std::set<int32_t> lstIDs;
40 
41  n = sensedData.size();
42  out << n;
43  for (i = 0; i < n; i++)
44  {
45  int32_t id = sensedData[i].landmarkID;
46  if (id != INVALID_LANDMARK_ID)
47  {
48  if (0 != lstIDs.count(id))
49  THROW_EXCEPTION_FMT("Duplicate landmark ID=%i found.", (int)id);
50  lstIDs.insert(id);
51  }
52 
53  out << sensedData[i].range << sensedData[i].yaw << sensedData[i].pitch
54  << id;
55 
56  if (validCovariances) out << sensedData[i].covariance;
57  }
58 
59  out << sensorLabel;
60 }
61 
63  mrpt::serialization::CArchive& in, uint8_t version)
64 {
65  switch (version)
66  {
67  case 0:
68  case 1:
69  case 2:
70  case 3:
71  {
72  uint32_t i, n;
73 
74  // The data
75  in >> minSensorDistance >> maxSensorDistance;
76 
77  if (version >= 3)
78  {
79  in >> fieldOfView_yaw >> fieldOfView_pitch;
80  }
81  else
82  {
83  float fieldOfView;
84  in >> fieldOfView;
85 
86  fieldOfView_yaw = fieldOfView_pitch = fieldOfView;
87  }
88 
89  in >> sensorLocationOnRobot;
90 
91  if (version >= 2)
92  in >> timestamp;
93  else
94  timestamp = INVALID_TIMESTAMP;
95 
96  if (version >= 3)
97  {
98  in >> validCovariances;
99  if (!validCovariances)
100  in >> sensor_std_range >> sensor_std_yaw >>
101  sensor_std_pitch;
102  }
103  else
104  validCovariances = false;
105 
106  in >> n;
107  sensedData.resize(n);
108 
109  // Detect duplicate landmarks ID, what is an error!
110  std::set<int32_t> lstIDs;
111 
112  for (i = 0; i < n; i++)
113  {
114  in >> sensedData[i].range >> sensedData[i].yaw >>
115  sensedData[i].pitch >> sensedData[i].landmarkID;
116 
117  if (version >= 3 && validCovariances)
118  in >> sensedData[i].covariance;
119 
120  int32_t id = sensedData[i].landmarkID;
121  if (id != INVALID_LANDMARK_ID)
122  {
123  if (0 != lstIDs.count(id))
125  "Duplicate landmark ID=%i found.", (int)id);
126  lstIDs.insert(id);
127  }
128  }
129 
130  if (version >= 1)
131  in >> sensorLabel;
132  else
133  sensorLabel = "";
134  }
135  break;
136  default:
138  };
139 }
140 
142 {
143  printf("[CObservationBearingRange::debugPrintOut] Dumping:\n");
144  printf(
145  "[CObservationBearingRange::debugPrintOut] minSensorDistance:\t%f\n",
146  minSensorDistance);
147  printf(
148  "[CObservationBearingRange::debugPrintOut] maxSensorDistance:\t%f:\n",
149  maxSensorDistance);
150  printf(
151  "[CObservationBearingRange::debugPrintOut] %u landmarks:\n",
152  static_cast<unsigned>(sensedData.size()));
153 
154  size_t i, n = sensedData.size();
155  for (i = 0; i < n; i++)
156  printf(
157  "[CObservationBearingRange::debugPrintOut] \tID[%i]: y:%fdeg "
158  "p:%fdeg range: %f\n",
159  sensedData[i].landmarkID, RAD2DEG(sensedData[i].yaw),
160  RAD2DEG(sensedData[i].pitch), sensedData[i].range);
161 }
162 
164 {
165  using namespace std;
167 
168  o << "Homogeneous matrix for the sensor's 3D pose, relative to robot "
169  "base:\n";
170  o << sensorLocationOnRobot.getHomogeneousMatrixVal<CMatrixDouble44>()
171  << sensorLocationOnRobot << endl
172  << endl;
173 
174  o << "Do observations have individual covariance matrices? "
175  << (validCovariances ? "YES" : "NO") << endl
176  << endl;
177 
178  o << "Default noise sigmas:" << endl;
179  o << "sensor_std_range (m) : " << sensor_std_range << endl;
180  o << "sensor_std_yaw (deg) : " << RAD2DEG(sensor_std_yaw) << endl;
181  o << "sensor_std_pitch (deg) : " << RAD2DEG(sensor_std_pitch) << endl;
182 
183  o << endl;
184 
185  // For each entry in this sequence:
186  o << " LANDMARK_ID RANGE (m) YAW (deg) PITCH (deg) COV. MATRIX "
187  "(optional)"
188  << endl;
189  o << "---------------------------------------------------------------------"
190  "-----------------"
191  << endl;
192  for (const auto& q : sensedData)
193  {
194  o << " ";
195  if (q.landmarkID == INVALID_LANDMARK_ID)
196  o << "(NO ID)";
197  else
198  o << format("%7u", q.landmarkID);
199 
200  o << format(
201  " %10.03f %10.03f %10.03f ", q.range,
203  RAD2DEG(mrpt::math::wrapToPi(q.pitch)));
204 
205  if (validCovariances)
206  o << q.covariance.inMatlabFormat() << endl;
207  else
208  o << " (N/A)\n";
209  }
210 }
A compile-time fixed-size numeric matrix container.
Definition: CMatrixFixed.h:33
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
STL namespace.
#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...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
mrpt::vision::TStereoCalibResults out
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.
constexpr double RAD2DEG(const double x)
Radians to degrees.
This observation represents a number of range-bearing value pairs, each one for a detected landmark...
#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
#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 2.0.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020