Main MRPT website > C++ reference for MRPT 1.5.7
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-2017, 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/utils/CTicTac.h>
16 #include <mrpt/utils/color_maps.h>
17 #include <mrpt/utils/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>
25 #include <mrpt/utils/CStream.h>
26 
27 using namespace mrpt;
28 using namespace mrpt::maps;
29 using namespace mrpt::obs;
30 using namespace mrpt::utils;
31 using namespace mrpt::poses;
32 using namespace std;
33 using namespace mrpt::math;
34 
35 // =========== Begin of Map definition ============
37 
39  min_x(-2),
40  max_x(2),
41  min_y(-2),
42  max_y(2),
43  resolution(0.10f),
44  mapType(CGasConcentrationGridMap2D::mrKernelDM)
45 {
46 }
47 
49 {
50  // [<sectionNamePrefix>+"_creationOpts"]
51  const std::string sSectCreation = sectionNamePrefix+string("_creationOpts");
52  MRPT_LOAD_CONFIG_VAR(min_x, float, source,sSectCreation);
53  MRPT_LOAD_CONFIG_VAR(max_x, float, source,sSectCreation);
54  MRPT_LOAD_CONFIG_VAR(min_y, float, source,sSectCreation);
55  MRPT_LOAD_CONFIG_VAR(max_y, float, source,sSectCreation);
56  MRPT_LOAD_CONFIG_VAR(resolution, float, source,sSectCreation);
57  mapType = source.read_enum<CGasConcentrationGridMap2D::TMapRepresentation>(sSectCreation,"mapType",mapType);
58 
59  insertionOpts.loadFromConfigFile(source, sectionNamePrefix+string("_insertOpts") );
60 }
61 
63 {
65  LOADABLEOPTS_DUMP_VAR(min_x , float);
66  LOADABLEOPTS_DUMP_VAR(max_x , float);
67  LOADABLEOPTS_DUMP_VAR(min_y , float);
68  LOADABLEOPTS_DUMP_VAR(max_y , float);
69  LOADABLEOPTS_DUMP_VAR(resolution , float);
70 
71  this->insertionOpts.dumpToTextStream(out);
72 }
73 
75 {
79  return obj;
80 }
81 // =========== End of Map definition Block =========
82 
84 
85 // Short-cut:
86 #define LUT_TABLE (*(LUT.table))
87 
88 /*---------------------------------------------------------------
89  Constructor
90  ---------------------------------------------------------------*/
92  TMapRepresentation mapType,
93  float x_min,
94  float x_max,
95  float y_min,
96  float y_max,
97  float resolution ) :
98  CRandomFieldGridMap2D(mapType, x_min,x_max,y_min,y_max,resolution ),
99  insertionOptions()
100 {
101  // Override defaults:
105 
106  // Set the grid to initial values (and adjust covariance matrices,...)
107  // Also, calling clear() is mandatory to end initialization of our base class (read note in CRandomFieldGridMap2D::CRandomFieldGridMap2D)
109 
110  // Create WindGrids with same dimensions that the original map
111  windGrid_module.setSize( x_min,x_max,y_min,y_max,resolution );
112  windGrid_direction.setSize( x_min,x_max,y_min,y_max,resolution );
113 
114  //initialize counter for advection simulation
116 }
117 
119 {
120 }
121 
122 /*---------------------------------------------------------------
123  clear
124  ---------------------------------------------------------------*/
126 {
127  // Just do the generic clear:
129 
130  // Anything else special for this derived class?
131 
133  {
134  //Set default values of the wind grid
137 
138  /*float S = windGrid_direction.getSizeX() * windGrid_direction.getSizeY();
139 
140  for( unsigned int y=windGrid_direction.getSizeY()/2; y<windGrid_direction.getSizeY(); y++ )
141  {
142  for( unsigned int x=0; x<windGrid_direction.getSizeX(); x++ )
143  {
144  double * wind_cell = windGrid_direction.cellByIndex(x,y);
145  *wind_cell = 3*3.141516/2;
146 }
147  }*/
148 
149  // Generate Look-Up Table of the Gaussian weights due to wind advection.
151  {
152  //mrpt::system::pause();
153  THROW_EXCEPTION("Problem with LUT wind table");
154 
155  }
156 
157  }
158 }
159 
160 
161 /*---------------------------------------------------------------
162  insertObservation
163  ---------------------------------------------------------------*/
165  const CObservation *obs,
166  const CPose3D *robotPose )
167 {
168  MRPT_START
169 
170  CPose2D robotPose2D;
171  CPose3D robotPose3D;
172 
173  if (robotPose)
174  {
175  robotPose2D = CPose2D(*robotPose);
176  robotPose3D = (*robotPose);
177  }
178  else
179  {
180  // Default values are (0,0,0)
181  }
182 
183  if ( IS_CLASS(obs, CObservationGasSensors ))
184  {
185  /********************************************************************
186  OBSERVATION TYPE: CObservationGasSensors
187  ********************************************************************/
188  const CObservationGasSensors *o = static_cast<const CObservationGasSensors*>( obs );
189 
190  if ( o->sensorLabel.compare(insertionOptions.gasSensorLabel) == 0 )
191  {
192  float sensorReading;
193  CPose2D sensorPose;
194 
195  if ( o->sensorLabel.compare("MCEnose")==0 || o->sensorLabel.compare("Full_MCEnose")==0 )
196  {
199 
200  // Compute the 3D sensor pose in world coordinates:
201  sensorPose = CPose2D( CPose3D(robotPose2D) + CPose3D(it->eNosePoseOnTheRobot) );
202 
203  // Compute the sensor reading value (Volts):
204  if (insertionOptions.gasSensorType==0x0000){ //compute the mean
205  sensorReading = math::mean( it->readingsVoltage );
206  }
207  else
208  {
209  // Look for the correct sensor type
210  size_t i;
211  for (i=0; i<it->sensorTypes.size(); i++)
212  {
213  if (it->sensorTypes.at(i) == int(insertionOptions.gasSensorType) )
214  break;
215  }
216 
217  if (i<it->sensorTypes.size()){
218  sensorReading = it->readingsVoltage[i];
219  }
220  else
221  {
222  cout << "Sensor especified not found, compute default mean value" << endl;
223  sensorReading = math::mean( it->readingsVoltage );
224  }
225  }
226  }
227  else //"GDM, RAE_PID, ENOSE_SIMUL
228  {
230  // Compute the 3D sensor pose in world coordinates:
231  sensorPose = CPose2D( CPose3D(robotPose2D) +CPose3D(it->eNosePoseOnTheRobot) );
232  sensorReading = it->readingsVoltage[0];
233  }
234 
235 
236  // Normalization:
237  sensorReading = (sensorReading - insertionOptions.R_min) /( insertionOptions.R_max - insertionOptions.R_min );
238 
239 
240  // Update the gross estimates of mean/vars for the whole reading history (see IROS2009 paper):
244 
245  // Finally, do the actual map update with that value:
246  this->insertIndividualReading(sensorReading, mrpt::math::TPoint2D(sensorPose.x(),sensorPose.y()) );
247  return true; // Done!
248  } //endif correct "gasSensorLabel"
249  } // end if "CObservationGasSensors"
250 
251  return false;
252  MRPT_END
253 }
254 
255 /*---------------------------------------------------------------
256  computeObservationLikelihood
257  ---------------------------------------------------------------*/
259  const CObservation *obs,
260  const CPose3D &takenFrom )
261 {
262  MRPT_UNUSED_PARAM(obs);MRPT_UNUSED_PARAM(takenFrom);
263 
264  THROW_EXCEPTION("Not implemented yet!");
265 }
266 
267 /*---------------------------------------------------------------
268  Implements the writing to a CStream capability of CSerializable objects
269  ---------------------------------------------------------------*/
271 {
272  if (version)
273  *version = 5;
274  else
275  {
277 
278  // To assure compatibility: The size of each cell:
279  uint32_t n = static_cast<uint32_t>(sizeof( TRandomFieldCell ));
280  out << n;
281 
282  // Save the map contents:
283  n = static_cast<uint32_t>(m_map.size());
284  out << n;
285 
286  // Save the "m_map": This requires special handling for big endian systems:
287 #if MRPT_IS_BIG_ENDIAN
288  for (uint32_t i=0;i<n;i++)
289  {
290  out << m_map[i].kf_mean << m_map[i].dm_mean << m_map[i].dmv_var_mean;
291  }
292 #else
293  // Little endian: just write all at once:
294  out.WriteBuffer( &m_map[0], sizeof(m_map[0])*m_map.size() ); // TODO: Do this endianness safe!!
295 #endif
296 
297 
298  // Version 1: Save the insertion options:
299  out << uint8_t(m_mapType)
300  << m_cov
301  << m_stackedCov;
302 
303  out << insertionOptions.sigma
312 
313  // New in v3:
315 
316  out << genericMapParams; // v4
317  }
318 }
319 
320 // Aux struct used below (must be at global scope for STL):
322 {
323  float mean, std;
324  float w,wr;
325 };
326 
327 /*---------------------------------------------------------------
328  Implements the reading from a CStream capability of CSerializable objects
329  ---------------------------------------------------------------*/
331 {
332  switch(version)
333  {
334  case 0:
335  case 1:
336  case 2:
337  case 3:
338  case 4:
339  case 5:
340  {
341  dyngridcommon_readFromStream(in, version<5);
342 
343  // To assure compatibility: The size of each cell:
344  uint32_t n;
345  in >> n;
346 
347  if (version<2)
348  { // Converter from old versions <=1
349  ASSERT_( n == static_cast<uint32_t>( sizeof( TOldCellTypeInVersion1 ) ));
350  // Load the map contents in an aux struct:
351  in >> n;
352  vector<TOldCellTypeInVersion1> old_map(n);
353  in.ReadBuffer( &old_map[0], sizeof(old_map[0])*old_map.size() );
354 
355  // Convert to newer format:
356  m_map.resize(n);
357  for (size_t k=0;k<n;k++)
358  {
359  m_map[k].kf_mean = (old_map[k].w!=0) ? old_map[k].wr : old_map[k].mean;
360  m_map[k].kf_std = (old_map[k].w!=0) ? old_map[k].w : old_map[k].std;
361  }
362  }
363  else
364  {
365  ASSERT_EQUAL_( n , static_cast<uint32_t>( sizeof( TRandomFieldCell ) ));
366  // Load the map contents:
367  in >> n;
368  m_map.resize(n);
369 
370  // Read the note in writeToStream()
371 #if MRPT_IS_BIG_ENDIAN
372  for (uint32_t i=0;i<n;i++)
373  in >> m_map[i].kf_mean >> m_map[i].dm_mean >> m_map[i].dmv_var_mean;
374 #else
375  // Little endian: just read all at once:
376  in.ReadBuffer( &m_map[0], sizeof(m_map[0])*m_map.size() );
377 #endif
378  }
379 
380  // Version 1: Insertion options:
381  if (version>=1)
382  {
383  uint8_t i;
384  in >> i;
386 
387  in >> m_cov
388  >> m_stackedCov;
389 
390  in >> insertionOptions.sigma
399  }
400 
401  if (version>=3)
402  {
403  uint64_t N;
406  }
407 
408  if (version>=4)
409  in >> genericMapParams;
410 
412  } break;
413  default:
415 
416  };
417 
418 }
419 
420 /*---------------------------------------------------------------
421  TInsertionOptions
422  ---------------------------------------------------------------*/
424 
425  gasSensorLabel ( "MCEnose" ),
426  enose_id ( 0 ), //By default use the first enose
427  gasSensorType ( 0x0000 ), //By default use the mean between all e-nose sensors
428  windSensorLabel ( "windSensor" ),
429  useWindInformation ( false ), //By default dont use wind
430  std_windNoise_phi ( 0.2f ),
431  std_windNoise_mod ( 0.2f ),
432  default_wind_direction ( 0.0f ),
433  default_wind_speed ( 1.0f )
434 {
435 }
436 
437 /*---------------------------------------------------------------
438  dumpToTextStream
439  ---------------------------------------------------------------*/
441 {
442  out.printf("\n----------- [CGasConcentrationGridMap2D::TInsertionOptions] ------------ \n\n");
443  out.printf("[TInsertionOptions.Common] ------------ \n\n");
444  internal_dumpToTextStream_common(out); // Common params to all random fields maps:
445 
446  out.printf("[TInsertionOptions.GasSpecific] ------------ \n\n");
447  out.printf("gasSensorLabel = %s\n", gasSensorLabel.c_str() );
448  out.printf("enose_id = %u\n", (unsigned)enose_id);
449  out.printf("gasSensorType = %u\n", (unsigned)gasSensorType);
450  out.printf("windSensorLabel = %s\n", windSensorLabel.c_str() );
451  out.printf("useWindInformation = %u\n", useWindInformation);
452 
453  out.printf("advectionFreq = %f\n", advectionFreq);
454  out.printf("default_wind_direction = %f\n", default_wind_direction);
455  out.printf("default_wind_speed = %f\n", default_wind_speed);
456  out.printf("std_windNoise_phi = %f\n", std_windNoise_phi);
457  out.printf("std_windNoise_mod = %f\n", std_windNoise_mod);
458 
459  out.printf("\n");
460 }
461 
462 /*---------------------------------------------------------------
463  loadFromConfigFile
464  ---------------------------------------------------------------*/
466  const mrpt::utils::CConfigFileBase &iniFile,
467  const std::string &section)
468 {
469  // Common data fields for all random fields maps:
470  internal_loadFromConfigFile_common(iniFile,section);
471 
472  // Specific data fields for gasGridMaps
473  gasSensorLabel = iniFile.read_string(section.c_str(),"gasSensorLabel","Full_MCEnose",true);
474  enose_id = iniFile.read_int(section.c_str(),"enoseID",enose_id);
475  //Read sensor type in hexadecimal
476  {
477  std::string sensorType_str = iniFile.read_string(section.c_str(),"gasSensorType","-1",true);
478  int tmpSensorType;
479  stringstream convert ( sensorType_str );
480  convert>> std::hex >> tmpSensorType;
481 
482  if (tmpSensorType>=0)
483  {
484  // Valid number found:
485  gasSensorType = tmpSensorType;
486  }
487  else
488  { // fall back to old name, or default to current value:
489  gasSensorType = iniFile.read_int(section.c_str(),"KF_sensorType",gasSensorType,true);
490  }
491  }
492  windSensorLabel = iniFile.read_string(section.c_str(),"windSensorLabel","Full_MCEnose",true);
493 
494  //Indicates if wind information must be used for Advection Simulation
495  useWindInformation = iniFile.read_bool(section.c_str(),"useWindInformation","false",true);
496 
497  //(rad) The initial/default value of the wind direction
498  default_wind_direction = iniFile.read_float(section.c_str(),"default_wind_direction",0,false);
499  //(m/s) The initial/default value of the wind speed
500  default_wind_speed = iniFile.read_float(section.c_str(),"default_wind_speed",0,false);
501 
502  //(rad) The noise in the wind direction
503  std_windNoise_phi = iniFile.read_float(section.c_str(),"std_windNoise_phi",0,false);
504  //(m/s) The noise in the wind strenght
505  std_windNoise_mod = iniFile.read_float(section.c_str(),"std_windNoise_mod",0,false);
506 
507  //(m/s) The noise in the wind strenght
508  advectionFreq = iniFile.read_float(section.c_str(),"advectionFreq",1,true);
509 }
510 
511 
512 /*---------------------------------------------------------------
513  getAs3DObject
514 ---------------------------------------------------------------*/
515 void CGasConcentrationGridMap2D::getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj) const
516 {
517  MRPT_START
520  MRPT_END
521 }
522 
523 /*---------------------------------------------------------------
524  getAs3DObject
525 ---------------------------------------------------------------*/
527  mrpt::opengl::CSetOfObjectsPtr &meanObj,
528  mrpt::opengl::CSetOfObjectsPtr &varObj ) const
529 {
530  MRPT_START
532  CRandomFieldGridMap2D::getAs3DObject(meanObj,varObj);
533  MRPT_END
534 }
535 
536 /*---------------------------------------------------------------
537  getWindAs3DObject
538 ---------------------------------------------------------------*/
539 void CGasConcentrationGridMap2D::getWindAs3DObject( mrpt::opengl::CSetOfObjectsPtr &windObj) const
540 {
541  //Return an arrow map of the wind state (module(color) and direction).
542  float scale = 0.2f;
543  size_t arrow_separation = 5; //distance between arrows, expresed as times the cell resolution
544 
545 
546  //map limits
547  float x_min = getXMin();
548  float x_max = getXMax();
549  float y_min = getYMin();
550  float y_max = getYMax();
551  float resol = getResolution();
552 
553 
554  //Ensure map dimensions match with wind map
555  unsigned int wind_map_size = windGrid_direction.getSizeX() * windGrid_direction.getSizeY();
556  ASSERT_( wind_map_size == windGrid_module.getSizeX() * windGrid_module.getSizeY() );
557  if( m_map.size() != wind_map_size )
558  {
559  cout << " GAS MAP DIMENSIONS DO NOT MATCH WIND MAP " << endl;
560  //mrpt::system::pause();
561  }
562 
563 
564  unsigned int cx,cy;
565  vector<float> xs,ys;
566 
567  // xs: array of X-axis values
568  xs.resize( floor((x_max-x_min)/(arrow_separation*resol)) );
569  for (cx=0;cx<xs.size();cx++)
570  xs[cx] = x_min + arrow_separation*resol * cx;
571 
572  // ys: array of X-axis values
573  ys.resize( floor((y_max-y_min)/(arrow_separation*resol)) );
574  for (cy=0;cy<ys.size();cy++)
575  ys[cy] = y_min + arrow_separation*resol * cy;
576 
577  for (cy=0;cy<ys.size();cy++)
578  {
579  for (cx=0;cx<xs.size();cx++)
580  {
581  // Cell values [0,inf]:
582  double dir_xy = *windGrid_direction.cellByPos( xs[cx],ys[cy] );
583  double mod_xy = *windGrid_module.cellByPos( xs[cx],ys[cy] );
584 
585 
586  mrpt::opengl::CArrowPtr obj = mrpt::opengl::CArrow::Create(
587  xs[cx],ys[cy],0,
588  xs[cx]+scale*cos(dir_xy),ys[cy]+scale*sin(dir_xy),0,
589  1.15f*scale,0.3f*scale,0.35f*scale);
590 
591  float r,g,b;
592  jet2rgb( mod_xy,r,g,b );
593  obj->setColor(r,g,b);
594 
595  windObj->insert( obj );
596  }
597  }
598 }
599 
600 /*---------------------------------------------------------------
601  increaseUncertainty
602 ---------------------------------------------------------------*/
603 void CGasConcentrationGridMap2D::increaseUncertainty(const double STD_increase_value)
604 {
605  //Increase cell variance
606 // unsigned int cx,cy;
607 // double memory_retention;
608 
610  for( size_t it=0; it<m_map.size(); it++ )
611  {
612  m_stackedCov(it,0) = m_stackedCov(it,0)+STD_increase_value;
613  }
614 
615  //Update m_map.kf_std
617 
618  //for (cy=0; cy<m_size_y; cy++)
619  // {
620  // for (cx=0; cx<m_size_x; cx++)
621  // {
622  // // Forgetting_curve --> memory_retention = exp(-time/memory_relative_strenght)
623  // memory_retention = exp(- mrpt::system::timeDifference(m_map[cx + cy*m_size_x].last_updated, now()) / memory_relative_strenght);
624  // //Update Uncertainty (STD)
625  // m_map[cx + cy*m_size_x].kf_std = 1 - ( (1-m_map[cx + cy*m_size_x].updated_std) * memory_retention );
626  // }
627  // }
628 }
629 
630 
631 /*---------------------------------------------------------------
632  simulateAdvection
633 ---------------------------------------------------------------*/
634 bool CGasConcentrationGridMap2D::simulateAdvection( const double &STD_increase_value )
635 {
636 
637  /* 1- Ensure we can use Wind Information
638  -------------------------------------------------*/
640  return false;
641 
642  //Get time since last simulation
644  cout << endl << " - At since last simulation = " << At << "seconds" << endl;
645  //update time of last updated.
647 
648 
649  /* 3- Build Transition Matrix (SA)
650  This Matrix contains the probabilities of each cell
651  to "be displaced" to other cells by the wind effect.
652  ------------------------------------------------------*/
653  mrpt::utils::CTicTac tictac;
654  size_t i=0,c=0;
655  int cell_i_cx,cell_i_cy;
656  float mu_phi, mu_r, mu_modwind;
657  const size_t N = m_map.size();
658  mrpt::math::CMatrix A(N,N);
659  A.fill(0.0);
660  //std::vector<double> row_sum(N,0.0);
661  double *row_sum = (double*)calloc(N, sizeof(double));
662 
663  try
664  {
665  //Ensure map dimensions match with wind map
666  unsigned int wind_map_size = windGrid_direction.getSizeX() * windGrid_direction.getSizeY();
667  ASSERT_( wind_map_size == windGrid_module.getSizeX() * windGrid_module.getSizeY() );
668  if( N != wind_map_size )
669  {
670  cout << " GAS MAP DIMENSIONS DO NOT MATCH WIND INFORMATION " << endl;
671  //mrpt::system::pause();
672  }
673 
674  tictac.Tic();
675 
676  //Generate Sparse Matrix of the wind advection SA
677  for(i=0; i<N; i++)
678  {
679  //Cell_i indx and coordinates
680  idx2cxcy(i,cell_i_cx,cell_i_cy);
681 
682  //Read dirwind value of cell i
683  mu_phi = *windGrid_direction.cellByIndex(cell_i_cx,cell_i_cy); //[0,2*pi]
684  unsigned int phi_indx = round(mu_phi/LUT.phi_inc);
685 
686  //Read modwind value of cell i
687  mu_modwind = *windGrid_module.cellByIndex(cell_i_cx,cell_i_cy); //[0,inf)
688  mu_r = mu_modwind * At;
689  if( mu_r > LUT.max_r)
690  mu_r = LUT.max_r;
691  unsigned int r_indx = round(mu_r/LUT.r_inc);
692 
693  //Evaluate LUT
694  ASSERT_(phi_indx < LUT.phi_count);
695  ASSERT_(r_indx < LUT.r_count);
696 
697  //define label
698  vector<TGaussianCell> &cells_to_update = LUT_TABLE[phi_indx][r_indx];
699 
700  //Generate Sparse Matrix with the wind weights "SA"
701  for( unsigned int ci=0; ci<cells_to_update.size(); ci++)
702  {
703  int final_cx = cell_i_cx + cells_to_update[ci].cx;
704  int final_cy = cell_i_cy + cells_to_update[ci].cy;
705  //Check if affected cells is within the map
706  if( (final_cx>=0) && (final_cx<(int)getSizeX()) && (final_cy>=0) && (final_cy<(int)getSizeY()) )
707  {
708  int final_idx = final_cx + final_cy*getSizeX();
709 
710  // Add Value to SA Matrix
711  if( cells_to_update[ci].value != 0.0 )
712  {
713  A(final_idx,i) = cells_to_update[ci].value;
714  row_sum[final_idx] += cells_to_update[ci].value;
715  }
716  }
717  }//end-for ci
718  }//end-for cell i
719 
720  cout << " - SA matrix computed in " << tictac.Tac() << "s" << endl << endl;
721  }
722  catch( std::exception &e)
723  {
724  cout << " ######### EXCEPTION computing Transition Matrix (A) ##########\n: " << e.what() << endl;
725  cout << "on cell i= " << i << " c=" << c << endl << endl;
726  return false;
727  }
728 
729 
730  /* Update Mean + Variance as a Gaussian Mixture
731  ------------------------------------------------*/
732  try
733  {
734  tictac.Tic();
735  //std::vector<double> new_means(N,0.0);
736  double *new_means = (double*)calloc(N, sizeof(double));
737  //std::vector<double> new_variances(N,0.0);
738  double *new_variances = (double*)calloc(N, sizeof(double));
739 
740  for( size_t it_i=0; it_i<N; it_i++)
741  {
742  //--------
743  // mean
744  //--------
745  for( size_t it_j=0; it_j<N; it_j++)
746  {
747  if (m_map[it_j].kf_mean!=0 && A(it_i,it_j)!=0)
748  {
749  if (row_sum[it_i] >= 1)
750  new_means[it_i] += ( A(it_i,it_j)/row_sum[it_i] ) * m_map[it_j].kf_mean;
751  else
752  new_means[it_i] += A(it_i,it_j) * m_map[it_j].kf_mean;
753  }
754  }
755 
756 
757  //----------
758  // variance
759  //----------
760  //Consider special case (borders cells)
761  if (row_sum[it_i] < 1)
762  new_variances[it_i] = (1-row_sum[it_i]) * square(insertionOptions.KF_initialCellStd);
763 
764  for( size_t it_j=0; it_j<N; it_j++)
765  {
766  if (A(it_i,it_j)!=0)
767  {
768  if (row_sum[it_i] >= 1)
769  new_variances[it_i] += ( A(it_i,it_j)/row_sum[it_i] ) * ( m_stackedCov(it_j,0) + square( m_map[it_j].kf_mean - new_means[it_i]) );
770  else
771  new_variances[it_i] += A(it_i,it_j) * ( m_stackedCov(it_j,0) + square( m_map[it_j].kf_mean - new_means[it_i]) );
772  }
773  }
774  }
775 
776  //Update means and Cov of the Kalman filter state
777  for( size_t it_i=0; it_i<N; it_i++)
778  {
779  m_map[it_i].kf_mean = new_means[it_i]; //means
780 
781  //Variances
782  // Scale the Current Covariances with the new variances
783  for (size_t it_j=0; it_j<N; it_j++)
784  {
785  m_stackedCov(it_i,it_j) = (m_stackedCov(it_i,it_j)/m_stackedCov(it_i,it_i) )* new_variances[it_i]; //variances
786  m_stackedCov(it_j,it_i) = m_stackedCov(it_i,it_j);
787  }
788  }
791 
792  cout << " - Mean&Var updated in " << tictac.Tac() << "s" << endl;
793 
794 
795  //Free Memory
796  free(row_sum);
797  free(new_means);
798  free(new_variances);
799 
800  }
801  catch( std::exception &e)
802  {
803  cout << " ######### EXCEPTION Updating Covariances ##########\n: " << e.what() << endl;
804  cout << "on row i= " << i << " column c=" << c << endl << endl;
805  return false;
806  }
807 
808 
809  //cout << " Increasing general STD..." << endl;
810  increaseUncertainty(STD_increase_value);
811 
812  return true;
813 
814 }
815 
816 
817 
818 
819 /*---------------------------------------------------------------
820  build_Gaussian_Wind_Grid
821 ---------------------------------------------------------------*/
822 
824 /** Builds a LookUp table with the values of the Gaussian Weights result of the wind advection
825 * for a specific condition.
826 *
827 * The LUT contains the values of the Gaussian Weigths and the references to the cell indexes to be applied.
828 * Since the LUT is independent of the wind direction and angle, it generates the Gaussian Weights for different configurations
829 * of wind angle and module values.
830 *
831 * To increase precission, each cell of the grid is sub-divided in subcells of smaller size.
832 
833 * cell_i --> Cell origin (We consider our reference system in the bottom left corner of cell_i ).
834  Is the cell that contains the gas measurement which will be propagated by the wind.
835  The wind propagates in the shape of a 2D Gaussian with center in the target cell (cell_j)
836 * cell_j --> Target cell. Is the cell where falls the center of the Gaussian that models the propagation of the gas comming from cell_i.
837 */
838 
839  {
840 
841  cout << endl << "---------------------------------" << endl;
842  cout << " BUILDING GAUSSIAN WIND WEIGHTS " << endl;
843  cout << "---------------------------------" << endl << endl;
844 
845 
846  //-----------------------------
847  // PARAMS
848  //-----------------------------
849  LUT.resolution = getResolution(); //resolution of the grid-cells (m)
850  LUT.std_phi = insertionOptions.std_windNoise_phi; //Standard Deviation in wind Angle (cte)
851  LUT.std_r = insertionOptions.std_windNoise_mod / insertionOptions.advectionFreq; //Standard Deviation in wind module (cte)
852  std::string filename = format("Gaussian_Wind_Weights_res(%f)_stdPhi(%f)_stdR(%f).gz",LUT.resolution,LUT.std_phi,LUT.std_r);
853 
854  // Fixed Params:
855  LUT.phi_inc = M_PIf/8; //Increment in the wind Angle. (rad)
856  LUT.phi_count = round(2*M_PI/LUT.phi_inc)+1; //Number of angles to generate
857  LUT.r_inc = 0.1f; //Increment in the wind Module. (m)
858  LUT.max_r = 2; //maximum distance (m) to simulate
859  LUT.r_count = round(LUT.max_r/LUT.r_inc)+1; //Number of wind modules to simulate
860 
861  LUT.table = new vector<vector<vector<TGaussianCell> > >(LUT.phi_count, vector<vector<TGaussianCell> >(LUT.r_count,vector<TGaussianCell>()) );
862 
863  //LUT.table = new vector<vector<vector<vector<vector<TGaussianCell>>>>>(LUT.subcell_count, vector<vector<vector<vector<TGaussianCell>>>>(LUT.subcell_count, vector<vector<vector<TGaussianCell>>>(LUT.phi_count, vector<vector<TGaussianCell>>(LUT.r_count,vector<TGaussianCell>()) ) ) );
864 
865  //-----------------------------
866  // Check if file exists
867  //-----------------------------
868 
869  cout << "Looking for file: " << filename.c_str() << endl;
870 
871  if( mrpt::system::fileExists(filename.c_str()) )
872  {
873  // file exists. Load lookUptable from file
874  cout << "LookUp table found for this configuration. Loading..." << endl;
876 
877  }
878  else
879  {
880  // file does not exists. Generate LookUp table.
881  cout << "LookUp table NOT found. Generating table..." << endl;
882 
883  bool debug = true;
884  FILE *debug_file;
885 
886  if (debug)
887  {
888  debug_file = fopen ("simple_LUT.txt","w");
889  fprintf(debug_file," phi_inc = %.4f \n r_inc = %.4f \n",LUT.phi_inc,LUT.r_inc);
890  fprintf(debug_file," std_phi = %.4f \n std_r = %.4f \n",LUT.std_phi,LUT.std_r);
891  fprintf(debug_file,"[ phi ] [ r ] ---> (cx,cy)=Value\n");
892  fprintf(debug_file,"----------------------------------\n");
893  }
894 
895  // For the different possible angles (phi)
896  for (size_t phi_indx=0; phi_indx<LUT.phi_count; phi_indx++ )
897  {
898  // mean of the phi value
899  float phi = phi_indx * LUT.phi_inc;
900 
901  //For the different and possibe wind modules (r)
902  for (size_t r_indx=0; r_indx<LUT.r_count; r_indx++ )
903  {
904  //mean of the radius value
905  float r = r_indx * LUT.r_inc;
906 
907  if (debug)
908  {
909  fprintf(debug_file, "\n[%.2f] [%.2f] ---> ",phi, r);
910  }
911 
912  //Estimates Cell_i_position
913  //unsigned int cell_i_cx = 0;
914  //unsigned int cell_i_cy = 0;
915  float cell_i_x = LUT.resolution/2.0;
916  float cell_i_y = LUT.resolution/2.0;
917 
918  //Estimate target position according to the mean value of wind.
919  //float x_final = cell_i_x + r*cos(phi);
920  //float y_final = cell_i_y + r*sin(phi);
921 
922  //Determine cell_j coordinates respect to origin_cell
923  //int cell_j_cx = static_cast<int>(floor( (x_final)/LUT.resolution ));
924  //int cell_j_cy = static_cast<int>(floor( (y_final)/LUT.resolution ));
925  //Center of cell_j
926  //float cell_j_x = (cell_j_cx+0.5f)*LUT.resolution;
927  //float cell_j_y = (cell_j_cy+0.5f)*LUT.resolution;
928  //left bottom corner of cell_j
929  //float cell_j_xmin = cell_j_x - LUT.resolution/2.0;
930  //float cell_j_ymin = cell_j_y - LUT.resolution/2.0;
931 
932 
933 
934  /* ---------------------------------------------------------------------------------
935  Generate bounding-box (+/- 3std) to determine which cells to update
936  ---------------------------------------------------------------------------------*/
937  std::vector<double> vertex_x, vertex_y;
938  vertex_x.resize(14);
939  vertex_y.resize(14);
940  //Bounding-Box initialization
941  double minBBox_x = 1000;
942  double maxBBox_x = -1000;
943  double minBBox_y = 1000;
944  double maxBBox_y = -1000;
945 
946  //Consider special case for high uncertainty in PHI. The shape of the polygon is a donut.
947  double std_phi_BBox = LUT.std_phi;
948  if( std_phi_BBox > M_PI/3)
949  {
950  std_phi_BBox = M_PI/3; //To avoid problems generating the bounding box. For std>pi/3 the shape is always a donut.
951  }
952 
953  // Calculate bounding box limits
954  size_t indx = 0;
955  int sr=3;
956  for( int sd=(-3); sd<=(3); sd++ )
957  {
958  vertex_x[indx] = cell_i_x + (r+sr*LUT.std_r)*cos(phi+sd*std_phi_BBox);
959  if (vertex_x[indx] < minBBox_x)
960  minBBox_x = vertex_x[indx];
961  if (vertex_x[indx] > maxBBox_x)
962  maxBBox_x = vertex_x[indx];
963 
964  vertex_y[indx] = cell_i_y + (r+sr*LUT.std_r)*sin(phi+sd*std_phi_BBox);
965  if (vertex_y[indx] < minBBox_y)
966  minBBox_y = vertex_y[indx];
967  if (vertex_y[indx] > maxBBox_y)
968  maxBBox_y = vertex_y[indx];
969 
970  indx++;
971  }
972  sr=-3;
973  for( int sd=(3); sd>=(-3); sd-- )
974  {
975  vertex_x[indx] = cell_i_x + (r+sr*LUT.std_r)*cos(phi+sd*std_phi_BBox);
976  if (vertex_x[indx] < minBBox_x)
977  minBBox_x = vertex_x[indx];
978  if (vertex_x[indx] > maxBBox_x)
979  maxBBox_x = vertex_x[indx];
980 
981  vertex_y[indx] = cell_i_y + (r+sr*LUT.std_r)*sin(phi+sd*std_phi_BBox);
982  if (vertex_y[indx] < minBBox_y)
983  minBBox_y = vertex_y[indx];
984  if (vertex_y[indx] > maxBBox_y)
985  maxBBox_y = vertex_y[indx];
986 
987  indx++;
988  }
989 
990  /* ------------------------------------------------------------------------
991  Determine range of cells to update according to the Bounding-Box limits.
992  Origin cell is cx=cy= 0 x[0,res), y[0,res)
993  ---------------------------------------------------------------------------*/
994  int min_cx = static_cast<int>( floor(minBBox_x/LUT.resolution) );
995  int max_cx = static_cast<int>( floor(maxBBox_x/LUT.resolution) );
996  int min_cy = static_cast<int>( floor(minBBox_y/LUT.resolution) );
997  int max_cy = static_cast<int>( floor(maxBBox_y/LUT.resolution) );
998 
999  int num_cells_affected = (max_cx-min_cx+1) * (max_cy-min_cy+1);
1000 
1001  if( num_cells_affected == 1 )
1002  {
1003  //Concentration of cell_i moves to cell_a (cx,cy)
1004  TGaussianCell gauss_info;
1005  gauss_info.cx = min_cx; //since max_cx == min_cx
1006  gauss_info.cy = min_cy;
1007  gauss_info.value = 1; //prob = 1
1008 
1009  //Add cell volume to LookUp Table
1010  LUT_TABLE[phi_indx][r_indx].push_back(gauss_info);
1011 
1012  if (debug)
1013  {
1014  //Save to file (debug)
1015  fprintf(debug_file, "(%d,%d)=%.4f",gauss_info.cx, gauss_info.cy, gauss_info.value);
1016  }
1017  }
1018  else
1019  {
1020  // Estimate volume of the Gaussian under each affected cell
1021 
1022  float subcell_pres = LUT.resolution/10;
1023  //Determine the number of subcells inside the Bounding-Box
1024  const int BB_x_subcells = (int) (floor( (maxBBox_x - minBBox_x)/subcell_pres ) +1 );
1025  const int BB_y_subcells = (int) (floor( (maxBBox_y - minBBox_y)/subcell_pres ) +1 );
1026 
1027  double subcell_pres_x = (maxBBox_x - minBBox_x)/BB_x_subcells;
1028  double subcell_pres_y = (maxBBox_y - minBBox_y)/BB_y_subcells;
1029 
1030  //Save the W value of each cell using a map
1031  std::map<std::pair<int,int>, float> w_values;
1032  std::map<std::pair<int,int>, float>::iterator it;
1033  float sum_w = 0;
1034 
1035  for(int scy=0; scy<BB_y_subcells; scy++)
1036  {
1037  for(int scx=0; scx<BB_x_subcells; scx++)
1038  {
1039  //P-Subcell coordinates (center of the p-subcell)
1040  float subcell_a_x = minBBox_x + (scx+0.5f)*subcell_pres_x;
1041  float subcell_a_y = minBBox_y + (scy+0.5f)*subcell_pres_y;
1042 
1043  //distance and angle between cell_i and subcell_a
1044  float r_ia = sqrt( square(subcell_a_x-cell_i_x) + square(subcell_a_y-cell_i_y) );
1045  float phi_ia = atan2(subcell_a_y-cell_i_y, subcell_a_x-cell_i_x);
1046 
1047  //Volume Approximation of subcell_a (Gaussian Bivariate)
1048  float w = (1/(2*M_PI*LUT.std_r*LUT.std_phi)) * exp(-0.5*( square(r_ia-r)/square(LUT.std_r) + square(phi_ia-phi)/square(LUT.std_phi) ) );
1049  w += (1/(2*M_PI*LUT.std_r*LUT.std_phi)) * exp(-0.5*( square(r_ia-r)/square(LUT.std_r) + square(phi_ia+2*M_PI-phi)/square(LUT.std_phi) ) );
1050  w += (1/(2*M_PI*LUT.std_r*LUT.std_phi)) * exp(-0.5*( square(r_ia-r)/square(LUT.std_r) + square(phi_ia-2*M_PI-phi)/square(LUT.std_phi) ) );
1051 
1052  //Since we work with a cell grid, approximate the weight of the gaussian by the volume of the subcell_a
1053  if (r_ia != 0.0)
1054  w = (w * (subcell_pres_x*subcell_pres_y)/r_ia);
1055 
1056  //Determine cell index of the current subcell
1057  int cell_cx = static_cast<int>( floor(subcell_a_x/LUT.resolution) );
1058  int cell_cy = static_cast<int>( floor(subcell_a_y/LUT.resolution) );
1059 
1060  //Save w value
1061  it = w_values.find(std::make_pair(cell_cx,cell_cy));
1062  if( it!=w_values.end() ) //already exists
1063  w_values[std::make_pair(cell_cx,cell_cy)] += w;
1064  else
1065  w_values[std::make_pair(cell_cx,cell_cy)] = w;
1066 
1067  sum_w = sum_w + w;
1068  }//end-for scx
1069  }//end-for scy
1070 
1071 
1072  //SAVE to LUT
1073  for(it= w_values.begin(); it != w_values.end(); it++)
1074  {
1075  float w_final = (it->second)/sum_w; //normalization to 1
1076 
1077  if( w_final >= 0.001 )
1078  {
1079  //Save the weight of the gaussian volume for cell_a (cx,cy)
1080  TGaussianCell gauss_info;
1081  gauss_info.cx = it->first.first;
1082  gauss_info.cy = it->first.second;
1083  gauss_info.value = w_final;
1084 
1085  //Add cell volume to LookUp Table
1086  LUT_TABLE[phi_indx][r_indx].push_back(gauss_info);
1087 
1088  if (debug)
1089  {
1090  //Save to file (debug)
1091  fprintf(debug_file, "(%d,%d)=%.6f ",gauss_info.cx, gauss_info.cy, gauss_info.value);
1092  }
1093  }
1094  }
1095 
1096  //OLD WAY
1097 
1098  /* ---------------------------------------------------------
1099  Estimate the volume of the Gaussian on each affected cell
1100  //-----------------------------------------------------------*/
1101  //for(int cx=min_cx; cx<=max_cx; cx++)
1102  //{
1103  // for(int cy=min_cy; cy<=max_cy; cy++)
1104  // {
1105  // // Coordinates of affected cell (center of the cell)
1106  // float cell_a_x = (cx+0.5f)*LUT.resolution;
1107  // float cell_a_y = (cy+0.5f)*LUT.resolution;
1108  // float w_cell_a = 0.0; //initial Gaussian value of cell afected
1109 
1110  // // Estimate volume of the Gaussian under cell (a)
1111  // // Partition each cell into (p x p) subcells and evaluate the gaussian.
1112  // int p = 40;
1113  // float subcell_pres = LUT.resolution/p;
1114  // float cell_a_x_min = cell_a_x - LUT.resolution/2.0;
1115  // float cell_a_y_min = cell_a_y - LUT.resolution/2.0;
1116 
1117  //
1118  // for(int scy=0; scy<p; scy++)
1119  // {
1120  // for(int scx=0; scx<p; scx++)
1121  // {
1122  // //P-Subcell coordinates (center of the p-subcell)
1123  // float subcell_a_x = cell_a_x_min + (scx+0.5f)*subcell_pres;
1124  // float subcell_a_y = cell_a_y_min + (scy+0.5f)*subcell_pres;
1125 
1126  // //distance and angle between cell_i and subcell_a
1127  // float r_ia = sqrt( square(subcell_a_x-cell_i_x) + square(subcell_a_y-cell_i_y) );
1128  // float phi_ia = atan2(subcell_a_y-cell_i_y, subcell_a_x-cell_i_x);
1129 
1130  // //Volume Approximation of subcell_a (Gaussian Bivariate)
1131  // float w = (1/(2*M_PI*LUT.std_r*LUT.std_phi)) * exp(-0.5*( square(r_ia-r)/square(LUT.std_r) + square(phi_ia-phi)/square(LUT.std_phi) ) );
1132  // w += (1/(2*M_PI*LUT.std_r*LUT.std_phi)) * exp(-0.5*( square(r_ia-r)/square(LUT.std_r) + square(phi_ia+2*M_PI-phi)/square(LUT.std_phi) ) );
1133  // w += (1/(2*M_PI*LUT.std_r*LUT.std_phi)) * exp(-0.5*( square(r_ia-r)/square(LUT.std_r) + square(phi_ia-2*M_PI-phi)/square(LUT.std_phi) ) );
1134  //
1135  // //Since we work with a cell grid, approximate the weight of the gaussian by the volume of the subcell_a
1136  // if (r_ia != 0.0)
1137  // w_cell_a = w_cell_a + (w * square(subcell_pres)/r_ia);
1138  // }//end-for scx
1139  // }//end-for scy
1140 
1141  // //Save the weight of the gaussian volume for cell_a (cx,cy)
1142  // TGaussianCell gauss_info;
1143  // gauss_info.cx = cx;
1144  // gauss_info.cy = cy;
1145  // gauss_info.value = w_cell_a;
1146 
1147  // //Add cell volume to LookUp Table
1148  // LUT_TABLE[phi_indx][r_indx].push_back(gauss_info);
1149 
1150  // if (debug)
1151  // {
1152  // //Save to file (debug)
1153  // fprintf(debug_file, "(%d,%d)=%.6f ",gauss_info.cx, gauss_info.cy, gauss_info.value);
1154  // }
1155  //
1156  //
1157  // }//end-for cy
1158  //}//end-for cx
1159 
1160  }//end-if only one affected cell
1161 
1162  }//end-for r
1163  }//end-for phi
1164 
1165  if (debug)
1166  fclose(debug_file);
1167 
1168  //Save LUT to File
1170 
1171  }//end-if table not available
1172  }
1173 
1174 
1176 {
1177  // Save LUT to file
1178  cout << "Saving to File ....";
1179 
1180  CFileGZOutputStream f( format("Gaussian_Wind_Weights_res(%f)_stdPhi(%f)_stdR(%f).gz",LUT.resolution,LUT.std_phi,LUT.std_r) );
1181 
1182  if( !f.fileOpenCorrectly() )
1183  {
1184  return false;
1185  cout << "WARNING: Gaussian_Wind_Weights file NOT SAVED" << endl;
1186 }
1187 
1188  try
1189 {
1190  //Save params first
1191  f << LUT.resolution; //cell resolution used
1192  f << LUT.std_phi; //std_phi used
1193  f << LUT.std_r;
1194 
1195 
1196  f << LUT.phi_inc; //rad
1197  f << (float)LUT.phi_count;
1198  f << LUT.r_inc; //m
1199  f << LUT.max_r; //maximum distance (m)
1200  f << (float)LUT.r_count;
1201 
1202  //Save Multi-table
1203  //vector< vector< vector<TGaussianCell>>>>> *table;
1204 
1205  for (size_t phi_indx=0; phi_indx<LUT.phi_count; phi_indx++ )
1206  {
1207  for (size_t r_indx=0; r_indx<LUT.r_count; r_indx++ )
1208  {
1209  //save all cell values.
1210  size_t N = LUT_TABLE[phi_indx][r_indx].size();
1211  f << (float)N;
1212 
1213  for (size_t i=0; i<N; i++)
1214  {
1215  f << (float)LUT_TABLE[phi_indx][r_indx][i].cx;
1216  f << (float)LUT_TABLE[phi_indx][r_indx][i].cy;
1217  f << LUT_TABLE[phi_indx][r_indx][i].value;
1218  }
1219  }
1220 }
1221  cout << "DONE" << endl;
1222  f.close();
1223  return true;
1224  }
1225  catch(exception e)
1226  {
1227  cout << endl << "------------------------------------------------------------" << endl;
1228  cout << "EXCEPTION WHILE SAVING LUT TO FILE" << endl;
1229  cout << "Exception = " << e.what() << endl;
1230  f.close();
1231  return false;
1232  }
1233 
1234 }
1235 
1237 {
1238  // LOAD LUT from file
1239  cout << "Loading from File ....";
1240 
1241  try
1242  {
1243  CFileGZInputStream f( format("Gaussian_Wind_Weights_res(%f)_stdPhi(%f)_stdR(%f).gz",LUT.resolution,LUT.std_phi,LUT.std_r) );
1244 
1245  if( !f.fileOpenCorrectly() )
1246  {
1247  cout << "WARNING WHILE READING FROM: Gaussian_Wind_Weights" << endl;
1248  return false;
1249  }
1250 
1251  float t_float;
1252  unsigned int t_uint;
1253  // Ensure params from file are correct with the specified in the ini file
1254  f >> t_float;
1255  ASSERT_(LUT.resolution == t_float)
1256 
1257  f >> t_float;
1258  ASSERT_(LUT.std_phi == t_float);
1259 
1260  f >> t_float;
1261  ASSERT_(LUT.std_r == t_float);
1262 
1263  f >> t_float;
1264  ASSERT_(LUT.phi_inc == t_float);
1265 
1266  f >> t_float;
1267  t_uint = (unsigned int)t_float;
1268  ASSERT_(LUT.phi_count == t_uint);
1269 
1270  f >> t_float;
1271  ASSERT_(LUT.r_inc == t_float);
1272 
1273  f >> t_float;
1274  ASSERT_(LUT.max_r == t_float);
1275 
1276  f >> t_float;
1277  t_uint = (unsigned int)t_float;
1278  ASSERT_(LUT.r_count == t_uint);
1279 
1280  // Load Multi-table
1281  // vector< vector< vector<TGaussianCell>>>>> *table;
1282 
1283  for (size_t phi_indx=0; phi_indx<LUT.phi_count; phi_indx++ )
1284  {
1285  for (size_t r_indx=0; r_indx<LUT.r_count; r_indx++ )
1286  {
1287  //Number of cells to update
1288  size_t N;
1289  f >> t_float;
1290  N = (size_t)t_float;
1291 
1292  for (size_t i=0; i<N; i++)
1293  {
1294  TGaussianCell gauss_info;
1295  f >> t_float;
1296  gauss_info.cx = (int)t_float;
1297 
1298  f >> t_float;
1299  gauss_info.cy = (int)t_float;
1300 
1301  f >> gauss_info.value;
1302 
1303  //Add cell volume to LookUp Table
1304  LUT_TABLE[phi_indx][r_indx].push_back(gauss_info);
1305  }
1306 }
1307  }
1308  cout << "DONE" << endl;
1309  return true;
1310  }
1311  catch (exception e)
1312  {
1313  cout << endl << "------------------------------------------------------------" << endl;
1314  cout << "EXCEPTION WHILE LOADING LUT FROM FILE" << endl;
1315  cout << "Exception = " << e.what() << endl;
1316  return false;
1317  }
1318 }
size_t getSizeY() const
Returns the vertical size of grid map in cells count.
Definition: CDynamicGrid.h:223
#define ASSERT_EQUAL_(__A, __B)
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:34
float R_max
Limits for normalization of sensor readings.
double GMRF_saturate_max
(Default:-inf,+inf) Saturate the estimated mean in these limits
size_t ReadBuffer(void *Buffer, size_t Count)
Reads a block of bytes from the stream into Buffer On any error, or if ZERO bytes are read...
Definition: CStream.cpp:45
Virtual base for specifying the kind and parameters of one map (normally, to be inserted into mrpt::m...
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
Definition: os.cpp:255
std::string gasSensorLabel
The label of the CObservationGasSensor used to generate the map.
float sigma
The sigma of the "Parzen"-kernel Gaussian.
float read_float(const std::string &section, const std::string &name, float defaultValue, bool failIfNotFound=false) const
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
GLenum GLenum GLenum GLenum GLenum scale
Definition: glew.h:8359
static CArrowPtr Create()
ENUMTYPE read_enum(const std::string &section, const std::string &name, const ENUMTYPE &defaultValue, bool failIfNotFound=false) const
Reads an "enum" value, where the value in the config file can be either a numerical value or the symb...
float resolution
See CGasConcentrationGridMap2D::CGasConcentrationGridMap2D.
GLboolean GLboolean g
Definition: glew.h:5406
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.
struct MAPS_IMPEXP mrpt::maps::CGasConcentrationGridMap2D::TGaussianWindTable LUT
int BASE_IMPEXP void BASE_IMPEXP fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:272
double internal_computeObservationLikelihood(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D &takenFrom) MRPT_OVERRIDE
Internal method called by computeObservationLikelihood()
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
This must be inserted in all CSerializable classes implementation files.
const GLfloat * c
Definition: glew.h:10088
#define THROW_EXCEPTION(msg)
mrpt::utils::CDynamicGrid< double > windGrid_direction
uint16_t enose_id
id for the enose used to generate this map (must be < gasGrid_count)
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
void writeToStream(mrpt::utils::CStream &out, int *getVersion) const
Introduces a pure virtual method responsible for writing to a CStream.
Scalar * iterator
Definition: eigen_plugins.h:23
#define LOADABLEOPTS_DUMP_VAR(variableName, variableType)
Macro for dumping a variable to a stream, within the method "dumpToTextStream(out)" (Variable types a...
double getXMax() const
Returns the "x" coordinate of right side of grid map.
Definition: CDynamicGrid.h:229
This file implements several operations that operate element-wise on individual or pairs of container...
GLubyte GLubyte GLubyte GLubyte w
Definition: glew.h:1797
void dumpToTextStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
This method should clearly display all the contents of the structure in textual form, sending it to a CStream.
void WriteBuffer(const void *Buffer, size_t Count)
Writes a block of bytes to the stream from Buffer.
Definition: CStream.cpp:67
bool BASE_IMPEXP fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:124
void internal_loadFromConfigFile_common(const mrpt::utils::CConfigFileBase &source, const std::string &section)
See utils::CLoadableOptions.
mrpt::math::CMatrixD m_cov
The whole covariance matrix, used for the Kalman Filter map representation.
double getYMin() const
Returns the "y" coordinate of top side of grid map.
Definition: CDynamicGrid.h:232
mrpt::system::TTimeStamp now()
A shortcut for system::getCurrentTime.
Definition: datetime.h:70
int BASE_IMPEXP fprintf(FILE *fil, const char *format,...) MRPT_NO_THROWS MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:412
TMapRepresentation
The type of map representation to be used, see CRandomFieldGridMap2D for a discussion.
void internal_dumpToTextStream_common(mrpt::utils::CStream &out) const
See utils::CLoadableOptions.
STL namespace.
TMapGenericParams genericMapParams
Common params to all maps.
#define M_PI
Definition: bits.h:78
void getWindAs3DObject(mrpt::opengl::CSetOfObjectsPtr &windObj) const
Returns the 3D object representing the wind grid information.
mrpt::maps::CGasConcentrationGridMap2D::TInsertionOptions insertionOpts
Observations insertion options.
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.
void fill(const T &value)
Fills all the cells with the same value.
Definition: CDynamicGrid.h:101
void Tic()
Starts the stopwatch.
Definition: CTicTac.cpp:77
void loadFromConfigFile(const mrpt::utils::CConfigFileBase &source, const std::string &section) MRPT_OVERRIDE
This method load the options from a ".ini"-like file or memory-stored string list.
size_t getSizeX() const
Returns the horizontal size of grid map in cells count.
Definition: CDynamicGrid.h:220
GLuint in
Definition: glew.h:7146
This class allows loading and storing values and vectors of different types from a configuration text...
T square(const T x)
Inline function for the square of a number.
Definition: bits.h:52
TMapRepresentation m_mapType
The map representation type of this map, as passed in the constructor.
unsigned char uint8_t
Definition: rptypes.h:43
void loadFromConfigFile_map_specific(const mrpt::utils::CConfigFileBase &source, const std::string &sectionNamePrefix)
Load all map-specific params.
GLsizei const GLfloat * value
Definition: glew.h:1756
T * cellByPos(double x, double y)
Returns a pointer to the contents of a cell given by its coordinates, or NULL if it is out of the map...
Definition: CDynamicGrid.h:183
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
float KF_observationModelNoise
The sensor model noise (in normalized concentration units).
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
This base provides a set of functions for maths stuff.
Definition: CArrayNumeric.h:19
double x() const
Common members of all points & poses classes.
Definition: CPoseOrPoint.h:113
GLsizei n
Definition: glew.h:5051
#define M_PIf
math::TPose3D eNosePoseOnTheRobot
The pose of the sensors on the robot.
#define MRPT_END
float advectionFreq
Indicates if wind information must be used to simulate Advection.
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
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, ...)
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
std::vector< TRandomFieldCell > m_map
The cells.
Definition: CDynamicGrid.h:43
Transparently opens a compressed "gz" file and reads uncompressed data from it.
This class implements a high-performance stopwatch.
Definition: CTicTac.h:24
mrpt::utils::CDynamicGrid< double > windGrid_module
Gridmaps of the wind Direction/Module.
std::vector< std::vector< std::vector< TGaussianCell > > > * table
double getXMin() const
Returns the "x" coordinate of left side of grid map.
Definition: CDynamicGrid.h:226
bool simulateAdvection(const double &STD_increase_value)
Implements the transition model of the gasConcentration map using the information of the wind maps...
This namespace contains representation of robot actions and observations.
std::string BASE_IMPEXP format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:21
GLhandleARB obj
Definition: glew.h:3276
virtual void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
Returns a 3D object representing the map (mean)
double getYMax() const
Returns the "y" coordinate of bottom side of grid map.
Definition: CDynamicGrid.h:235
int version
Definition: mrpt_jpeglib.h:898
Declares a class derived from "CObservation" that represents a set of readings from gas sensors...
typedef int(WINAPI *PFNWGLRELEASEPBUFFERDCARBPROC)(HPBUFFERARB hPbuffer
T * cellByIndex(unsigned int cx, unsigned int cy)
Returns a pointer to the contents of a cell given by its cell indexes, or NULL if it is out of the ma...
Definition: CDynamicGrid.h:203
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
bool m_hasToRecoverMeanAndCov
Only for the KF2 implementation.
mrpt::system::TTimeStamp timeLastSimulated
The timestamp of the last time the advection simulation was executed.
virtual void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
Returns a 3D object representing the map.
void readFromStream(mrpt::utils::CStream &in, int version)
Introduces a pure virtual method responsible for loading from a CStream This can not be used directly...
#define MRPT_START
std::string sensorLabel
An arbitrary label that can be used to identify the sensor.
unsigned __int64 uint64_t
Definition: rptypes.h:52
#define MRPT_LOAD_CONFIG_VAR(variableName, variableType, configFileObject, sectionNameStr)
An useful macro for loading variables stored in a INI-like file under a key with the same name that t...
This 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...
#define CFileGZOutputStream
Saves data to a file and transparently compress the data using the given compression level...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
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.
A class used to store a 2D pose, including the 2D coordinate point and a heading (phi) angle...
Definition: CPose2D.h:36
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
Declares a class that represents any robot&#39;s observation.
bool fileOpenCorrectly()
Returns true if the file was open without errors.
std::vector< TObservationENose > m_readings
One entry per e-nose on the robot.
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::utils::CSerializable) is of t...
Definition: CObject.h:103
bool internal_insertObservation(const mrpt::obs::CObservation *obs, const mrpt::poses::CPose3D *robotPose=NULL) MRPT_OVERRIDE
Internal method called by insertObservation()
void dyngridcommon_writeToStream(mrpt::utils::CStream &out) const
Definition: CDynamicGrid.h:295
void BASE_IMPEXP 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:129
std::string windSensorLabel
The label of the WindSenor used to simulate advection.
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.
double Tac()
Stops the stopwatch.
Definition: CTicTac.cpp:92
GLdouble GLdouble GLdouble r
Definition: glew.h:1311
#define ASSERT_(f)
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...
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
void setSize(const double x_min, const double x_max, const double y_min, const double y_max, const double resolution, const T *fill_value=NULL)
Changes the size of the grid, ERASING all previous contents.
Definition: CDynamicGrid.h:69
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. ...
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
double BASE_IMPEXP 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.cpp:205
#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 ...
GLdouble GLdouble GLdouble b
Definition: glew.h:5092
void dumpToTextStream_map_specific(mrpt::utils::CStream &out) const
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:30
unsigned __int32 uint32_t
Definition: rptypes.h:49
void dyngridcommon_readFromStream(mrpt::utils::CStream &in, bool cast_from_float=false)
Definition: CDynamicGrid.h:300
Lightweight 2D point.
void recoverMeanAndCov() const
In the KF2 implementation, takes the auxiliary matrices and from them update the cells&#39; mean and std ...
static mrpt::maps::CMetricMap * internal_CreateFromMapDefinition(const mrpt::maps::TMetricMapInitializer &def)
float KF_initialCellStd
The initial standard deviation of each cell&#39;s concentration (will be stored both at each cell&#39;s struc...
void internal_clear() MRPT_OVERRIDE
Erase all the contents of the map.
double getResolution() const
Returns the resolution of the grid map.
Definition: CDynamicGrid.h:238
GLsizei GLsizei GLchar * source
Definition: glew.h:1739
float std_windNoise_phi
Frequency for simulating advection (only used to transform wind speed to distance) ...
float default_wind_direction
The std to consider on wind information measurements.
double GMRF_lambdaObsLoss
The loss of information of the observations with each iteration.
virtual int printf(const char *fmt,...) MRPT_printf_format_check(2
Writes a string to the stream in a textual form.
Definition: CStream.cpp:507
mrpt::maps::CGasConcentrationGridMap2D::TInsertionOptions insertionOptions
virtual void internal_clear() MRPT_OVERRIDE
Erase all the contents of the map.
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 ...
GLdouble GLdouble GLdouble GLdouble GLdouble GLdouble f
Definition: glew.h:5092
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:246



Page generated by Doxygen 1.8.11 for MRPT 1.5.7 Git: cdb1297 Tue Jun 12 13:44:11 2018 +0200 at mar jun 12 15:30:13 CEST 2018