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