MRPT  2.0.1
CGasConcentrationGridMap2D.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 "maps-precomp.h" // Precomp header
11 
12 #include <mrpt/core/round.h> // round()
13 #include <mrpt/img/color_maps.h>
17 #include <mrpt/math/CMatrixF.h>
20 #include <mrpt/opengl/CArrow.h>
23 #include <mrpt/system/CTicTac.h>
24 #include <mrpt/system/datetime.h>
25 #include <mrpt/system/filesystem.h>
26 #include <mrpt/system/os.h>
27 
28 using namespace mrpt;
29 using namespace mrpt::maps;
30 using namespace mrpt::obs;
31 using namespace mrpt::io;
32 using namespace mrpt::poses;
33 using namespace mrpt::img;
34 using namespace std;
35 using namespace mrpt::math;
36 
37 // =========== Begin of Map definition ============
39  "mrpt::maps::CGasConcentrationGridMap2D,gasGrid",
41 
43 
44  = default;
45 
48  const mrpt::config::CConfigFileBase& source,
49  const std::string& sectionNamePrefix)
50 {
51  // [<sectionNamePrefix>+"_creationOpts"]
52  const std::string sSectCreation =
53  sectionNamePrefix + string("_creationOpts");
54  MRPT_LOAD_CONFIG_VAR(min_x, float, source, sSectCreation);
55  MRPT_LOAD_CONFIG_VAR(max_x, float, source, sSectCreation);
56  MRPT_LOAD_CONFIG_VAR(min_y, float, source, sSectCreation);
57  MRPT_LOAD_CONFIG_VAR(max_y, float, source, sSectCreation);
58  MRPT_LOAD_CONFIG_VAR(resolution, float, source, sSectCreation);
60  sSectCreation, "mapType", mapType);
61 
62  insertionOpts.loadFromConfigFile(
63  source, sectionNamePrefix + string("_insertOpts"));
64 }
65 
67  std::ostream& out) const
68 {
69  out << mrpt::format(
70  "MAP TYPE = %s\n",
73  .c_str());
74  LOADABLEOPTS_DUMP_VAR(min_x, float);
75  LOADABLEOPTS_DUMP_VAR(max_x, float);
76  LOADABLEOPTS_DUMP_VAR(min_y, float);
77  LOADABLEOPTS_DUMP_VAR(max_y, float);
78  LOADABLEOPTS_DUMP_VAR(resolution, float);
79 
80  this->insertionOpts.dumpToTextStream(out);
81 }
82 
86 {
88  *dynamic_cast<const CGasConcentrationGridMap2D::TMapDefinition*>(&_def);
89  auto* obj = new CGasConcentrationGridMap2D(
90  def.mapType, def.min_x, def.max_x, def.min_y, def.max_y,
91  def.resolution);
92  obj->insertionOptions = def.insertionOpts;
93  return obj;
94 }
95 // =========== End of Map definition Block =========
96 
99 
100 // Short-cut:
101 #define LUT_TABLE (*(LUT.table))
102 
103 /*---------------------------------------------------------------
104  Constructor
105  ---------------------------------------------------------------*/
107  TMapRepresentation mapType, float x_min, float x_max, float y_min,
108  float y_max, float resolution)
109  : CRandomFieldGridMap2D(mapType, x_min, x_max, y_min, y_max, resolution),
110  insertionOptions()
111 {
112  // Override defaults:
113  insertionOptions.GMRF_saturate_min = 0;
114  insertionOptions.GMRF_saturate_max = 1;
115  insertionOptions.GMRF_lambdaObsLoss = 1.0;
116 
117  // Set the grid to initial values (and adjust covariance matrices,...)
118  // Also, calling clear() is mandatory to end initialization of our base
119  // class (read note in CRandomFieldGridMap2D::CRandomFieldGridMap2D)
121 
122  // Create WindGrids with same dimensions that the original map
123  windGrid_module.setSize(x_min, x_max, y_min, y_max, resolution);
124  windGrid_direction.setSize(x_min, x_max, y_min, y_max, resolution);
125 
126  // initialize counter for advection simulation
127  timeLastSimulated = mrpt::system::now();
128 }
129 
130 CGasConcentrationGridMap2D::~CGasConcentrationGridMap2D() = default;
131 /*---------------------------------------------------------------
132  clear
133  ---------------------------------------------------------------*/
134 void CGasConcentrationGridMap2D::internal_clear()
135 {
136  // Just do the generic clear:
138 
139  // Anything else special for this derived class?
140 
142  {
143  // Set default values of the wind grid
146 
147  /*float S = windGrid_direction.getSizeX() *
148 windGrid_direction.getSizeY();
149 
150  for( unsigned int y=windGrid_direction.getSizeY()/2;
151 y<windGrid_direction.getSizeY(); y++ )
152  {
153  for( unsigned int x=0; x<windGrid_direction.getSizeX(); x++ )
154  {
155  double * wind_cell = windGrid_direction.cellByIndex(x,y);
156  *wind_cell = 3*3.141516/2;
157 }
158  }*/
159 
160  // Generate Look-Up Table of the Gaussian weights due to wind advection.
162  {
163  // mrpt::system::pause();
164  THROW_EXCEPTION("Problem with LUT wind table");
165  }
166  }
167 }
168 
169 /*---------------------------------------------------------------
170  insertObservation
171  ---------------------------------------------------------------*/
173  const CObservation& obs, const CPose3D* robotPose)
174 {
175  MRPT_START
176 
177  CPose2D robotPose2D;
178  CPose3D robotPose3D;
179 
180  if (robotPose)
181  {
182  robotPose2D = CPose2D(*robotPose);
183  robotPose3D = (*robotPose);
184  }
185  else
186  {
187  // Default values are (0,0,0)
188  }
189 
191  {
192  /********************************************************************
193  OBSERVATION TYPE: CObservationGasSensors
194  ********************************************************************/
195  const auto& o = dynamic_cast<const CObservationGasSensors&>(obs);
196 
197  if (o.sensorLabel.compare(insertionOptions.gasSensorLabel) == 0)
198  {
199  float sensorReading;
200  CPose2D sensorPose;
201 
202  if (o.sensorLabel.compare("MCEnose") == 0 ||
203  o.sensorLabel.compare("Full_MCEnose") == 0)
204  {
205  ASSERT_(o.m_readings.size() > insertionOptions.enose_id);
207  &o.m_readings[insertionOptions.enose_id];
208 
209  // Compute the 3D sensor pose in world coordinates:
210  sensorPose = CPose2D(
211  CPose3D(robotPose2D) + CPose3D(it->eNosePoseOnTheRobot));
212 
213  // Compute the sensor reading value (Volts):
214  if (insertionOptions.gasSensorType == 0x0000)
215  { // compute the mean
216  sensorReading = d2f(math::mean(it->readingsVoltage));
217  }
218  else
219  {
220  // Look for the correct sensor type
221  size_t i;
222  for (i = 0; i < it->sensorTypes.size(); i++)
223  {
224  if (it->sensorTypes.at(i) ==
226  break;
227  }
228 
229  if (i < it->sensorTypes.size())
230  {
231  sensorReading = it->readingsVoltage[i];
232  }
233  else
234  {
235  cout << "Sensor especified not found, compute default "
236  "mean value"
237  << endl;
238  sensorReading = d2f(math::mean(it->readingsVoltage));
239  }
240  }
241  }
242  else //"GDM, RAE_PID, ENOSE_SIMUL
243  {
245  &o.m_readings[0];
246  // Compute the 3D sensor pose in world coordinates:
247  sensorPose = CPose2D(
248  CPose3D(robotPose2D) + CPose3D(it->eNosePoseOnTheRobot));
249  sensorReading = it->readingsVoltage[0];
250  }
251 
252  // Normalization:
253  sensorReading = (sensorReading - insertionOptions.R_min) /
255 
256  // Update the gross estimates of mean/vars for the whole reading
257  // history (see IROS2009 paper):
259  (sensorReading +
263  (square(sensorReading - m_average_normreadings_mean) +
267 
268  // Finally, do the actual map update with that value:
270  sensorReading,
271  mrpt::math::TPoint2D(sensorPose.x(), sensorPose.y()));
272  return true; // Done!
273  } // endif correct "gasSensorLabel"
274  } // end if "CObservationGasSensors"
275 
276  return false;
277  MRPT_END
278 }
279 
280 /*---------------------------------------------------------------
281  computeObservationLikelihood
282  ---------------------------------------------------------------*/
284  [[maybe_unused]] const CObservation& obs,
285  [[maybe_unused]] const CPose3D& takenFrom)
286 {
287  THROW_EXCEPTION("Not implemented yet!");
288 }
289 
293 {
295 
296  // To assure compatibility: The size of each cell:
297  auto n = static_cast<uint32_t>(sizeof(TRandomFieldCell));
298  out << n;
299 
300  // Save the map contents:
301  n = static_cast<uint32_t>(m_map.size());
302  out << n;
303 
304 // Save the "m_map": This requires special handling for big endian systems:
305 #if MRPT_IS_BIG_ENDIAN
306  for (uint32_t i = 0; i < n; i++)
307  {
308  out << m_map[i].kf_mean() << m_map[i].dm_mean()
309  << m_map[i].dmv_var_mean;
310  }
311 #else
312  // Little endian: just write all at once:
313  out.WriteBuffer(
314  &m_map[0],
315  sizeof(m_map[0]) * m_map.size()); // TODO: Do this endianness safe!!
316 #endif
317 
318  // Version 1: Save the insertion options:
319  out << uint8_t(m_mapType) << m_cov << m_stackedCov;
320 
327 
328  // New in v3:
330  << uint64_t(m_average_normreadings_count);
331 
332  out << genericMapParams; // v4
333 }
334 
335 // Aux struct used below (must be at global scope for STL):
337 {
338  float mean, std;
339  float w, wr;
340 };
341 
343  mrpt::serialization::CArchive& in, uint8_t version)
344 {
345  switch (version)
346  {
347  case 0:
348  case 1:
349  case 2:
350  case 3:
351  case 4:
352  case 5:
353  {
354  dyngridcommon_readFromStream(in, version < 5);
355 
356  // To assure compatibility: The size of each cell:
357  uint32_t n;
358  in >> n;
359 
360  if (version < 2)
361  { // Converter from old versions <=1
362  ASSERT_(
363  n == static_cast<uint32_t>(sizeof(TOldCellTypeInVersion1)));
364  // Load the map contents in an aux struct:
365  in >> n;
366  vector<TOldCellTypeInVersion1> old_map(n);
367  in.ReadBuffer(&old_map[0], sizeof(old_map[0]) * old_map.size());
368 
369  // Convert to newer format:
370  m_map.resize(n);
371  for (size_t k = 0; k < n; k++)
372  {
373  m_map[k].kf_mean() =
374  (old_map[k].w != 0) ? old_map[k].wr : old_map[k].mean;
375  m_map[k].kf_std() =
376  (old_map[k].w != 0) ? old_map[k].w : old_map[k].std;
377  }
378  }
379  else
380  {
382  n, static_cast<uint32_t>(sizeof(TRandomFieldCell)));
383  // Load the map contents:
384  in >> n;
385  m_map.resize(n);
386 
387 // Read the note in writeToStream()
388 #if MRPT_IS_BIG_ENDIAN
389  for (uint32_t i = 0; i < n; i++)
390  in >> m_map[i].kf_mean() >> m_map[i].dm_mean() >>
391  m_map[i].dmv_var_mean;
392 #else
393  // Little endian: just read all at once:
394  in.ReadBuffer(&m_map[0], sizeof(m_map[0]) * m_map.size());
395 #endif
396  }
397 
398  // Version 1: Insertion options:
399  if (version >= 1)
400  {
401  uint8_t i;
402  in >> i;
404 
405  in >> m_cov >> m_stackedCov;
406 
414  }
415 
416  if (version >= 3)
417  {
418  uint64_t N;
422  }
423 
424  if (version >= 4) in >> genericMapParams;
425 
427  }
428  break;
429  default:
431  };
432 }
433 
434 /*---------------------------------------------------------------
435  TInsertionOptions
436  ---------------------------------------------------------------*/
438  :
439 
440  gasSensorLabel("MCEnose"),
441  // By default use the mean between all e-nose sensors
442  windSensorLabel("windSensor")
443 
444 {
445 }
446 
448  std::ostream& out) const
449 {
450  out << "\n----------- [CGasConcentrationGridMap2D::TInsertionOptions] "
451  "------------ \n\n";
452  out << "[TInsertionOptions.Common] ------------ \n\n";
453  internal_dumpToTextStream_common(
454  out); // Common params to all random fields maps:
455 
456  out << "[TInsertionOptions.GasSpecific] ------------ \n\n";
457  out << mrpt::format(
458  "gasSensorLabel = %s\n",
459  gasSensorLabel.c_str());
460  out << mrpt::format(
461  "enose_id = %u\n", (unsigned)enose_id);
462  out << mrpt::format(
463  "gasSensorType = %u\n",
464  (unsigned)gasSensorType);
465  out << mrpt::format(
466  "windSensorLabel = %s\n",
467  windSensorLabel.c_str());
468  out << mrpt::format(
469  "useWindInformation = %u\n", useWindInformation);
470 
471  out << mrpt::format(
472  "advectionFreq = %f\n", advectionFreq);
473  out << mrpt::format(
474  "default_wind_direction = %f\n",
475  default_wind_direction);
476  out << mrpt::format(
477  "default_wind_speed = %f\n", default_wind_speed);
478  out << mrpt::format(
479  "std_windNoise_phi = %f\n", std_windNoise_phi);
480  out << mrpt::format(
481  "std_windNoise_mod = %f\n", std_windNoise_mod);
482 
483  out << "\n";
484 }
485 
486 /*---------------------------------------------------------------
487  loadFromConfigFile
488  ---------------------------------------------------------------*/
490  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
491 {
492  // Common data fields for all random fields maps:
493  internal_loadFromConfigFile_common(iniFile, section);
494 
495  // Specific data fields for gasGridMaps
496  gasSensorLabel = iniFile.read_string(
497  section.c_str(), "gasSensorLabel", "Full_MCEnose", true);
498  enose_id = iniFile.read_int(section.c_str(), "enoseID", enose_id);
499  // Read sensor type in hexadecimal
500  {
501  std::string sensorType_str =
502  iniFile.read_string(section.c_str(), "gasSensorType", "-1", true);
503  int tmpSensorType;
504  stringstream convert(sensorType_str);
505  convert >> std::hex >> tmpSensorType;
506 
507  if (tmpSensorType >= 0)
508  {
509  // Valid number found:
510  gasSensorType = tmpSensorType;
511  }
512  else
513  { // fall back to old name, or default to current value:
514  gasSensorType = iniFile.read_int(
515  section.c_str(), "KF_sensorType", gasSensorType, true);
516  }
517  }
518  windSensorLabel = iniFile.read_string(
519  section.c_str(), "windSensorLabel", "Full_MCEnose", true);
520 
521  // Indicates if wind information must be used for Advection Simulation
522  useWindInformation =
523  iniFile.read_bool(section.c_str(), "useWindInformation", "false", true);
524 
525  //(rad) The initial/default value of the wind direction
526  default_wind_direction =
527  iniFile.read_float(section.c_str(), "default_wind_direction", 0, false);
528  //(m/s) The initial/default value of the wind speed
529  default_wind_speed =
530  iniFile.read_float(section.c_str(), "default_wind_speed", 0, false);
531 
532  //(rad) The noise in the wind direction
533  std_windNoise_phi =
534  iniFile.read_float(section.c_str(), "std_windNoise_phi", 0, false);
535  //(m/s) The noise in the wind strenght
536  std_windNoise_mod =
537  iniFile.read_float(section.c_str(), "std_windNoise_mod", 0, false);
538 
539  //(m/s) The noise in the wind strenght
540  advectionFreq =
541  iniFile.read_float(section.c_str(), "advectionFreq", 1, true);
542 }
543 
544 /*---------------------------------------------------------------
545  getAs3DObject
546 ---------------------------------------------------------------*/
548  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
549 {
550  MRPT_START
553  MRPT_END
554 }
555 
556 /*---------------------------------------------------------------
557  getAs3DObject
558 ---------------------------------------------------------------*/
561  mrpt::opengl::CSetOfObjects::Ptr& varObj) const
562 {
563  MRPT_START
565  CRandomFieldGridMap2D::getAs3DObject(meanObj, varObj);
566  MRPT_END
567 }
568 
569 /*---------------------------------------------------------------
570  getWindAs3DObject
571 ---------------------------------------------------------------*/
573  mrpt::opengl::CSetOfObjects::Ptr& windObj) const
574 {
575  // Return an arrow map of the wind state (module(color) and direction).
576  float scale = 0.2f;
577  size_t arrow_separation =
578  5; // distance between arrows, expresed as times the cell resolution
579 
580  // map limits
581  float x_min = d2f(getXMin());
582  float x_max = d2f(getXMax());
583  float y_min = d2f(getYMin());
584  float y_max = d2f(getYMax());
585  float resol = d2f(getResolution());
586 
587  // Ensure map dimensions match with wind map
588  unsigned int wind_map_size =
590  ASSERT_(
591  wind_map_size ==
593  if (m_map.size() != wind_map_size)
594  {
595  cout << " GAS MAP DIMENSIONS DO NOT MATCH WIND MAP " << endl;
596  // mrpt::system::pause();
597  }
598 
599  size_t cx, cy;
600  vector<float> xs, ys;
601 
602  // xs: array of X-axis values
603  xs.resize(floor((x_max - x_min) / (arrow_separation * resol)));
604  for (cx = 0; cx < xs.size(); cx++)
605  xs[cx] = x_min + arrow_separation * resol * cx;
606 
607  // ys: array of X-axis values
608  ys.resize(floor((y_max - y_min) / (arrow_separation * resol)));
609  for (cy = 0; cy < ys.size(); cy++)
610  ys[cy] = y_min + arrow_separation * resol * cy;
611 
612  for (cy = 0; cy < ys.size(); cy++)
613  {
614  for (cx = 0; cx < xs.size(); cx++)
615  {
616  // Cell values [0,inf]:
617  double dir_xy = *windGrid_direction.cellByPos(xs[cx], ys[cy]);
618  double mod_xy = *windGrid_module.cellByPos(xs[cx], ys[cy]);
619 
620  auto obj = mrpt::opengl::CArrow::Create(
621  xs[cx], ys[cy], 0.f, xs[cx] + scale * (float)cos(dir_xy),
622  ys[cy] + scale * (float)sin(dir_xy), 0.f, 1.15f * scale,
623  0.3f * scale, 0.35f * scale);
624 
625  float r, g, b;
626  jet2rgb(mod_xy, r, g, b);
627  obj->setColor(r, g, b);
628 
629  windObj->insert(obj);
630  }
631  }
632 }
633 
634 /*---------------------------------------------------------------
635  increaseUncertainty
636 ---------------------------------------------------------------*/
638  const double STD_increase_value)
639 {
640  // Increase cell variance
641  // unsigned int cx,cy;
642  // double memory_retention;
643 
645  for (size_t it = 0; it < m_map.size(); it++)
646  {
647  m_stackedCov(it, 0) = m_stackedCov(it, 0) + STD_increase_value;
648  }
649 
650  // Update m_map.kf_std
652 
653  // for (cy=0; cy<m_size_y; cy++)
654  // {
655  // for (cx=0; cx<m_size_x; cx++)
656  // {
657  // // Forgetting_curve --> memory_retention =
658  // exp(-time/memory_relative_strenght)
659  // memory_retention = exp(- mrpt::system::timeDifference(m_map[cx +
660  // cy*m_size_x].last_updated, now()) / memory_relative_strenght);
661  // //Update Uncertainty (STD)
662  // m_map[cx + cy*m_size_x].kf_std = 1 - ( (1-m_map[cx +
663  // cy*m_size_x].updated_std) * memory_retention );
664  // }
665  // }
666 }
667 
668 /*---------------------------------------------------------------
669  simulateAdvection
670 ---------------------------------------------------------------*/
671 bool CGasConcentrationGridMap2D::simulateAdvection(double STD_increase_value)
672 {
673  /* 1- Ensure we can use Wind Information
674  -------------------------------------------------*/
675  if (!insertionOptions.useWindInformation) return false;
676 
677  // Get time since last simulation
678  double At =
680  cout << endl << " - At since last simulation = " << At << "seconds" << endl;
681  // update time of last updated.
683 
684  /* 3- Build Transition Matrix (SA)
685  This Matrix contains the probabilities of each cell
686  to "be displaced" to other cells by the wind effect.
687  ------------------------------------------------------*/
688  mrpt::system::CTicTac tictac;
689  size_t i = 0, c = 0;
690  int cell_i_cx, cell_i_cy;
691  float mu_phi, mu_r, mu_modwind;
692  const size_t N = m_map.size();
693  mrpt::math::CMatrixF A(N, N);
694  A.fill(0.0);
695  // std::vector<double> row_sum(N,0.0);
696  auto* row_sum = (double*)calloc(N, sizeof(double));
697 
698  try
699  {
700  // Ensure map dimensions match with wind map
701  unsigned int wind_map_size =
703  ASSERT_(
704  wind_map_size ==
706  if (N != wind_map_size)
707  {
708  cout << " GAS MAP DIMENSIONS DO NOT MATCH WIND INFORMATION "
709  << endl;
710  // mrpt::system::pause();
711  }
712 
713  tictac.Tic();
714 
715  // Generate Sparse Matrix of the wind advection SA
716  for (i = 0; i < N; i++)
717  {
718  // Cell_i indx and coordinates
719  idx2cxcy(i, cell_i_cx, cell_i_cy);
720 
721  // Read dirwind value of cell i
723  cell_i_cx, cell_i_cy); //[0,2*pi]
724  unsigned int phi_indx = round(mu_phi / LUT.phi_inc);
725 
726  // Read modwind value of cell i
727  mu_modwind =
728  *windGrid_module.cellByIndex(cell_i_cx, cell_i_cy); //[0,inf)
729  mu_r = mu_modwind * At;
730  if (mu_r > LUT.max_r) mu_r = LUT.max_r;
731  unsigned int r_indx = round(mu_r / LUT.r_inc);
732 
733  // Evaluate LUT
734  ASSERT_(phi_indx < LUT.phi_count);
735  ASSERT_(r_indx < LUT.r_count);
736 
737  // define label
738  vector<TGaussianCell>& cells_to_update =
739  LUT_TABLE[phi_indx][r_indx];
740 
741  // Generate Sparse Matrix with the wind weights "SA"
742  for (auto& ci : cells_to_update)
743  {
744  int final_cx = cell_i_cx + ci.cx;
745  int final_cy = cell_i_cy + ci.cy;
746  // Check if affected cells is within the map
747  if ((final_cx >= 0) && (final_cx < (int)getSizeX()) &&
748  (final_cy >= 0) && (final_cy < (int)getSizeY()))
749  {
750  int final_idx = final_cx + final_cy * getSizeX();
751 
752  // Add Value to SA Matrix
753  if (ci.value != 0.0)
754  {
755  A(final_idx, i) = ci.value;
756  row_sum[final_idx] += ci.value;
757  }
758  }
759  } // end-for ci
760  } // end-for cell i
761 
762  cout << " - SA matrix computed in " << tictac.Tac() << "s" << endl
763  << endl;
764  }
765  catch (const std::exception& e)
766  {
767  cout << " ######### EXCEPTION computing Transition Matrix (A) "
768  "##########\n: "
769  << e.what() << endl;
770  cout << "on cell i= " << i << " c=" << c << endl << endl;
771  return false;
772  }
773 
774  /* Update Mean + Variance as a Gaussian Mixture
775  ------------------------------------------------*/
776  try
777  {
778  tictac.Tic();
779  // std::vector<double> new_means(N,0.0);
780  auto* new_means = (double*)calloc(N, sizeof(double));
781  // std::vector<double> new_variances(N,0.0);
782  auto* new_variances = (double*)calloc(N, sizeof(double));
783 
784  for (size_t it_i = 0; it_i < N; it_i++)
785  {
786  //--------
787  // mean
788  //--------
789  for (size_t it_j = 0; it_j < N; it_j++)
790  {
791  if (m_map[it_j].kf_mean() != 0 && A(it_i, it_j) != 0)
792  {
793  if (row_sum[it_i] >= 1)
794  new_means[it_i] += (A(it_i, it_j) / row_sum[it_i]) *
795  m_map[it_j].kf_mean();
796  else
797  new_means[it_i] +=
798  A(it_i, it_j) * m_map[it_j].kf_mean();
799  }
800  }
801 
802  //----------
803  // variance
804  //----------
805  // Consider special case (borders cells)
806  if (row_sum[it_i] < 1)
807  new_variances[it_i] =
808  (1 - row_sum[it_i]) *
810 
811  for (size_t it_j = 0; it_j < N; it_j++)
812  {
813  if (A(it_i, it_j) != 0)
814  {
815  if (row_sum[it_i] >= 1)
816  new_variances[it_i] +=
817  (A(it_i, it_j) / row_sum[it_i]) *
818  (m_stackedCov(it_j, 0) +
819  square(m_map[it_j].kf_mean() - new_means[it_i]));
820  else
821  new_variances[it_i] +=
822  A(it_i, it_j) *
823  (m_stackedCov(it_j, 0) +
824  square(m_map[it_j].kf_mean() - new_means[it_i]));
825  }
826  }
827  }
828 
829  // Update means and Cov of the Kalman filter state
830  for (size_t it_i = 0; it_i < N; it_i++)
831  {
832  m_map[it_i].kf_mean() = new_means[it_i]; // means
833 
834  // Variances
835  // Scale the Current Covariances with the new variances
836  for (size_t it_j = 0; it_j < N; it_j++)
837  {
838  m_stackedCov(it_i, it_j) =
839  (m_stackedCov(it_i, it_j) / m_stackedCov(it_i, it_i)) *
840  new_variances[it_i]; // variances
841  m_stackedCov(it_j, it_i) = m_stackedCov(it_i, it_j);
842  }
843  }
846 
847  cout << " - Mean&Var updated in " << tictac.Tac() << "s" << endl;
848 
849  // Free Memory
850  free(row_sum);
851  free(new_means);
852  free(new_variances);
853  }
854  catch (const std::exception& e)
855  {
856  cout << " ######### EXCEPTION Updating Covariances ##########\n: "
857  << e.what() << endl;
858  cout << "on row i= " << i << " column c=" << c << endl << endl;
859  return false;
860  }
861 
862  // cout << " Increasing general STD..." << endl;
863  increaseUncertainty(STD_increase_value);
864 
865  return true;
866 }
867 
868 /*---------------------------------------------------------------
869  build_Gaussian_Wind_Grid
870 ---------------------------------------------------------------*/
871 
873 /** Builds a LookUp table with the values of the Gaussian Weights result of the
874 wind advection
875 * for a specific condition.
876 *
877 * The LUT contains the values of the Gaussian Weigths and the references to
878 the cell indexes to be applied.
879 * Since the LUT is independent of the wind direction and angle, it generates
880 the Gaussian Weights for different configurations
881 * of wind angle and module values.
882 *
883 * To increase precission, each cell of the grid is sub-divided in subcells of
884 smaller size.
885 
886 * cell_i --> Cell origin (We consider our reference system in the bottom left
887 corner of cell_i ).
888  Is the cell that contains the gas measurement which will be
889 propagated by the wind.
890  The wind propagates in the shape of a 2D Gaussian with center in
891 the target cell (cell_j)
892 * cell_j --> Target cell. Is the cell where falls the center of the Gaussian
893 that models the propagation of the gas comming from cell_i.
894 */
895 
896 {
897  cout << endl << "---------------------------------" << endl;
898  cout << " BUILDING GAUSSIAN WIND WEIGHTS " << endl;
899  cout << "---------------------------------" << endl << endl;
900 
901  //-----------------------------
902  // PARAMS
903  //-----------------------------
904  LUT.resolution = getResolution(); // resolution of the grid-cells (m)
905  LUT.std_phi =
907  .std_windNoise_phi; // Standard Deviation in wind Angle (cte)
910  .advectionFreq; // Standard Deviation in wind module (cte)
911  std::string filename = format(
912  "Gaussian_Wind_Weights_res(%f)_stdPhi(%f)_stdR(%f).gz", LUT.resolution,
913  LUT.std_phi, LUT.std_r);
914 
915  // Fixed Params:
916  LUT.phi_inc = M_PIf / 8; // Increment in the wind Angle. (rad)
917  LUT.phi_count =
918  round(2 * M_PI / LUT.phi_inc) + 1; // Number of angles to generate
919  LUT.r_inc = 0.1f; // Increment in the wind Module. (m)
920  LUT.max_r = 2; // maximum distance (m) to simulate
921  LUT.r_count =
922  round(LUT.max_r / LUT.r_inc) + 1; // Number of wind modules to simulate
923 
924  LUT.table = new vector<vector<vector<TGaussianCell>>>(
925  LUT.phi_count,
926  vector<vector<TGaussianCell>>(LUT.r_count, vector<TGaussianCell>()));
927 
928  // LUT.table = new
929  // vector<vector<vector<vector<vector<TGaussianCell>>>>>(LUT.subcell_count,
930  // vector<vector<vector<vector<TGaussianCell>>>>(LUT.subcell_count,
931  // vector<vector<vector<TGaussianCell>>>(LUT.phi_count,
932  // vector<vector<TGaussianCell>>(LUT.r_count,vector<TGaussianCell>()) ) ) );
933 
934  //-----------------------------
935  // Check if file exists
936  //-----------------------------
937 
938  cout << "Looking for file: " << filename.c_str() << endl;
939 
940  if (mrpt::system::fileExists(filename.c_str()))
941  {
942  // file exists. Load lookUptable from file
943  cout << "LookUp table found for this configuration. Loading..." << endl;
945  }
946  else
947  {
948  // file does not exists. Generate LookUp table.
949  cout << "LookUp table NOT found. Generating table..." << endl;
950 
951  bool debug = true;
952  FILE* debug_file;
953 
954  if (debug)
955  {
956  debug_file = fopen("simple_LUT.txt", "w");
957  fprintf(
958  debug_file, " phi_inc = %.4f \n r_inc = %.4f \n", LUT.phi_inc,
959  LUT.r_inc);
960  fprintf(
961  debug_file, " std_phi = %.4f \n std_r = %.4f \n", LUT.std_phi,
962  LUT.std_r);
963  fprintf(debug_file, "[ phi ] [ r ] ---> (cx,cy)=Value\n");
964  fprintf(debug_file, "----------------------------------\n");
965  }
966 
967  // For the different possible angles (phi)
968  for (size_t phi_indx = 0; phi_indx < LUT.phi_count; phi_indx++)
969  {
970  // mean of the phi value
971  float phi = phi_indx * LUT.phi_inc;
972 
973  // For the different and possibe wind modules (r)
974  for (size_t r_indx = 0; r_indx < LUT.r_count; r_indx++)
975  {
976  // mean of the radius value
977  float r = r_indx * LUT.r_inc;
978 
979  if (debug)
980  {
981  fprintf(debug_file, "\n[%.2f] [%.2f] ---> ", phi, r);
982  }
983 
984  // Estimates Cell_i_position
985  // unsigned int cell_i_cx = 0;
986  // unsigned int cell_i_cy = 0;
987  float cell_i_x = LUT.resolution / 2.0;
988  float cell_i_y = LUT.resolution / 2.0;
989 
990  // Estimate target position according to the mean value of wind.
991  // float x_final = cell_i_x + r*cos(phi);
992  // float y_final = cell_i_y + r*sin(phi);
993 
994  // Determine cell_j coordinates respect to origin_cell
995  // int cell_j_cx = static_cast<int>(floor(
996  // (x_final)/LUT.resolution ));
997  // int cell_j_cy = static_cast<int>(floor(
998  // (y_final)/LUT.resolution ));
999  // Center of cell_j
1000  // float cell_j_x = (cell_j_cx+0.5f)*LUT.resolution;
1001  // float cell_j_y = (cell_j_cy+0.5f)*LUT.resolution;
1002  // left bottom corner of cell_j
1003  // float cell_j_xmin = cell_j_x - LUT.resolution/2.0;
1004  // float cell_j_ymin = cell_j_y - LUT.resolution/2.0;
1005 
1006  /* ---------------------------------------------------------------------------------
1007  Generate bounding-box (+/- 3std) to determine which cells
1008  to update
1009  ---------------------------------------------------------------------------------*/
1010  std::vector<double> vertex_x, vertex_y;
1011  vertex_x.resize(14);
1012  vertex_y.resize(14);
1013  // Bounding-Box initialization
1014  double minBBox_x = 1000;
1015  double maxBBox_x = -1000;
1016  double minBBox_y = 1000;
1017  double maxBBox_y = -1000;
1018 
1019  // Consider special case for high uncertainty in PHI. The shape
1020  // of the polygon is a donut.
1021  double std_phi_BBox = LUT.std_phi;
1022  if (std_phi_BBox > M_PI / 3)
1023  {
1024  std_phi_BBox = M_PI / 3; // To avoid problems generating
1025  // the bounding box. For std>pi/3
1026  // the shape is always a donut.
1027  }
1028 
1029  // Calculate bounding box limits
1030  size_t indx = 0;
1031  int sr = 3;
1032  for (int sd = (-3); sd <= (3); sd++)
1033  {
1034  vertex_x[indx] =
1035  cell_i_x +
1036  (r + sr * LUT.std_r) * cos(phi + sd * std_phi_BBox);
1037  if (vertex_x[indx] < minBBox_x) minBBox_x = vertex_x[indx];
1038  if (vertex_x[indx] > maxBBox_x) maxBBox_x = vertex_x[indx];
1039 
1040  vertex_y[indx] =
1041  cell_i_y +
1042  (r + sr * LUT.std_r) * sin(phi + sd * std_phi_BBox);
1043  if (vertex_y[indx] < minBBox_y) minBBox_y = vertex_y[indx];
1044  if (vertex_y[indx] > maxBBox_y) maxBBox_y = vertex_y[indx];
1045 
1046  indx++;
1047  }
1048  sr = -3;
1049  for (int sd = (3); sd >= (-3); sd--)
1050  {
1051  vertex_x[indx] =
1052  cell_i_x +
1053  (r + sr * LUT.std_r) * cos(phi + sd * std_phi_BBox);
1054  if (vertex_x[indx] < minBBox_x) minBBox_x = vertex_x[indx];
1055  if (vertex_x[indx] > maxBBox_x) maxBBox_x = vertex_x[indx];
1056 
1057  vertex_y[indx] =
1058  cell_i_y +
1059  (r + sr * LUT.std_r) * sin(phi + sd * std_phi_BBox);
1060  if (vertex_y[indx] < minBBox_y) minBBox_y = vertex_y[indx];
1061  if (vertex_y[indx] > maxBBox_y) maxBBox_y = vertex_y[indx];
1062 
1063  indx++;
1064  }
1065 
1066  /* ------------------------------------------------------------------------
1067  Determine range of cells to update according to the
1068  Bounding-Box limits.
1069  Origin cell is cx=cy= 0 x[0,res), y[0,res)
1070  ---------------------------------------------------------------------------*/
1071  int min_cx =
1072  static_cast<int>(floor(minBBox_x / LUT.resolution));
1073  int max_cx =
1074  static_cast<int>(floor(maxBBox_x / LUT.resolution));
1075  int min_cy =
1076  static_cast<int>(floor(minBBox_y / LUT.resolution));
1077  int max_cy =
1078  static_cast<int>(floor(maxBBox_y / LUT.resolution));
1079 
1080  int num_cells_affected =
1081  (max_cx - min_cx + 1) * (max_cy - min_cy + 1);
1082 
1083  if (num_cells_affected == 1)
1084  {
1085  // Concentration of cell_i moves to cell_a (cx,cy)
1086  TGaussianCell gauss_info;
1087  gauss_info.cx = min_cx; // since max_cx == min_cx
1088  gauss_info.cy = min_cy;
1089  gauss_info.value = 1; // prob = 1
1090 
1091  // Add cell volume to LookUp Table
1092  LUT_TABLE[phi_indx][r_indx].push_back(gauss_info);
1093 
1094  if (debug)
1095  {
1096  // Save to file (debug)
1097  fprintf(
1098  debug_file, "(%d,%d)=%.4f", gauss_info.cx,
1099  gauss_info.cy, gauss_info.value);
1100  }
1101  }
1102  else
1103  {
1104  // Estimate volume of the Gaussian under each affected cell
1105 
1106  float subcell_pres = LUT.resolution / 10;
1107  // Determine the number of subcells inside the Bounding-Box
1108  const int BB_x_subcells =
1109  (int)(floor((maxBBox_x - minBBox_x) / subcell_pres) + 1);
1110  const int BB_y_subcells =
1111  (int)(floor((maxBBox_y - minBBox_y) / subcell_pres) + 1);
1112 
1113  double subcell_pres_x =
1114  (maxBBox_x - minBBox_x) / BB_x_subcells;
1115  double subcell_pres_y =
1116  (maxBBox_y - minBBox_y) / BB_y_subcells;
1117 
1118  // Save the W value of each cell using a map
1119  std::map<std::pair<int, int>, float> w_values;
1120  std::map<std::pair<int, int>, float>::iterator it;
1121  float sum_w = 0;
1122 
1123  for (int scy = 0; scy < BB_y_subcells; scy++)
1124  {
1125  for (int scx = 0; scx < BB_x_subcells; scx++)
1126  {
1127  // P-Subcell coordinates (center of the p-subcell)
1128  float subcell_a_x =
1129  minBBox_x + (scx + 0.5f) * subcell_pres_x;
1130  float subcell_a_y =
1131  minBBox_y + (scy + 0.5f) * subcell_pres_y;
1132 
1133  // distance and angle between cell_i and subcell_a
1134  float r_ia = sqrt(
1135  square(subcell_a_x - cell_i_x) +
1136  square(subcell_a_y - cell_i_y));
1137  float phi_ia = atan2(
1138  subcell_a_y - cell_i_y, subcell_a_x - cell_i_x);
1139 
1140  // Volume Approximation of subcell_a (Gaussian
1141  // Bivariate)
1142  float w =
1143  (1 / (2 * M_PI * LUT.std_r * LUT.std_phi)) *
1144  exp(-0.5 *
1145  (square(r_ia - r) / square(LUT.std_r) +
1146  square(phi_ia - phi) /
1147  square(LUT.std_phi)));
1148  w += (1 / (2 * M_PI * LUT.std_r * LUT.std_phi)) *
1149  exp(-0.5 *
1150  (square(r_ia - r) / square(LUT.std_r) +
1151  square(phi_ia + 2 * M_PI - phi) /
1152  square(LUT.std_phi)));
1153  w += (1 / (2 * M_PI * LUT.std_r * LUT.std_phi)) *
1154  exp(-0.5 *
1155  (square(r_ia - r) / square(LUT.std_r) +
1156  square(phi_ia - 2 * M_PI - phi) /
1157  square(LUT.std_phi)));
1158 
1159  // Since we work with a cell grid, approximate the
1160  // weight of the gaussian by the volume of the
1161  // subcell_a
1162  if (r_ia != 0.0)
1163  w =
1164  (w * (subcell_pres_x * subcell_pres_y) /
1165  r_ia);
1166 
1167  // Determine cell index of the current subcell
1168  int cell_cx = static_cast<int>(
1169  floor(subcell_a_x / LUT.resolution));
1170  int cell_cy = static_cast<int>(
1171  floor(subcell_a_y / LUT.resolution));
1172 
1173  // Save w value
1174  it =
1175  w_values.find(std::make_pair(cell_cx, cell_cy));
1176  if (it != w_values.end()) // already exists
1177  w_values[std::make_pair(cell_cx, cell_cy)] += w;
1178  else
1179  w_values[std::make_pair(cell_cx, cell_cy)] = w;
1180 
1181  sum_w = sum_w + w;
1182  } // end-for scx
1183  } // end-for scy
1184 
1185  // SAVE to LUT
1186  for (it = w_values.begin(); it != w_values.end(); it++)
1187  {
1188  float w_final =
1189  (it->second) / sum_w; // normalization to 1
1190 
1191  if (w_final >= 0.001)
1192  {
1193  // Save the weight of the gaussian volume for cell_a
1194  // (cx,cy)
1195  TGaussianCell gauss_info;
1196  gauss_info.cx = it->first.first;
1197  gauss_info.cy = it->first.second;
1198  gauss_info.value = w_final;
1199 
1200  // Add cell volume to LookUp Table
1201  LUT_TABLE[phi_indx][r_indx].push_back(gauss_info);
1202 
1203  if (debug)
1204  {
1205  // Save to file (debug)
1206  fprintf(
1207  debug_file, "(%d,%d)=%.6f ",
1208  gauss_info.cx, gauss_info.cy,
1209  gauss_info.value);
1210  }
1211  }
1212  }
1213 
1214  // OLD WAY
1215 
1216  /* ---------------------------------------------------------
1217  Estimate the volume of the Gaussian on each affected cell
1218  //-----------------------------------------------------------*/
1219  // for(int cx=min_cx; cx<=max_cx; cx++)
1220  //{
1221  // for(int cy=min_cy; cy<=max_cy; cy++)
1222  // {
1223  // // Coordinates of affected cell (center of the cell)
1224  // float cell_a_x = (cx+0.5f)*LUT.resolution;
1225  // float cell_a_y = (cy+0.5f)*LUT.resolution;
1226  // float w_cell_a = 0.0; //initial Gaussian value of
1227  // cell afected
1228 
1229  // // Estimate volume of the Gaussian under cell (a)
1230  // // Partition each cell into (p x p) subcells and
1231  // evaluate the gaussian.
1232  // int p = 40;
1233  // float subcell_pres = LUT.resolution/p;
1234  // float cell_a_x_min = cell_a_x - LUT.resolution/2.0;
1235  // float cell_a_y_min = cell_a_y - LUT.resolution/2.0;
1236 
1237  //
1238  // for(int scy=0; scy<p; scy++)
1239  // {
1240  // for(int scx=0; scx<p; scx++)
1241  // {
1242  // //P-Subcell coordinates (center of the
1243  // p-subcell)
1244  // float subcell_a_x = cell_a_x_min +
1245  //(scx+0.5f)*subcell_pres;
1246  // float subcell_a_y = cell_a_y_min +
1247  //(scy+0.5f)*subcell_pres;
1248 
1249  // //distance and angle between cell_i and
1250  // subcell_a
1251  // float r_ia = sqrt(
1252  // square(subcell_a_x-cell_i_x)
1253  //+
1254  // square(subcell_a_y-cell_i_y) );
1255  // float phi_ia = atan2(subcell_a_y-cell_i_y,
1256  // subcell_a_x-cell_i_x);
1257 
1258  // //Volume Approximation of subcell_a
1259  //(Gaussian
1260  // Bivariate)
1261  // float w = (1/(2*M_PI*LUT.std_r*LUT.std_phi))
1262  //*
1263  // exp(-0.5*( square(r_ia-r)/square(LUT.std_r) +
1264  // square(phi_ia-phi)/square(LUT.std_phi) ) );
1265  // w += (1/(2*M_PI*LUT.std_r*LUT.std_phi)) *
1266  // exp(-0.5*( square(r_ia-r)/square(LUT.std_r) +
1267  // square(phi_ia+2*M_PI-phi)/square(LUT.std_phi) ) );
1268  // w += (1/(2*M_PI*LUT.std_r*LUT.std_phi)) *
1269  // exp(-0.5*( square(r_ia-r)/square(LUT.std_r) +
1270  // square(phi_ia-2*M_PI-phi)/square(LUT.std_phi) ) );
1271  //
1272  // //Since we work with a cell grid,
1273  // approximate
1274  // the
1275  // weight of the gaussian by the volume of the subcell_a
1276  // if (r_ia != 0.0)
1277  // w_cell_a = w_cell_a + (w *
1278  // square(subcell_pres)/r_ia);
1279  // }//end-for scx
1280  // }//end-for scy
1281 
1282  // //Save the weight of the gaussian volume for cell_a
1283  //(cx,cy)
1284  // TGaussianCell gauss_info;
1285  // gauss_info.cx = cx;
1286  // gauss_info.cy = cy;
1287  // gauss_info.value = w_cell_a;
1288 
1289  // //Add cell volume to LookUp Table
1290  // LUT_TABLE[phi_indx][r_indx].push_back(gauss_info);
1291 
1292  // if (debug)
1293  // {
1294  // //Save to file (debug)
1295  // fprintf(debug_file, "(%d,%d)=%.6f
1296  //",gauss_info.cx, gauss_info.cy, gauss_info.value);
1297  // }
1298  //
1299  //
1300  // }//end-for cy
1301  //}//end-for cx
1302 
1303  } // end-if only one affected cell
1304 
1305  } // end-for r
1306  } // end-for phi
1307 
1308  if (debug) fclose(debug_file);
1309 
1310  // Save LUT to File
1312 
1313  } // end-if table not available
1314 }
1315 
1317 {
1318  // Save LUT to file
1319  cout << "Saving to File ....";
1320 
1322  "Gaussian_Wind_Weights_res(%f)_stdPhi(%f)_stdR(%f).gz", LUT.resolution,
1323  LUT.std_phi, LUT.std_r));
1324  if (!fo.fileOpenCorrectly())
1325  {
1326  return false;
1327  cout << "WARNING: Gaussian_Wind_Weights file NOT SAVED" << endl;
1328  }
1329  auto f = mrpt::serialization::archiveFrom(fo);
1330 
1331  try
1332  {
1333  // Save params first
1334  f << LUT.resolution; // cell resolution used
1335  f << LUT.std_phi; // std_phi used
1336  f << LUT.std_r;
1337 
1338  f << LUT.phi_inc; // rad
1339  f << (float)LUT.phi_count;
1340  f << LUT.r_inc; // m
1341  f << LUT.max_r; // maximum distance (m)
1342  f << (float)LUT.r_count;
1343 
1344  // Save Multi-table
1345  // vector< vector< vector<TGaussianCell>>>>> *table;
1346 
1347  for (size_t phi_indx = 0; phi_indx < LUT.phi_count; phi_indx++)
1348  {
1349  for (size_t r_indx = 0; r_indx < LUT.r_count; r_indx++)
1350  {
1351  // save all cell values.
1352  size_t N = LUT_TABLE[phi_indx][r_indx].size();
1353  f << (float)N;
1354 
1355  for (size_t i = 0; i < N; i++)
1356  {
1357  f << (float)LUT_TABLE[phi_indx][r_indx][i].cx;
1358  f << (float)LUT_TABLE[phi_indx][r_indx][i].cy;
1359  f << LUT_TABLE[phi_indx][r_indx][i].value;
1360  }
1361  }
1362  }
1363  cout << "DONE" << endl;
1364  return true;
1365  }
1366  catch (const std::exception& e)
1367  {
1368  cout << endl
1369  << "------------------------------------------------------------"
1370  << endl;
1371  cout << "EXCEPTION WHILE SAVING LUT TO FILE" << endl;
1372  cout << "Exception = " << e.what() << endl;
1373  return false;
1374  }
1375 }
1376 
1378 {
1379  // LOAD LUT from file
1380  cout << "Loading from File ....";
1381 
1382  try
1383  {
1385  "Gaussian_Wind_Weights_res(%f)_stdPhi(%f)_stdR(%f).gz",
1387  if (!fi.fileOpenCorrectly())
1388  {
1389  cout << "WARNING WHILE READING FROM: Gaussian_Wind_Weights" << endl;
1390  return false;
1391  }
1392  auto f = mrpt::serialization::archiveFrom(fi);
1393 
1394  float t_float;
1395  unsigned int t_uint;
1396  // Ensure params from file are correct with the specified in the ini
1397  // file
1398  f >> t_float;
1399  ASSERT_(LUT.resolution == t_float);
1400 
1401  f >> t_float;
1402  ASSERT_(LUT.std_phi == t_float);
1403 
1404  f >> t_float;
1405  ASSERT_(LUT.std_r == t_float);
1406 
1407  f >> t_float;
1408  ASSERT_(LUT.phi_inc == t_float);
1409 
1410  f >> t_float;
1411  t_uint = (unsigned int)t_float;
1412  ASSERT_(LUT.phi_count == t_uint);
1413 
1414  f >> t_float;
1415  ASSERT_(LUT.r_inc == t_float);
1416 
1417  f >> t_float;
1418  ASSERT_(LUT.max_r == t_float);
1419 
1420  f >> t_float;
1421  t_uint = (unsigned int)t_float;
1422  ASSERT_(LUT.r_count == t_uint);
1423 
1424  // Load Multi-table
1425  // vector< vector< vector<TGaussianCell>>>>> *table;
1426 
1427  for (size_t phi_indx = 0; phi_indx < LUT.phi_count; phi_indx++)
1428  {
1429  for (size_t r_indx = 0; r_indx < LUT.r_count; r_indx++)
1430  {
1431  // Number of cells to update
1432  size_t N;
1433  f >> t_float;
1434  N = (size_t)t_float;
1435 
1436  for (size_t i = 0; i < N; i++)
1437  {
1438  TGaussianCell gauss_info;
1439  f >> t_float;
1440  gauss_info.cx = (int)t_float;
1441 
1442  f >> t_float;
1443  gauss_info.cy = (int)t_float;
1444 
1445  f >> gauss_info.value;
1446 
1447  // Add cell volume to LookUp Table
1448  LUT_TABLE[phi_indx][r_indx].push_back(gauss_info);
1449  }
1450  }
1451  }
1452  cout << "DONE" << endl;
1453  return true;
1454  }
1455  catch (const std::exception& e)
1456  {
1457  cout << endl
1458  << "------------------------------------------------------------"
1459  << endl;
1460  cout << "EXCEPTION WHILE LOADING LUT FROM FILE" << endl;
1461  cout << "Exception = " << e.what() << endl;
1462  return false;
1463  }
1464 }
float min_x
See CGasConcentrationGridMap2D::CGasConcentrationGridMap2D.
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:86
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
std::vector< TRandomFieldCell > m_map
The cells.
Definition: CDynamicGrid.h:42
std::string gasSensorLabel
The label of the CObservationGasSensor used to generate the map.
float sigma
The sigma of the "Parzen"-kernel Gaussian.
void getWindAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &windObj) const
Returns the 3D object representing the wind grid information.
double getYMax() const
Returns the "y" coordinate of bottom side of grid map.
Definition: CDynamicGrid.h:252
#define MRPT_START
Definition: exceptions.h:241
void fill(const T &value)
Fills all the cells with the same value.
Definition: CDynamicGrid.h:109
IMPLEMENTS_SERIALIZABLE(CGasConcentrationGridMap2D, CRandomFieldGridMap2D, mrpt::maps) CGasConcentrationGridMap2D
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
bool enableSaveAs3DObject
(Default=true) If false, calling CMetricMap::getAs3DObject() will have no effects ...
float KF_defaultCellMeanValue
The default value for the mean of cells&#39; concentration.
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:275
double getResolution() const
Returns the resolution of the grid map.
Definition: CDynamicGrid.h:254
uint16_t enose_id
id for the enose used to generate this map (must be < gasGrid_count)
This file implements several operations that operate element-wise on individual or pairs of container...
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:128
double getYMin() const
Returns the "y" coordinate of top side of grid map.
Definition: CDynamicGrid.h:250
A high-performance stopwatch, with typical resolution of nanoseconds.
mrpt::math::CMatrixD m_cov
The whole covariance matrix, used for the Kalman Filter map representation.
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:86
TMapRepresentation
The type of map representation to be used, see CRandomFieldGridMap2D for a discussion.
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map.
void idx2cxcy(int idx, int &cx, int &cy) const
Transform a global (linear) cell index value into its corresponding (x,y) cell indexes.
Definition: CDynamicGrid.h:271
STL namespace.
TMapGenericParams genericMapParams
Common params to all maps.
Definition: CMetricMap.h:274
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &section) override
This method load the options from a ".ini"-like file or memory-stored string list.
mrpt::maps::CGasConcentrationGridMap2D::TInsertionOptions insertionOpts
Observations insertion options.
float cutoffRadius
The cutoff radius for updating cells.
CGasConcentrationGridMap2D(TMapRepresentation mapType=mrAchim, float x_min=-2, float x_max=2, float y_min=-2, float y_max=2, float resolution=0.1f)
Constructor.
bool fileOpenCorrectly() const
Returns true if the file was open without errors.
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map (mean)
void recoverMeanAndCov() const
In the KF2 implementation, takes the auxiliary matrices and from them update the cells&#39; mean and std ...
TMapRepresentation m_mapType
The map representation type of this map, as passed in the constructor.
bool fileOpenCorrectly() const
Returns true if the file was open without errors.
ENUMTYPE read_enum(const std::string &section, const std::string &name, const ENUMTYPE &defaultValue, bool failIfNotFound=false) const
Reads an "enum" value, where the value in the config file can be either a numerical value or the symb...
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT&#39;s CStream, std::istream, std::ostream, std::stringstream.
Definition: CArchive.h:592
bool simulateAdvection(double STD_increase_value)
Implements the transition model of the gasConcentration map using the information of the wind maps...
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
float KF_observationModelNoise
The sensor model noise (in normalized concentration units).
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
float d2f(const double d)
shortcut for static_cast<float>(double)
float R_min
Limits for normalization of sensor readings.
This class allows loading and storing values and vectors of different types from a configuration text...
This base provides a set of functions for maths stuff.
double getXMin() const
Returns the "x" coordinate of left side of grid map.
Definition: CDynamicGrid.h:246
static Ptr Create(Args &&... args)
Definition: CArrow.h:30
math::TPose3D eNosePoseOnTheRobot
The pose of the sensors on the robot.
float advectionFreq
Indicates if wind information must be used.
void jet2rgb(const float color_index, float &r, float &g, float &b)
Computes the RGB color components (range [0,1]) for the corresponding color index in the range [0...
Definition: color_maps.cpp:142
#define ASSERT_EQUAL_(__A, __B)
Assert comparing two values, reporting their actual values upon failure.
Definition: exceptions.h:137
The contents of each cell in a CRandomFieldGridMap2D map.
A helper class that can convert an enum value into its textual representation, and viceversa...
uint16_t gasSensorType
The sensor type for the gas concentration map (0x0000 ->mean of all installed sensors, 0x2600, 0x6810, ...)
double internal_computeObservationLikelihood(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D &takenFrom) override
Internal method called by computeObservationLikelihood()
T * cellByPos(double x, double y)
Returns a pointer to the contents of a cell given by its coordinates, or nullptr if it is out of the ...
Definition: CDynamicGrid.h:201
This namespace contains representation of robot actions and observations.
#define M_PIf
Definition: common.h:61
string iniFile(myDataDir+string("benchmark-options.ini"))
#define IS_CLASS(obj, class_name)
True if the given reference to object (derived from mrpt::rtti::CObject) is of the given class...
Definition: CObject.h:146
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:143
T * cellByIndex(unsigned int cx, unsigned int cy)
Returns a pointer to the contents of a cell given by its cell indexes, or nullptr if it is out of the...
Definition: CDynamicGrid.h:222
Declares a class derived from "CObservation" that represents a set of readings from gas sensors...
size_t getSizeX() const
Returns the horizontal size of grid map in cells count.
Definition: CDynamicGrid.h:242
void dyngridcommon_readFromStream(STREAM &in, bool cast_from_float=false)
Definition: CDynamicGrid.h:342
std::vector< std::vector< std::vector< TGaussianCell > > > * table
mrpt::containers::CDynamicGrid< double > windGrid_module
Gridmaps of the wind Direction/Module.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
bool m_hasToRecoverMeanAndCov
Only for the KF2 implementation.
size_t getSizeY() const
Returns the vertical size of grid map in cells count.
Definition: CDynamicGrid.h:244
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:408
mrpt::system::TTimeStamp timeLastSimulated
The timestamp of the last time the advection simulation was executed.
return_t square(const num_t x)
Inline function for the square of a number.
bool convert(const sensor_msgs::LaserScan &msg, const mrpt::poses::CPose3D &pose, mrpt::obs::CObservation2DRangeScan &obj)
Definition: laser_scan.cpp:20
#define MRPT_LOAD_CONFIG_VAR( variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define MAP_DEFINITION_REGISTER(_CLASSNAME_STRINGS, _CLASSNAME_WITH_NS)
Registers one map class into TMetricMapInitializer factory.
CRandomFieldGridMap2D represents a 2D grid map where each cell is associated one real-valued property...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
void loadFromConfigFile_map_specific(const mrpt::config::CConfigFileBase &source, const std::string &sectionNamePrefix) override
Load all map-specific params.
float KF_covSigma
The "sigma" for the initial covariance value between cells (in meters).
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:39
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
mrpt::maps::CGasConcentrationGridMap2D::TMapRepresentation mapType
The kind of map representation (see CGasConcentrationGridMap2D::CGasConcentrationGridMap2D) ...
void internal_clear() override
Erase all the contents of the map.
#define MRPT_END
Definition: exceptions.h:245
void dumpToTextStream_map_specific(std::ostream &out) const override
bool build_Gaussian_Wind_Grid()
Builds a LookUp table with the values of the Gaussian Weights result of the wind advection for a spec...
uint16_t KF_W_size
[mrKalmanApproximate] The size of the window of neighbor cells.
double mean(const CONTAINER &v)
Computes the mean value of a vector.
virtual void increaseUncertainty(const double STD_increase_value)
Increase the kf_std of all cells from the m_map This mehod is usually called by the main_map to simul...
size_t ReadBuffer(void *Buffer, size_t Count)
Reads a block of bytes from the stream into Buffer.
Definition: CArchive.cpp:25
double getXMax() const
Returns the "x" coordinate of right side of grid map.
Definition: CDynamicGrid.h:248
mrpt::containers::CDynamicGrid< double > windGrid_direction
Transparently opens a compressed "gz" file and reads uncompressed data from it.
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:257
mrpt::math::CMatrixD m_stackedCov
The compressed band diagonal matrix for the KF2 implementation.
CGasConcentrationGridMap2D represents a PDF of gas concentrations over a 2D area. ...
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
#define LUT_TABLE
std::vector< float > readingsVoltage
The set of readings (in volts) from the array of sensors (size of "sensorTypes" is the same that the ...
Saves data to a file and transparently compress the data using the given compression level...
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:75
void dumpToTextStream(std::ostream &out) const override
This method should clearly display all the contents of the structure in textual form, sending it to a std::ostream.
static mrpt::maps::CMetricMap * internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer &def)
void clear()
Clear the contents of this container.
Definition: ts_hash_map.h:183
float KF_initialCellStd
The initial standard deviation of each cell&#39;s concentration (will be stored both at each cell&#39;s struc...
float std_windNoise_phi
Frequency for simulating advection (only used.
bool internal_insertObservation(const mrpt::obs::CObservation &obs, const mrpt::poses::CPose3D *robotPose=nullptr) override
Internal method called by insertObservation()
mrpt::maps::CGasConcentrationGridMap2D::TInsertionOptions insertionOptions
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
void insertIndividualReading(const double sensorReading, const mrpt::math::TPoint2D &point, const bool update_map=true, const bool time_invariant=true, const double reading_stddev=.0)
Direct update of the map with a reading in a given position of the map, using the appropriate method ...
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020