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