Main MRPT website > C++ reference for MRPT 1.5.6
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-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 #include <mrpt/utils/CStream.h>
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::utils;
21 using namespace mrpt::poses;
22 
23 
24 // This must be added to any CSerializable class implementation file.
26 
27 /*---------------------------------------------------------------
28  Default constructor.
29  ---------------------------------------------------------------*/
31  minSensorDistance(0),
32  maxSensorDistance(0),
33  fieldOfView_yaw(DEG2RAD(180)),
34  fieldOfView_pitch(DEG2RAD(90)),
35  sensorLocationOnRobot(),
36  sensedData(),
37  validCovariances(false),
38  sensor_std_range(0),
39  sensor_std_yaw(0),
40  sensor_std_pitch(0)
41 {
42 }
43 
44 /*---------------------------------------------------------------
45  Implements the writing to a CStream capability of CSerializable objects
46  ---------------------------------------------------------------*/
48 {
49  if (version)
50  *version = 3;
51  else
52  {
53  uint32_t i,n;
54 
55  // The data
56  out << minSensorDistance
57  << maxSensorDistance
58  << fieldOfView_yaw
59  << fieldOfView_pitch
60  << sensorLocationOnRobot
61  << timestamp;
62 
63  out << validCovariances;
64  if (!validCovariances)
65  out << sensor_std_range << sensor_std_yaw << sensor_std_pitch;
66 
67  // Detect duplicate landmarks ID, which is an error!
68  std::set<int32_t> lstIDs;
69 
70  n = sensedData.size();
71  out << n;
72  for (i=0;i<n;i++)
73  {
74  int32_t id = sensedData[i].landmarkID;
75  if (id!=INVALID_LANDMARK_ID)
76  {
77  if (0!=lstIDs.count(id))
78  THROW_EXCEPTION_FMT("Duplicate landmark ID=%i found.",(int)id);
79  lstIDs.insert(id);
80  }
81 
82  out << sensedData[i].range
83  << sensedData[i].yaw
84  << sensedData[i].pitch
85  << id;
86 
87  if (validCovariances)
88  out << sensedData[i].covariance;
89  }
90 
91  out << sensorLabel;
92  }
93 }
94 
95 /*---------------------------------------------------------------
96  Implements the reading from a CStream capability of CSerializable objects
97  ---------------------------------------------------------------*/
99 {
100  switch(version)
101  {
102  case 0:
103  case 1:
104  case 2:
105  case 3:
106  {
107  uint32_t i,n;
108 
109  // The data
110  in >> minSensorDistance
111  >> maxSensorDistance;
112 
113  if (version>=3)
114  {
115  in >> fieldOfView_yaw
116  >> fieldOfView_pitch;
117  }
118  else
119  {
120  float fieldOfView;
121  in >> fieldOfView;
122 
123  fieldOfView_yaw =
124  fieldOfView_pitch = fieldOfView;
125  }
126 
127  in >> sensorLocationOnRobot;
128 
129  if (version>=2)
130  in >> timestamp;
131  else timestamp = INVALID_TIMESTAMP;
132 
133  if (version>=3)
134  {
135  in >> validCovariances;
136  if (!validCovariances)
137  in >> sensor_std_range >> sensor_std_yaw >> sensor_std_pitch;
138  } else
139  validCovariances = false;
140 
141  in >> n;
142  sensedData.resize(n);
143 
144  // Detect duplicate landmarks ID, what is an error!
145  std::set<int32_t> lstIDs;
146 
147  for (i=0;i<n;i++)
148  {
149  in >> sensedData[i].range
150  >> sensedData[i].yaw
151  >> sensedData[i].pitch
152  >> sensedData[i].landmarkID;
153 
154  if (version>=3 && validCovariances)
155  in >> sensedData[i].covariance;
156 
157  int32_t id = sensedData[i].landmarkID;
158  if (id!=INVALID_LANDMARK_ID)
159  {
160  if (0!=lstIDs.count(id))
161  THROW_EXCEPTION_FMT("Duplicate landmark ID=%i found.",(int)id);
162  lstIDs.insert(id);
163  }
164  }
165 
166  if (version>=1)
167  in >> sensorLabel;
168  else sensorLabel = "";
169 
170  } break;
171  default:
173 
174  };
175 
176 }
177 
178 /*---------------------------------------------------------------
179  Implements the writing to a CStream capability of CSerializable objects
180  ---------------------------------------------------------------*/
182 {
183  printf("[CObservationBearingRange::debugPrintOut] Dumping:\n");
184  printf("[CObservationBearingRange::debugPrintOut] minSensorDistance:\t%f\n",minSensorDistance);
185  printf("[CObservationBearingRange::debugPrintOut] maxSensorDistance:\t%f:\n",maxSensorDistance);
186  printf("[CObservationBearingRange::debugPrintOut] %u landmarks:\n",static_cast<unsigned>(sensedData.size()) );
187 
188  size_t i, n = sensedData.size();
189  for (i=0;i<n;i++)
190  printf("[CObservationBearingRange::debugPrintOut] \tID[%i]: y:%fdeg p:%fdeg range: %f\n",
191  sensedData[i].landmarkID,
192  RAD2DEG( sensedData[i].yaw ),
193  RAD2DEG( sensedData[i].pitch ),
194  sensedData[i].range );
195 }
196 
198 {
199  using namespace std;
201 
202  o << "Homogeneous matrix for the sensor's 3D pose, relative to robot base:\n";
203  o << sensorLocationOnRobot.getHomogeneousMatrixVal()
204  << sensorLocationOnRobot << endl << endl;
205 
206  o << "Do observations have individual covariance matrices? " << (validCovariances ? "YES":"NO") << endl << endl;
207 
208  o << "Default noise sigmas:" << endl;
209  o << "sensor_std_range (m) : " << sensor_std_range << endl;
210  o << "sensor_std_yaw (deg) : " << RAD2DEG(sensor_std_yaw) << endl;
211  o << "sensor_std_pitch (deg) : " << RAD2DEG(sensor_std_pitch) << endl;
212 
213  o << endl;
214 
215  // For each entry in this sequence:
216  o << " LANDMARK_ID RANGE (m) YAW (deg) PITCH (deg) COV. MATRIX (optional)" << endl;
217  o << "--------------------------------------------------------------------------------------" << endl;
218  for (size_t q=0;q<sensedData.size();q++)
219  {
220 
221  o << " ";
222  if (sensedData[q].landmarkID==INVALID_LANDMARK_ID)
223  o << "(NO ID)";
224  else o << format("%7u",sensedData[q].landmarkID);
225 
226  o << format(" %10.03f %10.03f %10.03f ",
227  sensedData[q].range,
228  RAD2DEG( mrpt::math::wrapToPi( sensedData[q].yaw)),
229  RAD2DEG( mrpt::math::wrapToPi(sensedData[q].pitch)) );
230 
231  if (validCovariances)
232  o << sensedData[q].covariance.inMatlabFormat() << endl;
233  else
234  o << " (N/A)\n";
235  }
236 
237 }
GLenum GLenum range
Definition: glew.h:7127
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
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.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
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...
int version
Definition: mrpt_jpeglib.h:898
#define DEG2RAD
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
Definition: wrap2pi.h:51
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:17
__int32 int32_t
Definition: rptypes.h:48
#define RAD2DEG
Declares a class that represents any robot's observation.
This file implements matrix/vector text and binary serialization.
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.
void debugPrintOut()
Prints out the contents of the object.
GLdouble GLdouble GLdouble GLdouble q
Definition: glew.h:1319
unsigned __int32 uint32_t
Definition: rptypes.h:49
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...



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