35 for (i = 0; i <
n; i++)
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;
44 out << sensorLabel << timestamp;
65 for (i = 0; i <
n; i++)
68 m_readings[i].eNosePoseOnTheRobot = aux.
asTPose();
69 in >> m_readings[i].readingsVoltage;
70 in >> m_readings[i].sensorTypes;
73 in >> m_readings[i].hasTemperature;
74 if (m_readings[i].hasTemperature)
75 in >> m_readings[i].temperature;
79 m_readings[i].hasTemperature =
false;
80 m_readings[i].temperature = 0;
106 ASSERT_(readings.size() == 16);
111 TPose3D(0.20, -0.15, 0.10, 0, 0, 0);
120 m_readings.push_back(eNose);
124 TPose3D(0.20, 0.15, 0.10, .0, .0, .0);
133 m_readings.push_back(eNose);
146 if (!m_readings.empty())
147 out_sensorPose =
CPose3D(m_readings[0].eNosePoseOnTheRobot);
149 out_sensorPose =
CPose3D(0, 0, 0);
154 for (
auto&
r : m_readings)
r.eNosePoseOnTheRobot = newSensorPose.
asTPose();
163 noise_filtering(reading, timestamp);
166 if (decimate_count != decimate_value)
175 m_antiNoise_window[winNoise_size / 2].reading_filtered,
176 m_antiNoise_window[winNoise_size / 2].timestamp);
180 reading = last_Obs.estimation;
181 timestamp = last_Obs.timestamp;
186 last_Obs.timestamp, last_Obs.reading, last_Obs.estimation,
193 cout <<
"Error when decimating \n";
208 temporal_Obs.reading = reading;
209 temporal_Obs.timestamp = timestamp;
212 if (m_antiNoise_window.empty())
215 temporal_Obs.reading_filtered = reading;
218 m_antiNoise_window.assign(winNoise_size, temporal_Obs);
223 m_antiNoise_window.erase(m_antiNoise_window.begin());
224 m_antiNoise_window.push_back(temporal_Obs);
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;
232 m_antiNoise_window.at(winNoise_size / 2).reading_filtered =
233 partial_sum / winNoise_size;
237 cout <<
"Error when filtering noise from readings \n";
252 if (reading < min_reading) min_reading = reading;
255 if (!first_iteration)
262 if ((incT > 0) & (!first_incT))
268 if (fabs(incT - fixed_incT) > (
double)(0.05))
269 cout <<
"IncT is not constant by HW." << endl;
273 if (incT > 0) first_incT =
false;
277 if (reading < last_Obs.reading)
279 last_Obs.tau = a_decay * abs(reading - min_reading) + b_decay;
283 last_Obs.tau = a_rise * abs(reading - min_reading) + b_rise;
290 last_Obs.estimation =
291 ((reading - last_Obs.reading) * last_Obs.tau / incT) +
294 last_Obs.estimation = reading;
297 last_Obs.timestamp = timestamp;
298 last_Obs.reading = reading;
303 last_Obs.tau = b_rise;
304 last_Obs.reading = reading;
305 last_Obs.timestamp = timestamp;
306 last_Obs.estimation =
308 first_iteration =
false;
313 cerr <<
"**Exception in " 314 "CObservationGasSensors::CMOSmodel::inverse_MOSmodeling** " 324 const float& estimation,
const float& tau)
333 if (!m_debug_dump) m_debug_dump =
new ofstream(
buffer);
335 if (m_debug_dump->is_open())
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";
344 cout <<
"Unable to open file";
352 for (
size_t j = 0; j < m_readings.size(); j++)
354 o <<
format(
"e-nose #%u:\n", (
unsigned)j);
360 m_readings[j].readingsVoltage.size() ==
361 m_readings[j].sensorTypes.size());
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);
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);
376 o <<
"Measured temperature: ";
377 if (m_readings[j].hasTemperature)
378 o <<
format(
"%.03f degC\n", m_readings[j].temperature);
380 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.
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)
This must be inserted in all CSerializable classes implementation files.
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
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...
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
const Scalar * const_iterator
#define INVALID_TIMESTAMP
Represents an invalid timestamp, where applicable.
double timestampTotime_t(const mrpt::system::TTimeStamp t)
Transform from TTimeStamp to standard "time_t" (actually a double number, it can contain fractions of...
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.