MRPT  2.0.4
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>
14 #include <mrpt/io/CFileGZInputStream.h>
15 #include <mrpt/maps/COccupancyGridMap2D.h>
16 #include <mrpt/maps/CRandomFieldGridMap2D.h>
17 #include <mrpt/maps/CSimpleMap.h>
18 #include <mrpt/math/CMatrixF.h>
19 #include <mrpt/math/utils.h>
20 #include <mrpt/opengl/CSetOfObjects.h>
21 #include <mrpt/opengl/CSetOfTriangles.h>
22 #include <mrpt/system/CTicTac.h>
23 #include <mrpt/system/CTimeLogger.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 
39 IMPLEMENTS_VIRTUAL_SERIALIZABLE(CRandomFieldGridMap2D, CMetricMap, mrpt::maps)
40 
41 /*---------------------------------------------------------------
42  Constructor
43  ---------------------------------------------------------------*/
44 CRandomFieldGridMap2D::CRandomFieldGridMap2D(
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),
48  COutputLogger("CRandomFieldGridMap2D"),
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 
62 CRandomFieldGridMap2D::~CRandomFieldGridMap2D() = default;
63 /** Changes the size of the grid, erasing previous contents. \sa resize */
64 void CRandomFieldGridMap2D::setSize(
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 {
69  CDynamicGrid<TRandomFieldCell>::setSize(
70  x_min, x_max, y_min, y_max, resolution, fill_value);
71  CMetricMap::clear();
72 }
73 
74 void CRandomFieldGridMap2D::setCellsConnectivity(
75  const ConnectivityDescriptor::Ptr& new_connectivity_descriptor)
76 {
77  m_gmrf_connectivity = new_connectivity_descriptor;
78 }
79 
80 /*---------------------------------------------------------------
81  clear
82  ---------------------------------------------------------------*/
83 void CRandomFieldGridMap2D::internal_clear()
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  {
106  MRPT_LOG_DEBUG_FMT(
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 
166  MRPT_LOG_DEBUG(
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 
230  MRPT_LOG_DEBUG_FMT(
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 
241  MRPT_LOG_DEBUG(
242  "[clear] Generating Prior based on 'Squared Differences'\n");
243  MRPT_LOG_DEBUG_FMT(
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 
253  mrpt::maps::COccupancyGridMap2D m_Ocgridmap;
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;
261  CFileGZInputStream fi(
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  {
287  THROW_EXCEPTION(
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
293  MRPT_LOG_DEBUG(
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 
300  MRPT_LOG_DEBUG_FMT(
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());
305  MRPT_LOG_DEBUG_FMT(
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());
310  MRPT_LOG_DEBUG_FMT(
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());
315  MRPT_LOG_DEBUG_FMT(
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 
335  MRPT_LOG_DEBUG_STREAM(
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");
356  MRPT_LOG_DEBUG_FMT(
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));
360  MRPT_LOG_DEBUG_FMT(
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)
490  MRPT_LOG_DEBUG(
491  "[CRandomFieldGridMap2D::clear] Initiating prior "
492  "(using user-supplied connectivity pattern)");
493  else
494  MRPT_LOG_DEBUG(
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 
555  MRPT_LOG_DEBUG_STREAM(
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  */
584 void CRandomFieldGridMap2D::insertObservation_KernelDM_DMV(
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  {
608  MRPT_LOG_DEBUG_FMT(
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 
634  MRPT_LOG_DEBUG(
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  ---------------------------------------------------------------*/
673 CRandomFieldGridMap2D::TInsertionOptionsCommon::TInsertionOptionsCommon()
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  ---------------------------------------------------------------*/
688 void CRandomFieldGridMap2D::TInsertionOptionsCommon::
689  internal_dumpToTextStream_common(std::ostream& out) const
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  ---------------------------------------------------------------*/
745 void CRandomFieldGridMap2D::TInsertionOptionsCommon::
746  internal_loadFromConfigFile_common(
747  const mrpt::config::CConfigFileBase& iniFile,
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 */
803 void CRandomFieldGridMap2D::getAsMatrix(
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  };
836  mrpt::saturate(
837  c, m_insertOptions_common->GMRF_saturate_min,
838  m_insertOptions_common->GMRF_saturate_max);
839  cells_mat(m_size_y - 1 - y, x) = c;
840  }
841  }
842  MRPT_END
843 }
844 
845 /*---------------------------------------------------------------
846  getAsBitmapFile
847  ---------------------------------------------------------------*/
848 void CRandomFieldGridMap2D::getAsBitmapFile(mrpt::img::CImage& out_img) const
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  ---------------------------------------------------------------*/
861 void CRandomFieldGridMap2D::resize(
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:
873  CDynamicGrid<TRandomFieldCell>::resize(
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 
957  MRPT_END_WITH_CLEAN_UP(
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(
1036  m_insertOptions_common->KF_initialCellStd);
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 /
1058  m_insertOptions_common->KF_covSigma));
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 
1083  MRPT_LOG_DEBUG_FMT(
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 =
1108  square(m_insertOptions_common->KF_initialCellStd);
1109  double* ptr = &template_row[0];
1110  const double res2 = square(m_resolution);
1111  const double KF_covSigma2 =
1112  square(m_insertOptions_common->KF_covSigma);
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:
1152  cell.kf_std() = m_insertOptions_common->KF_initialCellStd;
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  ---------------------------------------------------------------*/
1179 void CRandomFieldGridMap2D::insertObservation_KF(
1180  double normReading, const mrpt::math::TPoint2D& point)
1181 {
1182  MRPT_START
1183 
1184  const TRandomFieldCell defCell(
1185  m_insertOptions_common->KF_defaultCellMeanValue, // mean
1186  m_insertOptions_common->KF_initialCellStd // std
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(
1213  m_insertOptions_common
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 
1248  MRPT_LOG_DEBUG_FMT(
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  ---------------------------------------------------------------*/
1299 void CRandomFieldGridMap2D::saveMetricMapRepresentationToFile(
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"),
1346  MATRIX_FORMAT_FIXED);
1347  all_confs.saveToTextFile(
1348  filNamePrefix + std::string("_confidence.txt"),
1349  MATRIX_FORMAT_FIXED);
1350  }
1351  }
1352  break;
1353 
1354  case mrKalmanFilter:
1355  case mrKalmanApproximate:
1356  {
1357  recoverMeanAndCov();
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"),
1374  MATRIX_FORMAT_FIXED);
1375 
1376  if (m_mapType == mrKalmanApproximate)
1377  {
1378  m_stackedCov.saveToTextFile(
1379  filNamePrefix + std::string("_mean_compressed_cov.txt"),
1380  MATRIX_FORMAT_FIXED);
1381  }
1382  if (m_mapType == mrKalmanFilter)
1383  {
1384  // Save the covariance matrix:
1385  m_cov.saveToTextFile(
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"),
1427  MATRIX_FORMAT_FIXED);
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  ---------------------------------------------------------------*/
1444 void CRandomFieldGridMap2D::saveAsMatlab3DGraph(
1445  const std::string& filName) const
1446 {
1447  MRPT_START
1448 
1449  const double std_times = 3;
1450 
1451  ASSERT_(
1452  m_mapType == mrKalmanFilter || m_mapType == mrKalmanApproximate ||
1453  m_mapType == mrGMRF_SD);
1454 
1455  recoverMeanAndCov();
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 ",
1520  mrpt::saturate_val(
1521  cell->kf_mean() + std_times * cell->kf_std(),
1522  m_insertOptions_common->GMRF_saturate_min,
1523  m_insertOptions_common->GMRF_saturate_max));
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 ",
1540  mrpt::saturate_val(
1541  cell->kf_mean() - std_times * cell->kf_std(),
1542  m_insertOptions_common->GMRF_saturate_min,
1543  m_insertOptions_common->GMRF_saturate_max));
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 ---------------------------------------------------------------*/
1586 void CRandomFieldGridMap2D::getAs3DObject(
1587  mrpt::opengl::CSetOfObjects::Ptr& outObj) const
1588 {
1589  if (!genericMapParams.enableSaveAs3DObject) return;
1590 
1591  // Returns only the mean map
1592  mrpt::opengl::CSetOfObjects::Ptr other_obj =
1593  mrpt::opengl::CSetOfObjects::Create();
1594  getAs3DObject(outObj, other_obj);
1595 }
1596 
1597 /*---------------------------------------------------------------
1598  getAs3DObject
1599 ---------------------------------------------------------------*/
1600 void CRandomFieldGridMap2D::getAs3DObject(
1601  mrpt::opengl::CSetOfObjects::Ptr& meanObj,
1602  mrpt::opengl::CSetOfObjects::Ptr& varObj) const
1603 {
1604  if (!genericMapParams.enableSaveAs3DObject) return;
1605 
1606  recoverMeanAndCov(); // Only works for KF2 method
1607 
1608  mrpt::opengl::TTriangle triag;
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(),
1684  m_insertOptions_common->GMRF_saturate_min,
1685  m_insertOptions_common->GMRF_saturate_max);
1686  double c_x_1y = mrpt::saturate_val(
1687  cell_x_1y->kf_mean(),
1688  m_insertOptions_common->GMRF_saturate_min,
1689  m_insertOptions_common->GMRF_saturate_max);
1690  double c_xy_1 = mrpt::saturate_val(
1691  cell_xy_1->kf_mean(),
1692  m_insertOptions_common->GMRF_saturate_min,
1693  m_insertOptions_common->GMRF_saturate_max);
1694  double c_x_1y_1 = mrpt::saturate_val(
1695  cell_x_1y_1->kf_mean(),
1696  m_insertOptions_common->GMRF_saturate_min,
1697  m_insertOptions_common->GMRF_saturate_max);
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),
1856  m_insertOptions_common->GMRF_saturate_min,
1857  m_insertOptions_common->GMRF_saturate_max);
1858  double c_x_1y = mrpt::saturate_val(
1859  computeMeanCellValue_DM_DMV(cell_x_1y),
1860  m_insertOptions_common->GMRF_saturate_min,
1861  m_insertOptions_common->GMRF_saturate_max);
1862  double c_xy_1 = mrpt::saturate_val(
1863  computeMeanCellValue_DM_DMV(cell_xy_1),
1864  m_insertOptions_common->GMRF_saturate_min,
1865  m_insertOptions_common->GMRF_saturate_max);
1866  double c_x_1y_1 = mrpt::saturate_val(
1867  computeMeanCellValue_DM_DMV(cell_x_1y_1),
1868  m_insertOptions_common->GMRF_saturate_min,
1869  m_insertOptions_common->GMRF_saturate_max);
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  ---------------------------------------------------------------*/
1972 double CRandomFieldGridMap2D::computeConfidenceCellValue_DM_DMV(
1973  const TRandomFieldCell* cell) const
1974 {
1975  // A confidence measure:
1976  return 1.0 -
1977  std::exp(-square(
1978  cell->dm_mean_w() / m_insertOptions_common->dm_sigma_omega));
1979 }
1980 
1981 /*---------------------------------------------------------------
1982  computeMeanCellValue_DM_DMV
1983  ---------------------------------------------------------------*/
1984 double CRandomFieldGridMap2D::computeMeanCellValue_DM_DMV(
1985  const TRandomFieldCell* cell) const
1986 {
1987  // A confidence measure:
1988  const double alpha =
1989  1.0 - std::exp(-square(
1990  cell->dm_mean_w() / m_insertOptions_common->dm_sigma_omega));
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  ---------------------------------------------------------------*/
1999 double CRandomFieldGridMap2D::computeVarCellValue_DM_DMV(
2000  const TRandomFieldCell* cell) const
2001 {
2002  // A confidence measure:
2003  const double alpha =
2004  1.0 - std::exp(-square(
2005  cell->dm_mean_w() / m_insertOptions_common->dm_sigma_omega));
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 
2011 struct TInterpQuery
2012 {
2013  int cx, cy;
2014  double val, var;
2015  double coef;
2016 };
2017 
2018 /*---------------------------------------------------------------
2019  predictMeasurement
2020  ---------------------------------------------------------------*/
2021 void CRandomFieldGridMap2D::predictMeasurement(
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  {
2091  q.val = m_average_normreadings_mean;
2092  q.var = square(m_insertOptions_common->KF_initialCellStd);
2093  }
2094  else
2095  {
2096  q.val = computeMeanCellValue_DM_DMV(cell);
2097  q.var = square(m_insertOptions_common->KF_initialCellStd);
2098  }
2099  }
2100  break;
2101  case mrKernelDMV:
2102  {
2103  if (!cell)
2104  {
2105  q.val = m_average_normreadings_mean;
2106  q.var = square(m_insertOptions_common->KF_initialCellStd);
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 &&
2121  m_hasToRecoverMeanAndCov)
2122  recoverMeanAndCov(); // Just for KF2
2123 
2124  if (!cell)
2125  {
2126  q.val = m_insertOptions_common->KF_defaultCellMeanValue;
2127  q.var =
2128  square(m_insertOptions_common->KF_initialCellStd) +
2129  square(
2130  m_insertOptions_common->KF_observationModelNoise);
2131  }
2132  else
2133  {
2134  q.val = cell->kf_mean();
2135  q.var =
2136  square(cell->kf_std()) +
2137  square(
2138  m_insertOptions_common->KF_observationModelNoise);
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 =
2160  m_insertOptions_common->R_min +
2161  out_predict_response *
2162  (m_insertOptions_common->R_max - m_insertOptions_common->R_min);
2163 
2164  MRPT_END
2165 }
2166 
2167 /*---------------------------------------------------------------
2168  insertObservation_KF2
2169  ---------------------------------------------------------------*/
2170 void CRandomFieldGridMap2D::insertObservation_KF2(
2171  double normReading, const mrpt::math::TPoint2D& point)
2172 {
2173  MRPT_START
2174 
2175  MRPT_LOG_DEBUG_STREAM(
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(
2188  m_insertOptions_common->KF_defaultCellMeanValue, // mean
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)
2229  square(m_insertOptions_common->KF_observationModelNoise);
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)
2367  THROW_EXCEPTION_FMT(
2368  "Negative variance value appeared! Please increase the "
2369  "size of the window "
2370  "(W).\n(m_insertOptions_common->KF_covSigma=%f)",
2371  m_insertOptions_common->KF_covSigma);
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  ---------------------------------------------------------------*/
2385 void CRandomFieldGridMap2D::recoverMeanAndCov() const
2386 {
2387  if (!m_hasToRecoverMeanAndCov || (m_mapType != mrKalmanApproximate)) return;
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  ---------------------------------------------------------------*/
2399 void CRandomFieldGridMap2D::getMeanAndCov(
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 
2406  recoverMeanAndCov();
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  ---------------------------------------------------------------*/
2414 void CRandomFieldGridMap2D::getMeanAndSTD(
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  ---------------------------------------------------------------*/
2431 void CRandomFieldGridMap2D::setMeanAndSTD(
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 
2448 CRandomFieldGridMap2D::TMapRepresentation CRandomFieldGridMap2D::getMapType()
2449 {
2450  return m_mapType;
2451 }
2452 
2453 void CRandomFieldGridMap2D::updateMapEstimation()
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:
2468  THROW_EXCEPTION(
2469  "insertObservation() isn't implemented for selected 'mapType'");
2470  };
2471 }
2472 
2473 void CRandomFieldGridMap2D::insertIndividualReading(
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:
2493  insertObservation_GMRF(
2494  sensorReading, point, update_map, time_invariant,
2495  reading_stddev == .0
2496  ? m_insertOptions_common
2497  ->GMRF_lambdaObs // default information
2498  : 1.0 / mrpt::square(reading_stddev));
2499  break;
2500  default:
2501  THROW_EXCEPTION(
2502  "insertObservation() isn't implemented for selected 'mapType'");
2503  };
2504 }
2505 
2506 /*---------------------------------------------------------------
2507  insertObservation_GMRF
2508  ---------------------------------------------------------------*/
2509 void CRandomFieldGridMap2D::insertObservation_GMRF(
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);
2529  m_gmrf.addConstraint(
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  ---------------------------------------------------------------*/
2545 void CRandomFieldGridMap2D::updateMapEstimation_GMRF()
2546 {
2547  mrpt::math::CVectorDouble x_incr, x_var;
2548  m_gmrf.updateEstimation(
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_(
2553  m_insertOptions_common->GMRF_skip_variance ||
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  {
2559  m_map[j].gmrf_std() = m_insertOptions_common->GMRF_skip_variance
2560  ? .0
2561  : std::sqrt(x_var[j]);
2562  m_map[j].gmrf_mean() += x_incr[j];
2563 
2564  mrpt::saturate(
2565  m_map[j].gmrf_mean(), m_insertOptions_common->GMRF_saturate_min,
2566  m_insertOptions_common->GMRF_saturate_max);
2567  }
2568 
2569  // Update Information/Strength of Active Observations
2570  //---------------------------------------------------------
2571  if (m_insertOptions_common->GMRF_lambdaObsLoss != 0)
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 
2583  ito->Lambda -= m_insertOptions_common->GMRF_lambdaObsLoss;
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 
2596 bool CRandomFieldGridMap2D::exist_relation_between2cells(
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 
2709 float CRandomFieldGridMap2D::compute3DMatchingRatio(
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 ===========
2718 double CRandomFieldGridMap2D::TObservationGMRF::evaluateResidual() const
2719 {
2720  return m_parent->m_map[this->node_id].gmrf_mean() - this->obsValue;
2721 }
2722 double CRandomFieldGridMap2D::TObservationGMRF::getInformation() const
2723 {
2724  return this->Lambda;
2725 }
2726 void CRandomFieldGridMap2D::TObservationGMRF::evalJacobian(double& dr_dx) const
2727 {
2728  dr_dx = 1.0;
2729 }
2730 // ============ TPriorFactorGMRF ===========
2731 double CRandomFieldGridMap2D::TPriorFactorGMRF::evaluateResidual() const
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 }
2736 double CRandomFieldGridMap2D::TPriorFactorGMRF::getInformation() const
2737 {
2738  return this->Lambda;
2739 }
2740 void CRandomFieldGridMap2D::TPriorFactorGMRF::evalJacobian(
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 
2747 CRandomFieldGridMap2D::ConnectivityDescriptor::ConnectivityDescriptor() =
2748  default;
2749 CRandomFieldGridMap2D::ConnectivityDescriptor::~ConnectivityDescriptor() =
2750  default;
mrpt::maps::CMetricMap::clear
void clear()
Erase all the contents of the map.
Definition: CMetricMap.cpp:30
IMPLEMENTS_VIRTUAL_SERIALIZABLE
#define IMPLEMENTS_VIRTUAL_SERIALIZABLE(class_name, base_class, NS)
This must be inserted as implementation of some required members for virtual CSerializable classes: ...
Definition: CSerializable.h:177
mrpt::maps::COccupancyGridMap2D::getXMax
float getXMax() const
Returns the "x" coordinate of right side of grid map.
Definition: COccupancyGridMap2D.h:280
mrpt::system::CTicTac::Tac
double Tac() noexcept
Stops the stopwatch.
Definition: CTicTac.cpp:87
mrpt::maps::CRandomFieldGridMap2D::mrKernelDM
Gaussian kernel-based estimator (see discussion in mrpt::maps::CRandomFieldGridMap2D) ...
Definition: CRandomFieldGridMap2D.h:179
mrpt::maps::TMatchingRatioParams
Parameters for CMetricMap::compute3DMatchingRatio()
Definition: metric_map_types.h:64
mrpt::containers::CDynamicGrid< TRandomFieldCell >::m_map
std::vector< TRandomFieldCell > m_map
The cells.
Definition: CDynamicGrid.h:42
mrpt::img::colormap
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
mrpt::maps::COccupancyGridMap2D::getYMin
float getYMin() const
Returns the "y" coordinate of top side of grid map.
Definition: COccupancyGridMap2D.h:282
mrpt::maps::CSimpleMap::empty
bool empty() const
Returns size()!=0.
Definition: CSimpleMap.cpp:54
mrpt::maps::CRandomFieldGridMap2D::getMeanAndSTD
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)...
Definition: CRandomFieldGridMap2D.cpp:2414
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
MRPT_LOG_DEBUG
#define MRPT_LOG_DEBUG(_STRING)
Use: MRPT_LOG_DEBUG("message");
Definition: system/COutputLogger.h:427
mrpt::maps::TRandomFieldCell::dm_mean
double & dm_mean()
[Kernel-methods only] The cumulative weighted readings of this cell
Definition: CRandomFieldGridMap2D.h:63
mrpt::maps::CRandomFieldGridMap2D::TPriorFactorGMRF::evaluateResidual
double evaluateResidual() const override
Return the residual/error of this observation.
Definition: CRandomFieldGridMap2D.cpp:2731
mrpt::math::CMatrixD
This class is a "CSerializable" wrapper for "CMatrixDynamic<double>".
Definition: CMatrixD.h:23
mrpt::maps::TRandomFieldCell::kf_std
double & kf_std()
[KF-methods only] The standard deviation value of this cell
Definition: CRandomFieldGridMap2D.h:72
mrpt::math::CMatrixDynamic::resize
void resize(size_t row, size_t col)
Definition: CMatrixDynamic.h:356
mrpt::maps::TRandomFieldCell::dm_mean_w
double & dm_mean_w()
[Kernel-methods only] The cumulative weights (concentration = alpha
Definition: CRandomFieldGridMap2D.h:76
mrpt::maps::CRandomFieldGridMap2D::saveAsMatlab3DGraph
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...
Definition: CRandomFieldGridMap2D.cpp:1444
mrpt::graphs::ScalarFactorGraph::updateEstimation
void updateEstimation(mrpt::math::CVectorDouble &solved_x_inc, mrpt::math::CVectorDouble *solved_variances=nullptr)
Definition: ScalarFactorGraph.cpp:81
mrpt::maps::COccupancyGridMap2D::getResolution
float getResolution() const
Returns the resolution of the grid map.
Definition: COccupancyGridMap2D.h:286
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
mrpt::math::TPoint2D_data::x
T x
X,Y coordinates.
Definition: TPoint2D.h:25
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
mrpt::maps::CSimpleMap
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:32
mrpt::maps::TMapGenericParams::enableSaveAs3DObject
bool enableSaveAs3DObject
(Default=true) If false, calling CMetricMap::getAs3DObject() will have no effects ...
Definition: metric_map_types.h:84
mrpt::maps::CRandomFieldGridMap2D::TInsertionOptionsCommon::KF_defaultCellMeanValue
float KF_defaultCellMeanValue
The default value for the mean of cells&#39; concentration.
Definition: CRandomFieldGridMap2D.h:280
mrpt::system::os::fclose
int void fclose(FILE *f)
An OS-independent version of fclose.
Definition: os.cpp:286
mrpt::graphs::ScalarFactorGraph::eraseConstraint
bool eraseConstraint(const FactorBase &c)
Removes a constraint.
Definition: ScalarFactorGraph.cpp:52
mrpt::maps::CRandomFieldGridMap2D::TObservationGMRF::getInformation
double getInformation() const override
Return the inverse of the variance of this constraint.
Definition: CRandomFieldGridMap2D.cpp:2722
mrpt::opengl::TTriangle::x
const float & x(size_t i) const
Definition: TTriangle.h:90
mrpt::math::MatrixVectorBase::fill
void fill(const Scalar &val)
Definition: MatrixVectorBase.h:70
mrpt::maps::CRandomFieldGridMap2D::exist_relation_between2cells
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...
Definition: CRandomFieldGridMap2D.cpp:2596
mrpt::maps::CRandomFieldGridMap2D::ConnectivityDescriptor
Base class for user-supplied objects capable of describing cells connectivity, used to build prior fa...
Definition: CRandomFieldGridMap2D.h:339
mrpt::maps::CRandomFieldGridMap2D::insertObservation_GMRF
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...
Definition: CRandomFieldGridMap2D.cpp:2509
mrpt::opengl::TTriangle
A triangle (float coordinates) with RGBA colors (u8) and UV (texture coordinates) for each vertex...
Definition: TTriangle.h:35
mrpt::maps::CRandomFieldGridMap2D::TPriorFactorGMRF::evalJacobian
void evalJacobian(double &dr_dx_i, double &dr_dx_j) const override
Returns the derivative of the residual wrt the node values.
Definition: CRandomFieldGridMap2D.cpp:2740
mrpt::maps::CRandomFieldGridMap2D::m_mrf_factors_activeObs
std::vector< std::list< TObservationGMRF > > m_mrf_factors_activeObs
Vector with the active observations and their respective Information.
Definition: CRandomFieldGridMap2D.h:575
mrpt::system::CTicTac
A high-performance stopwatch, with typical resolution of nanoseconds.
Definition: system/CTicTac.h:17
mrpt::maps::CRandomFieldGridMap2D::m_cov
mrpt::math::CMatrixD m_cov
The whole covariance matrix, used for the Kalman Filter map representation.
Definition: CRandomFieldGridMap2D.h:504
mrpt::math::CVectorDynamic::size
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
Definition: CVectorDynamic.h:141
mrpt::maps::CRandomFieldGridMap2D::TPriorFactorGMRF::Lambda
double Lambda
"Information" of the observation (=inverse of the variance)
Definition: CRandomFieldGridMap2D.h:557
params
mrpt::vision::TStereoCalibParams params
Definition: chessboard_stereo_camera_calib_unittest.cpp:24
mrpt::maps::CRandomFieldGridMap2D::mrKernelDMV
Double mean + variance Gaussian kernel-based estimator (see discussion in mrpt::maps::CRandomFieldGri...
Definition: CRandomFieldGridMap2D.h:190
mrpt::maps::CRandomFieldGridMap2D::TMapRepresentation
TMapRepresentation
The type of map representation to be used, see CRandomFieldGridMap2D for a discussion.
Definition: CRandomFieldGridMap2D.h:175
mrpt::containers::CDynamicGrid< TRandomFieldCell >::idx2cxcy
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
mrpt::maps::CRandomFieldGridMap2D::getMeanAndCov
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...
Definition: CRandomFieldGridMap2D.cpp:2399
mrpt::maps::COccupancyGridMap2D::loadFromBitmapFile
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).
Definition: COccupancyGridMap2D_io.cpp:268
std
STL namespace.
mrpt::opengl::CSetOfObjects::Create
static Ptr Create(Args &&... args)
Definition: CSetOfObjects.h:28
mrpt::maps::CMetricMap::genericMapParams
TMapGenericParams genericMapParams
Common params to all maps.
Definition: CMetricMap.h:274
mrpt::containers::CDynamicGrid< TRandomFieldCell >::idx2x
double idx2x(int cx) const
Transform a cell index into a coordinate value of the cell central point.
Definition: CDynamicGrid.h:279
dir
const auto dir
Definition: chessboard_stereo_camera_calib_unittest.cpp:28
MRPT_END_WITH_CLEAN_UP
#define MRPT_END_WITH_CLEAN_UP(stuff)
Definition: exceptions.h:247
mrpt::maps::CRandomFieldGridMap2D::getMapType
TMapRepresentation getMapType()
Return the type of the random-field grid map, according to parameters passed on construction.
Definition: CRandomFieldGridMap2D.cpp:2448
mrpt::containers::CDynamicGrid< TRandomFieldCell >::m_map_castaway_const
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
mrpt::maps::CRandomFieldGridMap2D::TObservationGMRF::evalJacobian
void evalJacobian(double &dr_dx) const override
Returns the derivative of the residual wrt the node value.
Definition: CRandomFieldGridMap2D.cpp:2726
mrpt::math::MatrixVectorBase::saveToTextFile
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.
Definition: MatrixVectorBase_impl.h:159
mrpt::maps::CRandomFieldGridMap2D::getAs3DObject
void getAs3DObject(mrpt::opengl::CSetOfObjects::Ptr &outObj) const override
Returns a 3D object representing the map (mean)
Definition: CRandomFieldGridMap2D.cpp:1586
mrpt::maps::CRandomFieldGridMap2D::computeMeanCellValue_DM_DMV
double computeMeanCellValue_DM_DMV(const TRandomFieldCell *cell) const
Computes the average cell concentration, or the overall average value if it has never been observed...
Definition: CRandomFieldGridMap2D.cpp:1984
mrpt::maps::CRandomFieldGridMap2D::ConnectivityDescriptor::getEdgeInformation
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).
mrpt::maps::CRandomFieldGridMap2D::recoverMeanAndCov
void recoverMeanAndCov() const
In the KF2 implementation, takes the auxiliary matrices and from them update the cells&#39; mean and std ...
Definition: CRandomFieldGridMap2D.cpp:2385
mrpt::maps::CRandomFieldGridMap2D::TInsertionOptionsCommon::internal_dumpToTextStream_common
void internal_dumpToTextStream_common(std::ostream &out) const
See utils::CLoadableOptions.
Definition: CRandomFieldGridMap2D.cpp:689
mrpt::maps::CRandomFieldGridMap2D::m_mapType
TMapRepresentation m_mapType
The map representation type of this map, as passed in the constructor.
Definition: CRandomFieldGridMap2D.h:500
mrpt::maps::CRandomFieldGridMap2D::m_gmrf
mrpt::graphs::ScalarFactorGraph m_gmrf
Definition: CRandomFieldGridMap2D.h:527
mrpt::serialization::archiveFrom
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
mrpt::opengl::TTriangle::a
const uint8_t & a(size_t i) const
Definition: TTriangle.h:96
mrpt::containers::CDynamicGrid< TRandomFieldCell >::setSize
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
mrpt::saturate
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].
Definition: core/include/mrpt/core/bits_math.h:159
mrpt::opengl::TTriangle::vertices
std::array< Vertex, 3 > vertices
Definition: TTriangle.h:88
mrpt::maps::CRandomFieldGridMap2D::TPriorFactorGMRF::getInformation
double getInformation() const override
Return the inverse of the variance of this constraint.
Definition: CRandomFieldGridMap2D.cpp:2736
mrpt::maps::CRandomFieldGridMap2D::TInsertionOptionsCommon::KF_observationModelNoise
float KF_observationModelNoise
The sensor model noise (in normalized concentration units).
Definition: CRandomFieldGridMap2D.h:278
mrpt::maps::CRandomFieldGridMap2D::~CRandomFieldGridMap2D
~CRandomFieldGridMap2D() override
Destructor.
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::maps::CRandomFieldGridMap2D::m_insertOptions_common
TInsertionOptionsCommon * m_insertOptions_common
Common options to all random-field grid maps: pointer that is set to the derived-class instance of "i...
Definition: CRandomFieldGridMap2D.h:492
mrpt::maps::CRandomFieldGridMap2D::TObservationGMRF::time_invariant
bool time_invariant
whether the observation will lose weight (lambda) as time goes on (default false) ...
Definition: CRandomFieldGridMap2D.h:538
mrpt::maps::COccupancyGridMap2D::saveAsBitmapFile
bool saveAsBitmapFile(const std::string &file) const
Saves the gridmap as a graphical file (BMP,PNG,...).
Definition: COccupancyGridMap2D_io.cpp:36
mrpt::maps::CRandomFieldGridMap2D::TInsertionOptionsCommon::R_min
float R_min
Limits for normalization of sensor readings.
Definition: CRandomFieldGridMap2D.h:262
CSetOfTriangles.h
mrpt::maps::CRandomFieldGridMap2D::saveAsBitmapFile
virtual void saveAsBitmapFile(const std::string &filName) const
Save the current map as a graphical file (BMP,PNG,...).
Definition: CRandomFieldGridMap2D.cpp:791
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:44
mrpt::maps::CRandomFieldGridMap2D::isEmpty
bool isEmpty() const override
Returns true if the map is empty/no observation has been inserted (in this class it always return fal...
Definition: CRandomFieldGridMap2D.cpp:575
mrpt::math
This base provides a set of functions for maths stuff.
Definition: math/include/mrpt/math/bits_math.h:11
mrpt::maps::TRandomFieldCell::kf_mean
double & kf_mean()
[KF-methods only] The mean value of this cell
Definition: CRandomFieldGridMap2D.h:59
mrpt::maps::CRandomFieldGridMap2D::resize
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.
Definition: CRandomFieldGridMap2D.cpp:861
CSetOfObjects.h
mrpt::maps::CRandomFieldGridMap2D::updateMapEstimation_GMRF
void updateMapEstimation_GMRF()
solves the minimum quadratic system to determine the new concentration of each cell ...
Definition: CRandomFieldGridMap2D.cpp:2545
MRPT_LOG_DEBUG_FMT
#define MRPT_LOG_DEBUG_FMT(_FMT_STRING,...)
Use: MRPT_LOG_DEBUG_FMT("i=%u", i);
Definition: system/COutputLogger.h:461
mrpt::maps::COccupancyGridMap2D::getXMin
float getXMin() const
Returns the "x" coordinate of left side of grid map.
Definition: COccupancyGridMap2D.h:278
std::shared_ptr< ConnectivityDescriptor >
mrpt::maps::TRandomFieldCell
The contents of each cell in a CRandomFieldGridMap2D map.
Definition: CRandomFieldGridMap2D.h:36
mrpt::maps::CRandomFieldGridMap2D::setMeanAndSTD
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)...
Definition: CRandomFieldGridMap2D.cpp:2431
mrpt::system::COutputLogger
Versatile class for consistent logging and management of output messages.
Definition: system/COutputLogger.h:117
color_maps.h
mrpt::containers::CDynamicGrid< TRandomFieldCell >::cellByPos
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
mrpt::maps::CRandomFieldGridMap2D::mrKalmanFilter
"Brute-force" Kalman filter (see discussion in mrpt::maps::CRandomFieldGridMap2D) ...
Definition: CRandomFieldGridMap2D.h:185
mrpt::maps::CRandomFieldGridMap2D::setSize
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.
Definition: CRandomFieldGridMap2D.cpp:64
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
iniFile
string iniFile(myDataDir+string("benchmark-options.ini"))
val
int val
Definition: mrpt_jpeglib.h:957
mrpt::maps::CRandomFieldGridMap2D::insertObservation_KernelDM_DMV
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.
Definition: CRandomFieldGridMap2D.cpp:584
mrpt::maps::COccupancyGridMap2D::getSizeX
unsigned int getSizeX() const
Returns the horizontal size of grid map in cells count.
Definition: COccupancyGridMap2D.h:274
mrpt::img::CImage::setFromMatrix
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
mrpt::maps::CRandomFieldGridMap2D::TInsertionOptionsCommon::dm_sigma_omega
double dm_sigma_omega
[DM/DM+V methods] The scaling parameter for the confidence "alpha" values (see the IROS 2009 paper; s...
Definition: CRandomFieldGridMap2D.h:265
mrpt::containers::CDynamicGrid< TRandomFieldCell >::cellByIndex
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
mrpt::maps::CRandomFieldGridMap2D::insertObservation_KF
void insertObservation_KF(double normReading, const mrpt::math::TPoint2D &point)
The implementation of "insertObservation" for the (whole) Kalman Filter map model.
Definition: CRandomFieldGridMap2D.cpp:1179
MRPT_LOG_DEBUG_STREAM
#define MRPT_LOG_DEBUG_STREAM(__CONTENTS)
Use: MRPT_LOG_DEBUG_STREAM("Var=" << value << " foo=" << foo_var);
Definition: system/COutputLogger.h:471
mrpt::maps::CRandomFieldGridMap2D::getAsBitmapFile
virtual void getAsBitmapFile(mrpt::img::CImage &out_img) const
Returns an image just as described in saveAsBitmapFile.
Definition: CRandomFieldGridMap2D.cpp:848
mrpt::math::CMatrixDynamic::rows
size_type rows() const
Number of rows in the matrix.
Definition: CMatrixDynamic.h:337
CSimpleMap.h
mrpt::math::CMatrixDynamic::cols
size_type cols() const
Number of columns in the matrix.
Definition: CMatrixDynamic.h:340
mrpt::poses
Classes for 2D/3D geometry representation, both of single values and probability density distribution...
Definition: CHierarchicalMapMHPartition.h:22
mrpt::maps::CRandomFieldGridMap2D::mrKalmanApproximate
(see discussion in mrpt::maps::CRandomFieldGridMap2D)
Definition: CRandomFieldGridMap2D.h:187
COccupancyGridMap2D.h
mrpt::maps::CRandomFieldGridMap2D::TInsertionOptionsCommon::internal_loadFromConfigFile_common
void internal_loadFromConfigFile_common(const mrpt::config::CConfigFileBase &source, const std::string &section)
See utils::CLoadableOptions.
Definition: CRandomFieldGridMap2D.cpp:746
mrpt::math::TPoint2D_data::y
T y
Definition: TPoint2D.h:25
mrpt::math::TPoint2D_< double >
mrpt::maps::CRandomFieldGridMap2D::m_hasToRecoverMeanAndCov
bool m_hasToRecoverMeanAndCov
Only for the KF2 implementation.
Definition: CRandomFieldGridMap2D.h:514
mrpt::graphs::ScalarFactorGraph::addConstraint
void addConstraint(const UnaryFactorVirtualBase &listOfConstraints)
Insert constraints into the GMRF problem.
Definition: ScalarFactorGraph.cpp:43
mrpt::system::os::fprintf
int fprintf(FILE *fil, const char *format,...) noexcept MRPT_printf_format_check(2
An OS-independent version of fprintf.
Definition: os.cpp:419
mrpt::maps::CRandomFieldGridMap2D::setCellsConnectivity
void setCellsConnectivity(const ConnectivityDescriptor::Ptr &new_connectivity_descriptor)
Sets a custom object to define the connectivity between cells.
Definition: CRandomFieldGridMap2D.cpp:74
mrpt::square
return_t square(const num_t x)
Inline function for the square of a number.
Definition: core/include/mrpt/core/bits_math.h:23
mrpt::math::MATRIX_FORMAT_FIXED
fixed floating point &#39;f&#39;
Definition: MatrixVectorBase.h:39
MRPT_LOAD_CONFIG_VAR
#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...
Definition: config/CConfigFileBase.h:306
mrpt::maps::CRandomFieldGridMap2D::TInsertionOptionsCommon::GMRF_saturate_min
double GMRF_saturate_min
(Default:-inf,+inf) Saturate the estimated mean in these limits
Definition: CRandomFieldGridMap2D.h:310
mrpt::maps::COccupancyGridMap2D
A class for storing an occupancy grid map.
Definition: COccupancyGridMap2D.h:53
mrpt::img::cmJET
Definition: color_maps.h:35
mrpt::math::CMatrixF
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
mrpt::opengl::TTriangle::y
const float & y(size_t i) const
Definition: TTriangle.h:91
mrpt::maps::CRandomFieldGridMap2D
CRandomFieldGridMap2D represents a 2D grid map where each cell is associated one real-valued property...
Definition: CRandomFieldGridMap2D.h:152
mrpt::maps::TRandomFieldCell::gmrf_mean
double & gmrf_mean()
[GMRF only] The mean value of this cell
Definition: CRandomFieldGridMap2D.h:66
mrpt::maps::CRandomFieldGridMap2D::mrGMRF_SD
Gaussian Markov Random Field, squared differences prior weights between 4 neighboring cells (see disc...
Definition: CRandomFieldGridMap2D.h:197
mrpt::maps::CRandomFieldGridMap2D::TInsertionOptionsCommon::KF_covSigma
float KF_covSigma
The "sigma" for the initial covariance value between cells (in meters).
Definition: CRandomFieldGridMap2D.h:272
mrpt::maps::CMetricMap
Declares a virtual base class for all metric maps storage classes.
Definition: CMetricMap.h:52
mrpt::maps::CRandomFieldGridMap2D::TInsertionOptionsCommon::GMRF_skip_variance
bool GMRF_skip_variance
(Default:false) Skip the computation of the variance, just compute the mean
Definition: CRandomFieldGridMap2D.h:313
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
out
mrpt::vision::TStereoCalibResults out
Definition: chessboard_stereo_camera_calib_unittest.cpp:25
mrpt::maps::CRandomFieldGridMap2D::updateMapEstimation
void updateMapEstimation()
Run the method-specific procedure required to ensure that the mean & variances are up-to-date with al...
Definition: CRandomFieldGridMap2D.cpp:2453
mrpt::maps::CRandomFieldGridMap2D::saveMetricMapRepresentationToFile
void saveMetricMapRepresentationToFile(const std::string &filNamePrefix) const override
The implementation in this class just calls all the corresponding method of the contained metric maps...
Definition: CRandomFieldGridMap2D.cpp:1299
mrpt::opengl::TTriangle::z
const float & z(size_t i) const
Definition: TTriangle.h:92
mrpt::math::CMatrixDynamic::setSize
void setSize(size_t row, size_t col, bool zeroNewElements=false)
Changes the size of matrix, maintaining the previous contents.
Definition: CMatrixDynamic.h:352
mrpt::maps::CRandomFieldGridMap2D::internal_clear
void internal_clear() override
Erase all the contents of the map.
Definition: CRandomFieldGridMap2D.cpp:83
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::maps::TRandomFieldCell::dmv_var_mean
double dmv_var_mean
[Kernel DM-V only] The cumulative weighted variance of this cell
Definition: CRandomFieldGridMap2D.h:82
mrpt::maps::CRandomFieldGridMap2D::TInsertionOptionsCommon::KF_W_size
uint16_t KF_W_size
[mrKalmanApproximate] The size of the window of neighbor cells.
Definition: CRandomFieldGridMap2D.h:282
mrpt::containers::CDynamicGrid< TRandomFieldCell >::x2idx
int x2idx(double x) const
Transform a coordinate values into cell indexes.
Definition: CDynamicGrid.h:256
mrpt::maps::COccupancyGridMap2D::getYMax
float getYMax() const
Returns the "y" coordinate of bottom side of grid map.
Definition: COccupancyGridMap2D.h:284
mrpt::io::CFileGZInputStream
Transparently opens a compressed "gz" file and reads uncompressed data from it.
Definition: io/CFileGZInputStream.h:26
mrpt::maps::CRandomFieldGridMap2D::computeConfidenceCellValue_DM_DMV
double computeConfidenceCellValue_DM_DMV(const TRandomFieldCell *cell) const
Computes the confidence of the cell concentration (alpha)
Definition: CRandomFieldGridMap2D.cpp:1972
mrpt::maps::CRandomFieldGridMap2D::predictMeasurement
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...
Definition: CRandomFieldGridMap2D.cpp:2021
mrpt::system::os::fopen
FILE * fopen(const char *fileName, const char *mode) noexcept
An OS-independent version of fopen.
Definition: os.cpp:268
mrpt::maps::CRandomFieldGridMap2D::m_stackedCov
mrpt::math::CMatrixD m_stackedCov
The compressed band diagonal matrix for the KF2 implementation.
Definition: CRandomFieldGridMap2D.h:512
mrpt::img::CImage::saveToFile
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
mrpt::maps::CRandomFieldGridMap2D::TObservationGMRF::evaluateResidual
double evaluateResidual() const override
Return the residual/error of this observation.
Definition: CRandomFieldGridMap2D.cpp:2718
mrpt::maps::CRandomFieldGridMap2D::TObservationGMRF::Lambda
double Lambda
"Information" of the observation (=inverse of the variance)
Definition: CRandomFieldGridMap2D.h:535
CRandomFieldGridMap2D.h
mrpt::math::CVectorDynamic::resize
void resize(std::size_t N, bool zeroNewElements=false)
Definition: CVectorDynamic.h:151
mrpt::system::CTicTac::Tic
void Tic() noexcept
Starts the stopwatch.
Definition: CTicTac.cpp:76
mrpt::maps::CRandomFieldGridMap2D::computeVarCellValue_DM_DMV
double computeVarCellValue_DM_DMV(const TRandomFieldCell *cell) const
Computes the estimated variance of the cell concentration, or the overall average variance if it has ...
Definition: CRandomFieldGridMap2D.cpp:1999
mrpt::maps::COccupancyGridMap2D::getSizeY
unsigned int getSizeY() const
Returns the vertical size of grid map in cells count.
Definition: COccupancyGridMap2D.h:276
mrpt::saturate_val
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.
Definition: core/include/mrpt/core/bits_math.h:167
THROW_EXCEPTION_FMT
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
Definition: exceptions.h:69
mrpt::maps::CRandomFieldGridMap2D::insertObservation_KF2
void insertObservation_KF2(double normReading, const mrpt::math::TPoint2D &point)
The implementation of "insertObservation" for the Efficient Kalman Filter map model.
Definition: CRandomFieldGridMap2D.cpp:2170
resize
images resize(NUM_IMGS)
mrpt::maps::CMetricMap::loadFromSimpleMap
void loadFromSimpleMap(const mrpt::maps::CSimpleMap &Map)
!
Definition: CMetricMap.h:106
mrpt::maps::CRandomFieldGridMap2D::TInsertionOptionsCommon::KF_initialCellStd
float KF_initialCellStd
The initial standard deviation of each cell&#39;s concentration (will be stored both at each cell&#39;s struc...
Definition: CRandomFieldGridMap2D.h:276
mrpt::maps::CRandomFieldGridMap2D::getAsMatrix
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...
Definition: CRandomFieldGridMap2D.cpp:803
mrpt::maps::COccupancyGridMap2D::getCell
float getCell(int x, int y) const
Read the real valued [0,1] contents of a cell, given its index.
Definition: COccupancyGridMap2D.h:357
CMatrixF.h
mrpt::maps::CRandomFieldGridMap2D::TInsertionOptionsCommon::GMRF_lambdaObsLoss
double GMRF_lambdaObsLoss
The loss of information of the observations with each iteration.
Definition: CRandomFieldGridMap2D.h:293
mrpt::img::CImage
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:148
mrpt::system::os::memcpy
void memcpy(void *dest, size_t destSize, const void *src, size_t copyCount) noexcept
An OS and compiler independent version of "memcpy".
mrpt::maps::CRandomFieldGridMap2D::insertIndividualReading
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 ...
Definition: CRandomFieldGridMap2D.cpp:2473
mrpt::maps::CRandomFieldGridMap2D::compute3DMatchingRatio
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.
Definition: CRandomFieldGridMap2D.cpp:2709
mrpt::round
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.4 Git: 33de1d0ad Sat Jun 20 11:02:42 2020 +0200 at sáb jun 20 17:35:17 CEST 2020