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



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 23:51:15 UTC 2020