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



Page generated by Doxygen 1.8.14 for MRPT 2.0.5 Git: 40e60e732 Thu Jul 9 08:38:35 2020 +0200 at jue jul 9 08:45:11 CEST 2020