Main MRPT website > C++ reference for MRPT 1.5.7
CRandomFieldGridMap2D.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
11 
13 #include <mrpt/maps/CSimpleMap.h>
15 #include <mrpt/system/os.h>
16 #include <mrpt/math/CMatrix.h>
17 #include <mrpt/math/utils.h>
18 #include <mrpt/utils/CTicTac.h>
19 #include <mrpt/utils/CTimeLogger.h>
20 #include <mrpt/utils/color_maps.h>
21 #include <mrpt/utils/round.h>
25 
26 #include <numeric>
27 
28 using namespace mrpt;
29 using namespace mrpt::maps;
30 using namespace mrpt::math;
31 using namespace mrpt::obs;
32 using namespace mrpt::utils;
33 using namespace mrpt::poses;
34 using namespace mrpt::system;
35 using namespace std;
36 
38 
39 /*---------------------------------------------------------------
40  Constructor
41  ---------------------------------------------------------------*/
43  TMapRepresentation mapType,
44  double x_min, double x_max,
45  double y_min, double y_max,
46  double resolution
47  ) :
48  CDynamicGrid<TRandomFieldCell>( x_min,x_max,y_min,y_max,resolution ),
49  COutputLogger("CRandomFieldGridMap2D"),
50  m_rfgm_run_update_upon_clear(true),
51  m_insertOptions_common( NULL ),
52  m_mapType(mapType),
53  m_cov(0,0),
54  m_hasToRecoverMeanAndCov(true),
55  m_DM_lastCutOff(0),
56  m_average_normreadings_mean(0),
57  m_average_normreadings_var(0),
58  m_average_normreadings_count(0)
59 {
60  // We can't set "m_insertOptions_common" here via "getCommonInsertOptions()" since
61  // it's a pure virtual method and we're at the constructor.
62  // We need all derived classes to call ::clear() in their constructors so we reach internal_clear()
63  // and set there that variable...
64 }
65 
67 {
68 }
69 
70 /** Changes the size of the grid, erasing previous contents. \sa resize */
71 void CRandomFieldGridMap2D::setSize(const double x_min, const double x_max, const double y_min, const double y_max, const double resolution, const TRandomFieldCell * fill_value)
72 {
73  CDynamicGrid<TRandomFieldCell>::setSize(x_min,x_max,y_min,y_max,resolution,fill_value);
75 }
76 
78 {
79  m_gmrf_connectivity = new_connectivity_descriptor;
80 }
81 
82 /*---------------------------------------------------------------
83  clear
84  ---------------------------------------------------------------*/
86 {
87  // (Read the note in the constructor above)
88  m_insertOptions_common = getCommonInsertOptions(); // Get the pointer from child class
89 
90  m_average_normreadings_mean = 0;
91  m_average_normreadings_var = 0;
92  m_average_normreadings_count = 0;
93 
94  switch (m_mapType)
95  {
96  case mrKernelDM:
97  case mrKernelDMV:
98  {
99  // Set the grid to initial values:
100  TRandomFieldCell def(0,0);
101  fill( def );
102  }
103  break;
104 
105  case mrKalmanFilter:
106  {
107  MRPT_LOG_DEBUG_FMT("[clear] Setting covariance matrix to %ux%u\n",(unsigned int)(m_size_y*m_size_x),(unsigned int)(m_size_y*m_size_x));
108 
109  TRandomFieldCell def(
110  m_insertOptions_common->KF_defaultCellMeanValue, // mean
111  m_insertOptions_common->KF_initialCellStd // std
112  );
113 
114  fill( def );
115 
116  // Reset the covariance matrix:
117  m_cov.setSize( m_size_y*m_size_x, m_size_y*m_size_x );
118 
119 
120  // And load its default values:
121  const double KF_covSigma2 = square(m_insertOptions_common->KF_covSigma);
122  const double res2 = square(m_resolution);
123  const double std0sqr = square( m_insertOptions_common->KF_initialCellStd );
124 
125  for (size_t i = 0;i<m_cov.getRowCount();i++)
126  {
127  int cx1 = ( i % m_size_x );
128  int cy1 = ( i / m_size_x );
129 
130  for (size_t j = i;j<m_cov.getColCount();j++)
131  {
132  int cx2 = ( j % m_size_x );
133  int cy2 = ( j / m_size_x );
134 
135  if (i==j)
136  {
137  m_cov(i,j) = std0sqr;
138  }
139  else
140  {
141  m_cov(i,j) = std0sqr * exp( -0.5 * (res2 * static_cast<double>(square(cx1-cx2) + square(cy1-cy2)))/KF_covSigma2 );
142  m_cov(j,i) = m_cov(i,j);
143  }
144  } // for j
145  } // for i
146 
147  //m_cov.saveToTextFile("cov_init.txt",1);
148  }
149  break;
150  // and continue with:
151  case mrKalmanApproximate:
152  {
153  m_hasToRecoverMeanAndCov = true;
154 
155  CTicTac tictac;
156  tictac.Tic();
157 
158  MRPT_LOG_DEBUG("[CRandomFieldGridMap2D::clear] Resetting compressed cov. matrix and cells\n");
159  TRandomFieldCell def(
160  m_insertOptions_common->KF_defaultCellMeanValue, // mean
161  m_insertOptions_common->KF_initialCellStd // std
162  );
163 
164  fill( def );
165 
166  // Reset the covariance matrix:
167  // --------------------------------------
168  const signed W = m_insertOptions_common->KF_W_size;
169  const size_t N = m_map.size();
170  const size_t K = 2*W*(W+1)+1;
171 
172  const double KF_covSigma2 = square(m_insertOptions_common->KF_covSigma);
173  const double std0sqr = square( m_insertOptions_common->KF_initialCellStd );
174  const double res2 = square(m_resolution);
175 
176  m_stackedCov.setSize( N, K );
177 
178  // Populate it with the initial cov. values:
179  // ------------------------------------------
180  signed Acx, Acy;
181  const double *ptr_first_row = m_stackedCov.get_unsafe_row(0);
182 
183  for (size_t i=0;i<N;i++)
184  {
185  double *ptr = m_stackedCov.get_unsafe_row(i);
186 
187  if (i==0)
188  {
189  // 1) current cell
190  *ptr++ = std0sqr;
191 
192  // 2) W rest of the first row:
193  Acy = 0;
194  for (Acx=1;Acx<=W;Acx++)
195  *ptr++ = std0sqr * exp( -0.5 * (res2 * static_cast<double>(square(Acx) + square(Acy)))/KF_covSigma2 );
196 
197  // 3) The others W rows:
198  for (Acy=1;Acy<=W;Acy++)
199  for (Acx=-W;Acx<=W;Acx++)
200  *ptr++ = std0sqr * exp( -0.5 * (res2 * static_cast<double>(square(Acx) + square(Acy)))/KF_covSigma2 );
201  }
202  else
203  {
204  // Just copy the same:
205  memcpy( ptr,ptr_first_row, sizeof(double)*K );
206  }
207  }
208 
209  MRPT_LOG_DEBUG_FMT("[clear] %ux%u cells done in %.03fms\n", unsigned(m_size_x),unsigned(m_size_y),1000*tictac.Tac());
210  }
211  break;
212 
213  case mrGMRF_SD:
214  {
215  CTicTac tictac;
216  tictac.Tic();
217 
218  MRPT_LOG_DEBUG("[clear] Generating Prior based on 'Squared Differences'\n");
219  MRPT_LOG_DEBUG_FMT("[clear] Initial map dimension: %u cells, x=(%.2f,%.2f) y=(%.2f,%.2f)\n", static_cast<unsigned int>(m_map.size()), getXMin(),getXMax(),getYMin(),getYMax());
220 
221  // Set the gridmap (m_map) to initial values:
222  TRandomFieldCell def(0,0); // mean, std
223  fill( def );
224 
226  float res_coef = 1.f; // Default value
227 
228  if (this->m_insertOptions_common->GMRF_use_occupancy_information)
229  { //Load Occupancy Gridmap and resize
230  if( !m_insertOptions_common->GMRF_simplemap_file.empty() )
231  {
232  mrpt::maps::CSimpleMap simpleMap;
233  CFileGZInputStream(this->m_insertOptions_common->GMRF_simplemap_file) >> simpleMap;
234  ASSERT_(!simpleMap.empty())
235  m_Ocgridmap.loadFromSimpleMap(simpleMap);
236  res_coef = this->getResolution() / m_Ocgridmap.getResolution();
237  }
238  else if( !m_insertOptions_common->GMRF_gridmap_image_file.empty() )
239  {
240  //Load from image
241  const bool grid_loaded_ok = m_Ocgridmap.loadFromBitmapFile(this->m_insertOptions_common->GMRF_gridmap_image_file,this->m_insertOptions_common->GMRF_gridmap_image_res,this->m_insertOptions_common->GMRF_gridmap_image_cx,this->m_insertOptions_common->GMRF_gridmap_image_cy);
242  ASSERT_(grid_loaded_ok);
243  res_coef = this->getResolution() / this->m_insertOptions_common->GMRF_gridmap_image_res;
244  }
245  else {
246  THROW_EXCEPTION("Neither `simplemap_file` nor `gridmap_image_file` found in config/mission file. Quitting.");
247  }
248 
249  //Resize MRF Map to match Occupancy Gridmap dimmensions
250  MRPT_LOG_DEBUG("Resizing m_map to match Occupancy Gridmap dimensions");
251 
252  resize(m_Ocgridmap.getXMin(),m_Ocgridmap.getXMax(),m_Ocgridmap.getYMin(),m_Ocgridmap.getYMax(),def,0.0);
253 
254  MRPT_LOG_DEBUG_FMT("Occupancy Gridmap dimensions: x=(%.2f,%.2f)m y=(%.2f,%.2f)m \n",m_Ocgridmap.getXMin(),m_Ocgridmap.getXMax(),m_Ocgridmap.getYMin(),m_Ocgridmap.getYMax());
255  MRPT_LOG_DEBUG_FMT("Occupancy Gridmap dimensions: %u cells, cx=%i cy=%i\n\n", static_cast<unsigned int>(m_Ocgridmap.getSizeX()*m_Ocgridmap.getSizeY()), m_Ocgridmap.getSizeX(), m_Ocgridmap.getSizeY());
256  MRPT_LOG_DEBUG_FMT("New map dimensions: %u cells, x=(%.2f,%.2f) y=(%.2f,%.2f)\n", static_cast<unsigned int>(m_map.size()), getXMin(),getXMax(),getYMin(),getYMax());
257  MRPT_LOG_DEBUG_FMT("New map dimensions: %u cells, cx=%u cy=%u\n", static_cast<unsigned int>(m_map.size()), static_cast<unsigned int>(getSizeX()), static_cast<unsigned int>(getSizeY()));
258 
259  m_Ocgridmap.saveAsBitmapFile("./obstacle_map_from_MRPT_for_GMRF.png");
260  }
261 
262  //m_map number of cells
263  const size_t nodeCount = m_map.size();
264 
265  //Set initial factors: L "prior factors" + 0 "Observation factors"
266  const size_t nPriorFactors = (this->getSizeX() -1) * this->getSizeY() + this->getSizeX() * (this->getSizeY() -1); // L = (Nr-1)*Nc + Nr*(Nc-1); Full connected
267 
268  MRPT_LOG_DEBUG_STREAM( "[clear] Generating " << nPriorFactors << " prior factors for a map size of N=" << nodeCount << endl);
269 
270  m_gmrf.clear();
271  m_gmrf.initialize(nodeCount);
272 
273  m_mrf_factors_activeObs.clear();
274  m_mrf_factors_activeObs.resize(nodeCount); // All cells, no observation
275 
276  m_mrf_factors_priors.clear();
277 
278  //-------------------------------------
279  // Load default values for H_prior:
280  //-------------------------------------
281  if (!m_gmrf_connectivity.present() && this->m_insertOptions_common->GMRF_use_occupancy_information)
282  {
283  MRPT_LOG_DEBUG("LOADING PRIOR BASED ON OCCUPANCY GRIDMAP \n");
284  MRPT_LOG_DEBUG_FMT("MRF Map Dimmensions: %u x %u cells \n", static_cast<unsigned int>(m_size_x), static_cast<unsigned int>(m_size_y));
285  MRPT_LOG_DEBUG_FMT("Occupancy map Dimmensions: %i x %i cells \n", m_Ocgridmap.getSizeX(), m_Ocgridmap.getSizeY());
286  MRPT_LOG_DEBUG_FMT("Res_Coeff = %f pixels/celda",res_coef);
287 
288  //Use region growing algorithm to determine the cell interconnections (Factors)
289  size_t cx = 0;
290  size_t cy = 0;
291 
292  size_t cxoj_min, cxoj_max, cyoj_min, cyoj_max, seed_cxo, seed_cyo; //Cell j limits in the Occupancy
293  size_t cxoi_min, cxoi_max, cyoi_min, cyoi_max, objective_cxo, objective_cyo; //Cell i limits in the Occupancy
294  size_t cxo_min, cxo_max, cyo_min, cyo_max; //Cell i+j limits in the Occupancy
295  //bool first_obs = false;
296 
297  std::multimap<size_t, size_t> cell_interconnections; //Store the interconnections (relations) of each cell with its neighbourds
298 
299  for (size_t j=0; j<nodeCount; j++) //For each cell in the map
300  {
301  // Get cell_j indx-limits in Occuppancy gridmap
302  cxoj_min = floor(cx*res_coef);
303  cxoj_max = cxoj_min + ceil(res_coef-1);
304  cyoj_min = floor(cy*res_coef);
305  cyoj_max = cyoj_min + ceil(res_coef-1);
306 
307  seed_cxo = cxoj_min + ceil(res_coef/2-1);
308  seed_cyo = cyoj_min + ceil(res_coef/2-1);
309 
310  //If cell occpuped then add fake observation: to allow all cells having a solution
311  if ( m_Ocgridmap.getCell(seed_cxo,seed_cyo) < 0.5 )
312  {
313  TObservationGMRF new_obs(*this);
314  new_obs.node_id = j;
315  new_obs.obsValue = 0.0;
316  new_obs.Lambda = 10e-5;
317  new_obs.time_invariant = true; //Obs that will not dissapear with time.
318  m_mrf_factors_activeObs[j].push_back(new_obs);
319  m_gmrf.addConstraint(*m_mrf_factors_activeObs[j].rbegin()); // add to graph
320  }
321 
322  //Factor with the right node: H_ji = - Lamda_prior
323  //Factor with the upper node: H_ji = - Lamda_prior
324  //-------------------------------------------------
325  for (int neighbor=0;neighbor<2; neighbor++)
326  {
327  size_t i, cxi, cyi;
328 
329  if (neighbor == 0) {
330  if (cx >= (m_size_x - 1))
331  continue;
332  i = j + 1;
333  cxi = cx + 1;
334  cyi = cy;
335  }
336 
337  if (neighbor == 1) {
338  if (cy >= (m_size_y - 1))
339  continue;
340  i = j + m_size_x;
341  cxi = cx;
342  cyi = cy + 1;
343  }
344 
345  // Get cell_i indx-limits in Occuppancy gridmap
346  cxoi_min = floor(cxi*res_coef);
347  cxoi_max = cxoi_min + ceil(res_coef-1);
348  cyoi_min = floor(cyi*res_coef);
349  cyoi_max = cyoi_min + ceil(res_coef-1);
350 
351  objective_cxo = cxoi_min + ceil(res_coef/2-1);
352  objective_cyo = cyoi_min + ceil(res_coef/2-1);
353 
354  //Get overall indx of both cells together
355  cxo_min = min(cxoj_min, cxoi_min );
356  cxo_max = max(cxoj_max, cxoi_max );
357  cyo_min = min(cyoj_min, cyoi_min );
358  cyo_max = max(cyoj_max, cyoi_max );
359 
360  //Check using Region growing if cell j is connected to cell i (Occupancy gridmap)
361  if( exist_relation_between2cells(&m_Ocgridmap, cxo_min,cxo_max,cyo_min,cyo_max,seed_cxo,seed_cyo,objective_cxo,objective_cyo))
362  {
363  TPriorFactorGMRF new_prior(*this);
364  new_prior.node_id_i = i;
365  new_prior.node_id_j = j;
366  new_prior.Lambda = m_insertOptions_common->GMRF_lambdaPrior;
367 
368  m_mrf_factors_priors.push_back(new_prior);
369  m_gmrf.addConstraint(*m_mrf_factors_priors.rbegin()); // add to graph
370 
371  //Save relation between cells
372  cell_interconnections.insert ( std::pair<size_t,size_t>(j,i) );
373  cell_interconnections.insert ( std::pair<size_t,size_t>(i,j) );
374  }
375 
376  } // end for 2 neighbors
377 
378  // Increment j coordinates (row(x), col(y))
379  if (++cx>=m_size_x)
380  {
381  cx=0;
382  cy++;
383  }
384  } // end for "j"
385  }
386  else
387  {
388  ConnectivityDescriptor * custom_connectivity = m_gmrf_connectivity.pointer(); // Use a raw ptr to avoid the cost in the inner loops
389  if (custom_connectivity!=NULL)
390  MRPT_LOG_DEBUG("[CRandomFieldGridMap2D::clear] Initiating prior (using user-supplied connectivity pattern)");
391  else MRPT_LOG_DEBUG("[CRandomFieldGridMap2D::clear] Initiating prior (fully connected)");
392 
393  //---------------------------------------------------------------
394  // Load default values for H_prior without Occupancy information:
395  //---------------------------------------------------------------
396  size_t cx = 0, cy = 0;
397  for (size_t j=0; j<nodeCount; j++)
398  {
399  // add factors between this node and:
400  // 1) the right node: j +1
401  // 2) the bottom node: j+m_size_x
402  //-------------------------------------------------
403  const size_t c_idx_to_check[2] = { cx,cy };
404  const size_t c_idx_to_check_limits[2] = { m_size_x - 1,m_size_y - 1 };
405  const size_t c_neighbor_idx_incr[2] = { 1,m_size_x };
406 
407  for (int dir = 0; dir < 2; dir++)
408  {
409  if (c_idx_to_check[dir] >= c_idx_to_check_limits[dir])
410  continue;
411 
412  const size_t i = j + c_neighbor_idx_incr[dir];
413  ASSERT_(i<nodeCount);
414 
415 
416  double edge_lamdba = .0;
417  if (custom_connectivity != NULL)
418  {
419  const bool is_connected = custom_connectivity->getEdgeInformation(
420  this,
421  cx, cy,
422  cx + (dir==0 ? 1:0), cy + (dir == 1 ? 1 : 0),
423  edge_lamdba
424  );
425  if (!is_connected)
426  continue;
427  }
428  else
429  {
430  edge_lamdba = m_insertOptions_common->GMRF_lambdaPrior;
431  }
432  TPriorFactorGMRF new_prior(*this);
433  new_prior.node_id_i = i;
434  new_prior.node_id_j = j;
435  new_prior.Lambda = edge_lamdba;
436 
437  m_mrf_factors_priors.push_back(new_prior);
438  m_gmrf.addConstraint(*m_mrf_factors_priors.rbegin()); // add to graph
439  }
440 
441  // Increment j coordinates (row(x), col(y))
442  if (++cx>=m_size_x) {
443  cx=0;
444  cy++;
445  }
446  } // end for "j"
447  } // end if_use_Occupancy
448 
449  MRPT_LOG_DEBUG_STREAM( "[clear] Prior built in " << tictac.Tac() << " s ----------");
450 
451  if (m_rfgm_run_update_upon_clear) {
452  //Solve system and update map estimation
453  updateMapEstimation_GMRF();
454  }
455 
456  }//end case
457  break;
458  default:
459  cerr << "MAP TYPE NOT RECOGNIZED... QUITTING" << endl;
460  break;
461  } //end switch
462 }
463 
464 /*---------------------------------------------------------------
465  isEmpty
466  ---------------------------------------------------------------*/
468 {
469  return false;
470 }
471 
472 
473 /*---------------------------------------------------------------
474  insertObservation_KernelDM_DMV
475  ---------------------------------------------------------------*/
476 /** The implementation of "insertObservation" for Achim Lilienthal's map models DM & DM+V.
477 * \param normReading Is a [0,1] normalized concentration reading.
478 * \param is_DMV = false -> map type is Kernel DM; true -> map type is DM+V
479 */
481  double normReading,
482  const mrpt::math::TPoint2D &point,
483  bool is_DMV )
484 {
485  MRPT_START
486 
487  static const TRandomFieldCell defCell(0,0);
488 
489  // Assure we have room enough in the grid!
490  resize( point.x - m_insertOptions_common->cutoffRadius*2,
491  point.x + m_insertOptions_common->cutoffRadius*2,
492  point.y - m_insertOptions_common->cutoffRadius*2,
493  point.y + m_insertOptions_common->cutoffRadius*2,
494  defCell );
495 
496  // Compute the "parzen Gaussian" once only:
497  // -------------------------------------------------
498  int Ac_cutoff = round(m_insertOptions_common->cutoffRadius / m_resolution);
499  unsigned Ac_all = 1+2*Ac_cutoff;
500  double minWinValueAtCutOff = exp(-square(m_insertOptions_common->cutoffRadius/m_insertOptions_common->sigma) );
501 
502  if ( m_DM_lastCutOff!=m_insertOptions_common->cutoffRadius ||
503  m_DM_gaussWindow.size() != square(Ac_all) )
504  {
505  MRPT_LOG_DEBUG_FMT("[CRandomFieldGridMap2D::insertObservation_KernelDM_DMV] Precomputing window %ux%u\n",Ac_all,Ac_all);
506 
507  double dist;
508  double std = m_insertOptions_common->sigma;
509 
510  // Compute the window:
511  m_DM_gaussWindow.resize(Ac_all*Ac_all);
512  m_DM_lastCutOff=m_insertOptions_common->cutoffRadius;
513 
514  // Actually the array could be 1/4 of this size, but this
515  // way it's easier and it's late night now :-)
516  vector<float>::iterator it = m_DM_gaussWindow.begin();
517  for (unsigned cx=0;cx<Ac_all;cx++)
518  {
519  for (unsigned cy=0;cy<Ac_all;cy++)
520  {
521  dist = m_resolution * sqrt( static_cast<double>(square( Ac_cutoff+1-cx ) + square( Ac_cutoff+1-cy ) ) );
522  *(it++) = std::exp( - square(dist/std) );
523  }
524  }
525 
526  MRPT_LOG_DEBUG("[CRandomFieldGridMap2D::insertObservation_KernelDM_DMV] Done!");
527  } // end of computing the gauss. window.
528 
529  // Fuse with current content of grid (the MEAN of each cell):
530  // --------------------------------------------------------------
531  const int sensor_cx = x2idx( point.x );
532  const int sensor_cy = y2idx( point.y );
533  TRandomFieldCell *cell;
534  vector<float>::iterator windowIt = m_DM_gaussWindow.begin();
535 
536  for (int Acx=-Ac_cutoff;Acx<=Ac_cutoff;Acx++)
537  {
538  for (int Acy=-Ac_cutoff;Acy<=Ac_cutoff;++Acy, ++windowIt)
539  {
540  const double windowValue = *windowIt;
541 
542  if (windowValue>minWinValueAtCutOff)
543  {
544  cell = cellByIndex(sensor_cx+Acx,sensor_cy+Acy);
545  ASSERT_( cell!=NULL )
546 
547  cell->dm_mean_w += windowValue;
548  cell->dm_mean += windowValue * normReading;
549  if (is_DMV)
550  {
551  const double cell_var = square(normReading - computeMeanCellValue_DM_DMV(cell) );
552  cell->dmv_var_mean += windowValue * cell_var;
553  }
554  }
555  }
556  }
557 
558  MRPT_END
559 }
560 
561 
562 /*---------------------------------------------------------------
563  TInsertionOptionsCommon
564  ---------------------------------------------------------------*/
566  sigma ( 0.15f ),
567  cutoffRadius ( sigma * 3.0 ),
568  R_min ( 0 ),
569  R_max ( 3 ),
570  dm_sigma_omega ( 0.05 ), // See IROS 2009 paper (a scale parameter for the confidence)
571 
572  KF_covSigma ( 0.35f ), // in meters
573  KF_initialCellStd ( 1.0 ), // std in normalized concentration units
574  KF_observationModelNoise ( 0 ), // in normalized concentration units
575  KF_defaultCellMeanValue ( 0 ),
576  KF_W_size ( 4 ),
577 
578  GMRF_lambdaPrior ( 0.01f ), // [GMRF model] The information (Lambda) of fixed map constraints
579  GMRF_lambdaObs ( 10.0f ), // [GMRF model] The initial information (Lambda) of each observation (this information will decrease with time)
580  GMRF_lambdaObsLoss ( 0.0f ), //!< The loss of information of the observations with each iteration
581 
582  GMRF_use_occupancy_information ( false ),
583  GMRF_simplemap_file ( "" ),
584  GMRF_gridmap_image_file ( "" ),
585  GMRF_gridmap_image_res ( 0.01f ),
586  GMRF_gridmap_image_cx ( 0 ),
587  GMRF_gridmap_image_cy ( 0 ),
588 
589  GMRF_saturate_min ( -std::numeric_limits<double>::max() ),
590  GMRF_saturate_max ( std::numeric_limits<double>::max() ),
591  GMRF_skip_variance (false)
592 {
593 }
594 
595 /*---------------------------------------------------------------
596  internal_dumpToTextStream_common
597  ---------------------------------------------------------------*/
599 {
600  out.printf("sigma = %f\n", sigma);
601  out.printf("cutoffRadius = %f\n", cutoffRadius);
602  out.printf("R_min = %f\n", R_min);
603  out.printf("R_max = %f\n", R_max);
604  out.printf("dm_sigma_omega = %f\n", dm_sigma_omega);
605 
606  out.printf("KF_covSigma = %f\n", KF_covSigma);
607  out.printf("KF_initialCellStd = %f\n", KF_initialCellStd);
608  out.printf("KF_observationModelNoise = %f\n", KF_observationModelNoise);
609  out.printf("KF_defaultCellMeanValue = %f\n", KF_defaultCellMeanValue);
610  out.printf("KF_W_size = %u\n", (unsigned)KF_W_size);
611 
612  out.printf("GMRF_lambdaPrior = %f\n", GMRF_lambdaPrior);
613  out.printf("GMRF_lambdaObs = %f\n", GMRF_lambdaObs);
614  out.printf("GMRF_lambdaObsLoss = %f\n", GMRF_lambdaObs);
615 
616  out.printf("GMRF_use_occupancy_information = %s\n", GMRF_use_occupancy_information ? "YES":"NO" );
617  out.printf("GMRF_simplemap_file = %s\n", GMRF_simplemap_file.c_str());
618  out.printf("GMRF_gridmap_image_file = %s\n", GMRF_gridmap_image_file.c_str());
619  out.printf("GMRF_gridmap_image_res = %f\n", GMRF_gridmap_image_res);
620  out.printf("GMRF_gridmap_image_cx = %u\n", static_cast<unsigned int>(GMRF_gridmap_image_cx));
621  out.printf("GMRF_gridmap_image_cy = %u\n", static_cast<unsigned int>(GMRF_gridmap_image_cy));
622 }
623 
624 /*---------------------------------------------------------------
625  internal_loadFromConfigFile_common
626  ---------------------------------------------------------------*/
628  const mrpt::utils::CConfigFileBase &iniFile,
629  const std::string &section)
630 {
631  sigma = iniFile.read_float(section.c_str(),"sigma",sigma);
632  cutoffRadius = iniFile.read_float(section.c_str(),"cutoffRadius",cutoffRadius);
633  R_min = iniFile.read_float(section.c_str(),"R_min",R_min);
634  R_max = iniFile.read_float(section.c_str(),"R_max",R_max);
635  MRPT_LOAD_CONFIG_VAR(dm_sigma_omega, double, iniFile, section );
636 
637  KF_covSigma = iniFile.read_float(section.c_str(),"KF_covSigma",KF_covSigma);
638  KF_initialCellStd = iniFile.read_float(section.c_str(),"KF_initialCellStd",KF_initialCellStd);
639  KF_observationModelNoise= iniFile.read_float(section.c_str(),"KF_observationModelNoise",KF_observationModelNoise);
640  KF_defaultCellMeanValue = iniFile.read_float(section.c_str(),"KF_defaultCellMeanValue",KF_defaultCellMeanValue);
641  MRPT_LOAD_CONFIG_VAR(KF_W_size, int, iniFile, section );
642 
643  GMRF_lambdaPrior = iniFile.read_float(section.c_str(),"GMRF_lambdaPrior",GMRF_lambdaPrior);
644  GMRF_lambdaObs = iniFile.read_float(section.c_str(),"GMRF_lambdaObs",GMRF_lambdaObs);
645  GMRF_lambdaObsLoss = iniFile.read_float(section.c_str(),"GMRF_lambdaObsLoss",GMRF_lambdaObsLoss);
646 
647  GMRF_use_occupancy_information = iniFile.read_bool(section.c_str(),"GMRF_use_occupancy_information",false,false);
648  GMRF_simplemap_file = iniFile.read_string(section.c_str(),"simplemap_file","",false);
649  GMRF_gridmap_image_file = iniFile.read_string(section.c_str(),"gridmap_image_file","",false);
650  GMRF_gridmap_image_res = iniFile.read_float(section.c_str(),"gridmap_image_res",0.01f,false);
651  GMRF_gridmap_image_cx = iniFile.read_int(section.c_str(),"gridmap_image_cx",0,false);
652  GMRF_gridmap_image_cy = iniFile.read_int(section.c_str(),"gridmap_image_cy",0,false);
653 }
654 
655 
656 
657 /*---------------------------------------------------------------
658  saveAsBitmapFile
659  ---------------------------------------------------------------*/
661 {
662  MRPT_START
663 
665  getAsBitmapFile(img);
666  img.saveToFile(filName);
667 
668  MRPT_END
669 }
670 
671 /** Like saveAsBitmapFile(), but returns the data in matrix form */
673 {
674  MRPT_START
675  cells_mat.resize(m_size_y,m_size_x);
676  recoverMeanAndCov(); // Only has effects for KF2 method
677 
678  for (unsigned int y=0;y<m_size_y;y++)
679  {
680  for (unsigned int x=0;x<m_size_x;x++)
681  {
682  const TRandomFieldCell *cell = cellByIndex(x,y);
683  ASSERT_( cell!=NULL );
684  double c;
685 
686  switch (m_mapType)
687  {
688  case mrKernelDM:
689  case mrKernelDMV:
690  c = computeMeanCellValue_DM_DMV(cell);
691  break;
692 
693  case mrKalmanFilter:
694  case mrKalmanApproximate:
695  c = cell->kf_mean;
696  break;
697  case mrGMRF_SD:
698  c = cell->gmrf_mean;
699  break;
700 
701  default:
702  THROW_EXCEPTION("Unknown m_mapType!!");
703  };
705  cells_mat(m_size_y-1-y,x) = c;
706  }
707  }
708  MRPT_END
709 }
710 
711 /*---------------------------------------------------------------
712  getAsBitmapFile
713  ---------------------------------------------------------------*/
715 {
716  MRPT_START
717  mrpt::math::CMatrixDouble cells_mat;
718  getAsMatrix(cells_mat);
719  out_img.setFromMatrix(cells_mat, false /* vals are not normalized in by default [0,1] */);
720  MRPT_END
721 }
722 
723 /*---------------------------------------------------------------
724  resize
725  ---------------------------------------------------------------*/
727  double new_x_min,double new_x_max,double new_y_min,double new_y_max,
728  const TRandomFieldCell& defaultValueNewCells,
729  double additionalMarginMeters)
730 {
731  MRPT_START
732 
733  size_t old_sizeX = m_size_x;
734  size_t old_sizeY = m_size_y;
735  double old_x_min = m_x_min;
736  double old_y_min = m_y_min;
737 
738  // The parent class method:
739  CDynamicGrid<TRandomFieldCell>::resize(new_x_min,new_x_max,new_y_min,new_y_max,defaultValueNewCells,additionalMarginMeters);
740 
741  // Do we really resized?
742  if ( m_size_x != old_sizeX || m_size_y != old_sizeY )
743  {
744  // YES:
745  // If we are in a Kalman Filter representation, also build the new covariance matrix:
747  {
748  // ------------------------------------------
749  // Update the covariance matrix
750  // ------------------------------------------
751  size_t i,j,N = m_size_y*m_size_x; // The new number of cells
752  CMatrixD oldCov( m_cov ); // Make a copy
753 
754  //m_cov.saveToTextFile("__debug_cov_before_resize.txt");
755 
756  printf("[CRandomFieldGridMap2D::resize] Resizing from %ux%u to %ux%u (%u cells)\n",
757  static_cast<unsigned>(old_sizeX),
758  static_cast<unsigned>(old_sizeY),
759  static_cast<unsigned>(m_size_x),
760  static_cast<unsigned>(m_size_y),
761  static_cast<unsigned>(m_size_x*m_size_y) );
762 
763  m_cov.setSize( N,N );
764 
765  // Compute the new cells at the left and the bottom:
766  size_t Acx_left = round((old_x_min - m_x_min)/m_resolution);
767  size_t Acy_bottom = round((old_y_min - m_y_min)/m_resolution);
768 
769  // -------------------------------------------------------
770  // STEP 1: Copy the old map values:
771  // -------------------------------------------------------
772  for (i = 0;i<N;i++)
773  {
774  size_t cx1 = i % m_size_x;
775  size_t cy1 = i / m_size_x;
776 
777  bool C1_isFromOldMap = Acx_left<=cx1 && cx1<(Acx_left+old_sizeX) &&
778  Acy_bottom<=cy1 && cy1<(Acy_bottom+old_sizeY);
779 
780  if ( C1_isFromOldMap )
781  {
782  for (j = i;j<N;j++)
783  {
784  size_t cx2 = j % m_size_x;
785  size_t cy2 = j / m_size_x;
786 
787  bool C2_isFromOldMap = Acx_left<=cx2 && cx2<(Acx_left+old_sizeX) &&
788  Acy_bottom<=cy2 && cy2<(Acy_bottom+old_sizeY);
789 
790  // Were both cells in the old map??? --> Copy it!
791  if ( C1_isFromOldMap && C2_isFromOldMap )
792  {
793  // Copy values for the old matrix:
794  unsigned int idx_c1 = ( (cx1 - Acx_left) + old_sizeX * (cy1 - Acy_bottom ) );
795  unsigned int idx_c2 = ( (cx2 - Acx_left) + old_sizeX * (cy2 - Acy_bottom ) );
796 
797  MRPT_START
798 
799  ASSERT_( cx1 >= Acx_left );
800  ASSERT_( cy1 >= Acy_bottom );
801  ASSERT_( (cx1 - Acx_left )<old_sizeX );
802  ASSERT_( (cy1 - Acy_bottom )<old_sizeY );
803 
804  ASSERT_( cx2 >= Acx_left );
805  ASSERT_( cy2 >= Acy_bottom );
806  ASSERT_( (cx2 - Acx_left )<old_sizeX );
807  ASSERT_( (cy2 - Acy_bottom )<old_sizeY );
808 
809  ASSERT_( idx_c1<old_sizeX*old_sizeY );
810  ASSERT_( idx_c2<old_sizeX*old_sizeY );
811 
813  printf("cx1=%u\n",static_cast<unsigned>(cx1)); \
814  printf("cy1=%u\n",static_cast<unsigned>(cy1)); \
815  printf("cx2=%u\n",static_cast<unsigned>(cx2)); \
816  printf("cy2=%u\n",static_cast<unsigned>(cy2)); \
817  printf("Acx_left=%u\n",static_cast<unsigned>(Acx_left)); \
818  printf("Acy_bottom=%u\n",static_cast<unsigned>(Acy_bottom)); \
819  printf("idx_c1=%u\n",static_cast<unsigned>(idx_c1)); \
820  printf("idx_c2=%u\n",static_cast<unsigned>(idx_c2)); \
821  );
822 
823  m_cov(i,j) = oldCov( idx_c1, idx_c2 );
824  m_cov(j,i) = m_cov(i,j);
825 
826  if (i==j)
827  ASSERT_( idx_c1 == idx_c2 );
828 
829  if (i==j && m_cov(i,i)<0)
830  {
831  printf("\ni=%u \nj=%i \ncx1,cy1 = %u,%u \n cx2,cy2=%u,%u \nidx_c1=%u \nidx_c2=%u \nAcx_left=%u \nAcy_bottom=%u \nold_sizeX=%u \n",
832  static_cast<unsigned>(i),
833  static_cast<unsigned>(j),
834  static_cast<unsigned>(cx1),
835  static_cast<unsigned>(cy1),
836  static_cast<unsigned>(cx2),
837  static_cast<unsigned>(cy2),
838  static_cast<unsigned>(idx_c1),
839  static_cast<unsigned>(idx_c2),
840  static_cast<unsigned>(Acx_left),
841  static_cast<unsigned>(Acy_bottom),
842  static_cast<unsigned>(old_sizeX)
843  );
844  }
845  }
846 
847  ASSERT_( !isNaN( m_cov(i,j) ) );
848 
849  } // for j
850  }
851  } // for i
852 
853  // -------------------------------------------------------
854  // STEP 2: Set default values for new cells
855  // -------------------------------------------------------
856  for (i=0;i<N;i++)
857  {
858  size_t cx1 = i % m_size_x;
859  size_t cy1 = i / m_size_x;
860 
861  bool C1_isFromOldMap = Acx_left<=cx1 && cx1<(Acx_left+old_sizeX) &&
862  Acy_bottom<=cy1 && cy1<(Acy_bottom+old_sizeY);
863  for (j=i;j<N;j++)
864  {
865  size_t cx2 = j % m_size_x;
866  size_t cy2 = j / m_size_x;
867 
868  bool C2_isFromOldMap = Acx_left<=cx2 && cx2<(Acx_left+old_sizeX) &&
869  Acy_bottom<=cy2 && cy2<(Acy_bottom+old_sizeY);
870  double dist=0;
871 
872  // If both cells were NOT in the old map??? --> Introduce default values:
873  if ( !C1_isFromOldMap || !C2_isFromOldMap )
874  {
875  // Set a new starting value:
876  if (i==j)
877  {
879  }
880  else
881  {
882  dist = m_resolution*sqrt( static_cast<double>( square(cx1-cx2) + square(cy1-cy2) ));
883  double K = sqrt(m_cov(i,i)*m_cov(j,j));
884 
885  if ( isNaN( K ) )
886  {
887  printf("c(i,i)=%e c(j,j)=%e\n",m_cov(i,i),m_cov(j,j));
888  ASSERT_( !isNaN( K ) );
889  }
890 
891  m_cov(i,j) = K * exp( -0.5 * square( dist/m_insertOptions_common->KF_covSigma ) );
892  m_cov(j,i) = m_cov(i,j);
893  }
894 
895  ASSERT_( !isNaN( m_cov(i,j) ) );
896  }
897  } // for j
898  } // for i
899 
900 
901  //m_cov.saveToTextFile("__debug_cov_after_resize.txt");
902  // Resize done!
903  printf("[CRandomFieldGridMap2D::resize] Done\n");
904 
905  } // end of Kalman Filter map
906  else if (m_mapType==mrKalmanApproximate )
907  {
908  // ------------------------------------------
909  // Approximate-Kalman filter
910  // ------------------------------------------
911 
912  // Cells with "std" == -1 are new ones, we have to change their std
913  // to "m_insertOptions_common->KF_initialCellStd", then adapt appropriately
914  // the compressed cov. matrix:
915 
916  MRPT_LOG_DEBUG_FMT("[resize] Resizing from %ux%u to %ux%u (%u cells)\n",
917  static_cast<unsigned>(old_sizeX),
918  static_cast<unsigned>(old_sizeY),
919  static_cast<unsigned>(m_size_x),
920  static_cast<unsigned>(m_size_y),
921  static_cast<unsigned>(m_size_x*m_size_y) );
922 
923  // Adapt the size of the cov. matrix:
924  const signed W = m_insertOptions_common->KF_W_size;
925  const size_t N = m_map.size();
926  const size_t K = 2*W*(W+1)+1;
927  ASSERT_( K==m_stackedCov.getColCount() );
928  ASSERT_( old_sizeX*old_sizeY== m_stackedCov.getRowCount() );
929 
930  // Compute the new cells at the left and the bottom:
931  size_t Acx_left = round((old_x_min - m_x_min)/m_resolution);
932  size_t Acy_bottom = round((old_y_min - m_y_min)/m_resolution);
933 
934  m_stackedCov.setSize( N,K );
935 
936  // Prepare the template for new cells:
937  CVectorDouble template_row(K);
938  {
939  const double std0sqr = square( m_insertOptions_common->KF_initialCellStd );
940  double *ptr = &template_row[0];
941  const double res2 = square(m_resolution);
942  const double KF_covSigma2 = square(m_insertOptions_common->KF_covSigma);
943 
944  // 1) current cell
945  *ptr++ = std0sqr;
946 
947  // 2) W rest of the first row:
948  int Acx, Acy = 0;
949  for (Acx=1;Acx<=W;Acx++)
950  *ptr++ = std0sqr * exp( -0.5 * (res2 * static_cast<double>(square(Acx) + square(Acy)))/KF_covSigma2 );
951 
952  // 3) The others W rows:
953  for (Acy=1;Acy<=W;Acy++)
954  for (Acx=-W;Acx<=W;Acx++)
955  *ptr++ = std0sqr * exp( -0.5 * (res2 * static_cast<double>(square(Acx) + square(Acy)))/KF_covSigma2 );
956  }
957 
958  // Go thru all the cells, from the bottom to the top so
959  // we don't need to make a temporary copy of the covariances:
960  for (size_t i=N-1;i<N;i--) // i<N will become false for "i=-1" ;-)
961  {
962  int cx,cy;
963  idx2cxcy(i,cx,cy);
964 
965  const size_t old_idx_of_i = (cx-Acx_left) + (cy-Acy_bottom)*old_sizeX;
966 
967  TRandomFieldCell &cell = m_map[i];
968  if (cell.kf_std<0)
969  {
970  // "i" is a new cell, fix its std and fill out the compressed covariance:
972 
973  double *new_row = m_stackedCov.get_unsafe_row(i);
974  memcpy( new_row,&template_row[0], sizeof(double)*K );
975  }
976  else
977  {
978  // "i" is an existing old cell: just copy the "m_stackedCov" row:
979  ASSERT_(old_idx_of_i<m_stackedCov.getRowCount());
980  if (old_idx_of_i!=i) // Copy row only if it's moved
981  {
982  const double *ptr_old = m_stackedCov.get_unsafe_row(old_idx_of_i);
983  double *ptr_new = m_stackedCov.get_unsafe_row(i);
984  memcpy( ptr_new,ptr_old, sizeof(double)*K );
985  }
986  }
987  } // end for i
988  } // end of Kalman-Approximate map
989  }
990 
991  MRPT_END
992 }
993 
994 
995 /*---------------------------------------------------------------
996  insertObservation_KF
997  ---------------------------------------------------------------*/
999  double normReading,
1000  const mrpt::math::TPoint2D &point )
1001 {
1002  MRPT_START
1003 
1004  const TRandomFieldCell defCell(
1007  );
1008 
1009  // Assure we have room enough in the grid!
1010  resize( point.x - 1,
1011  point.x + 1,
1012  point.y - 1,
1013  point.y + 1,
1014  defCell );
1015 
1016  // --------------------------------------------------------
1017  // The Kalman-Filter estimation of the MRF grid-map:
1018  // --------------------------------------------------------
1019 
1020  // Prediction stage of KF:
1021  // ------------------------------------
1022  // Nothing to do here (static map)
1023 
1024  // Update stage of KF:
1025  // We directly apply optimized formulas arising
1026  // from our concrete sensor model.
1027  // -------------------------------------------------
1028  int cellIdx = xy2idx( point.x, point.y );
1029  TRandomFieldCell *cell = cellByPos( point.x, point.y );
1030  ASSERT_(cell!=NULL);
1031  size_t N,i,j;
1032 
1033  double yk = normReading - cell->kf_mean; // Predicted observation mean
1034  double sk = m_cov(cellIdx,cellIdx) + square(m_insertOptions_common->KF_observationModelNoise); // Predicted observation variance
1035  double sk_1 = 1.0 / sk;
1036 
1037  // The kalman gain:
1039 
1040  static CTicTac tictac;
1041  MRPT_LOG_DEBUG("[insertObservation_KF] Updating mean values...");
1042  tictac.Tic();
1043 
1044  // Update mean values:
1045  // ---------------------------------------------------------
1046  for (i=0,it = m_map.begin();it!=m_map.end();++it,++i)
1047  //it->kf_mean = it->kf_mean + yk * sk_1 * m_cov.get_unsafe(i,cellIdx);
1048  it->kf_mean += yk * sk_1 * m_cov(i,cellIdx);
1049 
1050  MRPT_LOG_DEBUG_FMT("Done in %.03fms\n", tictac.Tac() * 1000);
1051 
1052  // Update covariance matrix values:
1053  // ---------------------------------------------------------
1054  N = m_cov.getRowCount();
1055 
1056  MRPT_LOG_DEBUG("[insertObservation_KF] Updating covariance matrix...");
1057  tictac.Tic();
1058 
1059  // We need to refer to the old cov: make an efficient copy of it:
1060  double *oldCov = (double*)/*mrpt_alloca*/malloc( sizeof(double)*N*N );
1061  double *oldCov_ptr = oldCov;
1062  for (i=0;i<N;i++)
1063  {
1064  memcpy( oldCov_ptr, m_cov.get_unsafe_row(i) , sizeof(double)*N );
1065  oldCov_ptr+=N;
1066  }
1067 
1068  MRPT_LOG_DEBUG_FMT("Copy matrix %ux%u: %.06fms\n", (unsigned)m_cov.getRowCount(), (unsigned)m_cov.getColCount(), tictac.Tac()*1000 );
1069 
1070  // The following follows from the expansion of Kalman Filter matrix equations
1071  // TODO: Add references to some paper (if any)?
1072  const double *oldCov_row_c = oldCov+cellIdx*N;
1073  for (i=0;i<N;i++)
1074  {
1075  const double oldCov_i_c = oldCov[i*N+cellIdx];
1076  const double sk_1_oldCov_i_c = sk_1 * oldCov_i_c;
1077 
1078  const double *oldCov_row_i = oldCov+i*N;
1079  for (j=i;j<N;j++)
1080  {
1081  double new_cov_ij = oldCov_row_i[j] - sk_1_oldCov_i_c * oldCov_row_c[j];
1082 
1083  // Make symmetric:
1084  m_cov.set_unsafe(i,j, new_cov_ij );
1085  m_cov.set_unsafe(j,i, new_cov_ij );
1086 
1087  // Update the "std" in the cell as well:
1088  if (i==j)
1089  {
1090  if (m_cov(i,i)<0){
1091  printf("Wrong insertion in KF! m_cov(%u,%u) = %.5f",static_cast<unsigned int>(i),static_cast<unsigned int>(i),m_cov(i,i));
1092  }
1093 
1094  ASSERT_( m_cov(i,i)>=0 );
1095  m_map[ i ].kf_std = sqrt( new_cov_ij );
1096  }
1097  } // j
1098  } // i
1099 
1100  // Free mem:
1101  /*mrpt_alloca_*/ free( oldCov );
1102 
1103  MRPT_LOG_DEBUG_FMT("Done! %.03fms\n", tictac.Tac()*1000 );
1104 
1105  MRPT_END
1106 }
1107 
1108 
1109 /*---------------------------------------------------------------
1110  saveMetricMapRepresentationToFile
1111  ---------------------------------------------------------------*/
1113  const std::string &filNamePrefix ) const
1114 {
1115  std::string fil;
1116 
1117  // Save as a bitmap:
1118 #if MRPT_HAS_OPENCV
1119  fil = filNamePrefix + std::string("_mean.png");
1120  saveAsBitmapFile( fil );
1121 #endif
1122 
1123  // Save dimensions of the grid (for any mapping algorithm):
1124  CMatrix DIMs(1,4);
1125  DIMs(0,0)=m_x_min;
1126  DIMs(0,1)=m_x_max;
1127  DIMs(0,2)=m_y_min;
1128  DIMs(0,3)=m_y_max;
1129 
1130  DIMs.saveToTextFile( filNamePrefix + std::string("_grid_limits.txt"), MATRIX_FORMAT_FIXED, false /* add mrpt header */, "% Grid limits: [x_min x_max y_min y_max]\n" );
1131 
1132 
1133  switch(m_mapType)
1134  {
1135  case mrKernelDM:
1136  case mrKernelDMV:
1137  {
1138  CMatrix all_means(m_size_y,m_size_x);
1139  CMatrix all_vars(m_size_y,m_size_x);
1140  CMatrix all_confs(m_size_y,m_size_x);
1141 
1142  for (size_t y=0;y<m_size_y;y++)
1143  for (size_t x=0;x<m_size_x;x++)
1144  {
1145  const TRandomFieldCell * cell =cellByIndex(x,y);
1146  all_means(y,x) = computeMeanCellValue_DM_DMV(cell);
1147  all_vars(y,x) = computeVarCellValue_DM_DMV(cell);
1148  all_confs(y,x) = computeConfidenceCellValue_DM_DMV(cell);
1149  }
1150 
1151  all_means.saveToTextFile( filNamePrefix + std::string("_mean.txt"), MATRIX_FORMAT_FIXED );
1152  if (m_mapType == mrKernelDMV)
1153  {
1154  all_vars.saveToTextFile( filNamePrefix + std::string("_var.txt"), MATRIX_FORMAT_FIXED );
1155  all_confs.saveToTextFile( filNamePrefix + std::string("_confidence.txt"), MATRIX_FORMAT_FIXED );
1156  }
1157  }
1158  break;
1159 
1160  case mrKalmanFilter:
1161  case mrKalmanApproximate:
1162  {
1164 
1165  // Save the mean and std matrix:
1166  CMatrix MEAN( m_size_y,m_size_x);
1167  CMatrix STDs( m_size_y, m_size_x );
1168 
1169  for (size_t i=0;i<m_size_y;i++)
1170  for (size_t j=0;j<m_size_x;j++)
1171  {
1172  MEAN(i,j)=cellByIndex(j,i)->kf_mean;
1173  STDs(i,j)=cellByIndex(j,i)->kf_std;
1174  }
1175 
1176  MEAN.saveToTextFile( filNamePrefix + std::string("_mean.txt"), MATRIX_FORMAT_FIXED );
1177  STDs.saveToTextFile( filNamePrefix + std::string("_cells_std.txt"), MATRIX_FORMAT_FIXED );
1178 
1179  if ( m_mapType == mrKalmanApproximate )
1180  {
1181  m_stackedCov.saveToTextFile( filNamePrefix + std::string("_mean_compressed_cov.txt"), MATRIX_FORMAT_FIXED );
1182  }
1183  if (m_mapType == mrKalmanFilter)
1184  {
1185  // Save the covariance matrix:
1186  m_cov.saveToTextFile( filNamePrefix + std::string("_mean_cov.txt") );
1187  }
1188 
1189  // And also as bitmap:
1190  STDs.normalize();
1191  CImage img_cov(STDs, true);
1192  img_cov.saveToFile(filNamePrefix + std::string("_cells_std.png"), true /* vertical flip */);
1193 
1194  // Save the 3D graphs:
1195  saveAsMatlab3DGraph( filNamePrefix + std::string("_3D.m") );
1196  }
1197  break;
1198 
1199  case mrGMRF_SD:
1200  {
1201  // Save the mean and std matrix:
1202  CMatrix MEAN( m_size_y, m_size_x );
1203  CMatrix STDs( m_size_y, m_size_x );
1204  CMatrixD XYZ( m_size_y*m_size_x, 4 );
1205 
1206  size_t idx=0;
1207  for (size_t i=0; i<m_size_y; ++i)
1208  {
1209  for (size_t j=0; j<m_size_x; ++j,++idx)
1210  {
1211  MEAN(i,j) = cellByIndex(j,i)->gmrf_mean;
1212  STDs(i,j) = cellByIndex(j,i)->gmrf_std;
1213 
1214  XYZ(idx,0) = idx2x(j);
1215  XYZ(idx,1) = idx2y(i);
1216  XYZ(idx,2) = cellByIndex(j,i)->gmrf_mean;
1217  XYZ(idx,3) = cellByIndex(j,i)->gmrf_std;
1218  }
1219  }
1220 
1221  MEAN.saveToTextFile( filNamePrefix + std::string("_mean.txt"), MATRIX_FORMAT_FIXED );
1222  STDs.saveToTextFile( filNamePrefix + std::string("_cells_std.txt"), MATRIX_FORMAT_FIXED );
1223  XYZ.saveToTextFile( filNamePrefix + std::string("_xyz_and_std.txt"), MATRIX_FORMAT_FIXED, false, "% Columns: GRID_X GRID_Y ESTIMATED_Z STD_DEV_OF_ESTIMATED_Z \n" );
1224  }
1225  break;
1226 
1227  default:
1228  THROW_EXCEPTION("Unknown method!");
1229  }; // end switch
1230 }
1231 
1232 /*---------------------------------------------------------------
1233  saveAsMatlab3DGraph
1234  ---------------------------------------------------------------*/
1236 {
1237  MRPT_START
1238 
1239  const double std_times = 3;
1240 
1242 
1244 
1245  FILE *f= os::fopen( filName.c_str(), "wt" );
1246  if (!f)
1247  THROW_EXCEPTION("Couldn't create output file!");
1248 
1249  os::fprintf(f,"%%-------------------------------------------------------\n");
1250  os::fprintf(f,"%% File automatically generated using the MRPT method:\n");
1251  os::fprintf(f,"%%'CRandomFieldGridMap2D::saveAsMatlab3DGraph'\n");
1252  os::fprintf(f,"%%\n");
1253  os::fprintf(f,"%% ~ MRPT ~\n");
1254  os::fprintf(f,"%% Jose Luis Blanco Claraco, University of Malaga @ 2006-2007\n");
1255  os::fprintf(f,"%% http://www.isa.uma.es/ \n");
1256  os::fprintf(f,"%%-------------------------------------------------------\n\n");
1257 
1258 
1259  unsigned int cx,cy;
1260  vector<double> xs,ys;
1261 
1262  // xs: array of X-axis values
1263  os::fprintf(f,"xs = [");
1264  xs.resize( m_size_x );
1265  for (cx=0;cx<m_size_x;cx++)
1266  {
1267  xs[cx] = m_x_min + m_resolution * cx;
1268  os::fprintf(f,"%f ", xs[cx]);
1269  }
1270  os::fprintf(f,"];\n");
1271 
1272  // ys: array of X-axis values
1273  os::fprintf(f,"ys = [");
1274  ys.resize( m_size_y );
1275  for (cy=0;cy<m_size_y;cy++)
1276  {
1277  ys[cy] = m_y_min + m_resolution * cy;
1278  os::fprintf(f,"%f ", ys[cy]);
1279  }
1280  os::fprintf(f,"];\n");
1281 
1282  // z_mean: matrix with mean concentration values
1283  os::fprintf(f,"z_mean = [\n");
1284  for (cy=0;cy<m_size_y;cy++)
1285  {
1286  for (cx=0;cx<m_size_x;cx++)
1287  {
1288  const TRandomFieldCell *cell = cellByIndex( cx,cy );
1289  ASSERT_( cell!=NULL );
1290  os::fprintf(f,"%e ", cell->kf_mean );
1291  } // for cx
1292 
1293  if (cy<(m_size_y-1))
1294  os::fprintf(f,"; ...\n");
1295  } // for cy
1296  os::fprintf(f,"];\n\n");
1297 
1298  // z_upper: matrix with upper confidence levels for concentration values
1299  os::fprintf(f,"z_upper = [\n");
1300  for (cy=0;cy<m_size_y;cy++)
1301  {
1302  for (cx=0;cx<m_size_x;cx++)
1303  {
1304  const TRandomFieldCell *cell = cellByIndex( cx,cy );
1305  ASSERT_( cell!=NULL );
1307  } // for cx
1308 
1309  if (cy<(m_size_y-1))
1310  os::fprintf(f,"; ...\n");
1311  } // for cy
1312  os::fprintf(f,"];\n\n");
1313 
1314  // z_lowe: matrix with lower confidence levels for concentration values
1315  os::fprintf(f,"z_lower = [\n");
1316  for (cy=0;cy<m_size_y;cy++)
1317  {
1318  for (cx=0;cx<m_size_x;cx++)
1319  {
1320  const TRandomFieldCell *cell = cellByIndex( cx,cy );
1321  ASSERT_(cell!=NULL);
1323  } // for cx
1324 
1325  if (cy<(m_size_y-1))
1326  os::fprintf(f,"; ...\n");
1327  } // for cy
1328  os::fprintf(f,"];\n\n");
1329 
1330  // Plot them:
1331  os::fprintf(f,"hold off;\n");
1332  os::fprintf(f,"S1 = surf(xs,ys,z_mean);\n");
1333  os::fprintf(f,"hold on;\n");
1334  os::fprintf(f,"S2 = surf(xs,ys,z_upper);\n");
1335  os::fprintf(f,"S3 = surf(xs,ys,z_lower);\n");
1336  os::fprintf(f,"\n");
1337  os::fprintf(f,"set(S1,'FaceAlpha',0.9,'EdgeAlpha',0.9);\n");
1338  os::fprintf(f,"set(S2,'FaceAlpha',0.4,'EdgeAlpha',0.4);\n");
1339  os::fprintf(f,"set(S3,'FaceAlpha',0.2,'EdgeAlpha',0.2);\n");
1340  os::fprintf(f,"\n");
1341  os::fprintf(f,"set(gca,'PlotBoxAspectRatio',[%f %f %f]);\n",
1342  m_x_max-m_x_min,
1343  m_y_max-m_y_min,
1344  4.0 );
1345  os::fprintf(f,"view(-40,30)\n");
1346  os::fprintf(f,"axis([%f %f %f %f 0 1]);\n",
1347  m_x_min,
1348  m_x_max,
1349  m_y_min,
1350  m_y_max
1351  );
1352 
1353  os::fprintf(f,"\nfigure; imagesc(xs,ys,z_mean);axis equal;title('Mean value');colorbar;");
1354  os::fprintf(f,"\nfigure; imagesc(xs,ys,(z_upper-z_mean)/%f);axis equal;title('Std dev of estimated value');colorbar;",std_times);
1355 
1356  fclose(f);
1357 
1358  MRPT_END
1359 
1360 }
1361 
1362 /*---------------------------------------------------------------
1363  getAs3DObject
1364 ---------------------------------------------------------------*/
1365 void CRandomFieldGridMap2D::getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &outObj ) const
1366 {
1368 
1369  //Returns only the mean map
1370  mrpt::opengl::CSetOfObjectsPtr other_obj = mrpt::opengl::CSetOfObjects::Create();
1371  getAs3DObject(outObj, other_obj);
1372 }
1373 
1374 
1375 /*---------------------------------------------------------------
1376  getAs3DObject
1377 ---------------------------------------------------------------*/
1378 void CRandomFieldGridMap2D::getAs3DObject( mrpt::opengl::CSetOfObjectsPtr &meanObj, mrpt::opengl::CSetOfObjectsPtr &varObj ) const
1379 {
1381 
1382  recoverMeanAndCov(); //Only works for KF2 method
1383 
1385 
1386  unsigned int cx,cy;
1387  vector<double> xs,ys;
1388 
1389  // xs: array of X-axis values
1390  xs.resize( m_size_x );
1391  for (cx=0;cx<m_size_x;cx++) xs[cx] = m_x_min + m_resolution * cx;
1392 
1393  // ys: array of Y-axis values
1394  ys.resize( m_size_y );
1395  for (cy=0;cy<m_size_y;cy++) ys[cy] = m_y_min + m_resolution * cy;
1396 
1397  // Draw the surfaces:
1398  switch(m_mapType)
1399  {
1400  case mrKalmanFilter:
1401  case mrKalmanApproximate:
1402  case mrGMRF_SD:
1403  {
1404  // for Kalman models:
1405  // ----------------------------------
1406  opengl::CSetOfTrianglesPtr obj_m = opengl::CSetOfTriangles::Create();
1407  obj_m->enableTransparency(true);
1408  opengl::CSetOfTrianglesPtr obj_v = opengl::CSetOfTriangles::Create();
1409  obj_v->enableTransparency(true);
1410 
1411  // Compute mean max/min values:
1412  // ---------------------------------------
1413  double maxVal_m=0, minVal_m=1, AMaxMin_m, maxVal_v=0, minVal_v=1, AMaxMin_v;
1414  double c,v;
1415  for (cy=1;cy<m_size_y;cy++)
1416  {
1417  for (cx=1;cx<m_size_x;cx++)
1418  {
1419  const TRandomFieldCell *cell_xy = cellByIndex( cx,cy ); ASSERT_( cell_xy!=NULL );
1420  //mean
1421  c = cell_xy->kf_mean;
1422  minVal_m = min(minVal_m, c);
1423  maxVal_m = max(maxVal_m, c);
1424  //variance
1425  v = square(cell_xy->kf_std);
1426  minVal_v = min(minVal_v, v);
1427  maxVal_v = max(maxVal_v, v);
1428  }
1429  }
1430 
1431  AMaxMin_m = maxVal_m - minVal_m;
1432  if (AMaxMin_m==0) AMaxMin_m=1;
1433  AMaxMin_v = maxVal_v - minVal_v;
1434  if (AMaxMin_v==0) AMaxMin_v=1;
1435 
1436  // ---------------------------------------
1437  // Compute Maps
1438  // ---------------------------------------
1439  triag.a[0]=triag.a[1]=triag.a[2]= 0.75f; // alpha (transparency)
1440  for (cy=1;cy<m_size_y;cy++)
1441  {
1442  for (cx=1;cx<m_size_x;cx++)
1443  {
1444  // Cell values:
1445  const TRandomFieldCell *cell_xy = cellByIndex( cx,cy ); ASSERT_( cell_xy!=NULL );
1446  const TRandomFieldCell *cell_x_1y = cellByIndex( cx-1,cy ); ASSERT_( cell_x_1y!=NULL );
1447  const TRandomFieldCell *cell_xy_1 = cellByIndex( cx,cy-1 ); ASSERT_( cell_xy_1!=NULL );
1448  const TRandomFieldCell *cell_x_1y_1 = cellByIndex( cx-1,cy-1 ); ASSERT_( cell_x_1y_1!=NULL );
1449 
1450  // MEAN values
1451  //-----------------
1456 
1457  double col_xy = c_xy;
1458  double col_x_1y = c_x_1y;
1459  double col_xy_1 = c_xy_1;
1460  double col_x_1y_1 = c_x_1y_1;
1461 
1462  // Triangle #1:
1463  triag.x[0] = xs[cx]; triag.y[0] = ys[cy]; triag.z[0] = c_xy;
1464  triag.x[1] = xs[cx]; triag.y[1] = ys[cy-1]; triag.z[1] = c_xy_1;
1465  triag.x[2] = xs[cx-1]; triag.y[2] = ys[cy-1]; triag.z[2] = c_x_1y_1;
1466  jet2rgb( col_xy,triag.r[0],triag.g[0],triag.b[0] );
1467  jet2rgb( col_xy_1,triag.r[1],triag.g[1],triag.b[1] );
1468  jet2rgb( col_x_1y_1,triag.r[2],triag.g[2],triag.b[2] );
1469  obj_m->insertTriangle( triag );
1470 
1471  // Triangle #2:
1472  triag.x[0] = xs[cx]; triag.y[0] = ys[cy]; triag.z[0] = c_xy;
1473  triag.x[1] = xs[cx-1]; triag.y[1] = ys[cy-1]; triag.z[1] = c_x_1y_1;
1474  triag.x[2] = xs[cx-1]; triag.y[2] = ys[cy]; triag.z[2] = c_x_1y;
1475  jet2rgb( col_xy,triag.r[0],triag.g[0],triag.b[0] );
1476  jet2rgb( col_x_1y_1,triag.r[1],triag.g[1],triag.b[1] );
1477  jet2rgb( col_x_1y,triag.r[2],triag.g[2],triag.b[2] );
1478  obj_m->insertTriangle( triag );
1479 
1480  // VARIANCE values
1481  //------------------
1482  double v_xy = min(10.0,max(0.0, square(cell_xy->kf_std) ) );
1483  double v_x_1y = min(10.0,max(0.0, square(cell_x_1y->kf_std) ) );
1484  double v_xy_1 = min(10.0,max(0.0, square(cell_xy_1->kf_std) ) );
1485  double v_x_1y_1 = min(10.0,max(0.0, square(cell_x_1y_1->kf_std) ) );
1486 
1487  col_xy = v_xy/10; //min(1.0,max(0.0, (v_xy-minVal_v)/AMaxMin_v ) );
1488  col_x_1y = v_x_1y/10; //min(1.0,max(0.0, (v_x_1y-minVal_v)/AMaxMin_v ) );
1489  col_xy_1 = v_xy_1/10; //min(1.0,max(0.0, (v_xy_1-minVal_v)/AMaxMin_v ) );
1490  col_x_1y_1 = v_x_1y_1/10; //min(1.0,max(0.0, (v_x_1y_1-minVal_v)/AMaxMin_v ) );
1491 
1492 
1493  // Triangle #1:
1494  triag.x[0] = xs[cx]; triag.y[0] = ys[cy]; triag.z[0] = c_xy + v_xy;
1495  triag.x[1] = xs[cx]; triag.y[1] = ys[cy-1]; triag.z[1] = c_xy_1 + v_xy_1;
1496  triag.x[2] = xs[cx-1]; triag.y[2] = ys[cy-1]; triag.z[2] = c_x_1y_1+ v_x_1y_1;
1497  jet2rgb( col_xy,triag.r[0],triag.g[0],triag.b[0] );
1498  jet2rgb( col_xy_1,triag.r[1],triag.g[1],triag.b[1] );
1499  jet2rgb( col_x_1y_1,triag.r[2],triag.g[2],triag.b[2] );
1500  obj_v->insertTriangle( triag );
1501 
1502  // Triangle #2:
1503  triag.x[0] = xs[cx]; triag.y[0] = ys[cy]; triag.z[0] = c_xy + v_xy;
1504  triag.x[1] = xs[cx-1]; triag.y[1] = ys[cy-1]; triag.z[1] = c_x_1y_1+ v_x_1y_1;
1505  triag.x[2] = xs[cx-1]; triag.y[2] = ys[cy]; triag.z[2] = c_x_1y + v_x_1y;
1506  jet2rgb( col_xy,triag.r[0],triag.g[0],triag.b[0] );
1507  jet2rgb( col_x_1y_1,triag.r[1],triag.g[1],triag.b[1] );
1508  jet2rgb( col_x_1y,triag.r[2],triag.g[2],triag.b[2] );
1509  obj_v->insertTriangle( triag );
1510 
1511  } // for cx
1512  } // for cy
1513  meanObj->insert( obj_m );
1514  varObj->insert( obj_v );
1515  }
1516  break; // end KF models
1517 
1518  case mrKernelDM:
1519  case mrKernelDMV:
1520  {
1521  // Draw for Kernel model:
1522  // ----------------------------------
1523  opengl::CSetOfTrianglesPtr obj_m = opengl::CSetOfTriangles::Create();
1524  obj_m->enableTransparency(true);
1525  opengl::CSetOfTrianglesPtr obj_v = opengl::CSetOfTriangles::Create();
1526  obj_v->enableTransparency(true);
1527 
1528  // Compute mean max/min values:
1529  // ---------------------------------------
1530  double maxVal_m=0, minVal_m=1, AMaxMin_m, maxVal_v=0, minVal_v=1, AMaxMin_v;
1531  double c,v;
1532  for (cy=1;cy<m_size_y;cy++)
1533  {
1534  for (cx=1;cx<m_size_x;cx++)
1535  {
1536  const TRandomFieldCell *cell_xy = cellByIndex( cx,cy ); ASSERT_( cell_xy!=NULL );
1537  //mean
1538  c = computeMeanCellValue_DM_DMV( cell_xy );
1539  minVal_m = min(minVal_m, c);
1540  maxVal_m = max(maxVal_m, c);
1541  //variance
1542  v = computeVarCellValue_DM_DMV( cell_xy );
1543  minVal_v = min(minVal_v, v);
1544  maxVal_v = max(maxVal_v, v);
1545  }
1546  }
1547 
1548  AMaxMin_m = maxVal_m - minVal_m;
1549  if (AMaxMin_m==0) AMaxMin_m=1;
1550  AMaxMin_v = maxVal_v - minVal_v;
1551  if (AMaxMin_v==0) AMaxMin_v=1;
1552 
1553  // ---------------------------------------
1554  // Compute Maps
1555  // ---------------------------------------
1556  triag.a[0]=triag.a[1]=triag.a[2]= 0.75f; // alpha (transparency)
1557  for (cy=1;cy<m_size_y;cy++)
1558  {
1559  for (cx=1;cx<m_size_x;cx++)
1560  {
1561  // Cell values:
1562  const TRandomFieldCell *cell_xy = cellByIndex( cx,cy ); ASSERT_( cell_xy!=NULL );
1563  const TRandomFieldCell *cell_x_1y = cellByIndex( cx-1,cy ); ASSERT_( cell_x_1y!=NULL );
1564  const TRandomFieldCell *cell_xy_1 = cellByIndex( cx,cy-1 ); ASSERT_( cell_xy_1!=NULL );
1565  const TRandomFieldCell *cell_x_1y_1 = cellByIndex( cx-1,cy-1 ); ASSERT_( cell_x_1y_1!=NULL );
1566 
1567  // MEAN values
1568  //-----------------
1573 
1574  double col_xy = c_xy;
1575  double col_x_1y = c_x_1y;
1576  double col_xy_1 = c_xy_1;
1577  double col_x_1y_1 = c_x_1y_1;
1578 
1579  // Triangle #1:
1580  triag.x[0] = xs[cx]; triag.y[0] = ys[cy]; triag.z[0] = c_xy;
1581  triag.x[1] = xs[cx]; triag.y[1] = ys[cy-1]; triag.z[1] = c_xy_1;
1582  triag.x[2] = xs[cx-1]; triag.y[2] = ys[cy-1]; triag.z[2] = c_x_1y_1;
1583  jet2rgb( col_xy,triag.r[0],triag.g[0],triag.b[0] );
1584  jet2rgb( col_xy_1,triag.r[1],triag.g[1],triag.b[1] );
1585  jet2rgb( col_x_1y_1,triag.r[2],triag.g[2],triag.b[2] );
1586 
1587  obj_m->insertTriangle( triag );
1588 
1589  // Triangle #2:
1590  triag.x[0] = xs[cx]; triag.y[0] = ys[cy]; triag.z[0] = c_xy;
1591  triag.x[1] = xs[cx-1]; triag.y[1] = ys[cy-1]; triag.z[1] = c_x_1y_1;
1592  triag.x[2] = xs[cx-1]; triag.y[2] = ys[cy]; triag.z[2] = c_x_1y;
1593  jet2rgb( col_xy,triag.r[0],triag.g[0],triag.b[0] );
1594  jet2rgb( col_x_1y_1,triag.r[1],triag.g[1],triag.b[1] );
1595  jet2rgb( col_x_1y,triag.r[2],triag.g[2],triag.b[2] );
1596  obj_m->insertTriangle( triag );
1597 
1598  // VARIANCE values
1599  //------------------
1600  double v_xy = min(1.0,max(0.0, computeVarCellValue_DM_DMV(cell_xy) ) );
1601  double v_x_1y = min(1.0,max(0.0, computeVarCellValue_DM_DMV(cell_x_1y) ) );
1602  double v_xy_1 = min(1.0,max(0.0, computeVarCellValue_DM_DMV(cell_xy_1) ) );
1603  double v_x_1y_1 = min(1.0,max(0.0, computeVarCellValue_DM_DMV(cell_x_1y_1) ) );
1604 
1605  col_xy = v_xy;
1606  col_x_1y = v_x_1y;
1607  col_xy_1 = v_xy_1;
1608  col_x_1y_1 = v_x_1y_1;
1609 
1610  // Triangle #1:
1611  triag.x[0] = xs[cx]; triag.y[0] = ys[cy]; triag.z[0] = v_xy;
1612  triag.x[1] = xs[cx]; triag.y[1] = ys[cy-1]; triag.z[1] = v_xy_1;
1613  triag.x[2] = xs[cx-1]; triag.y[2] = ys[cy-1]; triag.z[2] = v_x_1y_1;
1614  jet2rgb( col_xy,triag.r[0],triag.g[0],triag.b[0] );
1615  jet2rgb( col_xy_1,triag.r[1],triag.g[1],triag.b[1] );
1616  jet2rgb( col_x_1y_1,triag.r[2],triag.g[2],triag.b[2] );
1617 
1618  obj_v->insertTriangle( triag );
1619 
1620  // Triangle #2:
1621  triag.x[0] = xs[cx]; triag.y[0] = ys[cy]; triag.z[0] = v_xy;
1622  triag.x[1] = xs[cx-1]; triag.y[1] = ys[cy-1]; triag.z[1] = v_x_1y_1;
1623  triag.x[2] = xs[cx-1]; triag.y[2] = ys[cy]; triag.z[2] = v_x_1y;
1624  jet2rgb( col_xy,triag.r[0],triag.g[0],triag.b[0] );
1625  jet2rgb( col_x_1y_1,triag.r[1],triag.g[1],triag.b[1] );
1626  jet2rgb( col_x_1y,triag.r[2],triag.g[2],triag.b[2] );
1627  obj_v->insertTriangle( triag );
1628 
1629 
1630  } // for cx
1631  } // for cy
1632  meanObj->insert( obj_m );
1633  varObj->insert( obj_v );
1634  }
1635  break; // end Kernel models
1636  }; // end switch maptype
1637 }
1638 
1639 
1640 
1641 /*---------------------------------------------------------------
1642  computeConfidenceCellValue_DM_DMV
1643  ---------------------------------------------------------------*/
1645 {
1646  // A confidence measure:
1647  return 1.0 - std::exp(-square(cell->dm_mean_w/m_insertOptions_common->dm_sigma_omega));
1648 }
1649 
1650 /*---------------------------------------------------------------
1651  computeMeanCellValue_DM_DMV
1652  ---------------------------------------------------------------*/
1654 {
1655  // A confidence measure:
1656  const double alpha = 1.0 - std::exp(-square(cell->dm_mean_w/m_insertOptions_common->dm_sigma_omega));
1657  const double r_val = (cell->dm_mean_w>0) ? (cell->dm_mean / cell->dm_mean_w) : 0;
1658  return alpha * r_val + (1-alpha) * m_average_normreadings_mean;
1659 }
1660 
1661 /*---------------------------------------------------------------
1662  computeVarCellValue_DM_DMV
1663  ---------------------------------------------------------------*/
1665 {
1666  // A confidence measure:
1667  const double alpha = 1.0 - std::exp(-square(cell->dm_mean_w/m_insertOptions_common->dm_sigma_omega));
1668  const double r_val = (cell->dm_mean_w>0) ? (cell->dmv_var_mean / cell->dm_mean_w) : 0;
1669  return alpha * r_val + (1-alpha) * m_average_normreadings_var;
1670 }
1671 
1672 
1674 {
1675  int cx,cy;
1676  double val, var;
1677  double coef;
1678 };
1679 
1680 /*---------------------------------------------------------------
1681  predictMeasurement
1682  ---------------------------------------------------------------*/
1684  const double x,
1685  const double y,
1686  double &out_predict_response,
1687  double &out_predict_response_variance,
1688  bool do_sensor_normalization,
1689  const TGridInterpolationMethod interp_method
1690  )
1691 {
1692  MRPT_START
1693 
1694  vector<TInterpQuery> queries;
1695  switch (interp_method)
1696  {
1697  case gimNearest:
1698  queries.resize(1);
1699  queries[0].cx = x2idx(x);
1700  queries[0].cy = y2idx(y);
1701  queries[0].coef = 1.0;
1702  break;
1703 
1704  case gimBilinear:
1705  if (x<=m_x_min+m_resolution*0.5 ||
1706  y<=m_y_min+m_resolution*0.5 ||
1707  x>=m_x_max-m_resolution*0.5 ||
1708  x>=m_x_max-m_resolution*0.5)
1709  {
1710  // Too close to a border:
1711  queries.resize(1);
1712  queries[0].cx = x2idx(x);
1713  queries[0].cy = y2idx(y);
1714  queries[0].coef = 1.0;
1715  }
1716  else
1717  {
1718  queries.resize(4);
1719  const double K_1 = 1.0/(m_resolution*m_resolution);
1720  // 11
1721  queries[0].cx = x2idx(x-m_resolution*0.5);
1722  queries[0].cy = y2idx(y-m_resolution*0.5);
1723  // 12
1724  queries[1].cx = x2idx(x-m_resolution*0.5);
1725  queries[1].cy = y2idx(y+m_resolution*0.5);
1726  // 21
1727  queries[2].cx = x2idx(x+m_resolution*0.5);
1728  queries[2].cy = y2idx(y-m_resolution*0.5);
1729  // 22
1730  queries[3].cx = x2idx(x+m_resolution*0.5);
1731  queries[3].cy = y2idx(y+m_resolution*0.5);
1732  // Weights:
1733  queries[0].coef = K_1*(idx2x(queries[3].cx)-x)*(idx2y(queries[3].cy)-y);
1734  queries[1].coef = K_1*(idx2x(queries[3].cx)-x)*(y-idx2y(queries[0].cy));
1735  queries[2].coef = K_1*(x-idx2x(queries[0].cx))*(idx2y(queries[3].cy)-y);
1736  queries[3].coef = K_1*(x-idx2x(queries[0].cx))*(y-idx2y(queries[0].cy));
1737  }
1738  break;
1739  default:
1740  THROW_EXCEPTION("Unknown interpolation method!");
1741  };
1742 
1743  // Run queries:
1744  for (size_t i=0;i<queries.size();i++)
1745  {
1746  TInterpQuery & q = queries[i];
1747 
1748  const TRandomFieldCell *cell = cellByIndex( q.cx, q.cy);
1749  switch (m_mapType)
1750  {
1751  case mrKernelDM:
1752  {
1753  if (!cell) {
1756  }
1757  else {
1758  q.val = computeMeanCellValue_DM_DMV(cell);
1760  }
1761  }
1762  break;
1763  case mrKernelDMV:
1764  {
1765  if (!cell) {
1768  }
1769  else {
1770  q.val = computeMeanCellValue_DM_DMV(cell);
1771  q.var = computeVarCellValue_DM_DMV(cell);
1772  }
1773  }
1774  break;
1775 
1776  case mrKalmanFilter:
1777  case mrKalmanApproximate:
1778  case mrGMRF_SD:
1779  {
1781  recoverMeanAndCov(); // Just for KF2
1782 
1783  if (!cell) {
1786  }
1787  else {
1788  q.val = cell->kf_mean;
1790  }
1791  }
1792  break;
1793 
1794  default:
1795  THROW_EXCEPTION("Invalid map type.");
1796  };
1797  }
1798 
1799  // Sum coeffs:
1800  out_predict_response = 0;
1801  out_predict_response_variance = 0;
1802  for (size_t i=0;i<queries.size();i++) {
1803  out_predict_response += queries[i].val * queries[i].coef;
1804  out_predict_response_variance += queries[i].var * queries[i].coef;
1805  }
1806 
1807  // Un-do the sensor normalization:
1808  if (do_sensor_normalization)
1809  out_predict_response = m_insertOptions_common->R_min + out_predict_response * ( m_insertOptions_common->R_max - m_insertOptions_common->R_min );
1810 
1811  MRPT_END
1812 }
1813 
1814 
1815 /*---------------------------------------------------------------
1816  insertObservation_KF2
1817  ---------------------------------------------------------------*/
1819  double normReading,
1820  const mrpt::math::TPoint2D &point )
1821 {
1822  MRPT_START
1823 
1824  MRPT_LOG_DEBUG_STREAM( "Inserting KF2: (" << normReading << ") at Position" << point << endl);
1825 
1826  const signed W = m_insertOptions_common->KF_W_size;
1827  const size_t K = 2*W*(W+1)+1;
1828  const size_t W21 = 2*W+1;
1829  const size_t W21sqr = W21*W21;
1830 
1831  ASSERT_(W>=2);
1832 
1833  m_hasToRecoverMeanAndCov = true;
1834 
1835  const TRandomFieldCell defCell(
1837  -1 // Just to indicate that cells are new, next changed to: m_insertOptions_common->KF_initialCellStd // std
1838  );
1839 
1840  // Assure we have room enough in the grid!
1841  const double Aspace = (W+1) * m_resolution;
1842 
1843  resize( point.x - Aspace,
1844  point.x + Aspace,
1845  point.y - Aspace,
1846  point.y + Aspace,
1847  defCell,
1848  Aspace );
1849 
1850  // --------------------------------------------------------
1851  // The Kalman-Filter estimation of the MRF grid-map:
1852  // --------------------------------------------------------
1853  const size_t N = m_map.size();
1854 
1855  ASSERT_(K==m_stackedCov.getColCount());
1856  ASSERT_(N==m_stackedCov.getRowCount());
1857 
1858  // Prediction stage of KF:
1859  // ------------------------------------
1860  // Nothing to do here (static map)
1861 
1862  // Update stage of KF:
1863  // We directly apply optimized formulas arising
1864  // from our concrete sensor model.
1865  // -------------------------------------------------
1866 
1867  //const double KF_covSigma2 = square(m_insertOptions_common->KF_covSigma);
1868  //const double std0 = m_insertOptions_common->KF_initialCellStd;
1869  //const double res2 = square(m_resolution);
1870  const int cellIdx = xy2idx( point.x, point.y );
1871  TRandomFieldCell *cell = cellByPos( point.x, point.y );
1872  ASSERT_(cell!=NULL);
1873 
1874  // Predicted observation mean
1875  double yk = normReading - cell->kf_mean;
1876 
1877  // Predicted observation variance
1878  double sk =
1879  m_stackedCov(cellIdx,0) + // Variance of that cell: cov(i,i)
1881 
1882  double sk_1 = 1.0 / sk;
1883 
1884  static CTicTac tictac;
1885  MRPT_LOG_DEBUG("[insertObservation_KF2] Updating mean values...");
1886  tictac.Tic();
1887 
1888  // ------------------------------------------------------------
1889  // Update mean values:
1890  // Process only those cells in the vecinity of the cell (c):
1891  //
1892  // What follows is *** REALLY UGLY *** for efficiency, sorry!! :-)
1893  // ------------------------------------------------------------
1894  const int cx_c = x2idx( point.x );
1895  const int cy_c = y2idx( point.y );
1896 
1897  const int Acx0 = max(-W, -cx_c);
1898  const int Acy0 = max(-W, -cy_c);
1899  const int Acx1 = min( W, int(m_size_x)-1-cx_c);
1900  const int Acy1 = min( W, int(m_size_y)-1-cy_c);
1901 
1902  // We will fill this now, so we already have it for updating the
1903  // covariances next:
1904  CVectorDouble cross_covs_c_i( W21sqr, 0); // Indexes are relative to the (2W+1)x(2W+1) window centered at "cellIdx".
1905  vector_int window_idxs ( W21sqr, -1 ); // The real-map indexes for each element in the window, or -1 if there are out of the map (for cells close to the border)
1906 
1907  // 1) First, the cells before "c":
1908  for (int Acy=Acy0;Acy<=0;Acy++)
1909  {
1910  int limit_cx = Acy<0 ? Acx1 : -1;
1911 
1912  size_t idx = cx_c+Acx0 + m_size_x * ( cy_c+Acy );
1913  int idx_c_in_idx = -Acy*W21 - Acx0;
1914 
1915  int window_idx = Acx0+W + (Acy+W)*W21;
1916 
1917  for (int Acx=Acx0;Acx<=limit_cx;Acx++)
1918  {
1919  ASSERT_(idx_c_in_idx>0);
1920  const double cov_i_c = m_stackedCov(idx,idx_c_in_idx);
1921  //JGmonroy - review m_stackedCov
1922 
1923  m_map[idx].kf_mean += yk * sk_1 * cov_i_c;
1924 
1925  // Save window indexes:
1926  cross_covs_c_i[window_idx] = cov_i_c;
1927  window_idxs[window_idx++] = idx;
1928 
1929  idx_c_in_idx--;
1930  idx++;
1931  }
1932  }
1933 
1934 
1935 
1936  // 2) The cell "c" itself, and the rest within the window:
1937  for (int Acy=0;Acy<=Acy1;Acy++)
1938  {
1939  int start_cx = Acy>0 ? Acx0 : 0;
1940 
1941  size_t idx = cx_c+start_cx + m_size_x * ( cy_c+Acy );
1942  int idx_i_in_c;
1943  if (Acy>0)
1944  idx_i_in_c= (W+1) + (Acy-1)*W21+ (start_cx + W); // Rest of rows
1945  else idx_i_in_c= 0; // First row.
1946 
1947  int window_idx = start_cx+W + (Acy+W)*W21;
1948 
1949  for (int Acx=start_cx;Acx<=Acx1;Acx++)
1950  {
1951  ASSERT_(idx_i_in_c>=0 && idx_i_in_c<int(K));
1952 
1953  double cov_i_c = m_stackedCov(cellIdx,idx_i_in_c);
1954  m_map[idx].kf_mean += yk * sk_1 * cov_i_c;
1955 
1956  // Save window indexes:
1957  cross_covs_c_i[window_idx] = cov_i_c;
1958  window_idxs[window_idx++] = idx;
1959 
1960  idx_i_in_c++;
1961  idx++;
1962  }
1963  }
1964 
1965 
1966  // ------------------------------------------------------------
1967  // Update cross-covariances values:
1968  // Process only those cells in the vecinity of the cell (c)
1969  // ------------------------------------------------------------
1970 
1971  // First, we need the cross-covariances between each cell (i) and
1972  // (c) in the window around (c). We have this in "cross_covs_c_i[k]"
1973  // for k=[0,(2K+1)^2-1], and the indexes in "window_idxs[k]".
1974  for (size_t i=0;i<W21sqr;i++)
1975  {
1976  const int idx_i = window_idxs[i];
1977  if (idx_i<0) continue; // out of the map
1978 
1979  // Extract the x,y indexes:
1980  int cx_i, cy_i;
1981  idx2cxcy(idx_i,cx_i,cy_i);
1982 
1983  const double cov_c_i = cross_covs_c_i[i];
1984 
1985  for (size_t j=i;j<W21sqr;j++)
1986  {
1987  const int idx_j = window_idxs[j];
1988  if (idx_j<0) continue; // out of the map
1989 
1990  int cx_j, cy_j;
1991  idx2cxcy(idx_j,cx_j,cy_j);
1992 
1993  // The cells (i,j) may be too far from each other:
1994  const int Ax = cx_j-cx_i;
1995  if (Ax>W) continue; // Next cell (may be more rows after the current one...)
1996 
1997  const int Ay = cy_j-cy_i;
1998  if (Ay>W) break; // We are absolutely sure out of i's window.
1999 
2000  const double cov_c_j = cross_covs_c_i[j];
2001 
2002  int idx_j_in_i;
2003  if (Ay>0)
2004  idx_j_in_i = Ax+W + (Ay-1)*W21 + W+1;
2005  else idx_j_in_i = Ax; // First row:
2006 
2007  // Kalman update of the cross-covariances:
2008  double &cov_to_change = m_stackedCov(idx_i,idx_j_in_i);
2009  double Delta_cov = cov_c_j * cov_c_i * sk_1;
2010  if (i==j && cov_to_change<Delta_cov)
2011  THROW_EXCEPTION_FMT("Negative variance value appeared! Please increase the size of the window (W).\n(m_insertOptions_common->KF_covSigma=%f)",m_insertOptions_common->KF_covSigma);
2012 
2013  cov_to_change -= Delta_cov;
2014 
2015  } // end for j
2016  } // end for i
2017 
2018 
2019  MRPT_LOG_DEBUG_FMT("Done in %.03fms\n", tictac.Tac()*1000 );
2020 
2021  MRPT_END
2022 }
2023 /*---------------------------------------------------------------
2024  recoverMeanAndCov
2025  ---------------------------------------------------------------*/
2027 {
2029  m_hasToRecoverMeanAndCov = false;
2030 
2031  // Just recover the std of each cell:
2032  const size_t N = m_map.size();
2033  for (size_t i=0;i<N;i++)
2034  m_map_castaway_const()[i].kf_std = sqrt( m_stackedCov(i,0) );
2035 }
2036 
2037 
2038 /*---------------------------------------------------------------
2039  getMeanAndCov
2040  ---------------------------------------------------------------*/
2042 {
2043  const size_t N = BASE::m_map.size();
2044  out_means.resize(N);
2045  for (size_t i=0;i<N;++i)
2046  out_means[i] = BASE::m_map[i].kf_mean;
2047 
2049  out_cov = m_cov;
2050 }
2051 
2052 
2053 
2054 /*---------------------------------------------------------------
2055  getMeanAndSTD
2056  ---------------------------------------------------------------*/
2058 {
2059  const size_t N = BASE::m_map.size();
2060  out_means.resize(N);
2061  out_STD.resize(N);
2062 
2063  for (size_t i=0;i<N;++i)
2064  {
2065  out_means[i] = BASE::m_map[i].kf_mean;
2066  out_STD[i] = sqrt(m_stackedCov(i,0));
2067  }
2068 }
2069 
2070 
2071 /*---------------------------------------------------------------
2072  setMeanAndSTD
2073  ---------------------------------------------------------------*/
2075 {
2076  //Assure dimmensions match
2077  const size_t N = BASE::m_map.size();
2078  ASSERT_( N == size_t(in_means.size()) );
2079  ASSERT_( N == size_t(in_std.size()) );
2080 
2081  m_hasToRecoverMeanAndCov = true;
2082  for (size_t i=0; i<N; ++i)
2083  {
2084  m_map[i].kf_mean = in_means[i]; //update mean values
2085  m_stackedCov(i,0) = square(in_std[i]); //update variance values
2086  }
2087  recoverMeanAndCov(); //update STD values from cov matrix
2088 }
2089 
2090 
2092 {
2093  return m_mapType;
2094 }
2095 
2097 {
2098  switch (m_mapType)
2099  {
2100  case mrKernelDM:
2101  case mrKernelDMV:
2102  case mrKalmanFilter:
2103  case mrKalmanApproximate:
2104  // Nothing to do, already done in the insert method...
2105  break;
2106 
2107  case mrGMRF_SD:
2108  this->updateMapEstimation_GMRF();
2109  break;
2110  default:
2111  THROW_EXCEPTION("insertObservation() isn't implemented for selected 'mapType'")
2112  };
2113 }
2114 
2115 void CRandomFieldGridMap2D::insertIndividualReading(const double sensorReading,const mrpt::math::TPoint2D & point, const bool update_map,const bool time_invariant, const double reading_stddev )
2116 {
2117  switch (m_mapType)
2118  {
2119  case mrKernelDM: insertObservation_KernelDM_DMV(sensorReading,point, false); break;
2120  case mrKernelDMV: insertObservation_KernelDM_DMV(sensorReading,point, true); break;
2121  case mrKalmanFilter: insertObservation_KF(sensorReading,point); break;
2122  case mrKalmanApproximate: insertObservation_KF2(sensorReading,point);break;
2123  case mrGMRF_SD: insertObservation_GMRF(sensorReading,point,update_map,time_invariant,
2124  reading_stddev==.0
2125  ?
2126  m_insertOptions_common->GMRF_lambdaObs // default information
2127  :
2128  1.0/mrpt::math::square(reading_stddev)
2129  ); break;
2130  default:
2131  THROW_EXCEPTION("insertObservation() isn't implemented for selected 'mapType'")
2132  };
2133 }
2134 
2135 
2136 /*---------------------------------------------------------------
2137  insertObservation_GMRF
2138  ---------------------------------------------------------------*/
2140  double normReading,
2141  const mrpt::math::TPoint2D &point,const bool update_map,
2142  const bool time_invariant,
2143  const double reading_information)
2144 {
2145 
2146  try{
2147  //Get index of observed cell
2148  const int cellIdx = xy2idx( point.x, point.y );
2149  TRandomFieldCell *cell = cellByPos( point.x, point.y );
2150  ASSERT_(cell!=NULL);
2151 
2152  // Insert observation in the vector of Active Observations
2153  TObservationGMRF new_obs(*this);
2154  new_obs.node_id = cellIdx;
2155  new_obs.obsValue = normReading;
2156  new_obs.Lambda = reading_information;
2157  new_obs.time_invariant = time_invariant;
2158 
2159  m_mrf_factors_activeObs[cellIdx].push_back(new_obs);
2160  m_gmrf.addConstraint(*m_mrf_factors_activeObs[cellIdx].rbegin()); // add to graph
2161 
2162  }catch(std::exception e){
2163  cerr << "Exception while Inserting new Observation: " << e.what() << endl;
2164  }
2165 
2166  //Solve system and update map estimation
2167  if (update_map) updateMapEstimation_GMRF();
2168 }
2169 
2170 /*---------------------------------------------------------------
2171  updateMapEstimation_GMRF
2172  ---------------------------------------------------------------*/
2174 {
2175  Eigen::VectorXd x_incr, x_var;
2177 
2178  ASSERT_(size_t(m_map.size()) == size_t(x_incr.size()));
2179  ASSERT_(m_insertOptions_common->GMRF_skip_variance || size_t(m_map.size()) == size_t(x_var.size()));
2180 
2181  // Update Mean-Variance in the base grid class
2182  for (size_t j = 0; j<m_map.size(); j++)
2183  {
2184  m_map[j].gmrf_std = m_insertOptions_common->GMRF_skip_variance ? .0 : std::sqrt(x_var[j] );
2185  m_map[j].gmrf_mean += x_incr[j];
2186 
2188  }
2189 
2190  // Update Information/Strength of Active Observations
2191  //---------------------------------------------------------
2193  {
2194  for (auto &obs : m_mrf_factors_activeObs) {
2195  for (auto ito = obs.begin(); ito != obs.end(); )
2196  {
2197  if (!ito->time_invariant) {
2198  ++ito;
2199  continue;
2200  }
2201 
2203  if (ito->Lambda < 0)
2204  {
2205  m_gmrf.eraseConstraint(*ito);
2206  ito = obs.erase(ito);
2207  }
2208  else
2209  ++ito;
2210  }
2211  }
2212  }
2213 }
2214 
2215 
2216 
2218  const mrpt::maps::COccupancyGridMap2D *m_Ocgridmap,
2219  size_t cxo_min,
2220  size_t cxo_max,
2221  size_t cyo_min,
2222  size_t cyo_max,
2223  const size_t seed_cxo,
2224  const size_t seed_cyo,
2225  const size_t objective_cxo,
2226  const size_t objective_cyo)
2227 {
2228  //printf("Checking relation between cells (%i,%i) and (%i,%i)", seed_cxo,seed_cyo,objective_cxo,objective_cyo);
2229 
2230  //Ensure delimited region is within the Occupancy map
2231  cxo_min = max (cxo_min, (size_t)0);
2232  cxo_max = min (cxo_max, (size_t)m_Ocgridmap->getSizeX()-1);
2233  cyo_min = max (cyo_min, (size_t)0);
2234  cyo_max = min (cyo_max, (size_t)m_Ocgridmap->getSizeY()-1);
2235 
2236  //printf("Under gridlimits cx=(%i,%i) and cy=(%i,%i) \n", cxo_min,cxo_max,cyo_min,cyo_max);
2237 
2238  //Check that seed and objective are inside the delimited Occupancy gridmap
2239  if( (seed_cxo < cxo_min) || (seed_cxo >= cxo_max) || (seed_cyo < cyo_min) || (seed_cyo >= cyo_max) )
2240  {
2241  //cout << "Seed out of bounds (false)" << endl;
2242  return false;
2243  }
2244  if( (objective_cxo < cxo_min) || (objective_cxo >= cxo_max) || (objective_cyo < cyo_min) || (objective_cyo >= cyo_max) )
2245  {
2246  //cout << "Objective out of bounds (false)" << endl;
2247  return false;
2248  }
2249 
2250  // Check that seed and obj have similar occupancy (0,1)
2251  if ( (m_Ocgridmap->getCell(seed_cxo,seed_cyo)<0.5) != (m_Ocgridmap->getCell(objective_cxo,objective_cyo)<0.5) )
2252  {
2253  //cout << "Seed and objective have diff occupation (false)" << endl;
2254  return false;
2255  }
2256 
2257 
2258  //Create Matrix for region growing (row,col)
2259  mrpt::math::CMatrixUInt matExp(cxo_max-cxo_min+1, cyo_max-cyo_min+1);
2260  //cout << "Matrix creted with dimension:" << matExp.getRowCount() << " x " << matExp.getColCount() << endl;
2261  //CMatrix matExp(cxo_max-cxo_min+1, cyo_max-cyo_min+1);
2262  matExp.fill(0);
2263 
2264  //Add seed
2265  matExp(seed_cxo-cxo_min,seed_cyo-cyo_min) = 1;
2266  int seedsOld = 0;
2267  int seedsNew = 1;
2268 
2269  //NOT VERY EFFICIENT!!
2270  while (seedsOld < seedsNew)
2271  {
2272  seedsOld = seedsNew;
2273 
2274  for (size_t col=0; col<matExp.getColCount(); col++)
2275  {
2276  for (size_t row=0; row<matExp.getRowCount(); row++)
2277  {
2278  //test if cell needs to be expanded
2279  if( matExp(row,col) == 1)
2280  {
2281  matExp(row,col) = 2; //mark as expanded
2282  //check if neighbourds have similar occupancy (expand)
2283  for (int i=-1;i<=1;i++)
2284  {
2285  for (int j=-1;j<=1;j++)
2286  {
2287  //check that neighbour is inside the map
2288  if( (int(row)+j>=0) && (int(row)+j<=int(matExp.getRowCount()-1)) && (int(col)+i>=0) && (int(col)+i<=int(matExp.getColCount())-1) )
2289  {
2290  if( !( (i==0 && j==0) || !(matExp(row+j,col+i)==0) ))
2291  {
2292  //check if expand
2293  if ( (m_Ocgridmap->getCell(row+cxo_min,col+cyo_min)<0.5) == (m_Ocgridmap->getCell(row+j+cxo_min,col+i+cyo_min)<0.5))
2294  {
2295  if ( (row+j+cxo_min == objective_cxo) && (col+i+cyo_min == objective_cyo) )
2296  {
2297  //cout << "Connection Success (true)" << endl;
2298  return true; //Objective connected
2299  }
2300  matExp(row+j,col+i) = 1;
2301  seedsNew++;
2302  }
2303  }
2304  }
2305  }
2306  }
2307  }
2308  }
2309  }
2310  }
2311  //if not connection to he objective is found, then return false
2312  //cout << "Connection not found (false)" << endl;
2313  return false;
2314 }
2315 
2317 {
2318  MRPT_UNUSED_PARAM(otherMap); MRPT_UNUSED_PARAM(otherMapPose);
2319  MRPT_UNUSED_PARAM(params);
2320  return 0;
2321 }
2322 
2323 
2324 // ============ TObservationGMRF ===========
2326 {
2327  return m_parent->m_map[this->node_id].gmrf_mean - this->obsValue;
2328 }
2330 {
2331  return this->Lambda;
2332 }
2334 {
2335  dr_dx = 1.0;
2336 }
2337 // ============ TPriorFactorGMRF ===========
2339 {
2340  return m_parent->m_map[this->node_id_i].gmrf_mean - m_parent->m_map[this->node_id_j].gmrf_mean;
2341 }
2343 {
2344  return this->Lambda;
2345 }
2346 void CRandomFieldGridMap2D::TPriorFactorGMRF::evalJacobian(double &dr_dx_i, double &dr_dx_j) const
2347 {
2348  dr_dx_i = +1.0;
2349  dr_dx_j = -1.0;
2350 }
2351 
2352 
2354 {
2355 }
2356 
2358 {
2359 }
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:34
void BASE_IMPEXP memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) MRPT_NO_THROWS
An OS and compiler independent version of "memcpy".
Definition: os.cpp:358
const GLdouble * v
Definition: glew.h:1296
std::string GMRF_gridmap_image_file
image name of the occupancy_gridmap
float R_max
Limits for normalization of sensor readings.
double GMRF_saturate_max
(Default:-inf,+inf) Saturate the estimated mean in these limits
GLint GLint GLint GLint GLint GLint y
Definition: glew.h:1166
Gaussian kernel-based estimator (see discussion in mrpt::maps::CRandomFieldGridMap2D) ...
Parameters for CMetricMap::compute3DMatchingRatio()
FILE BASE_IMPEXP * fopen(const char *fileName, const char *mode) MRPT_NO_THROWS
An OS-independent version of fopen.
Definition: os.cpp:255
virtual bool isEmpty() const MRPT_OVERRIDE
Returns true if the map is empty/no observation has been inserted (in this class it always return fal...
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
GLenum GLenum GLvoid * row
Definition: glew.h:2903
double y
X,Y coordinates.
Classes for serialization, sockets, ini-file manipulation, streams, list of properties-values, timewatch, extensions to STL.
Definition: zip.h:16
double computeMeanCellValue_DM_DMV(const TRandomFieldCell *cell) const
Computes the average cell concentration, or the overall average value if it has never been observed...
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
Definition: CMatrixD.h:30
#define min(a, b)
EIGEN_STRONG_INLINE void fill(const Scalar v)
Definition: eigen_plugins.h:38
#define MRPT_END_WITH_CLEAN_UP(stuff)
This namespace provides a OS-independent interface to many useful functions: filenames manipulation...
Definition: math_frwds.h:29
double getInformation() const MRPT_OVERRIDE
Return the inverse of the variance of this constraint.
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
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 BASE_IMPEXP void BASE_IMPEXP fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:272
bool eraseConstraint(const FactorBase &c)
Removes a constraint. Return true if found and deleted correctly.
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:101
GLclampf GLclampf GLclampf alpha
Definition: glew.h:1425
std::vector< TRandomFieldCell > & m_map_castaway_const() const
Used only from logically const method that really need to modify the object.
Definition: CDynamicGrid.h:45
bool exist_relation_between2cells(const mrpt::maps::COccupancyGridMap2D *m_Ocgridmap, size_t cxo_min, size_t cxo_max, size_t cyo_min, size_t cyo_max, const size_t seed_cxo, const size_t seed_cyo, const size_t objective_cxo, const size_t objective_cyo)
Check if two cells of the gridmap (m_map) are connected, based on the provided occupancy gridmap...
Base class for user-supplied objects capable of describing cells connectivity, used to build prior fa...
void insertObservation_GMRF(double normReading, const mrpt::math::TPoint2D &point, const bool update_map, const bool time_invariant, const double reading_information)
The implementation of "insertObservation" for the Gaussian Markov Random Field map model...
const GLfloat * c
Definition: glew.h:10088
#define THROW_EXCEPTION(msg)
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
int read_int(const std::string &section, const std::string &name, int defaultValue, bool failIfNotFound=false) const
Scalar * iterator
Definition: eigen_plugins.h:23
std::vector< std::list< TObservationGMRF > > m_mrf_factors_activeObs
Vector with the active observations and their respective Information.
mrpt::math::CMatrixD m_cov
The whole covariance matrix, used for the Kalman Filter map representation.
void internal_loadFromConfigFile_common(const mrpt::utils::CConfigFileBase &source, const std::string &section)
See utils::CLoadableOptions.
virtual void saveAsMatlab3DGraph(const std::string &filName) const
Save a matlab ".m" file which represents as 3D surfaces the mean and a given confidence level for the...
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
double Lambda
"Information" of the observation (=inverse of the variance)
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
Double mean + variance Gaussian kernel-based estimator (see discussion in mrpt::maps::CRandomFieldGri...
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.
double gmrf_mean
[GMRF only] The mean value of this cell
STL namespace.
TMapGenericParams genericMapParams
Common params to all maps.
float getResolution() const
Returns the resolution of the grid map.
float getYMin() const
Returns the "y" coordinate of top side of grid map.
double GMRF_lambdaPrior
The information (Lambda) of fixed map constraints.
virtual void saveAsBitmapFile(const std::string &filName) const
Save the current map as a graphical file (BMP,PNG,...).
virtual void resize(double new_x_min, double new_x_max, double new_y_min, double new_y_max, const TRandomFieldCell &defaultValueNewCells, double additionalMarginMeters=1.0f) MRPT_OVERRIDE
Changes the size of the grid, maintaining previous contents.
TMapRepresentation getMapType()
Return the type of the random-field grid map, according to parameters passed on construction.
void Tic()
Starts the stopwatch.
Definition: CTicTac.cpp:77
virtual bool getEdgeInformation(const CRandomFieldGridMap2D *parent, size_t icx, size_t icy, size_t jcx, size_t jcy, double &out_edge_information)=0
Implement the check of whether node i=(icx,icy) is connected with node j=(jcx,jcy).
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.
mrpt::graphs::ScalarFactorGraph m_gmrf
double dm_mean_w
[Kernel-methods only] The cumulative weights (concentration = alpha * dm_mean / dm_mean_w + (1-alpha)...
TRandomFieldCell * 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
bool BASE_IMPEXP isNaN(float f) MRPT_NO_THROWS
Returns true if the number is NaN.
Definition: math.cpp:1679
std::string read_string(const std::string &section, const std::string &name, const std::string &defaultValue, bool failIfNotFound=false) const
T saturate_val(const T &value, const T sat_min, const T sat_max)
Like saturate() but it returns the value instead of modifying the variable.
GLint GLvoid * img
Definition: glew.h:1290
int x2idx(double x) const
Transform a coordinate values into cell indexes.
Definition: CDynamicGrid.h:241
float KF_observationModelNoise
The sensor model noise (in normalized concentration units).
bool saveAsBitmapFile(const std::string &file) const
Saves the gridmap as a graphical file (BMP,PNG,...).
TInsertionOptionsCommon * m_insertOptions_common
Common options to all random-field grid maps: pointer that is set to the derived-class instance of "i...
void evalJacobian(double &dr_dx_i, double &dr_dx_j) const MRPT_OVERRIDE
Returns the derivative of the residual wrt the node values.
bool time_invariant
whether the observation will lose weight (lambda) as time goes on (default false) ...
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
A 2D grid of dynamic size which stores any kind of data at each cell.
Definition: CDynamicGrid.h:40
#define IMPLEMENTS_VIRTUAL_SERIALIZABLE(class_name, base_class_name, NameSpace)
This must be inserted as implementation of some required members for virtual CSerializable classes: ...
void updateMapEstimation_GMRF()
solves the minimum quadratic system to determine the new concentration of each cell ...
double kf_mean
[KF-methods only] The mean value of this cell
#define MRPT_END
float compute3DMatchingRatio(const mrpt::maps::CMetricMap *otherMap, const mrpt::poses::CPose3D &otherMapPose, const TMatchingRatioParams &params) const MRPT_OVERRIDE
See docs in base class: in this class this always returns 0.
#define MRPT_UNUSED_PARAM(a)
Can be used to avoid "not used parameters" warnings from the compiler.
double evaluateResidual() const MRPT_OVERRIDE
Return the residual/error of this observation.
The contents of each cell in a CRandomFieldGridMap2D map.
void setMeanAndSTD(mrpt::math::CVectorDouble &out_means, mrpt::math::CVectorDouble &out_STD)
Load the mean and STD vectors of the full Kalman filter estimate (works for all KF-based methods)...
bool saveToFile(const std::string &fileName, int jpeg_quality=95) const
Save the image to a file, whose format is determined from the extension (internally uses OpenCV)...
Definition: CImage.cpp:299
bool loadFromBitmapFile(const std::string &file, float resolution, float xCentralPixel=-1, float yCentralPixel=-1)
Load the gridmap from a image in a file (the format can be any supported by CImage::loadFromFile).
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
double getInformation() const MRPT_OVERRIDE
Return the inverse of the variance of this constraint.
"Brute-force" Kalman filter (see discussion in mrpt::maps::CRandomFieldGridMap2D) ...
virtual void setSize(const double x_min, const double x_max, const double y_min, const double y_max, const double resolution, const TRandomFieldCell *fill_value=NULL)
Changes the size of the grid, erasing previous contents.
GLint GLint GLint GLint GLint x
Definition: glew.h:1166
This namespace contains representation of robot actions and observations.
static CSetOfTrianglesPtr Create()
EIGEN_STRONG_INLINE void setSize(size_t row, size_t col)
Changes the size of matrix, maintaining its previous content as possible and padding with zeros where...
void insertObservation_KernelDM_DMV(double normReading, const mrpt::math::TPoint2D &point, bool is_DMV)
The implementation of "insertObservation" for Achim Lilienthal&#39;s map models DM & DM+V.
double computeVarCellValue_DM_DMV(const TRandomFieldCell *cell) const
Computes the estimated variance of the cell concentration, or the overall average variance if it has ...
virtual void getAs3DObject(mrpt::opengl::CSetOfObjectsPtr &outObj) const MRPT_OVERRIDE
Returns a 3D object representing the map (mean)
double GMRF_lambdaObs
The initial information (Lambda) of each observation (this information will decrease with time) ...
#define MRPT_LOG_DEBUG(_STRING)
double dm_sigma_omega
[DM/DM+V methods] The scaling parameter for the confidence "alpha" values (see the IROS 2009 paper; s...
float getXMin() const
Returns the "x" coordinate of left side of grid map.
void insertObservation_KF(double normReading, const mrpt::math::TPoint2D &point)
The implementation of "insertObservation" for the (whole) Kalman Filter map model.
typedef int(WINAPI *PFNWGLRELEASEPBUFFERDCARBPROC)(HPBUFFERARB hPbuffer
float getYMax() const
Returns the "y" coordinate of bottom side of grid map.
TRandomFieldCell * 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
void getMeanAndSTD(mrpt::math::CVectorDouble &out_means, mrpt::math::CVectorDouble &out_STD) const
Return the mean and STD vectors of the full Kalman filter estimate (works for all KF-based methods)...
double evaluateResidual() const MRPT_OVERRIDE
Return the residual/error of this observation.
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CPoint.h:17
(see discussion in mrpt::maps::CRandomFieldGridMap2D)
bool m_hasToRecoverMeanAndCov
Only for the KF2 implementation.
void addConstraint(const UnaryFactorVirtualBase &listOfConstraints)
Insert constraints into the GMRF problem.
unsigned int getSizeY() const
Returns the vertical size of grid map in cells count.
void setFromMatrix(const Eigen::MatrixBase< Derived > &m, bool matrix_is_normalized=true)
Set the image from a matrix, interpreted as grayscale intensity values, in the range [0...
Definition: CImage.h:746
A class for storing an occupancy grid map.
virtual void getAsBitmapFile(mrpt::utils::CImage &out_img) const
Returns an image just as described in saveAsBitmapFile.
#define MRPT_START
#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...
float getXMax() const
Returns the "x" coordinate of right side of grid map.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
void setCellsConnectivity(const ConnectivityDescriptorPtr &new_connectivity_descriptor)
Sets a custom object to define the connectivity between cells.
CRandomFieldGridMap2D represents a 2D grid map where each cell is associated one real-valued property...
GLsizei const GLcharARB ** string
Definition: glew.h:3293
Gaussian Markov Random Field, squared differences prior weights between 4 neighboring cells (see disc...
bool empty() const
Returns size()!=0.
Definition: CSimpleMap.cpp:72
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.
bool GMRF_skip_variance
(Default:false) Skip the computation of the variance, just compute the mean
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
float getCell(int x, int y) const
Read the real valued [0,1] contents of a cell, given its index.
void updateMapEstimation()
Run the method-specific procedure required to ensure that the mean & variances are up-to-date with al...
T square(const T x)
Inline function for the square of a number.
void saturate(T &var, const T sat_min, const T sat_max)
Saturate the value of var (the variable gets modified) so it does not get out of [min,max].
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
stlplus::smart_ptr< ConnectivityDescriptor > ConnectivityDescriptorPtr
void evalJacobian(double &dr_dx) const MRPT_OVERRIDE
Returns the derivative of the residual wrt the node value.
double dmv_var_mean
[Kernel DM-V only] The cumulative weighted variance of this cell
bool GMRF_use_occupancy_information
whether to use information of an occupancy_gridmap map for building the GMRF
std::string GMRF_simplemap_file
simplemap_file name of the occupancy_gridmap
uint16_t KF_W_size
[mrKalmanApproximate] The size of the window of neighbor cells.
double Tac()
Stops the stopwatch.
Definition: CTicTac.cpp:92
#define ASSERT_(f)
GLfloat * params
Definition: glew.h:1436
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
void updateEstimation(Eigen::VectorXd &solved_x_inc, Eigen::VectorXd *solved_variances=NULL)
void getMeanAndCov(mrpt::math::CVectorDouble &out_means, mrpt::math::CMatrixDouble &out_cov) const
Return the mean and covariance vector of the full Kalman filter estimate (works for all KF-based meth...
virtual void predictMeasurement(const double x, const double y, double &out_predict_response, double &out_predict_response_variance, bool do_sensor_normalization, const TGridInterpolationMethod interp_method=gimNearest)
Returns the prediction of the measurement at some (x,y) coordinates, and its certainty (in the form o...
std::vector< int32_t > vector_int
Definition: types_simple.h:23
mrpt::math::CMatrixD m_stackedCov
The compressed band diagonal matrix for the KF2 implementation.
GLuint GLfloat * val
Definition: glew.h:7785
unsigned int getSizeX() const
Returns the horizontal size of grid map in cells count.
double Lambda
"Information" of the observation (=inverse of the variance)
bool read_bool(const std::string &section, const std::string &name, bool defaultValue, bool failIfNotFound=false) const
double dm_mean
[Kernel-methods only] The cumulative weighted readings of this cell
size_t GMRF_gridmap_image_cx
Pixel coordinates of the origin for the occupancy_gridmap.
GLdouble GLdouble GLdouble GLdouble q
Definition: glew.h:1319
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:30
Lightweight 2D point.
double idx2x(int cx) const
Transform a cell index into a coordinate value of the cell central point.
Definition: CDynamicGrid.h:253
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
virtual void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const MRPT_OVERRIDE
The implementation in this class just calls all the corresponding method of the contained metric maps...
void insertObservation_KF2(double normReading, const mrpt::math::TPoint2D &point)
The implementation of "insertObservation" for the Efficient Kalman Filter map model.
double GMRF_gridmap_image_res
occupancy_gridmap resolution: size of each pixel (m)
void loadFromSimpleMap(const mrpt::maps::CSimpleMap &Map)
Load the map contents from a CSimpleMap object, erasing all previous content of the map...
void recoverMeanAndCov() const
In the KF2 implementation, takes the auxiliary matrices and from them update the cells&#39; mean and std ...
float KF_initialCellStd
The initial standard deviation of each cell&#39;s concentration (will be stored both at each cell&#39;s struc...
size_t GMRF_gridmap_image_cy
Pixel coordinates of the origin for the occupancy_gridmap.
double computeConfidenceCellValue_DM_DMV(const TRandomFieldCell *cell) const
Computes the confidence of the cell concentration (alpha)
virtual void getAsMatrix(mrpt::math::CMatrixDouble &out_mat) const
Like saveAsBitmapFile(), but returns the data in matrix form (first row in the matrix is the upper (y...
double kf_std
[KF-methods only] The standard deviation value of this cell
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
fixed floating point &#39;f&#39;
Definition: math_frwds.h:66
virtual void internal_clear() MRPT_OVERRIDE
Erase all the contents of the map.
static CSetOfObjectsPtr Create()
int xy2idx(double x, double y) const
Definition: CDynamicGrid.h:243
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: 2190203 Tue May 15 02:01:15 2018 +0200 at miƩ may 16 12:40:16 CEST 2018