37 for (i = 0; i <
n; i++)
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;
46 out << sensorLabel << timestamp;
67 for (i = 0; i <
n; i++)
70 m_readings[i].eNosePoseOnTheRobot = aux.
asTPose();
71 in >> m_readings[i].readingsVoltage;
72 in >> m_readings[i].sensorTypes;
75 in >> m_readings[i].hasTemperature;
76 if (m_readings[i].hasTemperature)
77 in >> m_readings[i].temperature;
81 m_readings[i].hasTemperature =
false;
82 m_readings[i].temperature = 0;
108 ASSERT_(readings.size() == 16);
113 TPose3D(0.20, -0.15, 0.10, 0, 0, 0);
122 m_readings.push_back(eNose);
126 TPose3D(0.20, 0.15, 0.10, .0, .0, .0);
135 m_readings.push_back(eNose);
148 if (!m_readings.empty())
149 out_sensorPose =
CPose3D(m_readings[0].eNosePoseOnTheRobot);
151 out_sensorPose =
CPose3D(0, 0, 0);
156 for (
auto&
r : m_readings)
r.eNosePoseOnTheRobot = newSensorPose.
asTPose();
165 noise_filtering(reading, timestamp);
168 if (decimate_count != decimate_value)
177 m_antiNoise_window[winNoise_size / 2].reading_filtered,
178 m_antiNoise_window[winNoise_size / 2].timestamp);
182 reading = last_Obs.estimation;
183 timestamp = last_Obs.timestamp;
188 last_Obs.timestamp, last_Obs.reading, last_Obs.estimation,
195 cout <<
"Error when decimating \n";
210 temporal_Obs.reading = reading;
211 temporal_Obs.timestamp = timestamp;
214 if (m_antiNoise_window.empty())
217 temporal_Obs.reading_filtered = reading;
220 m_antiNoise_window.assign(winNoise_size, temporal_Obs);
225 m_antiNoise_window.erase(m_antiNoise_window.begin());
226 m_antiNoise_window.push_back(temporal_Obs);
230 float partial_sum = 0;
231 for (
auto& i : m_antiNoise_window) partial_sum += i.reading;
233 m_antiNoise_window.at(winNoise_size / 2).reading_filtered =
234 partial_sum / winNoise_size;
238 cout <<
"Error when filtering noise from readings \n";
253 if (reading < min_reading) min_reading = reading;
256 if (!first_iteration)
263 if ((incT > 0) & (!first_incT))
269 if (fabs(incT - fixed_incT) > (
double)(0.05))
270 cout <<
"IncT is not constant by HW." << endl;
274 if (incT > 0) first_incT =
false;
278 if (reading < last_Obs.reading)
280 last_Obs.tau = a_decay * abs(reading - min_reading) + b_decay;
284 last_Obs.tau = a_rise * abs(reading - min_reading) + b_rise;
291 last_Obs.estimation =
292 ((reading - last_Obs.reading) * last_Obs.tau / incT) +
295 last_Obs.estimation = reading;
298 last_Obs.timestamp = timestamp;
299 last_Obs.reading = reading;
304 last_Obs.tau = b_rise;
305 last_Obs.reading = reading;
306 last_Obs.timestamp = timestamp;
307 last_Obs.estimation =
309 first_iteration =
false;
312 catch (
const exception& e)
314 cerr <<
"**Exception in " 315 "CObservationGasSensors::CMOSmodel::inverse_MOSmodeling** " 325 const float& estimation,
const float& tau)
334 if (!m_debug_dump) m_debug_dump =
new ofstream(
buffer);
336 if (m_debug_dump->is_open())
338 *m_debug_dump <<
format(
"%f \t", time);
339 *m_debug_dump <<
format(
"%f \t", reading);
340 *m_debug_dump <<
format(
"%f \t", estimation);
341 *m_debug_dump <<
format(
"%f \t", tau);
342 *m_debug_dump <<
"\n";
345 cout <<
"Unable to open file";
353 for (
size_t j = 0; j < m_readings.size(); j++)
355 o <<
format(
"e-nose #%u:\n", (
unsigned)j);
357 vector<float>::const_iterator it;
358 std::vector<int>::const_iterator itKind;
361 m_readings[j].readingsVoltage.size() ==
362 m_readings[j].sensorTypes.size());
364 for (it = m_readings[j].readingsVoltage.begin(),
365 itKind = m_readings[j].sensorTypes.begin();
366 it != m_readings[j].readingsVoltage.end(); it++, itKind++)
367 o <<
format(
"%04X: %.03f ", *itKind, *it);
372 " Sensor pose on robot: (x,y,z)=(%.02f,%.02f,%.02f)\n",
373 m_readings[j].eNosePoseOnTheRobot.x,
374 m_readings[j].eNosePoseOnTheRobot.y,
375 m_readings[j].eNosePoseOnTheRobot.z);
377 o <<
"Measured temperature: ";
378 if (m_readings[j].hasTemperature)
379 o <<
format(
"%.03f degC\n", m_readings[j].temperature);
381 o <<
"NOT AVAILABLE\n";
mrpt::math::TPose3D asTPose() const
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.
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 ×tamp)
Reduce noise by averaging with a mobile window of specific size (winNoise_size)
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
The structure for each e-nose.
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.
#define ASSERT_(f)
Defines an assertion mechanism.
mrpt::Clock::time_point TTimeStamp
A system independent time type, it holds the the number of 100-nanosecond intervals since January 1...
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...
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...
void save_log_map(const mrpt::system::TTimeStamp ×tamp, 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.
GLdouble GLdouble GLdouble r
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).
Declares a class that represents any robot's observation.
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Lightweight 3D pose (three spatial coordinates, plus three angular coordinates).
void inverse_MOSmodeling(const float &reading, const mrpt::system::TTimeStamp ×tamp)
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...
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
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
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 ×tamp)
Obtain an estimation of the gas distribution based on raw sensor readings.