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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019