Main MRPT website > C++ reference for MRPT 1.5.6
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-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 
13 #include <mrpt/utils/CStream.h>
14 #include <mrpt/system/os.h>
15 
16 using namespace mrpt::obs;
17 using namespace mrpt::utils;
18 using namespace mrpt::poses;
19 using namespace mrpt::math;
20 using namespace std;
21 
22 
23 // This must be added to any CSerializable class implementation file.
25 
26 /** Constructor
27  */
29  m_readings()
30 {
31 
32 }
33 
34 /*---------------------------------------------------------------
35  Implements the writing to a CStream capability of CSerializable objects
36  ---------------------------------------------------------------*/
38 {
39  if (version)
40  *version = 5;
41  else
42  {
43  uint32_t i,n = m_readings.size();
44  out << n;
45 
46  for (i=0;i<n;i++)
47  {
48  out << CPose3D(m_readings[i].eNosePoseOnTheRobot);
49  out << m_readings[i].readingsVoltage;
50  out << m_readings[i].sensorTypes;
51  out << m_readings[i].hasTemperature;
52  if (m_readings[i].hasTemperature)
53  out << m_readings[i].temperature;
54  }
55 
56  out << sensorLabel
57  << timestamp;
58 
59  }
60 }
61 
62 /*---------------------------------------------------------------
63  Implements the reading from a CStream capability of CSerializable objects
64  ---------------------------------------------------------------*/
66 {
67  switch(version)
68  {
69  case 2:
70  case 3:
71  case 4:
72  case 5:
73  {
74  // A general set of e-nose descriptors:
75  uint32_t i,n;
76 
77  in >> n;
78  m_readings.resize(n);
79 
80  CPose3D aux;
81 
82  for (i=0;i<n;i++)
83  {
84 
85  in >> aux; m_readings[i].eNosePoseOnTheRobot = aux;
86  in >> m_readings[i].readingsVoltage;
87  in >> m_readings[i].sensorTypes;
88  if (version>=3)
89  {
90  in >> m_readings[i].hasTemperature;
91  if (m_readings[i].hasTemperature)
92  in >> m_readings[i].temperature;
93  }
94  else
95  {
96  m_readings[i].hasTemperature=false;
97  m_readings[i].temperature=0;
98  }
99  }
100 
101  if (version>=4)
102  in >> sensorLabel;
103  else sensorLabel = "";
104 
105  if (version>=5)
106  in >> timestamp;
107  else timestamp = INVALID_TIMESTAMP;
108 
109 
110  }
111  break;
112  case 0:
113  case 1:
114  {
115  TObservationENose eNose;
116 
117  m_readings.clear();
118 
119  // There was a single set of 16 values from "Sancho" (DEC-2006)
120  CVectorFloat readings;
121  in >> readings;
122 
123  ASSERT_(readings.size()==16);
124 
125  // There was TWO e-noses:
126  // (1)
127  eNose.eNosePoseOnTheRobot = CPose3D( 0.20f,-0.15f, 0.10f ); // (x,y,z) only
128  eNose.readingsVoltage.resize(4);
129  eNose.readingsVoltage[0] = readings[2];
130  eNose.readingsVoltage[1] = readings[4];
131  eNose.readingsVoltage[2] = readings[5];
132  eNose.readingsVoltage[3] = readings[6];
133 
134  eNose.sensorTypes.clear();
135  eNose.sensorTypes.resize(4,0);
136  m_readings.push_back(eNose);
137 
138  // (2)
139  eNose.eNosePoseOnTheRobot = CPose3D( 0.20f, 0.15f, 0.10f ); // (x,y,z) only
140  eNose.readingsVoltage.resize(4);
141  eNose.readingsVoltage[0] = readings[8];
142  eNose.readingsVoltage[1] = readings[10];
143  eNose.readingsVoltage[2] = readings[12];
144  eNose.readingsVoltage[3] = readings[14];
145 
146  eNose.sensorTypes.clear();
147  eNose.sensorTypes.resize(4,0);
148  m_readings.push_back(eNose);
149 
150  } break;
151  default:
153 
154  };
155 
156 }
157 
158 
159 /*---------------------------------------------------------------
160  getSensorPose
161  ---------------------------------------------------------------*/
162 void CObservationGasSensors::getSensorPose( CPose3D &out_sensorPose ) const
163 {
164  if (!m_readings.empty())
165  out_sensorPose = CPose3D(m_readings[0].eNosePoseOnTheRobot);
166  else out_sensorPose = CPose3D(0,0,0);
167 }
168 
169 /*---------------------------------------------------------------
170  setSensorPose
171  ---------------------------------------------------------------*/
172 void CObservationGasSensors::setSensorPose( const CPose3D &newSensorPose )
173 {
174  size_t i, n = m_readings.size();
175  if (n)
176  for (i=0;i<n;i++)
177  m_readings[i].eNosePoseOnTheRobot= mrpt::math::TPose3D(newSensorPose);
178 }
179 
180 
181 
182 
183 
184 
185 /*---------------------------------------------------------------
186  CMOSmodel
187  ---------------------------------------------------------------*/
188 /** Constructor
189  */
190 
192  winNoise_size(30),
193  decimate_value(6),
194  a_rise(0),
195  b_rise(0),
196  a_decay(0),
197  b_decay(0),
198  save_maplog(false),
199  last_Obs(),
200  temporal_Obs(),
201  m_debug_dump(NULL),
202  decimate_count(1),
203  fixed_incT(0),
204  first_incT(true),
205  min_reading(10),
206  first_iteration(true)
207 {
208 }
209 
210 
212 {
213 }
214 
215 /*---------------------------------------------------------------
216  get_GasDistribution_estimation
217  ---------------------------------------------------------------*/
219 {
220  try{
221  //Noise filtering
222  noise_filtering(reading, timestamp);
223 
224  //Decimate
225  if ( decimate_count != decimate_value ){
226  decimate_count++;
227  return false;
228  }
229 
230  //Gas concentration estimation based on FIRST ORDER + NONLINEAR COMPENSATIONS DYNAMICS
231  inverse_MOSmodeling( m_antiNoise_window[winNoise_size/2].reading_filtered, m_antiNoise_window[winNoise_size/2].timestamp);
232  decimate_count = 1;
233 
234  //update (output)
235  reading = last_Obs.estimation;
236  timestamp = last_Obs.timestamp;
237 
238  //Save data map in log file for Matlab visualization
239  if (save_maplog)
240  save_log_map(last_Obs.timestamp, last_Obs.reading, last_Obs.estimation, last_Obs.tau );
241 
242  return true;
243 
244  }catch(...){
245  cout << "Error when decimating \n" ;
247  return false;
248  }
249 }
250 
251 /*---------------------------------------------------------------
252  noise_filtering (smooth)
253  ---------------------------------------------------------------*/
255 {
256  try{
257  //Store values in the temporal Observation
258  temporal_Obs.reading = reading;
259  temporal_Obs.timestamp = timestamp;
260 
261  // If first reading from E-nose
262  if ( m_antiNoise_window.empty() )
263  {
264  // use default values
265  temporal_Obs.reading_filtered = reading;
266 
267  // Populate the noise window
268  m_antiNoise_window.assign( winNoise_size, temporal_Obs );
269 
270  }else{
271  //Erase the first element (the oldest), and add the new one
272  m_antiNoise_window.erase( m_antiNoise_window.begin() );
273  m_antiNoise_window.push_back(temporal_Obs);
274  }
275 
276  //Average data to reduce noise (Noise Filtering)
277  float partial_sum = 0;
278  for (size_t i=0; i<m_antiNoise_window.size(); i++)
279  partial_sum += m_antiNoise_window.at(i).reading;
280 
281  m_antiNoise_window.at(winNoise_size/2).reading_filtered = partial_sum / winNoise_size;
282 
283  }catch(...){
284  cout << "Error when filtering noise from readings \n" ;
286  }
287 }
288 
289 
290 
291 /*---------------------------------------------------------------
292  inverse_MOSmodeling
293  ---------------------------------------------------------------*/
295 {
296  try{
297 
298  //Keep the minimum reading value as an approximation to the basline level
299  if (reading < min_reading)
300  min_reading = reading;
301 
302  // Check if estimation posible (not possible in the first iteration)
303  if ( !first_iteration)
304  {
305  //Assure the samples are provided at constant rate (important for the correct gas distribution estimation)
306  double incT = mrpt::system::timeDifference(last_Obs.timestamp,timestamp);
307 
308  if ( (incT >0) & (!first_incT) ){ //not the same sample due to initialization of buffers
309  if (fixed_incT == 0)
310  fixed_incT = incT;
311  else
312  //ASSERT_(fabs(incT - fixed_incT) < (double)(0.05));
313  if (fabs(incT - fixed_incT) > (double)(0.05))
314  cout << "IncT is not constant by HW." << endl;
315  }
316  else
317  {
318  if (incT > 0)
319  first_incT = false;
320  }
321 
322 
323  //slope<0 -->Decay
324  if ( reading < last_Obs.reading )
325  {
326  last_Obs.tau = a_decay * abs(reading-min_reading) + b_decay;
327  }
328  else //slope>=0 -->rise
329  {
330  last_Obs.tau = a_rise * abs(reading-min_reading) + b_rise;
331  }//end-if
332 
333  //New estimation values -- Ziegler-Nichols model --
334  if( incT >0)
335  //Initially there may come repetetive values till m_antiNoise_window is full populated.
336  last_Obs.estimation = ( (reading -last_Obs.reading)*last_Obs.tau/incT) + reading;
337  else
338  last_Obs.estimation = reading;
339 
340 
341  //Prepare the New observation
342  last_Obs.timestamp = timestamp;
343  last_Obs.reading = reading;
344 
345  }else{
346  // First filtered reading (use default values)
347  last_Obs.tau = b_rise;
348  last_Obs.reading = reading;
349  last_Obs.timestamp = timestamp;
350  last_Obs.estimation = reading; //No estimation possible at this step
351  first_iteration = false;
352  }//end-if estimation values
353 
354 
355  }catch(exception e){
356  cerr << "**Exception in CObservationGasSensors::CMOSmodel::inverse_MOSmodeling** " << e.what() << endl;
357  }
358 }
359 
360 /*---------------------------------------------------------------
361  save_log_map
362  ---------------------------------------------------------------*/
365  const float &reading,
366  const float &estimation,
367  const float &tau
368  )
369 {
370 
371  //function to save in a log file the information of the generated gas distribution estimation
372 
373  double time = mrpt::system::timestampTotime_t(timestamp);
374  char buffer [50];
375  sprintf (buffer, "./log_MOSmodel_GasDistribution.txt");
376 
377  if (!m_debug_dump)
378  m_debug_dump = new ofstream(buffer);
379 
380  if (m_debug_dump->is_open())
381  {
382  *m_debug_dump << format("%f \t", time );
383  *m_debug_dump << format("%f \t", reading );
384  *m_debug_dump << format("%f \t", estimation );
385  *m_debug_dump << format("%f \t", tau );
386  *m_debug_dump << "\n";
387  }
388  else cout << "Unable to open file";
389 }
390 
392 {
393  using namespace std;
395 
396  for (size_t j=0;j<m_readings.size();j++)
397  {
398  o << format("e-nose #%u:\n",(unsigned)j);
399 
402 
403  ASSERT_( m_readings[j].readingsVoltage.size() == m_readings[j].sensorTypes.size());
404 
405  for (it=m_readings[j].readingsVoltage.begin(),itKind=m_readings[j].sensorTypes.begin();it!=m_readings[j].readingsVoltage.end();it++,itKind++)
406  o << format( "%04X: %.03f ", *itKind, *it);
407 
408  o << endl;
409 
410  o << format(" Sensor pose on robot: (x,y,z)=(%.02f,%.02f,%.02f)\n",
411  m_readings[j].eNosePoseOnTheRobot.x,
412  m_readings[j].eNosePoseOnTheRobot.y,
413  m_readings[j].eNosePoseOnTheRobot.z );
414 
415  o << "Measured temperature: ";
416  if (m_readings[j].hasTemperature)
417  o << format("%.03f degC\n", m_readings[j].temperature );
418  else
419  o << "NOT AVAILABLE\n";
420  }
421 }
uint64_t TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
Definition: datetime.h:30
void noise_filtering(const float &reading, const mrpt::system::TTimeStamp &timestamp)
Reduce noise by averaging with a mobile window of specific size (winNoise_size)
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
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...
void BASE_IMPEXP pause(const std::string &msg=std::string("Press any key to continue...")) MRPT_NO_THROWS
Shows the message "Press any key to continue" (or other custom message) to the current standard outpu...
Definition: os.cpp:435
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
vector_int sensorTypes
The kind of sensors in the array (size of "sensorTypes" is the same that the size of "readingsVoltage...
const Scalar * const_iterator
Definition: eigen_plugins.h:24
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
math::TPose3D eNosePoseOnTheRobot
The pose of the sensors on the robot.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
GLuint buffer
Definition: glew.h:1585
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
Declares a class derived from "CObservation" that represents a set of readings from gas sensors...
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
Definition: datetime.h:17
mrpt::system::TTimeStamp timestamp
The associated UTC time-stamp. Where available, this should contain the accurate satellite-based time...
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const MRPT_OVERRIDE
A general method to retrieve the sensor pose on the robot.
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.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
Declares a class that represents any robot's observation.
std::vector< TObservationENose > m_readings
One entry per e-nose on the robot.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
#define ASSERT_(f)
void inverse_MOSmodeling(const float &reading, const mrpt::system::TTimeStamp &timestamp)
Estimates the gas concentration based on readings and sensor model.
int BASE_IMPEXP sprintf(char *buf, size_t bufSize, const char *format,...) MRPT_NO_THROWS 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:191
double BASE_IMPEXP 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:205
std::vector< float > readingsVoltage
The set of readings (in volts) from the array of sensors (size of "sensorTypes" is the same that the ...
unsigned __int32 uint32_t
Definition: rptypes.h:49
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) MRPT_OVERRIDE
A general method to change the sensor pose on the robot.
double BASE_IMPEXP 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:53
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092
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.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