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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 765b969e7 Sun Sep 22 19:55:28 2019 +0200 at dom sep 22 20:00:14 CEST 2019