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


Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ae4571287 Thu Nov 23 00:06:53 2017 +0100 at dom oct 27 23:51:55 CET 2019