Main MRPT website > C++ reference for MRPT 1.9.9
CObservationGasSensors.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 <iostream>
16 
17 using namespace mrpt::obs;
18 using namespace mrpt::poses;
19 using namespace mrpt::math;
20 using namespace std;
21 
22 // This must be added to any CSerializable class implementation file.
24 
25 /** Constructor
26  */
31 {
32  uint32_t i, n = m_readings.size();
33  out << n;
34 
35  for (i = 0; i < n; i++)
36  {
37  out << CPose3D(m_readings[i].eNosePoseOnTheRobot);
38  out << m_readings[i].readingsVoltage;
39  out << m_readings[i].sensorTypes;
40  out << m_readings[i].hasTemperature;
41  if (m_readings[i].hasTemperature) out << m_readings[i].temperature;
42  }
43 
44  out << sensorLabel << timestamp;
45 }
46 
49 {
50  switch (version)
51  {
52  case 2:
53  case 3:
54  case 4:
55  case 5:
56  {
57  // A general set of e-nose descriptors:
58  uint32_t i, n;
59 
60  in >> n;
61  m_readings.resize(n);
62 
63  CPose3D aux;
64 
65  for (i = 0; i < n; i++)
66  {
67  in >> aux;
68  m_readings[i].eNosePoseOnTheRobot = aux.asTPose();
69  in >> m_readings[i].readingsVoltage;
70  in >> m_readings[i].sensorTypes;
71  if (version >= 3)
72  {
73  in >> m_readings[i].hasTemperature;
74  if (m_readings[i].hasTemperature)
75  in >> m_readings[i].temperature;
76  }
77  else
78  {
79  m_readings[i].hasTemperature = false;
80  m_readings[i].temperature = 0;
81  }
82  }
83 
84  if (version >= 4)
85  in >> sensorLabel;
86  else
87  sensorLabel = "";
88 
89  if (version >= 5)
90  in >> timestamp;
91  else
92  timestamp = INVALID_TIMESTAMP;
93  }
94  break;
95  case 0:
96  case 1:
97  {
98  TObservationENose eNose;
99 
100  m_readings.clear();
101 
102  // There was a single set of 16 values from "Sancho" (DEC-2006)
103  CVectorFloat readings;
104  in >> readings;
105 
106  ASSERT_(readings.size() == 16);
107 
108  // There was TWO e-noses:
109  // (1)
110  eNose.eNosePoseOnTheRobot =
111  TPose3D(0.20, -0.15, 0.10, 0, 0, 0); // (x,y,z) only
112  eNose.readingsVoltage.resize(4);
113  eNose.readingsVoltage[0] = readings[2];
114  eNose.readingsVoltage[1] = readings[4];
115  eNose.readingsVoltage[2] = readings[5];
116  eNose.readingsVoltage[3] = readings[6];
117 
118  eNose.sensorTypes.clear();
119  eNose.sensorTypes.resize(4, 0);
120  m_readings.push_back(eNose);
121 
122  // (2)
123  eNose.eNosePoseOnTheRobot =
124  TPose3D(0.20, 0.15, 0.10, .0, .0, .0); // (x,y,z) only
125  eNose.readingsVoltage.resize(4);
126  eNose.readingsVoltage[0] = readings[8];
127  eNose.readingsVoltage[1] = readings[10];
128  eNose.readingsVoltage[2] = readings[12];
129  eNose.readingsVoltage[3] = readings[14];
130 
131  eNose.sensorTypes.clear();
132  eNose.sensorTypes.resize(4, 0);
133  m_readings.push_back(eNose);
134  }
135  break;
136  default:
138  };
139 }
140 
141 /*---------------------------------------------------------------
142  getSensorPose
143  ---------------------------------------------------------------*/
145 {
146  if (!m_readings.empty())
147  out_sensorPose = CPose3D(m_readings[0].eNosePoseOnTheRobot);
148  else
149  out_sensorPose = CPose3D(0, 0, 0);
150 }
151 
153 {
154  for (auto& r : m_readings) r.eNosePoseOnTheRobot = newSensorPose.asTPose();
155 }
156 
158  float& reading, mrpt::system::TTimeStamp& timestamp)
159 {
160  try
161  {
162  // Noise filtering
163  noise_filtering(reading, timestamp);
164 
165  // Decimate
166  if (decimate_count != decimate_value)
167  {
168  decimate_count++;
169  return false;
170  }
171 
172  // Gas concentration estimation based on FIRST ORDER + NONLINEAR
173  // COMPENSATIONS DYNAMICS
174  inverse_MOSmodeling(
175  m_antiNoise_window[winNoise_size / 2].reading_filtered,
176  m_antiNoise_window[winNoise_size / 2].timestamp);
177  decimate_count = 1;
178 
179  // update (output)
180  reading = last_Obs.estimation;
181  timestamp = last_Obs.timestamp;
182 
183  // Save data map in log file for Matlab visualization
184  if (save_maplog)
185  save_log_map(
186  last_Obs.timestamp, last_Obs.reading, last_Obs.estimation,
187  last_Obs.tau);
188 
189  return true;
190  }
191  catch (...)
192  {
193  cout << "Error when decimating \n";
195  return false;
196  }
197 }
198 
199 /*---------------------------------------------------------------
200  noise_filtering (smooth)
201  ---------------------------------------------------------------*/
203  const float& reading, const mrpt::system::TTimeStamp& timestamp)
204 {
205  try
206  {
207  // Store values in the temporal Observation
208  temporal_Obs.reading = reading;
209  temporal_Obs.timestamp = timestamp;
210 
211  // If first reading from E-nose
212  if (m_antiNoise_window.empty())
213  {
214  // use default values
215  temporal_Obs.reading_filtered = reading;
216 
217  // Populate the noise window
218  m_antiNoise_window.assign(winNoise_size, temporal_Obs);
219  }
220  else
221  {
222  // Erase the first element (the oldest), and add the new one
223  m_antiNoise_window.erase(m_antiNoise_window.begin());
224  m_antiNoise_window.push_back(temporal_Obs);
225  }
226 
227  // Average data to reduce noise (Noise Filtering)
228  float partial_sum = 0;
229  for (size_t i = 0; i < m_antiNoise_window.size(); i++)
230  partial_sum += m_antiNoise_window.at(i).reading;
231 
232  m_antiNoise_window.at(winNoise_size / 2).reading_filtered =
233  partial_sum / winNoise_size;
234  }
235  catch (...)
236  {
237  cout << "Error when filtering noise from readings \n";
239  }
240 }
241 
242 /*---------------------------------------------------------------
243  inverse_MOSmodeling
244  ---------------------------------------------------------------*/
246  const float& reading, const mrpt::system::TTimeStamp& timestamp)
247 {
248  try
249  {
250  // Keep the minimum reading value as an approximation to the basline
251  // level
252  if (reading < min_reading) min_reading = reading;
253 
254  // Check if estimation posible (not possible in the first iteration)
255  if (!first_iteration)
256  {
257  // Assure the samples are provided at constant rate (important for
258  // the correct gas distribution estimation)
259  double incT =
260  mrpt::system::timeDifference(last_Obs.timestamp, timestamp);
261 
262  if ((incT > 0) & (!first_incT))
263  { // not the same sample due to initialization of buffers
264  if (fixed_incT == 0)
265  fixed_incT = incT;
266  else
267  // ASSERT_(fabs(incT - fixed_incT) < (double)(0.05));
268  if (fabs(incT - fixed_incT) > (double)(0.05))
269  cout << "IncT is not constant by HW." << endl;
270  }
271  else
272  {
273  if (incT > 0) first_incT = false;
274  }
275 
276  // slope<0 -->Decay
277  if (reading < last_Obs.reading)
278  {
279  last_Obs.tau = a_decay * abs(reading - min_reading) + b_decay;
280  }
281  else // slope>=0 -->rise
282  {
283  last_Obs.tau = a_rise * abs(reading - min_reading) + b_rise;
284  } // end-if
285 
286  // New estimation values -- Ziegler-Nichols model --
287  if (incT > 0)
288  // Initially there may come repetetive values till
289  // m_antiNoise_window is full populated.
290  last_Obs.estimation =
291  ((reading - last_Obs.reading) * last_Obs.tau / incT) +
292  reading;
293  else
294  last_Obs.estimation = reading;
295 
296  // Prepare the New observation
297  last_Obs.timestamp = timestamp;
298  last_Obs.reading = reading;
299  }
300  else
301  {
302  // First filtered reading (use default values)
303  last_Obs.tau = b_rise;
304  last_Obs.reading = reading;
305  last_Obs.timestamp = timestamp;
306  last_Obs.estimation =
307  reading; // No estimation possible at this step
308  first_iteration = false;
309  } // end-if estimation values
310  }
311  catch (exception e)
312  {
313  cerr << "**Exception in "
314  "CObservationGasSensors::CMOSmodel::inverse_MOSmodeling** "
315  << e.what() << endl;
316  }
317 }
318 
319 /*---------------------------------------------------------------
320  save_log_map
321  ---------------------------------------------------------------*/
323  const mrpt::system::TTimeStamp& timestamp, const float& reading,
324  const float& estimation, const float& tau)
325 {
326  // function to save in a log file the information of the generated gas
327  // distribution estimation
328 
329  double time = mrpt::system::timestampTotime_t(timestamp);
330  char buffer[50];
331  sprintf(buffer, "./log_MOSmodel_GasDistribution.txt");
332 
333  if (!m_debug_dump) m_debug_dump = new ofstream(buffer);
334 
335  if (m_debug_dump->is_open())
336  {
337  *m_debug_dump << format("%f \t", time);
338  *m_debug_dump << format("%f \t", reading);
339  *m_debug_dump << format("%f \t", estimation);
340  *m_debug_dump << format("%f \t", tau);
341  *m_debug_dump << "\n";
342  }
343  else
344  cout << "Unable to open file";
345 }
346 
348 {
349  using namespace std;
351 
352  for (size_t j = 0; j < m_readings.size(); j++)
353  {
354  o << format("e-nose #%u:\n", (unsigned)j);
355 
358 
359  ASSERT_(
360  m_readings[j].readingsVoltage.size() ==
361  m_readings[j].sensorTypes.size());
362 
363  for (it = m_readings[j].readingsVoltage.begin(),
364  itKind = m_readings[j].sensorTypes.begin();
365  it != m_readings[j].readingsVoltage.end(); it++, itKind++)
366  o << format("%04X: %.03f ", *itKind, *it);
367 
368  o << endl;
369 
370  o << format(
371  " Sensor pose on robot: (x,y,z)=(%.02f,%.02f,%.02f)\n",
372  m_readings[j].eNosePoseOnTheRobot.x,
373  m_readings[j].eNosePoseOnTheRobot.y,
374  m_readings[j].eNosePoseOnTheRobot.z);
375 
376  o << "Measured temperature: ";
377  if (m_readings[j].hasTemperature)
378  o << format("%.03f degC\n", m_readings[j].temperature);
379  else
380  o << "NOT AVAILABLE\n";
381  }
382 }
mrpt::math::TPose3D asTPose() const
Definition: CPose3D.cpp:1046
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
void noise_filtering(const float &reading, const mrpt::system::TTimeStamp &timestamp)
Reduce noise by averaging with a mobile window of specific size (winNoise_size)
GLuint buffer
Definition: glext.h:3917
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
GLenum GLsizei n
Definition: glext.h:5074
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:44
STL namespace.
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const override
A general method to retrieve the sensor pose on the robot.
unsigned char uint8_t
Definition: rptypes.h:41
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:90
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
This base provides a set of functions for maths stuff.
math::TPose3D eNosePoseOnTheRobot
The pose of the sensors on the robot.
std::vector< int > sensorTypes
The kind of sensors in the array (size of "sensorTypes" is the same that the size of "readingsVoltage...
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
Declares a class derived from "CObservation" that represents a set of readings from gas sensors...
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) override
A general method to change the sensor pose on the robot.
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 pause(const std::string &msg=std::string("Press any key to continue...")) noexcept
Shows the message "Press any key to continue" (or other custom message) to the current standard outpu...
Definition: os.cpp:428
void save_log_map(const mrpt::system::TTimeStamp &timestamp, const float &reading, const float &estimation, const float &tau)
Save the gas distribution estiamtion into a log file for offline representation.
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:48
GLdouble GLdouble GLdouble r
Definition: glext.h:3705
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
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
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
GLuint in
Definition: glext.h:7274
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:31
void inverse_MOSmodeling(const float &reading, const mrpt::system::TTimeStamp &timestamp)
Estimates the gas concentration based on readings and sensor model.
double timeDifference(const mrpt::system::TTimeStamp t_first, const mrpt::system::TTimeStamp t_later)
Returns the time difference from t1 to t2 (positive if t2 is posterior to t1), in seconds...
Definition: datetime.cpp:209
std::vector< float > readingsVoltage
The set of readings (in volts) from the array of sensors (size of "sensorTypes" is the same that the ...
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
unsigned __int32 uint32_t
Definition: rptypes.h:47
const Scalar * const_iterator
Definition: eigen_plugins.h:27
double timestampTotime_t(const mrpt::system::TTimeStamp t)
Transform from TTimeStamp to standard "time_t" (actually a double number, it can contain fractions of...
Definition: datetime.cpp:56
int sprintf(char *buf, size_t bufSize, const char *format,...) noexcept MRPT_printf_format_check(3
An OS-independent version of sprintf (Notice the bufSize param, which may be ignored in some compiler...
Definition: os.cpp:189
virtual void getDescriptionAsText(std::ostream &o) const
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
bool get_GasDistribution_estimation(float &reading, mrpt::system::TTimeStamp &timestamp)
Obtain an estimation of the gas distribution based on raw sensor readings.



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