MRPT  1.9.9
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-2019, 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 
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 = 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 = 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  const CObservation& obs, const CPose3D& takenFrom)
285 {
286  MRPT_UNUSED_PARAM(obs);
287  MRPT_UNUSED_PARAM(takenFrom);
288 
289  THROW_EXCEPTION("Not implemented yet!");
290 }
291 
295 {
297 
298  // To assure compatibility: The size of each cell:
299  auto n = static_cast<uint32_t>(sizeof(TRandomFieldCell));
300  out << n;
301 
302  // Save the map contents:
303  n = static_cast<uint32_t>(m_map.size());
304  out << n;
305 
306 // Save the "m_map": This requires special handling for big endian systems:
307 #if MRPT_IS_BIG_ENDIAN
308  for (uint32_t i = 0; i < n; i++)
309  {
310  out << m_map[i].kf_mean << m_map[i].dm_mean << m_map[i].dmv_var_mean;
311  }
312 #else
313  // Little endian: just write all at once:
314  out.WriteBuffer(
315  &m_map[0],
316  sizeof(m_map[0]) * m_map.size()); // TODO: Do this endianness safe!!
317 #endif
318 
319  // Version 1: Save the insertion options:
320  out << uint8_t(m_mapType) << m_cov << m_stackedCov;
321 
328 
329  // New in v3:
332 
333  out << genericMapParams; // v4
334 }
335 
336 // Aux struct used below (must be at global scope for STL):
338 {
339  float mean, std;
340  float w, wr;
341 };
342 
344  mrpt::serialization::CArchive& in, uint8_t version)
345 {
346  switch (version)
347  {
348  case 0:
349  case 1:
350  case 2:
351  case 3:
352  case 4:
353  case 5:
354  {
355  dyngridcommon_readFromStream(in, version < 5);
356 
357  // To assure compatibility: The size of each cell:
358  uint32_t n;
359  in >> n;
360 
361  if (version < 2)
362  { // Converter from old versions <=1
363  ASSERT_(
364  n == static_cast<uint32_t>(sizeof(TOldCellTypeInVersion1)));
365  // Load the map contents in an aux struct:
366  in >> n;
367  vector<TOldCellTypeInVersion1> old_map(n);
368  in.ReadBuffer(&old_map[0], sizeof(old_map[0]) * old_map.size());
369 
370  // Convert to newer format:
371  m_map.resize(n);
372  for (size_t k = 0; k < n; k++)
373  {
374  m_map[k].kf_mean =
375  (old_map[k].w != 0) ? old_map[k].wr : old_map[k].mean;
376  m_map[k].kf_std =
377  (old_map[k].w != 0) ? old_map[k].w : old_map[k].std;
378  }
379  }
380  else
381  {
383  n, static_cast<uint32_t>(sizeof(TRandomFieldCell)));
384  // Load the map contents:
385  in >> n;
386  m_map.resize(n);
387 
388 // Read the note in writeToStream()
389 #if MRPT_IS_BIG_ENDIAN
390  for (uint32_t i = 0; i < n; i++)
391  in >> m_map[i].kf_mean >> m_map[i].dm_mean >>
392  m_map[i].dmv_var_mean;
393 #else
394  // Little endian: just read all at once:
395  in.ReadBuffer(&m_map[0], sizeof(m_map[0]) * m_map.size());
396 #endif
397  }
398 
399  // Version 1: Insertion options:
400  if (version >= 1)
401  {
402  uint8_t i;
403  in >> i;
405 
406  in >> m_cov >> m_stackedCov;
407 
415  }
416 
417  if (version >= 3)
418  {
419  uint64_t N;
423  }
424 
425  if (version >= 4) in >> genericMapParams;
426 
428  }
429  break;
430  default:
432  };
433 }
434 
435 /*---------------------------------------------------------------
436  TInsertionOptions
437  ---------------------------------------------------------------*/
439  :
440 
441  gasSensorLabel("MCEnose"),
442  // By default use the mean between all e-nose sensors
443  windSensorLabel("windSensor")
444 
445 {
446 }
447 
449  std::ostream& out) const
450 {
451  out << "\n----------- [CGasConcentrationGridMap2D::TInsertionOptions] "
452  "------------ \n\n";
453  out << "[TInsertionOptions.Common] ------------ \n\n";
454  internal_dumpToTextStream_common(
455  out); // Common params to all random fields maps:
456 
457  out << "[TInsertionOptions.GasSpecific] ------------ \n\n";
458  out << mrpt::format(
459  "gasSensorLabel = %s\n",
460  gasSensorLabel.c_str());
461  out << mrpt::format(
462  "enose_id = %u\n", (unsigned)enose_id);
463  out << mrpt::format(
464  "gasSensorType = %u\n",
465  (unsigned)gasSensorType);
466  out << mrpt::format(
467  "windSensorLabel = %s\n",
468  windSensorLabel.c_str());
469  out << mrpt::format(
470  "useWindInformation = %u\n", useWindInformation);
471 
472  out << mrpt::format(
473  "advectionFreq = %f\n", advectionFreq);
474  out << mrpt::format(
475  "default_wind_direction = %f\n",
476  default_wind_direction);
477  out << mrpt::format(
478  "default_wind_speed = %f\n", default_wind_speed);
479  out << mrpt::format(
480  "std_windNoise_phi = %f\n", std_windNoise_phi);
481  out << mrpt::format(
482  "std_windNoise_mod = %f\n", std_windNoise_mod);
483 
484  out << "\n";
485 }
486 
487 /*---------------------------------------------------------------
488  loadFromConfigFile
489  ---------------------------------------------------------------*/
491  const mrpt::config::CConfigFileBase& iniFile, const std::string& section)
492 {
493  // Common data fields for all random fields maps:
494  internal_loadFromConfigFile_common(iniFile, section);
495 
496  // Specific data fields for gasGridMaps
497  gasSensorLabel = iniFile.read_string(
498  section.c_str(), "gasSensorLabel", "Full_MCEnose", true);
499  enose_id = iniFile.read_int(section.c_str(), "enoseID", enose_id);
500  // Read sensor type in hexadecimal
501  {
502  std::string sensorType_str =
503  iniFile.read_string(section.c_str(), "gasSensorType", "-1", true);
504  int tmpSensorType;
505  stringstream convert(sensorType_str);
506  convert >> std::hex >> tmpSensorType;
507 
508  if (tmpSensorType >= 0)
509  {
510  // Valid number found:
511  gasSensorType = tmpSensorType;
512  }
513  else
514  { // fall back to old name, or default to current value:
515  gasSensorType = iniFile.read_int(
516  section.c_str(), "KF_sensorType", gasSensorType, true);
517  }
518  }
519  windSensorLabel = iniFile.read_string(
520  section.c_str(), "windSensorLabel", "Full_MCEnose", true);
521 
522  // Indicates if wind information must be used for Advection Simulation
523  useWindInformation =
524  iniFile.read_bool(section.c_str(), "useWindInformation", "false", true);
525 
526  //(rad) The initial/default value of the wind direction
527  default_wind_direction =
528  iniFile.read_float(section.c_str(), "default_wind_direction", 0, false);
529  //(m/s) The initial/default value of the wind speed
530  default_wind_speed =
531  iniFile.read_float(section.c_str(), "default_wind_speed", 0, false);
532 
533  //(rad) The noise in the wind direction
534  std_windNoise_phi =
535  iniFile.read_float(section.c_str(), "std_windNoise_phi", 0, false);
536  //(m/s) The noise in the wind strenght
537  std_windNoise_mod =
538  iniFile.read_float(section.c_str(), "std_windNoise_mod", 0, false);
539 
540  //(m/s) The noise in the wind strenght
541  advectionFreq =
542  iniFile.read_float(section.c_str(), "advectionFreq", 1, true);
543 }
544 
545 /*---------------------------------------------------------------
546  getAs3DObject
547 ---------------------------------------------------------------*/
549  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
550 {
551  MRPT_START
554  MRPT_END
555 }
556 
557 /*---------------------------------------------------------------
558  getAs3DObject
559 ---------------------------------------------------------------*/
562  mrpt::opengl::CSetOfObjects::Ptr& varObj) const
563 {
564  MRPT_START
566  CRandomFieldGridMap2D::getAs3DObject(meanObj, varObj);
567  MRPT_END
568 }
569 
570 /*---------------------------------------------------------------
571  getWindAs3DObject
572 ---------------------------------------------------------------*/
574  mrpt::opengl::CSetOfObjects::Ptr& windObj) const
575 {
576  // Return an arrow map of the wind state (module(color) and direction).
577  float scale = 0.2f;
578  size_t arrow_separation =
579  5; // distance between arrows, expresed as times the cell resolution
580 
581  // map limits
582  float x_min = getXMin();
583  float x_max = getXMax();
584  float y_min = getYMin();
585  float y_max = getYMax();
586  float resol = getResolution();
587 
588  // Ensure map dimensions match with wind map
589  unsigned int wind_map_size =
591  ASSERT_(
592  wind_map_size ==
594  if (m_map.size() != wind_map_size)
595  {
596  cout << " GAS MAP DIMENSIONS DO NOT MATCH WIND MAP " << endl;
597  // mrpt::system::pause();
598  }
599 
600  size_t cx, cy;
601  vector<float> xs, ys;
602 
603  // xs: array of X-axis values
604  xs.resize(floor((x_max - x_min) / (arrow_separation * resol)));
605  for (cx = 0; cx < xs.size(); cx++)
606  xs[cx] = x_min + arrow_separation * resol * cx;
607 
608  // ys: array of X-axis values
609  ys.resize(floor((y_max - y_min) / (arrow_separation * resol)));
610  for (cy = 0; cy < ys.size(); cy++)
611  ys[cy] = y_min + arrow_separation * resol * cy;
612 
613  for (cy = 0; cy < ys.size(); cy++)
614  {
615  for (cx = 0; cx < xs.size(); cx++)
616  {
617  // Cell values [0,inf]:
618  double dir_xy = *windGrid_direction.cellByPos(xs[cx], ys[cy]);
619  double mod_xy = *windGrid_module.cellByPos(xs[cx], ys[cy]);
620 
622  xs[cx], ys[cy], 0.f, xs[cx] + scale * (float)cos(dir_xy),
623  ys[cy] + scale * (float)sin(dir_xy), 0.f, 1.15f * scale,
624  0.3f * scale, 0.35f * scale);
625 
626  float r, g, b;
627  jet2rgb(mod_xy, r, g, b);
628  obj->setColor(r, g, b);
629 
630  windObj->insert(obj);
631  }
632  }
633 }
634 
635 /*---------------------------------------------------------------
636  increaseUncertainty
637 ---------------------------------------------------------------*/
639  const double STD_increase_value)
640 {
641  // Increase cell variance
642  // unsigned int cx,cy;
643  // double memory_retention;
644 
646  for (size_t it = 0; it < m_map.size(); it++)
647  {
648  m_stackedCov(it, 0) = m_stackedCov(it, 0) + STD_increase_value;
649  }
650 
651  // Update m_map.kf_std
653 
654  // for (cy=0; cy<m_size_y; cy++)
655  // {
656  // for (cx=0; cx<m_size_x; cx++)
657  // {
658  // // Forgetting_curve --> memory_retention =
659  // exp(-time/memory_relative_strenght)
660  // memory_retention = exp(- mrpt::system::timeDifference(m_map[cx +
661  // cy*m_size_x].last_updated, now()) / memory_relative_strenght);
662  // //Update Uncertainty (STD)
663  // m_map[cx + cy*m_size_x].kf_std = 1 - ( (1-m_map[cx +
664  // cy*m_size_x].updated_std) * memory_retention );
665  // }
666  // }
667 }
668 
669 /*---------------------------------------------------------------
670  simulateAdvection
671 ---------------------------------------------------------------*/
673  const double& STD_increase_value)
674 {
675  /* 1- Ensure we can use Wind Information
676  -------------------------------------------------*/
677  if (!insertionOptions.useWindInformation) return false;
678 
679  // Get time since last simulation
680  double At =
682  cout << endl << " - At since last simulation = " << At << "seconds" << endl;
683  // update time of last updated.
685 
686  /* 3- Build Transition Matrix (SA)
687  This Matrix contains the probabilities of each cell
688  to "be displaced" to other cells by the wind effect.
689  ------------------------------------------------------*/
690  mrpt::system::CTicTac tictac;
691  size_t i = 0, c = 0;
692  int cell_i_cx, cell_i_cy;
693  float mu_phi, mu_r, mu_modwind;
694  const size_t N = m_map.size();
695  mrpt::math::CMatrixF A(N, N);
696  A.fill(0.0);
697  // std::vector<double> row_sum(N,0.0);
698  auto* row_sum = (double*)calloc(N, sizeof(double));
699 
700  try
701  {
702  // Ensure map dimensions match with wind map
703  unsigned int wind_map_size =
705  ASSERT_(
706  wind_map_size ==
708  if (N != wind_map_size)
709  {
710  cout << " GAS MAP DIMENSIONS DO NOT MATCH WIND INFORMATION "
711  << endl;
712  // mrpt::system::pause();
713  }
714 
715  tictac.Tic();
716 
717  // Generate Sparse Matrix of the wind advection SA
718  for (i = 0; i < N; i++)
719  {
720  // Cell_i indx and coordinates
721  idx2cxcy(i, cell_i_cx, cell_i_cy);
722 
723  // Read dirwind value of cell i
725  cell_i_cx, cell_i_cy); //[0,2*pi]
726  unsigned int phi_indx = round(mu_phi / LUT.phi_inc);
727 
728  // Read modwind value of cell i
729  mu_modwind =
730  *windGrid_module.cellByIndex(cell_i_cx, cell_i_cy); //[0,inf)
731  mu_r = mu_modwind * At;
732  if (mu_r > LUT.max_r) mu_r = LUT.max_r;
733  unsigned int r_indx = round(mu_r / LUT.r_inc);
734 
735  // Evaluate LUT
736  ASSERT_(phi_indx < LUT.phi_count);
737  ASSERT_(r_indx < LUT.r_count);
738 
739  // define label
740  vector<TGaussianCell>& cells_to_update =
741  LUT_TABLE[phi_indx][r_indx];
742 
743  // Generate Sparse Matrix with the wind weights "SA"
744  for (auto& ci : cells_to_update)
745  {
746  int final_cx = cell_i_cx + ci.cx;
747  int final_cy = cell_i_cy + ci.cy;
748  // Check if affected cells is within the map
749  if ((final_cx >= 0) && (final_cx < (int)getSizeX()) &&
750  (final_cy >= 0) && (final_cy < (int)getSizeY()))
751  {
752  int final_idx = final_cx + final_cy * getSizeX();
753 
754  // Add Value to SA Matrix
755  if (ci.value != 0.0)
756  {
757  A(final_idx, i) = ci.value;
758  row_sum[final_idx] += ci.value;
759  }
760  }
761  } // end-for ci
762  } // end-for cell i
763 
764  cout << " - SA matrix computed in " << tictac.Tac() << "s" << endl
765  << endl;
766  }
767  catch (const std::exception& e)
768  {
769  cout << " ######### EXCEPTION computing Transition Matrix (A) "
770  "##########\n: "
771  << e.what() << endl;
772  cout << "on cell i= " << i << " c=" << c << endl << endl;
773  return false;
774  }
775 
776  /* Update Mean + Variance as a Gaussian Mixture
777  ------------------------------------------------*/
778  try
779  {
780  tictac.Tic();
781  // std::vector<double> new_means(N,0.0);
782  auto* new_means = (double*)calloc(N, sizeof(double));
783  // std::vector<double> new_variances(N,0.0);
784  auto* new_variances = (double*)calloc(N, sizeof(double));
785 
786  for (size_t it_i = 0; it_i < N; it_i++)
787  {
788  //--------
789  // mean
790  //--------
791  for (size_t it_j = 0; it_j < N; it_j++)
792  {
793  if (m_map[it_j].kf_mean != 0 && A(it_i, it_j) != 0)
794  {
795  if (row_sum[it_i] >= 1)
796  new_means[it_i] += (A(it_i, it_j) / row_sum[it_i]) *
797  m_map[it_j].kf_mean;
798  else
799  new_means[it_i] += A(it_i, it_j) * m_map[it_j].kf_mean;
800  }
801  }
802 
803  //----------
804  // variance
805  //----------
806  // Consider special case (borders cells)
807  if (row_sum[it_i] < 1)
808  new_variances[it_i] =
809  (1 - row_sum[it_i]) *
811 
812  for (size_t it_j = 0; it_j < N; it_j++)
813  {
814  if (A(it_i, it_j) != 0)
815  {
816  if (row_sum[it_i] >= 1)
817  new_variances[it_i] +=
818  (A(it_i, it_j) / row_sum[it_i]) *
819  (m_stackedCov(it_j, 0) +
820  square(m_map[it_j].kf_mean - new_means[it_i]));
821  else
822  new_variances[it_i] +=
823  A(it_i, it_j) *
824  (m_stackedCov(it_j, 0) +
825  square(m_map[it_j].kf_mean - new_means[it_i]));
826  }
827  }
828  }
829 
830  // Update means and Cov of the Kalman filter state
831  for (size_t it_i = 0; it_i < N; it_i++)
832  {
833  m_map[it_i].kf_mean = new_means[it_i]; // means
834 
835  // Variances
836  // Scale the Current Covariances with the new variances
837  for (size_t it_j = 0; it_j < N; it_j++)
838  {
839  m_stackedCov(it_i, it_j) =
840  (m_stackedCov(it_i, it_j) / m_stackedCov(it_i, it_i)) *
841  new_variances[it_i]; // variances
842  m_stackedCov(it_j, it_i) = m_stackedCov(it_i, it_j);
843  }
844  }
847 
848  cout << " - Mean&Var updated in " << tictac.Tac() << "s" << endl;
849 
850  // Free Memory
851  free(row_sum);
852  free(new_means);
853  free(new_variances);
854  }
855  catch (const std::exception& e)
856  {
857  cout << " ######### EXCEPTION Updating Covariances ##########\n: "
858  << e.what() << endl;
859  cout << "on row i= " << i << " column c=" << c << endl << endl;
860  return false;
861  }
862 
863  // cout << " Increasing general STD..." << endl;
864  increaseUncertainty(STD_increase_value);
865 
866  return true;
867 }
868 
869 /*---------------------------------------------------------------
870  build_Gaussian_Wind_Grid
871 ---------------------------------------------------------------*/
872 
874 /** Builds a LookUp table with the values of the Gaussian Weights result of the
875 wind advection
876 * for a specific condition.
877 *
878 * The LUT contains the values of the Gaussian Weigths and the references to
879 the cell indexes to be applied.
880 * Since the LUT is independent of the wind direction and angle, it generates
881 the Gaussian Weights for different configurations
882 * of wind angle and module values.
883 *
884 * To increase precission, each cell of the grid is sub-divided in subcells of
885 smaller size.
886 
887 * cell_i --> Cell origin (We consider our reference system in the bottom left
888 corner of cell_i ).
889  Is the cell that contains the gas measurement which will be
890 propagated by the wind.
891  The wind propagates in the shape of a 2D Gaussian with center in
892 the target cell (cell_j)
893 * cell_j --> Target cell. Is the cell where falls the center of the Gaussian
894 that models the propagation of the gas comming from cell_i.
895 */
896 
897 {
898  cout << endl << "---------------------------------" << endl;
899  cout << " BUILDING GAUSSIAN WIND WEIGHTS " << endl;
900  cout << "---------------------------------" << endl << endl;
901 
902  //-----------------------------
903  // PARAMS
904  //-----------------------------
905  LUT.resolution = getResolution(); // resolution of the grid-cells (m)
906  LUT.std_phi =
908  .std_windNoise_phi; // Standard Deviation in wind Angle (cte)
911  .advectionFreq; // Standard Deviation in wind module (cte)
912  std::string filename = format(
913  "Gaussian_Wind_Weights_res(%f)_stdPhi(%f)_stdR(%f).gz", LUT.resolution,
914  LUT.std_phi, LUT.std_r);
915 
916  // Fixed Params:
917  LUT.phi_inc = M_PIf / 8; // Increment in the wind Angle. (rad)
918  LUT.phi_count =
919  round(2 * M_PI / LUT.phi_inc) + 1; // Number of angles to generate
920  LUT.r_inc = 0.1f; // Increment in the wind Module. (m)
921  LUT.max_r = 2; // maximum distance (m) to simulate
922  LUT.r_count =
923  round(LUT.max_r / LUT.r_inc) + 1; // Number of wind modules to simulate
924 
925  LUT.table = new vector<vector<vector<TGaussianCell>>>(
926  LUT.phi_count,
927  vector<vector<TGaussianCell>>(LUT.r_count, vector<TGaussianCell>()));
928 
929  // LUT.table = new
930  // vector<vector<vector<vector<vector<TGaussianCell>>>>>(LUT.subcell_count,
931  // vector<vector<vector<vector<TGaussianCell>>>>(LUT.subcell_count,
932  // vector<vector<vector<TGaussianCell>>>(LUT.phi_count,
933  // vector<vector<TGaussianCell>>(LUT.r_count,vector<TGaussianCell>()) ) ) );
934 
935  //-----------------------------
936  // Check if file exists
937  //-----------------------------
938 
939  cout << "Looking for file: " << filename.c_str() << endl;
940 
941  if (mrpt::system::fileExists(filename.c_str()))
942  {
943  // file exists. Load lookUptable from file
944  cout << "LookUp table found for this configuration. Loading..." << endl;
946  }
947  else
948  {
949  // file does not exists. Generate LookUp table.
950  cout << "LookUp table NOT found. Generating table..." << endl;
951 
952  bool debug = true;
953  FILE* debug_file;
954 
955  if (debug)
956  {
957  debug_file = fopen("simple_LUT.txt", "w");
958  fprintf(
959  debug_file, " phi_inc = %.4f \n r_inc = %.4f \n", LUT.phi_inc,
960  LUT.r_inc);
961  fprintf(
962  debug_file, " std_phi = %.4f \n std_r = %.4f \n", LUT.std_phi,
963  LUT.std_r);
964  fprintf(debug_file, "[ phi ] [ r ] ---> (cx,cy)=Value\n");
965  fprintf(debug_file, "----------------------------------\n");
966  }
967 
968  // For the different possible angles (phi)
969  for (size_t phi_indx = 0; phi_indx < LUT.phi_count; phi_indx++)
970  {
971  // mean of the phi value
972  float phi = phi_indx * LUT.phi_inc;
973 
974  // For the different and possibe wind modules (r)
975  for (size_t r_indx = 0; r_indx < LUT.r_count; r_indx++)
976  {
977  // mean of the radius value
978  float r = r_indx * LUT.r_inc;
979 
980  if (debug)
981  {
982  fprintf(debug_file, "\n[%.2f] [%.2f] ---> ", phi, r);
983  }
984 
985  // Estimates Cell_i_position
986  // unsigned int cell_i_cx = 0;
987  // unsigned int cell_i_cy = 0;
988  float cell_i_x = LUT.resolution / 2.0;
989  float cell_i_y = LUT.resolution / 2.0;
990 
991  // Estimate target position according to the mean value of wind.
992  // float x_final = cell_i_x + r*cos(phi);
993  // float y_final = cell_i_y + r*sin(phi);
994 
995  // Determine cell_j coordinates respect to origin_cell
996  // int cell_j_cx = static_cast<int>(floor(
997  // (x_final)/LUT.resolution ));
998  // int cell_j_cy = static_cast<int>(floor(
999  // (y_final)/LUT.resolution ));
1000  // Center of cell_j
1001  // float cell_j_x = (cell_j_cx+0.5f)*LUT.resolution;
1002  // float cell_j_y = (cell_j_cy+0.5f)*LUT.resolution;
1003  // left bottom corner of cell_j
1004  // float cell_j_xmin = cell_j_x - LUT.resolution/2.0;
1005  // float cell_j_ymin = cell_j_y - LUT.resolution/2.0;
1006 
1007  /* ---------------------------------------------------------------------------------
1008  Generate bounding-box (+/- 3std) to determine which cells
1009  to update
1010  ---------------------------------------------------------------------------------*/
1011  std::vector<double> vertex_x, vertex_y;
1012  vertex_x.resize(14);
1013  vertex_y.resize(14);
1014  // Bounding-Box initialization
1015  double minBBox_x = 1000;
1016  double maxBBox_x = -1000;
1017  double minBBox_y = 1000;
1018  double maxBBox_y = -1000;
1019 
1020  // Consider special case for high uncertainty in PHI. The shape
1021  // of the polygon is a donut.
1022  double std_phi_BBox = LUT.std_phi;
1023  if (std_phi_BBox > M_PI / 3)
1024  {
1025  std_phi_BBox = M_PI / 3; // To avoid problems generating
1026  // the bounding box. For std>pi/3
1027  // the shape is always a donut.
1028  }
1029 
1030  // Calculate bounding box limits
1031  size_t indx = 0;
1032  int sr = 3;
1033  for (int sd = (-3); sd <= (3); sd++)
1034  {
1035  vertex_x[indx] =
1036  cell_i_x +
1037  (r + sr * LUT.std_r) * cos(phi + sd * std_phi_BBox);
1038  if (vertex_x[indx] < minBBox_x) minBBox_x = vertex_x[indx];
1039  if (vertex_x[indx] > maxBBox_x) maxBBox_x = vertex_x[indx];
1040 
1041  vertex_y[indx] =
1042  cell_i_y +
1043  (r + sr * LUT.std_r) * sin(phi + sd * std_phi_BBox);
1044  if (vertex_y[indx] < minBBox_y) minBBox_y = vertex_y[indx];
1045  if (vertex_y[indx] > maxBBox_y) maxBBox_y = vertex_y[indx];
1046 
1047  indx++;
1048  }
1049  sr = -3;
1050  for (int sd = (3); sd >= (-3); sd--)
1051  {
1052  vertex_x[indx] =
1053  cell_i_x +
1054  (r + sr * LUT.std_r) * cos(phi + sd * std_phi_BBox);
1055  if (vertex_x[indx] < minBBox_x) minBBox_x = vertex_x[indx];
1056  if (vertex_x[indx] > maxBBox_x) maxBBox_x = vertex_x[indx];
1057 
1058  vertex_y[indx] =
1059  cell_i_y +
1060  (r + sr * LUT.std_r) * sin(phi + sd * std_phi_BBox);
1061  if (vertex_y[indx] < minBBox_y) minBBox_y = vertex_y[indx];
1062  if (vertex_y[indx] > maxBBox_y) maxBBox_y = vertex_y[indx];
1063 
1064  indx++;
1065  }
1066 
1067  /* ------------------------------------------------------------------------
1068  Determine range of cells to update according to the
1069  Bounding-Box limits.
1070  Origin cell is cx=cy= 0 x[0,res), y[0,res)
1071  ---------------------------------------------------------------------------*/
1072  int min_cx =
1073  static_cast<int>(floor(minBBox_x / LUT.resolution));
1074  int max_cx =
1075  static_cast<int>(floor(maxBBox_x / LUT.resolution));
1076  int min_cy =
1077  static_cast<int>(floor(minBBox_y / LUT.resolution));
1078  int max_cy =
1079  static_cast<int>(floor(maxBBox_y / LUT.resolution));
1080 
1081  int num_cells_affected =
1082  (max_cx - min_cx + 1) * (max_cy - min_cy + 1);
1083 
1084  if (num_cells_affected == 1)
1085  {
1086  // Concentration of cell_i moves to cell_a (cx,cy)
1087  TGaussianCell gauss_info;
1088  gauss_info.cx = min_cx; // since max_cx == min_cx
1089  gauss_info.cy = min_cy;
1090  gauss_info.value = 1; // prob = 1
1091 
1092  // Add cell volume to LookUp Table
1093  LUT_TABLE[phi_indx][r_indx].push_back(gauss_info);
1094 
1095  if (debug)
1096  {
1097  // Save to file (debug)
1098  fprintf(
1099  debug_file, "(%d,%d)=%.4f", gauss_info.cx,
1100  gauss_info.cy, gauss_info.value);
1101  }
1102  }
1103  else
1104  {
1105  // Estimate volume of the Gaussian under each affected cell
1106 
1107  float subcell_pres = LUT.resolution / 10;
1108  // Determine the number of subcells inside the Bounding-Box
1109  const int BB_x_subcells =
1110  (int)(floor((maxBBox_x - minBBox_x) / subcell_pres) + 1);
1111  const int BB_y_subcells =
1112  (int)(floor((maxBBox_y - minBBox_y) / subcell_pres) + 1);
1113 
1114  double subcell_pres_x =
1115  (maxBBox_x - minBBox_x) / BB_x_subcells;
1116  double subcell_pres_y =
1117  (maxBBox_y - minBBox_y) / BB_y_subcells;
1118 
1119  // Save the W value of each cell using a map
1120  std::map<std::pair<int, int>, float> w_values;
1121  std::map<std::pair<int, int>, float>::iterator it;
1122  float sum_w = 0;
1123 
1124  for (int scy = 0; scy < BB_y_subcells; scy++)
1125  {
1126  for (int scx = 0; scx < BB_x_subcells; scx++)
1127  {
1128  // P-Subcell coordinates (center of the p-subcell)
1129  float subcell_a_x =
1130  minBBox_x + (scx + 0.5f) * subcell_pres_x;
1131  float subcell_a_y =
1132  minBBox_y + (scy + 0.5f) * subcell_pres_y;
1133 
1134  // distance and angle between cell_i and subcell_a
1135  float r_ia = sqrt(
1136  square(subcell_a_x - cell_i_x) +
1137  square(subcell_a_y - cell_i_y));
1138  float phi_ia = atan2(
1139  subcell_a_y - cell_i_y, subcell_a_x - cell_i_x);
1140 
1141  // Volume Approximation of subcell_a (Gaussian
1142  // Bivariate)
1143  float w =
1144  (1 / (2 * M_PI * LUT.std_r * LUT.std_phi)) *
1145  exp(-0.5 *
1146  (square(r_ia - r) / square(LUT.std_r) +
1147  square(phi_ia - phi) /
1148  square(LUT.std_phi)));
1149  w += (1 / (2 * M_PI * LUT.std_r * LUT.std_phi)) *
1150  exp(-0.5 *
1151  (square(r_ia - r) / square(LUT.std_r) +
1152  square(phi_ia + 2 * M_PI - phi) /
1153  square(LUT.std_phi)));
1154  w += (1 / (2 * M_PI * LUT.std_r * LUT.std_phi)) *
1155  exp(-0.5 *
1156  (square(r_ia - r) / square(LUT.std_r) +
1157  square(phi_ia - 2 * M_PI - phi) /
1158  square(LUT.std_phi)));
1159 
1160  // Since we work with a cell grid, approximate the
1161  // weight of the gaussian by the volume of the
1162  // subcell_a
1163  if (r_ia != 0.0)
1164  w =
1165  (w * (subcell_pres_x * subcell_pres_y) /
1166  r_ia);
1167 
1168  // Determine cell index of the current subcell
1169  int cell_cx = static_cast<int>(
1170  floor(subcell_a_x / LUT.resolution));
1171  int cell_cy = static_cast<int>(
1172  floor(subcell_a_y / LUT.resolution));
1173 
1174  // Save w value
1175  it =
1176  w_values.find(std::make_pair(cell_cx, cell_cy));
1177  if (it != w_values.end()) // already exists
1178  w_values[std::make_pair(cell_cx, cell_cy)] += w;
1179  else
1180  w_values[std::make_pair(cell_cx, cell_cy)] = w;
1181 
1182  sum_w = sum_w + w;
1183  } // end-for scx
1184  } // end-for scy
1185 
1186  // SAVE to LUT
1187  for (it = w_values.begin(); it != w_values.end(); it++)
1188  {
1189  float w_final =
1190  (it->second) / sum_w; // normalization to 1
1191 
1192  if (w_final >= 0.001)
1193  {
1194  // Save the weight of the gaussian volume for cell_a
1195  // (cx,cy)
1196  TGaussianCell gauss_info;
1197  gauss_info.cx = it->first.first;
1198  gauss_info.cy = it->first.second;
1199  gauss_info.value = w_final;
1200 
1201  // Add cell volume to LookUp Table
1202  LUT_TABLE[phi_indx][r_indx].push_back(gauss_info);
1203 
1204  if (debug)
1205  {
1206  // Save to file (debug)
1207  fprintf(
1208  debug_file, "(%d,%d)=%.6f ",
1209  gauss_info.cx, gauss_info.cy,
1210  gauss_info.value);
1211  }
1212  }
1213  }
1214 
1215  // OLD WAY
1216 
1217  /* ---------------------------------------------------------
1218  Estimate the volume of the Gaussian on each affected cell
1219  //-----------------------------------------------------------*/
1220  // for(int cx=min_cx; cx<=max_cx; cx++)
1221  //{
1222  // for(int cy=min_cy; cy<=max_cy; cy++)
1223  // {
1224  // // Coordinates of affected cell (center of the cell)
1225  // float cell_a_x = (cx+0.5f)*LUT.resolution;
1226  // float cell_a_y = (cy+0.5f)*LUT.resolution;
1227  // float w_cell_a = 0.0; //initial Gaussian value of
1228  // cell afected
1229 
1230  // // Estimate volume of the Gaussian under cell (a)
1231  // // Partition each cell into (p x p) subcells and
1232  // evaluate the gaussian.
1233  // int p = 40;
1234  // float subcell_pres = LUT.resolution/p;
1235  // float cell_a_x_min = cell_a_x - LUT.resolution/2.0;
1236  // float cell_a_y_min = cell_a_y - LUT.resolution/2.0;
1237 
1238  //
1239  // for(int scy=0; scy<p; scy++)
1240  // {
1241  // for(int scx=0; scx<p; scx++)
1242  // {
1243  // //P-Subcell coordinates (center of the
1244  // p-subcell)
1245  // float subcell_a_x = cell_a_x_min +
1246  //(scx+0.5f)*subcell_pres;
1247  // float subcell_a_y = cell_a_y_min +
1248  //(scy+0.5f)*subcell_pres;
1249 
1250  // //distance and angle between cell_i and
1251  // subcell_a
1252  // float r_ia = sqrt(
1253  // square(subcell_a_x-cell_i_x)
1254  //+
1255  // square(subcell_a_y-cell_i_y) );
1256  // float phi_ia = atan2(subcell_a_y-cell_i_y,
1257  // subcell_a_x-cell_i_x);
1258 
1259  // //Volume Approximation of subcell_a
1260  //(Gaussian
1261  // Bivariate)
1262  // float w = (1/(2*M_PI*LUT.std_r*LUT.std_phi))
1263  //*
1264  // exp(-0.5*( square(r_ia-r)/square(LUT.std_r) +
1265  // square(phi_ia-phi)/square(LUT.std_phi) ) );
1266  // w += (1/(2*M_PI*LUT.std_r*LUT.std_phi)) *
1267  // exp(-0.5*( square(r_ia-r)/square(LUT.std_r) +
1268  // square(phi_ia+2*M_PI-phi)/square(LUT.std_phi) ) );
1269  // w += (1/(2*M_PI*LUT.std_r*LUT.std_phi)) *
1270  // exp(-0.5*( square(r_ia-r)/square(LUT.std_r) +
1271  // square(phi_ia-2*M_PI-phi)/square(LUT.std_phi) ) );
1272  //
1273  // //Since we work with a cell grid,
1274  // approximate
1275  // the
1276  // weight of the gaussian by the volume of the subcell_a
1277  // if (r_ia != 0.0)
1278  // w_cell_a = w_cell_a + (w *
1279  // square(subcell_pres)/r_ia);
1280  // }//end-for scx
1281  // }//end-for scy
1282 
1283  // //Save the weight of the gaussian volume for cell_a
1284  //(cx,cy)
1285  // TGaussianCell gauss_info;
1286  // gauss_info.cx = cx;
1287  // gauss_info.cy = cy;
1288  // gauss_info.value = w_cell_a;
1289 
1290  // //Add cell volume to LookUp Table
1291  // LUT_TABLE[phi_indx][r_indx].push_back(gauss_info);
1292 
1293  // if (debug)
1294  // {
1295  // //Save to file (debug)
1296  // fprintf(debug_file, "(%d,%d)=%.6f
1297  //",gauss_info.cx, gauss_info.cy, gauss_info.value);
1298  // }
1299  //
1300  //
1301  // }//end-for cy
1302  //}//end-for cx
1303 
1304  } // end-if only one affected cell
1305 
1306  } // end-for r
1307  } // end-for phi
1308 
1309  if (debug) fclose(debug_file);
1310 
1311  // Save LUT to File
1313 
1314  } // end-if table not available
1315 }
1316 
1318 {
1319  // Save LUT to file
1320  cout << "Saving to File ....";
1321 
1323  "Gaussian_Wind_Weights_res(%f)_stdPhi(%f)_stdR(%f).gz", LUT.resolution,
1324  LUT.std_phi, LUT.std_r));
1325  if (!fo.fileOpenCorrectly())
1326  {
1327  return false;
1328  cout << "WARNING: Gaussian_Wind_Weights file NOT SAVED" << endl;
1329  }
1330  auto f = mrpt::serialization::archiveFrom(fo);
1331 
1332  try
1333  {
1334  // Save params first
1335  f << LUT.resolution; // cell resolution used
1336  f << LUT.std_phi; // std_phi used
1337  f << LUT.std_r;
1338 
1339  f << LUT.phi_inc; // rad
1340  f << (float)LUT.phi_count;
1341  f << LUT.r_inc; // m
1342  f << LUT.max_r; // maximum distance (m)
1343  f << (float)LUT.r_count;
1344 
1345  // Save Multi-table
1346  // vector< vector< vector<TGaussianCell>>>>> *table;
1347 
1348  for (size_t phi_indx = 0; phi_indx < LUT.phi_count; phi_indx++)
1349  {
1350  for (size_t r_indx = 0; r_indx < LUT.r_count; r_indx++)
1351  {
1352  // save all cell values.
1353  size_t N = LUT_TABLE[phi_indx][r_indx].size();
1354  f << (float)N;
1355 
1356  for (size_t i = 0; i < N; i++)
1357  {
1358  f << (float)LUT_TABLE[phi_indx][r_indx][i].cx;
1359  f << (float)LUT_TABLE[phi_indx][r_indx][i].cy;
1360  f << LUT_TABLE[phi_indx][r_indx][i].value;
1361  }
1362  }
1363  }
1364  cout << "DONE" << endl;
1365  return true;
1366  }
1367  catch (const std::exception& e)
1368  {
1369  cout << endl
1370  << "------------------------------------------------------------"
1371  << endl;
1372  cout << "EXCEPTION WHILE SAVING LUT TO FILE" << endl;
1373  cout << "Exception = " << e.what() << endl;
1374  return false;
1375  }
1376 }
1377 
1379 {
1380  // LOAD LUT from file
1381  cout << "Loading from File ....";
1382 
1383  try
1384  {
1386  "Gaussian_Wind_Weights_res(%f)_stdPhi(%f)_stdR(%f).gz",
1388  if (!fi.fileOpenCorrectly())
1389  {
1390  cout << "WARNING WHILE READING FROM: Gaussian_Wind_Weights" << endl;
1391  return false;
1392  }
1393  auto f = mrpt::serialization::archiveFrom(fi);
1394 
1395  float t_float;
1396  unsigned int t_uint;
1397  // Ensure params from file are correct with the specified in the ini
1398  // file
1399  f >> t_float;
1400  ASSERT_(LUT.resolution == t_float);
1401 
1402  f >> t_float;
1403  ASSERT_(LUT.std_phi == t_float);
1404 
1405  f >> t_float;
1406  ASSERT_(LUT.std_r == t_float);
1407 
1408  f >> t_float;
1409  ASSERT_(LUT.phi_inc == t_float);
1410 
1411  f >> t_float;
1412  t_uint = (unsigned int)t_float;
1413  ASSERT_(LUT.phi_count == t_uint);
1414 
1415  f >> t_float;
1416  ASSERT_(LUT.r_inc == t_float);
1417 
1418  f >> t_float;
1419  ASSERT_(LUT.max_r == t_float);
1420 
1421  f >> t_float;
1422  t_uint = (unsigned int)t_float;
1423  ASSERT_(LUT.r_count == t_uint);
1424 
1425  // Load Multi-table
1426  // vector< vector< vector<TGaussianCell>>>>> *table;
1427 
1428  for (size_t phi_indx = 0; phi_indx < LUT.phi_count; phi_indx++)
1429  {
1430  for (size_t r_indx = 0; r_indx < LUT.r_count; r_indx++)
1431  {
1432  // Number of cells to update
1433  size_t N;
1434  f >> t_float;
1435  N = (size_t)t_float;
1436 
1437  for (size_t i = 0; i < N; i++)
1438  {
1439  TGaussianCell gauss_info;
1440  f >> t_float;
1441  gauss_info.cx = (int)t_float;
1442 
1443  f >> t_float;
1444  gauss_info.cy = (int)t_float;
1445 
1446  f >> gauss_info.value;
1447 
1448  // Add cell volume to LookUp Table
1449  LUT_TABLE[phi_indx][r_indx].push_back(gauss_info);
1450  }
1451  }
1452  }
1453  cout << "DONE" << endl;
1454  return true;
1455  }
1456  catch (const std::exception& e)
1457  {
1458  cout << endl
1459  << "------------------------------------------------------------"
1460  << endl;
1461  cout << "EXCEPTION WHILE LOADING LUT FROM FILE" << endl;
1462  cout << "Exception = " << e.what() << endl;
1463  return false;
1464  }
1465 }
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
GLenum GLenum GLenum GLenum GLenum scale
Definition: glext.h:6604
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)
GLenum GLsizei n
Definition: glext.h:5136
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.
STL namespace.
TMapGenericParams genericMapParams
Common params to all maps.
Definition: CMetricMap.h:275
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.
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
bool fileOpenCorrectly() const
Returns true if the file was open without errors.
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4199
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.
#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:591
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 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
CGasConcentrationGridMap2D(TMapRepresentation mapType=mrAchim, float x_min=-2, float x_max=2, float y_min=-2, float y_max=2, float resolution=0.1)
Constructor.
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
T square(const T x)
Inline function for the square of a number.
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, ...)
const GLubyte * c
Definition: glext.h:6406
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
bool simulateAdvection(const double &STD_increase_value)
Implements the transition model of the gasConcentration map using the information of the wind maps...
GLubyte g
Definition: glext.h:6372
This namespace contains representation of robot actions and observations.
#define M_PIf
Definition: common.h:61
string iniFile(myDataDir+string("benchmark-options.ini"))
GLubyte GLubyte b
Definition: glext.h:6372
#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
GLsizei const GLchar ** string
Definition: glext.h:4116
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:410
mrpt::system::TTimeStamp timeLastSimulated
The timestamp of the last time the advection simulation was executed.
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
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
void loadFromConfigFile_map_specific(const mrpt::config::CConfigFileBase &source, const std::string &sectionNamePrefix) override
Load all map-specific params.
unsigned __int64 uint64_t
Definition: glext.h:3457
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
GLuint in
Definition: glext.h:7391
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.
GLsizei GLsizei GLchar * source
Definition: glext.h:4097
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...
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 ...
void idx2cxcy(const 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
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
Lightweight 2D point.
Definition: TPoint2D.h:31
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 ...
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.
Definition: common.h:186
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 1.9.9 Git: 4363012a5 Tue Nov 19 10:55:26 2019 +0100 at mar nov 19 11:00:13 CET 2019